From c18625c99c4dcd51b3ea45532def7c11fb03763f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tur=C3=A1nszki=20J=C3=A1nos?= Date: Sun, 23 Jun 2024 17:56:47 +0200 Subject: [PATCH] Jolt physics (#867) --- Editor/RigidBodyWindow.cpp | 13 +- Editor/SoftBodyWindow.cpp | 2 - WickedEngine/BULLET/Bullet-C-Api.h | 176 - .../BroadphaseCollision/btAxisSweep3.cpp | 37 - .../BroadphaseCollision/btAxisSweep3.h | 1051 ----- .../btBroadphaseInterface.h | 82 - .../BroadphaseCollision/btBroadphaseProxy.cpp | 17 - .../BroadphaseCollision/btBroadphaseProxy.h | 270 -- .../btCollisionAlgorithm.cpp | 23 - .../btCollisionAlgorithm.h | 81 - .../BroadphaseCollision/btDbvt.cpp | 1295 ------ .../BroadphaseCollision/btDbvt.h | 1270 ------ .../BroadphaseCollision/btDbvtBroadphase.cpp | 796 ---- .../BroadphaseCollision/btDbvtBroadphase.h | 146 - .../BroadphaseCollision/btDispatcher.cpp | 22 - .../BroadphaseCollision/btDispatcher.h | 107 - .../btMultiSapBroadphase.cpp | 489 --- .../btMultiSapBroadphase.h | 151 - .../btOverlappingPairCache.cpp | 633 --- .../btOverlappingPairCache.h | 470 --- .../btOverlappingPairCallback.h | 40 - .../BroadphaseCollision/btQuantizedBvh.cpp | 1393 ------- .../BroadphaseCollision/btQuantizedBvh.h | 581 --- .../btSimpleBroadphase.cpp | 349 -- .../BroadphaseCollision/btSimpleBroadphase.h | 171 - .../SphereTriangleDetector.cpp | 200 - .../SphereTriangleDetector.h | 51 - .../btActivatingCollisionAlgorithm.cpp | 47 - .../btActivatingCollisionAlgorithm.h | 36 - .../btBox2dBox2dCollisionAlgorithm.cpp | 421 -- .../btBox2dBox2dCollisionAlgorithm.h | 66 - .../btBoxBoxCollisionAlgorithm.cpp | 84 - .../btBoxBoxCollisionAlgorithm.h | 66 - .../CollisionDispatch/btBoxBoxDetector.cpp | 718 ---- .../CollisionDispatch/btBoxBoxDetector.h | 44 - .../btCollisionConfiguration.h | 46 - .../CollisionDispatch/btCollisionCreateFunc.h | 45 - .../btCollisionDispatcher.cpp | 314 -- .../CollisionDispatch/btCollisionDispatcher.h | 171 - .../CollisionDispatch/btCollisionObject.cpp | 117 - .../CollisionDispatch/btCollisionObject.h | 565 --- .../btCollisionObjectWrapper.h | 43 - .../CollisionDispatch/btCollisionWorld.cpp | 1552 ------- .../CollisionDispatch/btCollisionWorld.h | 526 --- .../btCompoundCollisionAlgorithm.cpp | 375 -- .../btCompoundCollisionAlgorithm.h | 99 - .../btCompoundCompoundCollisionAlgorithm.cpp | 421 -- .../btCompoundCompoundCollisionAlgorithm.h | 90 - .../btConvex2dConvex2dAlgorithm.cpp | 246 -- .../btConvex2dConvex2dAlgorithm.h | 95 - .../btConvexConcaveCollisionAlgorithm.cpp | 335 -- .../btConvexConcaveCollisionAlgorithm.h | 121 - .../btConvexConvexAlgorithm.cpp | 783 ---- .../btConvexConvexAlgorithm.h | 108 - .../btConvexPlaneCollisionAlgorithm.cpp | 174 - .../btConvexPlaneCollisionAlgorithm.h | 84 - .../btDefaultCollisionConfiguration.cpp | 307 -- .../btDefaultCollisionConfiguration.h | 127 - .../btEmptyCollisionAlgorithm.cpp | 34 - .../btEmptyCollisionAlgorithm.h | 54 - .../CollisionDispatch/btGhostObject.cpp | 171 - .../CollisionDispatch/btGhostObject.h | 175 - .../btHashedSimplePairCache.cpp | 278 -- .../btHashedSimplePairCache.h | 174 - .../btInternalEdgeUtility.cpp | 842 ---- .../CollisionDispatch/btInternalEdgeUtility.h | 47 - .../CollisionDispatch/btManifoldResult.cpp | 154 - .../CollisionDispatch/btManifoldResult.h | 150 - .../btSimulationIslandManager.cpp | 450 -- .../btSimulationIslandManager.h | 81 - .../btSphereBoxCollisionAlgorithm.cpp | 214 - .../btSphereBoxCollisionAlgorithm.h | 75 - .../btSphereSphereCollisionAlgorithm.cpp | 106 - .../btSphereSphereCollisionAlgorithm.h | 66 - .../btSphereTriangleCollisionAlgorithm.cpp | 84 - .../btSphereTriangleCollisionAlgorithm.h | 69 - .../CollisionDispatch/btUnionFind.cpp | 82 - .../CollisionDispatch/btUnionFind.h | 129 - .../CollisionShapes/btBox2dShape.cpp | 42 - .../CollisionShapes/btBox2dShape.h | 371 -- .../CollisionShapes/btBoxShape.cpp | 51 - .../CollisionShapes/btBoxShape.h | 314 -- .../btBvhTriangleMeshShape.cpp | 466 --- .../CollisionShapes/btBvhTriangleMeshShape.h | 145 - .../CollisionShapes/btCapsuleShape.cpp | 171 - .../CollisionShapes/btCapsuleShape.h | 184 - .../CollisionShapes/btCollisionMargin.h | 27 - .../CollisionShapes/btCollisionShape.cpp | 119 - .../CollisionShapes/btCollisionShape.h | 159 - .../CollisionShapes/btCompoundShape.cpp | 356 -- .../CollisionShapes/btCompoundShape.h | 212 - .../CollisionShapes/btConcaveShape.cpp | 27 - .../CollisionShapes/btConcaveShape.h | 62 - .../CollisionShapes/btConeShape.cpp | 147 - .../CollisionShapes/btConeShape.h | 171 - .../CollisionShapes/btConvex2dShape.cpp | 92 - .../CollisionShapes/btConvex2dShape.h | 82 - .../CollisionShapes/btConvexHullShape.cpp | 250 -- .../CollisionShapes/btConvexHullShape.h | 122 - .../CollisionShapes/btConvexInternalShape.cpp | 151 - .../CollisionShapes/btConvexInternalShape.h | 224 - .../btConvexPointCloudShape.cpp | 139 - .../CollisionShapes/btConvexPointCloudShape.h | 105 - .../CollisionShapes/btConvexPolyhedron.cpp | 302 -- .../CollisionShapes/btConvexPolyhedron.h | 65 - .../CollisionShapes/btConvexShape.cpp | 455 -- .../CollisionShapes/btConvexShape.h | 84 - .../btConvexTriangleMeshShape.cpp | 315 -- .../btConvexTriangleMeshShape.h | 77 - .../CollisionShapes/btCylinderShape.cpp | 281 -- .../CollisionShapes/btCylinderShape.h | 213 - .../CollisionShapes/btEmptyShape.cpp | 50 - .../CollisionShapes/btEmptyShape.h | 72 - .../btHeightfieldTerrainShape.cpp | 410 -- .../btHeightfieldTerrainShape.h | 167 - .../CollisionShapes/btMaterial.h | 35 - .../CollisionShapes/btMinkowskiSumShape.cpp | 60 - .../CollisionShapes/btMinkowskiSumShape.h | 62 - .../CollisionShapes/btMultiSphereShape.cpp | 182 - .../CollisionShapes/btMultiSphereShape.h | 101 - .../btMultimaterialTriangleMeshShape.cpp | 45 - .../btMultimaterialTriangleMeshShape.h | 120 - .../CollisionShapes/btOptimizedBvh.cpp | 391 -- .../CollisionShapes/btOptimizedBvh.h | 65 - .../btPolyhedralConvexShape.cpp | 500 --- .../CollisionShapes/btPolyhedralConvexShape.h | 116 - .../btScaledBvhTriangleMeshShape.cpp | 121 - .../btScaledBvhTriangleMeshShape.h | 95 - .../CollisionShapes/btShapeHull.cpp | 170 - .../CollisionShapes/btShapeHull.h | 61 - .../CollisionShapes/btSphereShape.cpp | 71 - .../CollisionShapes/btSphereShape.h | 73 - .../CollisionShapes/btStaticPlaneShape.cpp | 107 - .../CollisionShapes/btStaticPlaneShape.h | 105 - .../btStridingMeshInterface.cpp | 381 -- .../CollisionShapes/btStridingMeshInterface.h | 164 - .../CollisionShapes/btTetrahedronShape.cpp | 218 - .../CollisionShapes/btTetrahedronShape.h | 76 - .../CollisionShapes/btTriangleBuffer.cpp | 35 - .../CollisionShapes/btTriangleBuffer.h | 69 - .../CollisionShapes/btTriangleCallback.cpp | 28 - .../CollisionShapes/btTriangleCallback.h | 42 - .../btTriangleIndexVertexArray.cpp | 95 - .../btTriangleIndexVertexArray.h | 133 - .../btTriangleIndexVertexMaterialArray.cpp | 86 - .../btTriangleIndexVertexMaterialArray.h | 84 - .../CollisionShapes/btTriangleInfoMap.h | 241 -- .../CollisionShapes/btTriangleMesh.cpp | 162 - .../CollisionShapes/btTriangleMesh.h | 69 - .../CollisionShapes/btTriangleMeshShape.cpp | 207 - .../CollisionShapes/btTriangleMeshShape.h | 90 - .../CollisionShapes/btTriangleShape.h | 184 - .../CollisionShapes/btUniformScalingShape.cpp | 160 - .../CollisionShapes/btUniformScalingShape.h | 89 - WickedEngine/BULLET/BulletCollision/Doxyfile | 746 ---- .../BulletCollision/Gimpact/btBoxCollision.h | 645 --- .../BulletCollision/Gimpact/btClipPolygon.h | 182 - .../Gimpact/btCompoundFromGimpact.h | 93 - .../Gimpact/btContactProcessing.cpp | 181 - .../Gimpact/btContactProcessing.h | 145 - .../BulletCollision/Gimpact/btGImpactBvh.cpp | 498 --- .../BulletCollision/Gimpact/btGImpactBvh.h | 396 -- .../Gimpact/btGImpactCollisionAlgorithm.cpp | 932 ----- .../Gimpact/btGImpactCollisionAlgorithm.h | 310 -- .../Gimpact/btGImpactMassUtil.h | 60 - .../Gimpact/btGImpactQuantizedBvh.cpp | 528 --- .../Gimpact/btGImpactQuantizedBvh.h | 372 -- .../Gimpact/btGImpactShape.cpp | 238 -- .../BulletCollision/Gimpact/btGImpactShape.h | 1184 ------ .../Gimpact/btGenericPoolAllocator.cpp | 283 -- .../Gimpact/btGenericPoolAllocator.h | 163 - .../Gimpact/btGeometryOperations.h | 212 - .../BulletCollision/Gimpact/btQuantization.h | 88 - .../Gimpact/btTriangleShapeEx.cpp | 218 - .../Gimpact/btTriangleShapeEx.h | 180 - .../BulletCollision/Gimpact/gim_array.h | 324 -- .../Gimpact/gim_basic_geometry_operations.h | 543 --- .../BulletCollision/Gimpact/gim_bitset.h | 123 - .../Gimpact/gim_box_collision.h | 588 --- .../BulletCollision/Gimpact/gim_box_set.cpp | 182 - .../BulletCollision/Gimpact/gim_box_set.h | 674 --- .../Gimpact/gim_clip_polygon.h | 210 - .../BulletCollision/Gimpact/gim_contact.cpp | 146 - .../BulletCollision/Gimpact/gim_contact.h | 164 - .../BulletCollision/Gimpact/gim_geom_types.h | 97 - .../BulletCollision/Gimpact/gim_geometry.h | 42 - .../BulletCollision/Gimpact/gim_hash_table.h | 902 ---- .../BulletCollision/Gimpact/gim_linear_math.h | 1573 ------- .../BULLET/BulletCollision/Gimpact/gim_math.h | 157 - .../BulletCollision/Gimpact/gim_memory.cpp | 135 - .../BulletCollision/Gimpact/gim_memory.h | 190 - .../BulletCollision/Gimpact/gim_radixsort.h | 406 -- .../Gimpact/gim_tri_collision.cpp | 640 --- .../Gimpact/gim_tri_collision.h | 379 -- .../btContinuousConvexCollision.cpp | 242 -- .../btContinuousConvexCollision.h | 59 - .../NarrowPhaseCollision/btConvexCast.cpp | 20 - .../NarrowPhaseCollision/btConvexCast.h | 73 - .../btConvexPenetrationDepthSolver.h | 40 - .../btDiscreteCollisionDetectorInterface.h | 88 - .../NarrowPhaseCollision/btGjkConvexCast.cpp | 176 - .../NarrowPhaseCollision/btGjkConvexCast.h | 50 - .../NarrowPhaseCollision/btGjkEpa2.cpp | 1031 ----- .../NarrowPhaseCollision/btGjkEpa2.h | 75 - .../btGjkEpaPenetrationDepthSolver.cpp | 66 - .../btGjkEpaPenetrationDepthSolver.h | 43 - .../btGjkPairDetector.cpp | 480 --- .../NarrowPhaseCollision/btGjkPairDetector.h | 103 - .../NarrowPhaseCollision/btManifoldPoint.h | 156 - .../btMinkowskiPenetrationDepthSolver.cpp | 361 -- .../btMinkowskiPenetrationDepthSolver.h | 40 - .../btPersistentManifold.cpp | 305 -- .../btPersistentManifold.h | 240 -- .../NarrowPhaseCollision/btPointCollector.h | 64 - .../btPolyhedralContactClipping.cpp | 570 --- .../btPolyhedralContactClipping.h | 46 - .../btRaycastCallback.cpp | 178 - .../NarrowPhaseCollision/btRaycastCallback.h | 72 - .../btSimplexSolverInterface.h | 63 - .../btSubSimplexConvexCast.cpp | 160 - .../btSubSimplexConvexCast.h | 50 - .../btVoronoiSimplexSolver.cpp | 609 --- .../btVoronoiSimplexSolver.h | 181 - .../btCharacterControllerInterface.h | 47 - .../btKinematicCharacterController.cpp | 770 ---- .../btKinematicCharacterController.h | 170 - .../btConeTwistConstraint.cpp | 1141 ----- .../ConstraintSolver/btConeTwistConstraint.h | 381 -- .../ConstraintSolver/btConstraintSolver.h | 64 - .../ConstraintSolver/btContactConstraint.cpp | 178 - .../ConstraintSolver/btContactConstraint.h | 71 - .../ConstraintSolver/btContactSolverInfo.h | 159 - .../ConstraintSolver/btFixedConstraint.cpp | 129 - .../ConstraintSolver/btFixedConstraint.h | 49 - .../ConstraintSolver/btGearConstraint.cpp | 54 - .../ConstraintSolver/btGearConstraint.h | 152 - .../btGeneric6DofConstraint.cpp | 1063 ----- .../btGeneric6DofConstraint.h | 640 --- .../btGeneric6DofSpringConstraint.cpp | 185 - .../btGeneric6DofSpringConstraint.h | 121 - .../ConstraintSolver/btHinge2Constraint.cpp | 66 - .../ConstraintSolver/btHinge2Constraint.h | 60 - .../ConstraintSolver/btHingeConstraint.cpp | 1046 ----- .../ConstraintSolver/btHingeConstraint.h | 412 -- .../ConstraintSolver/btJacobianEntry.h | 155 - .../btPoint2PointConstraint.cpp | 229 -- .../btPoint2PointConstraint.h | 175 - .../btSequentialImpulseConstraintSolver.cpp | 1739 -------- .../btSequentialImpulseConstraintSolver.h | 148 - .../ConstraintSolver/btSliderConstraint.cpp | 864 ---- .../ConstraintSolver/btSliderConstraint.h | 361 -- .../btSolve2LinearConstraint.cpp | 255 -- .../btSolve2LinearConstraint.h | 107 - .../ConstraintSolver/btSolverBody.h | 306 -- .../ConstraintSolver/btSolverConstraint.h | 80 - .../ConstraintSolver/btTypedConstraint.cpp | 222 - .../ConstraintSolver/btTypedConstraint.h | 544 --- .../btUniversalConstraint.cpp | 87 - .../ConstraintSolver/btUniversalConstraint.h | 65 - .../BulletDynamics/Dynamics/Bullet-C-API.cpp | 405 -- .../Dynamics/btActionInterface.h | 46 - .../Dynamics/btDiscreteDynamicsWorld.cpp | 1459 ------- .../Dynamics/btDiscreteDynamicsWorld.h | 234 -- .../BulletDynamics/Dynamics/btDynamicsWorld.h | 167 - .../BulletDynamics/Dynamics/btRigidBody.cpp | 400 -- .../BulletDynamics/Dynamics/btRigidBody.h | 604 --- .../Dynamics/btSimpleDynamicsWorld.cpp | 280 -- .../Dynamics/btSimpleDynamicsWorld.h | 89 - .../Featherstone/btMultiBody.cpp | 1009 ----- .../BulletDynamics/Featherstone/btMultiBody.h | 466 --- .../Featherstone/btMultiBodyConstraint.cpp | 527 --- .../Featherstone/btMultiBodyConstraint.h | 166 - .../btMultiBodyConstraintSolver.cpp | 795 ---- .../btMultiBodyConstraintSolver.h | 85 - .../Featherstone/btMultiBodyDynamicsWorld.cpp | 578 --- .../Featherstone/btMultiBodyDynamicsWorld.h | 56 - .../btMultiBodyJointLimitConstraint.cpp | 133 - .../btMultiBodyJointLimitConstraint.h | 44 - .../Featherstone/btMultiBodyJointMotor.cpp | 89 - .../Featherstone/btMultiBodyJointMotor.h | 47 - .../Featherstone/btMultiBodyLink.h | 110 - .../Featherstone/btMultiBodyLinkCollider.h | 92 - .../Featherstone/btMultiBodyPoint2Point.cpp | 143 - .../Featherstone/btMultiBodyPoint2Point.h | 60 - .../btMultiBodySolverConstraint.h | 82 - .../MLCPSolvers/btDantzigLCP.cpp | 2079 ---------- .../BulletDynamics/MLCPSolvers/btDantzigLCP.h | 77 - .../MLCPSolvers/btDantzigSolver.h | 112 - .../MLCPSolvers/btMLCPSolver.cpp | 626 --- .../BulletDynamics/MLCPSolvers/btMLCPSolver.h | 81 - .../MLCPSolvers/btMLCPSolverInterface.h | 33 - .../BulletDynamics/MLCPSolvers/btPATHSolver.h | 151 - .../MLCPSolvers/btSolveProjectedGaussSeidel.h | 80 - .../Vehicle/btRaycastVehicle.cpp | 771 ---- .../BulletDynamics/Vehicle/btRaycastVehicle.h | 236 -- .../Vehicle/btVehicleRaycaster.h | 35 - .../BulletDynamics/Vehicle/btWheelInfo.cpp | 56 - .../BulletDynamics/Vehicle/btWheelInfo.h | 119 - .../btDefaultSoftBodySolver.cpp | 151 - .../BulletSoftBody/btDefaultSoftBodySolver.h | 63 - .../BULLET/BulletSoftBody/btSoftBody.cpp | 3655 ----------------- .../BULLET/BulletSoftBody/btSoftBody.h | 1000 ----- .../btSoftBodyConcaveCollisionAlgorithm.cpp | 357 -- .../btSoftBodyConcaveCollisionAlgorithm.h | 155 - .../BULLET/BulletSoftBody/btSoftBodyData.h | 217 - .../BulletSoftBody/btSoftBodyHelpers.cpp | 1055 ----- .../BULLET/BulletSoftBody/btSoftBodyHelpers.h | 143 - .../BulletSoftBody/btSoftBodyInternals.h | 908 ---- ...oftBodyRigidBodyCollisionConfiguration.cpp | 134 - ...tSoftBodyRigidBodyCollisionConfiguration.h | 48 - .../btSoftBodySolverVertexBuffer.h | 165 - .../BULLET/BulletSoftBody/btSoftBodySolvers.h | 154 - .../btSoftRigidCollisionAlgorithm.cpp | 86 - .../btSoftRigidCollisionAlgorithm.h | 75 - .../btSoftRigidDynamicsWorld.cpp | 367 -- .../BulletSoftBody/btSoftRigidDynamicsWorld.h | 107 - .../btSoftSoftCollisionAlgorithm.cpp | 48 - .../btSoftSoftCollisionAlgorithm.h | 69 - .../BULLET/BulletSoftBody/btSparseSDF.h | 319 -- WickedEngine/BULLET/CMakeLists.txt | 463 --- WickedEngine/BULLET/LinearMath/btAabbUtil2.h | 232 -- .../BULLET/LinearMath/btAlignedAllocator.cpp | 181 - .../BULLET/LinearMath/btAlignedAllocator.h | 107 - .../BULLET/LinearMath/btAlignedObjectArray.h | 511 --- .../BULLET/LinearMath/btConvexHull.cpp | 1167 ------ WickedEngine/BULLET/LinearMath/btConvexHull.h | 241 -- .../LinearMath/btConvexHullComputer.cpp | 2755 ------------- .../BULLET/LinearMath/btConvexHullComputer.h | 103 - .../BULLET/LinearMath/btDefaultMotionState.h | 42 - .../BULLET/LinearMath/btGeometryUtil.cpp | 185 - .../BULLET/LinearMath/btGeometryUtil.h | 42 - .../LinearMath/btGrahamScan2dConvexHull.h | 117 - WickedEngine/BULLET/LinearMath/btHashMap.h | 450 -- WickedEngine/BULLET/LinearMath/btIDebugDraw.h | 445 -- WickedEngine/BULLET/LinearMath/btList.h | 73 - WickedEngine/BULLET/LinearMath/btMatrix3x3.h | 1367 ------ WickedEngine/BULLET/LinearMath/btMatrixX.h | 504 --- WickedEngine/BULLET/LinearMath/btMinMax.h | 71 - .../BULLET/LinearMath/btMotionState.h | 40 - .../LinearMath/btPolarDecomposition.cpp | 99 - .../BULLET/LinearMath/btPolarDecomposition.h | 73 - .../BULLET/LinearMath/btPoolAllocator.h | 121 - WickedEngine/BULLET/LinearMath/btQuadWord.h | 244 -- WickedEngine/BULLET/LinearMath/btQuaternion.h | 909 ---- .../BULLET/LinearMath/btQuickprof.cpp | 566 --- WickedEngine/BULLET/LinearMath/btQuickprof.h | 203 - WickedEngine/BULLET/LinearMath/btRandom.h | 42 - WickedEngine/BULLET/LinearMath/btScalar.h | 735 ---- .../BULLET/LinearMath/btSerializer.cpp | 991 ----- WickedEngine/BULLET/LinearMath/btSerializer.h | 639 --- WickedEngine/BULLET/LinearMath/btStackAlloc.h | 116 - WickedEngine/BULLET/LinearMath/btTransform.h | 305 -- .../BULLET/LinearMath/btTransformUtil.h | 228 - WickedEngine/BULLET/LinearMath/btVector3.cpp | 1664 -------- WickedEngine/BULLET/LinearMath/btVector3.h | 1352 ------ WickedEngine/BULLET/btBulletCollisionCommon.h | 68 - WickedEngine/BULLET/btBulletDynamicsCommon.h | 51 - WickedEngine/CMakeLists.txt | 9 +- .../Jolt/AABBTree/AABBTreeBuilder.cpp | 239 ++ WickedEngine/Jolt/AABBTree/AABBTreeBuilder.h | 110 + WickedEngine/Jolt/AABBTree/AABBTreeToBuffer.h | 245 ++ .../NodeCodec/NodeCodecQuadTreeHalfFloat.h | 287 ++ .../TriangleCodecIndexed8BitPackSOA4Flags.h | 456 ++ WickedEngine/Jolt/ConfigurationString.h | 88 + WickedEngine/Jolt/Core/ARMNeon.h | 88 + WickedEngine/Jolt/Core/Array.h | 598 +++ WickedEngine/Jolt/Core/Atomics.h | 44 + WickedEngine/Jolt/Core/ByteBuffer.h | 74 + WickedEngine/Jolt/Core/Color.cpp | 38 + WickedEngine/Jolt/Core/Color.h | 84 + WickedEngine/Jolt/Core/Core.h | 577 +++ WickedEngine/Jolt/Core/FPControlWord.h | 135 + WickedEngine/Jolt/Core/FPException.h | 74 + WickedEngine/Jolt/Core/FPFlushDenormals.h | 41 + WickedEngine/Jolt/Core/Factory.cpp | 89 + WickedEngine/Jolt/Core/Factory.h | 54 + WickedEngine/Jolt/Core/FixedSizeFreeList.h | 122 + WickedEngine/Jolt/Core/FixedSizeFreeList.inl | 211 + WickedEngine/Jolt/Core/HashCombine.h | 97 + WickedEngine/Jolt/Core/InsertionSort.h | 58 + WickedEngine/Jolt/Core/IssueReporting.cpp | 31 + WickedEngine/Jolt/Core/IssueReporting.h | 38 + WickedEngine/Jolt/Core/JobSystem.h | 305 ++ WickedEngine/Jolt/Core/JobSystem.inl | 56 + .../Jolt/Core/JobSystemSingleThreaded.cpp | 65 + .../Jolt/Core/JobSystemSingleThreaded.h | 62 + .../Jolt/Core/JobSystemThreadPool.cpp | 360 ++ WickedEngine/Jolt/Core/JobSystemThreadPool.h | 101 + .../Jolt/Core/JobSystemWithBarrier.cpp | 227 + WickedEngine/Jolt/Core/JobSystemWithBarrier.h | 85 + WickedEngine/Jolt/Core/LinearCurve.cpp | 51 + WickedEngine/Jolt/Core/LinearCurve.h | 67 + WickedEngine/Jolt/Core/LockFreeHashMap.h | 182 + WickedEngine/Jolt/Core/LockFreeHashMap.inl | 351 ++ WickedEngine/Jolt/Core/Memory.cpp | 85 + WickedEngine/Jolt/Core/Memory.h | 58 + WickedEngine/Jolt/Core/Mutex.h | 223 + WickedEngine/Jolt/Core/MutexArray.h | 98 + WickedEngine/Jolt/Core/NonCopyable.h | 18 + WickedEngine/Jolt/Core/Profiler.cpp | 346 ++ WickedEngine/Jolt/Core/Profiler.h | 284 ++ WickedEngine/Jolt/Core/Profiler.inl | 89 + WickedEngine/Jolt/Core/QuickSort.h | 137 + WickedEngine/Jolt/Core/RTTI.cpp | 149 + WickedEngine/Jolt/Core/RTTI.h | 436 ++ WickedEngine/Jolt/Core/Reference.h | 226 + WickedEngine/Jolt/Core/Result.h | 177 + WickedEngine/Jolt/Core/STLAlignedAllocator.h | 72 + WickedEngine/Jolt/Core/STLAllocator.h | 127 + WickedEngine/Jolt/Core/STLTempAllocator.h | 80 + WickedEngine/Jolt/Core/ScopeExit.h | 49 + WickedEngine/Jolt/Core/Semaphore.cpp | 80 + WickedEngine/Jolt/Core/Semaphore.h | 51 + WickedEngine/Jolt/Core/StaticArray.h | 325 ++ WickedEngine/Jolt/Core/StreamIn.h | 119 + WickedEngine/Jolt/Core/StreamOut.h | 97 + WickedEngine/Jolt/Core/StreamUtils.h | 167 + WickedEngine/Jolt/Core/StreamWrapper.h | 53 + WickedEngine/Jolt/Core/StringTools.cpp | 101 + WickedEngine/Jolt/Core/StringTools.h | 51 + WickedEngine/Jolt/Core/TempAllocator.h | 122 + WickedEngine/Jolt/Core/TickCounter.cpp | 34 + WickedEngine/Jolt/Core/TickCounter.h | 49 + WickedEngine/Jolt/Core/UnorderedMap.h | 15 + WickedEngine/Jolt/Core/UnorderedSet.h | 15 + WickedEngine/Jolt/Geometry/AABox.h | 304 ++ WickedEngine/Jolt/Geometry/AABox4.h | 224 + WickedEngine/Jolt/Geometry/ClipPoly.h | 200 + WickedEngine/Jolt/Geometry/ClosestPoint.h | 498 +++ .../Jolt/Geometry/ConvexHullBuilder.cpp | 1464 +++++++ .../Jolt/Geometry/ConvexHullBuilder.h | 276 ++ .../Jolt/Geometry/ConvexHullBuilder2D.cpp | 335 ++ .../Jolt/Geometry/ConvexHullBuilder2D.h | 105 + WickedEngine/Jolt/Geometry/ConvexSupport.h | 188 + .../Jolt/Geometry/EPAConvexHullBuilder.h | 844 ++++ .../Jolt/Geometry/EPAPenetrationDepth.h | 555 +++ WickedEngine/Jolt/Geometry/Ellipse.h | 77 + WickedEngine/Jolt/Geometry/GJKClosestPoint.h | 952 +++++ WickedEngine/Jolt/Geometry/IndexedTriangle.h | 115 + WickedEngine/Jolt/Geometry/Indexify.cpp | 218 + WickedEngine/Jolt/Geometry/Indexify.h | 19 + WickedEngine/Jolt/Geometry/MortonCode.h | 40 + WickedEngine/Jolt/Geometry/OrientedBox.cpp | 178 + WickedEngine/Jolt/Geometry/OrientedBox.h | 39 + WickedEngine/Jolt/Geometry/Plane.h | 86 + WickedEngine/Jolt/Geometry/RayAABox.h | 241 ++ WickedEngine/Jolt/Geometry/RayAABox8.h | 76 + WickedEngine/Jolt/Geometry/RayCapsule.h | 37 + WickedEngine/Jolt/Geometry/RayCylinder.h | 101 + WickedEngine/Jolt/Geometry/RaySphere.h | 96 + WickedEngine/Jolt/Geometry/RayTriangle.h | 158 + WickedEngine/Jolt/Geometry/RayTriangle8.h | 91 + WickedEngine/Jolt/Geometry/Sphere.h | 72 + WickedEngine/Jolt/Geometry/Triangle.h | 34 + WickedEngine/Jolt/Jolt.cmake | 660 +++ WickedEngine/Jolt/Jolt.h | 16 + WickedEngine/Jolt/Jolt.natvis | 97 + WickedEngine/Jolt/Math/DMat44.h | 158 + WickedEngine/Jolt/Math/DMat44.inl | 310 ++ WickedEngine/Jolt/Math/DVec3.h | 288 ++ WickedEngine/Jolt/Math/DVec3.inl | 930 +++++ WickedEngine/Jolt/Math/Double3.h | 48 + WickedEngine/Jolt/Math/DynMatrix.h | 31 + WickedEngine/Jolt/Math/EigenValueSymmetric.h | 175 + WickedEngine/Jolt/Math/FindRoot.h | 42 + WickedEngine/Jolt/Math/Float2.h | 36 + WickedEngine/Jolt/Math/Float3.h | 50 + WickedEngine/Jolt/Math/Float4.h | 33 + WickedEngine/Jolt/Math/GaussianElimination.h | 102 + WickedEngine/Jolt/Math/HalfFloat.h | 204 + WickedEngine/Jolt/Math/Mat44.h | 243 ++ WickedEngine/Jolt/Math/Mat44.inl | 952 +++++ WickedEngine/Jolt/Math/Math.h | 203 + WickedEngine/Jolt/Math/MathTypes.h | 34 + WickedEngine/Jolt/Math/Matrix.h | 259 ++ WickedEngine/Jolt/Math/Quat.h | 255 ++ WickedEngine/Jolt/Math/Quat.inl | 328 ++ WickedEngine/Jolt/Math/Real.h | 44 + WickedEngine/Jolt/Math/Swizzle.h | 19 + WickedEngine/Jolt/Math/Trigonometry.h | 79 + WickedEngine/Jolt/Math/UVec4.h | 220 + WickedEngine/Jolt/Math/UVec4.inl | 573 +++ WickedEngine/Jolt/Math/UVec8.h | 100 + WickedEngine/Jolt/Math/UVec8.inl | 138 + WickedEngine/Jolt/Math/Vec3.cpp | 71 + WickedEngine/Jolt/Math/Vec3.h | 295 ++ WickedEngine/Jolt/Math/Vec3.inl | 853 ++++ WickedEngine/Jolt/Math/Vec4.h | 283 ++ WickedEngine/Jolt/Math/Vec4.inl | 978 +++++ WickedEngine/Jolt/Math/Vec8.h | 112 + WickedEngine/Jolt/Math/Vec8.inl | 148 + WickedEngine/Jolt/Math/Vector.h | 211 + .../ObjectStream/GetPrimitiveTypeOfType.h | 54 + .../Jolt/ObjectStream/ObjectStream.cpp | 38 + WickedEngine/Jolt/ObjectStream/ObjectStream.h | 333 ++ .../ObjectStream/ObjectStreamBinaryIn.cpp | 234 ++ .../Jolt/ObjectStream/ObjectStreamBinaryIn.h | 55 + .../ObjectStream/ObjectStreamBinaryOut.cpp | 155 + .../Jolt/ObjectStream/ObjectStreamBinaryOut.h | 55 + .../Jolt/ObjectStream/ObjectStreamIn.cpp | 621 +++ .../Jolt/ObjectStream/ObjectStreamIn.h | 148 + .../Jolt/ObjectStream/ObjectStreamOut.cpp | 168 + .../Jolt/ObjectStream/ObjectStreamOut.h | 104 + .../Jolt/ObjectStream/ObjectStreamTextIn.cpp | 396 ++ .../Jolt/ObjectStream/ObjectStreamTextIn.h | 53 + .../Jolt/ObjectStream/ObjectStreamTextOut.cpp | 231 ++ .../Jolt/ObjectStream/ObjectStreamTextOut.h | 60 + .../Jolt/ObjectStream/ObjectStreamTypes.h | 24 + .../Jolt/ObjectStream/SerializableAttribute.h | 111 + .../ObjectStream/SerializableAttributeEnum.h | 67 + .../ObjectStream/SerializableAttributeTyped.h | 60 + .../Jolt/ObjectStream/SerializableObject.cpp | 15 + .../Jolt/ObjectStream/SerializableObject.h | 164 + .../Jolt/ObjectStream/TypeDeclarations.cpp | 55 + .../Jolt/ObjectStream/TypeDeclarations.h | 41 + WickedEngine/Jolt/Physics/Body/AllowedDOFs.h | 68 + WickedEngine/Jolt/Physics/Body/Body.cpp | 413 ++ WickedEngine/Jolt/Physics/Body/Body.h | 388 ++ WickedEngine/Jolt/Physics/Body/Body.inl | 197 + WickedEngine/Jolt/Physics/Body/BodyAccess.cpp | 18 + WickedEngine/Jolt/Physics/Body/BodyAccess.h | 55 + .../Physics/Body/BodyActivationListener.h | 28 + .../Physics/Body/BodyCreationSettings.cpp | 234 ++ .../Jolt/Physics/Body/BodyCreationSettings.h | 124 + WickedEngine/Jolt/Physics/Body/BodyFilter.h | 102 + WickedEngine/Jolt/Physics/Body/BodyID.h | 100 + .../Jolt/Physics/Body/BodyInterface.cpp | 1034 +++++ .../Jolt/Physics/Body/BodyInterface.h | 283 ++ WickedEngine/Jolt/Physics/Body/BodyLock.h | 111 + .../Jolt/Physics/Body/BodyLockInterface.h | 134 + .../Jolt/Physics/Body/BodyLockMulti.h | 104 + .../Jolt/Physics/Body/BodyManager.cpp | 1152 ++++++ WickedEngine/Jolt/Physics/Body/BodyManager.h | 377 ++ WickedEngine/Jolt/Physics/Body/BodyPair.h | 36 + WickedEngine/Jolt/Physics/Body/BodyType.h | 19 + .../Jolt/Physics/Body/MassProperties.cpp | 185 + .../Jolt/Physics/Body/MassProperties.h | 58 + .../Jolt/Physics/Body/MotionProperties.cpp | 90 + .../Jolt/Physics/Body/MotionProperties.h | 278 ++ .../Jolt/Physics/Body/MotionProperties.inl | 168 + .../Jolt/Physics/Body/MotionQuality.h | 31 + WickedEngine/Jolt/Physics/Body/MotionType.h | 17 + .../Jolt/Physics/Character/Character.cpp | 318 ++ .../Jolt/Physics/Character/Character.h | 140 + .../Jolt/Physics/Character/CharacterBase.cpp | 59 + .../Jolt/Physics/Character/CharacterBase.h | 157 + .../Physics/Character/CharacterVirtual.cpp | 1581 +++++++ .../Jolt/Physics/Character/CharacterVirtual.h | 507 +++ .../Jolt/Physics/Collision/AABoxCast.h | 20 + .../Jolt/Physics/Collision/ActiveEdgeMode.h | 17 + .../Jolt/Physics/Collision/ActiveEdges.h | 114 + .../Jolt/Physics/Collision/BackFaceMode.h | 16 + .../Collision/BroadPhase/BroadPhase.cpp | 16 + .../Physics/Collision/BroadPhase/BroadPhase.h | 112 + .../BroadPhase/BroadPhaseBruteForce.cpp | 313 ++ .../BroadPhase/BroadPhaseBruteForce.h | 38 + .../Collision/BroadPhase/BroadPhaseLayer.h | 148 + .../BroadPhase/BroadPhaseLayerInterfaceMask.h | 92 + .../BroadPhaseLayerInterfaceTable.h | 64 + .../BroadPhase/BroadPhaseQuadTree.cpp | 609 +++ .../Collision/BroadPhase/BroadPhaseQuadTree.h | 108 + .../Collision/BroadPhase/BroadPhaseQuery.h | 53 + .../ObjectVsBroadPhaseLayerFilterMask.h | 35 + .../ObjectVsBroadPhaseLayerFilterTable.h | 66 + .../Physics/Collision/BroadPhase/QuadTree.cpp | 1686 ++++++++ .../Physics/Collision/BroadPhase/QuadTree.h | 390 ++ .../Collision/CastConvexVsTriangles.cpp | 109 + .../Physics/Collision/CastConvexVsTriangles.h | 46 + .../Jolt/Physics/Collision/CastResult.h | 37 + .../Collision/CastSphereVsTriangles.cpp | 223 + .../Physics/Collision/CastSphereVsTriangles.h | 49 + .../Jolt/Physics/Collision/CollectFacesMode.h | 16 + .../Collision/CollideConvexVsTriangles.cpp | 150 + .../Collision/CollideConvexVsTriangles.h | 56 + .../Physics/Collision/CollidePointResult.h | 25 + .../Jolt/Physics/Collision/CollideShape.h | 105 + .../CollideSoftBodyVerticesVsTriangles.h | 98 + .../Collision/CollideSphereVsTriangles.cpp | 123 + .../Collision/CollideSphereVsTriangles.h | 50 + .../Physics/Collision/CollisionCollector.h | 102 + .../Collision/CollisionCollectorImpl.h | 134 + .../Physics/Collision/CollisionDispatch.cpp | 107 + .../Physics/Collision/CollisionDispatch.h | 97 + .../Jolt/Physics/Collision/CollisionGroup.cpp | 33 + .../Jolt/Physics/Collision/CollisionGroup.h | 94 + .../Jolt/Physics/Collision/ContactListener.h | 114 + .../Collision/EstimateCollisionResponse.cpp | 213 + .../Collision/EstimateCollisionResponse.h | 48 + .../Jolt/Physics/Collision/GroupFilter.cpp | 32 + .../Jolt/Physics/Collision/GroupFilter.h | 41 + .../Physics/Collision/GroupFilterTable.cpp | 38 + .../Jolt/Physics/Collision/GroupFilterTable.h | 130 + .../Collision/InternalEdgeRemovingCollector.h | 237 ++ .../Collision/ManifoldBetweenTwoFaces.cpp | 237 ++ .../Collision/ManifoldBetweenTwoFaces.h | 44 + .../Physics/Collision/NarrowPhaseQuery.cpp | 412 ++ .../Jolt/Physics/Collision/NarrowPhaseQuery.h | 74 + .../Physics/Collision/NarrowPhaseStats.cpp | 62 + .../Jolt/Physics/Collision/NarrowPhaseStats.h | 110 + .../Jolt/Physics/Collision/ObjectLayer.h | 111 + .../Collision/ObjectLayerPairFilterMask.h | 52 + .../Collision/ObjectLayerPairFilterTable.h | 78 + .../Physics/Collision/PhysicsMaterial.cpp | 35 + .../Jolt/Physics/Collision/PhysicsMaterial.h | 52 + .../Collision/PhysicsMaterialSimple.cpp | 38 + .../Physics/Collision/PhysicsMaterialSimple.h | 37 + WickedEngine/Jolt/Physics/Collision/RayCast.h | 81 + .../Jolt/Physics/Collision/Shape/BoxShape.cpp | 318 ++ .../Jolt/Physics/Collision/Shape/BoxShape.h | 115 + .../Physics/Collision/Shape/CapsuleShape.cpp | 446 ++ .../Physics/Collision/Shape/CapsuleShape.h | 128 + .../Physics/Collision/Shape/CompoundShape.cpp | 398 ++ .../Physics/Collision/Shape/CompoundShape.h | 344 ++ .../Collision/Shape/CompoundShapeVisitors.h | 460 +++ .../Collision/Shape/ConvexHullShape.cpp | 1311 ++++++ .../Physics/Collision/Shape/ConvexHullShape.h | 202 + .../Physics/Collision/Shape/ConvexShape.cpp | 559 +++ .../Physics/Collision/Shape/ConvexShape.h | 150 + .../Physics/Collision/Shape/CylinderShape.cpp | 417 ++ .../Physics/Collision/Shape/CylinderShape.h | 126 + .../Collision/Shape/DecoratedShape.cpp | 87 + .../Physics/Collision/Shape/DecoratedShape.h | 70 + .../Collision/Shape/GetTrianglesContext.h | 248 ++ .../Collision/Shape/HeightFieldShape.cpp | 2700 ++++++++++++ .../Collision/Shape/HeightFieldShape.h | 373 ++ .../Physics/Collision/Shape/MeshShape.cpp | 1244 ++++++ .../Jolt/Physics/Collision/Shape/MeshShape.h | 208 + .../Collision/Shape/MutableCompoundShape.cpp | 574 +++ .../Collision/Shape/MutableCompoundShape.h | 169 + .../Shape/OffsetCenterOfMassShape.cpp | 217 + .../Collision/Shape/OffsetCenterOfMassShape.h | 143 + .../PolyhedronSubmergedVolumeCalculator.h | 319 ++ .../Shape/RotatedTranslatedShape.cpp | 315 ++ .../Collision/Shape/RotatedTranslatedShape.h | 158 + .../Physics/Collision/Shape/ScaleHelpers.h | 68 + .../Physics/Collision/Shape/ScaledShape.cpp | 226 + .../Physics/Collision/Shape/ScaledShape.h | 140 + .../Jolt/Physics/Collision/Shape/Shape.cpp | 309 ++ .../Jolt/Physics/Collision/Shape/Shape.h | 447 ++ .../Physics/Collision/Shape/SphereShape.cpp | 352 ++ .../Physics/Collision/Shape/SphereShape.h | 125 + .../Collision/Shape/StaticCompoundShape.cpp | 674 +++ .../Collision/Shape/StaticCompoundShape.h | 139 + .../Jolt/Physics/Collision/Shape/SubShapeID.h | 138 + .../Physics/Collision/Shape/SubShapeIDPair.h | 80 + .../Collision/Shape/TaperedCapsuleShape.cpp | 458 +++ .../Shape/TaperedCapsuleShape.gliffy | 1 + .../Collision/Shape/TaperedCapsuleShape.h | 125 + .../Physics/Collision/Shape/TriangleShape.cpp | 413 ++ .../Physics/Collision/Shape/TriangleShape.h | 138 + .../Jolt/Physics/Collision/ShapeCast.h | 170 + .../Jolt/Physics/Collision/ShapeFilter.h | 72 + .../Physics/Collision/SortReverseAndStore.h | 48 + .../Physics/Collision/TransformedShape.cpp | 180 + .../Jolt/Physics/Collision/TransformedShape.h | 194 + .../Constraints/CalculateSolverSteps.h | 66 + .../Physics/Constraints/ConeConstraint.cpp | 246 ++ .../Jolt/Physics/Constraints/ConeConstraint.h | 133 + .../Jolt/Physics/Constraints/Constraint.cpp | 73 + .../Jolt/Physics/Constraints/Constraint.h | 238 ++ .../Physics/Constraints/ConstraintManager.cpp | 289 ++ .../Physics/Constraints/ConstraintManager.h | 99 + .../ConstraintPart/AngleConstraintPart.h | 257 ++ .../ConstraintPart/AxisConstraintPart.h | 682 +++ .../ConstraintPart/DualAxisConstraintPart.h | 276 ++ .../ConstraintPart/GearConstraintPart.h | 195 + .../HingeRotationConstraintPart.h | 222 + .../IndependentAxisConstraintPart.h | 246 ++ .../ConstraintPart/PointConstraintPart.h | 239 ++ .../RackAndPinionConstraintPart.h | 196 + .../RotationEulerConstraintPart.h | 270 ++ .../RotationQuatConstraintPart.h | 246 ++ .../Constraints/ConstraintPart/SpringPart.h | 169 + .../ConstraintPart/SwingTwistConstraintPart.h | 597 +++ .../Constraints/ContactConstraintManager.cpp | 1718 ++++++++ .../Constraints/ContactConstraintManager.h | 513 +++ .../Constraints/DistanceConstraint.cpp | 266 ++ .../Physics/Constraints/DistanceConstraint.h | 120 + .../Physics/Constraints/FixedConstraint.cpp | 215 + .../Physics/Constraints/FixedConstraint.h | 96 + .../Physics/Constraints/GearConstraint.cpp | 188 + .../Jolt/Physics/Constraints/GearConstraint.h | 116 + .../Physics/Constraints/HingeConstraint.cpp | 424 ++ .../Physics/Constraints/HingeConstraint.h | 182 + .../Physics/Constraints/MotorSettings.cpp | 43 + .../Jolt/Physics/Constraints/MotorSettings.h | 66 + .../Physics/Constraints/PathConstraint.cpp | 458 +++ .../Jolt/Physics/Constraints/PathConstraint.h | 186 + .../Constraints/PathConstraintPath.cpp | 85 + .../Physics/Constraints/PathConstraintPath.h | 71 + .../Constraints/PathConstraintPathHermite.cpp | 308 ++ .../Constraints/PathConstraintPathHermite.h | 54 + .../Physics/Constraints/PointConstraint.cpp | 157 + .../Physics/Constraints/PointConstraint.h | 94 + .../Physics/Constraints/PulleyConstraint.cpp | 253 ++ .../Physics/Constraints/PulleyConstraint.h | 137 + .../Constraints/RackAndPinionConstraint.cpp | 189 + .../Constraints/RackAndPinionConstraint.h | 118 + .../Physics/Constraints/SixDOFConstraint.cpp | 900 ++++ .../Physics/Constraints/SixDOFConstraint.h | 289 ++ .../Physics/Constraints/SliderConstraint.cpp | 501 +++ .../Physics/Constraints/SliderConstraint.h | 198 + .../Physics/Constraints/SpringSettings.cpp | 35 + .../Jolt/Physics/Constraints/SpringSettings.h | 70 + .../Constraints/SwingTwistConstraint.cpp | 524 +++ .../Constraints/SwingTwistConstraint.h | 197 + .../Physics/Constraints/TwoBodyConstraint.cpp | 56 + .../Physics/Constraints/TwoBodyConstraint.h | 65 + WickedEngine/Jolt/Physics/DeterminismLog.cpp | 17 + WickedEngine/Jolt/Physics/DeterminismLog.h | 159 + WickedEngine/Jolt/Physics/EActivation.h | 16 + .../Jolt/Physics/EPhysicsUpdateError.h | 37 + WickedEngine/Jolt/Physics/IslandBuilder.cpp | 484 +++ WickedEngine/Jolt/Physics/IslandBuilder.h | 125 + .../Jolt/Physics/LargeIslandSplitter.cpp | 579 +++ .../Jolt/Physics/LargeIslandSplitter.h | 185 + WickedEngine/Jolt/Physics/PhysicsLock.cpp | 17 + WickedEngine/Jolt/Physics/PhysicsLock.h | 169 + WickedEngine/Jolt/Physics/PhysicsScene.cpp | 261 ++ WickedEngine/Jolt/Physics/PhysicsScene.h | 104 + WickedEngine/Jolt/Physics/PhysicsSettings.h | 119 + .../Jolt/Physics/PhysicsStepListener.h | 27 + WickedEngine/Jolt/Physics/PhysicsSystem.cpp | 2702 ++++++++++++ WickedEngine/Jolt/Physics/PhysicsSystem.h | 320 ++ .../Jolt/Physics/PhysicsUpdateContext.cpp | 23 + .../Jolt/Physics/PhysicsUpdateContext.h | 172 + WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.cpp | 705 ++++ WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.h | 240 ++ .../SoftBody/SoftBodyContactListener.h | 55 + .../SoftBody/SoftBodyCreationSettings.cpp | 122 + .../SoftBody/SoftBodyCreationSettings.h | 73 + .../Jolt/Physics/SoftBody/SoftBodyManifold.h | 59 + .../SoftBody/SoftBodyMotionProperties.cpp | 1206 ++++++ .../SoftBody/SoftBodyMotionProperties.h | 267 ++ .../Jolt/Physics/SoftBody/SoftBodyShape.cpp | 338 ++ .../Jolt/Physics/SoftBody/SoftBodyShape.h | 73 + .../SoftBody/SoftBodySharedSettings.cpp | 1023 +++++ .../Physics/SoftBody/SoftBodySharedSettings.h | 335 ++ .../Physics/SoftBody/SoftBodyUpdateContext.h | 54 + .../Jolt/Physics/SoftBody/SoftBodyVertex.h | 28 + WickedEngine/Jolt/Physics/StateRecorder.h | 66 + .../Jolt/Physics/StateRecorderImpl.cpp | 90 + WickedEngine/Jolt/Physics/StateRecorderImpl.h | 47 + .../Physics/Vehicle/MotorcycleController.cpp | 293 ++ .../Physics/Vehicle/MotorcycleController.h | 116 + .../Vehicle/TrackedVehicleController.cpp | 531 +++ .../Vehicle/TrackedVehicleController.h | 166 + .../Physics/Vehicle/VehicleAntiRollBar.cpp | 33 + .../Jolt/Physics/Vehicle/VehicleAntiRollBar.h | 31 + .../Vehicle/VehicleCollisionTester.cpp | 376 ++ .../Physics/Vehicle/VehicleCollisionTester.h | 146 + .../Physics/Vehicle/VehicleConstraint.cpp | 697 ++++ .../Jolt/Physics/Vehicle/VehicleConstraint.h | 246 ++ .../Physics/Vehicle/VehicleController.cpp | 17 + .../Jolt/Physics/Vehicle/VehicleController.h | 80 + .../Physics/Vehicle/VehicleDifferential.cpp | 81 + .../Physics/Vehicle/VehicleDifferential.h | 39 + .../Jolt/Physics/Vehicle/VehicleEngine.cpp | 122 + .../Jolt/Physics/Vehicle/VehicleEngine.h | 93 + .../Jolt/Physics/Vehicle/VehicleTrack.cpp | 52 + .../Jolt/Physics/Vehicle/VehicleTrack.h | 56 + .../Physics/Vehicle/VehicleTransmission.cpp | 159 + .../Physics/Vehicle/VehicleTransmission.h | 87 + WickedEngine/Jolt/Physics/Vehicle/Wheel.cpp | 93 + WickedEngine/Jolt/Physics/Vehicle/Wheel.h | 148 + .../Vehicle/WheeledVehicleController.cpp | 845 ++++ .../Vehicle/WheeledVehicleController.h | 199 + WickedEngine/Jolt/RegisterTypes.cpp | 198 + WickedEngine/Jolt/RegisterTypes.h | 29 + WickedEngine/Jolt/Renderer/DebugRenderer.cpp | 1071 +++++ WickedEngine/Jolt/Renderer/DebugRenderer.h | 347 ++ .../Jolt/Renderer/DebugRendererPlayback.cpp | 168 + .../Jolt/Renderer/DebugRendererPlayback.h | 48 + .../Jolt/Renderer/DebugRendererRecorder.cpp | 158 + .../Jolt/Renderer/DebugRendererRecorder.h | 130 + .../Jolt/Renderer/DebugRendererSimple.cpp | 80 + .../Jolt/Renderer/DebugRendererSimple.h | 88 + .../Jolt/Skeleton/SkeletalAnimation.cpp | 110 + .../Jolt/Skeleton/SkeletalAnimation.h | 77 + WickedEngine/Jolt/Skeleton/Skeleton.cpp | 82 + WickedEngine/Jolt/Skeleton/Skeleton.h | 72 + WickedEngine/Jolt/Skeleton/SkeletonMapper.cpp | 237 ++ WickedEngine/Jolt/Skeleton/SkeletonMapper.h | 145 + WickedEngine/Jolt/Skeleton/SkeletonPose.cpp | 87 + WickedEngine/Jolt/Skeleton/SkeletonPose.h | 82 + .../Jolt/TriangleGrouper/TriangleGrouper.h | 27 + .../TriangleGrouperClosestCentroid.cpp | 95 + .../TriangleGrouperClosestCentroid.h | 21 + .../TriangleGrouper/TriangleGrouperMorton.cpp | 49 + .../TriangleGrouper/TriangleGrouperMorton.h | 20 + .../TriangleSplitter/TriangleSplitter.cpp | 67 + .../Jolt/TriangleSplitter/TriangleSplitter.h | 84 + .../TriangleSplitterBinning.cpp | 112 + .../TriangleSplitterBinning.h | 52 + .../TriangleSplitterFixedLeafSize.cpp | 170 + .../TriangleSplitterFixedLeafSize.h | 55 + .../TriangleSplitterLongestAxis.cpp | 31 + .../TriangleSplitterLongestAxis.h | 28 + .../TriangleSplitter/TriangleSplitterMean.cpp | 40 + .../TriangleSplitter/TriangleSplitterMean.h | 28 + .../TriangleSplitterMorton.cpp | 63 + .../TriangleSplitter/TriangleSplitterMorton.h | 32 + WickedEngine/WickedEngine_SOURCE.vcxitems | 796 ++-- .../WickedEngine_SOURCE.vcxitems.filters | 2400 ++++++----- WickedEngine/WickedEngine_Windows.vcxproj | 4 +- WickedEngine/wiPhysics_Bullet.cpp | 1873 --------- WickedEngine/wiPhysics_Jolt.cpp | 1950 +++++++++ WickedEngine/wiScene.h | 2 +- WickedEngine/wiScene_Components.h | 12 +- WickedEngine/wiScene_Serializers.cpp | 6 + WickedEngine/wiVersion.cpp | 2 +- third_party_software.txt | 22 +- 812 files changed, 99430 insertions(+), 109133 deletions(-) delete mode 100644 WickedEngine/BULLET/Bullet-C-Api.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.h delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionConfiguration.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionMargin.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMaterial.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleBuffer.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleBuffer.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleInfoMap.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMesh.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMesh.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Doxyfile delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btBoxCollision.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btClipPolygon.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btCompoundFromGimpact.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactMassUtil.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGeometryOperations.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btQuantization.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_array.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_basic_geometry_operations.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_bitset.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_collision.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_clip_polygon.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_geom_types.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_geometry.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_hash_table.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_linear_math.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_math.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_radixsort.h delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPointCollector.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp delete mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConstraintSolver.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactSolverInfo.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btJacobianEntry.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverBody.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/Bullet-C-API.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btActionInterface.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btDynamicsWorld.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyLink.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigSolver.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolver.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btPATHSolver.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btVehicleRaycaster.h delete mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.cpp delete mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBody.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBody.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyData.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyInternals.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodySolverVertexBuffer.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodySolvers.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.h delete mode 100644 WickedEngine/BULLET/BulletSoftBody/btSparseSDF.h delete mode 100644 WickedEngine/BULLET/CMakeLists.txt delete mode 100644 WickedEngine/BULLET/LinearMath/btAabbUtil2.h delete mode 100644 WickedEngine/BULLET/LinearMath/btAlignedAllocator.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btAlignedAllocator.h delete mode 100644 WickedEngine/BULLET/LinearMath/btAlignedObjectArray.h delete mode 100644 WickedEngine/BULLET/LinearMath/btConvexHull.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btConvexHull.h delete mode 100644 WickedEngine/BULLET/LinearMath/btConvexHullComputer.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btConvexHullComputer.h delete mode 100644 WickedEngine/BULLET/LinearMath/btDefaultMotionState.h delete mode 100644 WickedEngine/BULLET/LinearMath/btGeometryUtil.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btGeometryUtil.h delete mode 100644 WickedEngine/BULLET/LinearMath/btGrahamScan2dConvexHull.h delete mode 100644 WickedEngine/BULLET/LinearMath/btHashMap.h delete mode 100644 WickedEngine/BULLET/LinearMath/btIDebugDraw.h delete mode 100644 WickedEngine/BULLET/LinearMath/btList.h delete mode 100644 WickedEngine/BULLET/LinearMath/btMatrix3x3.h delete mode 100644 WickedEngine/BULLET/LinearMath/btMatrixX.h delete mode 100644 WickedEngine/BULLET/LinearMath/btMinMax.h delete mode 100644 WickedEngine/BULLET/LinearMath/btMotionState.h delete mode 100644 WickedEngine/BULLET/LinearMath/btPolarDecomposition.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btPolarDecomposition.h delete mode 100644 WickedEngine/BULLET/LinearMath/btPoolAllocator.h delete mode 100644 WickedEngine/BULLET/LinearMath/btQuadWord.h delete mode 100644 WickedEngine/BULLET/LinearMath/btQuaternion.h delete mode 100644 WickedEngine/BULLET/LinearMath/btQuickprof.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btQuickprof.h delete mode 100644 WickedEngine/BULLET/LinearMath/btRandom.h delete mode 100644 WickedEngine/BULLET/LinearMath/btScalar.h delete mode 100644 WickedEngine/BULLET/LinearMath/btSerializer.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btSerializer.h delete mode 100644 WickedEngine/BULLET/LinearMath/btStackAlloc.h delete mode 100644 WickedEngine/BULLET/LinearMath/btTransform.h delete mode 100644 WickedEngine/BULLET/LinearMath/btTransformUtil.h delete mode 100644 WickedEngine/BULLET/LinearMath/btVector3.cpp delete mode 100644 WickedEngine/BULLET/LinearMath/btVector3.h delete mode 100644 WickedEngine/BULLET/btBulletCollisionCommon.h delete mode 100644 WickedEngine/BULLET/btBulletDynamicsCommon.h create mode 100644 WickedEngine/Jolt/AABBTree/AABBTreeBuilder.cpp create mode 100644 WickedEngine/Jolt/AABBTree/AABBTreeBuilder.h create mode 100644 WickedEngine/Jolt/AABBTree/AABBTreeToBuffer.h create mode 100644 WickedEngine/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h create mode 100644 WickedEngine/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h create mode 100644 WickedEngine/Jolt/ConfigurationString.h create mode 100644 WickedEngine/Jolt/Core/ARMNeon.h create mode 100644 WickedEngine/Jolt/Core/Array.h create mode 100644 WickedEngine/Jolt/Core/Atomics.h create mode 100644 WickedEngine/Jolt/Core/ByteBuffer.h create mode 100644 WickedEngine/Jolt/Core/Color.cpp create mode 100644 WickedEngine/Jolt/Core/Color.h create mode 100644 WickedEngine/Jolt/Core/Core.h create mode 100644 WickedEngine/Jolt/Core/FPControlWord.h create mode 100644 WickedEngine/Jolt/Core/FPException.h create mode 100644 WickedEngine/Jolt/Core/FPFlushDenormals.h create mode 100644 WickedEngine/Jolt/Core/Factory.cpp create mode 100644 WickedEngine/Jolt/Core/Factory.h create mode 100644 WickedEngine/Jolt/Core/FixedSizeFreeList.h create mode 100644 WickedEngine/Jolt/Core/FixedSizeFreeList.inl create mode 100644 WickedEngine/Jolt/Core/HashCombine.h create mode 100644 WickedEngine/Jolt/Core/InsertionSort.h create mode 100644 WickedEngine/Jolt/Core/IssueReporting.cpp create mode 100644 WickedEngine/Jolt/Core/IssueReporting.h create mode 100644 WickedEngine/Jolt/Core/JobSystem.h create mode 100644 WickedEngine/Jolt/Core/JobSystem.inl create mode 100644 WickedEngine/Jolt/Core/JobSystemSingleThreaded.cpp create mode 100644 WickedEngine/Jolt/Core/JobSystemSingleThreaded.h create mode 100644 WickedEngine/Jolt/Core/JobSystemThreadPool.cpp create mode 100644 WickedEngine/Jolt/Core/JobSystemThreadPool.h create mode 100644 WickedEngine/Jolt/Core/JobSystemWithBarrier.cpp create mode 100644 WickedEngine/Jolt/Core/JobSystemWithBarrier.h create mode 100644 WickedEngine/Jolt/Core/LinearCurve.cpp create mode 100644 WickedEngine/Jolt/Core/LinearCurve.h create mode 100644 WickedEngine/Jolt/Core/LockFreeHashMap.h create mode 100644 WickedEngine/Jolt/Core/LockFreeHashMap.inl create mode 100644 WickedEngine/Jolt/Core/Memory.cpp create mode 100644 WickedEngine/Jolt/Core/Memory.h create mode 100644 WickedEngine/Jolt/Core/Mutex.h create mode 100644 WickedEngine/Jolt/Core/MutexArray.h create mode 100644 WickedEngine/Jolt/Core/NonCopyable.h create mode 100644 WickedEngine/Jolt/Core/Profiler.cpp create mode 100644 WickedEngine/Jolt/Core/Profiler.h create mode 100644 WickedEngine/Jolt/Core/Profiler.inl create mode 100644 WickedEngine/Jolt/Core/QuickSort.h create mode 100644 WickedEngine/Jolt/Core/RTTI.cpp create mode 100644 WickedEngine/Jolt/Core/RTTI.h create mode 100644 WickedEngine/Jolt/Core/Reference.h create mode 100644 WickedEngine/Jolt/Core/Result.h create mode 100644 WickedEngine/Jolt/Core/STLAlignedAllocator.h create mode 100644 WickedEngine/Jolt/Core/STLAllocator.h create mode 100644 WickedEngine/Jolt/Core/STLTempAllocator.h create mode 100644 WickedEngine/Jolt/Core/ScopeExit.h create mode 100644 WickedEngine/Jolt/Core/Semaphore.cpp create mode 100644 WickedEngine/Jolt/Core/Semaphore.h create mode 100644 WickedEngine/Jolt/Core/StaticArray.h create mode 100644 WickedEngine/Jolt/Core/StreamIn.h create mode 100644 WickedEngine/Jolt/Core/StreamOut.h create mode 100644 WickedEngine/Jolt/Core/StreamUtils.h create mode 100644 WickedEngine/Jolt/Core/StreamWrapper.h create mode 100644 WickedEngine/Jolt/Core/StringTools.cpp create mode 100644 WickedEngine/Jolt/Core/StringTools.h create mode 100644 WickedEngine/Jolt/Core/TempAllocator.h create mode 100644 WickedEngine/Jolt/Core/TickCounter.cpp create mode 100644 WickedEngine/Jolt/Core/TickCounter.h create mode 100644 WickedEngine/Jolt/Core/UnorderedMap.h create mode 100644 WickedEngine/Jolt/Core/UnorderedSet.h create mode 100644 WickedEngine/Jolt/Geometry/AABox.h create mode 100644 WickedEngine/Jolt/Geometry/AABox4.h create mode 100644 WickedEngine/Jolt/Geometry/ClipPoly.h create mode 100644 WickedEngine/Jolt/Geometry/ClosestPoint.h create mode 100644 WickedEngine/Jolt/Geometry/ConvexHullBuilder.cpp create mode 100644 WickedEngine/Jolt/Geometry/ConvexHullBuilder.h create mode 100644 WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.cpp create mode 100644 WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.h create mode 100644 WickedEngine/Jolt/Geometry/ConvexSupport.h create mode 100644 WickedEngine/Jolt/Geometry/EPAConvexHullBuilder.h create mode 100644 WickedEngine/Jolt/Geometry/EPAPenetrationDepth.h create mode 100644 WickedEngine/Jolt/Geometry/Ellipse.h create mode 100644 WickedEngine/Jolt/Geometry/GJKClosestPoint.h create mode 100644 WickedEngine/Jolt/Geometry/IndexedTriangle.h create mode 100644 WickedEngine/Jolt/Geometry/Indexify.cpp create mode 100644 WickedEngine/Jolt/Geometry/Indexify.h create mode 100644 WickedEngine/Jolt/Geometry/MortonCode.h create mode 100644 WickedEngine/Jolt/Geometry/OrientedBox.cpp create mode 100644 WickedEngine/Jolt/Geometry/OrientedBox.h create mode 100644 WickedEngine/Jolt/Geometry/Plane.h create mode 100644 WickedEngine/Jolt/Geometry/RayAABox.h create mode 100644 WickedEngine/Jolt/Geometry/RayAABox8.h create mode 100644 WickedEngine/Jolt/Geometry/RayCapsule.h create mode 100644 WickedEngine/Jolt/Geometry/RayCylinder.h create mode 100644 WickedEngine/Jolt/Geometry/RaySphere.h create mode 100644 WickedEngine/Jolt/Geometry/RayTriangle.h create mode 100644 WickedEngine/Jolt/Geometry/RayTriangle8.h create mode 100644 WickedEngine/Jolt/Geometry/Sphere.h create mode 100644 WickedEngine/Jolt/Geometry/Triangle.h create mode 100644 WickedEngine/Jolt/Jolt.cmake create mode 100644 WickedEngine/Jolt/Jolt.h create mode 100644 WickedEngine/Jolt/Jolt.natvis create mode 100644 WickedEngine/Jolt/Math/DMat44.h create mode 100644 WickedEngine/Jolt/Math/DMat44.inl create mode 100644 WickedEngine/Jolt/Math/DVec3.h create mode 100644 WickedEngine/Jolt/Math/DVec3.inl create mode 100644 WickedEngine/Jolt/Math/Double3.h create mode 100644 WickedEngine/Jolt/Math/DynMatrix.h create mode 100644 WickedEngine/Jolt/Math/EigenValueSymmetric.h create mode 100644 WickedEngine/Jolt/Math/FindRoot.h create mode 100644 WickedEngine/Jolt/Math/Float2.h create mode 100644 WickedEngine/Jolt/Math/Float3.h create mode 100644 WickedEngine/Jolt/Math/Float4.h create mode 100644 WickedEngine/Jolt/Math/GaussianElimination.h create mode 100644 WickedEngine/Jolt/Math/HalfFloat.h create mode 100644 WickedEngine/Jolt/Math/Mat44.h create mode 100644 WickedEngine/Jolt/Math/Mat44.inl create mode 100644 WickedEngine/Jolt/Math/Math.h create mode 100644 WickedEngine/Jolt/Math/MathTypes.h create mode 100644 WickedEngine/Jolt/Math/Matrix.h create mode 100644 WickedEngine/Jolt/Math/Quat.h create mode 100644 WickedEngine/Jolt/Math/Quat.inl create mode 100644 WickedEngine/Jolt/Math/Real.h create mode 100644 WickedEngine/Jolt/Math/Swizzle.h create mode 100644 WickedEngine/Jolt/Math/Trigonometry.h create mode 100644 WickedEngine/Jolt/Math/UVec4.h create mode 100644 WickedEngine/Jolt/Math/UVec4.inl create mode 100644 WickedEngine/Jolt/Math/UVec8.h create mode 100644 WickedEngine/Jolt/Math/UVec8.inl create mode 100644 WickedEngine/Jolt/Math/Vec3.cpp create mode 100644 WickedEngine/Jolt/Math/Vec3.h create mode 100644 WickedEngine/Jolt/Math/Vec3.inl create mode 100644 WickedEngine/Jolt/Math/Vec4.h create mode 100644 WickedEngine/Jolt/Math/Vec4.inl create mode 100644 WickedEngine/Jolt/Math/Vec8.h create mode 100644 WickedEngine/Jolt/Math/Vec8.inl create mode 100644 WickedEngine/Jolt/Math/Vector.h create mode 100644 WickedEngine/Jolt/ObjectStream/GetPrimitiveTypeOfType.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStream.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStream.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamIn.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamIn.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamOut.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamOut.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.h create mode 100644 WickedEngine/Jolt/ObjectStream/ObjectStreamTypes.h create mode 100644 WickedEngine/Jolt/ObjectStream/SerializableAttribute.h create mode 100644 WickedEngine/Jolt/ObjectStream/SerializableAttributeEnum.h create mode 100644 WickedEngine/Jolt/ObjectStream/SerializableAttributeTyped.h create mode 100644 WickedEngine/Jolt/ObjectStream/SerializableObject.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/SerializableObject.h create mode 100644 WickedEngine/Jolt/ObjectStream/TypeDeclarations.cpp create mode 100644 WickedEngine/Jolt/ObjectStream/TypeDeclarations.h create mode 100644 WickedEngine/Jolt/Physics/Body/AllowedDOFs.h create mode 100644 WickedEngine/Jolt/Physics/Body/Body.cpp create mode 100644 WickedEngine/Jolt/Physics/Body/Body.h create mode 100644 WickedEngine/Jolt/Physics/Body/Body.inl create mode 100644 WickedEngine/Jolt/Physics/Body/BodyAccess.cpp create mode 100644 WickedEngine/Jolt/Physics/Body/BodyAccess.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyActivationListener.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyCreationSettings.cpp create mode 100644 WickedEngine/Jolt/Physics/Body/BodyCreationSettings.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyFilter.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyID.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyInterface.cpp create mode 100644 WickedEngine/Jolt/Physics/Body/BodyInterface.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyLock.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyLockInterface.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyLockMulti.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyManager.cpp create mode 100644 WickedEngine/Jolt/Physics/Body/BodyManager.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyPair.h create mode 100644 WickedEngine/Jolt/Physics/Body/BodyType.h create mode 100644 WickedEngine/Jolt/Physics/Body/MassProperties.cpp create mode 100644 WickedEngine/Jolt/Physics/Body/MassProperties.h create mode 100644 WickedEngine/Jolt/Physics/Body/MotionProperties.cpp create mode 100644 WickedEngine/Jolt/Physics/Body/MotionProperties.h create mode 100644 WickedEngine/Jolt/Physics/Body/MotionProperties.inl create mode 100644 WickedEngine/Jolt/Physics/Body/MotionQuality.h create mode 100644 WickedEngine/Jolt/Physics/Body/MotionType.h create mode 100644 WickedEngine/Jolt/Physics/Character/Character.cpp create mode 100644 WickedEngine/Jolt/Physics/Character/Character.h create mode 100644 WickedEngine/Jolt/Physics/Character/CharacterBase.cpp create mode 100644 WickedEngine/Jolt/Physics/Character/CharacterBase.h create mode 100644 WickedEngine/Jolt/Physics/Character/CharacterVirtual.cpp create mode 100644 WickedEngine/Jolt/Physics/Character/CharacterVirtual.h create mode 100644 WickedEngine/Jolt/Physics/Collision/AABoxCast.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ActiveEdgeMode.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ActiveEdges.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BackFaceMode.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CastResult.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollectFacesMode.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollidePointResult.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollideShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollisionCollector.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollisionCollectorImpl.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollisionDispatch.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/CollisionDispatch.h create mode 100644 WickedEngine/Jolt/Physics/Collision/CollisionGroup.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/CollisionGroup.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ContactListener.h create mode 100644 WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.h create mode 100644 WickedEngine/Jolt/Physics/Collision/GroupFilter.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/GroupFilter.h create mode 100644 WickedEngine/Jolt/Physics/Collision/GroupFilterTable.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/GroupFilterTable.h create mode 100644 WickedEngine/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h create mode 100644 WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.h create mode 100644 WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ObjectLayer.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h create mode 100644 WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.h create mode 100644 WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.h create mode 100644 WickedEngine/Jolt/Physics/Collision/RayCast.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/GetTrianglesContext.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/ScaleHelpers.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/Shape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/Shape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/SubShapeID.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/SubShapeIDPair.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ShapeCast.h create mode 100644 WickedEngine/Jolt/Physics/Collision/ShapeFilter.h create mode 100644 WickedEngine/Jolt/Physics/Collision/SortReverseAndStore.h create mode 100644 WickedEngine/Jolt/Physics/Collision/TransformedShape.cpp create mode 100644 WickedEngine/Jolt/Physics/Collision/TransformedShape.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/CalculateSolverSteps.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConeConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConeConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/Constraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/Constraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintManager.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintManager.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/FixedConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/FixedConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/GearConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/GearConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/HingeConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/HingeConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/MotorSettings.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/MotorSettings.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/PathConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/PathConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/PointConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/PointConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/SliderConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/SliderConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/SpringSettings.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/SpringSettings.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.h create mode 100644 WickedEngine/Jolt/Physics/DeterminismLog.cpp create mode 100644 WickedEngine/Jolt/Physics/DeterminismLog.h create mode 100644 WickedEngine/Jolt/Physics/EActivation.h create mode 100644 WickedEngine/Jolt/Physics/EPhysicsUpdateError.h create mode 100644 WickedEngine/Jolt/Physics/IslandBuilder.cpp create mode 100644 WickedEngine/Jolt/Physics/IslandBuilder.h create mode 100644 WickedEngine/Jolt/Physics/LargeIslandSplitter.cpp create mode 100644 WickedEngine/Jolt/Physics/LargeIslandSplitter.h create mode 100644 WickedEngine/Jolt/Physics/PhysicsLock.cpp create mode 100644 WickedEngine/Jolt/Physics/PhysicsLock.h create mode 100644 WickedEngine/Jolt/Physics/PhysicsScene.cpp create mode 100644 WickedEngine/Jolt/Physics/PhysicsScene.h create mode 100644 WickedEngine/Jolt/Physics/PhysicsSettings.h create mode 100644 WickedEngine/Jolt/Physics/PhysicsStepListener.h create mode 100644 WickedEngine/Jolt/Physics/PhysicsSystem.cpp create mode 100644 WickedEngine/Jolt/Physics/PhysicsSystem.h create mode 100644 WickedEngine/Jolt/Physics/PhysicsUpdateContext.cpp create mode 100644 WickedEngine/Jolt/Physics/PhysicsUpdateContext.h create mode 100644 WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.cpp create mode 100644 WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyContactListener.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyManifold.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.cpp create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h create mode 100644 WickedEngine/Jolt/Physics/SoftBody/SoftBodyVertex.h create mode 100644 WickedEngine/Jolt/Physics/StateRecorder.h create mode 100644 WickedEngine/Jolt/Physics/StateRecorderImpl.cpp create mode 100644 WickedEngine/Jolt/Physics/StateRecorderImpl.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleController.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleController.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/Wheel.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/Wheel.h create mode 100644 WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.cpp create mode 100644 WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.h create mode 100644 WickedEngine/Jolt/RegisterTypes.cpp create mode 100644 WickedEngine/Jolt/RegisterTypes.h create mode 100644 WickedEngine/Jolt/Renderer/DebugRenderer.cpp create mode 100644 WickedEngine/Jolt/Renderer/DebugRenderer.h create mode 100644 WickedEngine/Jolt/Renderer/DebugRendererPlayback.cpp create mode 100644 WickedEngine/Jolt/Renderer/DebugRendererPlayback.h create mode 100644 WickedEngine/Jolt/Renderer/DebugRendererRecorder.cpp create mode 100644 WickedEngine/Jolt/Renderer/DebugRendererRecorder.h create mode 100644 WickedEngine/Jolt/Renderer/DebugRendererSimple.cpp create mode 100644 WickedEngine/Jolt/Renderer/DebugRendererSimple.h create mode 100644 WickedEngine/Jolt/Skeleton/SkeletalAnimation.cpp create mode 100644 WickedEngine/Jolt/Skeleton/SkeletalAnimation.h create mode 100644 WickedEngine/Jolt/Skeleton/Skeleton.cpp create mode 100644 WickedEngine/Jolt/Skeleton/Skeleton.h create mode 100644 WickedEngine/Jolt/Skeleton/SkeletonMapper.cpp create mode 100644 WickedEngine/Jolt/Skeleton/SkeletonMapper.h create mode 100644 WickedEngine/Jolt/Skeleton/SkeletonPose.cpp create mode 100644 WickedEngine/Jolt/Skeleton/SkeletonPose.h create mode 100644 WickedEngine/Jolt/TriangleGrouper/TriangleGrouper.h create mode 100644 WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp create mode 100644 WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h create mode 100644 WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp create mode 100644 WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.h create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.cpp create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.h create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.h create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.cpp create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.h create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp create mode 100644 WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.h delete mode 100644 WickedEngine/wiPhysics_Bullet.cpp create mode 100644 WickedEngine/wiPhysics_Jolt.cpp diff --git a/Editor/RigidBodyWindow.cpp b/Editor/RigidBodyWindow.cpp index f38d1109c..520c407ff 100644 --- a/Editor/RigidBodyWindow.cpp +++ b/Editor/RigidBodyWindow.cpp @@ -68,7 +68,6 @@ void RigidBodyWindow::Create(EditorComponent* _editor) switch (shape) { case RigidBodyPhysicsComponent::CollisionShape::BOX: - case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: XSlider.SetEnabled(true); YSlider.SetEnabled(true); ZSlider.SetEnabled(true); @@ -87,11 +86,7 @@ void RigidBodyWindow::Create(EditorComponent* _editor) XSlider.SetValue(physicscomponent->sphere.radius); break; case RigidBodyPhysicsComponent::CollisionShape::CAPSULE: - if (physicscomponent->shape != RigidBodyPhysicsComponent::CollisionShape::CAPSULE) - { - physicscomponent->physicsobject = nullptr; - physicscomponent->shape = RigidBodyPhysicsComponent::CollisionShape::CAPSULE; - } + case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: XSlider.SetEnabled(true); YSlider.SetEnabled(true); XSlider.SetText("Height"); @@ -122,13 +117,13 @@ void RigidBodyWindow::Create(EditorComponent* _editor) { default: case RigidBodyPhysicsComponent::CollisionShape::BOX: - case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: physicscomponent->box.halfextents.x = args.fValue; break; case RigidBodyPhysicsComponent::CollisionShape::SPHERE: physicscomponent->sphere.radius = args.fValue; break; case RigidBodyPhysicsComponent::CollisionShape::CAPSULE: + case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: physicscomponent->capsule.height = args.fValue; break; } @@ -148,10 +143,10 @@ void RigidBodyWindow::Create(EditorComponent* _editor) { default: case RigidBodyPhysicsComponent::CollisionShape::BOX: - case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: physicscomponent->box.halfextents.y = args.fValue; break; case RigidBodyPhysicsComponent::CollisionShape::CAPSULE: + case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: physicscomponent->capsule.radius = args.fValue; break; } @@ -207,7 +202,6 @@ void RigidBodyWindow::Create(EditorComponent* _editor) if (physicscomponent != nullptr) { physicscomponent->friction = args.fValue; - physicscomponent->physicsobject = {}; } }); AddWidget(&frictionSlider); @@ -221,7 +215,6 @@ void RigidBodyWindow::Create(EditorComponent* _editor) if (physicscomponent != nullptr) { physicscomponent->restitution = args.fValue; - physicscomponent->physicsobject = {}; } }); AddWidget(&restitutionSlider); diff --git a/Editor/SoftBodyWindow.cpp b/Editor/SoftBodyWindow.cpp index 7e638fc8b..e550353c9 100644 --- a/Editor/SoftBodyWindow.cpp +++ b/Editor/SoftBodyWindow.cpp @@ -57,7 +57,6 @@ void SoftBodyWindow::Create(EditorComponent* _editor) SoftBodyPhysicsComponent* physicscomponent = editor->GetCurrentScene().softbodies.GetComponent(entity); if (physicscomponent != nullptr) { - physicscomponent->physicsobject = {}; physicscomponent->friction = args.fValue; } }); @@ -71,7 +70,6 @@ void SoftBodyWindow::Create(EditorComponent* _editor) SoftBodyPhysicsComponent* physicscomponent = editor->GetCurrentScene().softbodies.GetComponent(entity); if (physicscomponent != nullptr) { - physicscomponent->physicsobject = {}; physicscomponent->restitution = args.fValue; } }); diff --git a/WickedEngine/BULLET/Bullet-C-Api.h b/WickedEngine/BULLET/Bullet-C-Api.h deleted file mode 100644 index f27a17d51..000000000 --- a/WickedEngine/BULLET/Bullet-C-Api.h +++ /dev/null @@ -1,176 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* - Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's. - Work in progress, functionality will be added on demand. - - If possible, use the richer Bullet C++ API, by including "btBulletDynamicsCommon.h" -*/ - -#ifndef BULLET_C_API_H -#define BULLET_C_API_H - -#define PL_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name - -#ifdef BT_USE_DOUBLE_PRECISION -typedef double plReal; -#else -typedef float plReal; -#endif - -typedef plReal plVector3[3]; -typedef plReal plQuaternion[4]; - -#ifdef __cplusplus -extern "C" { -#endif - -/** Particular physics SDK (C-API) */ - PL_DECLARE_HANDLE(plPhysicsSdkHandle); - -/** Dynamics world, belonging to some physics SDK (C-API)*/ - PL_DECLARE_HANDLE(plDynamicsWorldHandle); - -/** Rigid Body that can be part of a Dynamics World (C-API)*/ - PL_DECLARE_HANDLE(plRigidBodyHandle); - -/** Collision Shape/Geometry, property of a Rigid Body (C-API)*/ - PL_DECLARE_HANDLE(plCollisionShapeHandle); - -/** Constraint for Rigid Bodies (C-API)*/ - PL_DECLARE_HANDLE(plConstraintHandle); - -/** Triangle Mesh interface (C-API)*/ - PL_DECLARE_HANDLE(plMeshInterfaceHandle); - -/** Broadphase Scene/Proxy Handles (C-API)*/ - PL_DECLARE_HANDLE(plCollisionBroadphaseHandle); - PL_DECLARE_HANDLE(plBroadphaseProxyHandle); - PL_DECLARE_HANDLE(plCollisionWorldHandle); - -/** - Create and Delete a Physics SDK -*/ - - extern plPhysicsSdkHandle plNewBulletSdk(void); //this could be also another sdk, like ODE, PhysX etc. - extern void plDeletePhysicsSdk(plPhysicsSdkHandle physicsSdk); - -/** Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */ - - typedef void(*btBroadphaseCallback)(void* clientData, void* object1,void* object2); - - extern plCollisionBroadphaseHandle plCreateSapBroadphase(btBroadphaseCallback beginCallback,btBroadphaseCallback endCallback); - - extern void plDestroyBroadphase(plCollisionBroadphaseHandle bp); - - extern plBroadphaseProxyHandle plCreateProxy(plCollisionBroadphaseHandle bp, void* clientData, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ); - - extern void plDestroyProxy(plCollisionBroadphaseHandle bp, plBroadphaseProxyHandle proxyHandle); - - extern void plSetBoundingBox(plBroadphaseProxyHandle proxyHandle, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ); - -/* todo: add pair cache support with queries like add/remove/find pair */ - - extern plCollisionWorldHandle plCreateCollisionWorld(plPhysicsSdkHandle physicsSdk); - -/* todo: add/remove objects */ - - -/* Dynamics World */ - - extern plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdk); - - extern void plDeleteDynamicsWorld(plDynamicsWorldHandle world); - - extern void plStepSimulation(plDynamicsWorldHandle, plReal timeStep); - - extern void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object); - - extern void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object); - - -/* Rigid Body */ - - extern plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionShapeHandle cshape ); - - extern void plDeleteRigidBody(plRigidBodyHandle body); - - -/* Collision Shape definition */ - - extern plCollisionShapeHandle plNewSphereShape(plReal radius); - extern plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z); - extern plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height); - extern plCollisionShapeHandle plNewConeShape(plReal radius, plReal height); - extern plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height); - extern plCollisionShapeHandle plNewCompoundShape(void); - extern void plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn); - - extern void plDeleteShape(plCollisionShapeHandle shape); - - /* Convex Meshes */ - extern plCollisionShapeHandle plNewConvexHullShape(void); - extern void plAddVertex(plCollisionShapeHandle convexHull, plReal x,plReal y,plReal z); -/* Concave static triangle meshes */ - extern plMeshInterfaceHandle plNewMeshInterface(void); - extern void plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2); - extern plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle); - - extern void plSetScaling(plCollisionShapeHandle shape, plVector3 scaling); - -/* SOLID has Response Callback/Table/Management */ -/* PhysX has Triggers, User Callbacks and filtering */ -/* ODE has the typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); */ - -/* typedef void plUpdatedPositionCallback(void* userData, plRigidBodyHandle rbHandle, plVector3 pos); */ -/* typedef void plUpdatedOrientationCallback(void* userData, plRigidBodyHandle rbHandle, plQuaternion orientation); */ - - /* get world transform */ - extern void plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix); - extern void plGetPosition(plRigidBodyHandle object,plVector3 position); - extern void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation); - - /* set world transform (position/orientation) */ - extern void plSetPosition(plRigidBodyHandle object, const plVector3 position); - extern void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation); - extern void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient); - extern void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix); - - typedef struct plRayCastResult { - plRigidBodyHandle m_body; - plCollisionShapeHandle m_shape; - plVector3 m_positionWorld; - plVector3 m_normalWorld; - } plRayCastResult; - - extern int plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plRayCastResult res); - - /* Sweep API */ - - /* extern plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); */ - - /* Continuous Collision Detection API */ - - // needed for source/blender/blenkernel/intern/collision.c - double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]); - -#ifdef __cplusplus -} -#endif - - -#endif //BULLET_C_API_H - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp deleted file mode 100644 index 77763305b..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ /dev/null @@ -1,37 +0,0 @@ - -//Bullet Continuous Collision Detection and Physics Library -//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - - -// -// btAxisSweep3 -// -// Copyright (c) 2006 Simon Hobbs -// -// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -// -// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: -// -// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -// -// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -// -// 3. This notice may not be removed or altered from any source distribution. -#include "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) -{ - // 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) -{ - // 1 handle is reserved as sentinel - btAssert(maxHandles > 1 && maxHandles < 2147483647); -} diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.h deleted file mode 100644 index cd6e1a892..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ /dev/null @@ -1,1051 +0,0 @@ -//Bullet Continuous Collision Detection and Physics Library -//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -// -// btAxisSweep3.h -// -// Copyright (c) 2006 Simon Hobbs -// -// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -// -// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: -// -// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -// -// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -// -// 3. This notice may not be removed or altered from any source distribution. - -#ifndef BT_AXIS_SWEEP_3_H -#define BT_AXIS_SWEEP_3_H - -#include "LinearMath/btVector3.h" -#include "btOverlappingPairCache.h" -#include "btBroadphaseInterface.h" -#include "btBroadphaseProxy.h" -#include "btOverlappingPairCallback.h" -#include "btDbvtBroadphase.h" - -//#define DEBUG_BROADPHASE 1 -#define USE_OVERLAP_TEST_ON_REMOVES 1 - -/// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase. -/// It uses quantized integers to represent the begin and end points for each of the 3 axis. -/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead. -template -class btAxisSweep3Internal : public btBroadphaseInterface -{ -protected: - - BP_FP_INT_TYPE m_bpHandleMask; - BP_FP_INT_TYPE m_handleSentinel; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - class Edge - { - public: - BP_FP_INT_TYPE m_pos; // low bit is min/max - BP_FP_INT_TYPE m_handle; - - BP_FP_INT_TYPE IsMax() const {return static_cast(m_pos & 1);} - }; - -public: - class Handle : public btBroadphaseProxy - { - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - // indexes into the edge arrays - BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 -// BP_FP_INT_TYPE m_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 - - -protected: - btVector3 m_worldAabbMin; // overall system bounds - btVector3 m_worldAabbMax; // overall system bounds - - 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 - - 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; - - ///additional dynamic aabb structure, used to accelerate ray cast queries. - ///can be disabled using a optional argument in the constructor - btDbvtBroadphase* m_raycastAccelerator; - btOverlappingPairCache* m_nullPairCache; - - - // allocation/deallocation - BP_FP_INT_TYPE allocHandle(); - void freeHandle(BP_FP_INT_TYPE handle); - - - 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 - - //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 ); - -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); - - 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,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); - void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher); - void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} - - virtual void resetPool(btDispatcher* dispatcher); - - void processAllOverlappingPairs(btOverlapCallback* callback); - - //Broadphase Interface - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); - 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); - - btOverlappingPairCache* getOverlappingPairCache() - { - return m_pairCache; - } - const btOverlappingPairCache* getOverlappingPairCache() const - { - return m_pairCache; - } - - void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback) - { - m_userPairCallback = pairCallback; - } - const btOverlappingPairCallback* getOverlappingPairUserCallback() const - { - return m_userPairCallback; - } - - ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame - ///will add some transform later - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin = m_worldAabbMin; - aabbMax = m_worldAabbMax; - } - - virtual void printStats() - { -/* printf("btAxisSweep3.h\n"); - printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); - printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(), - m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ()); - */ - - } - -}; - -//////////////////////////////////////////////////////////////////// - - - - -#ifdef DEBUG_BROADPHASE -#include - -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); - - int i; - for (i=0;im_handle); - int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; - char beginOrEnd; - beginOrEnd=pEdge->IsMax()?'E':'B'; - printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex); - } - - if (checkCardinality) - btAssert(numEdges == m_numHandles*2+1); -} -#endif //DEBUG_BROADPHASE - -template -btBroadphaseProxy* btAxisSweep3Internal::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) -{ - (void)shapeType; - BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy); - - Handle* handle = getHandle(handleId); - - if (m_raycastAccelerator) - { - btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0); - handle->m_dbvtProxy = rayProxy; - } - return handle; -} - - - -template -void btAxisSweep3Internal::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - Handle* handle = static_cast(proxy); - if (m_raycastAccelerator) - m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher); - removeHandle(static_cast(handle->m_uniqueId), dispatcher); -} - -template -void btAxisSweep3Internal::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) -{ - Handle* handle = static_cast(proxy); - handle->m_aabbMin = aabbMin; - handle->m_aabbMax = aabbMax; - updateHandle(static_cast(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); - if (m_raycastAccelerator) - m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher); - -} - -template -void btAxisSweep3Internal::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) -{ - if (m_raycastAccelerator) - { - m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax); - } else - { - //choose axis? - BP_FP_INT_TYPE axis = 0; - //for each proxy - for (BP_FP_INT_TYPE i=1;i -void btAxisSweep3Internal::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) -{ - if (m_raycastAccelerator) - { - 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)) - { - callback.process(handle); - } - } - } - } -} - - - -template -void btAxisSweep3Internal::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - Handle* pHandle = static_cast(proxy); - aabbMin = pHandle->m_aabbMin; - aabbMax = pHandle->m_aabbMax; -} - - -template -void btAxisSweep3Internal::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - Handle* pHandle = static_cast(proxy); - - unsigned short vecInMin[3]; - unsigned short vecInMax[3]; - - vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ; - vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ; - vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ; - vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ; - vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ; - vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ; - - aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ())); - aabbMin += m_worldAabbMin; - - aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ())); - aabbMax += m_worldAabbMin; -} - - - - -template -btAxisSweep3Internal::btAxisSweep3Internal(const 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 - - if (!m_pairCache) - { - 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 - } - - //btAssert(bounds.HasVolume()); - - // init bounds - m_worldAabbMin = worldAabbMin; - m_worldAabbMax = worldAabbMax; - - btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; - - BP_FP_INT_TYPE maxInt = m_handleSentinel; - - m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; - - // allocate handles buffer, using btAlignedAlloc, and put all handles on free list - m_pHandles = new Handle[maxHandles]; - - m_maxHandles = maxHandles; - m_numHandles = 0; - - // handle 0 is reserved as the null index, and is also used as the sentinel - m_firstFreeHandle = 1; - { - for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) - m_pHandles[i].SetNextFree(static_cast(i + 1)); - m_pHandles[maxHandles - 1].SetNextFree(0); - } - - { - // allocate edge buffers - for (int i = 0; i < 3; i++) - { - m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16); - m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2]; - } - } - //removed overlap management - - // make boundary sentinels - - m_pHandles[0].m_clientObject = 0; - - for (int axis = 0; axis < 3; axis++) - { - m_pHandles[0].m_minEdges[axis] = 0; - m_pHandles[0].m_maxEdges[axis] = 1; - - m_pEdges[axis][0].m_pos = 0; - m_pEdges[axis][0].m_handle = 0; - m_pEdges[axis][1].m_pos = m_handleSentinel; - m_pEdges[axis][1].m_handle = 0; -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - - } - -} - -template -btAxisSweep3Internal::~btAxisSweep3Internal() -{ - if (m_raycastAccelerator) - { - m_nullPairCache->~btOverlappingPairCache(); - btAlignedFree(m_nullPairCache); - m_raycastAccelerator->~btDbvtBroadphase(); - btAlignedFree (m_raycastAccelerator); - } - - for (int i = 2; i >= 0; i--) - { - btAlignedFree(m_pEdgesRawPtr[i]); - } - delete [] m_pHandles; - - if (m_ownsPairCache) - { - m_pairCache->~btOverlappingPairCache(); - btAlignedFree(m_pairCache); - } -} - -template -void btAxisSweep3Internal::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const -{ -#ifdef OLD_CLAMPING_METHOD - ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax] - ///see http://code.google.com/p/bullet/issues/detail?id=87 - btVector3 clampedPoint(point); - clampedPoint.setMax(m_worldAabbMin); - clampedPoint.setMin(m_worldAabbMax); - btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; - out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax); - out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax); - out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax); -#else - btVector3 v = (point - m_worldAabbMin) * m_quantize; - out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax); - out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax); - out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax); -#endif //OLD_CLAMPING_METHOD -} - - -template -BP_FP_INT_TYPE btAxisSweep3Internal::allocHandle() -{ - btAssert(m_firstFreeHandle); - - BP_FP_INT_TYPE handle = m_firstFreeHandle; - m_firstFreeHandle = getHandle(handle)->GetNextFree(); - m_numHandles++; - - return handle; -} - -template -void btAxisSweep3Internal::freeHandle(BP_FP_INT_TYPE handle) -{ - btAssert(handle > 0 && handle < m_maxHandles); - - getHandle(handle)->SetNextFree(m_firstFreeHandle); - m_firstFreeHandle = handle; - - m_numHandles--; -} - - -template -BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) -{ - // quantize the bounds - BP_FP_INT_TYPE min[3], max[3]; - quantize(min, aabbMin, 0); - quantize(max, aabbMax, 1); - - // allocate a handle - BP_FP_INT_TYPE handle = allocHandle(); - - - Handle* pHandle = getHandle(handle); - - pHandle->m_uniqueId = static_cast(handle); - //pHandle->m_pOverlaps = 0; - pHandle->m_clientObject = pOwner; - pHandle->m_collisionFilterGroup = collisionFilterGroup; - pHandle->m_collisionFilterMask = collisionFilterMask; - pHandle->m_multiSapParentProxy = multiSapProxy; - - // compute current limit of edge arrays - BP_FP_INT_TYPE limit = static_cast(m_numHandles * 2); - - - // insert new edges just inside the max boundary edge - for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++) - { - - m_pHandles[0].m_maxEdges[axis] += 2; - - m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; - - m_pEdges[axis][limit - 1].m_pos = min[axis]; - m_pEdges[axis][limit - 1].m_handle = handle; - - m_pEdges[axis][limit].m_pos = max[axis]; - m_pEdges[axis][limit].m_handle = handle; - - pHandle->m_minEdges[axis] = static_cast(limit - 1); - pHandle->m_maxEdges[axis] = limit; - } - - // now sort the new edges to their correct position - sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false); - sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false); - sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false); - sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false); - sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true); - sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true); - - - return handle; -} - - -template -void btAxisSweep3Internal::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher) -{ - - Handle* pHandle = getHandle(handle); - - //explicitly remove the pairs containing the proxy - //we could do it also in the sortMinUp (passing true) - ///@todo: compare performance - if (!m_pairCache->hasDeferredRemoval()) - { - m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); - } - - // compute current limit of edge arrays - int limit = static_cast(m_numHandles * 2); - - int axis; - - for (axis = 0;axis<3;axis++) - { - m_pHandles[0].m_maxEdges[axis] -= 2; - } - - // remove the edges by sorting them up to the end of the list - for ( axis = 0; axis < 3; axis++) - { - Edge* pEdges = m_pEdges[axis]; - BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; - pEdges[max].m_pos = m_handleSentinel; - - sortMaxUp(axis,max,dispatcher,false); - - - BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; - pEdges[i].m_pos = m_handleSentinel; - - - sortMinUp(axis,i,dispatcher,false); - - pEdges[limit-1].m_handle = 0; - pEdges[limit-1].m_pos = m_handleSentinel; - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis,false); -#endif //DEBUG_BROADPHASE - - - } - - - // free the handle - freeHandle(handle); - - -} - -template -void btAxisSweep3Internal::resetPool(btDispatcher* /*dispatcher*/) -{ - if (m_numHandles == 0) - { - m_firstFreeHandle = 1; - { - for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++) - m_pHandles[i].SetNextFree(static_cast(i + 1)); - m_pHandles[m_maxHandles - 1].SetNextFree(0); - } - } -} - - -extern int gOverlappingPairs; -//#include - -template -void btAxisSweep3Internal::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - - if (m_pairCache->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - - - int i; - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;iprocessOverlap(pair); - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - m_pairCache->cleanOverlappingPair(pair,dispatcher); - - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - m_invalidPair++; - gOverlappingPairs--; - } - - } - - ///if you don't like to skip the invalid pairs in the array, execute following code: - #define CLEAN_INVALID_PAIRS 1 - #ifdef CLEAN_INVALID_PAIRS - - //perform a sort, to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS - - //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); - } - -} - - -template -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; - } - } - return true; -} - -template -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] || - 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; - } - return true; -} - -template -void btAxisSweep3Internal::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) -{ -// btAssert(bounds.IsFinite()); - //btAssert(bounds.HasVolume()); - - Handle* pHandle = getHandle(handle); - - // quantize the new bounds - BP_FP_INT_TYPE min[3], max[3]; - quantize(min, aabbMin, 0); - quantize(max, aabbMax, 1); - - // update changed edges - for (int axis = 0; axis < 3; axis++) - { - BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis]; - BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis]; - - int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos; - int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos; - - m_pEdges[axis][emin].m_pos = min[axis]; - m_pEdges[axis][emax].m_pos = max[axis]; - - // expand (only adds overlaps) - if (dmin < 0) - sortMinDown(axis, emin,dispatcher,true); - - if (dmax > 0) - sortMaxUp(axis, emax,dispatcher,true); - - // shrink (only removes overlaps) - if (dmin > 0) - sortMinUp(axis, emin,dispatcher,true); - - if (dmax < 0) - sortMaxDown(axis, emax,dispatcher,true); - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - } - - -} - - - - -// sorting a min edge downwards can only ever *add* overlaps -template -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); - - while (pEdge->m_pos < pPrev->m_pos) - { - Handle* pHandlePrev = getHandle(pPrev->m_handle); - - if (pPrev->IsMax()) - { - // if previous edge is a maximum check the bounds and add an overlap if necessary - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2)) - { - m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev); - if (m_userPairCallback) - m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev); - - //AddOverlap(pEdge->m_handle, pPrev->m_handle); - - } - - // update edge reference in other handle - pHandlePrev->m_maxEdges[axis]++; - } - else - pHandlePrev->m_minEdges[axis]++; - - pHandleEdge->m_minEdges[axis]--; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pPrev; - *pPrev = swap; - - // decrement - pEdge--; - pPrev--; - } - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - -} - -// sorting a min edge upwards can only ever *remove* overlaps -template -void btAxisSweep3Internal::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) -{ - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pNext = pEdge + 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) - { - Handle* pHandleNext = getHandle(pNext->m_handle); - - if (pNext->IsMax()) - { - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pNext->m_handle); - 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 -#ifdef USE_OVERLAP_TEST_ON_REMOVES - && testOverlap2D(handle0,handle1,axis1,axis2) -#endif //USE_OVERLAP_TEST_ON_REMOVES - ) - { - - - m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); - if (m_userPairCallback) - m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); - - } - - - // update edge reference in other handle - pHandleNext->m_maxEdges[axis]--; - } - else - pHandleNext->m_minEdges[axis]--; - - pHandleEdge->m_minEdges[axis]++; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pNext; - *pNext = swap; - - // increment - pEdge++; - pNext++; - } - - -} - -// sorting a max edge downwards can only ever *remove* overlaps -template -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); - - while (pEdge->m_pos < pPrev->m_pos) - { - Handle* pHandlePrev = getHandle(pPrev->m_handle); - - if (!pPrev->IsMax()) - { - // if previous edge was a minimum remove any overlap between the two handles - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pPrev->m_handle); - const int axis1 = (1 << axis) & 3; - const int axis2 = (1 << axis1) & 3; - - if (updateOverlaps -#ifdef 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); - if (m_userPairCallback) - m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); - - - - } - - // update edge reference in other handle - pHandlePrev->m_minEdges[axis]++;; - } - else - pHandlePrev->m_maxEdges[axis]++; - - pHandleEdge->m_maxEdges[axis]--; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pPrev; - *pPrev = swap; - - // decrement - pEdge--; - pPrev--; - } - - -#ifdef DEBUG_BROADPHASE - debugPrintAxis(axis); -#endif //DEBUG_BROADPHASE - -} - -// sorting a max edge upwards can only ever *add* overlaps -template -void btAxisSweep3Internal::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) -{ - Edge* pEdge = m_pEdges[axis] + edge; - Edge* pNext = pEdge + 1; - Handle* pHandleEdge = getHandle(pEdge->m_handle); - - while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) - { - Handle* pHandleNext = getHandle(pNext->m_handle); - - 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)) - { - Handle* handle0 = getHandle(pEdge->m_handle); - Handle* handle1 = getHandle(pNext->m_handle); - m_pairCache->addOverlappingPair(handle0,handle1); - if (m_userPairCallback) - m_userPairCallback->addOverlappingPair(handle0,handle1); - } - - // update edge reference in other handle - pHandleNext->m_minEdges[axis]--; - } - else - pHandleNext->m_maxEdges[axis]--; - - pHandleEdge->m_maxEdges[axis]++; - - // swap the edges - Edge swap = *pEdge; - *pEdge = *pNext; - *pNext = swap; - - // increment - pEdge++; - pNext++; - } - -} - - - -//////////////////////////////////////////////////////////////////// - - -/// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. -/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats. -/// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance. -class btAxisSweep3 : public btAxisSweep3Internal -{ -public: - - btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - -}; - -/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune. -/// This comes at the cost of more memory per handle, and a bit slower performance. -/// It uses arrays rather then lists for storage of the 3 axis. -class bt32BitAxisSweep3 : public btAxisSweep3Internal -{ -public: - - bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - -}; - -#endif - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h deleted file mode 100644 index f1bf00594..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_BROADPHASE_INTERFACE_H -#define BT_BROADPHASE_INTERFACE_H - - - -struct btDispatcherInfo; -class btDispatcher; -#include "btBroadphaseProxy.h" - -class btOverlappingPairCache; - - - -struct btBroadphaseAabbCallback -{ - virtual ~btBroadphaseAabbCallback() {} - virtual bool process(const btBroadphaseProxy* proxy) = 0; -}; - - -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; - - virtual ~btBroadphaseRayCallback() {} -}; - -#include "LinearMath/btVector3.h" - -///The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs. -///Some implementations for this broadphase interface include btAxisSweep3, bt32BitAxisSweep3 and btDbvtBroadphase. -///The actual overlapping pair management, storage, adding and removing of pairs is dealt by the btOverlappingPairCache class. -class btBroadphaseInterface -{ -public: - virtual ~btBroadphaseInterface() {} - - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0; - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0; - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0; - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)) = 0; - - 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 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; - - ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher) { (void) dispatcher; }; - - virtual void printStats() = 0; - -}; - -#endif //BT_BROADPHASE_INTERFACE_H diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp deleted file mode 100644 index f4d7341f8..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp +++ /dev/null @@ -1,17 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btBroadphaseProxy.h" - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h deleted file mode 100644 index bb58b8289..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ /dev/null @@ -1,270 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_BROADPHASE_PROXY_H -#define BT_BROADPHASE_PROXY_H - -#include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedAllocator.h" - - -/// btDispatcher uses these types -/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave -/// to facilitate type checking -/// CUSTOM_POLYHEDRAL_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE and CUSTOM_CONCAVE_SHAPE_TYPE can be used to extend Bullet without modifying source code -enum BroadphaseNativeTypes -{ - // polyhedral convex shapes - BOX_SHAPE_PROXYTYPE, - TRIANGLE_SHAPE_PROXYTYPE, - TETRAHEDRAL_SHAPE_PROXYTYPE, - CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE, - CONVEX_HULL_SHAPE_PROXYTYPE, - CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE, - CUSTOM_POLYHEDRAL_SHAPE_TYPE, -//implicit convex shapes -IMPLICIT_CONVEX_SHAPES_START_HERE, - SPHERE_SHAPE_PROXYTYPE, - MULTI_SPHERE_SHAPE_PROXYTYPE, - CAPSULE_SHAPE_PROXYTYPE, - CONE_SHAPE_PROXYTYPE, - CONVEX_SHAPE_PROXYTYPE, - CYLINDER_SHAPE_PROXYTYPE, - UNIFORM_SCALING_SHAPE_PROXYTYPE, - MINKOWSKI_SUM_SHAPE_PROXYTYPE, - MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, - BOX_2D_SHAPE_PROXYTYPE, - CONVEX_2D_SHAPE_PROXYTYPE, - CUSTOM_CONVEX_SHAPE_TYPE, -//concave shapes -CONCAVE_SHAPES_START_HERE, - //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! - TRIANGLE_MESH_SHAPE_PROXYTYPE, - SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE, - ///used for demo integration FAST/Swift collision library and Bullet - FAST_CONCAVE_MESH_PROXYTYPE, - //terrain - TERRAIN_SHAPE_PROXYTYPE, -///Used for GIMPACT Trimesh integration - GIMPACT_SHAPE_PROXYTYPE, -///Multimaterial mesh - MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE, - - EMPTY_SHAPE_PROXYTYPE, - STATIC_PLANE_PROXYTYPE, - CUSTOM_CONCAVE_SHAPE_TYPE, -CONCAVE_SHAPES_END_HERE, - - COMPOUND_SHAPE_PROXYTYPE, - - SOFTBODY_SHAPE_PROXYTYPE, - HFFLUID_SHAPE_PROXYTYPE, - HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE, - INVALID_SHAPE_PROXYTYPE, - - MAX_BROADPHASE_COLLISION_TYPES - -}; - - -///The btBroadphaseProxy is the main class that can be used with the Bullet broadphases. -///It stores collision shape type information, collision filter information and a client object, typically a btCollisionObject or btRigidBody. -ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy -{ - -BT_DECLARE_ALIGNED_ALLOCATOR(); - - ///optional filtering to cull potential collisions - enum CollisionFilterGroups - { - 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; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - void* m_multiSapParentProxy; - int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. - - btVector3 m_aabbMin; - btVector3 m_aabbMax; - - SIMD_FORCE_INLINE int getUid() const - { - return m_uniqueId; - } - - //used for memory pools - btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0) - { - } - - btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) - :m_clientObject(userPtr), - m_collisionFilterGroup(collisionFilterGroup), - m_collisionFilterMask(collisionFilterMask), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax) - { - m_multiSapParentProxy = multiSapParentProxy; - } - - - - static SIMD_FORCE_INLINE bool isPolyhedral(int proxyType) - { - return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE); - } - - static SIMD_FORCE_INLINE bool isConvex(int proxyType) - { - return (proxyType < CONCAVE_SHAPES_START_HERE); - } - - static SIMD_FORCE_INLINE bool isNonMoving(int proxyType) - { - return (isConcave(proxyType) && !(proxyType==GIMPACT_SHAPE_PROXYTYPE)); - } - - static SIMD_FORCE_INLINE bool isConcave(int proxyType) - { - return ((proxyType > CONCAVE_SHAPES_START_HERE) && - (proxyType < CONCAVE_SHAPES_END_HERE)); - } - static SIMD_FORCE_INLINE bool isCompound(int proxyType) - { - return (proxyType == COMPOUND_SHAPE_PROXYTYPE); - } - - static SIMD_FORCE_INLINE bool isSoftBody(int proxyType) - { - return (proxyType == SOFTBODY_SHAPE_PROXYTYPE); - } - - static SIMD_FORCE_INLINE bool isInfinite(int proxyType) - { - return (proxyType == STATIC_PLANE_PROXYTYPE); - } - - static SIMD_FORCE_INLINE bool isConvex2d(int 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 -{ - btBroadphasePair () - : - m_pProxy0(0), - m_pProxy1(0), - m_algorithm(0), - m_internalInfo1(0) - { - } - -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) - { - } - 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_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. - -}; - -/* -//comparison for set operation, see Solid DT_Encounter -SIMD_FORCE_INLINE bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) -{ - return a.m_pProxy0 < b.m_pProxy0 || - (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1); -} -*/ - - - -class btBroadphasePairSortPredicate -{ - 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; - - 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) -{ - return (a.m_pProxy0 == b.m_pProxy0) && (a.m_pProxy1 == b.m_pProxy1); -} - - -#endif //BT_BROADPHASE_PROXY_H - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp deleted file mode 100644 index c95d1be0f..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp +++ /dev/null @@ -1,23 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btCollisionAlgorithm.h" -#include "btDispatcher.h" - -btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) -{ - m_dispatcher = ci.m_dispatcher1; -} - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h deleted file mode 100644 index 405656236..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ /dev/null @@ -1,81 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION_ALGORITHM_H -#define BT_COLLISION_ALGORITHM_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" - -struct btBroadphaseProxy; -class btDispatcher; -class btManifoldResult; -class btCollisionObject; -struct btCollisionObjectWrapper; -struct btDispatcherInfo; -class btPersistentManifold; - -typedef btAlignedObjectArray btManifoldArray; - -struct btCollisionAlgorithmConstructionInfo -{ - btCollisionAlgorithmConstructionInfo() - :m_dispatcher1(0), - m_manifold(0) - { - } - btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) - :m_dispatcher1(dispatcher) - { - (void)temp; - } - - 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(); - -public: - - btCollisionAlgorithm() {}; - - btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - - virtual ~btCollisionAlgorithm() {}; - - 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 void getAllContactManifolds(btManifoldArray& manifoldArray) = 0; -}; - - -#endif //BT_COLLISION_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.cpp deleted file mode 100644 index 95443af50..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ /dev/null @@ -1,1295 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btDbvt implementation by Nathanael Presson - -#include "btDbvt.h" - -// -typedef btAlignedObjectArray tNodeArray; -typedef btAlignedObjectArray tConstNodeArray; - -// -struct btDbvtNodeEnumerator : btDbvt::ICollide -{ - tConstNodeArray nodes; - void Process(const btDbvtNode* n) { nodes.push_back(n); } -}; - -// -static DBVT_INLINE int indexof(const btDbvtNode* node) -{ - return(node->parent->childs[1]==node); -} - -// -static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, - const btDbvtVolume& b) -{ -#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) - ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); - btDbvtVolume& res=*(btDbvtVolume*)locals; -#else - btDbvtVolume res; -#endif - Merge(a,b,res); - return(res); -} - -// volume+edge lengths -static DBVT_INLINE btScalar size(const btDbvtVolume& a) -{ - const btVector3 edges=a.Lengths(); - return( edges.x()*edges.y()*edges.z()+ - edges.x()+edges.y()+edges.z()); -} - -// -static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) -{ - if(node->isinternal()) - { - getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[1],depth+1,maxdepth); - } else maxdepth=btMax(maxdepth,depth); -} - -// -static DBVT_INLINE void deletenode( btDbvt* pdbvt, - btDbvtNode* node) -{ - btAlignedFree(pdbvt->m_free); - pdbvt->m_free=node; -} - -// -static void recursedeletenode( btDbvt* pdbvt, - btDbvtNode* node) -{ - if(!node->isleaf()) - { - recursedeletenode(pdbvt,node->childs[0]); - recursedeletenode(pdbvt,node->childs[1]); - } - if(node==pdbvt->m_root) pdbvt->m_root=0; - deletenode(pdbvt,node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - void* data) -{ - btDbvtNode* node; - if(pdbvt->m_free) - { node=pdbvt->m_free;pdbvt->m_free=0; } - else - { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } - node->parent = parent; - node->data = data; - node->childs[1] = 0; - return(node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume, - void* data) -{ - btDbvtNode* node=createnode(pdbvt,parent,data); - node->volume=volume; - return(node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume0, - const btDbvtVolume& volume1, - void* data) -{ - btDbvtNode* node=createnode(pdbvt,parent,data); - Merge(volume0,volume1,node->volume); - return(node); -} - -// -static void insertleaf( btDbvt* pdbvt, - btDbvtNode* root, - btDbvtNode* leaf) -{ - if(!pdbvt->m_root) - { - pdbvt->m_root = leaf; - leaf->parent = 0; - } - else - { - if(!root->isleaf()) - { - do { - root=root->childs[Select( leaf->volume, - root->childs[0]->volume, - root->childs[1]->volume)]; - } while(!root->isleaf()); - } - btDbvtNode* prev=root->parent; - btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); - if(prev) - { - prev->childs[indexof(root)] = node; - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - do { - if(!prev->volume.Contain(node->volume)) - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - else - break; - node=prev; - } while(0!=(prev=node->parent)); - } - else - { - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - pdbvt->m_root = node; - } - } -} - -// -static btDbvtNode* removeleaf( btDbvt* pdbvt, - btDbvtNode* leaf) -{ - if(leaf==pdbvt->m_root) - { - pdbvt->m_root=0; - return(0); - } - else - { - btDbvtNode* parent=leaf->parent; - btDbvtNode* prev=parent->parent; - btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; - if(prev) - { - prev->childs[indexof(parent)]=sibling; - sibling->parent=prev; - deletenode(pdbvt,parent); - while(prev) - { - const btDbvtVolume pb=prev->volume; - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - if(NotEqual(pb,prev->volume)) - { - prev=prev->parent; - } else break; - } - return(prev?prev:pdbvt->m_root); - } - else - { - pdbvt->m_root=sibling; - sibling->parent=0; - deletenode(pdbvt,parent); - return(pdbvt->m_root); - } - } -} - -// -static void fetchleaves(btDbvt* pdbvt, - btDbvtNode* root, - tNodeArray& leaves, - int depth=-1) -{ - if(root->isinternal()&&depth) - { - fetchleaves(pdbvt,root->childs[0],leaves,depth-1); - fetchleaves(pdbvt,root->childs[1],leaves,depth-1); - deletenode(pdbvt,root); - } - else - { - leaves.push_back(root); - } -} - -// -static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, - const btVector3& org, - const btVector3& axis) -{ - left.resize(0); - right.resize(0); - for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) - left.push_back(leaves[i]); - else - right.push_back(leaves[i]); - } -} - -// -static btDbvtVolume bounds( const tNodeArray& leaves) -{ -#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE - ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); - btDbvtVolume& volume=*(btDbvtVolume*)locals; - volume=leaves[0]->volume; -#else - btDbvtVolume volume=leaves[0]->volume; -#endif - for(int i=1,ni=leaves.size();ivolume,volume); - } - return(volume); -} - -// -static void bottomup( btDbvt* pdbvt, - tNodeArray& leaves) -{ - while(leaves.size()>1) - { - btScalar minsize=SIMD_INFINITY; - int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); - if(szvolume,n[1]->volume,0); - p->childs[0] = n[0]; - p->childs[1] = n[1]; - n[0]->parent = p; - n[1]->parent = p; - leaves[minidx[0]] = p; - leaves.swap(minidx[1],leaves.size()-1); - leaves.pop_back(); - } -} - -// -static btDbvtNode* topdown(btDbvt* pdbvt, - tNodeArray& leaves, - int bu_treshold) -{ - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - if(leaves.size()>1) - { - if(leaves.size()>bu_treshold) - { - const btDbvtVolume vol=bounds(leaves); - const btVector3 org=vol.Center(); - tNodeArray sets[2]; - int bestaxis=-1; - int bestmidp=leaves.size(); - int splitcount[3][2]={{0,0},{0,0},{0,0}}; - int i; - for( i=0;ivolume.Center()-org; - for(int j=0;j<3;++j) - { - ++splitcount[j][btDot(x,axis[j])>0?1:0]; - } - } - for( i=0;i<3;++i) - { - if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) - { - const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); - if(midp=0) - { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leaves,sets[0],sets[1],org,axis[bestaxis]); - } - else - { - sets[0].reserve(leaves.size()/2+1); - sets[1].reserve(leaves.size()/2); - for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); - node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); - node->childs[0]->parent=node; - node->childs[1]->parent=node; - return(node); - } - else - { - bottomup(pdbvt,leaves); - return(leaves[0]); - } - } - return(leaves[0]); -} - -// -static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) -{ - btDbvtNode* p=n->parent; - btAssert(n->isinternal()); - if(p>n) - { - const int i=indexof(n); - const int j=1-i; - btDbvtNode* s=p->childs[j]; - btDbvtNode* q=p->parent; - btAssert(n==p->childs[i]); - if(q) q->childs[indexof(p)]=n; else r=n; - s->parent=n; - p->parent=n; - n->parent=q; - p->childs[0]=n->childs[0]; - p->childs[1]=n->childs[1]; - n->childs[0]->parent=p; - n->childs[1]->parent=p; - n->childs[i]=p; - n->childs[j]=s; - btSwap(p->volume,n->volume); - return(p); - } - return(n); -} - -#if 0 -static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) -{ - while(n&&(count--)) n=n->parent; - return(n); -} -#endif - -// -// Api -// - -// -btDbvt::btDbvt() -{ - m_root = 0; - m_free = 0; - m_lkhd = -1; - m_leaves = 0; - m_opath = 0; -} - -// -btDbvt::~btDbvt() -{ - clear(); -} - -// -void btDbvt::clear() -{ - if(m_root) - recursedeletenode(this,m_root); - btAlignedFree(m_free); - m_free=0; - m_lkhd = -1; - m_stkStack.clear(); - m_opath = 0; - -} - -// -void btDbvt::optimizeBottomUp() -{ - if(m_root) - { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - bottomup(this,leaves); - m_root=leaves[0]; - } -} - -// -void btDbvt::optimizeTopDown(int bu_treshold) -{ - if(m_root) - { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); - } -} - -// -void btDbvt::optimizeIncremental(int passes) -{ - if(passes<0) passes=m_leaves; - if(m_root&&(passes>0)) - { - do { - btDbvtNode* node=m_root; - unsigned bit=0; - while(node->isinternal()) - { - node=sort(node,m_root)->childs[(m_opath>>bit)&1]; - bit=(bit+1)&(sizeof(unsigned)*8-1); - } - update(node); - ++m_opath; - } while(--passes); - } -} - -// -btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) -{ - btDbvtNode* leaf=createnode(this,0,volume,data); - insertleaf(this,m_root,leaf); - ++m_leaves; - return(leaf); -} - -// -void btDbvt::update(btDbvtNode* leaf,int lookahead) -{ - btDbvtNode* root=removeleaf(this,leaf); - if(root) - { - if(lookahead>=0) - { - for(int i=0;(iparent;++i) - { - root=root->parent; - } - } else root=m_root; - } - insertleaf(this,root,leaf); -} - -// -void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume) -{ - btDbvtNode* root=removeleaf(this,leaf); - if(root) - { - if(m_lkhd>=0) - { - for(int i=0;(iparent;++i) - { - root=root->parent; - } - } else root=m_root; - } - leaf->volume=volume; - insertleaf(this,root,leaf); -} - -// -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin) -{ - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); - volume.SignedExpand(velocity); - update(leaf,volume); - return(true); -} - -// -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity) -{ - if(leaf->volume.Contain(volume)) return(false); - volume.SignedExpand(velocity); - update(leaf,volume); - return(true); -} - -// -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin) -{ - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); - update(leaf,volume); - return(true); -} - -// -void btDbvt::remove(btDbvtNode* leaf) -{ - removeleaf(this,leaf); - deletenode(this,leaf); - --m_leaves; -} - -// -void btDbvt::write(IWriter* iwriter) const -{ - btDbvtNodeEnumerator nodes; - nodes.nodes.reserve(m_leaves*2); - enumNodes(m_root,nodes); - iwriter->Prepare(m_root,nodes.nodes.size()); - for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); - if(n->isinternal()) - { - const int c0=nodes.nodes.findLinearSearch(n->childs[0]); - const int c1=nodes.nodes.findLinearSearch(n->childs[1]); - iwriter->WriteNode(n,i,p,c0,c1); - } - else - { - iwriter->WriteLeaf(n,i,p); - } - } -} - -// -void btDbvt::clone(btDbvt& dest,IClone* iclone) const -{ - dest.clear(); - 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.pop_back(); - if(e.parent!=0) - e.parent->childs[i&1]=n; - else - dest.m_root=n; - if(e.node->isinternal()) - { - stack.push_back(sStkCLN(e.node->childs[0],n)); - stack.push_back(sStkCLN(e.node->childs[1],n)); - } - else - { - iclone->CloneLeaf(n); - } - } while(stack.size()>0); - } -} - -// -int btDbvt::maxdepth(const btDbvtNode* node) -{ - int depth=0; - if(node) getmaxdepth(node,1,depth); - return(depth); -} - -// -int btDbvt::countLeaves(const btDbvtNode* node) -{ - if(node->isinternal()) - return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); - else - return(1); -} - -// -void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) -{ - if(node->isinternal()) - { - extractLeaves(node->childs[0],leaves); - extractLeaves(node->childs[1],leaves); - } - else - { - leaves.push_back(node); - } -} - -// -#if DBVT_ENABLE_BENCHMARK - -#include -#include -#include "LinearMath/btQuickProf.h" - -/* -q6600,2.4ghz - -/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32" -/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch" -/Fo"..\..\out\release8\build\libbulletcollision\\" -/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb" -/W3 /nologo /c /Wp64 /Zi /errorReport:prompt - -Benchmarking dbvt... -World scale: 100.000000 -Extents base: 1.000000 -Extents range: 4.000000 -Leaves: 8192 -sizeof(btDbvtVolume): 32 bytes -sizeof(btDbvtNode): 44 bytes -[1] btDbvtVolume intersections: 3499 ms (-1%) -[2] btDbvtVolume merges: 1934 ms (0%) -[3] btDbvt::collideTT: 5485 ms (-21%) -[4] btDbvt::collideTT self: 2814 ms (-20%) -[5] btDbvt::collideTT xform: 7379 ms (-1%) -[6] btDbvt::collideTT xform,self: 7270 ms (-2%) -[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s) -[8] insert/remove: 2093 ms (0%),(1001983 ir/s) -[9] updates (teleport): 1879 ms (-3%),(1116100 u/s) -[10] updates (jitter): 1244 ms (-4%),(1685813 u/s) -[11] optimize (incremental): 2514 ms (0%),(1668000 o/s) -[12] btDbvtVolume notequal: 3659 ms (0%) -[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) -[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) -[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) -[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) -[17] btDbvtVolume select: 3419 ms (0%) -*/ - -struct btDbvtBenchmark -{ - struct NilPolicy : btDbvt::ICollide - { - NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} - void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*,btScalar depth) - { - ++m_pcount; - if(m_checksort) - { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } - } - int m_pcount; - btScalar m_depth; - bool m_checksort; - }; - struct P14 : btDbvt::ICollide - { - struct Node - { - const btDbvtNode* leaf; - btScalar depth; - }; - void Process(const btDbvtNode* leaf,btScalar depth) - { - Node n; - n.leaf = leaf; - n.depth = depth; - } - static int sortfnc(const Node& a,const Node& b) - { - if(a.depthb.depth) return(-1); - return(0); - } - btAlignedObjectArray m_nodes; - }; - struct P15 : btDbvt::ICollide - { - struct Node - { - const btDbvtNode* leaf; - btScalar depth; - }; - void Process(const btDbvtNode* leaf) - { - Node n; - n.leaf = leaf; - n.depth = dot(leaf->volume.Center(),m_axis); - } - static int sortfnc(const Node& a,const Node& b) - { - if(a.depthb.depth) return(-1); - return(0); - } - btAlignedObjectArray m_nodes; - btVector3 m_axis; - }; - static btScalar RandUnit() - { - return(rand()/(btScalar)RAND_MAX); - } - static btVector3 RandVector3() - { - return(btVector3(RandUnit(),RandUnit(),RandUnit())); - } - static btVector3 RandVector3(btScalar cs) - { - return(RandVector3()*cs-btVector3(cs,cs,cs)/2); - } - static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) - { - return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); - } - static btTransform RandTransform(btScalar cs) - { - btTransform t; - t.setOrigin(RandVector3(cs)); - t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); - return(t); - } - static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) - { - dbvt.clear(); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark5_Iterations); - for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark6_Iterations); - for(int i=0;i rayorg; - btAlignedObjectArray raydir; - btDbvtBenchmark::NilPolicy policy; - rayorg.resize(cfgBenchmark7_Iterations); - raydir.resize(cfgBenchmark7_Iterations); - for(int i=0;i leaves; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); - printf("[9] updates (teleport): "); - wallclock.reset(); - for(int i=0;i(leaves[rand()%cfgLeaves]), - btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); - } - } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); - } - if(cfgBenchmark10_Enable) - {// Benchmark 10 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray leaves; - btAlignedObjectArray vectors; - vectors.resize(cfgBenchmark10_Iterations); - for(int i=0;i(leaves[rand()%cfgLeaves]); - btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); - dbvt.update(l,v); - } - } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); - } - if(cfgBenchmark11_Enable) - {// Benchmark 11 - srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[11] optimize (incremental): "); - wallclock.reset(); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i vectors; - btDbvtBenchmark::NilPolicy policy; - vectors.resize(cfgBenchmark13_Iterations); - for(int i=0;i vectors; - btDbvtBenchmark::P14 policy; - vectors.resize(cfgBenchmark14_Iterations); - for(int i=0;i vectors; - btDbvtBenchmark::P15 policy; - vectors.resize(cfgBenchmark15_Iterations); - for(int i=0;i batch; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - batch.reserve(cfgBenchmark16_BatchCount); - printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); - wallclock.reset(); - for(int i=0;i volumes; - btAlignedObjectArray results; - btAlignedObjectArray indices; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - indices.resize(cfgLeaves); - for(int i=0;i= 1400) -#define DBVT_USE_TEMPLATE 1 -#else -#define DBVT_USE_TEMPLATE 0 -#endif -#else -#define DBVT_USE_TEMPLATE 0 -#endif - -// Use only intrinsics instead of inline asm -#define DBVT_USE_INTRINSIC_SSE 1 - -// Using memmov for collideOCL -#define DBVT_USE_MEMMOVE 1 - -// Enable benchmarking code -#define DBVT_ENABLE_BENCHMARK 0 - -// Inlining -#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 -#else -#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) -#include -#endif - -// -// Auto config and checks -// - -#if DBVT_USE_TEMPLATE -#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; -#else -#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} -#define DBVT_VIRTUAL virtual -#define DBVT_PREFIX -#define DBVT_IPOLICY ICollide& policy -#define DBVT_CHECKTYPE -#endif - -#if DBVT_USE_MEMMOVE -#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) -#include -#endif -#include -#endif - -#ifndef DBVT_USE_TEMPLATE -#error "DBVT_USE_TEMPLATE undefined" -#endif - -#ifndef DBVT_USE_MEMMOVE -#error "DBVT_USE_MEMMOVE undefined" -#endif - -#ifndef DBVT_ENABLE_BENCHMARK -#error "DBVT_ENABLE_BENCHMARK undefined" -#endif - -#ifndef DBVT_SELECT_IMPL -#error "DBVT_SELECT_IMPL undefined" -#endif - -#ifndef DBVT_MERGE_IMPL -#error "DBVT_MERGE_IMPL undefined" -#endif - -#ifndef DBVT_INT0_IMPL -#error "DBVT_INT0_IMPL undefined" -#endif - -// -// Defaults volumes -// - -/* btDbvtAabbMm */ -struct btDbvtAabbMm -{ - DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } - DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } - DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } - DBVT_INLINE const btVector3& Mins() const { return(mi); } - DBVT_INLINE const btVector3& Maxs() const { return(mx); } - static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); - static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); - static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); - static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); - static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); - DBVT_INLINE void Expand(const btVector3& e); - DBVT_INLINE void SignedExpand(const btVector3& e); - DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; - DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; - DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const 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); } - -private: - DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; -private: - btVector3 mi,mx; -}; - -// Types -typedef btDbvtAabbMm btDbvtVolume; - -/* btDbvtNode */ -struct btDbvtNode -{ - btDbvtVolume volume; - btDbvtNode* parent; - DBVT_INLINE bool isleaf() const { return(childs[1]==0); } - DBVT_INLINE bool isinternal() const { return(!isleaf()); } - union - { - btDbvtNode* childs[2]; - void* data; - int dataAsInt; - }; -}; - -///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). -///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. -///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. -struct btDbvt -{ - /* Stack element */ - struct sStkNN - { - const btDbvtNode* a; - const btDbvtNode* b; - sStkNN() {} - sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} - }; - struct sStkNP - { - const btDbvtNode* node; - int mask; - sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} - }; - struct sStkNPS - { - const btDbvtNode* node; - int mask; - btScalar value; - sStkNPS() {} - sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} - }; - struct sStkCLN - { - const btDbvtNode* node; - btDbvtNode* parent; - sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} - }; - // Policies/Interfaces - - /* ICollide */ - struct ICollide - { - DBVT_VIRTUAL_DTOR(ICollide) - DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } - DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } - DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } - }; - /* IWriter */ - struct IWriter - { - virtual ~IWriter() {} - virtual void Prepare(const btDbvtNode* root,int numnodes)=0; - virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; - virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; - }; - /* IClone */ - struct IClone - { - virtual ~IClone() {} - virtual void CloneLeaf(btDbvtNode*) {} - }; - - // Constants - enum { - SIMPLE_STACKSIZE = 64, - DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 - }; - - // Fields - btDbvtNode* m_root; - btDbvtNode* m_free; - int m_lkhd; - int m_leaves; - unsigned m_opath; - - - btAlignedObjectArray m_stkStack; - mutable btAlignedObjectArray m_rayTestStack; - - - // 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); -#if DBVT_ENABLE_BENCHMARK - static void benchmark(); -#else - static void benchmark(){} -#endif - // DBVT_IPOLICY must support ICollide policy/interface - DBVT_PREFIX - static void enumNodes( const btDbvtNode* root, - DBVT_IPOLICY); - DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); - DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); - - DBVT_PREFIX - void collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); -#if 0 - DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY); - DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY); -#endif - - DBVT_PREFIX - void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - 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); - ///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, - DBVT_IPOLICY) const; - - DBVT_PREFIX - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); - DBVT_PREFIX - static void collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fullsort=true); - DBVT_PREFIX - static void collideTU( const btDbvtNode* root, - DBVT_IPOLICY); - // Helpers - static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) - { - int m=0; - while(l>1; - if(a[i[m]].value>=v) l=m+1; else h=m; - } - return(h); - } - 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; } - else - { i=stock.size();stock.push_back(value); } - return(i); - } - // -private: - btDbvt(const btDbvt&) {} -}; - -// -// Inline's -// - -// -inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) -{ - btDbvtAabbMm box; - box.mi=c-e;box.mx=c+e; - return(box); -} - -// -inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) -{ - return(FromCE(c,btVector3(r,r,r))); -} - -// -inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) -{ - btDbvtAabbMm box; - box.mi=mi;box.mx=mx; - return(box); -} - -// -inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) -{ - btDbvtAabbMm box; - box.mi=box.mx=pts[0]; - for(int i=1;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]); -} - -// -DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const -{ - return( (mi.x()<=a.mi.x())&& - (mi.y()<=a.mi.y())&& - (mi.z()<=a.mi.z())&& - (mx.x()>=a.mx.x())&& - (mx.y()>=a.mx.y())&& - (mx.z()>=a.mx.z())); -} - -// -DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const -{ - btVector3 pi,px; - switch(s) - { - case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); - pi=btVector3(mx.x(),mx.y(),mx.z());break; - case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); - pi=btVector3(mi.x(),mx.y(),mx.z());break; - case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); - pi=btVector3(mx.x(),mi.y(),mx.z());break; - case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); - pi=btVector3(mi.x(),mi.y(),mx.z());break; - case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); - pi=btVector3(mx.x(),mx.y(),mi.z());break; - case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); - pi=btVector3(mi.x(),mx.y(),mi.z());break; - case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); - pi=btVector3(mx.x(),mi.y(),mi.z());break; - case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); - pi=btVector3(mi.x(),mi.y(),mi.z());break; - } - if((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 -{ - 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 -{ - for(int i=0;i<3;++i) - { - if(d[i]<0) - { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } - else - { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } - } -} - -// -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) -{ -#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); -#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 -} - - - -// -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& b) -{ - return( (b.x()>=a.mi.x())&& - (b.y()>=a.mi.y())&& - (b.z()>=a.mi.z())&& - (b.x()<=a.mx.x())&& - (b.y()<=a.mx.y())&& - (b.z()<=a.mx.z())); -} - - - - - -////////////////////////////////////// - - -// -DBVT_INLINE 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, - 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*/}; -#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 - { - __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)); - - btSSEUnion tmp; - tmp.ssereg = _mm_cmple_ss(bmi,ami); - return tmp.ints[0]&1; - -#else - ATTRIBUTE_ALIGNED16(__int32 r[1]); - __asm - { - mov eax,o - mov ecx,a - mov edx,b - movaps xmm0,[eax] - 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(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; - } -#endif -} - -// -DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) -{ - return( (a.mi.x()!=b.mi.x())|| - (a.mi.y()!=b.mi.y())|| - (a.mi.z()!=b.mi.z())|| - (a.mx.x()!=b.mx.x())|| - (a.mx.y()!=b.mx.y())|| - (a.mx.z()!=b.mx.z())); -} - -// -// Inline's -// - -// -DBVT_PREFIX -inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - policy.Process(root); - if(root->isinternal()) - { - enumNodes(root->childs[0],policy); - enumNodes(root->childs[1],policy); - } -} - -// -DBVT_PREFIX -inline void btDbvt::enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root->isinternal()) - { - enumLeaves(root->childs[0],policy); - enumLeaves(root->childs[1],policy); - } - else - { - policy.Process(root); - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root0&&root1) - { - 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) - { - 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(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); - } - } - 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) -{ - DBVT_CHECKTYPE - if(root0&&root1) - { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - - m_stkStack.resize(DOUBLE_STACKSIZE); - m_stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=m_stkStack[--depth]; - if(depth>treshold) - { - 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(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } - } - else - { - 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 -// -DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root0&&root1) - { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - btAlignedObjectArray stkStack; - stkStack.resize(DOUBLE_STACKSIZE); - stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=stkStack[--depth]; - if(Intersect(p.a->volume,p.b->volume,xform)) - { - if(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); - } -} -// -DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY) -{ - const btTransform xform=xform0.inverse()*xform1; - collideTT(root0,root1,xform,policy); -} -#endif - -// -DBVT_PREFIX -inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) const -{ - DBVT_CHECKTYPE - if(root) - { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray stack; - stack.resize(0); - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->volume,volume)) - { - if(n->isinternal()) - { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); - } - else - { - policy.Process(n); - } - } - } while(stack.size()>0); - } -} - -DBVT_PREFIX -inline void btDbvt::rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - DBVT_IPOLICY) const -{ - (void) rayTo; - DBVT_CHECKTYPE - if(root) - { - btVector3 resultNormal; - - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; - btAlignedObjectArray& stack = m_rayTestStack; - stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; - btVector3 bounds[2]; - 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) - { - 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::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; - - stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; - btVector3 bounds[2]; - do { - const btDbvtNode* node=stack[--depth]; - - bounds[0] = node->volume.Mins(); - bounds[1] = node->volume.Maxs(); - - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); - -#ifdef COMPARE_BTRAY_AABB2 - btScalar param=1.f; - bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); - btAssert(result1 == result2); -#endif //TEST_BTRAY_AABB2 - - if(result1) - { - 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) - { - const int inside=(1< stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); - } - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(sStkNP(root,0)); - do { - sStkNP se=stack[stack.size()-1]; - bool out=false; - stack.pop_back(); - for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - 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); - } - 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); -#if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); -#else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; -#endif - stack[j]=allocate(ifree,stock,nes[q]); - /* Insert 1 */ - j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); - stack.push_back(0); -#if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); -#else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; -#endif - stack[j]=allocate(ifree,stock,nes[1-q]); - } - else - { - stack.push_back(allocate(ifree,stock,nes[q])); - stack.push_back(allocate(ifree,stock,nes[1-q])); - } - } - else - { - policy.Process(se.node,se.value); - } - } - } while(stack.size()); - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root) - { - btAlignedObjectArray stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(policy.Descent(n)) - { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { policy.Process(n); } - } - } while(stack.size()>0); - } -} - -// -// PP Cleanup -// - -#undef DBVT_USE_MEMMOVE -#undef DBVT_USE_TEMPLATE -#undef DBVT_VIRTUAL_DTOR -#undef DBVT_VIRTUAL -#undef DBVT_PREFIX -#undef DBVT_IPOLICY -#undef DBVT_CHECKTYPE -#undef DBVT_IMPL_GENERIC -#undef DBVT_IMPL_SSE -#undef DBVT_USE_INTRINSIC_SSE -#undef DBVT_SELECT_IMPL -#undef DBVT_MERGE_IMPL -#undef DBVT_INT0_IMPL - -#endif diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp deleted file mode 100644 index 75cfac643..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ /dev/null @@ -1,796 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///btDbvtBroadphase implementation by Nathanael Presson - -#include "btDbvtBroadphase.h" - -// -// Profiling -// - -#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK -#include -#endif - -#if DBVT_BP_PROFILE -struct ProfileScope -{ - __forceinline ProfileScope(btClock& clock,unsigned long& value) : - m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) - { - } - __forceinline ~ProfileScope() - { - (*m_value)+=m_clock->getTimeMicroseconds()-m_base; - } - btClock* m_clock; - unsigned long* m_value; - unsigned long m_base; -}; -#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) -#else -#define SPC(_value_) -#endif - -// -// Helpers -// - -// -template -static inline void listappend(T* item,T*& list) -{ - item->links[0]=0; - item->links[1]=list; - if(list) list->links[0]=item; - list=item; -} - -// -template -static inline void listremove(T* item,T*& list) -{ - if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; - if(item->links[1]) item->links[1]->links[0]=item->links[0]; -} - -// -template -static inline int listcount(T* root) -{ - int n=0; - while(root) { ++n;root=root->links[1]; } - return(n); -} - -// -template -static inline void clear(T& value) -{ - static const struct ZeroDummy : T {} zerodummy; - value=zerodummy; -} - -// -// Colliders -// - -/* Tree collider */ -struct btDbvtTreeCollider : btDbvt::ICollide -{ - btDbvtBroadphase* pbp; - btDbvtProxy* proxy; - btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} - void Process(const btDbvtNode* na,const btDbvtNode* nb) - { - if(na!=nb) - { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; -#if DBVT_BP_SORTPAIRS - if(pa->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); -#endif - pbp->m_paircache->addOverlappingPair(pa,pb); - ++pbp->m_newpairs; - } - } - void Process(const btDbvtNode* n) - { - Process(n,proxy->leaf); - } -}; - -// -// btDbvtBroadphase -// - -// -btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) -{ - m_deferedcollide = false; - m_needcleanup = true; - m_releasepaircache = (paircache!=0)?false:true; - m_prediction = 0; - m_stageCurrent = 0; - m_fixedleft = 0; - m_fupdates = 1; - m_dupdates = 0; - m_cupdates = 10; - m_newpairs = 1; - m_updates_call = 0; - m_updates_done = 0; - m_updates_ratio = 0; - m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); - m_gid = 0; - m_pid = 0; - m_cid = 0; - for(int i=0;i<=STAGECOUNT;++i) - { - m_stageRoots[i]=0; - } -#if DBVT_BP_PROFILE - clear(m_profiling); -#endif -} - -// -btDbvtBroadphase::~btDbvtBroadphase() -{ - if(m_releasepaircache) - { - m_paircache->~btOverlappingPairCache(); - btAlignedFree(m_paircache); - } -} - -// -btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, - const btVector3& aabbMax, - int /*shapeType*/, - void* userPtr, - short int collisionFilterGroup, - short int collisionFilterMask, - btDispatcher* /*dispatcher*/, - void* /*multiSapProxy*/) -{ - btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, - collisionFilterGroup, - collisionFilterMask); - - btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); - - //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); - proxy->stage = m_stageCurrent; - proxy->m_uniqueId = ++m_gid; - proxy->leaf = m_sets[0].insert(aabb,proxy); - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(!m_deferedcollide) - { - btDbvtTreeCollider collider(this); - collider.proxy=proxy; - m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); - m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); - } - return(proxy); -} - -// -void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, - btDispatcher* dispatcher) -{ - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - if(proxy->stage==STAGECOUNT) - m_sets[1].remove(proxy->leaf); - else - m_sets[0].remove(proxy->leaf); - listremove(proxy,m_stageRoots[proxy->stage]); - m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); - btAlignedFree(proxy); - m_needcleanup=true; -} - -void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - aabbMin = proxy->m_aabbMin; - aabbMax = proxy->m_aabbMax; -} - -struct BroadphaseRayTester : btDbvt::ICollide -{ - btBroadphaseRayCallback& m_rayCallback; - BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) - :m_rayCallback(orgCallback) - { - } - void Process(const btDbvtNode* leaf) - { - btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; - m_rayCallback.process(proxy); - } -}; - -void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) -{ - BroadphaseRayTester callback(rayCallback); - - m_sets[0].rayTestInternal( m_sets[0].m_root, - rayFrom, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - callback); - - m_sets[1].rayTestInternal( m_sets[1].m_root, - rayFrom, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - callback); - -} - - -struct BroadphaseAabbTester : btDbvt::ICollide -{ - btBroadphaseAabbCallback& m_aabbCallback; - BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback) - :m_aabbCallback(orgCallback) - { - } - void Process(const btDbvtNode* leaf) - { - btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; - m_aabbCallback.process(proxy); - } -}; - -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); - -} - - - -// -void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) -{ - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); -#if DBVT_BP_PREVENTFALSEUPDATE - if(NotEqual(aabb,proxy->leaf->volume)) -#endif - { - bool docollide=false; - if(proxy->stage==STAGECOUNT) - {/* fixed -> dynamic set */ - m_sets[1].remove(proxy->leaf); - proxy->leaf=m_sets[0].insert(aabb,proxy); - docollide=true; - } - else - {/* dynamic set */ - ++m_updates_call; - if(Intersect(proxy->leaf->volume,aabb)) - {/* Moving */ - - const btVector3 delta=aabbMin-proxy->m_aabbMin; - btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); - if(delta[0]<0) velocity[0]=-velocity[0]; - if(delta[1]<0) velocity[1]=-velocity[1]; - if(delta[2]<0) velocity[2]=-velocity[2]; - if ( -#ifdef DBVT_BP_MARGIN - m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) -#else - m_sets[0].update(proxy->leaf,aabb,velocity) -#endif - ) - { - ++m_updates_done; - docollide=true; - } - } - else - {/* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); - ++m_updates_done; - docollide=true; - } - } - listremove(proxy,m_stageRoots[proxy->stage]); - proxy->m_aabbMin = aabbMin; - proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) - { - m_needcleanup=true; - if(!m_deferedcollide) - { - btDbvtTreeCollider collider(this); - m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); - } - } - } -} - - -// -void btDbvtBroadphase::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 */ - 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; - } - listremove(proxy,m_stageRoots[proxy->stage]); - proxy->m_aabbMin = aabbMin; - proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) - { - m_needcleanup=true; - if(!m_deferedcollide) - { - btDbvtTreeCollider collider(this); - m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); - } - } -} - -// -void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - collide(dispatcher); -#if DBVT_BP_PROFILE - if(0==(m_pid%DBVT_BP_PROFILING_RATE)) - { - printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); - unsigned int total=m_profiling.m_total; - if(total<=0) total=1; - printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); - printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); - printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); - printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); - const unsigned long sum=m_profiling.m_ddcollide+ - m_profiling.m_fdcollide+ - m_profiling.m_cleanup; - printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); - printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); - clear(m_profiling); - m_clock.reset(); - } -#endif - - performDeferredRemoval(dispatcher); - -} - -void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) -{ - - if (m_paircache->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - int invalidPair = 0; - - - int i; - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;ileaf->volume,pb->leaf->volume); - - if (hasOverlap) - { - needsRemoval = false; - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - m_paircache->cleanOverlappingPair(pair,dispatcher); - - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - invalidPair++; - } - - } - - //perform a sort, to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); - } -} - -// -void btDbvtBroadphase::collide(btDispatcher* dispatcher) -{ - /*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"); - } -*/ - - - - SPC(m_profiling.m_total); - /* optimize */ - m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); - if(m_fixedleft) - { - const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; - m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); - m_fixedleft=btMax(0,m_fixedleft-count); - } - /* dynamic -> fixed set */ - m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; - btDbvtProxy* current=m_stageRoots[m_stageCurrent]; - if(current) - { - btDbvtTreeCollider collider(this); - do { - btDbvtProxy* next=current->links[1]; - listremove(current,m_stageRoots[current->stage]); - listappend(current,m_stageRoots[STAGECOUNT]); -#if DBVT_BP_ACCURATESLEEPING - m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); - collider.proxy=current; - btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); -#endif - m_sets[0].remove(current->leaf); - ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); - current->leaf = m_sets[1].insert(curAabb,current); - current->stage = STAGECOUNT; - current = next; - } while(current); - m_fixedleft=m_sets[1].m_leaves; - m_needcleanup=true; - } - /* collide dynamics */ - { - btDbvtTreeCollider collider(this); - if(m_deferedcollide) - { - SPC(m_profiling.m_fdcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); - } - if(m_deferedcollide) - { - SPC(m_profiling.m_ddcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); - } - } - /* clean up */ - if(m_needcleanup) - { - SPC(m_profiling.m_cleanup); - btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); - if(pairs.size()>0) - { - - int ni=btMin(pairs.size(),btMax(m_newpairs,(pairs.size()*m_cupdates)/100)); - for(int i=0;ileaf->volume,pb->leaf->volume)) - { -#if DBVT_BP_SORTPAIRS - if(pa->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); -#endif - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; - } - } - if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; - } - } - ++m_pid; - m_newpairs=1; - m_needcleanup=false; - if(m_updates_call>0) - { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } - else - { m_updates_ratio=0; } - m_updates_done/=2; - m_updates_call/=2; -} - -// -void btDbvtBroadphase::optimize() -{ - m_sets[0].optimizeTopDown(); - m_sets[1].optimizeTopDown(); -} - -// -btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() -{ - return(m_paircache); -} - -// -const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const -{ - return(m_paircache); -} - -// -void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const -{ - - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; - - if(!m_sets[0].empty()) - if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, - m_sets[1].m_root->volume,bounds); - else - bounds=m_sets[0].m_root->volume; - else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; - else - bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); - aabbMin=bounds.Mins(); - aabbMax=bounds.Maxs(); -} - -void btDbvtBroadphase::resetPool(btDispatcher* dispatcher) -{ - - int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; - if (!totalObjects) - { - //reset internal dynamic tree data structures - m_sets[0].clear(); - m_sets[1].clear(); - - m_deferedcollide = false; - m_needcleanup = true; - m_stageCurrent = 0; - m_fixedleft = 0; - m_fupdates = 1; - m_dupdates = 0; - m_cupdates = 10; - m_newpairs = 1; - m_updates_call = 0; - m_updates_done = 0; - m_updates_ratio = 0; - - m_gid = 0; - m_pid = 0; - m_cid = 0; - for(int i=0;i<=STAGECOUNT;++i) - { - m_stageRoots[i]=0; - } - } -} - -// -void btDbvtBroadphase::printStats() -{} - -// -#if DBVT_BP_ENABLE_BENCHMARK - -struct btBroadphaseBenchmark -{ - struct Experiment - { - const char* name; - int object_count; - int update_count; - int spawn_count; - int iterations; - btScalar speed; - btScalar amplitude; - }; - struct Object - { - btVector3 center; - btVector3 extents; - btBroadphaseProxy* proxy; - btScalar time; - void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) - { - time += speed; - center[0] = btCos(time*(btScalar)2.17)*amplitude+ - btSin(time)*amplitude/2; - center[1] = btCos(time*(btScalar)1.38)*amplitude+ - btSin(time)*amplitude; - center[2] = btSin(time*(btScalar)0.777)*amplitude; - pbi->setAabb(proxy,center-extents,center+extents,0); - } - }; - static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } - static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } - static void OutputTime(const char* name,btClock& c,unsigned count=0) - { - const unsigned long us=c.getTimeMicroseconds(); - const unsigned long ms=(us+500)/1000; - const btScalar sec=us/(btScalar)(1000*1000); - if(count>0) - printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); - else - printf("%s : %u us (%u ms)\r\n",name,us,ms); - } -}; - -void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) -{ - static const btBroadphaseBenchmark::Experiment experiments[]= - { - {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, - /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, - {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ - }; - static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); - btAlignedObjectArray objects; - btClock wallclock; - /* Begin */ - for(int iexp=0;iexpcenter[0]=btBroadphaseBenchmark::UnitRand()*50; - po->center[1]=btBroadphaseBenchmark::UnitRand()*50; - po->center[2]=btBroadphaseBenchmark::UnitRand()*50; - po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; - po->time=btBroadphaseBenchmark::UnitRand()*2000; - po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); - objects.push_back(po); - } - btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); - /* First update */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); - } - btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); - /* Updates */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); - } - pbi->calculateOverlappingPairs(0); - } - btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); - /* Clean up */ - wallclock.reset(); - for(int i=0;idestroyProxy(objects[i]->proxy,0); - delete objects[i]; - } - objects.resize(0); - btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); - } - -} -#else -void btDbvtBroadphase::benchmark(btBroadphaseInterface*) -{} -#endif - -#if DBVT_BP_PROFILE -#undef SPC -#endif - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h deleted file mode 100644 index 18b64ad0e..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ /dev/null @@ -1,146 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///btDbvtBroadphase implementation by Nathanael Presson -#ifndef BT_DBVT_BROADPHASE_H -#define BT_DBVT_BROADPHASE_H - -#include "BulletCollision/BroadphaseCollision/btDbvt.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" - -// -// Compile time config -// - -#define DBVT_BP_PROFILE 0 -//#define DBVT_BP_SORTPAIRS 1 -#define DBVT_BP_PREVENTFALSEUPDATE 0 -#define DBVT_BP_ACCURATESLEEPING 0 -#define DBVT_BP_ENABLE_BENCHMARK 0 -#define DBVT_BP_MARGIN (btScalar)0.05 - -#if DBVT_BP_PROFILE -#define DBVT_BP_PROFILING_RATE 256 -#include "LinearMath/btQuickprof.h" -#endif - -// -// btDbvtProxy -// -struct btDbvtProxy : btBroadphaseProxy -{ - /* Fields */ - //btDbvtAabbMm aabb; - btDbvtNode* leaf; - btDbvtProxy* links[2]; - int stage; - /* ctor */ - btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : - btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) - { - links[0]=links[1]=0; - } -}; - -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 -{ - /* 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? -#if DBVT_BP_PROFILE - btClock m_clock; - struct { - unsigned long m_total; - unsigned long m_ddcollide; - unsigned long m_fdcollide; - unsigned long m_cleanup; - unsigned long m_jobcount; - } m_profiling; -#endif - /* Methods */ - btDbvtBroadphase(btOverlappingPairCache* paircache=0); - ~btDbvtBroadphase(); - void collide(btDispatcher* dispatcher); - void optimize(); - - /* btBroadphaseInterface Implementation */ - btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); - 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) - { - m_prediction = prediction; - } - btScalar getVelocityPrediction() const - { - return m_prediction; - } - - ///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*); - - -}; - -#endif diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.cpp deleted file mode 100644 index 20768225b..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btDispatcher.h" - -btDispatcher::~btDispatcher() -{ - -} - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h deleted file mode 100644 index 89c307d14..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_DISPATCHER_H -#define BT_DISPATCHER_H -#include "LinearMath/btScalar.h" - -class btCollisionAlgorithm; -struct btBroadphaseProxy; -class btRigidBody; -class btCollisionObject; -class btOverlappingPairCache; -struct btCollisionObjectWrapper; - -class btPersistentManifold; -class btPoolAllocator; - -struct btDispatcherInfo -{ - enum DispatchFunc - { - DISPATCH_DISCRETE = 1, - 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) - { - - } - 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; -}; - -///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. -///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic). -class btDispatcher -{ - - -public: - virtual ~btDispatcher() ; - - virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0; - - virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0; - - virtual void releaseManifold(btPersistentManifold* manifold)=0; - - virtual void clearManifold(btPersistentManifold* manifold)=0; - - virtual bool needsCollision(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 int getNumManifolds() const = 0; - - virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0; - - virtual btPersistentManifold** getInternalManifoldPointer() = 0; - - virtual btPoolAllocator* getInternalManifoldPool() = 0; - - virtual const btPoolAllocator* getInternalManifoldPool() const = 0; - - virtual void* allocateCollisionAlgorithm(int size) = 0; - - virtual void freeCollisionAlgorithm(void* ptr) = 0; - -}; - - -#endif //BT_DISPATCHER_H diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp deleted file mode 100644 index 81369fe9b..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ /dev/null @@ -1,489 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiSapBroadphase.h" - -#include "btSimpleBroadphase.h" -#include "LinearMath/btAabbUtil2.h" -#include "btQuantizedBvh.h" - -/// btSapBroadphaseArray m_sapBroadphases; - -/// btOverlappingPairCache* m_overlappingPairs; -extern int gOverlappingPairs; - -/* -class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache -{ -public: - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) - { - return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); - } -}; - -*/ - -btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) -:m_overlappingPairs(pairCache), -m_optimizedAabbTree(0), -m_ownsPairCache(false), -m_invalidPair(0) -{ - if (!m_overlappingPairs) - { - m_ownsPairCache = true; - void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); - m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); - } - - struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback - { - virtual ~btMultiSapOverlapFilterCallback() - {} - // return true when pairs need collision - virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const - { - btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; - btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; - - bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; - collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); - - return collides; - } - }; - - void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); - m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); - - m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); -// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); -// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); -} - -btMultiSapBroadphase::~btMultiSapBroadphase() -{ - if (m_ownsPairCache) - { - m_overlappingPairs->~btOverlappingPairCache(); - btAlignedFree(m_overlappingPairs); - } -} - - -void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) -{ - m_optimizedAabbTree = new btQuantizedBvh(); - m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); - QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); - for (int i=0;igetBroadphaseAabb(aabbMin,aabbMax); - m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); - int partId = 0; - node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; - nodes.push_back(node); - } - m_optimizedAabbTree->buildInternal(); -} - -btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/) -{ - //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested - - void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16); - btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask); - m_multiSapProxies.push_back(proxy); - - ///this should deal with inserting/removal into child broadphases - setAabb(proxy,aabbMin,aabbMax,dispatcher); - return proxy; -} - -void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) -{ - ///not yet - btAssert(0); - -} - - -void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase) -{ - void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16); - btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy; - bridgeProxyRef->m_childProxy = childProxy; - bridgeProxyRef->m_childBroadphase = childBroadphase; - parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef); -} - - -bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax); -bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax) -{ -return -amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() && -amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() && -amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); -} - - - - - - -void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - btMultiSapProxy* multiProxy = static_cast(proxy); - aabbMin = multiProxy->m_aabbMin; - aabbMax = multiProxy->m_aabbMax; -} - -void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) -{ - for (int i=0;i - -void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) -{ - btMultiSapProxy* multiProxy = static_cast(proxy); - multiProxy->m_aabbMin = aabbMin; - multiProxy->m_aabbMax = aabbMax; - - -// bool fullyContained = false; -// bool alreadyInSimple = false; - - - - - struct MyNodeOverlapCallback : public btNodeOverlapCallback - { - btMultiSapBroadphase* m_multiSap; - btMultiSapProxy* m_multiProxy; - btDispatcher* m_dispatcher; - - MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) - :m_multiSap(multiSap), - m_multiProxy(multiProxy), - m_dispatcher(dispatcher) - { - - } - - virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) - { - btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; - - int containingBroadphaseIndex = -1; - //already found? - for (int i=0;im_bridgeProxies.size();i++) - { - - if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) - { - containingBroadphaseIndex = i; - break; - } - } - if (containingBroadphaseIndex<0) - { - //add it - btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy); - m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase); - - } - } - }; - - MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); - - - - - if (m_optimizedAabbTree) - m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - int i; - - for ( i=0;im_bridgeProxies.size();i++) - { - btVector3 worldAabbMin,worldAabbMax; - multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); - bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - if (!overlapsBroadphase) - { - //remove it now - btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; - - btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; - bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); - - multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); - multiProxy->m_bridgeProxies.pop_back(); - - } - } - - - /* - - if (1) - { - - //find broadphase that contain this multiProxy - int numChildBroadphases = getBroadphaseArray().size(); - for (int i=0;igetBroadphaseAabb(worldAabbMin,worldAabbMax); - bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - - // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - int containingBroadphaseIndex = -1; - - //if already contains this - - for (int i=0;im_bridgeProxies.size();i++) - { - if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) - { - containingBroadphaseIndex = i; - } - alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); - } - - if (overlapsBroadphase) - { - if (containingBroadphaseIndex<0) - { - btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,childBroadphase); - } - } else - { - if (containingBroadphaseIndex>=0) - { - //remove - btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; - - btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; - bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); - - multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); - multiProxy->m_bridgeProxies.pop_back(); - } - } - } - - - ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) - ///hopefully we don't end up with many entries here (can assert/provide feedback on stats) - if (0)//!multiProxy->m_bridgeProxies.size()) - { - ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision - ///this is needed to be able to calculate the aabb overlap - btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); - } - } - - if (!multiProxy->m_bridgeProxies.size()) - { - ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision - ///this is needed to be able to calculate the aabb overlap - btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); - } -*/ - - - //update - for ( i=0;im_bridgeProxies.size();i++) - { - btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; - bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); - } - -} -bool stopUpdating=false; - - - -class btMultiSapBroadphasePairSortPredicate -{ - public: - - bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) const - { - btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0; - - return aProxy0 > bProxy0 || - (aProxy0 == bProxy0 && aProxy1 > bProxy1) || - (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); - } -}; - - - ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb -void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - -// m_simpleBroadphase->calculateOverlappingPairs(dispatcher); - - if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); - - // quicksort(overlappingPairArray,0,overlappingPairArray.size()); - - overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - - - int i; - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;im_multiSapParentProxy : 0; - btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0; - btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; - - bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); - - previousPair = pair; - - bool needsRemoval = false; - - if (!isDuplicate) - { - bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); - - if (hasOverlap) - { - needsRemoval = false;//callback->processOverlap(pair); - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); - - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - m_invalidPair++; - gOverlappingPairs--; - } - - } - - ///if you don't like to skip the invalid pairs in the array, execute following code: - #define CLEAN_INVALID_PAIRS 1 - #ifdef CLEAN_INVALID_PAIRS - - //perform a sort, to sort 'invalid' pairs to the end - //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); - overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS - - //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); - } - - -} - - -bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) -{ - btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; - btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; - - return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, - multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); - -} - - -void btMultiSapBroadphase::printStats() -{ -/* printf("---------------------------------\n"); - - printf("btMultiSapBroadphase.h\n"); - printf("numHandles = %d\n",m_multiSapProxies.size()); - //find broadphase that contain this multiProxy - int numChildBroadphases = getBroadphaseArray().size(); - for (int i=0;iprintStats(); - - } - */ - -} - -void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) -{ - // not yet -} diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h deleted file mode 100644 index 7bcfe6b13..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ /dev/null @@ -1,151 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#ifndef BT_MULTI_SAP_BROADPHASE -#define BT_MULTI_SAP_BROADPHASE - -#include "btBroadphaseInterface.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btOverlappingPairCache.h" - - -class btBroadphaseInterface; -class btSimpleBroadphase; - - -typedef btAlignedObjectArray btSapBroadphaseArray; - -///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead. -///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases. -///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time. -///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy. -///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328 -///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329 -class btMultiSapBroadphase :public btBroadphaseInterface -{ - btSapBroadphaseArray m_sapBroadphases; - - btSimpleBroadphase* m_simpleBroadphase; - - btOverlappingPairCache* m_overlappingPairs; - - class btQuantizedBvh* m_optimizedAabbTree; - - - bool m_ownsPairCache; - - btOverlapFilterCallback* m_filterCallback; - - int m_invalidPair; - - struct btBridgeProxy - { - btBroadphaseProxy* m_childProxy; - btBroadphaseInterface* m_childBroadphase; - }; - - -public: - - struct btMultiSapProxy : public btBroadphaseProxy - { - - ///array with all the entries that this proxy belongs to - btAlignedObjectArray m_bridgeProxies; - btVector3 m_aabbMin; - btVector3 m_aabbMax; - - int m_shapeType; - -/* void* m_userPtr; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; -*/ - btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) - :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax), - m_shapeType(shapeType) - { - m_multiSapParentProxy =this; - } - - - }; - -protected: - - - btAlignedObjectArray m_multiSapProxies; - -public: - - btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0); - - - btSapBroadphaseArray& getBroadphaseArray() - { - return m_sapBroadphases; - } - - const btSapBroadphaseArray& getBroadphaseArray() const - { - return m_sapBroadphases; - } - - virtual ~btMultiSapBroadphase(); - - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); - - void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase); - - ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - - bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - virtual btOverlappingPairCache* getOverlappingPairCache() - { - return m_overlappingPairs; - } - virtual const btOverlappingPairCache* getOverlappingPairCache() const - { - return m_overlappingPairs; - } - - ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame - ///will add some transform later - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); - aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); - } - - void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); - - virtual void printStats(); - - void quicksort (btBroadphasePairArray& a, int lo, int hi); - - ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher); - -}; - -#endif //BT_MULTI_SAP_BROADPHASE diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp deleted file mode 100644 index ae22dadc7..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ /dev/null @@ -1,633 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btOverlappingPairCache.h" - -#include "btDispatcher.h" -#include "btCollisionAlgorithm.h" -#include "LinearMath/btAabbUtil2.h" - -#include - -int gOverlappingPairs = 0; - -int gRemovePairs =0; -int gAddedPairs =0; -int gFindPairs =0; - - - - -btHashedOverlappingPairCache::btHashedOverlappingPairCache(): - m_overlapFilterCallback(0), - m_blockedForChanges(false), - m_ghostPairCallback(0) -{ - int initialAllocatedSize= 2; - m_overlappingPairArray.reserve(initialAllocatedSize); - growTables(); -} - - - - -btHashedOverlappingPairCache::~btHashedOverlappingPairCache() -{ -} - - - -void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) -{ - if (pair.m_algorithm && dispatcher) - { - { - pair.m_algorithm->~btCollisionAlgorithm(); - dispatcher->freeCollisionAlgorithm(pair.m_algorithm); - pair.m_algorithm=0; - } - } -} - - - - -void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class CleanPairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; - btDispatcher* m_dispatcher; - - public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - if ((pair.m_pProxy0 == m_cleanProxy) || - (pair.m_pProxy1 == m_cleanProxy)) - { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); - } - return false; - } - - }; - - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); - -} - - - - -void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class RemovePairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_obsoleteProxy; - - public: - RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); - } - - }; - - - RemovePairCallback removeCallback(proxy); - - processAllOverlappingPairs(&removeCallback,dispatcher); -} - - - - - -btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) -{ - gFindPairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); - int proxyId1 = proxy0->getUid(); - int proxyId2 = proxy1->getUid(); - - /*if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2);*/ - - int hash = static_cast(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 != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) - { - index = m_next[index]; - } - - if (index == BT_NULL_PAIR) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; -} - -//#include - -void btHashedOverlappingPairCache::growTables() -{ - - int newCapacity = m_overlappingPairArray.capacity(); - - if (m_hashTable.size() < newCapacity) - { - //grow hashtable and next table - int curHashtableSize = m_hashTable.size(); - - m_hashTable.resize(newCapacity); - m_next.resize(newCapacity); - - - int i; - - for (i= 0; i < newCapacity; ++i) - { - m_hashTable[i] = BT_NULL_PAIR; - } - for (i = 0; i < newCapacity; ++i) - { - m_next[i] = BT_NULL_PAIR; - } - - for(i=0;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 - m_next[i] = m_hashTable[hashValue]; - m_hashTable[hashValue] = i; - } - - - } -} - -btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) -{ - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); - int proxyId1 = proxy0->getUid(); - int proxyId2 = proxy1->getUid(); - - /*if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2);*/ - - int hash = static_cast(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) - { - 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(); - void* mem = &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 = new (mem) btBroadphasePair(*proxy0,*proxy1); -// pair->m_pProxy0 = proxy0; -// pair->m_pProxy1 = proxy1; - pair->m_algorithm = 0; - pair->m_internalTmpValue = 0; - - - m_next[count] = m_hashTable[hash]; - m_hashTable[hash] = count; - - return pair; -} - - - -void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) -{ - gRemovePairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); - int proxyId1 = proxy0->getUid(); - int proxyId2 = proxy1->getUid(); - - /*if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2);*/ - - int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); - - btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); - if (pair == NULL) - { - return 0; - } - - cleanOverlappingPair(*pair,dispatcher); - - void* userData = pair->m_internalInfo1; - - btAssert(pair->m_pProxy0->getUid() == proxyId1); - btAssert(pair->m_pProxy1->getUid() == proxyId2); - - int pairIndex = int(pair - &m_overlappingPairArray[0]); - btAssert(pairIndex < m_overlappingPairArray.size()); - - // Remove the pair from the hash table. - int index = m_hashTable[hash]; - btAssert(index != BT_NULL_PAIR); - - int previous = BT_NULL_PAIR; - while (index != pairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_NULL_PAIR) - { - btAssert(m_next[previous] == pairIndex); - m_next[previous] = m_next[pairIndex]; - } - else - { - m_hashTable[hash] = m_next[pairIndex]; - } - - // We now move the last pair into spot of the - // pair being removed. We need to fix the hash - // table indices to support the move. - - int lastPairIndex = m_overlappingPairArray.size() - 1; - - if (m_ghostPairCallback) - m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); - - // If the removed pair is the last pair, we are done. - if (lastPairIndex == pairIndex) - { - m_overlappingPairArray.pop_back(); - return userData; - } - - // Remove the last pair from the hash table. - const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; - /* missing swap here too, Nat. */ - int lastHash = static_cast(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); - - previous = BT_NULL_PAIR; - while (index != lastPairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_NULL_PAIR) - { - btAssert(m_next[previous] == lastPairIndex); - m_next[previous] = m_next[lastPairIndex]; - } - else - { - m_hashTable[lastHash] = m_next[lastPairIndex]; - } - - // Copy the last pair into the remove pair's spot. - m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; - - // Insert the last pair into the hash table - m_next[pairIndex] = m_hashTable[lastHash]; - m_hashTable[lastHash] = pairIndex; - - m_overlappingPairArray.pop_back(); - - return userData; -} -//#include - -void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) -{ - - 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 - { - i++; - } - } -} - -void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) -{ - ///need to keep hashmap in sync with pair address, so rebuild all - btBroadphasePairArray tmpPairs; - int i; - for (i=0;iremoveOverlappingPair(proxy0, proxy1,dispatcher); - - m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); - m_overlappingPairArray.pop_back(); - return userData; - } - } - - return 0; -} - - - - - - - - -btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) -{ - //don't add overlap with own - btAssert(proxy0 != proxy1); - - if (!needsBroadphaseCollision(proxy0,proxy1)) - return 0; - - void* mem = &m_overlappingPairArray.expandNonInitializing(); - btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); - - gOverlappingPairs++; - gAddedPairs++; - - if (m_ghostPairCallback) - m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); - return pair; - -} - -///this findPair becomes really slow. Either sort the list to speedup the query, or -///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. -///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) -///Also we can use a 2D bitmap, which can be useful for a future GPU implementation - btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) -{ - if (!needsBroadphaseCollision(proxy0,proxy1)) - return 0; - - btBroadphasePair tmpPair(*proxy0,*proxy1); - int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); - - if (findIndex < m_overlappingPairArray.size()) - { - //btAssert(it != m_overlappingPairSet.end()); - btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; - return pair; - } - return 0; -} - - - - - - - - - - -//#include - -void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) -{ - - int i; - - for (i=0;iprocessOverlap(*pair)) - { - cleanOverlappingPair(*pair,dispatcher); - pair->m_pProxy0 = 0; - pair->m_pProxy1 = 0; - m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - m_overlappingPairArray.pop_back(); - gOverlappingPairs--; - } else - { - i++; - } - } -} - - - - -btSortedOverlappingPairCache::btSortedOverlappingPairCache(): - m_blockedForChanges(false), - m_hasDeferredRemoval(true), - m_overlapFilterCallback(0), - m_ghostPairCallback(0) -{ - int initialAllocatedSize= 2; - m_overlappingPairArray.reserve(initialAllocatedSize); -} - -btSortedOverlappingPairCache::~btSortedOverlappingPairCache() -{ -} - -void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) -{ - if (pair.m_algorithm) - { - { - pair.m_algorithm->~btCollisionAlgorithm(); - dispatcher->freeCollisionAlgorithm(pair.m_algorithm); - pair.m_algorithm=0; - gRemovePairs--; - } - } -} - - -void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class CleanPairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; - btDispatcher* m_dispatcher; - - public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - if ((pair.m_pProxy0 == m_cleanProxy) || - (pair.m_pProxy1 == m_cleanProxy)) - { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); - } - return false; - } - - }; - - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); - -} - - -void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class RemovePairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_obsoleteProxy; - - public: - RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); - } - - }; - - RemovePairCallback removeCallback(proxy); - - processAllOverlappingPairs(&removeCallback,dispatcher); -} - -void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) -{ - //should already be sorted -} - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h deleted file mode 100644 index eee90e473..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ /dev/null @@ -1,470 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_OVERLAPPING_PAIR_CACHE_H -#define BT_OVERLAPPING_PAIR_CACHE_H - - -#include "btBroadphaseInterface.h" -#include "btBroadphaseProxy.h" -#include "btOverlappingPairCallback.h" - -#include "LinearMath/btAlignedObjectArray.h" -class btDispatcher; - -typedef btAlignedObjectArray btBroadphasePairArray; - -struct btOverlapCallback -{ - virtual ~btOverlapCallback() - {} - //return true for deletion of the pair - 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; -}; - - - - - - - -extern int gRemovePairs; -extern int gAddedPairs; -extern int gFindPairs; - -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 btBroadphasePair* getOverlappingPairArrayPtr() = 0; - - virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 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 setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; - - virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 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 -class btHashedOverlappingPairCache : public btOverlappingPairCache -{ - btBroadphasePairArray m_overlappingPairArray; - btOverlapFilterCallback* m_overlapFilterCallback; - bool m_blockedForChanges; - -protected: - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - btOverlappingPairCallback* m_ghostPairCallback; - - -public: - btHashedOverlappingPairCache(); - virtual ~btHashedOverlappingPairCache(); - - - 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 - { - 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; - } - - // 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) - { - gAddedPairs++; - - if (!needsBroadphaseCollision(proxy0,proxy1)) - return 0; - - return internalAddPair(proxy0,proxy1); - } - - - - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); - - virtual btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - const btBroadphasePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } - - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - - - - btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); - - int GetCount() const { return m_overlappingPairArray.size(); } -// btBroadphasePair* GetPairs() { return m_pairs; } - - btOverlapFilterCallback* getOverlapFilterCallback() - { - return m_overlapFilterCallback; - } - - void setOverlapFilterCallback(btOverlapFilterCallback* callback) - { - m_overlapFilterCallback = callback; - } - - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } -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; - } - - /* - // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm - // This assumes proxyId1 and proxyId2 are 16-bit. - SIMD_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; - } - */ - - - - SIMD_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); - } - - - - - - 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 (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2); - #endif - - int index = m_hashTable[hash]; - - while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) - { - index = m_next[index]; - } - - if ( index == BT_NULL_PAIR ) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; - } - - virtual bool hasDeferredRemoval() - { - return false; - } - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) - { - m_ghostPairCallback = ghostPairCallback; - } - - virtual void sortOverlappingPairs(btDispatcher* dispatcher); - - - -}; - - - - -///btSortedOverlappingPairCache maintains the objects with overlapping AABB -///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase -class btSortedOverlappingPairCache : public btOverlappingPairCache -{ - 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; - - ///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; - - btOverlappingPairCallback* m_ghostPairCallback; - - public: - - btSortedOverlappingPairCache(); - virtual ~btSortedOverlappingPairCache(); - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); - - void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); - - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - - btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - - - 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; - } - - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - const btBroadphasePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } - - - - - btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } - - btOverlapFilterCallback* getOverlapFilterCallback() - { - return m_overlapFilterCallback; - } - - 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; - -public: - - virtual btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) - { - - } - - virtual int getNumOverlappingPairs() const - { - return 0; - } - - virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) - { - - } - - virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) - { - } - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) - { - } - - virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/) - { - return 0; - } - - virtual bool hasDeferredRemoval() - { - return true; - } - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) - { - - } - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) - { - return 0; - } - - virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/) - { - return 0; - } - - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) - { - } - - virtual void sortOverlappingPairs(btDispatcher* dispatcher) - { - (void) dispatcher; - } - - -}; - - -#endif //BT_OVERLAPPING_PAIR_CACHE_H - - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h deleted file mode 100644 index 9c7b6f813..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h +++ /dev/null @@ -1,40 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef OVERLAPPING_PAIR_CALLBACK_H -#define OVERLAPPING_PAIR_CALLBACK_H - -class btDispatcher; -struct btBroadphasePair; - -///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. -class btOverlappingPairCallback -{ -public: - virtual ~btOverlappingPairCallback() - { - - } - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) = 0; - - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) = 0; - - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) = 0; - -}; - -#endif //OVERLAPPING_PAIR_CALLBACK_H diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp deleted file mode 100644 index 889216df5..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ /dev/null @@ -1,1393 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btQuantizedBvh.h" - -#include "LinearMath/btAabbUtil2.h" -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btSerializer.h" - -#define RAYAABB2 - -btQuantizedBvh::btQuantizedBvh() : - m_bulletVersion(BT_BULLET_VERSION), - m_useQuantization(false), - //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) - m_traversalMode(TRAVERSAL_STACKLESS) - //m_traversalMode(TRAVERSAL_RECURSIVE) - ,m_subtreeHeaderCount(0) //PCK: add this line -{ - m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY); - m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); -} - - - - - -void btQuantizedBvh::buildInternal() -{ - ///assumes that caller filled in the m_quantizedLeafNodes - m_useQuantization = true; - int numLeafNodes = 0; - - if (m_useQuantization) - { - //now we have an array of leafnodes in m_leafNodes - numLeafNodes = m_quantizedLeafNodes.size(); - - m_quantizedContiguousNodes.resize(2*numLeafNodes); - - } - - m_curNodeIndex = 0; - - buildTree(0,numLeafNodes); - - ///if the entire tree is small then subtree size, we need to create a header info for the tree - if(m_useQuantization && !m_SubtreeHeaders.size()) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); - subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); - subtree.m_rootNodeIndex = 0; - subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); - } - - //PCK: update the copy of the size - m_subtreeHeaderCount = m_SubtreeHeaders.size(); - - //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary - m_quantizedLeafNodes.clear(); - m_leafNodes.clear(); -} - - - -///just for debugging, to visualize the individual patches/subtrees -#ifdef DEBUG_PATCH_COLORS -btVector3 color[4]= -{ - btVector3(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) -{ - //enlarge the AABB to avoid division by zero when initializing the quantization values - btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); - m_bvhAabbMin = bvhAabbMin - clampValue; - m_bvhAabbMax = bvhAabbMax + clampValue; - btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; - m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; - - m_useQuantization = true; - - { - unsigned short vecIn[3]; - btVector3 v; - { - quantize(vecIn,m_bvhAabbMin,false); - v = unQuantize(vecIn); - m_bvhAabbMin.setMin(v-clampValue); - } - { - 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() -{ -} - -#ifdef DEBUG_TREE_BUILDING -int gStackDepth = 0; -int gMaxStackDepth = 0; -#endif //DEBUG_TREE_BUILDING - -void btQuantizedBvh::buildTree (int startIndex,int endIndex) -{ -#ifdef DEBUG_TREE_BUILDING - gStackDepth++; - if (gStackDepth > gMaxStackDepth) - gMaxStackDepth = gStackDepth; -#endif //DEBUG_TREE_BUILDING - - - int splitAxis, splitIndex, i; - int numIndices =endIndex-startIndex; - int curIndex = m_curNodeIndex; - - btAssert(numIndices>0); - - if (numIndices==1) - { -#ifdef DEBUG_TREE_BUILDING - gStackDepth--; -#endif //DEBUG_TREE_BUILDING - - assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); - - m_curNodeIndex++; - return; - } - //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. - - splitAxis = calcSplittingAxis(startIndex,endIndex); - - splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); - - int internalNodeIndex = m_curNodeIndex; - - //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value. - //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values - setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization - setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization - - - for (i=startIndex;im_escapeIndex; - - int leftChildNodexIndex = m_curNodeIndex; - - //build left child tree - buildTree(startIndex,splitIndex); - - int rightChildNodexIndex = m_curNodeIndex; - //build right child tree - buildTree(splitIndex,endIndex); - -#ifdef DEBUG_TREE_BUILDING - gStackDepth--; -#endif //DEBUG_TREE_BUILDING - - int escapeIndex = m_curNodeIndex - curIndex; - - if (m_useQuantization) - { - //escapeIndex is the number of nodes of this subtree - const int sizeQuantizedNode =sizeof(btQuantizedBvhNode); - const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; - if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) - { - updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); - } - } else - { - - } - - setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); - -} - -void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) -{ - btAssert(m_useQuantization); - - btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; - int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); - int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); - - btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; - int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); - int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); - - if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); - subtree.setAabbFromQuantizeNode(leftChildNode); - subtree.m_rootNodeIndex = leftChildNodexIndex; - subtree.m_subtreeSize = leftSubTreeSize; - } - - if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); - subtree.setAabbFromQuantizeNode(rightChildNode); - subtree.m_rootNodeIndex = rightChildNodexIndex; - subtree.m_subtreeSize = rightSubTreeSize; - } - - //PCK: update the copy of the size - m_subtreeHeaderCount = m_SubtreeHeaders.size(); -} - - -int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) -{ - int i; - int splitIndex =startIndex; - int numIndices = endIndex - startIndex; - btScalar splitValue; - - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i splitValue) - { - //swap - swapLeafNodes(i,splitIndex); - splitIndex++; - } - } - - //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex - //otherwise the tree-building might fail due to stack-overflows in certain cases. - //unbalanced1 is unsafe: it can cause stack overflows - //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); - - //unbalanced2 should work too: always use center (perfect balanced trees) - //bool unbalanced2 = true; - - //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); - - if (unbalanced) - { - splitIndex = startIndex+ (numIndices>>1); - } - - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - (void)unbal; - btAssert(!unbal); - - return splitIndex; -} - - -int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) -{ - int i; - - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - int numIndices = endIndex-startIndex; - - for (i=startIndex;im_aabbMinOrg,rootNode->m_aabbMaxOrg); - isLeafNode = rootNode->m_escapeIndex == -1; - - //PCK: unsigned instead of bool - if (isLeafNode && (aabbOverlap != 0)) - { - nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); - } - - //PCK: unsigned instead of bool - if ((aabbOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->m_escapeIndex; - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - -/* -///this was the original recursive traversal, before we optimized towards stackless traversal -void btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const -{ - bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); - if (aabbOverlap) - { - isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); - if (isLeafNode) - { - nodeCallback->processNode(rootNode); - } else - { - walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax); - walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax); - } - } - -} -*/ - -void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const -{ - btAssert(m_useQuantization); - - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned aabbOverlap; - - //PCK: unsigned instead of bool - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); - isLeafNode = currentNode->isLeafNode(); - - //PCK: unsigned instead of bool - if (aabbOverlap != 0) - { - if (isLeafNode) - { - nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex()); - } else - { - //process left and right children - const btQuantizedBvhNode* leftChildNode = currentNode+1; - walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); - - const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); - walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); - } - } -} - - - -void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const -{ - btAssert(!m_useQuantization); - - const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0]; - int escapeIndex, curIndex = 0; - int walkIterations = 0; - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned aabbOverlap=0; - unsigned rayBoxOverlap=0; - btScalar lambda_max = 1.0; - - /* Quick pruning by quantized box */ - btVector3 rayAabbMin = raySource; - btVector3 rayAabbMax = raySource; - rayAabbMin.setMin(rayTarget); - rayAabbMax.setMax(rayTarget); - - /* Add box cast extents to bounding box */ - rayAabbMin += aabbMin; - rayAabbMax += aabbMax; - -#ifdef RAYAABB2 - btVector3 rayDir = (rayTarget-raySource); - rayDir.normalize (); - lambda_max = rayDir.dot(rayTarget-raySource); - ///what about division by zero? --> just set rayDirection[i] to 1.0 - btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; -#endif - - btVector3 bounds[2]; - - while (curIndex < m_curNodeIndex) - { - btScalar param = 1.0; - //catch bugs in tree data - btAssert (walkIterations < m_curNodeIndex); - - walkIterations++; - - bounds[0] = rootNode->m_aabbMinOrg; - bounds[1] = rootNode->m_aabbMaxOrg; - /* Add box cast extents */ - bounds[0] -= aabbMax; - bounds[1] -= aabbMin; - - aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); - //perhaps profile if it is worth doing the aabbOverlap test first - -#ifdef RAYAABB2 - ///careful with this check: need to check division by zero (above) and fix the unQuantize method - ///thanks Joerg/hiker for the reproduction case! - ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 - rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; - -#else - btVector3 normal; - rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal); -#endif - - isLeafNode = rootNode->m_escapeIndex == -1; - - //PCK: unsigned instead of bool - if (isLeafNode && (rayBoxOverlap != 0)) - { - nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); - } - - //PCK: unsigned instead of bool - if ((rayBoxOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->m_escapeIndex; - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - - - -void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const -{ - btAssert(m_useQuantization); - - int curIndex = startNodeIndex; - int walkIterations = 0; - int subTreeSize = endNodeIndex - startNodeIndex; - (void)subTreeSize; - - const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; - int escapeIndex; - - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned boxBoxOverlap = 0; - unsigned rayBoxOverlap = 0; - - btScalar lambda_max = 1.0; - -#ifdef RAYAABB2 - btVector3 rayDirection = (rayTarget-raySource); - rayDirection.normalize (); - lambda_max = rayDirection.dot(rayTarget-raySource); - ///what about division by zero? --> just set rayDirection[i] to 1.0 - rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[0]; - rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[1]; - rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDirection[2]; - unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; -#endif - - /* Quick pruning by quantized box */ - btVector3 rayAabbMin = raySource; - btVector3 rayAabbMax = raySource; - rayAabbMin.setMin(rayTarget); - rayAabbMax.setMax(rayTarget); - - /* Add box cast extents to bounding box */ - rayAabbMin += aabbMin; - rayAabbMax += aabbMax; - - unsigned short int quantizedQueryAabbMin[3]; - unsigned short int quantizedQueryAabbMax[3]; - quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0); - quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1); - - while (curIndex < endNodeIndex) - { - -//#define VISUALLY_ANALYZE_BVH 1 -#ifdef VISUALLY_ANALYZE_BVH - //some code snippet to debugDraw aabb, to visually analyze bvh structure - static int drawPatch = 0; - //need some global access to a debugDrawer - extern btIDebugDraw* debugDrawerPtr; - if (curIndex==drawPatch) - { - btVector3 aabbMin,aabbMax; - aabbMin = unQuantize(rootNode->m_quantizedAabbMin); - aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); - } -#endif//VISUALLY_ANALYZE_BVH - - //catch bugs in tree data - btAssert (walkIterations < subTreeSize); - - walkIterations++; - //PCK: unsigned instead of bool - // only interested if this is closer than any previous hit - btScalar param = 1.0; - rayBoxOverlap = 0; - boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); - isLeafNode = rootNode->isLeafNode(); - if (boxBoxOverlap) - { - btVector3 bounds[2]; - bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); - bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); - /* Add box cast extents */ - bounds[0] -= aabbMax; - bounds[1] -= aabbMin; - btVector3 normal; -#if 0 - bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); - bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal); - if (ra2 != ra) - { - printf("functions don't match\n"); - } -#endif -#ifdef RAYAABB2 - ///careful with this check: need to check division by zero (above) and fix the unQuantize method - ///thanks Joerg/hiker for the reproduction case! - ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 - - //BT_PROFILE("btRayAabb2"); - rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); - -#else - rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); -#endif - } - - if (isLeafNode && rayBoxOverlap) - { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); - } - - //PCK: unsigned instead of bool - if ((rayBoxOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->getEscapeIndex(); - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - -void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const -{ - btAssert(m_useQuantization); - - int curIndex = startNodeIndex; - int walkIterations = 0; - int subTreeSize = endNodeIndex - startNodeIndex; - (void)subTreeSize; - - const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; - int escapeIndex; - - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned aabbOverlap; - - while (curIndex < endNodeIndex) - { - -//#define VISUALLY_ANALYZE_BVH 1 -#ifdef VISUALLY_ANALYZE_BVH - //some code snippet to debugDraw aabb, to visually analyze bvh structure - static int drawPatch = 0; - //need some global access to a debugDrawer - extern btIDebugDraw* debugDrawerPtr; - if (curIndex==drawPatch) - { - btVector3 aabbMin,aabbMax; - aabbMin = unQuantize(rootNode->m_quantizedAabbMin); - aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); - } -#endif//VISUALLY_ANALYZE_BVH - - //catch bugs in tree data - btAssert (walkIterations < subTreeSize); - - walkIterations++; - //PCK: unsigned instead of bool - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); - isLeafNode = rootNode->isLeafNode(); - - if (isLeafNode && aabbOverlap) - { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); - } - - //PCK: unsigned instead of bool - if ((aabbOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->getEscapeIndex(); - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - -//This traversal can be called from Playstation 3 SPU -void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const -{ - btAssert(m_useQuantization); - - int i; - - - for (i=0;im_SubtreeHeaders.size();i++) - { - const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - - //PCK: unsigned instead of bool - unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); - if (overlap != 0) - { - walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, - subtree.m_rootNodeIndex, - subtree.m_rootNodeIndex+subtree.m_subtreeSize); - } - } -} - - -void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const -{ - reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0)); -} - - -void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const -{ - //always use stackless - - if (m_useQuantization) - { - walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); - } - else - { - walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); - } - /* - { - //recursive traversal - btVector3 qaabbMin = raySource; - btVector3 qaabbMax = raySource; - qaabbMin.setMin(rayTarget); - qaabbMax.setMax(rayTarget); - qaabbMin += aabbMin; - qaabbMax += aabbMax; - reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); - } - */ - -} - - -void btQuantizedBvh::swapLeafNodes(int i,int splitIndex) -{ - if (m_useQuantization) - { - btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; - m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; - m_quantizedLeafNodes[splitIndex] = tmp; - } else - { - btOptimizedBvhNode tmp = m_leafNodes[i]; - m_leafNodes[i] = m_leafNodes[splitIndex]; - m_leafNodes[splitIndex] = tmp; - } -} - -void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) -{ - if (m_useQuantization) - { - m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; - } else - { - m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; - } -} - -//PCK: include -#include - -#if 0 -//PCK: consts -static const unsigned BVH_ALIGNMENT = 16; -static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; - -static const unsigned BVH_ALIGNMENT_BLOCKS = 2; -#endif - - -unsigned int btQuantizedBvh::getAlignmentSerializationPadding() -{ - // I changed this to 0 since the extra padding is not needed or used. - return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; -} - -unsigned btQuantizedBvh::calculateSerializeBufferSize() const -{ - unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding(); - baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; - if (m_useQuantization) - { - return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode); - } - return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); -} - -bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) 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)) - { - ///check alignedment for buffer? - btAssert(0); - return false; - } -*/ - - btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer; - - // construct the class so the virtual function table, etc will be set up - // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor - new (targetBvh) btQuantizedBvh; - - if (i_swapEndian) - { - targetBvh->m_curNodeIndex = static_cast(btSwapEndian(m_curNodeIndex)); - - - btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin); - btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax); - btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization); - - targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode); - targetBvh->m_subtreeHeaderCount = static_cast(btSwapEndian(m_subtreeHeaderCount)); - } - else - { - targetBvh->m_curNodeIndex = m_curNodeIndex; - targetBvh->m_bvhAabbMin = m_bvhAabbMin; - targetBvh->m_bvhAabbMax = m_bvhAabbMax; - targetBvh->m_bvhQuantization = m_bvhQuantization; - targetBvh->m_traversalMode = m_traversalMode; - targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount; - } - - targetBvh->m_useQuantization = m_useQuantization; - - unsigned char *nodeData = (unsigned char *)targetBvh; - nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - int nodeCount = m_curNodeIndex; - - if (m_useQuantization) - { - targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); - } - } - else - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]; - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]; - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex; - - - } - } - nodeData += sizeof(btQuantizedBvhNode) * nodeCount; - - // this clears the pointer in the member variable it doesn't really do anything to the data - // it does call the destructor on the contained objects, but they are all classes with no destructor defined - // so the memory (which is not freed) is left alone - targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(NULL, 0, 0); - } - else - { - targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); - btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); - - targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex)); - targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart)); - targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex)); - } - } - else - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg; - targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg; - - targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex; - targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart; - targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex; - } - } - nodeData += sizeof(btOptimizedBvhNode) * nodeCount; - - // this clears the pointer in the member variable it doesn't really do anything to the data - // it does call the destructor on the contained objects, but they are all classes with no destructor defined - // so the memory (which is not freed) is left alone - targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0); - } - - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - // Now serialize the subtree headers - targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount); - if (i_swapEndian) - { - for (int i = 0; i < m_subtreeHeaderCount; i++) - { - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]); - - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]); - - targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex)); - targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize)); - } - } - else - { - for (int i = 0; i < m_subtreeHeaderCount; i++) - { - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]); - - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]); - - targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex); - targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize); - - // need to clear padding in destination buffer - targetBvh->m_SubtreeHeaders[i].m_padding[0] = 0; - targetBvh->m_SubtreeHeaders[i].m_padding[1] = 0; - targetBvh->m_SubtreeHeaders[i].m_padding[2] = 0; - } - } - nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; - - // this clears the pointer in the member variable it doesn't really do anything to the data - // it does call the destructor on the contained objects, but they are all classes with no destructor defined - // so the memory (which is not freed) is left alone - targetBvh->m_SubtreeHeaders.initializeFromBuffer(NULL, 0, 0); - - // this wipes the virtual function table pointer at the start of the buffer for the class - *((void**)o_alignedDataBuffer) = NULL; - - return true; -} - -btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) -{ - - if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) - { - return NULL; - } - btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer; - - if (i_swapEndian) - { - bvh->m_curNodeIndex = static_cast(btSwapEndian(bvh->m_curNodeIndex)); - - btUnSwapVector3Endian(bvh->m_bvhAabbMin); - btUnSwapVector3Endian(bvh->m_bvhAabbMax); - btUnSwapVector3Endian(bvh->m_bvhQuantization); - - bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode); - bvh->m_subtreeHeaderCount = static_cast(btSwapEndian(bvh->m_subtreeHeaderCount)); - } - - unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize(); - btAssert(calculatedBufSize <= i_dataBufferSize); - - if (calculatedBufSize > i_dataBufferSize) - { - return NULL; - } - - unsigned char *nodeData = (unsigned char *)bvh; - nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - int nodeCount = bvh->m_curNodeIndex; - - // Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor - // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor - new (bvh) btQuantizedBvh(*bvh, false); - - if (bvh->m_useQuantization) - { - bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); - - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); - - bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); - } - } - nodeData += sizeof(btQuantizedBvhNode) * nodeCount; - } - else - { - bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); - btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); - - bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(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)); - } - } - nodeData += sizeof(btOptimizedBvhNode) * nodeCount; - } - - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - // Now serialize the subtree headers - bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount); - if (i_swapEndian) - { - for (int i = 0; i < bvh->m_subtreeHeaderCount; i++) - { - bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]); - - bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]); - - bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex)); - bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize)); - } - } - - return bvh; -} - -// Constructor that prevents btVector3's default constructor from being called -btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : -m_bvhAabbMin(self.m_bvhAabbMin), -m_bvhAabbMax(self.m_bvhAabbMax), -m_bvhQuantization(self.m_bvhQuantization), -m_bulletVersion(BT_BULLET_VERSION) -{ - -} - -void btQuantizedBvh::deSerializeFloat(struct btQuantizedBvhFloatData& 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) - { - btOptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr; - - for (int i=0;im_aabbMaxOrg); - m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg); - m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex; - m_contiguousNodes[i].m_subPart = memPtr->m_subPart; - m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex; - } - } - } - - { - int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes; - m_quantizedContiguousNodes.resize(numElem); - - if (numElem) - { - btQuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr; - for (int i=0;im_escapeIndexOrTriangleIndex; - m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; - m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; - m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; - m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; - m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; - m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; - } - } - } - - m_traversalMode = 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[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 btQuantizedBvh::deSerializeDouble(struct btQuantizedBvhDoubleData& 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) - { - btOptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr; - - for (int i=0;im_aabbMaxOrg); - m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg); - m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex; - m_contiguousNodes[i].m_subPart = memPtr->m_subPart; - m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex; - } - } - } - - { - int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes; - m_quantizedContiguousNodes.resize(numElem); - - if (numElem) - { - btQuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr; - for (int i=0;im_escapeIndexOrTriangleIndex; - m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; - m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; - m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; - m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; - m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; - m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; - } - } - } - - m_traversalMode = 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[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* 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); - if (quantizedData->m_contiguousNodesPtr) - { - int sz = sizeof(btOptimizedBvhNodeData); - int numElem = m_contiguousNodes.size(); - 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); - memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex; - memPtr->m_subPart = m_contiguousNodes[i].m_subPart; - memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex; - } - 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); - if (quantizedData->m_quantizedContiguousNodesPtr) - { - int sz = sizeof(btQuantizedBvhNodeData); - int numElem = m_quantizedContiguousNodes.size(); - 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]; - memPtr->m_quantizedAabbMax[1] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[1]; - memPtr->m_quantizedAabbMax[2] = m_quantizedContiguousNodes[i].m_quantizedAabbMax[2]; - memPtr->m_quantizedAabbMin[0] = m_quantizedContiguousNodes[i].m_quantizedAabbMin[0]; - 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]); - } - - 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); - if (quantizedData->m_subTreeInfoPtr) - { - int sz = sizeof(btBvhSubtreeInfoData); - int numElem = m_SubtreeHeaders.size(); - 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]; - memPtr->m_quantizedAabbMax[2] = m_SubtreeHeaders[i].m_quantizedAabbMax[2]; - memPtr->m_quantizedAabbMin[0] = m_SubtreeHeaders[i].m_quantizedAabbMin[0]; - memPtr->m_quantizedAabbMin[1] = m_SubtreeHeaders[i].m_quantizedAabbMin[1]; - memPtr->m_quantizedAabbMin[2] = m_SubtreeHeaders[i].m_quantizedAabbMin[2]; - - 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]); - } - return btQuantizedBvhDataName; -} - - - - - diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.h deleted file mode 100644 index 78382da79..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ /dev/null @@ -1,581 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_QUANTIZED_BVH_H -#define BT_QUANTIZED_BVH_H - -class btSerializer; - -//#define DEBUG_CHECK_DEQUANTIZATION 1 -#ifdef DEBUG_CHECK_DEQUANTIZATION -#ifdef __SPU__ -#define printf spu_printf -#endif //__SPU__ - -#include -#include -#endif //DEBUG_CHECK_DEQUANTIZATION - -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedAllocator.h" - -#ifdef BT_USE_DOUBLE_PRECISION -#define btQuantizedBvhData btQuantizedBvhDoubleData -#define btOptimizedBvhNodeData btOptimizedBvhNodeDoubleData -#define btQuantizedBvhDataName "btQuantizedBvhDoubleData" -#else -#define btQuantizedBvhData btQuantizedBvhFloatData -#define btOptimizedBvhNodeData btOptimizedBvhNodeFloatData -#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 - -// 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 - -///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 -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; - //4 bytes - int m_escapeIndexOrTriangleIndex; - - bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrTriangleIndex >= 0); - } - int getEscapeIndex() const - { - btAssert(!isLeafNode()); - return -m_escapeIndexOrTriangleIndex; - } - int getTriangleIndex() const - { - btAssert(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 - { - btAssert(isLeafNode()); - // Get only the highest bits where the part index is stored - 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 -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - //32 bytes - btVector3 m_aabbMinOrg; - btVector3 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]; -}; - - -///btBvhSubtreeInfo provides info to gather a subtree of limited size -ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo -{ -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - //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() - { - //memset(&m_padding[0], 0, sizeof(m_padding)); - } - - - void setAabbFromQuantizeNode(const btQuantizedBvhNode& 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 btNodeOverlapCallback -{ -public: - virtual ~btNodeOverlapCallback() {}; - - virtual void processNode(int subPart, int triangleIndex) = 0; -}; - -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btAlignedObjectArray.h" - - - -///for code readability: -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, and by the btMultiSapBroadphase. -///It is recommended to use quantization for better performance and lower memory requirements. -ATTRIBUTE_ALIGNED16(class) btQuantizedBvh -{ -public: - enum btTraversalMode - { - TRAVERSAL_STACKLESS = 0, - TRAVERSAL_STACKLESS_CACHE_FRIENDLY, - TRAVERSAL_RECURSIVE - }; - -protected: - - - btVector3 m_bvhAabbMin; - btVector3 m_bvhAabbMax; - btVector3 m_bvhQuantization; - - int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. - - int m_curNodeIndex; - //quantization data - bool m_useQuantization; - - - - NodeArray m_leafNodes; - NodeArray m_contiguousNodes; - QuantizedNodeArray m_quantizedLeafNodes; - QuantizedNodeArray m_quantizedContiguousNodes; - - btTraversalMode m_traversalMode; - BvhSubtreeInfoArray m_SubtreeHeaders; - - //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray - 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) - { - if (m_useQuantization) - { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); - } else - { - m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; - - } - } - void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) - { - if (m_useQuantization) - { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); - } else - { - m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; - } - } - - btVector3 getAabbMin(int nodeIndex) const - { - if (m_useQuantization) - { - return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); - } - //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) - { - 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) - { - 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(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; - - ///tree traversal designed for small-memory processors like PS3 SPU - 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; - - ///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 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; } - ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized - 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; - - SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const - { - - btAssert(m_useQuantization); - - btAssert(point.getX() <= m_bvhAabbMax.getX()); - btAssert(point.getY() <= m_bvhAabbMax.getY()); - btAssert(point.getZ() <= m_bvhAabbMax.getZ()); - - btAssert(point.getX() >= m_bvhAabbMin.getX()); - btAssert(point.getY() >= m_bvhAabbMin.getY()); - btAssert(point.getZ() >= m_bvhAabbMin.getZ()); - - btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; - ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative - ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) - ///@todo: double-check this - 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)); - } - - -#ifdef DEBUG_CHECK_DEQUANTIZATION - btVector3 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 - - } - - - 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); - - } - - SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const - { - 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; - } - - ///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) - { - m_traversalMode = traversalMode; - } - - - SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() - { - return m_quantizedContiguousNodes; - } - - - 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; - - ///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 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, btSerializer* serializer) const; - - virtual void deSerializeFloat(struct btQuantizedBvhFloatData& quantizedBvhFloatData); - - virtual void deSerializeDouble(struct btQuantizedBvhDoubleData& quantizedBvhDoubleData); - - -//////////////////////////////////////////////////////////////////// - - SIMD_FORCE_INLINE bool isQuantized() - { - return m_useQuantization; - } - -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); - -} -; - - -struct btBvhSubtreeInfoData -{ - 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; - 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]; -}; - - -struct btQuantizedBvhNodeData -{ - unsigned short m_quantizedAabbMin[3]; - unsigned short m_quantizedAabbMax[3]; - int m_escapeIndexOrTriangleIndex; -}; - -struct btQuantizedBvhFloatData -{ - btVector3FloatData m_bvhAabbMin; - btVector3FloatData m_bvhAabbMax; - btVector3FloatData m_bvhQuantization; - int m_curNodeIndex; - int m_useQuantization; - int m_numContiguousLeafNodes; - int m_numQuantizedContiguousNodes; - btOptimizedBvhNodeFloatData *m_contiguousNodesPtr; - btQuantizedBvhNodeData *m_quantizedContiguousNodesPtr; - btBvhSubtreeInfoData *m_subTreeInfoPtr; - int m_traversalMode; - int m_numSubtreeHeaders; - -}; - -struct btQuantizedBvhDoubleData -{ - btVector3DoubleData m_bvhAabbMin; - btVector3DoubleData m_bvhAabbMax; - btVector3DoubleData m_bvhQuantization; - int m_curNodeIndex; - int m_useQuantization; - int m_numContiguousLeafNodes; - int m_numQuantizedContiguousNodes; - btOptimizedBvhNodeDoubleData *m_contiguousNodesPtr; - btQuantizedBvhNodeData *m_quantizedContiguousNodesPtr; - - int m_traversalMode; - int m_numSubtreeHeaders; - btBvhSubtreeInfoData *m_subTreeInfoPtr; -}; - - -SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const -{ - return sizeof(btQuantizedBvhData); -} - - - -#endif //BT_QUANTIZED_BVH_H diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp deleted file mode 100644 index 752fcd0fe..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ /dev/null @@ -1,349 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btAabbUtil2.h" - -#include - -extern int gOverlappingPairs; - -void btSimpleBroadphase::validate() -{ - for (int i=0;i~btOverlappingPairCache(); - btAlignedFree(m_pairCache); - } -} - - -btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy) -{ - if (m_numHandles >= m_maxHandles) - { - btAssert(0); - 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]); - - int newHandleIndex = allocHandle(); - btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy); - - return proxy; -} - -class RemovingOverlapCallback : public btOverlapCallback -{ -protected: - virtual bool processOverlap(btBroadphasePair& pair) - { - (void)pair; - btAssert(0); - return false; - } -}; - -class RemovePairContainingProxy -{ - - btBroadphaseProxy* m_targetProxy; - public: - virtual ~RemovePairContainingProxy() - { - } -protected: - virtual bool processOverlap(btBroadphasePair& pair) - { - btSimpleBroadphaseProxy* proxy0 = static_cast(pair.m_pProxy0); - btSimpleBroadphaseProxy* proxy1 = static_cast(pair.m_pProxy1); - - return ((m_targetProxy == proxy0 || m_targetProxy == proxy1)); - }; -}; - -void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher) -{ - - btSimpleBroadphaseProxy* proxy0 = static_cast(proxyOrg); - freeHandle(proxy0); - - m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher); - - //validate(); - -} - -void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); - aabbMin = sbp->m_aabbMin; - aabbMax = sbp->m_aabbMax; -} - -void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* /*dispatcher*/) -{ - btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); - sbp->m_aabbMin = aabbMin; - sbp->m_aabbMax = aabbMax; -} - -void btSimpleBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) -{ - for (int i=0; i <= m_LastHandleIndex; i++) - { - btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; - if(!proxy->m_clientObject) - { - continue; - } - rayCallback.process(proxy); - } -} - - -void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) -{ - for (int i=0; i <= m_LastHandleIndex; i++) - { - btSimpleBroadphaseProxy* proxy = &m_pHandles[i]; - if(!proxy->m_clientObject) - { - continue; - } - if (TestAabbAgainstAabb2(aabbMin,aabbMax,proxy->m_aabbMin,proxy->m_aabbMax)) - { - callback.process(proxy); - } - } -} - - - - - - - -bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1) -{ - 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))); - } -}; - -void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - //first check for new overlapping pairs - int i,j; - if (m_numHandles >= 0) - { - int new_largest_index = -1; - for (i=0; i <= m_LastHandleIndex; i++) - { - btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i]; - if(!proxy0->m_clientObject) - { - continue; - } - new_largest_index = i; - for (j=i+1; j <= m_LastHandleIndex; j++) - { - btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j]; - btAssert(proxy0 != proxy1); - if(!proxy1->m_clientObject) - { - continue; - } - - btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); - btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); - - if (aabbOverlap(p0,p1)) - { - if ( !m_pairCache->findPair(proxy0,proxy1)) - { - m_pairCache->addOverlappingPair(proxy0,proxy1); - } - } else - { - if (!m_pairCache->hasDeferredRemoval()) - { - if ( m_pairCache->findPair(proxy0,proxy1)) - { - m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher); - } - } - } - } - } - - m_LastHandleIndex = new_largest_index; - - if (m_ownsPairCache && m_pairCache->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;iprocessOverlap(pair); - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - m_pairCache->cleanOverlappingPair(pair,dispatcher); - - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - m_invalidPair++; - gOverlappingPairs--; - } - - } - - ///if you don't like to skip the invalid pairs in the array, execute following code: -#define CLEAN_INVALID_PAIRS 1 -#ifdef CLEAN_INVALID_PAIRS - - //perform a sort, to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; -#endif//CLEAN_INVALID_PAIRS - - } - } -} - - -bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) -{ - btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); - btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); - return aabbOverlap(p0,p1); -} - -void btSimpleBroadphase::resetPool(btDispatcher* dispatcher) -{ - //not yet -} diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h deleted file mode 100644 index 7cb3c40a0..000000000 --- a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ /dev/null @@ -1,171 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SIMPLE_BROADPHASE_H -#define BT_SIMPLE_BROADPHASE_H - - -#include "btOverlappingPairCache.h" - - -struct btSimpleBroadphaseProxy : public btBroadphaseProxy -{ - int m_nextFree; - -// int m_handleId; - - - btSimpleBroadphaseProxy() {}; - - btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy) - :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy) - { - (void)shapeType; - } - - - 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; - - btSimpleBroadphaseProxy* m_pHandles; // handles pool - - void* m_pHandlesRawPtr; - 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) - { - m_LastHandleIndex = freeHandle; - } - return freeHandle; - } - - void freeHandle(btSimpleBroadphaseProxy* proxy) - { - int handle = int(proxy-m_pHandles); - btAssert(handle >= 0 && handle < m_maxHandles); - if(handle == m_LastHandleIndex) - { - m_LastHandleIndex--; - } - proxy->SetNextFree(m_firstFreeHandle); - m_firstFreeHandle = handle; - - proxy->m_clientObject = 0; - - m_numHandles--; - } - - btOverlappingPairCache* m_pairCache; - bool m_ownsPairCache; - - int m_invalidPair; - - - - inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) - { - btSimpleBroadphaseProxy* proxy0 = static_cast(proxy); - return proxy0; - } - - inline const btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) const - { - const btSimpleBroadphaseProxy* proxy0 = static_cast(proxy); - return proxy0; - } - - ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher); - - - void validate(); - -protected: - - - - -public: - btSimpleBroadphase(int maxProxies=16384,btOverlappingPairCache* overlappingPairCache=0); - virtual ~btSimpleBroadphase(); - - - static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1); - - - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); - - virtual void calculateOverlappingPairs(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); - - btOverlappingPairCache* getOverlappingPairCache() - { - return m_pairCache; - } - const btOverlappingPairCache* getOverlappingPairCache() const - { - return m_pairCache; - } - - 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 - { - 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() - { -// printf("btSimpleBroadphase.h\n"); -// printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); - } -}; - - - -#endif //BT_SIMPLE_BROADPHASE_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp deleted file mode 100644 index 634017809..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ /dev/null @@ -1,200 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "LinearMath/btScalar.h" -#include "SphereTriangleDetector.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - - -SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) -:m_sphere(sphere), -m_triangle(triangle), -m_contactBreakingThreshold(contactBreakingThreshold) -{ - -} - -void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) -{ - - (void)debugDraw; - const btTransform& transformA = input.m_transformA; - const btTransform& transformB = input.m_transformB; - - btVector3 point,normal; - btScalar timeOfImpact = btScalar(1.); - btScalar depth = btScalar(0.); -// output.m_distance = btScalar(BT_LARGE_FLOAT); - //move sphere into triangle space - btTransform sphereInTr = transformB.inverseTimes(transformA); - - if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) - { - if (swapResults) - { - btVector3 normalOnB = transformB.getBasis()*normal; - btVector3 normalOnA = -normalOnB; - btVector3 pointOnA = transformB*point+normalOnB*depth; - output.addContactPoint(normalOnA,pointOnA,depth); - } else - { - output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); - } - } - -} - - - -// See also geometrictools.com -// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest); - -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { - btVector3 diff = p - from; - btVector3 v = to - from; - btScalar t = v.dot(diff); - - if (t > 0) { - btScalar dotVV = v.dot(v); - if (t < dotVV) { - t /= dotVV; - diff -= t*v; - } else { - t = 1; - diff -= v; - } - } else - t = 0; - - nearest = from + t*v; - return diff.dot(diff); -} - -bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { - btVector3 lp(p); - btVector3 lnormal(normal); - - return pointInTriangle(vertices, lnormal, &lp); -} - -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]); - normal.normalize(); - btVector3 p1ToCentre = sphereCenter - vertices[0]; - btScalar distanceFromPlane = p1ToCentre.dot(normal); - - if (distanceFromPlane < btScalar(0.)) - { - //triangle facing the other way - distanceFromPlane *= btScalar(-1.); - normal *= btScalar(-1.); - } - - bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - - // Check for contact / intersection - bool hasContact = false; - btVector3 contactPoint; - if (isInsideContactPlane) { - if (facecontains(sphereCenter,vertices,normal)) { - // Inside the contact wedge - touches a point on the shell plane - hasContact = true; - contactPoint = sphereCenter - normal*distanceFromPlane; - } else { - // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; - btVector3 nearestOnEdge; - for (int i = 0; i < m_triangle->getNumEdges(); i++) { - - btVector3 pa; - btVector3 pb; - - m_triangle->getEdge(i,pa,pb); - - btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge); - if (distanceSqr < contactCapsuleRadiusSqr) { - // Yep, we're inside a capsule - hasContact = true; - contactPoint = nearestOnEdge; - } - - } - } - } - - if (hasContact) { - btVector3 contactToCentre = sphereCenter - contactPoint; - btScalar distanceSqr = contactToCentre.length2(); - - if (distanceSqr < radiusWithThreshold*radiusWithThreshold) - { - if (distanceSqr>SIMD_EPSILON) - { - btScalar distance = btSqrt(distanceSqr); - resultNormal = contactToCentre; - resultNormal.normalize(); - point = contactPoint; - depth = -(radius-distance); - } else - { - resultNormal = normal; - point = contactPoint; - depth = -radius; - } - return true; - } - } - - return false; -} - - -bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) -{ - const btVector3* p1 = &vertices[0]; - const btVector3* p2 = &vertices[1]; - const btVector3* p3 = &vertices[2]; - - btVector3 edge1( *p2 - *p1 ); - btVector3 edge2( *p3 - *p2 ); - btVector3 edge3( *p1 - *p3 ); - - btVector3 p1_to_p( *p - *p1 ); - btVector3 p2_to_p( *p - *p2 ); - btVector3 p3_to_p( *p - *p3 ); - - btVector3 edge1_normal( edge1.cross(normal)); - btVector3 edge2_normal( edge2.cross(normal)); - btVector3 edge3_normal( edge3.cross(normal)); - - btScalar r1, r2, r3; - r1 = edge1_normal.dot( p1_to_p ); - r2 = edge2_normal.dot( p2_to_p ); - r3 = edge3_normal.dot( p3_to_p ); - if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || - ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) - return true; - return false; - -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.h deleted file mode 100644 index 22953af43..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ /dev/null @@ -1,51 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SPHERE_TRIANGLE_DETECTOR_H -#define BT_SPHERE_TRIANGLE_DETECTOR_H - -#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); - - SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold); - - virtual ~SphereTriangleDetector() {}; - - 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); - - btSphereShape* m_sphere; - btTriangleShape* m_triangle; - btScalar m_contactBreakingThreshold; - -}; -#endif //BT_SPHERE_TRIANGLE_DETECTOR_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp deleted file mode 100644 index 57f146493..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btActivatingCollisionAlgorithm.h" -#include "btCollisionDispatcher.h" -#include "btCollisionObject.h" - -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) -:btCollisionAlgorithm(ci) -//, -//m_colObj0(0), -//m_colObj1(0) -{ -} -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, 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(); -// } -} - -btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() -{ -// m_colObj0->activate(); -// m_colObj1->activate(); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h deleted file mode 100644 index 489812b96..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ /dev/null @@ -1,36 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H -#define __BT_ACTIVATING_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - -///This class is not enabled yet (work-in-progress) to more aggressively activate objects. -class btActivatingCollisionAlgorithm : public btCollisionAlgorithm -{ -// btCollisionObject* m_colObj0; -// btCollisionObject* m_colObj1; - -public: - - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); - - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - - virtual ~btActivatingCollisionAlgorithm(); - -}; -#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp deleted file mode 100644 index 2c3627782..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ /dev/null @@ -1,421 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library. -///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross - -#include "btBox2dBox2dCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" -#include "BulletCollision/CollisionShapes/btBox2dShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -#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) -{ - if (!m_manifoldPtr && m_dispatcher->needsCollision(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); - -//#include -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()); - - // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } - -} - -btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - //not yet - return 1.f; -} - - -struct ClipVertex -{ - btVector3 v; - int id; - //b2ContactID id; - //b2ContactID id; -}; - -#define b2Dot(a,b) (a).dot(b) -#define b2Mul(a,b) (a)*(b) -#define b2MulT(a,b) (a).transpose()*(b) -#define b2Cross(a,b) (a).cross(b) -#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f) - -int b2_maxManifoldPoints =2; - -static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], - const btVector3& normal, btScalar offset) -{ - // Start with no output points - int numOut = 0; - - // Calculate the distance of end points to the line - btScalar distance0 = b2Dot(normal, vIn[0].v) - offset; - btScalar distance1 = b2Dot(normal, vIn[1].v) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - btScalar interp = distance0 / (distance0 - distance1); - vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - if (distance0 > 0.0f) - { - vOut[numOut].id = vIn[0].id; - } - else - { - vOut[numOut].id = vIn[1].id; - } - ++numOut; - } - - return numOut; -} - -// Find the separation between poly1 and poly2 for a give edge normal on poly1. -static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1, - const btBox2dShape* poly2, const btTransform& xf2) -{ - const btVector3* vertices1 = poly1->getVertices(); - const btVector3* normals1 = poly1->getNormals(); - - int count2 = poly2->getVertexCount(); - const btVector3* vertices2 = poly2->getVertices(); - - btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); - - // Convert normal from poly1's frame into poly2's frame. - btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]); - btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World); - - // Find support vertex on poly2 for -normal. - int index = 0; - btScalar minDot = BT_LARGE_FLOAT; - - if( count2 > 0 ) - index = (int) normal1.minDot( vertices2, count2, minDot); - - btVector3 v1 = b2Mul(xf1, vertices1[edge1]); - btVector3 v2 = b2Mul(xf2, vertices2[index]); - btScalar separation = b2Dot(v2 - v1, normal1World); - return separation; -} - -// Find the max separation between poly1 and poly2 using edge normals from poly1. -static btScalar FindMaxSeparation(int* edgeIndex, - const btBox2dShape* poly1, const btTransform& xf1, - const btBox2dShape* poly2, const btTransform& xf2) -{ - int count1 = poly1->getVertexCount(); - const btVector3* normals1 = poly1->getNormals(); - - // Vector pointing from the centroid of poly1 to the centroid of poly2. - btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid()); - btVector3 dLocal1 = b2MulT(xf1.getBasis(), d); - - // Find edge normal on poly1 that has the largest projection onto d. - int edge = 0; - btScalar maxDot; - 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); - if (s > 0.0f) - { - return s; - } - - // Check the separation for the previous edge normal. - int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; - btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); - if (sPrev > 0.0f) - { - return sPrev; - } - - // Check the separation for the next edge normal. - int nextEdge = edge + 1 < count1 ? edge + 1 : 0; - btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); - if (sNext > 0.0f) - { - return sNext; - } - - // Find the best edge and the search direction. - int bestEdge; - btScalar bestSeparation; - int increment; - if (sPrev > s && sPrev > sNext) - { - increment = -1; - bestEdge = prevEdge; - bestSeparation = sPrev; - } - else if (sNext > s) - { - increment = 1; - bestEdge = nextEdge; - bestSeparation = sNext; - } - else - { - *edgeIndex = edge; - return s; - } - - // Perform a local search for the best edge normal. - for ( ; ; ) - { - if (increment == -1) - edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; - else - edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; - - s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); - if (s > 0.0f) - { - return s; - } - - if (s > bestSeparation) - { - bestEdge = edge; - bestSeparation = s; - } - else - { - break; - } - } - - *edgeIndex = bestEdge; - return bestSeparation; -} - -static void FindIncidentEdge(ClipVertex c[2], - const btBox2dShape* poly1, const btTransform& xf1, int edge1, - const btBox2dShape* poly2, const btTransform& xf2) -{ - const btVector3* normals1 = poly1->getNormals(); - - int count2 = poly2->getVertexCount(); - const btVector3* vertices2 = poly2->getVertices(); - const btVector3* normals2 = poly2->getNormals(); - - btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); - - // Get the normal of the reference edge in poly2's frame. - btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1])); - - // Find the incident edge on poly2. - int index = 0; - btScalar minDot = BT_LARGE_FLOAT; - for (int i = 0; i < count2; ++i) - { - btScalar dot = b2Dot(normal1, normals2[i]); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - // Build the clip vertices for the incident edge. - int i1 = index; - int i2 = i1 + 1 < count2 ? i1 + 1 : 0; - - c[0].v = b2Mul(xf2, vertices2[i1]); -// c[0].id.features.referenceEdge = (unsigned char)edge1; -// c[0].id.features.incidentEdge = (unsigned char)i1; -// c[0].id.features.incidentVertex = 0; - - c[1].v = b2Mul(xf2, vertices2[i2]); -// c[1].id.features.referenceEdge = (unsigned char)edge1; -// c[1].id.features.incidentEdge = (unsigned char)i2; -// c[1].id.features.incidentVertex = 1; -} - -// Find edge normal of max separation on A - return if separating axis is found -// Find edge normal of max separation on B - return if separation axis is found -// Choose reference edge as min(minA, minB) -// Find incident edge -// Clip - -// The normal points from 1 to 2 -void b2CollidePolygons(btManifoldResult* manifold, - const btBox2dShape* polyA, const btTransform& xfA, - const btBox2dShape* polyB, const btTransform& xfB) -{ - - int edgeA = 0; - btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); - if (separationA > 0.0f) - return; - - int edgeB = 0; - btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); - if (separationB > 0.0f) - return; - - const btBox2dShape* poly1; // reference poly - const btBox2dShape* poly2; // incident poly - btTransform xf1, xf2; - int edge1; // reference edge - unsigned char flip; - const btScalar k_relativeTol = 0.98f; - const btScalar k_absoluteTol = 0.001f; - - // TODO_ERIN use "radius" of poly for absolute tolerance. - if (separationB > k_relativeTol * separationA + k_absoluteTol) - { - poly1 = polyB; - poly2 = polyA; - xf1 = xfB; - xf2 = xfA; - edge1 = edgeB; - flip = 1; - } - else - { - poly1 = polyA; - poly2 = polyB; - xf1 = xfA; - xf2 = xfB; - edge1 = edgeA; - flip = 0; - } - - ClipVertex incidentEdge[2]; - FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); - - int count1 = poly1->getVertexCount(); - const btVector3* vertices1 = poly1->getVertices(); - - btVector3 v11 = vertices1[edge1]; - btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; - - //btVector3 dv = v12 - v11; - btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11); - sideNormal.normalize(); - btVector3 frontNormal = btCrossS(sideNormal, 1.0f); - - - v11 = b2Mul(xf1, v11); - v12 = b2Mul(xf1, v12); - - btScalar frontOffset = b2Dot(frontNormal, v11); - btScalar sideOffset1 = -b2Dot(sideNormal, v11); - btScalar sideOffset2 = b2Dot(sideNormal, v12); - - // Clip incident edge against extruded edge1 side edges. - ClipVertex clipPoints1[2]; - clipPoints1[0].v.setValue(0,0,0); - clipPoints1[1].v.setValue(0,0,0); - - ClipVertex clipPoints2[2]; - clipPoints2[0].v.setValue(0,0,0); - clipPoints2[1].v.setValue(0,0,0); - - - int np; - - // Clip to box side 1 - np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); - - if (np < 2) - return; - - // Clip to negative box side 1 - np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); - - if (np < 2) - { - return; - } - - // Now clipPoints2 contains the clipped points. - btVector3 manifoldNormal = flip ? -frontNormal : frontNormal; - - int pointCount = 0; - for (int i = 0; i < b2_maxManifoldPoints; ++i) - { - btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset; - - if (separation <= 0.0f) - { - - //b2ManifoldPoint* cp = manifold->points + pointCount; - //btScalar separation = separation; - //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v); - //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v); - - manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation); - -// cp->id = clipPoints2[i].id; -// cp->id.features.flip = flip; - ++pointCount; - } - } - -// manifold->pointCount = pointCount;} -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h deleted file mode 100644 index 6ea6e89bd..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ /dev/null @@ -1,66 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H -#define BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H - -#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" - -class btPersistentManifold; - -///box-box collision detection -class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - - virtual ~btBox2dBox2dCollisionAlgorithm(); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp deleted file mode 100644 index ac68968f5..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btBoxBoxCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "btBoxBoxDetector.h" -#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) -{ - if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject())) - { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); - m_ownManifold = true; - } -} - -btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -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 - m_manifoldPtr->clearManifold(); -#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); - -#ifdef USE_PERSISTENT_CONTACTS - // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } -#endif //USE_PERSISTENT_CONTACTS - -} - -btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - //not yet - return 1.f; -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h deleted file mode 100644 index 59808df5a..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ /dev/null @@ -1,66 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_BOX_BOX__COLLISION_ALGORITHM_H -#define BT_BOX_BOX__COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" - -class btPersistentManifold; - -///box-box collision detection -class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - - virtual ~btBoxBoxCollisionAlgorithm(); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp deleted file mode 100644 index 7043bde34..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ /dev/null @@ -1,718 +0,0 @@ -/* - * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - Bullet Continuous Collision Detection and Physics Library - Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///ODE box-box collision detection is adapted to work with Bullet - -#include "btBoxBoxDetector.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" - -#include -#include - -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. -// `normal' returns the contact normal. -// `depth' returns the maximum penetration depth along that normal. -// `return_code' returns a number indicating the type of contact that was -// detected: -// 1,2,3 = box 2 intersects with a face of box 1 -// 4,5,6 = box 1 intersects with a face of box 2 -// 7..15 = edge-edge contact -// `maxc' is the maximum number of contacts allowed to be generated, i.e. -// the size of the `contact' array. -// `contact' and `skip' are the contact array information provided to the -// collision functions. this function only fills in the position and depth -// fields. -struct dContactGeom; -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) -#define dInfinity FLT_MAX - - -/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } -PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } -PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } -PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } -*/ -static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } -static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } -static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } -static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } -#define dMULTIPLYOP1_331(A,op,B,C) \ -{\ - (A)[0] op dDOT41((B),(C)); \ - (A)[1] op dDOT41((B+1),(C)); \ - (A)[2] op dDOT41((B+2),(C)); \ -} - -#define dMULTIPLYOP0_331(A,op,B,C) \ -{ \ - (A)[0] op dDOT((B),(C)); \ - (A)[1] op dDOT((B+4),(C)); \ - (A)[2] op dDOT((B+8),(C)); \ -} - -#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) - -typedef btScalar dMatrix3[4*3]; - -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta); -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta) -{ - btVector3 p; - p[0] = pb[0] - pa[0]; - p[1] = pb[1] - pa[1]; - p[2] = pb[2] - pa[2]; - btScalar uaub = dDOT(ua,ub); - btScalar q1 = dDOT(ua,p); - btScalar q2 = -dDOT(ub,p); - btScalar d = 1-uaub*uaub; - if (d <= btScalar(0.0001f)) { - // @@@ this needs to be made more robust - *alpha = 0; - *beta = 0; - } - else { - d = 1.f/d; - *alpha = (q1 + uaub*q2)*d; - *beta = (uaub*q1 + q2)*d; - } -} - - - -// find all the intersection points between the 2D rectangle with vertices -// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), -// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). -// -// the intersection points are returned as x,y pairs in the 'ret' array. -// the number of intersection points is returned by the function (this will -// be in the range 0 to 8). - -static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) -{ - // q (and r) contain nq (and nr) coordinate points for the current (and - // chopped) polygons - int nq=4,nr=0; - btScalar buffer[16]; - btScalar *q = p; - btScalar *r = ret; - for (int dir=0; dir <= 1; dir++) { - // direction notation: xy[0] = x axis, xy[1] = y axis - for (int sign=-1; sign <= 1; sign += 2) { - // chop q along the line xy[dir] = sign*h[dir] - btScalar *pq = q; - btScalar *pr = r; - nr = 0; - for (int i=nq; i > 0; i--) { - // go through all points in q and all lines between adjacent points - if (sign*pq[dir] < h[dir]) { - // this point is inside the chopping line - pr[0] = pq[0]; - pr[1] = pq[1]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } - } - btScalar *nextq = (i > 1) ? pq+2 : q; - if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { - // this line crosses the chopping line - pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / - (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); - pr[dir] = sign*h[dir]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } - } - pq += 2; - } - q = r; - r = (q==ret) ? buffer : ret; - nq = nr; - } - } - done: - if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); - return nr; -} - - -#define M__PI 3.14159265f - -// given n points in the plane (array p, of size 2*n), generate m points that -// best represent the whole set. the definition of 'best' here is not -// predetermined - the idea is to select points that give good box-box -// collision detection behavior. the chosen point indexes are returned in the -// array iret (of size m). 'i0' is always the first entry in the array. -// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be -// in the range [0..n-1]. - -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) -{ - // compute the centroid of the polygon in cx,cy - int i,j; - btScalar a,cx,cy,q; - if (n==1) { - cx = p[0]; - cy = p[1]; - } - else if (n==2) { - cx = btScalar(0.5)*(p[0] + p[2]); - cy = btScalar(0.5)*(p[1] + p[3]); - } - else { - a = 0; - cx = 0; - cy = 0; - for (i=0; i<(n-1); i++) { - q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; - a += q; - cx += q*(p[i*2]+p[i*2+2]); - cy += q*(p[i*2+1]+p[i*2+3]); - } - q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - if (btFabs(a+q) > SIMD_EPSILON) - { - a = 1.f/(btScalar(3.0)*(a+q)); - } else - { - a=BT_LARGE_FLOAT; - } - cx = a*(cx + q*(p[n*2-2]+p[0])); - cy = a*(cy + q*(p[n*2-1]+p[1])); - } - - // compute the angle of each point w.r.t. the centroid - btScalar A[8]; - for (i=0; i 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; - } - } - } -#if defined(DEBUG) || defined (_DEBUG) - btAssert (*iret != i0); // ensure iret got set -#endif - avail[*iret] = 0; - iret++; - } -} - - - -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) -{ - const btScalar fudge_factor = btScalar(1.05); - btVector3 p,pp,normalC(0.f,0.f,0.f); - const btScalar *normalR = 0; - btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, - Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; - int i,j,invert_normal,code; - - // get vector from centers of box 1 to box 2, relative to box 1 - p = p2 - p1; - dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 - - // get side lengths / 2 - A[0] = side1[0]*btScalar(0.5); - A[1] = side1[1]*btScalar(0.5); - A[2] = side1[2]*btScalar(0.5); - B[0] = side2[0]*btScalar(0.5); - B[1] = side2[1]*btScalar(0.5); - B[2] = side2[2]*btScalar(0.5); - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); - R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); - R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); - - Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); - Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); - Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); - - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. - -#define TST(expr1,expr2,norm,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ - if (s2 > s) { \ - s = s2; \ - normalR = norm; \ - invert_normal = ((expr1) < 0); \ - code = (cc); \ - } - - s = -dInfinity; - invert_normal = 0; - code = 0; - - // separating axis = u1,u2,u3 - TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); - TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); - TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); - - // separating axis = v1,v2,v3 - TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); - TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); - TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); - - // note: cross product axes need to be scaled when s is computed. - // normal (n1,n2,n3) is relative to box 1. -#undef TST -#define TST(expr1,expr2,n1,n2,n3,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > 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); - - Q11 += fudge2; - Q12 += fudge2; - Q13 += fudge2; - - Q21 += fudge2; - Q22 += fudge2; - Q23 += 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 = u2 x (v1,v2,v3) - TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); - TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); - TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); - - // separating axis = u3 x (v1,v2,v3) - TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); - TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); - TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); - -#undef TST - - if (!code) return 0; - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if (normalR) { - normal[0] = normalR[0]; - normal[1] = normalR[4]; - normal[2] = normalR[8]; - } - else { - dMULTIPLY0_331 (normal,R1,normalC); - } - if (invert_normal) { - normal[0] = -normal[0]; - normal[1] = -normal[1]; - normal[2] = -normal[2]; - } - *depth = -s; - - // compute contact point(s) - - if (code > 6) { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - btVector3 pa; - btScalar sign; - for (i=0; i<3; i++) pa[i] = p1[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); - for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; - } - - // find a point pb on the intersecting edge of box 2 - btVector3 pb; - for (i=0; i<3; i++) pb[i] = p2[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); - for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; - } - - btScalar alpha,beta; - btVector3 ua,ub; - for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; - for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; - - dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); - for (i=0; i<3; i++) pa[i] += ua[i]*alpha; - for (i=0; i<3; i++) pb[i] += ub[i]*beta; - - { - - //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); - //contact[0].depth = *depth; - btVector3 pointInWorld; - -#ifdef USE_CENTER_POINT - for (i=0; i<3; i++) - pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); - output.addContactPoint(-normal,pointInWorld,-*depth); -#else - output.addContactPoint(-normal,pb,-*depth); - -#endif // - *return_code = code; - } - return 1; - } - - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). define face 'a' to be the reference - // face (i.e. the normal vector is perpendicular to this) and face 'b' to be - // the incident face (the closest face of the other box). - - const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; - if (code <= 3) { - Ra = R1; - Rb = R2; - pa = p1; - pb = p2; - Sa = A; - Sb = B; - } - else { - Ra = R2; - Rb = R1; - pa = p2; - pb = p1; - Sa = B; - Sb = A; - } - - // nr = normal vector of reference face dotted with axes of incident box. - // anr = absolute values of nr. - btVector3 normal2,nr,anr; - if (code <= 3) { - normal2[0] = normal[0]; - normal2[1] = normal[1]; - normal2[2] = normal[2]; - } - else { - normal2[0] = -normal[0]; - normal2[1] = -normal[1]; - normal2[2] = -normal[2]; - } - dMULTIPLY1_331 (nr,Rb,normal2); - anr[0] = btFabs (nr[0]); - anr[1] = btFabs (nr[1]); - anr[2] = btFabs (nr[2]); - - // find the largest compontent of anr: this corresponds to the normal - // for the indident face. the other axis numbers of the indicent face - // are stored in a1,a2. - int lanr,a1,a2; - if (anr[1] > anr[0]) { - if (anr[1] > anr[2]) { - a1 = 0; - lanr = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - else { - if (anr[0] > anr[2]) { - lanr = 0; - a1 = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - - // compute center point of incident face, in reference-face coordinates - btVector3 center; - if (nr[lanr] < 0) { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; - } - else { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; - } - - // find the normal and non-normal axis numbers of the reference box - int codeN,code1,code2; - if (code <= 3) codeN = code-1; else codeN = code-4; - if (codeN==0) { - code1 = 1; - code2 = 2; - } - else if (codeN==1) { - code1 = 0; - code2 = 2; - } - else { - code1 = 0; - code2 = 1; - } - - // find the four corners of the incident face, in reference-face coordinates - btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) - btScalar c1,c2,m11,m12,m21,m22; - c1 = dDOT14 (center,Ra+code1); - c2 = dDOT14 (center,Ra+code2); - // optimize this? - we have already computed this data above, but it is not - // stored in an easy-to-index format. for now it's quicker just to recompute - // the four dot products. - m11 = dDOT44 (Ra+code1,Rb+a1); - m12 = dDOT44 (Ra+code1,Rb+a2); - m21 = dDOT44 (Ra+code2,Rb+a1); - m22 = dDOT44 (Ra+code2,Rb+a2); - { - btScalar k1 = m11*Sb[a1]; - btScalar k2 = m21*Sb[a1]; - btScalar k3 = m12*Sb[a2]; - btScalar k4 = m22*Sb[a2]; - quad[0] = c1 - k1 - k3; - quad[1] = c2 - k2 - k4; - quad[2] = c1 - k1 + k3; - quad[3] = c2 - k2 + k4; - quad[4] = c1 + k1 + k3; - quad[5] = c2 + k2 + k4; - quad[6] = c1 + k1 - k3; - quad[7] = c2 + k2 - k4; - } - - // find the size of the reference face - btScalar rect[2]; - rect[0] = Sa[code1]; - rect[1] = Sa[code2]; - - // intersect the incident and reference faces - btScalar ret[16]; - int n = intersectRectQuad2 (rect,quad,ret); - if (n < 1) return 0; // this should never happen - - // convert the intersection points into reference-face coordinates, - // and compute the contact position and depth for each point. only keep - // those points that have a positive (penetrating) depth. delete points in - // the 'ret' array as necessary so that 'point' and 'ret' correspond. - btScalar point[3*8]; // penetrating contact points - btScalar dep[8]; // depths for those points - btScalar det1 = 1.f/(m11*m22 - m12*m21); - m11 *= det1; - m12 *= det1; - m21 *= det1; - m22 *= det1; - int cnum = 0; // number of penetrating contact points found - for (j=0; j < n; j++) { - btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); - btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); - for (i=0; i<3; i++) point[cnum*3+i] = - center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; - dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); - if (dep[cnum] >= 0) { - ret[cnum*2] = ret[j*2]; - ret[cnum*2+1] = ret[j*2+1]; - cnum++; - } - } - if (cnum < 1) return 0; // this should never happen - - // we can't generate more contacts than we actually have - if (maxc > cnum) maxc = cnum; - if (maxc < 1) maxc = 1; - - if (cnum <= maxc) { - - 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]); - } - } - } - 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 - { - 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*/) -{ - - const btTransform& transformA = input.m_transformA; - const btTransform& transformB = input.m_transformB; - - int skip = 0; - dContactGeom *contact = 0; - - dMatrix3 R1; - dMatrix3 R2; - - for (int j=0;j<3;j++) - { - R1[0+4*j] = transformA.getBasis()[j].x(); - R2[0+4*j] = transformB.getBasis()[j].x(); - - R1[1+4*j] = transformA.getBasis()[j].y(); - R2[1+4*j] = transformB.getBasis()[j].y(); - - - R1[2+4*j] = transformA.getBasis()[j].z(); - R2[2+4*j] = transformB.getBasis()[j].z(); - - } - - - - btVector3 normal; - btScalar depth; - int return_code; - int maxc = 4; - - - dBoxBox2 (transformA.getOrigin(), - R1, - 2.f*m_box1->getHalfExtentsWithMargin(), - transformB.getOrigin(), - R2, - 2.f*m_box2->getHalfExtentsWithMargin(), - normal, &depth, &return_code, - maxc, contact, skip, - output - ); - -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.h deleted file mode 100644 index 392437770..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - -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_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 -{ - const btBoxShape* m_box1; - const btBoxShape* m_box2; - -public: - - btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2); - - virtual ~btBoxBoxDetector() {}; - - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); - -}; - -#endif //BT_BOX_BOX_DETECTOR_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionConfiguration.h deleted file mode 100644 index 669498494..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION_CONFIGURATION -#define BT_COLLISION_CONFIGURATION - -struct btCollisionAlgorithmCreateFunc; - -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 -{ - -public: - - virtual ~btCollisionConfiguration() - { - } - - ///memory pools - virtual btPoolAllocator* getPersistentManifoldPool() = 0; - - virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; - - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; - -}; - -#endif //BT_COLLISION_CONFIGURATION - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h deleted file mode 100644 index 62ee66c4e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ /dev/null @@ -1,45 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION_CREATE_FUNC -#define BT_COLLISION_CREATE_FUNC - -#include "LinearMath/btAlignedObjectArray.h" -class btCollisionAlgorithm; -class btCollisionObject; -struct btCollisionObjectWrapper; -struct btCollisionAlgorithmConstructionInfo; - -///Used by the btCollisionDispatcher to register and create instances for btCollisionAlgorithm -struct btCollisionAlgorithmCreateFunc -{ - bool m_swapped; - - btCollisionAlgorithmCreateFunc() - :m_swapped(false) - { - } - virtual ~btCollisionAlgorithmCreateFunc(){}; - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - - (void)body0Wrap; - (void)body1Wrap; - return 0; - } -}; -#endif //BT_COLLISION_CREATE_FUNC - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp deleted file mode 100644 index 669d0b6b5..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ /dev/null @@ -1,314 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btCollisionDispatcher.h" - - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "LinearMath/btPoolAllocator.h" -#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" -#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) -{ - int i; - - setNearCallback(defaultNearCallback); - - m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); - - m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); - - for (i=0;igetCollisionAlgorithmCreateFunc(i,j); - btAssert(m_doubleDispatch[i][j]); - } - } - - -} - - -void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) -{ - m_doubleDispatch[proxyType0][proxyType1] = createFunc; -} - -btCollisionDispatcher::~btCollisionDispatcher() -{ -} - -btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) -{ - gNumManifold++; - - //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 = 0; - - if (m_persistentManifoldPoolAllocator->getFreeCount()) - { - mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); - } else - { - //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); - manifold->m_index1a = m_manifoldsPtr.size(); - m_manifoldsPtr.push_back(manifold); - - return manifold; -} - -void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) -{ - manifold->clearManifold(); -} - - -void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) -{ - - gNumManifold--; - - //printf("releaseManifold: gNumManifold %d\n",gNumManifold); - clearManifold(manifold); - - int findIndex = manifold->m_index1a; - btAssert(findIndex < m_manifoldsPtr.size()); - m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); - m_manifoldsPtr[findIndex]->m_index1a = findIndex; - m_manifoldsPtr.pop_back(); - - manifold->~btPersistentManifold(); - if (m_persistentManifoldPoolAllocator->validPtr(manifold)) - { - m_persistentManifoldPoolAllocator->freeMemory(manifold); - } else - { - btAlignedFree(manifold); - } - -} - - - -btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold) -{ - - btCollisionAlgorithmConstructionInfo ci; - - ci.m_dispatcher1 = this; - ci.m_manifold = sharedManifold; - btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap); - - return algo; -} - - - - -bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1) -{ - //here you can do filtering - bool hasResponse = - (body0->hasContactResponse() && body1->hasContactResponse()); - //no response between two static/kinematic bodies: - hasResponse = hasResponse && - ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject())); - return hasResponse; -} - -bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) -{ - btAssert(body0); - btAssert(body1); - - bool needsCollision = true; - -#ifdef BT_DEBUG - if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED)) - { - //broadphase filtering already deals with this - if (body0->isStaticOrKinematicObject() && body1->isStaticOrKinematicObject()) - { - m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED; - printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); - } - } -#endif //BT_DEBUG - - if ((!body0->isActive()) && (!body1->isActive())) - needsCollision = false; - else if (!body0->checkCollideWith(body1)) - needsCollision = false; - - return needsCollision ; - -} - - - -///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) -///this is useful for the collision dispatcher. -class btCollisionPairCallback : public btOverlapCallback -{ - const btDispatcherInfo& m_dispatchInfo; - btCollisionDispatcher* m_dispatcher; - -public: - - btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) - :m_dispatchInfo(dispatchInfo), - m_dispatcher(dispatcher) - { - } - - /*btCollisionPairCallback& operator=(btCollisionPairCallback& other) - { - m_dispatchInfo = other.m_dispatchInfo; - m_dispatcher = other.m_dispatcher; - return *this; - } - */ - - - virtual ~btCollisionPairCallback() {} - - - virtual bool processOverlap(btBroadphasePair& pair) - { - (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); - - return false; - } -}; - - - -void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) -{ - //m_blockedForChanges = true; - - btCollisionPairCallback collisionCallback(dispatchInfo,this); - - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); - - //m_blockedForChanges = false; - -} - - - - -//by default, Bullet will use this near callback -void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) -{ - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - - if (dispatcher.needsCollision(colObj0,colObj1)) - { - 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); - } - - 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) -{ - if (m_collisionAlgorithmPoolAllocator->getFreeCount()) - { - return m_collisionAlgorithmPoolAllocator->allocate(size); - } - - //warn user for overflow? - return btAlignedAlloc(static_cast(size), 16); -} - -void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) -{ - if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) - { - m_collisionAlgorithmPoolAllocator->freeMemory(ptr); - } else - { - btAlignedFree(ptr); - } -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.h deleted file mode 100644 index 92696ee54..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ /dev/null @@ -1,171 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION__DISPATCHER_H -#define BT_COLLISION__DISPATCHER_H - -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "LinearMath/btAlignedObjectArray.h" - -class btIDebugDraw; -class btOverlappingPairCache; -class btPoolAllocator; -class btCollisionConfiguration; - -#include "btCollisionCreateFunc.h" - -#define USE_DISPATCH_REGISTRY_ARRAY 1 - -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; - - btAlignedObjectArray m_manifoldsPtr; - - btManifoldResult m_defaultManifoldResult; - - btNearCallback m_nearCallback; - - btPoolAllocator* m_collisionAlgorithmPoolAllocator; - - btPoolAllocator* m_persistentManifoldPoolAllocator; - - btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - - btCollisionConfiguration* m_collisionConfiguration; - - -public: - - enum DispatcherFlags - { - CD_STATIC_STATIC_REPORTED = 1, - CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2, - CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 - }; - - int getDispatcherFlags() const - { - return m_dispatcherFlags; - } - - void setDispatcherFlags(int flags) - { - m_dispatcherFlags = flags; - } - - ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions - void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); - - int getNumManifolds() const - { - return int( m_manifoldsPtr.size()); - } - - btPersistentManifold** getInternalManifoldPointer() - { - return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0; - } - - btPersistentManifold* getManifoldByIndexInternal(int index) - { - return m_manifoldsPtr[index]; - } - - const btPersistentManifold* getManifoldByIndexInternal(int index) const - { - return m_manifoldsPtr[index]; - } - - btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration); - - virtual ~btCollisionDispatcher(); - - 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 = 0); - - 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; - } - - btNearCallback getNearCallback() const - { - return m_nearCallback; - } - - //by default, Bullet will use this near callback - static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); - - virtual void* allocateCollisionAlgorithm(int size); - - virtual void freeCollisionAlgorithm(void* ptr); - - btCollisionConfiguration* getCollisionConfiguration() - { - return m_collisionConfiguration; - } - - const btCollisionConfiguration* getCollisionConfiguration() const - { - return m_collisionConfiguration; - } - - void setCollisionConfiguration(btCollisionConfiguration* config) - { - m_collisionConfiguration = config; - } - - virtual btPoolAllocator* getInternalManifoldPool() - { - return m_persistentManifoldPoolAllocator; - } - - virtual const btPoolAllocator* getInternalManifoldPool() const - { - return m_persistentManifoldPoolAllocator; - } - -}; - -#endif //BT_COLLISION__DISPATCHER_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.cpp deleted file mode 100644 index d09241000..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btCollisionObject.h" -#include "LinearMath/btSerializer.h" - -btCollisionObject::btCollisionObject() - : m_anisotropicFriction(1.f,1.f,1.f), - m_hasAnisotropicFriction(false), - m_contactProcessingThreshold(BT_LARGE_FLOAT), - m_broadphaseHandle(0), - m_collisionShape(0), - m_extensionPointer(0), - m_rootCollisionShape(0), - m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), - m_islandTag1(-1), - m_companionId(-1), - m_activationState1(1), - m_deactivationTime(btScalar(0.)), - m_friction(btScalar(0.5)), - m_rollingFriction(0.0f), - m_restitution(btScalar(0.)), - m_internalType(CO_COLLISION_OBJECT), - m_userObjectPointer(0), - m_hitFraction(btScalar(1.)), - m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdMotionThreshold(btScalar(0.)), - m_checkCollideWith(false), - m_updateRevision(0) -{ - m_worldTransform.setIdentity(); -} - -btCollisionObject::~btCollisionObject() -{ -} - -void btCollisionObject::setActivationState(int newState) const -{ - if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) - m_activationState1 = newState; -} - -void btCollisionObject::forceActivationState(int newState) const -{ - m_activationState1 = newState; -} - -void btCollisionObject::activate(bool forceActivation) const -{ - if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) - { - setActivationState(ACTIVE_TAG); - m_deactivationTime = btScalar(0.); - } -} - -const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const -{ - - btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer; - - m_worldTransform.serialize(dataOut->m_worldTransform); - m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform); - m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity); - m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity); - m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction); - dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction; - dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold; - dataOut->m_broadphaseHandle = 0; - dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape); - dataOut->m_rootCollisionShape = 0;//@todo - dataOut->m_collisionFlags = m_collisionFlags; - dataOut->m_islandTag1 = m_islandTag1; - dataOut->m_companionId = m_companionId; - dataOut->m_activationState1 = m_activationState1; - dataOut->m_deactivationTime = m_deactivationTime; - dataOut->m_friction = m_friction; - dataOut->m_rollingFriction = m_rollingFriction; - dataOut->m_restitution = m_restitution; - dataOut->m_internalType = m_internalType; - - char* name = (char*) serializer->findNameForPointer(this); - dataOut->m_name = (char*)serializer->getUniquePointer(name); - if (dataOut->m_name) - { - serializer->serializeName(name); - } - dataOut->m_hitFraction = m_hitFraction; - dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; - dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; - dataOut->m_checkCollideWith = m_checkCollideWith; - - return btCollisionObjectDataName; -} - - -void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const -{ - int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); - const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.h deleted file mode 100644 index 7f9508ce5..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.h +++ /dev/null @@ -1,565 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION_OBJECT_H -#define BT_COLLISION_OBJECT_H - -#include "LinearMath/btTransform.h" - -//island management, m_activationState1 -#define ACTIVE_TAG 1 -#define ISLAND_SLEEPING 2 -#define WANTS_DEACTIVATION 3 -#define DISABLE_DEACTIVATION 4 -#define DISABLE_SIMULATION 5 - -struct btBroadphaseProxy; -class btCollisionShape; -struct btCollisionShapeData; -#include "LinearMath/btMotionState.h" -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btAlignedObjectArray.h" - -typedef btAlignedObjectArray btCollisionObjectArray; - -#ifdef BT_USE_DOUBLE_PRECISION -#define btCollisionObjectData btCollisionObjectDoubleData -#define btCollisionObjectDataName "btCollisionObjectDoubleData" -#else -#define btCollisionObjectData btCollisionObjectFloatData -#define btCollisionObjectDataName "btCollisionObjectFloatData" -#endif - - -/// 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 -{ - -protected: - - 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) - //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; - - btBroadphaseProxy* m_broadphaseHandle; - btCollisionShape* m_collisionShape; - ///m_extensionPointer is used by some internal low-level Bullet extensions. - 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; - - int m_collisionFlags; - - int m_islandTag1; - int m_companionId; - - mutable int m_activationState1; - mutable btScalar m_deactivationTime; - - btScalar m_friction; - btScalar m_restitution; - btScalar m_rollingFriction; - - ///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; - - ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - union - { - void* m_userObjectPointer; - long long m_userIndex; - }; - - ///time of impact calculation - btScalar m_hitFraction; - - ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar m_ccdSweptSphereRadius; - - /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold - btScalar m_ccdMotionThreshold; - - /// If some object should have elaborate collision filtering by sub-classes - int m_checkCollideWith; - - ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. - int m_updateRevision; - - virtual bool checkCollideWithOverride(const btCollisionObject* /* co */) const - { - return true; - } - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - enum CollisionFlags - { - 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_CHARACTER_OBJECT = 16, - CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing - CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing - }; - - enum CollisionObjectTypes - { - 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 - }; - - enum AnisotropicFrictionFlags - { - CF_ANISOTROPIC_FRICTION_DISABLED=0, - CF_ANISOTROPIC_FRICTION = 1, - CF_ANISOTROPIC_ROLLING_FRICTION = 2 - }; - - 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); - } - - const btVector3& getAnisotropicFriction() const - { - return m_anisotropicFriction; - } - 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 hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const - { - 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) - { - m_contactProcessingThreshold = contactProcessingThreshold; - } - btScalar getContactProcessingThreshold() const - { - return m_contactProcessingThreshold; - } - - SIMD_FORCE_INLINE bool isStaticObject() const { - return (m_collisionFlags & CF_STATIC_OBJECT) != 0; - } - - SIMD_FORCE_INLINE bool isKinematicObject() const - { - return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; - } - - SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const - { - return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ; - } - - SIMD_FORCE_INLINE bool hasContactResponse() const { - return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0; - } - - - btCollisionObject(); - - virtual ~btCollisionObject(); - - virtual void setCollisionShape(btCollisionShape* collisionShape) - { - m_updateRevision++; - m_collisionShape = collisionShape; - m_rootCollisionShape = collisionShape; - } - - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const - { - return m_collisionShape; - } - - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() - { - return m_collisionShape; - } - - - - - - ///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 - { - 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) - { - m_extensionPointer = pointer; - } - - SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} - - void setActivationState(int newState) const; - - void setDeactivationTime(btScalar time) - { - m_deactivationTime = time; - } - btScalar getDeactivationTime() const - { - return m_deactivationTime; - } - - void forceActivationState(int newState) const; - - void activate(bool forceActivation = false) const; - - SIMD_FORCE_INLINE bool isActive() const - { - return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); - } - - void setRestitution(btScalar rest) - { - m_updateRevision++; - m_restitution = rest; - } - btScalar getRestitution() const - { - return m_restitution; - } - void setFriction(btScalar frict) - { - m_updateRevision++; - m_friction = frict; - } - btScalar getFriction() const - { - return m_friction; - } - - void setRollingFriction(btScalar frict) - { - m_updateRevision++; - m_rollingFriction = frict; - } - btScalar getRollingFriction() const - { - return m_rollingFriction; - } - - - ///reserved for Bullet internal usage - int getInternalType() const - { - return m_internalType; - } - - btTransform& getWorldTransform() - { - return m_worldTransform; - } - - const btTransform& getWorldTransform() const - { - return m_worldTransform; - } - - void setWorldTransform(const btTransform& worldTrans) - { - m_updateRevision++; - m_worldTransform = worldTrans; - } - - - SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() - { - return m_broadphaseHandle; - } - - SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const - { - return m_broadphaseHandle; - } - - void setBroadphaseHandle(btBroadphaseProxy* handle) - { - m_broadphaseHandle = handle; - } - - - const btTransform& getInterpolationWorldTransform() const - { - return m_interpolationWorldTransform; - } - - btTransform& getInterpolationWorldTransform() - { - return m_interpolationWorldTransform; - } - - void setInterpolationWorldTransform(const btTransform& trans) - { - m_updateRevision++; - m_interpolationWorldTransform = trans; - } - - void setInterpolationLinearVelocity(const btVector3& linvel) - { - m_updateRevision++; - m_interpolationLinearVelocity = linvel; - } - - void setInterpolationAngularVelocity(const btVector3& angvel) - { - m_updateRevision++; - m_interpolationAngularVelocity = angvel; - } - - const btVector3& getInterpolationLinearVelocity() const - { - return m_interpolationLinearVelocity; - } - - const btVector3& getInterpolationAngularVelocity() const - { - return m_interpolationAngularVelocity; - } - - SIMD_FORCE_INLINE int getIslandTag() const - { - return m_islandTag1; - } - - void setIslandTag(int tag) - { - m_islandTag1 = tag; - } - - SIMD_FORCE_INLINE int getCompanionId() const - { - return m_companionId; - } - - void setCompanionId(int id) - { - m_companionId = id; - } - - SIMD_FORCE_INLINE btScalar getHitFraction() const - { - return m_hitFraction; - } - - void setHitFraction(btScalar hitFraction) - { - m_hitFraction = hitFraction; - } - - - SIMD_FORCE_INLINE int getCollisionFlags() const - { - return m_collisionFlags; - } - - void setCollisionFlags(int flags) - { - m_collisionFlags = flags; - } - - ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar getCcdSweptSphereRadius() const - { - return m_ccdSweptSphereRadius; - } - - ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - void setCcdSweptSphereRadius(btScalar radius) - { - m_ccdSweptSphereRadius = radius; - } - - btScalar getCcdMotionThreshold() const - { - return m_ccdMotionThreshold; - } - - btScalar getCcdSquareMotionThreshold() const - { - 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) - { - m_ccdMotionThreshold = ccdMotionThreshold; - } - - ///users can point to their objects, userPointer is not used by Bullet - void* getUserPointer() const - { - return m_userObjectPointer; - } - - long long getUserIndex() const - { - return m_userIndex; - } - ///users can point to their objects, userPointer is not used by Bullet - void setUserPointer(void* userPointer) - { - m_userObjectPointer = userPointer; - } - - ///users can point to their objects, userPointer is not used by Bullet - void setUserIndex(long long index) - { - m_userIndex = index; - } - - int getUpdateRevisionInternal() const - { - return m_updateRevision; - } - - - inline bool checkCollideWith(const btCollisionObject* co) const - { - if (m_checkCollideWith) - return checkCollideWithOverride(co); - - 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; - - virtual void serializeSingleObject(class btSerializer* serializer) const; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCollisionObjectDoubleData -{ - void *m_broadphaseHandle; - void *m_collisionShape; - btCollisionShapeData *m_rootCollisionShape; - char *m_name; - - btTransformDoubleData m_worldTransform; - btTransformDoubleData m_interpolationWorldTransform; - btVector3DoubleData m_interpolationLinearVelocity; - btVector3DoubleData m_interpolationAngularVelocity; - btVector3DoubleData 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]; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCollisionObjectFloatData -{ - void *m_broadphaseHandle; - void *m_collisionShape; - btCollisionShapeData *m_rootCollisionShape; - char *m_name; - - btTransformFloatData m_worldTransform; - btTransformFloatData m_interpolationWorldTransform; - btVector3FloatData m_interpolationLinearVelocity; - btVector3FloatData m_interpolationAngularVelocity; - btVector3FloatData 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]; -}; - - - -SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const -{ - return sizeof(btCollisionObjectData); -} - - - -#endif //BT_COLLISION_OBJECT_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h deleted file mode 100644 index 952440b7d..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef BT_COLLISION_OBJECT_WRAPPER_H -#define BT_COLLISION_OBJECT_WRAPPER_H - -///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 - -#define BT_DECLARE_STACK_ONLY_OBJECT \ - private: \ - void* operator new(size_t size); \ - void operator delete(void*); - -struct btCollisionObjectWrapper; -struct btCollisionObjectWrapper -{ -BT_DECLARE_STACK_ONLY_OBJECT - -private: - btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. - btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&); - -public: - const btCollisionObjectWrapper* m_parent; - const btCollisionShape* m_shape; - const btCollisionObject* m_collisionObject; - const btTransform& m_worldTransform; - 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) - {} - - 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 diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.cpp deleted file mode 100644 index 093c6f9b2..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ /dev/null @@ -1,1552 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btCollisionWorld.h" -#include "btCollisionDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" -#include "LinearMath/btAabbUtil2.h" -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btSerializer.h" -#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -#include "BulletCollision/Gimpact/btGImpactShape.h" -//#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 - -//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" - - -///for debug drawing - -//for debug rendering -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - - - -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) - { - // - // only clear the cached algorithms - // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); - collisionObject->setBroadphaseHandle(0); - } - } - - -} - - - - - - - - - - -void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) -{ - - btAssert(collisionObject); - - //check that the object isn't already added - btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); - - m_collisionObjects.push_back(collisionObject); - - //calculate new AABB - btTransform trans = collisionObject->getWorldTransform(); - - btVector3 minAabb; - btVector3 maxAabb; - collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); - - int type = collisionObject->getCollisionShape()->getShapeType(); - collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( - minAabb, - maxAabb, - type, - collisionObject, - collisionFilterGroup, - collisionFilterMask, - m_dispatcher1,0 - )) ; - - - - - -} - - - -void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) -{ - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - //need to increase the aabb for contact thresholds - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); - minAabb -= contactThreshold; - maxAabb += contactThreshold; - - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) - { - btVector3 minAabb2,maxAabb2; - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); - minAabb2 -= contactThreshold; - maxAabb2 += contactThreshold; - minAabb.setMin(minAabb2); - maxAabb.setMax(maxAabb2); - } - - btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; - - //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) - { - bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } else - { - //something went wrong, investigate - //this assert is unwanted in 3D modelers (danger of loosing work) - colObj->setActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe && m_debugDrawer) - { - reportMe = false; - m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); - m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); - m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); - m_debugDrawer->reportErrorWarning("Thanks.\n"); - } - } -} - -void btCollisionWorld::updateAabbs() -{ - BT_PROFILE("updateAabbs"); - - btTransform predictedTrans; - for ( int i=0;iisActive()) - { - updateSingleAabb(colObj); - } - } -} - - -void btCollisionWorld::computeOverlappingPairs() -{ - BT_PROFILE("calculateOverlappingPairs"); - m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); -} - -void btCollisionWorld::performDiscreteCollisionDetection() -{ - BT_PROFILE("performDiscreteCollisionDetection"); - - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - - updateAabbs(); - - computeOverlappingPairs(); - - btDispatcher* dispatcher = getDispatcher(); - { - BT_PROFILE("dispatchAllCollisionPairs"); - if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); - } - -} - - - -void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - - - //bool removeFromBroadphase = false; - - { - - btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); - if (bp) - { - // - // only clear the cached algorithms - // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); - collisionObject->setBroadphaseHandle(0); - } - } - - - //swapremove - m_collisionObjects.remove(collisionObject); - -} - - -void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) -{ - 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) -{ - btSphereShape pointShape(btScalar(0.0)); - pointShape.setMargin(0.f); - const btConvexShape* castShape = &pointShape; - const btCollisionShape* collisionShape = collisionObjectWrap->getCollisionShape(); - const btTransform& colObjWorldTransform = collisionObjectWrap->getWorldTransform(); - - if (collisionShape->isConvex()) - { - // BT_PROFILE("rayTestConvex"); - 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); - - //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); - bool condition = true; - btConvexCast* convexCasterPtr = 0; - if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest) - convexCasterPtr = &subSimplexConvexCaster; - else - convexCasterPtr = &gjkConvexCaster; - - btConvexCast& convexCaster = *convexCasterPtr; - - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) - { - //add hit - if (castResult.m_normal.length2() > btScalar(0.0001)) - { - if (castResult.m_fraction < resultCallback.m_closestHitFraction) - { -#ifdef USE_SUBSIMPLEX_CONVEX_CAST - //rotate normal into worldspace - castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; -#endif //USE_SUBSIMPLEX_CONVEX_CAST - - castResult.m_normal.normalize(); - btCollisionWorld::LocalRayResult localRayResult - ( - collisionObjectWrap->getCollisionObject(), - 0, - castResult.m_normal, - castResult.m_fraction - ); - - bool normalInWorldSpace = true; - resultCallback.addSingleResult(localRayResult, normalInWorldSpace); - - } - } - } - } else { - if (collisionShape->isConcave()) - { - - //ConvexCast::CastResult - 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) - { - } - - - 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); - } - - }; - - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - - // BT_PROFILE("rayTestConcave"); - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - ///optimized version for btBvhTriangleMeshShape - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform); - rcb.m_hitFraction = resultCallback.m_closestHitFraction; - triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); - } - else if(collisionShape->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) - { - btGImpactMeshShape* concaveShape = (btGImpactMeshShape*)collisionShape; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform); - rcb.m_hitFraction = resultCallback.m_closestHitFraction; - concaveShape->processAllTrianglesRay(&rcb,rayFromLocal,rayToLocal); - }else - { - //generic (slower) case - btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; - - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - - //ConvexCast::CastResult - - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback - { - btCollisionWorld::RayResultCallback* m_resultCallback; - 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) - { - } - - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - - btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalWorld, - hitFraction); - - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); - } - - }; - - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform); - rcb.m_hitFraction = resultCallback.m_closestHitFraction; - - btVector3 rayAabbMinLocal = rayFromLocal; - rayAabbMinLocal.setMin(rayToLocal); - btVector3 rayAabbMaxLocal = rayFromLocal; - rayAabbMaxLocal.setMax(rayToLocal); - - concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); - } - } else { - // BT_PROFILE("rayTestCompound"); - if (collisionShape->isCompound()) - { - struct LocalInfoAdder2 : public RayResultCallback - { - RayResultCallback* m_userCallback; - int m_i; - - LocalInfoAdder2 (int i, RayResultCallback *user) - : m_userCallback(user), m_i(i) - { - m_closestHitFraction = m_userCallback->m_closestHitFraction; - m_flags = m_userCallback->m_flags; - } - virtual bool needsCollision(btBroadphaseProxy* p) const - { - return m_userCallback->needsCollision(p); - } - - virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = -1; - shapeInfo.m_triangleIndex = m_i; - if (r.m_localShapeInfo == NULL) - r.m_localShapeInfo = &shapeInfo; - - const btScalar result = m_userCallback->addSingleResult(r, b); - m_closestHitFraction = m_userCallback->m_closestHitFraction; - return result; - } - }; - - struct RayTester : btDbvt::ICollide - { - const btCollisionObject* m_collisionObject; - const btCompoundShape* m_compoundShape; - const btTransform& m_colObjWorldTransform; - 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) - { - - } - - 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 - - - - LocalInfoAdder2 my_cb(i, &m_resultCallback); - - rayTestSingleInternal( - m_rayFromTrans, - 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, - colObjWorldTransform, - rayFromTrans, - rayToTrans, - resultCallback); -#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); - } - else -#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) -{ - 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) -{ - const btCollisionShape* collisionShape = colObjWrap->getCollisionShape(); - const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform(); - - if (collisionShape->isConvex()) - { - //BT_PROFILE("convexSweepConvex"); - btConvexCast::CastResult castResult; - castResult.m_allowedPenetration = allowedPenetration; - castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? - - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; - - btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver); - //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); - //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); - - btConvexCast* castPtr = &convexCaster1; - - - - if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) - { - //add hit - if (castResult.m_normal.length2() > btScalar(0.0001)) - { - if (castResult.m_fraction < resultCallback.m_closestHitFraction) - { - castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( - colObjWrap->getCollisionObject(), - 0, - castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction - ); - - bool normalInWorldSpace = true; - resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); - - } - } - } - } else { - if (collisionShape->isConcave()) - { - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - //BT_PROFILE("convexSweepbtBvhTriangleMesh"); - btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); - btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); - // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation - btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); - - //ConvexCast::CastResult - struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback - { - btCollisionWorld::ConvexResultCallback* m_resultCallback; - 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) - { - } - - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - if (hitFraction <= m_resultCallback->m_closestHitFraction) - { - - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - - bool normalInWorldSpace = true; - - - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); - } - return hitFraction; - } - - }; - - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,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 - { - 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); - btConvexCast* castPtr = &convexCaster1; - - if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) - { - //add hit - if (castResult.m_normal.length2() > btScalar(0.0001)) - { - if (castResult.m_fraction < resultCallback.m_closestHitFraction) - { - castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( - colObjWrap->getCollisionObject(), - 0, - castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction - ); - - bool normalInWorldSpace = true; - resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); - } - } - } - - } else - { - //BT_PROFILE("convexSweepConcave"); - btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); - btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); - // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation - btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); - - //ConvexCast::CastResult - struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback - { - btCollisionWorld::ConvexResultCallback* m_resultCallback; - 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) - { - } - - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - if (hitFraction <= m_resultCallback->m_closestHitFraction) - { - - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - - bool normalInWorldSpace = false; - - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); - } - return hitFraction; - } - - }; - - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform); - tccb.m_hitFraction = resultCallback.m_closestHitFraction; - tccb.m_allowedPenetration = allowedPenetration; - btVector3 boxMinLocal, boxMaxLocal; - castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); - - btVector3 rayAabbMinLocal = convexFromLocal; - rayAabbMinLocal.setMin(convexToLocal); - btVector3 rayAabbMaxLocal = convexFromLocal; - rayAabbMaxLocal.setMax(convexToLocal); - rayAabbMinLocal += boxMinLocal; - rayAabbMaxLocal += boxMaxLocal; - concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); - } - } - } else { - ///@todo : use AABB tree or other BVH acceleration structure! - if (collisionShape->isCompound()) - { - BT_PROFILE("convexSweepCompound"); - const btCompoundShape* compoundShape = static_cast(collisionShape); - int i=0; - for (i=0;igetNumChildShapes();i++) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - - struct LocalInfoAdder : public ConvexResultCallback { - ConvexResultCallback* m_userCallback; - int m_i; - - LocalInfoAdder (int i, ConvexResultCallback *user) - : m_userCallback(user), m_i(i) - { - m_closestHitFraction = m_userCallback->m_closestHitFraction; - } - virtual bool needsCollision(btBroadphaseProxy* p) const - { - return m_userCallback->needsCollision(p); - } - virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = -1; - shapeInfo.m_triangleIndex = m_i; - if (r.m_localShapeInfo == NULL) - r.m_localShapeInfo = &shapeInfo; - const btScalar result = m_userCallback->addSingleResult(r, b); - m_closestHitFraction = m_userCallback->m_closestHitFraction; - return result; - - } - }; - - LocalInfoAdder my_cb(i, &resultCallback); - - btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i); - - objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans, - &tmpObj,my_cb, allowedPenetration); - - } - } - } - } -} - - -struct btSingleRayCallback : public btBroadphaseRayCallback -{ - - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btCollisionWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld-rayFromWorld); - - rayDir.normalize (); - ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); - - } - - - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; -#if 0 -#ifdef RECALCULATE_AABB - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); -#else - //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); - const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; - const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; -#endif -#endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } -}; - -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const -{ - //BT_PROFILE("rayTest"); - /// use the broadphase to accelerate the search for objects, based on their aabb - /// and for each object with ray-aabb overlap, perform an exact ray test - btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); - -#ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); -#else - for (int i=0;igetNumCollisionObjects();i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - -} - - -struct btSingleSweepCallback : public btBroadphaseRayCallback -{ - - btTransform m_convexFromTrans; - btTransform m_convexToTrans; - btVector3 m_hitNormal; - const btCollisionWorld* m_world; - btCollisionWorld::ConvexResultCallback& m_resultCallback; - btScalar m_allowedCcdPenetration; - const btConvexShape* m_castShape; - - - btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration) - :m_convexFromTrans(convexFromTrans), - m_convexToTrans(convexToTrans), - m_world(world), - m_resultCallback(resultCallback), - m_allowedCcdPenetration(allowedPenetration), - m_castShape(castShape) - { - btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); - btVector3 rayDir = unnormalizedRayDir.normalized(); - ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(unnormalizedRayDir); - - } - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further convex sweep tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback, - m_allowedCcdPenetration); - } - - return true; - } -}; - - - -void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const -{ - - BT_PROFILE("convexSweepTest"); - /// use the broadphase to accelerate the search for objects, based on their aabb - /// and for each object with ray-aabb overlap, perform an exact ray test - /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical - - - - btTransform convexFromTrans,convexToTrans; - convexFromTrans = convexFromWorld; - convexToTrans = convexToWorld; - btVector3 castShapeAabbMin, castShapeAabbMax; - /* Compute AABB that encompasses angular movement */ - { - btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel); - btVector3 zeroLinVel; - zeroLinVel.setValue(0,0,0); - btTransform R; - 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); - - 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())) { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing - btVector3 hitNormal; - if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) - { - objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); - } - } - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE -} - - - -struct btBridgedManifoldResult : public btManifoldResult -{ - - btCollisionWorld::ContactResultCallback& m_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) - { - bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); - btVector3 pointA = pointInWorld + normalOnBInWorld * depth; - btVector3 localA; - btVector3 localB; - if (isSwapped) - { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); - localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else - { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); - localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } - - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); - newPt.m_positionWorldOnA = pointA; - newPt.m_positionWorldOnB = pointInWorld; - - //BP mod, store contact triangles. - if (isSwapped) - { - newPt.m_partId0 = m_partId1; - newPt.m_partId1 = m_partId0; - newPt.m_index0 = m_index1; - newPt.m_index1 = m_index0; - } else - { - newPt.m_partId0 = m_partId0; - newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; - } - - //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); - - } - -}; - - - -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) - { - } - - virtual bool process(const btBroadphaseProxy* proxy) - { - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - if (collisionObject == m_collisionObject) - return true; - - //only perform raycast if filterMask matches - 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); - - btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1); - if (algorithm) - { - btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); - //discrete collision detection query - - algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult); - - algorithm->~btCollisionAlgorithm(); - m_world->getDispatcher()->freeCollisionAlgorithm(algorithm); - } - } - return true; - } -}; - - -///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) -{ - 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) -{ - 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); - if (algorithm) - { - btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); - //discrete collision detection query - 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; - -public: - - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : - m_debugDrawer(debugDrawer), - m_color(color), - m_worldTrans(worldTrans) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - processTriangle(triangle,partId,triangleIndex); - } - - virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - - btVector3 wv0,wv1,wv2; - wv0 = m_worldTrans*triangle[0]; - wv1 = m_worldTrans*triangle[1]; - wv2 = m_worldTrans*triangle[2]; - 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); - } -}; - - -void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) -{ - // Draw a small simplex at the center of the object - 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--) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* colShape = compoundShape->getChildShape(i); - debugDrawObject(worldTransform*childTrans,colShape,color); - } - - } 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; - } - - case SPHERE_SHAPE_PROXYTYPE: - { - const btSphereShape* sphereShape = static_cast(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - - getDebugDrawer()->drawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - { - const btMultiSphereShape* multiSphereShape = static_cast(shape); - - btTransform childTransform; - childTransform.setIdentity(); - - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) - { - childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); - getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); - } - - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast(shape); - - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - - 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(); - - 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; - } - - 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 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); - } - - - - } - - } - } -} - - -void btCollisionWorld::debugDrawWorld() -{ - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) - { - int numManifolds = getDispatcher()->getNumManifolds(); - btVector3 color(1,1,0); - 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(),color); - } - } - } - - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))) - { - int i; - - for ( i=0;igetCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) - { - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) - { - btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.)); - switch(colObj->getActivationState()) - { - case ACTIVE_TAG: - color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break; - case ISLAND_SLEEPING: - color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break; - case WANTS_DEACTIVATION: - color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break; - case DISABLE_DEACTIVATION: - color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break; - case DISABLE_SIMULATION: - color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break; - default: - { - color = btVector3(btScalar(1),btScalar(0.),btScalar(0.)); - } - }; - - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); - } - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - btVector3 minAabb,maxAabb; - btVector3 colorvec(1,0,0); - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); - minAabb -= contactThreshold; - maxAabb += contactThreshold; - - btVector3 minAabb2,maxAabb2; - - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) - { - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); - minAabb2 -= contactThreshold; - maxAabb2 += contactThreshold; - minAabb.setMin(minAabb2); - maxAabb.setMax(maxAabb2); - } - - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); - } - } - - } - } -} - - -void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) -{ - int i; - //serialize all collision objects - for (i=0;igetInternalType() == btCollisionObject::CO_COLLISION_OBJECT) - { - colObj->serializeSingleObject(serializer); - } - } - - ///keep track of shapes already serialized - btHashMap serializedShapes; - - for (i=0;igetCollisionShape(); - - if (!serializedShapes.find(shape)) - { - serializedShapes.insert(shape,shape); - shape->serializeSingleShape(serializer); - } - } - -} - - -void btCollisionWorld::serialize(btSerializer* serializer) -{ - - serializer->startSerialization(); - - serializeCollisionObjects(serializer); - - serializer->finishSerialization(); -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.h deleted file mode 100644 index b3fffdecd..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ /dev/null @@ -1,526 +0,0 @@ -/* -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. -*/ - - -/** - * @mainpage Bullet Documentation - * - * @section intro_sec Introduction - * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). - * - * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution. - * There is the Physics Forum for feedback and general Collision Detection and Physics discussions. - * Please visit http://www.bulletphysics.org - * - * @section install_sec Installation - * - * @subsection step1 Step 1: Download - * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list - * - * @subsection step2 Step 2: Building - * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms. - * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. - * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. - * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects: - * cd Bullet/build - * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4 - * cd Bullet/build/gmake - * make - * - * An alternative to premake is cmake. You can download cmake from http://www.cmake.org - * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles. - * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles. - * You can also use cmake in the command-line. Here are some examples for various platforms: - * cmake . -G "Visual Studio 9 2008" - * cmake . -G Xcode - * cmake . -G "Unix Makefiles" - * Although cmake is recommended, you can also use autotools for UNIX: ./autogen.sh ./configure to create a Makefile and then run make. - * - * @subsection step3 Step 3: Testing demos - * Try to run and experiment with BasicDemo executable as a starting point. - * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation. - * The Dependencies can be seen in this documentation under Directories - * - * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation - * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. - * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld. - * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras) - * Bullet Collision Detection can also be used without the Dynamics/Extras. - * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. - * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation. - * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. - * - * @section copyright Copyright - * 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 - -class btCollisionShape; -class btConvexShape; -class btBroadphaseInterface; -class btSerializer; - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "btCollisionObject.h" -#include "btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "LinearMath/btAlignedObjectArray.h" - -///CollisionWorld is interface and container for the collision detection -class btCollisionWorld -{ - - -protected: - - btAlignedObjectArray m_collisionObjects; - - btDispatcher* m_dispatcher1; - - btDispatcherInfo m_dispatchInfo; - - btBroadphaseInterface* m_broadphasePairCache; - - 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); - -public: - - //this constructor doesn't own the dispatcher and paircache/broadphase - btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); - - virtual ~btCollisionWorld(); - - void setBroadphase(btBroadphaseInterface* pairCache) - { - m_broadphasePairCache = pairCache; - } - - const btBroadphaseInterface* getBroadphase() const - { - return m_broadphasePairCache; - } - - btBroadphaseInterface* getBroadphase() - { - return m_broadphasePairCache; - } - - btOverlappingPairCache* getPairCache() - { - return m_broadphasePairCache->getOverlappingPairCache(); - } - - - btDispatcher* getDispatcher() - { - return m_dispatcher1; - } - - const btDispatcher* getDispatcher() const - { - return m_dispatcher1; - } - - void updateSingleAabb(btCollisionObject* colObj); - - 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 setDebugDrawer(btIDebugDraw* debugDrawer) - { - m_debugDrawer = debugDrawer; - } - - virtual btIDebugDraw* getDebugDrawer() - { - return m_debugDrawer; - } - - 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 - { - int m_shapePart; - int m_triangleIndex; - - //const btCollisionShape* m_shapeTemp; - //const btTransform* m_shapeLocalTransform; - }; - - 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) - { - } - - const btCollisionObject* m_collisionObject; - LocalShapeInfo* m_localShapeInfo; - btVector3 m_hitNormalLocal; - btScalar m_hitFraction; - - }; - - ///RayResultCallback is used to report new raycast results - struct RayResultCallback - { - btScalar m_closestHitFraction; - const btCollisionObject* m_collisionObject; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke. - unsigned int m_flags; - - virtual ~RayResultCallback() - { - } - 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) - { - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; - collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); - return collides; - } - - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0; - }; - - struct ClosestRayResultCallback : public RayResultCallback - { - 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_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 - { - ///need to transform normal into worldspace - m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; - } - m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); - return rayResult.m_hitFraction; - } - }; - - struct AllHitsRayResultCallback : public RayResultCallback - { - AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld) - { - } - - btAlignedObjectArray m_collisionObjects; - - btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_rayToWorld; - - btAlignedObjectArray m_hitNormalWorld; - btAlignedObjectArray m_hitPointWorld; - btAlignedObjectArray m_hitFractions; - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) - { - m_collisionObject = rayResult.m_collisionObject; - m_collisionObjects.push_back(rayResult.m_collisionObject); - btVector3 hitNormalWorld; - if (normalInWorldSpace) - { - hitNormalWorld = rayResult.m_hitNormalLocal; - } else - { - ///need to transform normal into worldspace - hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; - } - m_hitNormalWorld.push_back(hitNormalWorld); - btVector3 hitPointWorld; - 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) - { - } - - 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 - { - btScalar m_closestHitFraction; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - - ConvexResultCallback() - :m_closestHitFraction(btScalar(1.)), - m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) - { - } - - virtual ~ConvexResultCallback() - { - } - - bool hasHit() const - { - return (m_closestHitFraction < btScalar(1.)); - } - - - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; - collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); - return collides; - } - - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; - }; - - struct ClosestConvexResultCallback : public ConvexResultCallback - { - 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_hitNormalWorld; - btVector3 m_hitPointWorld; - const btCollisionObject* m_hitCollisionObject; - - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) - { -//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 - { - ///need to transform normal into worldspace - m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; - } - m_hitPointWorld = convexResult.m_hitPointLocal; - return convexResult.m_hitFraction; - } - }; - - ///ContactResultCallback is used to report contact points - struct ContactResultCallback - { - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - - ContactResultCallback() - :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) - { - } - - virtual ~ContactResultCallback() - { - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; - collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); - return collides; - } - - virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0; - }; - - - - 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; - - /// 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; - - ///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); - - ///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); - - - /// 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 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 objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - const btCollisionObjectWrapper* colObjWrap, - ConvexResultCallback& resultCallback, btScalar allowedPenetration); - - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); - - btCollisionObjectArray& getCollisionObjectArray() - { - return m_collisionObjects; - } - - const btCollisionObjectArray& getCollisionObjectArray() const - { - return m_collisionObjects; - } - - - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - virtual void performDiscreteCollisionDetection(); - - btDispatcherInfo& getDispatchInfo() - { - return m_dispatchInfo; - } - - const btDispatcherInfo& getDispatchInfo() const - { - return m_dispatchInfo; - } - - bool getForceUpdateAllAabbs() const - { - return m_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); - -}; - - -#endif //BT_COLLISION_WORLD_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp deleted file mode 100644 index 991841ee2..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ /dev/null @@ -1,375 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - -#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btAabbUtil2.h" -#include "btManifoldResult.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -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) -{ - m_ownsManifold = false; - - 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); -} - -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 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 - { - - 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); - } - } -} - -void btCompoundCollisionAlgorithm::removeChildAlgorithms() -{ - int numChildren = m_childCollisionAlgorithms.size(); - int i; - for (i=0;i~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); - } - } -} - -btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() -{ - removeChildAlgorithms(); -} - - - - -struct btCompoundLeafCallback : btDbvt::ICollide -{ - -public: - - const btCollisionObjectWrapper* m_compoundColObjWrap; - const btCollisionObjectWrapper* m_otherObjWrap; - btDispatcher* m_dispatcher; - const btDispatcherInfo& m_dispatchInfo; - btManifoldResult* m_resultOut; - btCollisionAlgorithm** m_childCollisionAlgorithms; - btPersistentManifold* m_sharedManifold; - - btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold) - :m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithms(childCollisionAlgorithms), - m_sharedManifold(sharedManifold) - { - - } - - - void ProcessChildShape(const btCollisionShape* childShape,int index) - { - btAssert(index>=0); - const btCompoundShape* compoundShape = static_cast(m_compoundColObjWrap->getCollisionShape()); - btAssert(indexgetNumChildShapes()); - - - //backup - btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); - btTransform orgInterpolationTrans = m_compoundColObjWrap->getWorldTransform(); - const btTransform& childTrans = compoundShape->getChildTransform(index); - btTransform newChildWorldTrans = orgTrans*childTrans ; - - //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); - m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); - - if (gCompoundChildShapePairCallback) - { - if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) - return; - } - - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) - { - - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); - - - //the contactpoint is still projected back using the original inverted worldtrans - if (!m_childCollisionAlgorithms[index]) - m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold); - - - const btCollisionObjectWrapper* tmpWrap = 0; - - ///detect swapping case - if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) - { - tmpWrap = m_resultOut->getBody0Wrap(); - m_resultOut->setBody0Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersA(-1,index); - } else - { - tmpWrap = m_resultOut->getBody1Wrap(); - m_resultOut->setBody1Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersB(-1,index); - } - - - m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); - -#if 0 - if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - btVector3 worldAabbMin,worldAabbMax; - m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1)); - m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1)); - } -#endif - - if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) - { - m_resultOut->setBody0Wrap(tmpWrap); - } else - { - m_resultOut->setBody1Wrap(tmpWrap); - } - - } - } - void Process(const btDbvtNode* leaf) - { - int index = leaf->dataAsInt; - - const btCompoundShape* compoundShape = static_cast(m_compoundColObjWrap->getCollisionShape()); - const btCollisionShape* childShape = compoundShape->getChildShape(index); - -#if 0 - if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - btVector3 worldAabbMin,worldAabbMax; - btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); - btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax); - m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0)); - } -#endif - - ProcessChildShape(childShape,index); - - } -}; - - - - - - -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; - - btAssert (colObjWrap->getCollisionShape()->isCompound()); - const btCompoundShape* compoundShape = static_cast(colObjWrap->getCollisionShape()); - - ///btCompoundShape might have changed: - ////make sure the internal child collision algorithm caches are still valid - if (compoundShape->getUpdateRevision() != m_compoundShapeRevision) - { - ///clear and update all - removeChildAlgorithms(); - - preallocateChildAlgorithms(body0Wrap,body1Wrap); - } - - - 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); - - ///we need to refresh all contact manifolds - ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep - ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm - { - int i; - btManifoldArray manifoldArray; - for (i=0;igetAllContactManifolds(manifoldArray); - for (int m=0;mgetNumContacts()) - { - resultOut->setPersistentManifold(manifoldArray[m]); - resultOut->refreshContactPoints(); - resultOut->setPersistentManifold(0);//??necessary? - } - } - manifoldArray.resize(0); - } - } - } - - if (tree) - { - - btVector3 localAabbMin,localAabbMax; - btTransform otherInCompoundSpace; - otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); - otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); - - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); - //process all children, that overlap with the given AABB bounds - tree->collideTV(tree->m_root,bounds,callback); - - } else - { - //iterate over all children, perform an AABB check inside ProcessChildShape - int numChildren = m_childCollisionAlgorithms.size(); - int i; - for (i=0;igetChildShape(i),i); - } - } - - { - //iterate over all children, perform an AABB check inside ProcessChildShape - int numChildren = m_childCollisionAlgorithms.size(); - int i; - btManifoldArray manifoldArray; - const btCollisionShape* childShape = 0; - btTransform orgTrans; - btTransform orgInterpolationTrans; - btTransform newChildWorldTrans; - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;igetChildShape(i); - //if not longer overlapping, remove the algorithm - orgTrans = colObjWrap->getWorldTransform(); - orgInterpolationTrans = colObjWrap->getWorldTransform(); - const btTransform& childTrans = compoundShape->getChildTransform(i); - newChildWorldTrans = orgTrans*childTrans ; - - //perform an AABB check first - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); - otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); - - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) - { - m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); - m_childCollisionAlgorithms[i] = 0; - } - } - } - } -} - -btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - 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; - - btAssert (colObj->getCollisionShape()->isCompound()); - - btCompoundShape* compoundShape = static_cast(colObj->getCollisionShape()); - - //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps - //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals - //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: - //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 - //then use each overlapping node AABB against Tree0 - //and vise versa. - - btScalar hitFraction = btScalar(1.); - - int numChildren = m_childCollisionAlgorithms.size(); - int i; - btTransform orgTrans; - btScalar frac; - for (i=0;igetChildShape(i); - - //backup - orgTrans = colObj->getWorldTransform(); - - const btTransform& childTrans = compoundShape->getChildTransform(i); - //btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( orgTrans*childTrans ); - - //btCollisionShape* tmpShape = colObj->getCollisionShape(); - //colObj->internalSetTemporaryCollisionShape( childShape ); - frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); - if (fracinternalSetTemporaryCollisionShape( tmpShape); - colObj->setWorldTransform( orgTrans); - } - return hitFraction; - -} - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h deleted file mode 100644 index 536751456..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ /dev/null @@ -1,99 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - -#ifndef BT_COMPOUND_COLLISION_ALGORITHM_H -#define BT_COMPOUND_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "btCollisionCreateFunc.h" -#include "LinearMath/btAlignedObjectArray.h" -class btDispatcher; -class btCollisionObject; - -class btCollisionShape; -typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); -extern btShapePairCallback gCompoundChildShapePairCallback; - -/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - btAlignedObjectArray m_childCollisionAlgorithms; - bool m_isSwapped; - - class btPersistentManifold* m_sharedManifold; - bool m_ownsManifold; - - - int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated - - void removeChildAlgorithms(); - - void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - -public: - - btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); - - virtual ~btCompoundCollisionAlgorithm(); - - btCollisionAlgorithm* getChildAlgorithm (int n) const - { - return m_childCollisionAlgorithms[n]; - } - - - 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); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - int i; - for (i=0;igetAllContactManifolds(manifoldArray); - } - } - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_COMPOUND_COLLISION_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp deleted file mode 100644 index a52dd34fe..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ /dev/null @@ -1,421 +0,0 @@ -/* -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 "btCompoundCompoundCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btAabbUtil2.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - - -btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; - -btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_sharedManifold(ci.m_manifold) -{ - m_ownsManifold = false; - - void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); - m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); - - const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); - - const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - 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(); - m_childCollisionAlgorithmCache->~btHashedSimplePairCache(); - btAlignedFree(m_childCollisionAlgorithmCache); -} - -void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) -{ - int i; - btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;igetAllContactManifolds(manifoldArray); - } - } -} - - -void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() -{ - btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - - int numChildren = pairs.size(); - int i; - for (i=0;i~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(algo); - } - } - m_childCollisionAlgorithmCache->removeAllPairs(); -} - -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_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithmCache(childAlgorithmsCache), - m_sharedManifold(sharedManifold), - m_numOverlapPairs(0) - { - - } - - - - - void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) - { - m_numOverlapPairs++; - - - int childIndex0 = leaf0->dataAsInt; - int childIndex1 = leaf1->dataAsInt; - - - btAssert(childIndex0>=0); - btAssert(childIndex1>=0); - - - const btCompoundShape* compoundShape0 = static_cast(m_compound0ColObjWrap->getCollisionShape()); - btAssert(childIndex0getNumChildShapes()); - - const btCompoundShape* compoundShape1 = static_cast(m_compound1ColObjWrap->getCollisionShape()); - btAssert(childIndex1getNumChildShapes()); - - const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); - const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); - - //backup - btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); - const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); - btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; - - btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); - const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); - btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; - - - //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); - - if (gCompoundCompoundChildShapePairCallback) - { - if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) - return; - } - - 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); - - btCollisionAlgorithm* colAlgo = 0; - - if (pair) - { - colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - - } else - { - colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); - pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); - btAssert(pair); - pair->m_userPointer = colAlgo; - } - - btAssert(colAlgo); - - const btCollisionObjectWrapper* tmpWrap0 = 0; - const btCollisionObjectWrapper* tmpWrap1 = 0; - - tmpWrap0 = m_resultOut->getBody0Wrap(); - tmpWrap1 = m_resultOut->getBody1Wrap(); - - m_resultOut->setBody0Wrap(&compoundWrap0); - m_resultOut->setBody1Wrap(&compoundWrap1); - - m_resultOut->setShapeIdentifiersA(-1,childIndex0); - m_resultOut->setShapeIdentifiersB(-1,childIndex1); - - - colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); - - m_resultOut->setBody0Wrap(tmpWrap0); - m_resultOut->setBody1Wrap(tmpWrap1); - - - - } - } -}; - - -static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, const btTransform& xform) -{ - btVector3 newmin,newmax; - btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); - 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) -{ - - if(root0&&root1) - { - int depth=1; - int treshold=btDbvt::DOUBLE_STACKSIZE-4; - btAlignedObjectArray stkStack; - stkStack.resize(btDbvt::DOUBLE_STACKSIZE); - stkStack[0]=btDbvt::sStkNN(root0,root1); - do { - btDbvt::sStkNN p=stkStack[--depth]; - if(MyIntersect(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++]=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); - } - } - 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) -{ - - const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; - - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); - const btCompoundShape* compoundShape0 = static_cast(col0ObjWrap->getCollisionShape()); - const btCompoundShape* compoundShape1 = static_cast(col1ObjWrap->getCollisionShape()); - - ///btCompoundShape might have changed: - ////make sure the internal child collision algorithm caches are still valid - if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) - { - ///clear all - removeChildAlgorithms(); - } - - - ///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; - btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;igetAllContactManifolds(manifoldArray); - for (int m=0;mgetNumContacts()) - { - resultOut->setPersistentManifold(manifoldArray[m]); - resultOut->refreshContactPoints(); - resultOut->setPersistentManifold(0); - } - } - manifoldArray.resize(0); - } - } - } - - - const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); - const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); - - 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); - - //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); - - //remove non-overlapping child pairs - - { - 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); - } - - { - btTransform orgInterpolationTrans1; - const btCollisionShape* childShape1 = 0; - btTransform orgTrans1; - btTransform newChildWorldTrans1; - - childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); - orgTrans1 = col1ObjWrap->getWorldTransform(); - orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); - const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = orgTrans1*childTrans1 ; - childShape1->getAabb(newChildWorldTrans1,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)); - } - } - } - for (int i=0;iremoveOverlappingPair(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) -{ - btAssert(0); - return 0.f; - -} - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h deleted file mode 100644 index 7e2d7ad70..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ /dev/null @@ -1,90 +0,0 @@ -/* -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 BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H -#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H - -#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h" -class btDispatcher; -class btCollisionObject; - -class btCollisionShape; -typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); -extern btShapePairCallback gCompoundCompoundChildShapePairCallback; - -/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes -class btCompoundCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; - btSimplePairArray m_removePairs; - - class btPersistentManifold* m_sharedManifold; - bool m_ownsManifold; - - - 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); - -public: - - 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); - - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp deleted file mode 100644 index 4ec9ae713..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ /dev/null @@ -1,246 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConvex2dConvex2dAlgorithm.h" - -//#include -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" - - -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" - -#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - - -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" - -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) -{ - m_numPerturbationIterations = 0; - m_minimumPointsPerturbationThreshold = 3; - m_simplexSolver = simplexSolver; - m_pdSolver = pdSolver; -} - -btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() -{ -} - -btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,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), - m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) -{ - (void)body0Wrap; - (void)body1Wrap; -} - - - - -btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) -{ - m_lowLevelOfDetail = useLowLevel; -} - - - -extern btScalar gContactBreakingThreshold; - - -// -// Convex-Convex collision algorithm -// -void btConvex2dConvex2dAlgorithm ::processCollision (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_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; - - { - - - btGjkPairDetector::ClosestPointInput input; - - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); - //TODO: if (dispatchInfo.m_useContinuous) - gjkPairDetector.setMinkowskiA(min0); - gjkPairDetector.setMinkowskiB(min1); - - { - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; - } - - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB = body1Wrap->getWorldTransform(); - - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); - - btVector3 v0,v1; - btVector3 sepNormalWorldSpace; - - } - - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } - -} - - - - -btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold - - ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold - ///col0->m_worldTransform, - btScalar resultFraction = btScalar(1.); - - - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); - btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); - - if (squareMot0 < col0->getCcdSquareMotionThreshold() && - squareMot1 < col1->getCcdSquareMotionThreshold()) - return resultFraction; - - - //An adhoc way of testing the Continuous Collision Detection algorithms - //One object is approximated as a sphere, to simplify things - //Starting in penetration should report no time of impact - //For proper CCD, better accuracy and handling of 'allowed' penetration should be added - //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - - - /// Convex0 against sphere for Convex1 - { - btConvexShape* convex0 = static_cast(col0->getCollisionShape()); - - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation - btConvexCast::CastResult result; - btVoronoiSimplexSolver voronoiSimplex; - //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); - ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); - //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) - { - - //store result.m_fraction in both bodies - - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); - - if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); - - if (resultFraction > result.m_fraction) - resultFraction = result.m_fraction; - - } - - - - - } - - /// Sphere (for convex0) against Convex1 - { - btConvexShape* convex1 = static_cast(col1->getCollisionShape()); - - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation - btConvexCast::CastResult result; - btVoronoiSimplexSolver voronoiSimplex; - //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); - ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); - //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) - { - - //store result.m_fraction in both bodies - - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); - - if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); - - if (resultFraction > result.m_fraction) - resultFraction = result.m_fraction; - - } - } - - return resultFraction; - -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h deleted file mode 100644 index 18d9385a1..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ /dev/null @@ -1,95 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H -#define BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H - -#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#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; - btConvexPenetrationDepthSolver* m_pdSolver; - - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - -public: - - 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - 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); - - - const btPersistentManifold* getManifold() - { - return m_manifoldPtr; - } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - - btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - - virtual ~CreateFunc(); - - 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); - } - }; - - -}; - -#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp deleted file mode 100644 index e23f5f7a8..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ /dev/null @@ -1,335 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btConvexConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_isSwapped(isSwapped), -m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) -{ -} - -btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() -{ -} - -void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) -{ - if (m_btConvexTriangleCallback.m_manifoldPtr) - { - manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr); - } -} - - -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()); - - clearCache(); -} - -btConvexTriangleCallback::~btConvexTriangleCallback() -{ - clearCache(); - m_dispatcher->releaseManifold( m_manifoldPtr ); - -} - - -void btConvexTriangleCallback::clearCache() -{ - m_dispatcher->clearManifold(m_manifoldPtr); -} - - -void btConvexTriangleCallback::processTriangle(btVector3* triangle,int -partId, int triangleIndex) -{ - - if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) - { - return; - } - - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - - const btCollisionObject* ob = const_cast(m_triBodyWrap->getCollisionObject()); - - btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher1 = m_dispatcher; - - //const btCollisionObject* ob = static_cast(m_triBodyWrap->getCollisionObject()); - - - - -#if 0 - ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) - { - btVector3 color(1,1,0); - btTransform& tr = ob->getWorldTransform(); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); - } -#endif - - if (m_convexBodyWrap->getCollisionShape()->isConvex()) - { - 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? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr); - - const btCollisionObjectWrapper* tmpWrap = 0; - - if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) - { - tmpWrap = m_resultOut->getBody0Wrap(); - m_resultOut->setBody0Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersA(partId,triangleIndex); - } - else - { - tmpWrap = m_resultOut->getBody1Wrap(); - m_resultOut->setBody1Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersB(partId,triangleIndex); - } - - colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut); - - if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) - { - m_resultOut->setBody0Wrap(tmpWrap); - } 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) -{ - m_convexBodyWrap = convexBodyWrap; - m_triBodyWrap = triBodyWrap; - - m_dispatchInfoPtr = &dispatchInfo; - m_collisionMarginTriangle = collisionMarginTriangle; - m_resultOut = resultOut; - - //recalc aabbs - btTransform convexInTriangleSpace; - convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); - const btCollisionShape* convexShape = static_cast(m_convexBodyWrap->getCollisionShape()); - //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle; - btVector3 extra(extraMargin,extraMargin,extraMargin); - - 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) -{ - - - 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()) - { - 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) -{ - (void)resultOut; - (void)dispatchInfo; - btCollisionObject* convexbody = m_isSwapped ? body1 : body0; - btCollisionObject* triBody = m_isSwapped ? body0 : body1; - - - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) - - //only perform CCD above a certain threshold, this prevents blocking on the long run - //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); - if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) - { - return btScalar(1.); - } - - //const btVector3& from = convexbody->m_worldTransform.getOrigin(); - //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); - //todo: only do if the motion exceeds the 'radius' - - btTransform triInv = triBody->getWorldTransform().inverse(); - btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); - btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - - struct LocalTriangleSphereCastCallback : public btTriangleCallback - { - btTransform m_ccdSphereFromTrans; - btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; - - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - - - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { - } - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - //do a swept sphere for now - btTransform ident; - ident.setIdentity(); - btConvexCast::CastResult castResult; - castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - //local space? - - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) - { - if (m_hitFraction > castResult.m_fraction) - m_hitFraction = castResult.m_fraction; - } - - } - - }; - - - - - - if (triBody->getCollisionShape()->isConcave()) - { - btVector3 rayAabbMin = convexFromLocal.getOrigin(); - rayAabbMin.setMin(convexToLocal.getOrigin()); - btVector3 rayAabbMax = convexFromLocal.getOrigin(); - rayAabbMax.setMax(convexToLocal.getOrigin()); - btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); - - raycastCallback.m_hitFraction = convexbody->getHitFraction(); - - btCollisionObject* concavebody = triBody; - - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - - if (triangleMesh) - { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); - } - - - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) - { - convexbody->setHitFraction( raycastCallback.m_hitFraction); - return raycastCallback.m_hitFraction; - } - } - - return btScalar(1.); - -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h deleted file mode 100644 index e90d06eb1..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ /dev/null @@ -1,121 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H -#define BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "btCollisionCreateFunc.h" - -///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. -class btConvexTriangleCallback : public btTriangleCallback -{ - const btCollisionObjectWrapper* m_convexBodyWrap; - const btCollisionObjectWrapper* m_triBodyWrap; - - btVector3 m_aabbMin; - btVector3 m_aabbMax ; - - - btManifoldResult* m_resultOut; - btDispatcher* m_dispatcher; - const btDispatcherInfo* m_dispatchInfoPtr; - btScalar m_collisionMarginTriangle; - -public: -int m_triangleCount; - - btPersistentManifold* m_manifoldPtr; - - 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); - - void clearCache(); - - SIMD_FORCE_INLINE const btVector3& getAabbMin() const - { - return m_aabbMin; - } - SIMD_FORCE_INLINE const btVector3& getAabbMax() const - { - return m_aabbMax; - } - -}; - - - - -/// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - - bool m_isSwapped; - - btConvexTriangleCallback m_btConvexTriangleCallback; - - - -public: - - 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); - - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - void clearCache(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp deleted file mode 100644 index 7f2722aa4..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ /dev/null @@ -1,783 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance -///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums -///with reproduction case -//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 -//#define ZERO_MARGIN - -#include "btConvexConvexAlgorithm.h" - -//#include -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" - - - -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" - -#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - - -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" - -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -/////////// - - - -static SIMD_FORCE_INLINE void segmentsClosestPoints( - btVector3& ptsVector, - btVector3& offsetA, - btVector3& offsetB, - btScalar& tA, btScalar& tB, - const btVector3& translation, - const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) -{ - // compute the parameters of the closest points on each line segment - - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); - - btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - - if ( denom == 0.0f ) { - tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - tB = tA * dirA_dot_dirB - dirB_dot_trans; - - if ( tB < -hlenB ) { - tB = -hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } else if ( tB > hlenB ) { - tB = hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - // compute the closest points relative to segment centers. - - offsetA = dirA * tA; - offsetB = dirB * tB; - - ptsVector = translation - offsetA + offsetB; -} - - -static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( - btVector3& normalOnB, - btVector3& pointOnB, - btScalar capsuleLengthA, - btScalar capsuleRadiusA, - btScalar capsuleLengthB, - btScalar capsuleRadiusB, - int capsuleAxisA, - int capsuleAxisB, - const btTransform& transformA, - const btTransform& transformB, - btScalar distanceThreshold ) -{ - btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); - btVector3 translationA = transformA.getOrigin(); - btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB); - btVector3 translationB = transformB.getOrigin(); - - // translation between centers - - btVector3 translation = translationB - translationA; - - // compute the closest points of the capsule line segments - - btVector3 ptsVector; // the vector between the closest points - - btVector3 offsetA, offsetB; // offsets from segment centers to their closest points - btScalar tA, tB; // parameters on line segment - - segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, - directionA, capsuleLengthA, directionB, capsuleLengthB ); - - btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; - - if ( distance > distanceThreshold ) - return distance; - - btScalar lenSqr = ptsVector.length2(); - if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON)) - { - //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' - btVector3 q; - btPlaneSpace1(directionA,normalOnB,q); - } else - { - // compute the contact normal - normalOnB = ptsVector*-btRecipSqrt(lenSqr); - } - pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB; - - return distance; -} - - - - - - - -////////// - - - - - -btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) -{ - m_numPerturbationIterations = 0; - m_minimumPointsPerturbationThreshold = 3; - m_simplexSolver = simplexSolver; - m_pdSolver = pdSolver; -} - -btConvexConvexAlgorithm::CreateFunc::~CreateFunc() -{ -} - -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,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), -#ifdef USE_SEPDISTANCE_UTIL2 -m_sepDistance((static_cast(body0->getCollisionShape()))->getAngularMotionDisc(), - (static_cast(body1->getCollisionShape()))->getAngularMotionDisc()), -#endif -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) -{ - (void)body0Wrap; - (void)body1Wrap; -} - - - - -btConvexConvexAlgorithm::~btConvexConvexAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) -{ - m_lowLevelOfDetail = useLowLevel; -} - - -struct btPerturbedContactResult : public btManifoldResult -{ - btManifoldResult* m_originalManifoldResult; - btTransform m_transformA; - btTransform m_transformB; - btTransform m_unPerturbedTransform; - bool m_perturbA; - btIDebugDraw* m_debugDrawer; - - - btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer) - :m_originalManifoldResult(originalResult), - m_transformA(transformA), - m_transformB(transformB), - m_unPerturbedTransform(unPerturbedTransform), - m_perturbA(perturbA), - m_debugDrawer(debugDrawer) - { - } - virtual ~ btPerturbedContactResult() - { - } - - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth) - { - btVector3 endPt,startPt; - btScalar newDepth; - btVector3 newNormal; - - if (m_perturbA) - { - btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; - endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); - newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); - startPt = endPt+normalOnBInWorld*newDepth; - } else - { - endPt = pointInWorld + normalOnBInWorld*orgDepth; - startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld); - newDepth = (endPt - startPt).dot(normalOnBInWorld); - - } - -//#define DEBUG_CONTACTS 1 -#ifdef DEBUG_CONTACTS - m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0)); - m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0)); - m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1)); -#endif //DEBUG_CONTACTS - - - m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth); - } - -}; - -extern btScalar gContactBreakingThreshold; - - -// -// Convex-Convex collision algorithm -// -void btConvexConvexAlgorithm ::processCollision (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_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; -#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER - if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) - { - btCapsuleShape* capsuleA = (btCapsuleShape*) min0; - btCapsuleShape* capsuleB = (btCapsuleShape*) min1; - // btVector3 localScalingA = capsuleA->getLocalScaling(); - // btVector3 localScalingB = capsuleB->getLocalScaling(); - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); - - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), - capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); - - if (dist=(SIMD_EPSILON*SIMD_EPSILON)); - resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); - } - resultOut->refreshContactPoints(); - return; - } -#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER - - - - -#ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); - } - - if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) -#endif //USE_SEPDISTANCE_UTIL2 - - { - - - btGjkPairDetector::ClosestPointInput input; - - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); - //TODO: if (dispatchInfo.m_useContinuous) - gjkPairDetector.setMinkowskiA(min0); - gjkPairDetector.setMinkowskiB(min1); - -#ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - input.m_maximumDistanceSquared = BT_LARGE_FLOAT; - } else -#endif //USE_SEPDISTANCE_UTIL2 - { - //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(); -// } - - 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) - { - sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; - //now perturbe directions to get multiple contact points - - } - } -#endif //USE_SEPDISTANCE_UTIL2 - - if (min0->isPolyhedral() && min1->isPolyhedral()) - { - - - struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result - { - 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) - { - } - }; - - - struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result - { - 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) - { - } - - 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()) - { - - - - - 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) - { - 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()); - - btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); - - } - if (m_ownManifold) - { - 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(); - - 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()); - - } 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 - - btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - if (l2>SIMD_EPSILON) - { - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - //minDist = gjkPairDetector.getCachedSeparatingDistance(); - //maxDist = threshold; - minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); - foundSepAxis = true; - } - } - - - if (foundSepAxis) - { - btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); - } - - - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } - - return; - } - - } - - - } - - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); - - //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) - { - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - - 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); - - - 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 (m_ownManifold) - { - resultOut->refreshContactPoints(); - } - -} - - - -bool disableCcd = false; -btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold - - ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold - ///col0->m_worldTransform, - btScalar resultFraction = btScalar(1.); - - - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); - btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); - - if (squareMot0 < col0->getCcdSquareMotionThreshold() && - squareMot1 < col1->getCcdSquareMotionThreshold()) - return resultFraction; - - if (disableCcd) - return btScalar(1.); - - - //An adhoc way of testing the Continuous Collision Detection algorithms - //One object is approximated as a sphere, to simplify things - //Starting in penetration should report no time of impact - //For proper CCD, better accuracy and handling of 'allowed' penetration should be added - //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - - - /// Convex0 against sphere for Convex1 - { - btConvexShape* convex0 = static_cast(col0->getCollisionShape()); - - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation - btConvexCast::CastResult result; - btVoronoiSimplexSolver voronoiSimplex; - //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); - ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); - //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) - { - - //store result.m_fraction in both bodies - - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); - - if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); - - if (resultFraction > result.m_fraction) - resultFraction = result.m_fraction; - - } - - - - - } - - /// Sphere (for convex0) against Convex1 - { - btConvexShape* convex1 = static_cast(col1->getCollisionShape()); - - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation - btConvexCast::CastResult result; - btVoronoiSimplexSolver voronoiSimplex; - //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); - ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); - //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) - { - - //store result.m_fraction in both bodies - - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); - - if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); - - if (resultFraction > result.m_fraction) - resultFraction = result.m_fraction; - - } - } - - return resultFraction; - -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h deleted file mode 100644 index 51db0c654..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ /dev/null @@ -1,108 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_CONVEX_ALGORITHM_H -#define BT_CONVEX_CONVEX_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "btCollisionCreateFunc.h" -#include "btCollisionDispatcher.h" -#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil - -class btConvexPenetrationDepthSolver; - -///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise -///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions. -///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util -///for certain pairs that have a small size ratio - -//#define USE_SEPDISTANCE_UTIL2 1 - -///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects. -///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal. -///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888 -class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm -{ -#ifdef USE_SEPDISTANCE_UTIL2 - btConvexSeparatingDistanceUtil m_sepDistance; -#endif - btSimplexSolverInterface* m_simplexSolver; - btConvexPenetrationDepthSolver* m_pdSolver; - - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - - - ///cache separating vector to speedup collision detection - - -public: - - btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); - - virtual ~btConvexConvexAlgorithm(); - - 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 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); - - - const btPersistentManifold* getManifold() - { - return m_manifoldPtr; - } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - - btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - - virtual ~CreateFunc(); - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); - return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); - } - }; - - -}; - -#endif //BT_CONVEX_CONVEX_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp deleted file mode 100644 index cce2d95bc..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConvexPlaneCollisionAlgorithm.h" - -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -//#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) -{ - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap; - - if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject())) - { - m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()); - m_ownManifold = true; - } -} - - -btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -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; - - btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); - - bool hasCollision = false; - const btVector3& planeNormal = planeShape->getPlaneNormal(); - const btScalar& planeConstant = planeShape->getPlaneConstant(); - - btTransform convexWorldTransform = convexObjWrap->getWorldTransform(); - btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; - //now perturbe the convex-world transform - convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); - btTransform planeInConvex; - 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 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; - - hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); - resultOut->setPersistentManifold(m_manifoldPtr); - if (hasCollision) - { - /// report a contact. internally this will be kept persistent, and contact reduction is done - btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; - btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); - } -} - - -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; - - 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(); - btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); - btVector3 vtxInPlane = convexInPlaneTrans(vtx); - btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; - btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; - - hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); - resultOut->setPersistentManifold(m_manifoldPtr); - if (hasCollision) - { - /// report a contact. internally this will be kept persistent, and contact reduction is done - btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; - btVector3 pOnB = vtxInPlaneWorld; - 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()getAngularMotionDisc(); - perturbeAngle = gContactBreakingThreshold / radius; - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; - - btQuaternion perturbeRot(v0,perturbeAngle); - for (int i=0;igetNumContacts()) - { - resultOut->refreshContactPoints(); - } - } -} - -btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h deleted file mode 100644 index d28c430c4..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ /dev/null @@ -1,84 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_PLANE_COLLISION_ALGORITHM_H -#define BT_CONVEX_PLANE_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "btCollisionDispatcher.h" - -#include "LinearMath/btVector3.h" - -/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - -public: - - btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); - - virtual ~btConvexPlaneCollisionAlgorithm(); - - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - void collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - - CreateFunc() - : m_numPerturbationIterations(1), - m_minimumPointsPerturbationThreshold(0) - { - } - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(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,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); - } - } - }; - -}; - -#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp deleted file mode 100644 index c3cacec4a..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ /dev/null @@ -1,307 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btDefaultCollisionConfiguration.h" - -#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h" - -#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" -#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM -#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM -#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" - - - -#include "LinearMath/btPoolAllocator.h" - - - - - -btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) -//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) -{ - - void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16); - m_simplexSolver = new (mem)btVoronoiSimplexSolver(); - - if (constructionInfo.m_useEpaPenetrationAlgorithm) - { - mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); - m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; - }else - { - mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); - m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; - } - - //default CreationFunctions, filling the m_doubleDispatch table - mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); - m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); - mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); - m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; - mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); - m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(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; -#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); - m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); - m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; - m_boxSphereCF->m_swapped = true; -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - - mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); - m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); - m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; - m_triangleSphereCF->m_swapped = true; - - mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16); - m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc; - - //convex versus plane - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); - m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); - m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; - m_planeConvexCF->m_swapped = true; - - ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - int maxSize = sizeof(btConvexConvexAlgorithm); - int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); - int maxSize3 = sizeof(btCompoundCollisionAlgorithm); - int sl = sizeof(btConvexSeparatingDistanceUtil); - sl = sizeof(btGjkPairDetector); - int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); - - - if (constructionInfo.m_persistentManifoldPool) - { - m_ownsPersistentManifoldPool = false; - m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; - } else - { - m_ownsPersistentManifoldPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); - } - - if (constructionInfo.m_collisionAlgorithmPool) - { - m_ownsCollisionAlgorithmPool = false; - m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; - } else - { - m_ownsCollisionAlgorithmPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); - } - - -} - -btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() -{ - if (m_ownsCollisionAlgorithmPool) - { - m_collisionAlgorithmPool->~btPoolAllocator(); - btAlignedFree(m_collisionAlgorithmPool); - } - if (m_ownsPersistentManifoldPool) - { - m_persistentManifoldPool->~btPoolAllocator(); - btAlignedFree(m_persistentManifoldPool); - } - - m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConvexCreateFunc); - - m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConcaveCreateFunc); - m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedConvexConcaveCreateFunc); - - m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_compoundCreateFunc); - - m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree(m_compoundCompoundCreateFunc); - - m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedCompoundCreateFunc); - - m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_emptyCreateFunc); - - m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereSphereCF); - -#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereBoxCF); - m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxSphereCF); -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - - m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereTriangleCF); - m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_triangleSphereCF); - m_boxBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxBoxCF); - - m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexPlaneCF); - m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_planeConvexCF); - - m_simplexSolver->~btVoronoiSimplexSolver(); - btAlignedFree(m_simplexSolver); - - m_pdSolver->~btConvexPenetrationDepthSolver(); - - btAlignedFree(m_pdSolver); - - -} - - -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) -{ - - - - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) - { - return m_sphereSphereCF; - } -#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) - { - return m_sphereBoxCF; - } - - if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) - { - return m_boxSphereCF; - } -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - - - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) - { - return m_sphereTriangleCF; - } - - if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) - { - return m_triangleSphereCF; - } - - if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) - { - return m_boxBoxCF; - } - - if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) - { - return m_convexPlaneCF; - } - - if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) - { - return m_planeConvexCF; - } - - - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) - { - return m_convexConvexCreateFunc; - } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) - { - return m_convexConcaveCreateFunc; - } - - if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) - { - return m_swappedConvexConcaveCreateFunc; - } - - - if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) - { - return m_compoundCompoundCreateFunc; - } - - if (btBroadphaseProxy::isCompound(proxyType0)) - { - return m_compoundCreateFunc; - } else - { - if (btBroadphaseProxy::isCompound(proxyType1)) - { - return m_swappedCompoundCreateFunc; - } - } - - //failed to find an algorithm - return m_emptyCreateFunc; -} - -void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) -{ - btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc; - convexConvex->m_numPerturbationIterations = numPerturbationIterations; - convexConvex->m_minimumPointsPerturbationThreshold = 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/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h deleted file mode 100644 index 2078420e1..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ /dev/null @@ -1,127 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_DEFAULT_COLLISION_CONFIGURATION -#define BT_DEFAULT_COLLISION_CONFIGURATION - -#include "btCollisionConfiguration.h" -class btVoronoiSimplexSolver; -class btConvexPenetrationDepthSolver; - -struct btDefaultCollisionConstructionInfo -{ - 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) - { - } -}; - - - -///btCollisionConfiguration allows to configure Bullet collision detection -///stack allocator, pool memory allocators -///@todo: describe the meaning -class btDefaultCollisionConfiguration : public btCollisionConfiguration -{ - -protected: - - int m_persistentManifoldPoolSize; - - - btPoolAllocator* m_persistentManifoldPool; - bool m_ownsPersistentManifoldPool; - - - btPoolAllocator* m_collisionAlgorithmPool; - bool m_ownsCollisionAlgorithmPool; - - //default simplex/penetration depth solvers - btVoronoiSimplexSolver* m_simplexSolver; - 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_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; - -public: - - - btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); - - virtual ~btDefaultCollisionConfiguration(); - - ///memory pools - virtual btPoolAllocator* getPersistentManifoldPool() - { - return m_persistentManifoldPool; - } - - virtual btPoolAllocator* getCollisionAlgorithmPool() - { - return m_collisionAlgorithmPool; - } - - - virtual btVoronoiSimplexSolver* getSimplexSolver() - { - return m_simplexSolver; - } - - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); - - ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. - ///By default, this feature is disabled for best performance. - ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature. - ///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled - ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first. - ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points. - ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. - void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); - - void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); - -}; - -#endif //BT_DEFAULT_COLLISION_CONFIGURATION - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp deleted file mode 100644 index 5fa1c8be5..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btEmptyCollisionAlgorithm.h" - - - -btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) -{ -} - -void btEmptyAlgorithm::processCollision (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* ) -{ -} - -btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) -{ - return btScalar(1.); -} - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h deleted file mode 100644 index cb0f15218..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ /dev/null @@ -1,54 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_EMPTY_ALGORITH -#define BT_EMPTY_ALGORITH -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "btCollisionCreateFunc.h" -#include "btCollisionDispatcher.h" - -#define ATTRIBUTE_ALIGNED(a) - -///EmptyAlgorithm is a stub for unsupported collision pairs. -///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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -} ATTRIBUTE_ALIGNED(16); - -#endif //BT_EMPTY_ALGORITH diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.cpp deleted file mode 100644 index 86141fa68..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.cpp +++ /dev/null @@ -1,171 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGhostObject.h" -#include "btCollisionWorld.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "LinearMath/btAabbUtil2.h" - -btGhostObject::btGhostObject() -{ - m_internalType = CO_GHOST_OBJECT; -} - -btGhostObject::~btGhostObject() -{ - ///btGhostObject should have been removed from the world, so no overlapping objects - btAssert(!m_overlappingObjects.size()); -} - - -void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) -{ - btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; - btAssert(otherObject); - ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) - { - //not found - m_overlappingObjects.push_back(otherObject); - } -} - -void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) -{ - btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; - btAssert(otherObject); - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index~btHashedOverlappingPairCache(); - btAlignedFree( m_hashPairCache ); -} - -void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) -{ - btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); - btAssert(actualThisProxy); - - btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; - btAssert(otherObject); - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) - { - m_overlappingObjects.push_back(otherObject); - m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); - } -} - -void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) -{ - btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; - btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); - btAssert(actualThisProxy); - - btAssert(otherObject); - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (indexremoveOverlappingPair(actualThisProxy,otherProxy,dispatcher); - } -} - - -void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const -{ - btTransform convexFromTrans,convexToTrans; - convexFromTrans = convexFromWorld; - convexToTrans = convexToWorld; - btVector3 castShapeAabbMin, castShapeAabbMax; - /* Compute AABB that encompasses angular movement */ - { - btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); - btTransform R; - R.setIdentity (); - R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); - } - - /// go over all objects, and if the ray intersects their aabb + cast shape aabb, - // do a ray-shape query using convexCaster (CCD) - int i; - for (i=0;igetBroadphaseHandle())) { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing - btVector3 hitNormal; - if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) - { - btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); - } - } - } - -} - -void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const -{ - btTransform rayFromTrans; - rayFromTrans.setIdentity(); - rayFromTrans.setOrigin(rayFromWorld); - btTransform rayToTrans; - rayToTrans.setIdentity(); - rayToTrans.setOrigin(rayToWorld); - - - int i; - for (i=0;igetBroadphaseHandle())) - { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback); - } - } -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h deleted file mode 100644 index 8ec861385..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h +++ /dev/null @@ -1,175 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_GHOST_OBJECT_H -#define BT_GHOST_OBJECT_H - - -#include "btCollisionObject.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" -#include "LinearMath/btAlignedAllocator.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "btCollisionWorld.h" - -class btConvexShape; - -class btDispatcher; - -///The btGhostObject can keep track of all objects that are overlapping -///By default, this overlap is based on the AABB -///This is useful for creating a character controller, collision sensors/triggers, explosions etc. -///We plan on adding rayTest and other queries for the btGhostObject -ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject -{ -protected: - - btAlignedObjectArray m_overlappingObjects; - -public: - - btGhostObject(); - - virtual ~btGhostObject(); - - void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; - - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; - - ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); - ///this method is mainly for expert/internal use only. - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); - - int getNumOverlappingObjects() const - { - return m_overlappingObjects.size(); - } - - btCollisionObject* getOverlappingObject(int index) - { - return m_overlappingObjects[index]; - } - - const btCollisionObject* getOverlappingObject(int index) const - { - return m_overlappingObjects[index]; - } - - btAlignedObjectArray& getOverlappingPairs() - { - return m_overlappingObjects; - } - - const btAlignedObjectArray getOverlappingPairs() const - { - return m_overlappingObjects; - } - - // - // internal cast - // - - static const btGhostObject* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_GHOST_OBJECT) - return (const btGhostObject*)colObj; - return 0; - } - static btGhostObject* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_GHOST_OBJECT) - return (btGhostObject*)colObj; - return 0; - } - -}; - -class btPairCachingGhostObject : public btGhostObject -{ - btHashedOverlappingPairCache* m_hashPairCache; - -public: - - btPairCachingGhostObject(); - - virtual ~btPairCachingGhostObject(); - - ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); - - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); - - btHashedOverlappingPairCache* getOverlappingPairCache() - { - return m_hashPairCache; - } - -}; - - - -///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. -class btGhostPairCallback : public btOverlappingPairCallback -{ - -public: - btGhostPairCallback() - { - } - - virtual ~btGhostPairCallback() - { - - } - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) - { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); - if (ghost0) - ghost0->addOverlappingObjectInternal(proxy1, proxy0); - if (ghost1) - ghost1->addOverlappingObjectInternal(proxy0, proxy1); - return 0; - } - - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) - { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); - if (ghost0) - ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); - if (ghost1) - ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); - return 0; - } - - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) - { - btAssert(0); - //need to keep track of all ghost objects and call them here - //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); - } - - - -}; - -#endif - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp deleted file mode 100644 index cfcca5654..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btHashedSimplePairCache.h" - - -#include - -int gOverlappingSimplePairs = 0; -int gRemoveSimplePairs =0; -int gAddedSimplePairs =0; -int gFindSimplePairs =0; - - - - -btHashedSimplePairCache::btHashedSimplePairCache(): - m_blockedForChanges(false) -{ - 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; - m_overlappingPairArray.reserve(initialAllocatedSize); - growTables(); -} - - - -btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) -{ - gFindSimplePairs++; - - - /*if (indexA > indexB) - btSwap(indexA, indexB);*/ - - int hash = static_cast(getHash(static_cast(indexA), static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); - - if (hash >= m_hashTable.size()) - { - return NULL; - } - - int index = m_hashTable[hash]; - while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false) - { - index = m_next[index]; - } - - if (index == BT_SIMPLE_NULL_PAIR) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; -} - -//#include - -void btHashedSimplePairCache::growTables() -{ - - int newCapacity = m_overlappingPairArray.capacity(); - - if (m_hashTable.size() < newCapacity) - { - //grow hashtable and next table - int curHashtableSize = m_hashTable.size(); - - m_hashTable.resize(newCapacity); - m_next.resize(newCapacity); - - - int i; - - for (i= 0; i < newCapacity; ++i) - { - m_hashTable[i] = BT_SIMPLE_NULL_PAIR; - } - for (i = 0; i < newCapacity; ++i) - { - 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 - 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 - - - btSimplePair* pair = internalFindPair(indexA, indexB, hash); - if (pair != NULL) - { - return pair; - } - - int count = m_overlappingPairArray.size(); - int oldCapacity = m_overlappingPairArray.capacity(); - void* mem = &m_overlappingPairArray.expandNonInitializing(); - - int newCapacity = m_overlappingPairArray.capacity(); - - if (oldCapacity < newCapacity) - { - growTables(); - //hash with new capacity - hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); - } - - 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) -{ - gRemoveSimplePairs++; - - - /*if (indexA > indexB) - btSwap(indexA, indexB);*/ - - int hash = static_cast(getHash(static_cast(indexA),static_cast(indexB)) & (m_overlappingPairArray.capacity()-1)); - - btSimplePair* pair = internalFindPair(indexA, indexB, hash); - if (pair == NULL) - { - return 0; - } - - - void* userData = pair->m_userPointer; - - - int pairIndex = int(pair - &m_overlappingPairArray[0]); - btAssert(pairIndex < m_overlappingPairArray.size()); - - // Remove the pair from the hash table. - int index = m_hashTable[hash]; - btAssert(index != BT_SIMPLE_NULL_PAIR); - - int previous = BT_SIMPLE_NULL_PAIR; - while (index != pairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_SIMPLE_NULL_PAIR) - { - btAssert(m_next[previous] == pairIndex); - m_next[previous] = m_next[pairIndex]; - } - else - { - m_hashTable[hash] = m_next[pairIndex]; - } - - // We now move the last pair into spot of the - // pair being removed. We need to fix the hash - // table indices to support the move. - - int lastPairIndex = m_overlappingPairArray.size() - 1; - - // If the removed pair is the last pair, we are done. - if (lastPairIndex == pairIndex) - { - m_overlappingPairArray.pop_back(); - return userData; - } - - // Remove the last pair from the hash table. - const 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)); - - index = m_hashTable[lastHash]; - btAssert(index != BT_SIMPLE_NULL_PAIR); - - previous = BT_SIMPLE_NULL_PAIR; - while (index != lastPairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_SIMPLE_NULL_PAIR) - { - btAssert(m_next[previous] == lastPairIndex); - m_next[previous] = m_next[lastPairIndex]; - } - else - { - m_hashTable[lastHash] = m_next[lastPairIndex]; - } - - // Copy the last pair into the remove pair's spot. - m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; - - // Insert the last pair into the hash table - m_next[pairIndex] = m_hashTable[lastHash]; - m_hashTable[lastHash] = pairIndex; - - m_overlappingPairArray.pop_back(); - - return userData; -} -//#include - - - - - - - - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h deleted file mode 100644 index e88ef97e9..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ /dev/null @@ -1,174 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H -#define BT_HASHED_SIMPLE_PAIR_CACHE_H - - - -#include "LinearMath/btAlignedObjectArray.h" - -const int BT_SIMPLE_NULL_PAIR=0xffffffff; - -struct btSimplePair -{ - 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; - }; -}; - -typedef btAlignedObjectArray btSimplePairArray; - - - -extern int gOverlappingSimplePairs; -extern int gRemoveSimplePairs; -extern int gAddedSimplePairs; -extern int gFindSimplePairs; - - - - -class btHashedSimplePairCache -{ - btSimplePairArray m_overlappingPairArray; - - bool m_blockedForChanges; - - -protected: - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - - -public: - btHashedSimplePairCache(); - virtual ~btHashedSimplePairCache(); - - void removeAllPairs(); - - 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) - { - gAddedSimplePairs++; - - return internalAddPair(indexA,indexB); - } - - - virtual btSimplePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - - const btSimplePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - - btSimplePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - const btSimplePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } - - - btSimplePair* findPair(int indexA,int indexB); - - int GetCount() const { return m_overlappingPairArray.size(); } - - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } -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) - { - int key = static_cast(((unsigned int)indexA) | (((unsigned int)indexB) <<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); - } - - - - - - 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) - { - index = m_next[index]; - } - - if ( index == BT_SIMPLE_NULL_PAIR ) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; - } - - -}; - - - - -#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp deleted file mode 100644 index 73fa4e87e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ /dev/null @@ -1,842 +0,0 @@ -#include "btInternalEdgeUtility.h" - -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -//#define DEBUG_INTERNAL_EDGE - -#ifdef DEBUG_INTERNAL_EDGE -#include -#endif //DEBUG_INTERNAL_EDGE - - -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW -static btIDebugDraw* gDebugDrawer = 0; - -void btSetDebugDrawer(btIDebugDraw* debugDrawer) -{ - gDebugDrawer = debugDrawer; -} - -static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color) -{ - if (gDebugDrawer) - gDebugDrawer->drawLine(from,to,color); -} -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - -static int btGetHash(int partId, int 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) -{ - const btVector3 refAxis0 = edgeA; - const btVector3 refAxis1 = normalA; - const btVector3 swingAxis = normalB; - btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); - return angle; -} - - -struct btConnectivityProcessor : public btTriangleCallback -{ - int m_partIdA; - int m_triangleIndexA; - btVector3* m_triangleVerticesA; - btTriangleInfoMap* m_triangleInfoMap; - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - //skip self-collisions - if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) - return; - - //skip duplicates (disabled for now) - //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) - // return; - - //search for shared vertices and edges - int numshared = 0; - 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(); - if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) - return; - - - 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) - return; - -#if 0 - printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", - m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(), - m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(), - m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ()); - - printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex); - printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", - triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(), - triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(), - triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); -#endif - - for (int i=0;i<3;i++) - { - for (int j=0;j<3;j++) - { - if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) - { - sharedVertsA[numshared] = i; - sharedVertsB[numshared] = j; - numshared++; - ///degenerate case - if(numshared >= 3) - return; - } - } - ///degenerate case - if(numshared >= 3) - return; - } - switch (numshared) - { - case 0: - { - break; - } - case 1: - { - //shared vertex - break; - } - 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 - if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) - { - sharedVertsA[0] = 2; - sharedVertsA[1] = 0; - int tmp = sharedVertsB[1]; - sharedVertsB[1] = sharedVertsB[0]; - sharedVertsB[0] = tmp; - } - - int hash = btGetHash(m_partIdA,m_triangleIndexA); - - btTriangleInfo* info = m_triangleInfoMap->find(hash); - if (!info) - { - btTriangleInfo tmp; - m_triangleInfoMap->insert(hash,tmp); - info = m_triangleInfoMap->find(hash); - } - - int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; - int otherIndexA = 3-sumvertsA; - - - 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 tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); - //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); - - btVector3 normalA; - btVector3 normalB; - tA.calcNormal(normalA); - tB.calcNormal(normalB); - edge.normalize(); - btVector3 edgeCrossA = edge.cross(normalA).normalize(); - - { - btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; - if (edgeCrossA.dot(tmp) < 0) - { - edgeCrossA*=-1; - } - } - - btVector3 edgeCrossB = edge.cross(normalB).normalize(); - - { - btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; - if (edgeCrossB.dot(tmp) < 0) - { - edgeCrossB*=-1; - } - } - - btScalar angle2 = 0; - btScalar ang4 = 0.f; - - - btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); - btScalar len2 = calculatedEdge.length2(); - - btScalar correctedAngle(0); - btVector3 calculatedNormalB = normalA; - bool isConvex = false; - - if (len2m_planarEpsilon) - { - angle2 = 0.f; - ang4 = 0.f; - } else - { - - calculatedEdge.normalize(); - btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); - calculatedNormalA.normalize(); - 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.); - - correctedAngle = isConvex ? ang4 : -ang4; - btQuaternion orn2(calculatedEdge,-correctedAngle); - calculatedNormalB = btMatrix3x3(orn2)*normalA; - - - } - - - - - - //alternatively use - //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); - - - switch (sumvertsA) - { - case 1: - { - 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) - { - computedNormalB*=-1; - info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; - } -#ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) - { - printf("warning: normals not identical\n"); - } -#endif//DEBUG_INTERNAL_EDGE - - info->m_edgeV0V1Angle = -correctedAngle; - - if (isConvex) - info->m_flags |= TRI_INFO_V0V1_CONVEX; - break; - } - case 2: - { - btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) - { - computedNormalB*=-1; - info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; - } - -#ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) - { - printf("warning: normals not identical\n"); - } -#endif //DEBUG_INTERNAL_EDGE - info->m_edgeV2V0Angle = -correctedAngle; - if (isConvex) - info->m_flags |= TRI_INFO_V2V0_CONVEX; - break; - } - case 3: - { - 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; - } -#ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) - { - printf("warning: normals not identical\n"); - } -#endif //DEBUG_INTERNAL_EDGE - info->m_edgeV1V2Angle = -correctedAngle; - - if (isConvex) - info->m_flags |= TRI_INFO_V1V2_CONVEX; - break; - } - } - - break; - } - default: - { - // printf("warning: duplicate triangle\n"); - } - - } - } -}; -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// - -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()) - return; - - trimeshShape->setTriangleInfoMap(triangleInfoMap); - - btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); - const btVector3& meshScaling = meshInterface->getScaling(); - - for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) - { - 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; - //PHY_ScalarType indexType=0; - - btVector3 triangleVerts[3]; - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); - btVector3 aabbMin,aabbMax; - - for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) - { - unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); - - for (int j=2;j>=0;j--) - { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); - } - } - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangleVerts[0]); - aabbMax.setMax(triangleVerts[0]); - aabbMin.setMin(triangleVerts[1]); - aabbMax.setMax(triangleVerts[1]); - aabbMin.setMin(triangleVerts[2]); - aabbMax.setMax(triangleVerts[2]); - - btConnectivityProcessor connectivityProcessor; - connectivityProcessor.m_partIdA = partId; - connectivityProcessor.m_triangleIndexA = triangleIndex; - connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; - connectivityProcessor.m_triangleInfoMap = triangleInfoMap; - - 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) -{ - btVector3 lineDelta = line1 - line0; - - // Handle degenerate lines - if ( lineDelta.fuzzyZero()) - { - nearestPoint = line0; - } - else - { - btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta); - - // Clamp the point to conform to the segment's endpoints - if ( delta < 0 ) - delta = 0; - else if ( delta > 1 ) - delta = 1; - - nearestPoint = line0 + lineDelta*delta; - } -} - - - - -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); - - if (correctedEdgeAngle<0) - { - if (curAngle < correctedEdgeAngle) - { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; - return true; - } - } - - if (correctedEdgeAngle>=0) - { - if (curAngle > correctedEdgeAngle) - { - 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) -{ - //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 (!triangleInfoMapPtr) - return; - - 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; - - 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 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 tri_normal; - tri_shape->calcNormal(tri_normal); - - //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); - btVector3 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 - - - - bool isNearEdge = false; - - int numConcaveEdgeHits = 0; - int numConvexEdgeHits = 0; - - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - localContactNormalOnB.normalize();//is this necessary? - - // Get closest edge - 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; - } - } - // 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; - } - } - // 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) - { -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#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 - { - - 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) - { - numConcaveEdgeHits++; - } - else - { - numConvexEdgeHits++; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); - if (isClamped) - { - 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); -#ifdef 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 - - 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 - - - - btScalar len = (contact-nearest).length(); - if(lenm_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 - - btVector3 edge(v1-v2); - - 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) - { - 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) - { - 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); -#ifdef 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 - - 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 - - btScalar len = (contact-nearest).length(); - if(lenm_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 - - 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) - { - 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) - { - 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); - } -#endif //DEBUG_INTERNAL_EDGE - - if (isNearEdge) - { - - if (numConcaveEdgeHits>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 - { - 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) - { - return; - } - //modify the normal to be the triangle normal (or backfacing normal) - 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/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h deleted file mode 100644 index 7d9aafeee..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h +++ /dev/null @@ -1,47 +0,0 @@ - -#ifndef BT_INTERNAL_EDGE_UTILITY_H -#define BT_INTERNAL_EDGE_UTILITY_H - -#include "LinearMath/btHashMap.h" -#include "LinearMath/btVector3.h" - -#include "BulletCollision/CollisionShapes/btTriangleInfoMap.h" - -///The btInternalEdgeUtility helps to avoid or reduce artifacts due to wrong collision normals caused by internal edges. -///See also http://code.google.com/p/bullet/issues/detail?id=27 - -class btBvhTriangleMeshShape; -class btCollisionObject; -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_CONVEX_DOUBLE_SIDED = 4 -}; - - -///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo' -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); - -///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 - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.cpp deleted file mode 100644 index 4b2986a00..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "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; - - - -///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) -{ - btScalar friction = body0->getRollingFriction() * body1->getRollingFriction(); - - 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 friction = body0->getFriction() * body1->getFriction(); - - const btScalar MAX_FRICTION = btScalar(10.); - if (friction < -MAX_FRICTION) - friction = -MAX_FRICTION; - if (friction > MAX_FRICTION) - friction = MAX_FRICTION; - return friction; - -} - -btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1) -{ - return body0->getRestitution() * body1->getRestitution(); -} - - - -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 -{ -} - - -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()) - return; - - bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); - - btVector3 pointA = pointInWorld + normalOnBInWorld * depth; - - btVector3 localA; - btVector3 localB; - - if (isSwapped) - { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); - localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else - { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); - localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } - - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); - newPt.m_positionWorldOnA = pointA; - newPt.m_positionWorldOnB = pointInWorld; - - int insertIndex = m_manifoldPtr->getCacheEntry(newPt); - - newPt.m_combinedFriction = calculateCombinedFriction(m_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()); - 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_partId0 = m_partId0; - newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; - } - //printf("depth=%f\n",depth); - ///@todo, check this for any side effects - if (insertIndex >= 0) - { - //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); - m_manifoldPtr->replaceContactPoint(newPt,insertIndex); - } else - { - insertIndex = m_manifoldPtr->addManifoldPoint(newPt); - } - - //User can override friction and/or restitution - if (gContactAddedCallback && - //and if either of the two bodies requires custom material - ((m_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); - } - -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.h deleted file mode 100644 index 977b9a02f..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.h +++ /dev/null @@ -1,150 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_MANIFOLD_RESULT_H -#define BT_MANIFOLD_RESULT_H - -class btCollisionObject; -struct btCollisionObjectWrapper; - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btManifoldPoint; - -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - -#include "LinearMath/btTransform.h" -#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; - -//#define DEBUG_PART_INDEX 1 - - -///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_partId1; - int m_index0; - int m_index1; - - -public: - - btManifoldResult() -#ifdef DEBUG_PART_INDEX - : - m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX - { - } - - btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - - virtual ~btManifoldResult() {}; - - void setPersistentManifold(btPersistentManifold* manifoldPtr) - { - m_manifoldPtr = manifoldPtr; - } - - const btPersistentManifold* getPersistentManifold() const - { - return m_manifoldPtr; - } - btPersistentManifold* getPersistentManifold() - { - return m_manifoldPtr; - } - - virtual void setShapeIdentifiersA(int partId0,int index0) - { - m_partId0=partId0; - m_index0=index0; - } - - virtual void setShapeIdentifiersB( int partId1,int index1) - { - m_partId1=partId1; - m_index1=index1; - } - - - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); - - SIMD_FORCE_INLINE void refreshContactPoints() - { - btAssert(m_manifoldPtr); - if (!m_manifoldPtr->getNumContacts()) - return; - - bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); - - if (isSwapped) - { - m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform()); - } else - { - m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform()); - } - } - - const btCollisionObjectWrapper* getBody0Wrap() const - { - return m_body0Wrap; - } - const btCollisionObjectWrapper* getBody1Wrap() const - { - return m_body1Wrap; - } - - void setBody0Wrap(const btCollisionObjectWrapper* obj0Wrap) - { - m_body0Wrap = obj0Wrap; - } - - void setBody1Wrap(const btCollisionObjectWrapper* obj1Wrap) - { - m_body1Wrap = obj1Wrap; - } - - const btCollisionObject* getBody0Internal() const - { - return m_body0Wrap->getCollisionObject(); - } - - const btCollisionObject* getBody1Internal() const - { - return m_body1Wrap->getCollisionObject(); - } - - /// 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); -}; - -#endif //BT_MANIFOLD_RESULT_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp deleted file mode 100644 index 134478225..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ /dev/null @@ -1,450 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "LinearMath/btScalar.h" -#include "btSimulationIslandManager.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" - -//#include -#include "LinearMath/btQuickprof.h" - -btSimulationIslandManager::btSimulationIslandManager(): -m_splitIslands(true) -{ -} - -btSimulationIslandManager::~btSimulationIslandManager() -{ -} - - -void btSimulationIslandManager::initUnionFind(int n) -{ - m_unionFind.reset(n); -} - - -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; - - 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) -{ - - // put the index into m_controllers into m_tag - int index = 0; - { - - int i; - for (i=0;igetCollisionObjectArray().size(); i++) - { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; - //Adding filtering here - if (!collisionObject->isStaticOrKinematicObject()) - { - collisionObject->setIslandTag(index++); - } - collisionObject->setCompanionId(-1); - collisionObject->setHitFraction(btScalar(1.)); - } - } - // do the union find - - initUnionFind( index ); - - findUnions(dispatcher,colWorld); -} - -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) -{ - // put the islandId ('find' value) into m_tag - { - int index = 0; - int i; - for (i=0;igetCollisionObjectArray().size();i++) - { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; - if (!collisionObject->isStaticOrKinematicObject()) - { - 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 - { - collisionObject->setIslandTag(-1); - collisionObject->setCompanionId(-2); - } - } - } -} - - -#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) -{ - - initUnionFind( int (colWorld->getCollisionObjectArray().size())); - - // put the index into m_controllers into m_tag - { - - int index = 0; - int i; - for (i=0;igetCollisionObjectArray().size(); i++) - { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; - collisionObject->setIslandTag(index); - collisionObject->setCompanionId(-1); - collisionObject->setHitFraction(btScalar(1.)); - index++; - - } - } - // do the union find - - findUnions(dispatcher,colWorld); -} - -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) -{ - // put the islandId ('find' value) into m_tag - { - - - int index = 0; - int i; - for (i=0;igetCollisionObjectArray().size();i++) - { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; - if (!collisionObject->isStaticOrKinematicObject()) - { - collisionObject->setIslandTag( m_unionFind.find(index) ); - collisionObject->setCompanionId(-1); - } else - { - collisionObject->setIslandTag(-1); - collisionObject->setCompanionId(-2); - } - index++; - } - } -} - -#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION - -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(); - 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); - } -}; - - -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 startIslandIndex; - - - //update the sleeping state for bodies, if all are sleeping - for ( startIslandIndex=0;startIslandIndexgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) - { -// printf("error in island management\n"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - if (colObj0->getIslandTag() == islandId) - { - if (colObj0->getActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->getActivationState()== DISABLE_DEACTIVATION) - { - allSleeping = false; - } - } - } - - - if (allSleeping) - { - int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) - { -// printf("error in island management\n"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - - if (colObj0->getIslandTag() == islandId) - { - colObj0->setActivationState( ISLAND_SLEEPING ); - } - } - } else - { - - int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) - { -// printf("error in island management\n"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - - if (colObj0->getIslandTag() == islandId) - { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) - { - colObj0->setActivationState( WANTS_DEACTIVATION); - colObj0->setDeactivationTime(0.f); - } - } - } - } - } - - - int i; - int maxNumManifolds = dispatcher->getNumManifolds(); - -//#define SPLIT_ISLANDS 1 -//#ifdef 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) || - ((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(); - } - if(m_splitIslands) - { - //filtering for response - if (dispatcher->needsResponse(colObj0,colObj1)) - m_islandmanifold.push_back(manifold); - } - } - } -} - - - -///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) -{ - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - buildIslands(dispatcher,collisionWorld); - - int endIslandIndex=1; - int startIslandIndex; - int numElem = getUnionFind().getNumElements(); - - BT_PROFILE("processIslands"); - - if(!m_splitIslands) - { - btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); - int maxNumManifolds = dispatcher->getNumManifolds(); - callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); - } - else - { - // Sort manifolds, based on islands - // Sort the vector using predicate and std::sort - //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); - - int numManifolds = int (m_islandmanifold.size()); - - //tried a radix sort, but quicksort/heapsort seems still faster - //@todo rewrite island management - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); - //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); - - //now process all active islands (sets of manifolds for now) - - int startManifoldIndex = 0; - int endManifoldIndex = 1; - - //int islandId; - - - - // printf("Start Islands\n"); - - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for ( startIslandIndex=0;startIslandIndexisActive()) - 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); - } - - if (numIslandManifolds) - { - startManifoldIndex = endManifoldIndex; - } - - m_islandBodies.resize(0); - } - } // else if(!splitIslands) - -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.h deleted file mode 100644 index e24c6afec..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ /dev/null @@ -1,81 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SIMULATION_ISLAND_MANAGER_H -#define BT_SIMULATION_ISLAND_MANAGER_H - -#include "BulletCollision/CollisionDispatch/btUnionFind.h" -#include "btCollisionCreateFunc.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btCollisionObject.h" - -class btCollisionObject; -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; - - bool m_splitIslands; - -public: - btSimulationIslandManager(); - virtual ~btSimulationIslandManager(); - - - void initUnionFind(int n); - - - btUnionFind& getUnionFind() { return m_unionFind;} - - virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher); - virtual void storeIslandActivationState(btCollisionWorld* world); - - - void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld); - - - - struct IslandCallback - { - virtual ~IslandCallback() {}; - - virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; - }; - - void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); - - void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); - - bool getSplitIslands() - { - return m_splitIslands; - } - void setSplitIslands(bool doSplitIslands) - { - m_splitIslands = doSplitIslands; - } - -}; - -#endif //BT_SIMULATION_ISLAND_MANAGER_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp deleted file mode 100644 index e8b567e0e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ /dev/null @@ -1,214 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSphereBoxCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#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) -{ - 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_ownManifold = true; - } -} - - -btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - - - -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; - - btVector3 pOnBox; - - btVector3 normalOnSurfaceB; - btScalar penetrationDepth; - btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin(); - const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape(); - btScalar radius = sphere0->getRadius(); - btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold(); - - resultOut->setPersistentManifold(m_manifoldPtr); - - if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance)) - { - /// report a contact. internally this will be kept persistent, and contact reduction is done - resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth); - } - - if (m_ownManifold) - { - if (m_manifoldPtr->getNumContacts()) - { - resultOut->refreshContactPoints(); - } - } - -} - -btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} - - -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(); - btScalar boxMargin = boxShape->getMargin(); - penetrationDepth = 1.0f; - - // convert the sphere position to the box's local space - 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()) ); - - btScalar intersectionDist = fRadius + boxMargin; - btScalar contactDist = intersectionDist + maxContactDistance; - normal = sphereRelPos - closestPoint; - - //if there is no penetration, we are done - btScalar dist2 = normal.length2(); - if (dist2 > contactDist * contactDist) - { - return false; - } - - btScalar distance; - - //special case if the sphere center is inside the box - if (dist2 <= SIMD_EPSILON) - { - distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); - } - else //compute the penetration details - { - distance = normal.length(); - normal /= distance; - } - - pointOnBox = closestPoint + normal * boxMargin; -// 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.getBasis() * normal; - normal = tmp; - - return true; -} - -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)); - - 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)); - } - - faceDist = boxHalfExtent.getY() - sphereRelPos.getY(); - if (faceDist < minDist) - { - minDist = faceDist; - closestPoint = sphereRelPos; - closestPoint.setY( boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); - } - - faceDist = boxHalfExtent.getY() + sphereRelPos.getY(); - if (faceDist < minDist) - { - minDist = faceDist; - closestPoint = sphereRelPos; - closestPoint.setY( -boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); - } - - faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ(); - if (faceDist < minDist) - { - minDist = faceDist; - closestPoint = sphereRelPos; - closestPoint.setZ( boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); - } - - faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ(); - if (faceDist < minDist) - { - minDist = faceDist; - closestPoint = sphereRelPos; - closestPoint.setZ( -boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); - } - - return minDist; -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h deleted file mode 100644 index eefaedc9e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ /dev/null @@ -1,75 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SPHERE_BOX_COLLISION_ALGORITHM_H -#define BT_SPHERE_BOX_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "btCollisionDispatcher.h" - -#include "LinearMath/btVector3.h" - -/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - 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 - { - 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,true); - } - } - }; - -}; - -#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp deleted file mode 100644 index 36ba21f5b..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSphereSphereCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#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) -{ - if (!m_manifoldPtr) - { - m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject()); - m_ownManifold = true; - } -} - -btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)dispatchInfo; - - if (!m_manifoldPtr) - return; - - resultOut->setPersistentManifold(m_manifoldPtr); - - btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape(); - btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape(); - - 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 -#endif - - ///iff distance positive, don't generate a new contact - if ( len > (radius0+radius1)) - { -#ifndef CLEAR_MANIFOLD - resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD - return; - } - ///distance (negative means penetration) - btScalar dist = len - (radius0+radius1); - - btVector3 normalOnSurfaceB(1,0,0); - if (len > SIMD_EPSILON) - { - normalOnSurfaceB = diff / len; - } - - ///point on A (worldspace) - ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; - ///point on B (worldspace) - 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); - -#ifndef CLEAR_MANIFOLD - resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD - -} - -btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)col0; - (void)col1; - (void)dispatchInfo; - (void)resultOut; - - //not yet - return btScalar(1.); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h deleted file mode 100644 index 3517a568a..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ /dev/null @@ -1,66 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H -#define BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "btCollisionDispatcher.h" - -class btPersistentManifold; - -/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -/// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - -public: - 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - virtual ~btSphereSphereCollisionAlgorithm(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp deleted file mode 100644 index 280a4d355..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSphereTriangleCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#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) -{ - if (!m_manifoldPtr) - { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); - m_ownManifold = true; - } -} - -btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -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; - - 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()); - - btDiscreteCollisionDetectorInterface::ClosestPointInput input; - 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); - - if (m_ownManifold) - resultOut->refreshContactPoints(); - -} - -btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h deleted file mode 100644 index 6b6e39a72..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ /dev/null @@ -1,69 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#define BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "btCollisionDispatcher.h" - -/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -/// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - 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(const btCollisionAlgorithmConstructionInfo& ci) - : btActivatingCollisionAlgorithm(ci) {} - - 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 void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - virtual ~btSphereTriangleCollisionAlgorithm(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - - 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); - } - }; - -}; - -#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.cpp deleted file mode 100644 index 522293359..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btUnionFind.h" - - - -btUnionFind::~btUnionFind() -{ - Free(); - -} - -btUnionFind::btUnionFind() -{ - -} - -void btUnionFind::allocate(int N) -{ - m_elements.resize(N); -} -void btUnionFind::Free() -{ - m_elements.clear(); -} - - -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; - } -} - - -class btUnionFindElementSortPredicate -{ - 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() -{ - - //first store the original body index, and islandId - int numElements = m_elements.size(); - - for (int i=0;i m_elements; - - 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(); - - 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); - } - - 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)); - } - - 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 - } - - 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); - - } - return x; - } - - - }; - - -#endif //BT_UNION_FIND_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.cpp deleted file mode 100644 index ecce028c2..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.cpp +++ /dev/null @@ -1,42 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btBox2dShape.h" - - -//{ - - -void btBox2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); -} - - -void btBox2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - //btScalar margin = btScalar(0.); - btVector3 halfExtents = getHalfExtentsWithMargin(); - - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); - -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.h deleted file mode 100644 index ce333783e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.h +++ /dev/null @@ -1,371 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_OBB_BOX_2D_SHAPE_H -#define BT_OBB_BOX_2D_SHAPE_H - -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "LinearMath/btVector3.h" -#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 -{ - - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead - - btVector3 m_centroid; - btVector3 m_vertices[4]; - btVector3 m_normals[4]; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 getHalfExtentsWithMargin() const - { - btVector3 halfExtents = getHalfExtentsWithoutMargin(); - 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 - } - - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { - btVector3 halfExtents = getHalfExtentsWithoutMargin(); - 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())); - } - - 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())); - } - - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const - { - const btVector3& halfExtents = getHalfExtentsWithoutMargin(); - - for (int i=0;iboxHalfExtents.getY()) - minDimension = boxHalfExtents.getY(); - setSafeMargin(minDimension); - - m_shapeType = BOX_2D_SHAPE_PROXYTYPE; - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; - }; - - 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; - - } - virtual void setLocalScaling(const btVector3& scaling) - { - 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 calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - - - - int getVertexCount() const - { - return 4; - } - - virtual int getNumVertices()const - { - return 4; - } - - const btVector3* getVertices() const - { - return &m_vertices[0]; - } - - const btVector3* getNormals() const - { - return &m_normals[0]; - } - - - - - - - - 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()); - 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 - { - 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)); - } - - - 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); - } - } - - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const - //virtual void getEdge(int i,Edge& edge) const - { - int edgeVert0 = 0; - int edgeVert1 = 0; - - switch (i) - { - case 0: - edgeVert0 = 0; - edgeVert1 = 1; - break; - case 1: - edgeVert0 = 0; - edgeVert1 = 2; - 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); - - } - - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); - } - - - - - - 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)); - - return result; - } - - - //debugging - virtual const char* getName()const - { - return "Box2d"; - } - - virtual int getNumPreferredPenetrationDirections() const - { - return 6; - } - - 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); - } - } - -}; - -#endif //BT_OBB_BOX_2D_SHAPE_H - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.cpp deleted file mode 100644 index 3859138f1..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#include "btBoxShape.h" - -btBoxShape::btBoxShape( const btVector3& boxHalfExtents) -: btPolyhedralConvexShape() -{ - m_shapeType = BOX_SHAPE_PROXYTYPE; - - setSafeMargin(boxHalfExtents); - - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; -}; - - - - -void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); -} - - -void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - //btScalar margin = btScalar(0.); - btVector3 halfExtents = getHalfExtentsWithMargin(); - - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); - -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.h deleted file mode 100644 index 715e3f2ab..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.h +++ /dev/null @@ -1,314 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_OBB_BOX_MINKOWSKI_H -#define BT_OBB_BOX_MINKOWSKI_H - -#include "btPolyhedralConvexShape.h" -#include "btCollisionMargin.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btMinMax.h" - -///The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by half extents, in local shape coordinates. When used as part of a btCollisionObject or btRigidBody it will be an oriented box in world space. -ATTRIBUTE_ALIGNED16(class) btBoxShape: public btPolyhedralConvexShape -{ - - //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead - - -public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 getHalfExtentsWithMargin() const - { - btVector3 halfExtents = getHalfExtentsWithoutMargin(); - btVector3 margin(getMargin(),getMargin(),getMargin()); - halfExtents += margin; - return halfExtents; - } - - const btVector3& getHalfExtentsWithoutMargin() const - { - return m_implicitShapeDimensions;//scaling is included, margin is not - } - - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { - btVector3 halfExtents = getHalfExtentsWithoutMargin(); - 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())); - } - - 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())); - } - - 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)); - } - - - 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); - } - } - - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const - //virtual void getEdge(int i,Edge& edge) const - { - int edgeVert0 = 0; - int edgeVert1 = 0; - - switch (i) - { - case 0: - edgeVert0 = 0; - edgeVert1 = 1; - break; - case 1: - edgeVert0 = 0; - edgeVert1 = 2; - 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); - - } - - getVertex(edgeVert0,pa ); - getVertex(edgeVert1,pb ); - } - - - - - - 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)); - - return result; - } - - - //debugging - virtual const char* getName()const - { - return "Box"; - } - - virtual int getNumPreferredPenetrationDirections() const - { - return 6; - } - - 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); - } - } - -}; - - -#endif //BT_OBB_BOX_MINKOWSKI_H - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp deleted file mode 100644 index ace4cfa26..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ /dev/null @@ -1,466 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -//#define DISABLE_BVH - -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" -#include "LinearMath/btSerializer.h" - -///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. -///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. -btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh) -:btTriangleMeshShape(meshInterface), -m_bvh(0), -m_triangleInfoMap(0), -m_useQuantizedAabbCompression(useQuantizedAabbCompression), -m_ownsBvh(false) -{ - m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; - //construct bvh from meshInterface -#ifndef DISABLE_BVH - - if (buildBvh) - { - buildOptimizedBvh(); - } - -#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) -{ - m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; - //construct bvh from meshInterface -#ifndef DISABLE_BVH - - if (buildBvh) - { - void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); - m_bvh = new (mem) btOptimizedBvh(); - - m_bvh->build(meshInterface,m_useQuantizedAabbCompression,bvhAabbMin,bvhAabbMax); - m_ownsBvh = true; - } - -#endif //DISABLE_BVH - -} - -void btBvhTriangleMeshShape::partialRefitTree(const btVector3& aabbMin,const btVector3& aabbMax) -{ - m_bvh->refitPartial( m_meshInterface,aabbMin,aabbMax ); - - m_localAabbMin.setMin(aabbMin); - m_localAabbMax.setMax(aabbMax); -} - - -void btBvhTriangleMeshShape::refitTree(const btVector3& aabbMin,const btVector3& aabbMax) -{ - m_bvh->refit( m_meshInterface, aabbMin,aabbMax ); - - recalcLocalAabb(); -} - -btBvhTriangleMeshShape::~btBvhTriangleMeshShape() -{ - if (m_ownsBvh) - { - m_bvh->~btOptimizedBvh(); - btAlignedFree(m_bvh); - } -} - -void btBvhTriangleMeshShape::performRaycast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) -{ - struct MyNodeOverlapCallback : public btNodeOverlapCallback - { - btStridingMeshInterface* m_meshInterface; - btTriangleCallback* m_callback; - - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) - { - } - - virtual void processNode(int nodeSubPart, int nodeTriangleIndex) - { - btVector3 m_triangle[3]; - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - nodeSubPart); - - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - - const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) - { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); - } - } - - /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); - m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); - } - }; - - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); - - m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); -} - -void btBvhTriangleMeshShape::performConvexcast (btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax) -{ - struct MyNodeOverlapCallback : public btNodeOverlapCallback - { - btStridingMeshInterface* m_meshInterface; - btTriangleCallback* m_callback; - - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) - { - } - - virtual void processNode(int nodeSubPart, int nodeTriangleIndex) - { - btVector3 m_triangle[3]; - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - nodeSubPart); - - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - - const btVector3& meshScaling = m_meshInterface->getScaling(); - for (int j=2;j>=0;j--) - { - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3(btScalar(graphicsbase[0])*meshScaling.getX(),btScalar(graphicsbase[1])*meshScaling.getY(),btScalar(graphicsbase[2])*meshScaling.getZ()); - } - } - - /* Perform ray vs. triangle collision here */ - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); - m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); - } - }; - - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); - - m_bvh->reportBoxCastOverlappingNodex (&myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); -} - -//perform bvh tree traversal and report overlapping triangles to 'callback' -void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const -{ - -#ifdef DISABLE_BVH - //brute force traverse all triangles - btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax); -#else - - //first get all the nodes - - - struct MyNodeOverlapCallback : public btNodeOverlapCallback - { - btStridingMeshInterface* m_meshInterface; - btTriangleCallback* m_callback; - btVector3 m_triangle[3]; - - - MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) - :m_meshInterface(meshInterface), - m_callback(callback) - { - } - - virtual void processNode(int nodeSubPart, int nodeTriangleIndex) - { - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - nodeSubPart); - - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT||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 - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - - m_triangle[j] = btVector3( - btScalar(graphicsbase[0])*meshScaling.getX(), - btScalar(graphicsbase[1])*meshScaling.getY(), - btScalar(graphicsbase[2])*meshScaling.getZ()); - } -#ifdef DEBUG_TRIANGLE_MESH - printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z()); -#endif //DEBUG_TRIANGLE_MESH - } - - m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); - m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); - } - - }; - - MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); - - m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - -#endif//DISABLE_BVH - - -} - -void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) -{ - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - btTriangleMeshShape::setLocalScaling(scaling); - buildOptimizedBvh(); - } -} - -void btBvhTriangleMeshShape::buildOptimizedBvh() -{ - if (m_ownsBvh) - { - m_bvh->~btOptimizedBvh(); - btAlignedFree(m_bvh); - } - ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work - void* mem = btAlignedAlloc(sizeof(btOptimizedBvh),16); - m_bvh = new(mem) btOptimizedBvh(); - //rebuild the bvh... - m_bvh->build(m_meshInterface,m_useQuantizedAabbCompression,m_localAabbMin,m_localAabbMax); - m_ownsBvh = true; -} - -void btBvhTriangleMeshShape::setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& scaling) -{ - btAssert(!m_bvh); - btAssert(!m_ownsBvh); - - m_bvh = bvh; - m_ownsBvh = false; - // update the scaling without rebuilding the bvh - if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) - { - btTriangleMeshShape::setLocalScaling(scaling); - } -} - - - -///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btTriangleMeshShapeData* trimeshData = (btTriangleMeshShapeData*) dataBuffer; - - 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)) - { - void* chunk = serializer->findPointer(m_bvh); - if (chunk) - { -#ifdef BT_USE_DOUBLE_PRECISION - 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 - { - -#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 - - int sz = m_bvh->calculateSerializeBufferSizeNew(); - 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); - } - } else - { - trimeshData->m_quantizedFloatBvh = 0; - trimeshData->m_quantizedDoubleBvh = 0; - } - - - - if (m_triangleInfoMap && !(serializer->getSerializationFlags()&BT_SERIALIZE_NO_TRIANGLEINFOMAP)) - { - void* chunk = serializer->findPointer(m_triangleInfoMap); - if (chunk) - { - trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)chunk; - } else - { - trimeshData->m_triangleInfoMap = (btTriangleInfoMapData*)serializer->getUniquePointer(m_triangleInfoMap); - int sz = m_triangleInfoMap->calculateSerializeBufferSize(); - 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); - } - } else - { - trimeshData->m_triangleInfoMap = 0; - } - - return "btTriangleMeshShapeData"; -} - -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); - const char* structType = m_bvh->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_QUANTIZED_BVH_CODE,(void*)m_bvh); - } -} - -void btBvhTriangleMeshShape::serializeSingleTriangleInfoMap(btSerializer* serializer) const -{ - if (m_triangleInfoMap) - { - int len = m_triangleInfoMap->calculateSerializeBufferSize(); - 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); - } -} - - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h deleted file mode 100644 index 493d63553..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ /dev/null @@ -1,145 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_BVH_TRIANGLE_MESH_SHAPE_H -#define BT_BVH_TRIANGLE_MESH_SHAPE_H - -#include "btTriangleMeshShape.h" -#include "btOptimizedBvh.h" -#include "LinearMath/btAlignedAllocator.h" -#include "btTriangleInfoMap.h" - -///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. -///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. -///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 -{ - - btOptimizedBvh* m_bvh; - btTriangleInfoMap* m_triangleInfoMap; - - bool m_useQuantizedAabbCompression; - bool m_ownsBvh; - bool m_pad[11];////need padding due to alignment - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - - 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); - - virtual ~btBvhTriangleMeshShape(); - - 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); - - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - 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); - - //debugging - virtual const char* getName()const {return "BVHTRIANGLEMESH";} - - - virtual void setLocalScaling(const btVector3& scaling); - - btOptimizedBvh* getOptimizedBvh() - { - return m_bvh; - } - - void setOptimizedBvh(btOptimizedBvh* bvh, const btVector3& localScaling=btVector3(1,1,1)); - - void buildOptimizedBvh(); - - bool usesQuantizedAabbCompression() const - { - return m_useQuantizedAabbCompression; - } - - void setTriangleInfoMap(btTriangleInfoMap* triangleInfoMap) - { - m_triangleInfoMap = triangleInfoMap; - } - - const btTriangleInfoMap* getTriangleInfoMap() const - { - return m_triangleInfoMap; - } - - 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 void serializeSingleBvh(btSerializer* serializer) const; - - virtual void serializeSingleTriangleInfoMap(btSerializer* serializer) const; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btTriangleMeshShapeData -{ - btCollisionShapeData m_collisionShapeData; - - btStridingMeshInterfaceData m_meshInterface; - - btQuantizedBvhFloatData *m_quantizedFloatBvh; - btQuantizedBvhDoubleData *m_quantizedDoubleBvh; - - btTriangleInfoMapData *m_triangleInfoMap; - - float m_collisionMargin; - - char m_pad3[4]; - -}; - - -SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() const -{ - return sizeof(btTriangleMeshShapeData); -} - - - -#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.cpp deleted file mode 100644 index 864df26e9..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ /dev/null @@ -1,171 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btCapsuleShape.h" - -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btQuaternion.h" - -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () -{ - m_shapeType = CAPSULE_SHAPE_PROXYTYPE; - m_upAxis = 1; - m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); -} - - - btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const -{ - - btVector3 supVec(0,0,0); - - btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - btVector3 vtx; - btScalar newDot; - - btScalar radius = getRadius(); - - - { - btVector3 pos(0,0,0); - pos[getUpAxis()] = getHalfHeight(); - - vtx = pos +vec*(radius) - vec * getMargin(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[getUpAxis()] = -getHalfHeight(); - - vtx = pos +vec*(radius) - vec * getMargin(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - - return supVec; - -} - - void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - - - btScalar radius = getRadius(); - - for (int j=0;j maxDot) - { - maxDot = newDot; - supportVerticesOut[j] = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*(radius) - vec * getMargin(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supportVerticesOut[j] = vtx; - } - } - - } -} - - -void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - //as an approximation, take the inertia of the box that bounds the spheres - - btTransform ident; - ident.setIdentity(); - - - btScalar radius = getRadius(); - - btVector3 halfExtents(radius,radius,radius); - halfExtents[getUpAxis()]+=getHalfHeight(); - - btScalar margin = CONVEX_DISTANCE_MARGIN; - - btScalar lx=btScalar(2.)*(halfExtents[0]+margin); - btScalar ly=btScalar(2.)*(halfExtents[1]+margin); - btScalar lz=btScalar(2.)*(halfExtents[2]+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * btScalar(.08333333); - - inertia[0] = scaledmass * (y2+z2); - inertia[1] = scaledmass * (x2+z2); - inertia[2] = scaledmass * (x2+y2); - -} - -btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) -{ - m_upAxis = 0; - m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); -} - - - - - - -btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) -{ - m_upAxis = 2; - m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); -} - - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.h deleted file mode 100644 index 7578bb258..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.h +++ /dev/null @@ -1,184 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CAPSULE_SHAPE_H -#define BT_CAPSULE_SHAPE_H - -#include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - - -///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis. -///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. -///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres. -ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape -{ -protected: - int m_upAxis; - -protected: - ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. - btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCapsuleShape(btScalar radius,btScalar height); - - ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - 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; - - } - - virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const - { - btVector3 halfExtents(getRadius(),getRadius(),getRadius()); - halfExtents[m_upAxis] = getRadius() + getHalfHeight(); - halfExtents += btVector3(getMargin(),getMargin(),getMargin()); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - - aabbMin = center - extent; - aabbMax = center + extent; - } - - virtual const char* getName()const - { - return "CapsuleShape"; - } - - int getUpAxis() const - { - return m_upAxis; - } - - btScalar getRadius() const - { - int radiusAxis = (m_upAxis+2)%3; - return m_implicitShapeDimensions[radiusAxis]; - } - - btScalar getHalfHeight() const - { - return m_implicitShapeDimensions[m_upAxis]; - } - - virtual void setLocalScaling(const btVector3& scaling) - { - 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 btVector3 getAnisotropicRollingFrictionDirection() const - { - btVector3 aniDir(0,0,0); - aniDir[getUpAxis()]=1; - return aniDir; - } - - - 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; - - -}; - -///btCapsuleShapeX represents a capsule around the Z axis -///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. -class btCapsuleShapeX : public btCapsuleShape -{ -public: - - btCapsuleShapeX(btScalar radius,btScalar height); - - //debugging - virtual const char* getName()const - { - return "CapsuleX"; - } - - - -}; - -///btCapsuleShapeZ represents a capsule around the Z axis -///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. -class btCapsuleShapeZ : public btCapsuleShape -{ -public: - btCapsuleShapeZ(btScalar radius,btScalar height); - - //debugging - virtual const char* getName()const - { - return "CapsuleZ"; - } - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCapsuleShapeData -{ - btConvexInternalShapeData m_convexInternalShapeData; - - int m_upAxis; - - char m_padding[4]; -}; - -SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const -{ - return sizeof(btCapsuleShapeData); -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer; - - btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); - - shapeData->m_upAxis = m_upAxis; - - return "btCapsuleShapeData"; -} - -#endif //BT_CAPSULE_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionMargin.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionMargin.h deleted file mode 100644 index 474bf1fb4..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionMargin.h +++ /dev/null @@ -1,27 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION_MARGIN_H -#define BT_COLLISION_MARGIN_H - -///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 - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.cpp deleted file mode 100644 index 39ee21cad..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "LinearMath/btSerializer.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 btBulletCollisionProbe (); - -void btBulletCollisionProbe () {} -} - - - -void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const -{ - btTransform tr; - tr.setIdentity(); - btVector3 aabbMin,aabbMax; - - getAabb(tr,aabbMin,aabbMax); - - radius = (aabbMax-aabbMin).length()*btScalar(0.5); - center = (aabbMin+aabbMax)*btScalar(0.5); -} - - -btScalar btCollisionShape::getContactBreakingThreshold(btScalar defaultContactThreshold) const -{ - return getAngularMotionDisc() * defaultContactThreshold; -} - -btScalar btCollisionShape::getAngularMotionDisc() const -{ - ///@todo cache this value, to improve performance - btVector3 center; - btScalar disc; - getBoundingSphere(center,disc); - disc += (center).length(); - return disc; -} - -void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const -{ - //start with static aabb - getAabb(curTrans,temporalAabbMin,temporalAabbMax); - - btScalar temporalAabbMaxx = temporalAabbMax.getX(); - btScalar temporalAabbMaxy = temporalAabbMax.getY(); - btScalar temporalAabbMaxz = temporalAabbMax.getZ(); - btScalar temporalAabbMinx = temporalAabbMin.getX(); - btScalar temporalAabbMiny = temporalAabbMin.getY(); - btScalar temporalAabbMinz = temporalAabbMin.getZ(); - - // add linear motion - btVector3 linMotion = linvel*timeStep; - ///@todo: simd would have a vector max/min operation, instead of per-element access - if (linMotion.x() > btScalar(0.)) - temporalAabbMaxx += linMotion.x(); - else - temporalAabbMinx += linMotion.x(); - if (linMotion.y() > btScalar(0.)) - temporalAabbMaxy += linMotion.y(); - else - temporalAabbMiny += linMotion.y(); - if (linMotion.z() > btScalar(0.)) - temporalAabbMaxz += linMotion.z(); - else - temporalAabbMinz += linMotion.z(); - - //add conservative angular motion - btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; - btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); - temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); - temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); - - temporalAabbMin -= angularMotion3d; - temporalAabbMax += angularMotion3d; -} - -///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btCollisionShapeData* shapeData = (btCollisionShapeData*) dataBuffer; - char* name = (char*) serializer->findNameForPointer(this); - shapeData->m_name = (char*)serializer->getUniquePointer(name); - if (shapeData->m_name) - { - serializer->serializeName(name); - } - shapeData->m_shapeType = m_shapeType; - //shapeData->m_padding//?? - return "btCollisionShapeData"; -} - -void btCollisionShape::serializeSingleShape(btSerializer* serializer) const -{ - int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); - const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SHAPE_CODE,(void*)this); -} \ No newline at end of file diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.h deleted file mode 100644 index ff017a206..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.h +++ /dev/null @@ -1,159 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION_SHAPE_H -#define BT_COLLISION_SHAPE_H - -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#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 -{ -protected: - int m_shapeType; - void* m_userPointer; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCollisionShape() : m_shapeType (INVALID_SHAPE_PROXYTYPE), m_userPointer(0) - { - } - - virtual ~btCollisionShape() - { - } - - ///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 getBoundingSphere(btVector3& center,btScalar& radius) const; - - ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. - 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; - - - - SIMD_FORCE_INLINE bool isPolyhedral() const - { - return btBroadphaseProxy::isPolyhedral(getShapeType()); - } - - SIMD_FORCE_INLINE bool isConvex2d() const - { - return btBroadphaseProxy::isConvex2d(getShapeType()); - } - - SIMD_FORCE_INLINE bool isConvex() const - { - return btBroadphaseProxy::isConvex(getShapeType()); - } - SIMD_FORCE_INLINE bool isNonMoving() const - { - return btBroadphaseProxy::isNonMoving(getShapeType()); - } - SIMD_FORCE_INLINE bool isConcave() const - { - return btBroadphaseProxy::isConcave(getShapeType()); - } - SIMD_FORCE_INLINE bool isCompound() const - { - return btBroadphaseProxy::isCompound(getShapeType()); - } - - SIMD_FORCE_INLINE bool isSoftBody() const - { - return btBroadphaseProxy::isSoftBody(getShapeType()); - } - - ///isInfinite is used to catch simulation error (aabb check) - SIMD_FORCE_INLINE bool isInfinite() const - { - return btBroadphaseProxy::isInfinite(getShapeType()); - } - -#ifndef __SPU__ - 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__ - - - 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 - { - return btVector3(1,1,1); - } - virtual void setMargin(btScalar margin) = 0; - virtual btScalar getMargin() const = 0; - - - ///optional user data pointer - void setUserPointer(void* userPtr) - { - m_userPointer = userPtr; - } - - void* getUserPointer() const - { - return m_userPointer; - } - - 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 void serializeSingleShape(btSerializer* serializer) const; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCollisionShapeData -{ - char *m_name; - int m_shapeType; - char m_padding[4]; -}; - -SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const -{ - return sizeof(btCollisionShapeData); -} - - - -#endif //BT_COLLISION_SHAPE_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.cpp deleted file mode 100644 index 0aa75f2bf..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ /dev/null @@ -1,356 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btCompoundShape.h" -#include "btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" -#include "LinearMath/btSerializer.h" - -btCompoundShape::btCompoundShape(bool enableDynamicAabbTree) -: m_localAabbMin(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)), -m_localAabbMax(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)), -m_dynamicAabbTree(0), -m_updateRevision(1), -m_collisionMargin(btScalar(0.)), -m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) -{ - m_shapeType = COMPOUND_SHAPE_PROXYTYPE; - - if (enableDynamicAabbTree) - { - void* mem = btAlignedAlloc(sizeof(btDbvt),16); - m_dynamicAabbTree = new(mem) btDbvt(); - btAssert(mem==m_dynamicAabbTree); - } -} - - -btCompoundShape::~btCompoundShape() -{ - if (m_dynamicAabbTree) - { - m_dynamicAabbTree->~btDbvt(); - btAlignedFree(m_dynamicAabbTree); - } -} - -void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) -{ - m_updateRevision++; - //m_childTransforms.push_back(localTransform); - //m_childShapes.push_back(shape); - btCompoundShapeChild child; - child.m_node = 0; - child.m_transform = localTransform; - child.m_childShape = shape; - child.m_childShapeType = shape->getShapeType(); - child.m_childMargin = shape->getMargin(); - - - //extend the local aabbMin/aabbMax - btVector3 localAabbMin,localAabbMax; - shape->getAabb(localTransform,localAabbMin,localAabbMax); - for (int i=0;i<3;i++) - { - if (m_localAabbMin[i] > localAabbMin[i]) - { - m_localAabbMin[i] = localAabbMin[i]; - } - if (m_localAabbMax[i] < localAabbMax[i]) - { - m_localAabbMax[i] = localAabbMax[i]; - } - - } - if (m_dynamicAabbTree) - { - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); - int index = m_children.size(); - child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); - } - - m_children.push_back(child); - -} - -void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform,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); - //int index = m_children.size()-1; - m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds); - } - - if (shouldRecalculateLocalAabb) - { - recalculateLocalAabb(); - } -} - -void btCompoundShape::removeChildShapeByIndex(int childShapeIndex) -{ - m_updateRevision++; - btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size()); - if (m_dynamicAabbTree) - { - m_dynamicAabbTree->remove(m_children[childShapeIndex].m_node); - } - m_children.swap(childShapeIndex,m_children.size()-1); - 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--) - { - if(m_children[i].m_childShape == shape) - { - removeChildShapeByIndex(i); - } - } - - - - recalculateLocalAabb(); -} - -void btCompoundShape::recalculateLocalAabb() -{ - // Recalculate the local aabb - // Brute force, it iterates over all the shapes left. - - m_localAabbMin = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_localAabbMax = btVector3(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - - //extend the local aabbMin/aabbMax - for (int j = 0; j < m_children.size(); j++) - { - btVector3 localAabbMin,localAabbMax; - m_children[j].m_childShape->getAabb(m_children[j].m_transform, localAabbMin, localAabbMax); - for (int i=0;i<3;i++) - { - if (m_localAabbMin[i] > localAabbMin[i]) - m_localAabbMin[i] = localAabbMin[i]; - if (m_localAabbMax[i] < localAabbMax[i]) - m_localAabbMax[i] = localAabbMax[i]; - } - } -} - -///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const -{ - btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin); - btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin); - - //avoid an illegal AABB when there are no children - if (!m_children.size()) - { - localHalfExtents.setValue(0,0,0); - localCenter.setValue(0,0,0); - } - localHalfExtents += btVector3(getMargin(),getMargin(),getMargin()); - - - btMatrix3x3 abs_b = trans.getBasis().absolute(); - - btVector3 center = trans(localCenter); - - btVector3 extent = 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 -{ - //approximation: take the inertia from the aabb for now - btTransform ident; - ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); - - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz); - inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz); - inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly); - -} - - - - -void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const -{ - int n = m_children.size(); - - btScalar totalMass = 0; - btVector3 center(0, 0, 0); - int k; - - for (k = 0; k < n; k++) - { - btAssert(masses[k]>0); - center += m_children[k].m_transform.getOrigin() * masses[k]; - totalMass += masses[k]; - } - - 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++) - { - btVector3 i; - m_children[k].m_childShape->calculateLocalInertia(masses[k], i); - - const btTransform& t = m_children[k].m_transform; - btVector3 o = t.getOrigin() - center; - - //compute inertia tensor in coordinate system of compound shape - btMatrix3x3 j = t.getBasis().transpose(); - j[0] *= i[0]; - j[1] *= i[1]; - j[2] *= i[2]; - j = t.getBasis() * j; - - //add inertia tensor - tensor[0] += j[0]; - tensor[1] += j[1]; - tensor[2] += j[2]; - - //compute inertia tensor of pointmass at o - btScalar o2 = o.length2(); - j[0].setValue(o2, 0, 0); - j[1].setValue(0, o2, 0); - j[2].setValue(0, 0, o2); - j[0] += o * -o.x(); - j[1] += o * -o.y(); - j[2] += o * -o.z(); - - //add inertia tensor of pointmass - tensor[0] += masses[k] * j[0]; - tensor[1] += masses[k] * j[1]; - tensor[2] += masses[k] * j[2]; - } - - tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20); - inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]); -} - - - - - -void btCompoundShape::setLocalScaling(const btVector3& scaling) -{ - - 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 * scaling / m_localScaling; - m_children[i].m_childShape->setLocalScaling(childScale); - childTrans.setOrigin((childTrans.getOrigin()) * scaling / m_localScaling); - 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); - - 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); - - const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); - child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index); - } - } -} - - -///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btCompoundShape::serialize(void* dataBuffer, btSerializer* serializer) const -{ - - btCompoundShapeData* shapeData = (btCompoundShapeData*) dataBuffer; - btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); - - shapeData->m_collisionMargin = float(m_collisionMargin); - shapeData->m_numChildShapes = m_children.size(); - shapeData->m_childShapePtr = 0; - if (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++) - { - 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); - } - - 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); - } - return "btCompoundShapeData"; -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.h deleted file mode 100644 index 141034a8e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.h +++ /dev/null @@ -1,212 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COMPOUND_SHAPE_H -#define BT_COMPOUND_SHAPE_H - -#include "btCollisionShape.h" - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include "btCollisionMargin.h" -#include "LinearMath/btAlignedObjectArray.h" - -//class btOptimizedBvh; -struct btDbvt; - -ATTRIBUTE_ALIGNED16(struct) btCompoundShapeChild -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - 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 ); -} - -/// The btCompoundShape allows to store multiple other btCollisionShapes -/// This allows for moving concave collision objects. This is more general then the static concave btBvhTriangleMeshShape. -/// It has an (optional) dynamic aabb tree to accelerate early rejection tests. -/// @todo: This aabb tree can also be use to speed up ray tests on btCompoundShape, see http://code.google.com/p/bullet/issues/detail?id=25 -/// Currently, removal of child shapes is only supported when disabling the aabb tree (pass 'false' in the constructor of btCompoundShape) -ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape -{ - btAlignedObjectArray m_children; - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - - btDbvt* m_dynamicAabbTree; - - ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated - int m_updateRevision; - - btScalar m_collisionMargin; - -protected: - btVector3 m_localScaling; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btCompoundShape(bool enableDynamicAabbTree = true); - - virtual ~btCompoundShape(); - - void addChildShape(const btTransform& localTransform,btCollisionShape* shape); - - /// Remove all children shapes that contain the specified shape - virtual void removeChildShape(btCollisionShape* shape); - - void removeChildShapeByIndex(int childShapeindex); - - - int getNumChildShapes() const - { - return int (m_children.size()); - } - - btCollisionShape* getChildShape(int index) - { - return m_children[index].m_childShape; - } - const btCollisionShape* getChildShape(int index) const - { - return m_children[index].m_childShape; - } - - btTransform& getChildTransform(int index) - { - return m_children[index].m_transform; - } - 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); - - - btCompoundShapeChild* getChildList() - { - return &m_children[0]; - } - - ///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; - - /** 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 setLocalScaling(const btVector3& scaling); - - virtual const btVector3& getLocalScaling() const - { - return m_localScaling; - } - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; - } - virtual btScalar getMargin() const - { - return m_collisionMargin; - } - virtual const char* getName()const - { - return "Compound"; - } - - const btDbvt* getDynamicAabbTree() const - { - return m_dynamicAabbTree; - } - - btDbvt* getDynamicAabbTree() - { - return m_dynamicAabbTree; - } - - void createAabbTreeFromChildren(); - - ///computes the exact moment of inertia and the transform from the coordinate system defined by the principal axes of the moment of inertia - ///and the center of mass to the current coordinate system. "masses" points to an array of masses of the children. The resulting transform - ///"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; - - int getUpdateRevision() const - { - return m_updateRevision; - } - - 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; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCompoundShapeChildData -{ - btTransformFloatData m_transform; - btCollisionShapeData *m_childShape; - int m_childShapeType; - float m_childMargin; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btCompoundShapeData -{ - btCollisionShapeData m_collisionShapeData; - - btCompoundShapeChildData *m_childShapePtr; - - int m_numChildShapes; - - float m_collisionMargin; - -}; - - -SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const -{ - return sizeof(btCompoundShapeData); -} - - - - - - - -#endif //BT_COMPOUND_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.cpp deleted file mode 100644 index 58ff84a5b..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btConcaveShape.h" - -btConcaveShape::btConcaveShape() : m_collisionMargin(btScalar(0.)) -{ - -} - -btConcaveShape::~btConcaveShape() -{ - -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.h deleted file mode 100644 index 2917cc5b6..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONCAVE_SHAPE_H -#define BT_CONCAVE_SHAPE_H - -#include "btCollisionShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include "btTriangleCallback.h" - -/// PHY_ScalarType enumerates possible scalar types. -/// See the btStridingMeshInterface or btHeightfieldTerrainShape for its use -typedef enum PHY_ScalarType { - PHY_FLOAT, - PHY_DOUBLE, - PHY_INTEGER, - PHY_SHORT, - PHY_FIXEDPOINT88, - PHY_UCHAR -} PHY_ScalarType; - -///The btConcaveShape class provides an interface for non-moving (static) concave shapes. -///It has been implemented by the btStaticPlaneShape, btBvhTriangleMeshShape and btHeightfieldTerrainShape. -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 btScalar getMargin() const { - return m_collisionMargin; - } - virtual void setMargin(btScalar collisionMargin) - { - m_collisionMargin = collisionMargin; - } - - - -}; - -#endif //BT_CONCAVE_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.cpp deleted file mode 100644 index 2d83c8bfb..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConeShape.h" - - - -btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (), -m_radius (radius), -m_height(height) -{ - m_shapeType = CONE_SHAPE_PROXYTYPE; - setConeUpIndex(1); - btVector3 halfExtents; - m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); -} - -btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height): -btConeShape(radius,height) -{ - setConeUpIndex(2); -} - -btConeShapeX::btConeShapeX (btScalar radius,btScalar height): -btConeShape(radius,height) -{ - setConeUpIndex(0); -} - -///choose upAxis index -void btConeShape::setConeUpIndex(int upIndex) -{ - switch (upIndex) - { - case 0: - m_coneIndices[0] = 1; - m_coneIndices[1] = 0; - m_coneIndices[2] = 2; - break; - case 1: - m_coneIndices[0] = 0; - m_coneIndices[1] = 1; - m_coneIndices[2] = 2; - break; - case 2: - m_coneIndices[0] = 0; - m_coneIndices[1] = 2; - m_coneIndices[2] = 1; - break; - default: - btAssert(0); - }; - - m_implicitShapeDimensions[m_coneIndices[0]] = m_radius; - m_implicitShapeDimensions[m_coneIndices[1]] = m_height; - m_implicitShapeDimensions[m_coneIndices[2]] = m_radius; -} - -btVector3 btConeShape::coneLocalSupport(const btVector3& v) const -{ - - btScalar halfHeight = m_height * btScalar(0.5); - - if (v[m_coneIndices[1]] > v.length() * m_sinAngle) - { - btVector3 tmp; - - tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = halfHeight; - tmp[m_coneIndices[2]] = btScalar(0.); - return tmp; - } - else { - btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); - if (s > SIMD_EPSILON) { - btScalar d = m_radius / s; - btVector3 tmp; - tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; - tmp[m_coneIndices[1]] = -halfHeight; - tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; - return tmp; - } - else { - btVector3 tmp; - tmp[m_coneIndices[0]] = btScalar(0.); - tmp[m_coneIndices[1]] = -halfHeight; - tmp[m_coneIndices[2]] = btScalar(0.); - return tmp; - } - } - -} - -btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const -{ - return coneLocalSupport(vec); -} - -void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - for (int i=0;im_convexInternalShapeData,serializer); - - shapeData->m_upIndex = m_coneIndices[1]; - - return "btConeShapeData"; -} - -#endif //BT_CONE_MINKOWSKI_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.cpp deleted file mode 100644 index 10ea3e981..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConvex2dShape.h" - -btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape): -btConvexShape (), m_childConvexShape(convexChildShape) -{ - m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE; -} - -btConvex2dShape::~btConvex2dShape() -{ -} - - - -btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); -} - -void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); -} - - -btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const -{ - return m_childConvexShape->localGetSupportingVertex(vec); -} - - -void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - ///this linear upscaling is not realistic, but we don't deal with large mass ratios... - m_childConvexShape->calculateLocalInertia(mass,inertia); -} - - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - m_childConvexShape->getAabb(t,aabbMin,aabbMax); -} - -void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); -} - -void btConvex2dShape::setLocalScaling(const btVector3& scaling) -{ - m_childConvexShape->setLocalScaling(scaling); -} - -const btVector3& btConvex2dShape::getLocalScaling() const -{ - return m_childConvexShape->getLocalScaling(); -} - -void btConvex2dShape::setMargin(btScalar margin) -{ - m_childConvexShape->setMargin(margin); -} -btScalar btConvex2dShape::getMargin() const -{ - return m_childConvexShape->getMargin(); -} - -int btConvex2dShape::getNumPreferredPenetrationDirections() const -{ - return m_childConvexShape->getNumPreferredPenetrationDirections(); -} - -void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const -{ - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.h deleted file mode 100644 index bbd1caf42..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_2D_SHAPE_H -#define BT_CONVEX_2D_SHAPE_H - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#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 -{ - btConvexShape* m_childConvexShape; - - public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvex2dShape( btConvexShape* convexChildShape); - - virtual ~btConvex2dShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - btConvexShape* getChildShape() - { - return m_childConvexShape; - } - - const btConvexShape* getChildShape() const - { - return m_childConvexShape; - } - - 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; - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) 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; - - -}; - -#endif //BT_CONVEX_2D_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.cpp deleted file mode 100644 index 0623e351a..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#if defined (_WIN32) || defined (__i386__) -#define BT_USE_SSE_IN_API -#endif - -#include "btConvexHullShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btSerializer.h" - -btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape () -{ - m_shapeType = CONVEX_HULL_SHAPE_PROXYTYPE; - m_unscaledPoints.resize(numPoints); - - unsigned char* pointsAddress = (unsigned char*)points; - - for (int i=0;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; -#else - 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); - btVector3Data* memPtr = (btVector3Data*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]); - } - - return "btConvexHullShapeData"; -} - -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) - { - maxProj = dp; - witnesPtMax=pt; - } - } -#else - 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) - { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); - } - - -} - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.h deleted file mode 100644 index 3bd598ec4..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.h +++ /dev/null @@ -1,122 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_HULL_SHAPE_H -#define BT_CONVEX_HULL_SHAPE_H - -#include "btPolyhedralConvexShape.h" -#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 -{ - 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)); - - void addPoint(const btVector3& point, bool recalculateLocalAabb = true); - - - btVector3* getUnscaledPoints() - { - return &m_unscaledPoints[0]; - } - - const btVector3* getUnscaledPoints() const - { - return &m_unscaledPoints[0]; - } - - ///getPoints is obsolete, please use getUnscaledPoints - const btVector3* getPoints() const - { - return getUnscaledPoints(); - } - - - - - SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const - { - return m_unscaledPoints[i] * m_localScaling; - } - - 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; - - - //debugging - virtual const char* getName()const {return "Convex";} - - - virtual int getNumVertices() const; - virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; - - ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); - - 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; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConvexHullShapeData -{ - btConvexInternalShapeData m_convexInternalShapeData; - - btVector3FloatData *m_unscaledPointsFloatPtr; - btVector3DoubleData *m_unscaledPointsDoublePtr; - - int m_numUnscaledPoints; - char m_padding3[4]; - -}; - - -SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const -{ - return sizeof(btConvexHullShapeData); -} - - -#endif //BT_CONVEX_HULL_SHAPE_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.cpp deleted file mode 100644 index 083d60b1b..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btConvexInternalShape.h" - - - -btConvexInternalShape::btConvexInternalShape() -: m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), -m_collisionMargin(CONVEX_DISTANCE_MARGIN) -{ -} - - -void btConvexInternalShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling.absolute(); -} - - - -void btConvexInternalShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const -{ -#ifndef __SPU__ - //use localGetSupportingVertexWithoutMargin? - btScalar margin = getMargin(); - for (int i=0;i<3;i++) - { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); - - btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); - - btVector3 tmp = trans(sv); - maxAabb[i] = tmp[i]+margin; - vec[i] = btScalar(-1.); - tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); - minAabb[i] = tmp[i]-margin; - } -#endif -} - - - -btVector3 btConvexInternalShape::localGetSupportingVertex(const btVector3& vec)const -{ -#ifndef __SPU__ - - btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - - if ( getMargin()!=btScalar(0.) ) - { - btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } - vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; - } - return supVertex; - -#else - btAssert(0); - return btVector3(0,0,0); -#endif //__SPU__ - - } - - -btConvexInternalAabbCachingShape::btConvexInternalAabbCachingShape() - : btConvexInternalShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) -{ -} - - -void btConvexInternalAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const -{ - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); -} - -void btConvexInternalAabbCachingShape::setLocalScaling(const btVector3& scaling) -{ - btConvexInternalShape::setLocalScaling(scaling); - recalcLocalAabb(); -} - - -void btConvexInternalAabbCachingShape::recalcLocalAabb() -{ - m_isLocalAabbValid = true; - - #if 1 - static const btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - - btVector3 _supporting[] = - { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - - batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - for ( int i = 0; i < 3; ++i ) - { - m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; - m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; - } - - #else - - for (int i=0;i<3;i++) - { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); - btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; - vec[i] = btScalar(-1.); - tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; - } - #endif -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.h deleted file mode 100644 index 37e04f5fc..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ /dev/null @@ -1,224 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_INTERNAL_SHAPE_H -#define BT_CONVEX_INTERNAL_SHAPE_H - -#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), -///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 -{ - - protected: - - //local scaling. collisionMargin is not scaled ! - btVector3 m_localScaling; - - btVector3 m_implicitShapeDimensions; - - btScalar m_collisionMargin; - - btScalar m_padding; - - btConvexInternalShape(); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - virtual ~btConvexInternalShape() - { - - } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - - const btVector3& getImplicitShapeDimensions() const - { - return m_implicitShapeDimensions; - } - - ///warning: use setImplicitShapeDimensions with care - ///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) - { - m_implicitShapeDimensions = dimensions; - } - - void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) - { - btScalar safeMargin = defaultMarginMultiplier*minDimension; - if (safeMargin < getMargin()) - { - setMargin(safeMargin); - } - } - 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()]; - 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 - { - getAabbSlow(t,aabbMin,aabbMax); - } - - - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const - { - return m_localScaling; - } - - const btVector3& getLocalScalingNV() const - { - return m_localScaling; - } - - virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; - } - virtual btScalar getMargin() const - { - return m_collisionMargin; - } - - btScalar getMarginNV() const - { - return m_collisionMargin; - } - - virtual int getNumPreferredPenetrationDirections() const - { - return 0; - } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const - { - (void)penetrationVector; - (void)index; - btAssert(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; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btConvexInternalShapeData -{ - btCollisionShapeData m_collisionShapeData; - - btVector3FloatData m_localScaling; - - btVector3FloatData m_implicitShapeDimensions; - - float m_collisionMargin; - - int m_padding; - -}; - - - -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 -{ - btConvexInternalShapeData* shapeData = (btConvexInternalShapeData*) dataBuffer; - btCollisionShape::serialize(&shapeData->m_collisionShapeData, serializer); - - m_implicitShapeDimensions.serializeFloat(shapeData->m_implicitShapeDimensions); - m_localScaling.serializeFloat(shapeData->m_localScaling); - shapeData->m_collisionMargin = float(m_collisionMargin); - - 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; - -protected: - - btConvexInternalAabbCachingShape(); - - 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 - { - btAssert(m_isLocalAabbValid); - aabbMin = m_localAabbMin; - aabbMax = m_localAabbMax; - } - - 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); - } - -public: - - virtual void setLocalScaling(const btVector3& scaling); - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - void recalcLocalAabb(); - -}; - -#endif //BT_CONVEX_INTERNAL_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp deleted file mode 100644 index ad1d1bf78..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConvexPointCloudShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -#include "LinearMath/btQuaternion.h" - -void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; - recalcLocalAabb(); -} - -#ifndef __SPU__ -btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const -{ - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - btScalar maxDot = btScalar(-BT_LARGE_FLOAT); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - 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 -{ - 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 supVertex = localGetSupportingVertexWithoutMargin(vec); - - if ( getMargin()!=btScalar(0.) ) - { - btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } - vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; - } - return supVertex; -} - - -#endif - - - - - - -//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection -//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo -int btConvexPointCloudShape::getNumVertices() const -{ - return m_numPoints; -} - -int btConvexPointCloudShape::getNumEdges() const -{ - return 0; -} - -void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const -{ - btAssert (0); -} - -void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const -{ - vtx = m_unscaledPoints[i]*m_localScaling; -} - -int btConvexPointCloudShape::getNumPlanes() const -{ - return 0; -} - -void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const -{ - - btAssert(0); -} - -//not yet -bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const -{ - btAssert(0); - return false; -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.h deleted file mode 100644 index 54b5afac3..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.h +++ /dev/null @@ -1,105 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H -#define BT_CONVEX_POINT_CLOUD_SHAPE_H - -#include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include "LinearMath/btAlignedObjectArray.h" - -///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. -ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexAabbCachingShape -{ - btVector3* m_unscaledPoints; - int m_numPoints; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexPointCloudShape() - { - 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) - { - m_localScaling = localScaling; - m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; - m_unscaledPoints = points; - m_numPoints = numPoints; - - if (computeAabb) - recalcLocalAabb(); - } - - void setPoints (btVector3* points, int numPoints, bool computeAabb = true,const btVector3& localScaling=btVector3(1.f,1.f,1.f)) - { - m_unscaledPoints = points; - m_numPoints = numPoints; - m_localScaling = localScaling; - - if (computeAabb) - recalcLocalAabb(); - } - - SIMD_FORCE_INLINE btVector3* getUnscaledPoints() - { - return m_unscaledPoints; - } - - SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const - { - return m_unscaledPoints; - } - - SIMD_FORCE_INLINE int getNumPoints() const - { - return m_numPoints; - } - - SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const - { - return m_unscaledPoints[index] * m_localScaling; - } - -#ifndef __SPU__ - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; -#endif - - - //debugging - virtual const char* getName()const {return "ConvexPointCloud";} - - virtual int getNumVertices() const; - virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; - - ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); -}; - - -#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp deleted file mode 100644 index f4324c1f4..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ /dev/null @@ -1,302 +0,0 @@ -/* -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. -*/ - - -///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 - -#include "btConvexPolyhedron.h" -#include "LinearMath/btHashMap.h" - -btConvexPolyhedron::btConvexPolyhedron() -{ - -} -btConvexPolyhedron::~btConvexPolyhedron() -{ - -} - - -inline bool IsAlmostZero(const btVector3& v) -{ - if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false; - return true; -} - -struct btInternalVertexPair -{ - btInternalVertexPair(short int v0,short int v1) - :m_v0(v0), - m_v1(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); - } - bool equals(const btInternalVertexPair& other) const - { - return m_v0==other.m_v0 && m_v1==other.m_v1; - } -}; - -struct btInternalEdge -{ - btInternalEdge() - :m_face0(-1), - m_face1(-1) - { - } - short int m_face0; - short int m_face1; -}; - -// - -#ifdef TEST_INTERNAL_OBJECTS -bool btConvexPolyhedron::testContainment() const -{ - 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]); - - for(int i=0;i0.0f) - return false; - } - } - return true; -} -#endif - -void btConvexPolyhedron::initialize() -{ - - btHashMap edges; - - btScalar TotalArea = 0.0f; - - m_localCenter.setValue(0, 0, 0); - for(int i=0;im_face0>=0); - btAssert(edptr->m_face1<0); - edptr->m_face1 = i; - } else - { - btInternalEdge ed; - ed.m_face0 = i; - edges.insert(vp,ed); - } - } - } - -#ifdef USE_CONNECTED_FACES - for(int i=0;im_face0>=0); - btAssert(edptr->m_face1>=0); - - int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; - m_faces[i].m_connectedFaces[j] = connectedFace; - } - } -#endif//USE_CONNECTED_FACES - - for(int i=0;iMaxX) MaxX = pt.x(); - if(pt.y()MaxY) MaxY = pt.y(); - if(pt.z()MaxZ) MaxZ = pt.z(); - } - 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(3.0f); - const int LargestExtent = mE.maxAxis(); - 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; - 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 btScalar Step = (m_radius - r)/1024.0f; - const int e0 = (1< maxProj) - { - maxProj = dp; - witnesPtMax = pt; - } - } - if(minProj>maxProj) - { - btSwap(minProj,maxProj); - btSwap(witnesPtMin,witnesPtMax); - } -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.h deleted file mode 100644 index d3cd066ac..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.h +++ /dev/null @@ -1,65 +0,0 @@ -/* -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. -*/ - - -///This file was written by Erwin Coumans - - -#ifndef _BT_POLYHEDRAL_FEATURES_H -#define _BT_POLYHEDRAL_FEATURES_H - -#include "LinearMath/btTransform.h" -#include "LinearMath/btAlignedObjectArray.h" - -#define TEST_INTERNAL_OBJECTS 1 - - -struct btFace -{ - btAlignedObjectArray m_indices; -// btAlignedObjectArray m_connectedFaces; - btScalar m_plane[4]; -}; - - -ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron -{ - public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - 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; - - void initialize(); - bool testContainment() const; - - void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; -}; - - -#endif //_BT_POLYHEDRAL_FEATURES_H - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.cpp deleted file mode 100644 index f03d0b21e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.cpp +++ /dev/null @@ -1,455 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#if defined (_WIN32) || defined (__i386__) -#define BT_USE_SSE_IN_API -#endif - -#include "btConvexShape.h" -#include "btTriangleShape.h" -#include "btSphereShape.h" -#include "btCylinderShape.h" -#include "btConeShape.h" -#include "btCapsuleShape.h" -#include "btConvexHullShape.h" -#include "btConvexPointCloudShape.h" - -///not supported on IBM SDK, until we fix the alignment of btVector3 -#if defined (__CELLOS_LV2__) && defined (__SPU__) -#include -static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) -{ - vec_float4 result; - result = spu_mul( vec0, vec1 ); - result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); - return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); -} -#endif //__SPU__ - -btConvexShape::btConvexShape () -{ -} - -btConvexShape::~btConvexShape() -{ - -} - - -void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const -{ - btVector3 localAxis = dir*trans.getBasis(); - btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); - btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); - - min = vtx1.dot(dir); - max = vtx2.dot(dir); - - if(min>max) - { - btScalar tmp = min; - min = max; - max = tmp; - } -} - - -static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) -{ - - btVector3 vec = localDirOrg * localScaling; - -#if defined (__CELLOS_LV2__) && defined (__SPU__) - - btVector3 localDir = vec; - - vec_float4 v_distMax = {-FLT_MAX,0,0,0}; - vec_int4 v_idxMax = {-999,0,0,0}; - int v=0; - int numverts = numPoints; - - for(;v<(int)numverts-4;v+=4) { - vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); - vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); - vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); - vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); - const vec_int4 i0 = {v ,0,0,0}; - const vec_int4 i1 = {v+1,0,0,0}; - const vec_int4 i2 = {v+2,0,0,0}; - const vec_int4 i3 = {v+3,0,0,0}; - vec_uint4 retGt01 = spu_cmpgt(p0,p1); - vec_float4 pmax01 = spu_sel(p1,p0,retGt01); - vec_int4 imax01 = spu_sel(i1,i0,retGt01); - vec_uint4 retGt23 = spu_cmpgt(p2,p3); - vec_float4 pmax23 = spu_sel(p3,p2,retGt23); - vec_int4 imax23 = spu_sel(i3,i2,retGt23); - vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); - vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); - vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); - vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); - v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); - v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); - } - for(;v<(int)numverts;v++) { - vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); - const vec_int4 i = {v,0,0,0}; - vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); - v_distMax = spu_sel(p,v_distMax,retGtMax); - v_idxMax = spu_sel(i,v_idxMax,retGtMax); - } - int ptIndex = spu_extract(v_idxMax,0); - const btVector3& supVec= points[ptIndex] * localScaling; - return supVec; -#else - - btScalar maxDot; - long ptIndex = vec.maxDot( points, numPoints, maxDot); - btAssert(ptIndex >= 0); - btVector3 supVec = points[ptIndex] * localScaling; - return supVec; -#endif //__SPU__ -} - -btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const -{ - switch (m_shapeType) - { - case SPHERE_SHAPE_PROXYTYPE: - { - return btVector3(0,0,0); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); - -#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 -#else - 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(); - - btScalar radius = capsuleShape->getRadius(); - btVector3 supVec(0,0,0); - - btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - btVector3 vtx; - btScalar newDot; - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = halfHeight; - - //vtx = pos +vec*(radius); - vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); - newDot = vec.dot(vtx); - - - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = -halfHeight; - - //vtx = pos +vec*(radius); - vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - } - case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE: - { - btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this; - btVector3* points = convexPointCloudShape->getUnscaledPoints (); - int numPoints = convexPointCloudShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV()); - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - btConvexHullShape* convexHullShape = (btConvexHullShape*)this; - btVector3* points = convexHullShape->getUnscaledPoints(); - int numPoints = convexHullShape->getNumPoints (); - return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV()); - } - default: -#ifndef __SPU__ - return this->localGetSupportingVertexWithoutMargin (localDir); -#else - btAssert (0); -#endif - } - - // should never reach here - btAssert (0); - return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f)); -} - -btVector3 btConvexShape::localGetSupportVertexNonVirtual (const btVector3& localDir) const -{ - btVector3 localDirNorm = localDir; - if (localDirNorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - localDirNorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } - localDirNorm.normalize (); - - return localGetSupportVertexWithoutMarginNonVirtual(localDirNorm)+ getMarginNonVirtual() * localDirNorm; -} - -/* TODO: This should be bumped up to btCollisionShape () */ -btScalar btConvexShape::getMarginNonVirtual () const -{ - switch (m_shapeType) - { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - return sphereShape->getRadius (); - } - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - return convexShape->getMarginNV (); - } - case TRIANGLE_SHAPE_PROXYTYPE: - { - btTriangleShape* triangleShape = (btTriangleShape*)this; - return triangleShape->getMarginNV (); - } - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)this; - return cylShape->getMarginNV(); - } - case 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 (); -#else - btAssert (0); -#endif - } - - // should never reach here - btAssert (0); - return btScalar(0.0f); -} -#ifndef __SPU__ -void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const -{ - switch (m_shapeType) - { - case SPHERE_SHAPE_PROXYTYPE: - { - btSphereShape* sphereShape = (btSphereShape*)this; - btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); - btScalar margin = radius + sphereShape->getMarginNonVirtual(); - const btVector3& center = t.getOrigin(); - btVector3 extent(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - } - break; - case CYLINDER_SHAPE_PROXYTYPE: - /* fall through */ - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* convexShape = (btBoxShape*)this; - btScalar margin=convexShape->getMarginNonVirtual(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - halfExtents += btVector3(margin,margin,margin); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = 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++) - { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); - - btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()); - - btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; - vec[i] = btScalar(-1.); - tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis())); - aabbMin[i] = tmp[i]-margin; - } - } - break; - case CAPSULE_SHAPE_PROXYTYPE: - { - btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); - int m_upAxis = capsuleShape->getUpAxis(); - halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); - halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = 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); -#else - btAssert (0); -#endif - break; - } - - // should never reach here - btAssert (0); -} - -#endif //__SPU__ diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.h deleted file mode 100644 index 290cd9fd1..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.h +++ /dev/null @@ -1,84 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_SHAPE_INTERFACE1 -#define BT_CONVEX_SHAPE_INTERFACE1 - -#include "btCollisionShape.h" - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include "btCollisionMargin.h" -#include "LinearMath/btAlignedAllocator.h" - -#define MAX_PREFERRED_PENETRATION_DIRECTIONS 10 - -/// 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 -{ - - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexShape (); - - virtual ~btConvexShape(); - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const = 0; - - //////// - #ifndef __SPU__ - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const=0; - #endif //#ifndef __SPU__ - - btVector3 localGetSupportVertexWithoutMarginNonVirtual (const btVector3& vec) const; - btVector3 localGetSupportVertexNonVirtual (const btVector3& vec) const; - btScalar getMarginNonVirtual () const; - void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; - - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const; - - - //notice that the vectors should be unit length - 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; - - 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 setMargin(btScalar margin)=0; - - virtual btScalar getMargin() const=0; - - virtual int getNumPreferredPenetrationDirections() const=0; - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const=0; - - - - -}; - - - -#endif //BT_CONVEX_SHAPE_INTERFACE1 diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp deleted file mode 100644 index 0f9ced554..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ /dev/null @@ -1,315 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -#include "LinearMath/btQuaternion.h" -#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" - - -btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -: btPolyhedralConvexAabbCachingShape(), m_stridingMesh(meshInterface) -{ - m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; - if ( calcAabb ) - recalcLocalAabb(); -} - - - - -///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once -///but then we are duplicating -class LocalSupportVertexCallback: public btInternalTriangleIndexCallback -{ - - btVector3 m_supportVertexLocal; -public: - - btScalar m_maxDot; - btVector3 m_supportVecLocal; - - LocalSupportVertexCallback(const btVector3& supportVecLocal) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), - m_maxDot(btScalar(-BT_LARGE_FLOAT)), - m_supportVecLocal(supportVecLocal) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - (void)triangleIndex; - (void)partId; - - for (int i=0;i<3;i++) - { - btScalar dot = m_supportVecLocal.dot(triangle[i]); - if (dot > m_maxDot) - { - m_maxDot = dot; - m_supportVertexLocal = triangle[i]; - } - } - } - - btVector3 GetSupportVertexLocal() - { - return m_supportVertexLocal; - } - -}; - - - - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const -{ - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); - supVec = supportCallback.GetSupportVertexLocal(); - - return supVec; -} - -void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - //use 'w' component of supportVerticesOut? - { - for (int i=0;iInternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); - supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); - } - -} - - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const -{ - btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - - if ( getMargin()!=btScalar(0.) ) - { - btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } - vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; - } - return supVertex; -} - - - - - - - - - -//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection -//Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo -int btConvexTriangleMeshShape::getNumVertices() const -{ - //cache this? - return 0; - -} - -int btConvexTriangleMeshShape::getNumEdges() const -{ - return 0; -} - -void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const -{ - btAssert(0); -} - -void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const -{ - btAssert(0); -} - -int btConvexTriangleMeshShape::getNumPlanes() const -{ - return 0; -} - -void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const -{ - btAssert(0); -} - -//not yet -bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const -{ - btAssert(0); - return false; -} - - - -void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) -{ - m_stridingMesh->setScaling(scaling); - - recalcLocalAabb(); - -} - - -const btVector3& btConvexTriangleMeshShape::getLocalScaling() const -{ - return m_stridingMesh->getScaling(); -} - -void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const -{ - class CenterCallback: public btInternalTriangleIndexCallback - { - bool first; - btVector3 ref; - btVector3 sum; - btScalar volume; - - public: - - CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - if (first) - { - ref = triangle[0]; - first = false; - } - else - { - btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); - sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); - volume += vol; - } - } - - btVector3 getCenter() - { - return (volume > 0) ? sum / volume : ref; - } - - btScalar getVolume() - { - return volume * btScalar(1. / 6); - } - - }; - - class InertiaCallback: public btInternalTriangleIndexCallback - { - btMatrix3x3 sum; - btVector3 center; - - public: - - InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - btMatrix3x3 i; - btVector3 a = triangle[0] - center; - btVector3 b = triangle[1] - center; - btVector3 c = triangle[2] - center; - btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); - for (int j = 0; j < 3; j++) - { - for (int k = 0; k <= j; k++) - { - i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) - + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); - } - } - btScalar i00 = -i[0][0]; - btScalar i11 = -i[1][1]; - btScalar i22 = -i[2][2]; - i[0][0] = i11 + i22; - i[1][1] = i22 + i00; - i[2][2] = i00 + i11; - sum[0] += i[0]; - sum[1] += i[1]; - sum[2] += i[2]; - } - - btMatrix3x3& getInertia() - { - return sum; - } - - }; - - CenterCallback centerCallback; - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); - btVector3 center = centerCallback.getCenter(); - principal.setOrigin(center); - volume = centerCallback.getVolume(); - - InertiaCallback inertiaCallback(center); - m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); - - btMatrix3x3& i = inertiaCallback.getInertia(); - i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); - inertia.setValue(i[0][0], i[1][1], i[2][2]); - inertia /= volume; -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h deleted file mode 100644 index f338865ca..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ /dev/null @@ -1,77 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#ifndef BT_CONVEX_TRIANGLEMESH_SHAPE_H -#define BT_CONVEX_TRIANGLEMESH_SHAPE_H - - -#include "btPolyhedralConvexShape.h" -#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 -{ - - class btStridingMeshInterface* m_stridingMesh; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface, bool calcAabb = true); - - class btStridingMeshInterface* getMeshInterface() - { - return m_stridingMesh; - } - const class btStridingMeshInterface* getMeshInterface() const - { - 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 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 - ///and the center of mass to the current coordinate system. A mass of 1 is assumed, for other masses just multiply the computed "inertia" - ///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; - -}; - - - -#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.cpp deleted file mode 100644 index 6cfe43be4..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ /dev/null @@ -1,281 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btCylinderShape.h" - -btCylinderShape::btCylinderShape (const btVector3& halfExtents) -:btConvexInternalShape(), -m_upAxis(1) -{ - setSafeMargin(halfExtents); - - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; - m_shapeType = CYLINDER_SHAPE_PROXYTYPE; -} - - -btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) -:btCylinderShape(halfExtents) -{ - m_upAxis = 0; - -} - - -btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) -:btCylinderShape(halfExtents) -{ - m_upAxis = 2; - -} - -void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax); -} - -void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - -//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 - - /* - cylinder is defined as following: - * - * - principle axis aligned along y by default, radius in x, z-value not used - * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used - * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used - * - */ - - 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 - { - case 0: // cylinder is aligned along x - idxRadius = 1; - idxHeight = 0; - break; - case 2: // cylinder is aligned along z - idxRadius = 0; - idxHeight = 2; - break; - default: // cylinder is aligned along y - idxRadius = 0; - idxHeight = 1; - } - - // calculate squares - radius2 = halfExtents[idxRadius] * halfExtents[idxRadius]; - height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight]; - - // calculate tensor terms - btScalar t1 = div12 * height2 + div4 * radius2; - btScalar t2 = div2 * radius2; - - switch (m_upAxis) // set diagonal elements of inertia tensor - { - 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); - break; - default: // cylinder is aligned along y - inertia.setValue(t1,t2,t1); - } -#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()); - - 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) -{ -const int cylinderUpAxis = 0; -const int XX = 1; -const int YY = 0; -const int ZZ = 2; - - //mapping depends on how cylinder local orientation is - // extents of the cylinder is: X,Y is for radius, and Z for height - - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; - - - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return tmp; - } - else - { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return tmp; - } - - -} - - - - - - -inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) -{ - -const int cylinderUpAxis = 1; -const int XX = 0; -const int YY = 1; -const int ZZ = 2; - - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; - - - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return tmp; - } - else - { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return tmp; - } - -} - -inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) -{ -const int cylinderUpAxis = 2; -const int XX = 0; -const int YY = 2; -const int ZZ = 1; - - //mapping depends on how cylinder local orientation is - // extents of the cylinder is: X,Y is for radius, and Z for height - - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; - - - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return tmp; - } - else - { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return tmp; - } - - -} - -btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec); -} - - -btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec); -} -btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec); -} - -void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - for (int i=0;im_convexInternalShapeData,serializer); - - shapeData->m_upAxis = m_upAxis; - - return "btCylinderShapeData"; -} - - - -#endif //BT_CYLINDER_MINKOWSKI_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.cpp deleted file mode 100644 index a9e6df5c5..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btEmptyShape.h" - - -#include "btCollisionShape.h" - - -btEmptyShape::btEmptyShape() : btConcaveShape () -{ - m_shapeType = EMPTY_SHAPE_PROXYTYPE; -} - - -btEmptyShape::~btEmptyShape() -{ -} - - - ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version -void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - btVector3 margin(getMargin(),getMargin(),getMargin()); - - aabbMin = t.getOrigin() - margin; - - aabbMax = t.getOrigin() + margin; - -} - -void btEmptyShape::calculateLocalInertia(btScalar ,btVector3& ) const -{ - btAssert(0); -} - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.h deleted file mode 100644 index 069a79402..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.h +++ /dev/null @@ -1,72 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_EMPTY_SHAPE_H -#define BT_EMPTY_SHAPE_H - -#include "btConcaveShape.h" - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#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 -{ -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; - - - virtual void setLocalScaling(const btVector3& scaling) - { - m_localScaling = scaling; - } - virtual const btVector3& getLocalScaling() const - { - return m_localScaling; - } - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual const char* getName()const - { - return "Empty"; - } - - virtual void processAllTriangles(btTriangleCallback* ,const btVector3& ,const btVector3& ) const - { - } - -protected: - btVector3 m_localScaling; - -}; - - - -#endif //BT_EMPTY_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp deleted file mode 100644 index 26322791d..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ /dev/null @@ -1,410 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btHeightfieldTerrainShape.h" - -#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 -) -{ - initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - 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 - PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; - btScalar minHeight = 0.0f; - - // previously, height = uchar * maxHeight / 65535. - // So to preserve legacy behavior, heightScale = maxHeight / 65535 - btScalar heightScale = maxHeight / 65535; - - initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); -} - - - -void btHeightfieldTerrainShape::initialize -( -int heightStickWidth, int heightStickLength, 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(heightScale) -- do we care? Trust caller here - btAssert(minHeight <= maxHeight && "bad min/max height"); - btAssert(upAxis >= 0 && upAxis < 3 && - "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT && - "Bad height data type enum"); - - // initialize member variables - m_shapeType = TERRAIN_SHAPE_PROXYTYPE; - m_heightStickWidth = heightStickWidth; - m_heightStickLength = heightStickLength; - m_minHeight = minHeight; - m_maxHeight = maxHeight; - m_width = (btScalar) (heightStickWidth - 1); - m_length = (btScalar) (heightStickLength - 1); - m_heightScale = heightScale; - m_heightfieldDataUnknown = heightfieldData; - m_heightDataType = hdt; - m_flipQuadEdges = flipQuadEdges; - m_useDiamondSubdivision = false; - m_useZigzagSubdivision = false; - m_upAxis = upAxis; - m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); - - // determine min/max axis-aligned bounding box (aabb) values - switch (m_upAxis) - { - case 0: - { - m_localAabbMin.setValue(m_minHeight, 0, 0); - m_localAabbMax.setValue(m_maxHeight, m_width, m_length); - break; - } - case 1: - { - m_localAabbMin.setValue(0, m_minHeight, 0); - m_localAabbMax.setValue(m_width, m_maxHeight, m_length); - break; - }; - case 2: - { - m_localAabbMin.setValue(0, 0, m_minHeight); - m_localAabbMax.setValue(m_width, m_length, m_maxHeight); - break; - } - default: - { - //need to get valid m_upAxis - btAssert(0 && "Bad m_upAxis"); - } - } - - // remember origin (defined as exact middle of aabb) - m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); -} - - - -btHeightfieldTerrainShape::~btHeightfieldTerrainShape() -{ -} - - - -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); - - btVector3 localOrigin(0, 0, 0); - localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); - localOrigin *= m_localScaling; - - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = 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 -{ - btScalar val = 0.f; - switch (m_heightDataType) - { - case PHY_FLOAT: - { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; - break; - } - - case PHY_UCHAR: - { - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; - val = heightFieldValue * m_heightScale; - break; - } - - case PHY_SHORT: - { - short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; - val = hfValue * m_heightScale; - break; - } - - default: - { - btAssert(!"Bad m_heightDataType"); - } - } - - return val; -} - - - - -/// this returns the vertex in bullet-local coordinates -void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const -{ - btAssert(x>=0); - btAssert(y>=0); - btAssert(xstartX) - startX = quantizedAabbMin[1]; - if (quantizedAabbMax[1]startJ) - startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]startX) - startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]startJ) - startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]startX) - startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]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,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); - } else - { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j,vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - getVertex(x+1,j,vertices[0]); - //getVertex(x,j+1,vertices[1]); - getVertex(x+1,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); - } - } - } - - - -} - -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const -{ - //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); -} - -void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; -} -const btVector3& btHeightfieldTerrainShape::getLocalScaling() const -{ - return m_localScaling; -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h deleted file mode 100644 index 4a7a4a4bd..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ /dev/null @@ -1,167 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_HEIGHTFIELD_TERRAIN_SHAPE_H -#define BT_HEIGHTFIELD_TERRAIN_SHAPE_H - -#include "btConcaveShape.h" - -///btHeightfieldTerrainShape simulates a 2D heightfield terrain -/** - The caller is responsible for maintaining the heightfield array; this - class does not make a copy. - - The heightfield can be dynamic so long as the min/max height values - capture the extremes (heights must always be in that range). - - The local origin of the heightfield is assumed to be the exact - center (as determined by width and length and height, with each - axis multiplied by the localScaling). - - \b NOTE: be careful with coordinates. If you have a heightfield with a local - min height of -100m, and a max height of +500m, you may be tempted to place it - at the origin (0,0) and expect the heights in world coordinates to be - -100 to +500 meters. - Actually, the heights will be -300 to +300m, because bullet will re-center - the heightfield based on its AABB (which is determined by the min/max - heights). So keep in mind that once you create a btHeightfieldTerrainShape - object, the heights will be adjusted relative to the center of the AABB. This - is different to the behavior of many rendering engines, but is useful for - physics engines. - - Most (but not all) rendering and heightfield libraries assume upAxis = 1 - (that is, the y-axis is "up"). This class allows any of the 3 coordinates - to be "up". Make sure your choice of axis is consistent with your rendering - system. - - The heightfield heights are determined from the data type used for the - heightfieldData array. - - - PHY_UCHAR: height at a point is the uchar value at the - grid point, multipled by heightScale. uchar isn't recommended - because of its inability to deal with negative values, and - low resolution (8-bit). - - - PHY_SHORT: height at a point is the short int value at that grid - point, multipled by heightScale. - - - PHY_FLOAT: height at a point is the float value at that grid - point. heightScale is ignored when using the float heightfield - data type. - - Whatever the caller specifies as minHeight and maxHeight will be honored. - The class will not inspect the heightfield to discover the actual minimum - or maximum heights. These values are used to determine the heightfield's - axis-aligned bounding box, multiplied by localScaling. - - For usage and testing see the TerrainDemo. - */ -ATTRIBUTE_ALIGNED16(class) btHeightfieldTerrainShape : public btConcaveShape -{ -protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - btVector3 m_localOrigin; - - ///terrain data - int m_heightStickWidth; - int m_heightStickLength; - 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; - }; - - PHY_ScalarType m_heightDataType; - bool m_flipQuadEdges; - bool m_useDiamondSubdivision; - bool m_useZigzagSubdivision; - - 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; - - - - /// protected initialization - /** - Handles the work of constructors so that public constructors can be - backwards-compatible without a lot of copy/paste. - */ - void initialize(int heightStickWidth, int heightStickLength, - 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); - - /// legacy constructor - /** - The legacy constructor assumes the heightfield has a minimum height - of zero. Only unsigned char or floats are supported. For legacy - compatibility reasons, heightScale is calculated as maxHeight / 65535 - (and is only used when useFloatData = false). - */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,const void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); - - virtual ~btHeightfieldTerrainShape(); - - - 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;} - - 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 calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); - - virtual const btVector3& getLocalScaling() const; - - //debugging - virtual const char* getName()const {return "HEIGHTFIELD";} - -}; - -#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMaterial.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMaterial.h deleted file mode 100644 index 866f9b4da..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMaterial.h +++ /dev/null @@ -1,35 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// This file was created by Alex Silverman - -#ifndef BT_MATERIAL_H -#define BT_MATERIAL_H - -// 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: - btScalar m_friction; - btScalar m_restitution; - int pad[2]; - - btMaterial(){} - btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } -}; - -#endif // BT_MATERIAL_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp deleted file mode 100644 index 06707e24e..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btMinkowskiSumShape.h" - - -btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB) -: btConvexInternalShape (), -m_shapeA(shapeA), -m_shapeB(shapeB) -{ - m_shapeType = MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE; - m_transA.setIdentity(); - m_transB.setIdentity(); -} - -btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); - btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis())); - return supVertexA - supVertexB; -} - -void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - ///@todo: could make recursive use of batching. probably this shape is not used frequently. - for (int i=0;igetMargin() + m_shapeB->getMargin(); -} - - -void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - (void)mass; - btAssert(0); - inertia.setValue(0,0,0); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.h deleted file mode 100644 index a3f9a4723..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MINKOWSKI_SUM_SHAPE_H -#define BT_MINKOWSKI_SUM_SHAPE_H - -#include "btConvexInternalShape.h" -#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 -{ - - btTransform m_transA; - btTransform m_transB; - const btConvexShape* m_shapeA; - const btConvexShape* m_shapeB; - -public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB); - - 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; - - void setTransformA(const btTransform& transA) { m_transA = transA;} - void setTransformB(const btTransform& transB) { m_transB = transB;} - - const btTransform& getTransformA()const { return m_transA;} - const btTransform& GetTransformB()const { return m_transB;} - - - virtual btScalar getMargin() const; - - const btConvexShape* getShapeA() const { return m_shapeA;} - const btConvexShape* getShapeB() const { return m_shapeB;} - - virtual const char* getName()const - { - return "MinkowskiSum"; - } -}; - -#endif //BT_MINKOWSKI_SUM_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.cpp deleted file mode 100644 index a7362ea01..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ /dev/null @@ -1,182 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#if defined (_WIN32) || defined (__i386__) -#define BT_USE_SSE_IN_API -#endif - -#include "btMultiSphereShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btSerializer.h" - -btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres) -:btConvexInternalAabbCachingShape () -{ - m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE; - //btScalar startMargin = btScalar(BT_LARGE_FLOAT); - - m_localPositionArray.resize(numSpheres); - m_radiArray.resize(numSpheres); - for (int i=0;i maxDot ) - { - maxDot = newDot; - supVec = temp[i]; - } - } - - return supVec; - -} - - void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - - for (int j=0;j maxDot ) - { - maxDot = newDot; - supportVerticesOut[j] = temp[i]; - } - } - - } -} - - - - - - - - -void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - //as an approximation, take the inertia of the box that bounds the spheres - - btVector3 localAabbMin,localAabbMax; - getCachedLocalAabb(localAabbMin,localAabbMax); - btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5); - - btScalar lx=btScalar(2.)*(halfExtents.x()); - btScalar ly=btScalar(2.)*(halfExtents.y()); - btScalar lz=btScalar(2.)*(halfExtents.z()); - - inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + lz*lz), - mass/(btScalar(12.0)) * (lx*lx + ly*ly)); - -} - - -///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const -{ - 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_localPositionArraySize = numElem; - if (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]); - } - - return "btMultiSphereShapeData"; -} - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.h deleted file mode 100644 index 5d3b40268..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTI_SPHERE_MINKOWSKI_H -#define BT_MULTI_SPHERE_MINKOWSKI_H - -#include "btConvexInternalShape.h" -#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 -{ - - btAlignedObjectArray m_localPositionArray; - btAlignedObjectArray m_radiArray; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres); - - ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)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 - { - return m_localPositionArray[index]; - } - - btScalar getSphereRadius(int index) const - { - return m_radiArray[index]; - } - - - virtual const char* getName()const - { - return "MultiSphere"; - } - - 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 btPositionAndRadius -{ - btVector3FloatData m_pos; - float m_radius; -}; - -struct btMultiSphereShapeData -{ - btConvexInternalShapeData m_convexInternalShapeData; - - btPositionAndRadius *m_localPositionArrayPtr; - int m_localPositionArraySize; - char m_padding[4]; -}; - - - -SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const -{ - return sizeof(btMultiSphereShapeData); -} - - - -#endif //BT_MULTI_SPHERE_MINKOWSKI_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp deleted file mode 100644 index 58799ac96..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// This file was created by Alex Silverman - -#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" -//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" - - -///Obtains the material for a specific triangle -const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) -{ - const unsigned char * materialBase = 0; - int numMaterials; - PHY_ScalarType materialType; - int materialStride; - const unsigned char * triangleMaterialBase = 0; - int numTriangles; - int triangleMaterialStride; - PHY_ScalarType triangleType; - - ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, - &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); - - // return the pointer to the place with the friction for the triangle - // TODO: This depends on whether it's a moving mesh or not - // BUG IN GIMPACT - //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); - int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); - btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); - return (matVal); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h deleted file mode 100644 index 2b92ab7d1..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ /dev/null @@ -1,120 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// This file was created by Alex Silverman - -#ifndef BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H -#define BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H - -#include "btBvhTriangleMeshShape.h" -#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 -{ - btAlignedObjectArray m_materialList; - int ** m_triangleMaterials; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - 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; - - //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)); - } - } - - ///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; - - 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)); - - 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]); - m_materialLookup[i] = NULL; - } - btAlignedFree(m_materialValues); - m_materialLookup = NULL; -*/ - } - //debugging - virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} - - ///Obtains the material for a specific triangle - const btMaterial * getMaterialProperties(int partID, int triIndex); - -} -; - -#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.cpp deleted file mode 100644 index 6f36775f7..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ /dev/null @@ -1,391 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#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 - { - - NodeArray& m_triangleNodes; - - NodeTriangleCallback& operator=(NodeTriangleCallback& other) - { - m_triangleNodes.copyFromArray(other.m_triangleNodes); - return *this; - } - - NodeTriangleCallback(NodeArray& triangleNodes) - :m_triangleNodes(triangleNodes) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - btOptimizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangle[0]); - aabbMax.setMax(triangle[0]); - aabbMin.setMin(triangle[1]); - aabbMax.setMax(triangle[1]); - aabbMin.setMin(triangle[2]); - aabbMax.setMax(triangle[2]); - - //with quantization? - node.m_aabbMinOrg = aabbMin; - node.m_aabbMaxOrg = aabbMax; - - node.m_escapeIndex = -1; - - //for child nodes - node.m_subPart = partId; - node.m_triangleIndex = triangleIndex; - m_triangleNodes.push_back(node); - } - }; - struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback - { - QuantizedNodeArray& m_triangleNodes; - const btQuantizedBvh* m_optimizedTree; // for quantization - - QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) - { - m_triangleNodes.copyFromArray(other.m_triangleNodes); - m_optimizedTree = other.m_optimizedTree; - return *this; - } - - QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree) - :m_triangleNodes(triangleNodes),m_optimizedTree(tree) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - // The partId and triangle index must fit in the same (positive) integer - btAssert(partId < (1<=0); - - btQuantizedBvhNode node; - btVector3 aabbMin,aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangle[0]); - aabbMax.setMax(triangle[0]); - aabbMin.setMin(triangle[1]); - aabbMax.setMax(triangle[1]); - aabbMin.setMin(triangle[2]); - aabbMax.setMax(triangle[2]); - - //PCK: add these checks for zero dimensions of aabb - const btScalar MIN_AABB_DIMENSION = btScalar(0.002); - const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001); - if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION) - { - aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION); - aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION); - } - if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION) - { - aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION); - aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION); - } - if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION) - { - aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION); - aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION); - } - - m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); - - node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; - - m_triangleNodes.push_back(node); - } - }; - - - - int numLeafNodes = 0; - - - if (m_useQuantization) - { - - //initialize quantization values - setQuantizationValues(bvhAabbMin,bvhAabbMax); - - QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this); - - - triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax); - - //now we have an array of leafnodes in m_leafNodes - numLeafNodes = m_quantizedLeafNodes.size(); - - - m_quantizedContiguousNodes.resize(2*numLeafNodes); - - - } else - { - NodeTriangleCallback callback(m_leafNodes); - - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - - triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); - - //now we have an array of leafnodes in m_leafNodes - numLeafNodes = m_leafNodes.size(); - - m_contiguousNodes.resize(2*numLeafNodes); - } - - m_curNodeIndex = 0; - - buildTree(0,numLeafNodes); - - ///if the entire tree is small then subtree size, we need to create a header info for the tree - if(m_useQuantization && !m_SubtreeHeaders.size()) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); - subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); - subtree.m_rootNodeIndex = 0; - subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); - } - - //PCK: update the copy of the size - m_subtreeHeaderCount = m_SubtreeHeaders.size(); - - //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary - m_quantizedLeafNodes.clear(); - m_leafNodes.clear(); -} - - - - -void btOptimizedBvh::refit(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax) -{ - if (m_useQuantization) - { - - setQuantizationValues(aabbMin,aabbMax); - - updateBvhNodes(meshInterface,0,m_curNodeIndex,0); - - ///now update all subtree headers - - int i; - for (i=0;i m_bvhAabbMin.getX()); - btAssert(aabbMin.getY() > m_bvhAabbMin.getY()); - btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ()); - - btAssert(aabbMax.getX() < m_bvhAabbMax.getX()); - btAssert(aabbMax.getY() < m_bvhAabbMax.getY()); - btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ()); - - ///we should update all quantization values, using updateBvhNodes(meshInterface); - ///but we only update chunks that overlap the given aabb - - unsigned short quantizedQueryAabbMin[3]; - unsigned short quantizedQueryAabbMax[3]; - - quantize(&quantizedQueryAabbMin[0],aabbMin,0); - quantize(&quantizedQueryAabbMax[0],aabbMax,1); - - int i; - for (i=0;im_SubtreeHeaders.size();i++) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - - //PCK: unsigned instead of bool - unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); - if (overlap != 0) - { - updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); - - subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); - } - } - -} - -void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index) -{ - (void)index; - - btAssert(m_useQuantization); - - int curNodeSubPart=-1; - - //get access info to trianglemesh data - const unsigned char *vertexbase = 0; - int numverts = 0; - PHY_ScalarType type = PHY_INTEGER; - int stride = 0; - const unsigned char *indexbase = 0; - int indexstride = 0; - int numfaces = 0; - PHY_ScalarType indicestype = PHY_INTEGER; - - btVector3 triangleVerts[3]; - btVector3 aabbMin,aabbMax; - const btVector3& meshScaling = meshInterface->getScaling(); - - int i; - for (i=endNode-1;i>=firstNode;i--) - { - - - btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i]; - if (curNode.isLeafNode()) - { - //recalc aabb from triangle data - int nodeSubPart = curNode.getPartId(); - int nodeTriangleIndex = curNode.getTriangleIndex(); - if (nodeSubPart != curNodeSubPart) - { - if (curNodeSubPart >= 0) - meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart); - - curNodeSubPart = nodeSubPart; - btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); - } - //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, - - unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride); - - - for (int j=2;j>=0;j--) - { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); - } - else - { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); - } - } - - - - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangleVerts[0]); - aabbMax.setMax(triangleVerts[0]); - aabbMin.setMin(triangleVerts[1]); - aabbMax.setMax(triangleVerts[1]); - aabbMin.setMin(triangleVerts[2]); - aabbMax.setMax(triangleVerts[2]); - - quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0); - quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1); - - } else - { - //combine aabb from both children - - btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; - - btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] : - &m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()]; - - - { - for (int i=0;i<3;i++) - { - curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i]; - if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i]) - curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i]; - - curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i]; - if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i]) - curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i]; - } - } - } - - } - - if (curNodeSubPart >= 0) - meshInterface->unLockReadOnlyVertexBase(curNodeSubPart); - - -} - -///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' -btOptimizedBvh* btOptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) -{ - btQuantizedBvh* bvh = btQuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian); - - //we don't add additional data so just do a static upcast - return static_cast(bvh); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.h deleted file mode 100644 index 715961f55..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ /dev/null @@ -1,65 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///Contains contributions from Disney Studio's - -#ifndef BT_OPTIMIZED_BVH_H -#define BT_OPTIMIZED_BVH_H - -#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" - -class btStridingMeshInterface; - - -///The btOptimizedBvh extends the btQuantizedBvh to create AABB tree for triangle meshes, through the btStridingMeshInterface. -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 refit(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); - - /// Data buffer MUST be 16 byte aligned - virtual bool serializeInPlace(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const - { - 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); - - -}; - - -#endif //BT_OPTIMIZED_BVH_H - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp deleted file mode 100644 index 4854f370f..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ /dev/null @@ -1,500 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#if defined (_WIN32) || defined (__i386__) -#define BT_USE_SSE_IN_API -#endif - -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -#include "btConvexPolyhedron.h" -#include "LinearMath/btConvexHullComputer.h" -#include -#include "LinearMath/btGeometryUtil.h" -#include "LinearMath/btGrahamScan2dConvexHull.h" - - -btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), -m_polyhedron(0) -{ - -} - -btPolyhedralConvexShape::~btPolyhedralConvexShape() -{ - if (m_polyhedron) - { - m_polyhedron->~btConvexPolyhedron(); - btAlignedFree(m_polyhedron); - } -} - - -bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) -{ - - if (m_polyhedron) - { - m_polyhedron->~btConvexPolyhedron(); - btAlignedFree(m_polyhedron); - } - - void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); - m_polyhedron = new (mem) btConvexPolyhedron; - - btAlignedObjectArray orgVertices; - - for (int i=0;i 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 - { - - conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); - } - - - - btAlignedObjectArray faceNormals; - int numFaces = conv.faces.size(); - faceNormals.resize(numFaces); - btConvexHullComputer* convexUtil = &conv; - - - 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; - - btVector3 edges[3]; - int numEdges = 0; - //compute face normals - - 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; - newEdge.normalize(); - if (numEdges<2) - edges[numEdges++] = newEdge; - - edge = edge->getNextEdgeOfFace(); - } while (edge!=firstEdge); - - btScalar 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 - { - btAssert(0);//degenerate? - faceNormals[i].setZero(); - } - - for (int v=0;vm_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 - - btScalar faceWeldThreshold= 0.999f; - btAlignedObjectArray todoFaces; - for (int i=0;i coplanarFaceGroup; - 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--) - { - 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) - { - coplanarFaceGroup.push_back(i); - todoFaces.remove(i); - } - } - - - bool did_merge = false; - if (coplanarFaceGroup.size()>1) - { - //do the merge: use Graham Scan 2d convex hull - - btAlignedObjectArray orgpoints; - btVector3 averageFaceNormal(0,0,0); - - for (int i=0;im_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); - - for (int i=0;im_faces.push_back(combinedFace); - } - } - if(!did_merge) - { - for (int i=0;im_faces.push_back(face); - } - - } - - - - } - - m_polyhedron->initialize(); - - return true; -} - -#ifndef MIN - #define MIN(_a, _b) ((_a) < (_b) ? (_a) : (_b)) -#endif - -btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const -{ - - - btVector3 supVec(0,0,0); -#ifndef __SPU__ - int i; - btScalar maxDot(btScalar(-BT_LARGE_FLOAT)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - btVector3 vtx; - btScalar newDot; - - for( 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__ - return supVec; -} - - - -void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ -#ifndef __SPU__ - int i; - - btVector3 vtx; - btScalar newDot; - - for (i=0;i supportVerticesOut[j][3]) - { - supportVerticesOut[j] = temp[i]; - supportVerticesOut[j][3] = newDot; - } - } - } - -#endif //__SPU__ -} - - - -void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ -#ifndef __SPU__ - //not yet, return box inertia - - btScalar margin = getMargin(); - - btTransform ident; - ident.setIdentity(); - btVector3 aabbMin,aabbMax; - getAabb(ident,aabbMin,aabbMax); - btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); - - btScalar lx=btScalar(2.)*(halfExtents.x()+margin); - btScalar ly=btScalar(2.)*(halfExtents.y()+margin); - btScalar lz=btScalar(2.)*(halfExtents.z()+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * btScalar(0.08333333); - - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); -#endif //__SPU__ -} - - - -void btPolyhedralConvexAabbCachingShape::setLocalScaling(const btVector3& scaling) -{ - btConvexInternalShape::setLocalScaling(scaling); - recalcLocalAabb(); -} - -btPolyhedralConvexAabbCachingShape::btPolyhedralConvexAabbCachingShape() -:btPolyhedralConvexShape(), -m_localAabbMin(1,1,1), -m_localAabbMax(-1,-1,-1), -m_isLocalAabbValid(false) -{ -} - -void btPolyhedralConvexAabbCachingShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const -{ - getNonvirtualAabb(trans,aabbMin,aabbMax,getMargin()); -} - -void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() -{ - m_isLocalAabbValid = true; - - #if 1 - static const btVector3 _directions[] = - { - btVector3( 1., 0., 0.), - btVector3( 0., 1., 0.), - btVector3( 0., 0., 1.), - btVector3( -1., 0., 0.), - btVector3( 0., -1., 0.), - btVector3( 0., 0., -1.) - }; - - btVector3 _supporting[] = - { - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.), - btVector3( 0., 0., 0.) - }; - - batchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6); - - for ( int i = 0; i < 3; ++i ) - { - m_localAabbMax[i] = _supporting[i][i] + m_collisionMargin; - m_localAabbMin[i] = _supporting[i + 3][i] - m_collisionMargin; - } - - #else - - for (int i=0;i<3;i++) - { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); - btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]; - vec[i] = btScalar(-1.); - tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]; - } - #endif -} - - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h deleted file mode 100644 index 961d001a9..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ /dev/null @@ -1,116 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_POLYHEDRAL_CONVEX_SHAPE_H -#define BT_POLYHEDRAL_CONVEX_SHAPE_H - -#include "LinearMath/btMatrix3x3.h" -#include "btConvexInternalShape.h" -class btConvexPolyhedron; - - -///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes. -ATTRIBUTE_ALIGNED16(class) btPolyhedralConvexShape : public btConvexInternalShape -{ - - -protected: - - btConvexPolyhedron* m_polyhedron; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - - btPolyhedralConvexShape(); - - virtual ~btPolyhedralConvexShape(); - - ///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); - - 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 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 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; - -protected: - - 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 - { - btAssert(m_isLocalAabbValid); - aabbMin = m_localAabbMin; - aabbMax = m_localAabbMax; - } - -public: - - btPolyhedralConvexAabbCachingShape(); - - 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); - } - - virtual void setLocalScaling(const btVector3& scaling); - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - void recalcLocalAabb(); - -}; - -#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp deleted file mode 100644 index 6a337c786..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp +++ /dev/null @@ -1,121 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btScaledBvhTriangleMeshShape.h" - -btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) -:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) -{ - m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; -} - -btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() -{ -} - - -class btScaledTriangleCallback : public btTriangleCallback -{ - btTriangleCallback* m_originalCallback; - - btVector3 m_localScaling; - -public: - - btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) - :m_originalCallback(originalCallback), - m_localScaling(localScaling) - { - } - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - btVector3 newTriangle[3]; - newTriangle[0] = triangle[0]*m_localScaling; - newTriangle[1] = triangle[1]*m_localScaling; - newTriangle[2] = triangle[2]*m_localScaling; - m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); - } -}; - -void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const -{ - btScaledTriangleCallback scaledCallback(callback,m_localScaling); - - btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); - btVector3 scaledAabbMin,scaledAabbMax; - - ///support negative scaling - scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; - scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; - scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; - 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); -} - - -void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const -{ - btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); - btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); - - btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling; - btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling; - - localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; - localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; - localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; - localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; - localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; - localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; - - btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); - btScalar margin = m_bvhTriMeshShape->getMargin(); - localHalfExtents += btVector3(margin,margin,margin); - btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); - - btMatrix3x3 abs_b = trans.getBasis().absolute(); - - btVector3 center = trans(localCenter); - - btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); - aabbMin = center - extent; - aabbMax = center + extent; - -} - -void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; -} - -const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const -{ - return m_localScaling; -} - -void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - ///don't make this a movable object! -// btAssert(0); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h deleted file mode 100644 index 39049eaf0..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ /dev/null @@ -1,95 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H -#define BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H - -#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 -{ - - - btVector3 m_localScaling; - - btBvhTriangleMeshShape* m_bvhTriMeshShape; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - - 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 const btVector3& getLocalScaling() const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - btBvhTriangleMeshShape* getChildShape() - { - return m_bvhTriMeshShape; - } - - const btBvhTriangleMeshShape* getChildShape() const - { - return m_bvhTriMeshShape; - } - - //debugging - virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} - - 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; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btScaledTriangleMeshShapeData -{ - btTriangleMeshShapeData m_trimeshShapeData; - - btVector3FloatData m_localScaling; -}; - - -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 -{ - 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 diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.cpp deleted file mode 100644 index 3beaf8658..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -//btShapeHull was implemented by John McCutchan. - - -#include "btShapeHull.h" -#include "LinearMath/btConvexHull.h" - -#define NUM_UNITSPHERE_POINTS 42 - -btShapeHull::btShapeHull (const btConvexShape* shape) -{ - m_shape = shape; - m_vertices.clear (); - m_indices.clear(); - m_numIndices = 0; -} - -btShapeHull::~btShapeHull () -{ - m_indices.clear(); - m_vertices.clear (); -} - -bool -btShapeHull::buildHull (btScalar /*margin*/) -{ - int numSampleDirections = NUM_UNITSPHERE_POINTS; - { - int numPDA = m_shape->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - getUnitSpherePoints()[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - - btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - int i; - for (i = 0; i < numSampleDirections; i++) - { - supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]); - } - - HullDesc hd; - hd.mFlags = QF_TRIANGLES; - hd.mVcount = static_cast(numSampleDirections); - -#ifdef BT_USE_DOUBLE_PRECISION - hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof(btVector3); -#else - hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof (btVector3); -#endif - - HullLibrary hl; - HullResult hr; - if (hl.CreateConvexHull (hd, hr) == QE_FAIL) - { - return false; - } - - m_vertices.resize (static_cast(hr.mNumOutputVertices)); - - - for (i = 0; i < static_cast(hr.mNumOutputVertices); i++) - { - m_vertices[i] = hr.m_OutputVertices[i]; - } - m_numIndices = hr.mNumIndices; - m_indices.resize(static_cast(m_numIndices)); - for (i = 0; i < static_cast(m_numIndices); i++) - { - m_indices[i] = hr.m_Indices[i]; - } - - // free temporary hull result that we just copied - hl.ReleaseResult (hr); - - return true; -} - -int -btShapeHull::numTriangles () const -{ - return static_cast(m_numIndices / 3); -} - -int -btShapeHull::numVertices () const -{ - return m_vertices.size (); -} - -int -btShapeHull::numIndices () const -{ - return static_cast(m_numIndices); -} - - -btVector3* btShapeHull::getUnitSpherePoints() -{ - static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = - { - btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), - btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), - btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), - btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), - btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), - btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), - btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), - btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), - btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), - btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), - btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), - btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), - btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), - btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), - btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), - btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), - btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), - btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), - btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), - btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), - btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), - btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), - btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), - btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), - btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), - btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), - btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), - btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), - btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), - btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) - }; - return sUnitSpherePoints; -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.h deleted file mode 100644 index e959f198b..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.h +++ /dev/null @@ -1,61 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///btShapeHull implemented by John McCutchan. - -#ifndef BT_SHAPE_HULL_H -#define BT_SHAPE_HULL_H - -#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 -{ -protected: - - btAlignedObjectArray m_vertices; - btAlignedObjectArray m_indices; - unsigned int m_numIndices; - const btConvexShape* m_shape; - - static btVector3* getUnitSpherePoints(); - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btShapeHull (const btConvexShape* shape); - ~btShapeHull (); - - bool buildHull (btScalar margin); - - int numTriangles () const; - int numVertices () const; - int numIndices () const; - - const btVector3* getVertexPointer() const - { - return &m_vertices[0]; - } - const unsigned int* getIndexPointer() const - { - return &m_indices[0]; - } -}; - -#endif //BT_SHAPE_HULL_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.cpp deleted file mode 100644 index b9a736c0f..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSphereShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -#include "LinearMath/btQuaternion.h" - -btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - (void)vec; - return btVector3(btScalar(0.),btScalar(0.),btScalar(0.)); -} - -void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - (void)vectors; - - for (int i=0;iprocessTriangle(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); - -} - -void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - (void)mass; - - //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); -} - -void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; -} -const btVector3& btStaticPlaneShape::getLocalScaling() const -{ - return m_localScaling; -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.h deleted file mode 100644 index e6e328839..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ /dev/null @@ -1,105 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_STATIC_PLANE_SHAPE_H -#define BT_STATIC_PLANE_SHAPE_H - -#include "btConcaveShape.h" - - -///The btStaticPlaneShape simulates an infinite non-moving (static) collision plane. -ATTRIBUTE_ALIGNED16(class) btStaticPlaneShape : public btConcaveShape -{ -protected: - 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); - - virtual ~btStaticPlaneShape(); - - - 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 calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const; - - const btVector3& getPlaneNormal() const - { - return m_planeNormal; - } - - const btScalar& getPlaneConstant() const - { - return m_planeConstant; - } - - //debugging - virtual const char* getName()const {return "STATICPLANE";} - - 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; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btStaticPlaneShapeData -{ - btCollisionShapeData m_collisionShapeData; - - btVector3FloatData m_localScaling; - btVector3FloatData m_planeNormal; - float m_planeConstant; - char m_pad[4]; -}; - - -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 -{ - btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*) dataBuffer; - btCollisionShape::serialize(&planeData->m_collisionShapeData,serializer); - - m_localScaling.serializeFloat(planeData->m_localScaling); - m_planeNormal.serializeFloat(planeData->m_planeNormal); - planeData->m_planeConstant = float(m_planeConstant); - - return "btStaticPlaneShapeData"; -} - - -#endif //BT_STATIC_PLANE_SHAPE_H - - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp deleted file mode 100644 index b3d449676..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ /dev/null @@ -1,381 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btStridingMeshInterface.h" -#include "LinearMath/btSerializer.h" - -btStridingMeshInterface::~btStridingMeshInterface() -{ - -} - - -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 indexstride; - PHY_ScalarType type; - PHY_ScalarType gfxindextype; - 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;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: - { - double* graphicsbase; - - switch (gfxindextype) - { - case PHY_INTEGER: - { - for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - case PHY_SHORT: - { - for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - case PHY_UCHAR: - { - for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); - } - break; - } - default: - btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); - } - break; - } - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); - } - - unLockReadOnlyVertexBase(part); - } -} - -void btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax) -{ - - struct AabbCalculationCallback : public btInternalTriangleIndexCallback - { - 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)); - } - - virtual void internalProcessTriangleIndex(btVector3* 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(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 -{ - btStridingMeshInterfaceData* trimeshData = (btStridingMeshInterfaceData*) dataBuffer; - - trimeshData->m_numMeshParts = getNumSubParts(); - - //void* uniquePtr = 0; - - trimeshData->m_meshPartsPtr = 0; - - if (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); - - - // 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; - // 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;partm_numTriangles = numtriangles;//indices = 3*numtriangles - memPtr->m_numVertices = numverts; - memPtr->m_indices16 = 0; - memPtr->m_indices32 = 0; - memPtr->m_3indices16 = 0; - memPtr->m_3indices8 = 0; - memPtr->m_vertices3f = 0; - memPtr->m_vertices3d = 0; - - - switch (gfxindextype) - { - case PHY_INTEGER: - { - int numindices = numtriangles*3; - - if (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); - } - break; - } - case PHY_SHORT: - { - if (numtriangles) - { - btChunk* chunk = serializer->allocate(sizeof(btShortIntIndexTripletData),numtriangles); - btShortIntIndexTripletData* tmpIndices = (btShortIntIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices16 = (btShortIntIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindexfinalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); - } - break; - } - case PHY_UCHAR: - { - if (numtriangles) - { - btChunk* chunk = serializer->allocate(sizeof(btCharIndexTripletData),numtriangles); - btCharIndexTripletData* tmpIndices = (btCharIndexTripletData*)chunk->m_oldPtr; - memPtr->m_3indices8 = (btCharIndexTripletData*) serializer->getUniquePointer(tmpIndices); - for (gfxindex=0;gfxindexfinalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); - } - break; - } - default: - { - btAssert(0); - //unknown index type - } - } - - 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: - { - 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); - } - break; - } - - default: - btAssert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); - } - - unLockReadOnlyVertexBase(part); - } - - serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr); - } - - - m_scaling.serializeFloat(trimeshData->m_scaling); - return "btStridingMeshInterfaceData"; -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.h deleted file mode 100644 index 9fbe13976..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ /dev/null @@ -1,164 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_STRIDING_MESHINTERFACE_H -#define BT_STRIDING_MESHINTERFACE_H - -#include "LinearMath/btVector3.h" -#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 -{ - protected: - - btVector3 m_scaling; - - public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btStridingMeshInterface() :m_scaling(btScalar(1.),btScalar(1.),btScalar(1.)) - { - - } - - 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); - - /// 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 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; - - -}; - -struct btIntIndexData -{ - int m_value; -}; - -struct btShortIntIndexData -{ - short m_value; - char m_pad[2]; -}; - -struct btShortIntIndexTripletData -{ - short m_values[3]; - char m_pad[2]; -}; - -struct btCharIndexTripletData -{ - unsigned char m_values[3]; - char m_pad; -}; - - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMeshPartData -{ - btVector3FloatData *m_vertices3f; - btVector3DoubleData *m_vertices3d; - - btIntIndexData *m_indices32; - btShortIntIndexTripletData *m_3indices16; - btCharIndexTripletData *m_3indices8; - - btShortIntIndexData *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 btStridingMeshInterfaceData -{ - btMeshPartData *m_meshPartsPtr; - btVector3FloatData m_scaling; - int m_numMeshParts; - char m_padding[4]; -}; - - - - -SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() const -{ - return sizeof(btStridingMeshInterfaceData); -} - - - -#endif //BT_STRIDING_MESHINTERFACE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.cpp deleted file mode 100644 index 52f346bf7..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.cpp +++ /dev/null @@ -1,218 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btTetrahedronShape.h" -#include "LinearMath/btMatrix3x3.h" - -btBU_Simplex1to4::btBU_Simplex1to4() : btPolyhedralConvexAabbCachingShape (), -m_numVertices(0) -{ - m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; -} - -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) -{ - 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) -{ - m_shapeType = TETRAHEDRAL_SHAPE_PROXYTYPE; - addVertex(pt0); - addVertex(pt1); - addVertex(pt2); -} - -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); - addVertex(pt1); - addVertex(pt2); - addVertex(pt3); -} - - -void btBU_Simplex1to4::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ -#if 1 - 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); - - //just transform the vertices in worldspace, and take their AABB - for (int i=0;iprocessAllTriangles(&triBuf,aabbMin, aabbMax); -/// for (int i=0;i m_triangleBuffer; - -public: - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - int getNumTriangles() const - { - return int(m_triangleBuffer.size()); - } - - const btTriangle& getTriangle(int index) const - { - return m_triangleBuffer[index]; - } - - void clearBuffer() - { - m_triangleBuffer.clear(); - } - -}; - - -#endif //BT_TRIANGLE_BUFFER_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.cpp deleted file mode 100644 index f558bf6d2..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.cpp +++ /dev/null @@ -1,28 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btTriangleCallback.h" - -btTriangleCallback::~btTriangleCallback() -{ - -} - - -btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback() -{ - -} - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h deleted file mode 100644 index 461c57f87..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h +++ /dev/null @@ -1,42 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_TRIANGLE_CALLBACK_H -#define BT_TRIANGLE_CALLBACK_H - -#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; -}; - -class btInternalTriangleIndexCallback -{ -public: - - virtual ~btInternalTriangleIndexCallback(); - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) = 0; -}; - - - -#endif //BT_TRIANGLE_CALLBACK_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp deleted file mode 100644 index a665024cb..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btTriangleIndexVertexArray.h" - -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_triangleIndexStride = triangleIndexStride; - mesh.m_numVertices = numVertices; - 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) -{ - btAssert(subpart< getNumSubParts() ); - - btIndexedMesh& 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 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; - - 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 btTriangleIndexVertexArray::hasPremadeAabb() const -{ - return (m_hasAabb == 1); -} - - -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 -} - -void btTriangleIndexVertexArray::getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const -{ - *aabbMin = m_aabbMin; - *aabbMax = m_aabbMax; -} - - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h deleted file mode 100644 index 9e1544e87..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ /dev/null @@ -1,133 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_TRIANGLE_INDEX_VERTEX_ARRAY_H -#define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H - -#include "btStridingMeshInterface.h" -#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 -{ - 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; - - // 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; - - - 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 - { - } -} -; - - -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. -///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. -ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface -{ -protected: - IndexedMeshArray m_indexedMeshes; - int m_pad[2]; - 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) - { - } - - 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) - { - 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 btVector3& aabbMin, const btVector3& aabbMax ) const; - virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const; - -} -; - -#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp deleted file mode 100644 index dc562941a..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was created by Alex Silverman - -#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) -{ - btMaterialProperties mat; - - mat.m_numMaterials = numMaterials; - mat.m_materialBase = materialBase; - mat.m_materialStride = materialStride; -#ifdef BT_USE_DOUBLE_PRECISION - mat.m_materialType = PHY_DOUBLE; -#else - 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; - - 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) -{ - btAssert(subpart< getNumSubParts() ); - - btMaterialProperties& mats = m_materials[subpart]; - - numMaterials = mats.m_numMaterials; - (*materialBase) = (unsigned char *) mats.m_materialBase; -#ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; -#else - materialType = PHY_FLOAT; -#endif - materialStride = mats.m_materialStride; - - 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) -{ - btMaterialProperties& mats = m_materials[subpart]; - - numMaterials = mats.m_numMaterials; - (*materialBase) = (const unsigned char *) mats.m_materialBase; -#ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; -#else - materialType = PHY_FLOAT; -#endif - materialStride = mats.m_materialStride; - - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h deleted file mode 100644 index ba4f7b460..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h +++ /dev/null @@ -1,84 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was created by Alex Silverman - -#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H -#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H - -#include "btTriangleIndexVertexArray.h" - - -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; -}; - -typedef btAlignedObjectArray MaterialArray; - -///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray -///The addition of a material array allows for the utilization of the partID and -///triangleIndex that are returned in the ContactAddedCallback. As with -///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 -{ -protected: - MaterialArray m_materials; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - 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); - - 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; - } - - 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); - -} -; - -#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleInfoMap.h deleted file mode 100644 index 17deef89d..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleInfoMap.h +++ /dev/null @@ -1,241 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2010 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_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 -#define TRI_INFO_V2V0_CONVEX 4 - -#define TRI_INFO_V0V1_SWAP_NORMALB 8 -#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 -{ - btTriangleInfo() - { - m_edgeV0V1Angle = SIMD_2_PI; - m_edgeV1V2Angle = SIMD_2_PI; - m_edgeV2V0Angle = SIMD_2_PI; - m_flags=0; - } - - int m_flags; - - btScalar m_edgeV0V1Angle; - btScalar m_edgeV1V2Angle; - btScalar m_edgeV2V0Angle; - -}; - -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 -{ - 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_edgeDistanceThreshold = btScalar(0.1); - m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001); - m_maxEdgeAngleThreshold = SIMD_2_PI; - } - virtual ~btTriangleInfoMap() {} - - 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); - -}; - -///those fields have to be float and not btScalar for the serialization to work properly -struct btTriangleInfoData -{ - int m_flags; - float m_edgeV0V1Angle; - float m_edgeV1V2Angle; - float m_edgeV2V0Angle; -}; - -struct btTriangleInfoMapData -{ - int *m_hashTablePtr; - int *m_nextPtr; - btTriangleInfoData *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]; -}; - -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 -{ - btTriangleInfoMapData* tmapData = (btTriangleInfoMapData*) dataBuffer; - tmapData->m_convexEpsilon = (float)m_convexEpsilon; - tmapData->m_planarEpsilon = (float)m_planarEpsilon; - 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); - int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(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; - if (tmapData->m_nextPtr) - { - int sz = sizeof(int); - int numElem = tmapData->m_nextSize; - 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]); - } - - tmapData->m_numValues = m_valueArray.size(); - 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); - 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]); - } - - 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); - int* memPtr = (int*)chunk->m_oldPtr; - for (int i=0;ifinalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]); - - } - return "btTriangleInfoMapData"; -} - - - -///fills the dataBuffer and returns the struct name (and 0 on failure) -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_32bitIndices; - btAlignedObjectArray m_16bitIndices; - bool m_use32bitIndices; - bool m_use4componentVertices; - - - public: - btScalar m_weldingThreshold; - - btTriangleMesh (bool use32bitIndices=true,bool use4componentVertices=true); - - 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); - - int getNumTriangles() const; - - 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 - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp deleted file mode 100644 index 0e1795140..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btTriangleMeshShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btQuaternion.h" -#include "btStridingMeshInterface.h" -#include "LinearMath/btAabbUtil2.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - - -btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) -: btConcaveShape (), m_meshInterface(meshInterface) -{ - m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; - if(meshInterface->hasPremadeAabb()) - { - meshInterface->getPremadeAabb(&m_localAabbMin, &m_localAabbMax); - } - else - { - recalcLocalAabb(); - } -} - - -btTriangleMeshShape::~btTriangleMeshShape() -{ - -} - - - - -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); - - 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; -} - -void btTriangleMeshShape::recalcLocalAabb() -{ - for (int i=0;i<3;i++) - { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); - btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; - vec[i] = btScalar(-1.); - tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; - } -} - - - -class SupportVertexCallback : public btTriangleCallback -{ - - btVector3 m_supportVertexLocal; -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)) - - { - m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); - } - - virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - for (int i=0;i<3;i++) - { - btScalar dot = m_supportVecLocal.dot(triangle[i]); - if (dot > m_maxDot) - { - m_maxDot = dot; - m_supportVertexLocal = triangle[i]; - } - } - } - - btVector3 GetSupportVertexWorldSpace() - { - return m_worldTrans(m_supportVertexLocal); - } - - btVector3 GetSupportVertexLocal() - { - return m_supportVertexLocal; - } - -}; - - -void btTriangleMeshShape::setLocalScaling(const btVector3& scaling) -{ - m_meshInterface->setScaling(scaling); - recalcLocalAabb(); -} - -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 -{ - 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) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax)) - { - //check aabb in triangle-space, before doing this - m_callback->processTriangle(triangle,partId,triangleIndex); - } - - } - - }; - - FilteredCallback filterCallback(callback,aabbMin,aabbMax); - - m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax); -} - - - - - -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.)); -} - - -btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const -{ - btVector3 supportVertex; - - btTransform ident; - ident.setIdentity(); - - SupportVertexCallback supportCallback(vec,ident); - - 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/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.h deleted file mode 100644 index 453e58005..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ /dev/null @@ -1,90 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_TRIANGLE_MESH_SHAPE_H -#define BT_TRIANGLE_MESH_SHAPE_H - -#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 -{ -protected: - 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); - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - virtual ~btTriangleMeshShape(); - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const - { - btAssert(0); - return localGetSupportingVertex(vec); - } - - void recalcLocalAabb(); - - 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 calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const; - - btStridingMeshInterface* getMeshInterface() - { - return m_meshInterface; - } - - const btStridingMeshInterface* getMeshInterface() const - { - return m_meshInterface; - } - - const btVector3& getLocalAabbMin() const - { - return m_localAabbMin; - } - const btVector3& getLocalAabbMax() const - { - return m_localAabbMax; - } - - - - //debugging - virtual const char* getName()const {return "TRIANGLEMESH";} - - - -}; - - - - -#endif //BT_TRIANGLE_MESH_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleShape.h deleted file mode 100644 index a8a80f82f..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleShape.h +++ /dev/null @@ -1,184 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_OBB_TRIANGLE_MINKOWSKI_H -#define BT_OBB_TRIANGLE_MINKOWSKI_H - -#include "btConvexShape.h" -#include "btBoxShape.h" - -ATTRIBUTE_ALIGNED16(class) btTriangleShape : public btPolyhedralConvexShape -{ - - -public: - -BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_vertices1[3]; - - virtual int getNumVertices() const - { - return 3; - } - - btVector3& getVertexPtr(int index) - { - return m_vertices1[index]; - } - - const btVector3& getVertexPtr(int index) const - { - return m_vertices1[index]; - } - virtual void getVertex(int index,btVector3& vert) const - { - vert = m_vertices1[index]; - } - - virtual int getNumEdges() const - { - return 3; - } - - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const - { - getVertex(i,pa); - getVertex((i+1)%3,pb); - } - - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const - { -// btAssert(0); - getAabbSlow(t,aabbMin,aabbMax); - } - - btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const - { - 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 - { - for (int i=0;i= -tolerance && dist <= tolerance) - { - //inside check on edge-planes - int i; - for (i=0;i<3;i++) - { - btVector3 pa,pb; - getEdge(i,pa,pb); - btVector3 edge = pb-pa; - btVector3 edgeNormal = edge.cross(normal); - edgeNormal.normalize(); - btScalar dist = pt.dot( edgeNormal); - btScalar edgeConst = pa.dot(edgeNormal); - dist -= edgeConst; - if (dist < -tolerance) - return false; - } - - return true; - } - - return false; - } - //debugging - virtual const char* getName()const - { - return "Triangle"; - } - - virtual int getNumPreferredPenetrationDirections() const - { - return 2; - } - - virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const - { - calcNormal(penetrationVector); - if (index) - penetrationVector *= btScalar(-1.); - } - - -}; - -#endif //BT_OBB_TRIANGLE_MINKOWSKI_H - diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.cpp deleted file mode 100644 index b148bbd99..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btUniformScalingShape.h" - -btUniformScalingShape::btUniformScalingShape( btConvexShape* convexChildShape,btScalar uniformScalingFactor): -btConvexShape (), m_childConvexShape(convexChildShape), -m_uniformScalingFactor(uniformScalingFactor) -{ - m_shapeType = UNIFORM_SCALING_SHAPE_PROXYTYPE; -} - -btUniformScalingShape::~btUniformScalingShape() -{ -} - - -btVector3 btUniformScalingShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const -{ - btVector3 tmpVertex; - tmpVertex = m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); - return tmpVertex*m_uniformScalingFactor; -} - -void btUniformScalingShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); - int i; - for (i=0;ilocalGetSupportingVertex(vec); - return tmpVertex*m_uniformScalingFactor; -} - - -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); - 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 -{ - getAabbSlow(trans,aabbMin,aabbMax); - -} - -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 _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.) - }; - - for (int i=0;i<6;i++) - { - _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 ) - { - aabbMax1[i] = t(_supporting[i])[i]; - aabbMin1[i] = t(_supporting[i + 3])[i]; - } - btVector3 marginVec(getMargin(),getMargin(),getMargin()); - aabbMin = aabbMin1-marginVec; - aabbMax = aabbMax1+marginVec; - -#else - - btScalar margin = getMargin(); - for (int i=0;i<3;i++) - { - btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); - vec[i] = btScalar(1.); - btVector3 sv = localGetSupportingVertex(vec*t.getBasis()); - btVector3 tmp = t(sv); - aabbMax[i] = tmp[i]+margin; - vec[i] = btScalar(-1.); - sv = localGetSupportingVertex(vec*t.getBasis()); - tmp = t(sv); - aabbMin[i] = tmp[i]-margin; - } - -#endif -} - -void btUniformScalingShape::setLocalScaling(const btVector3& scaling) -{ - m_childConvexShape->setLocalScaling(scaling); -} - -const btVector3& btUniformScalingShape::getLocalScaling() const -{ - return m_childConvexShape->getLocalScaling(); -} - -void btUniformScalingShape::setMargin(btScalar margin) -{ - m_childConvexShape->setMargin(margin); -} -btScalar btUniformScalingShape::getMargin() const -{ - return m_childConvexShape->getMargin() * m_uniformScalingFactor; -} - -int btUniformScalingShape::getNumPreferredPenetrationDirections() const -{ - return m_childConvexShape->getNumPreferredPenetrationDirections(); -} - -void btUniformScalingShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const -{ - m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); -} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.h deleted file mode 100644 index a10f58d24..000000000 --- a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.h +++ /dev/null @@ -1,89 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_UNIFORM_SCALING_SHAPE_H -#define BT_UNIFORM_SCALING_SHAPE_H - -#include "btConvexShape.h" -#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 -{ - btConvexShape* m_childConvexShape; - - btScalar m_uniformScalingFactor; - - public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btUniformScalingShape( btConvexShape* convexChildShape, btScalar uniformScalingFactor); - - virtual ~btUniformScalingShape(); - - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - btScalar getUniformScalingFactor() const - { - return m_uniformScalingFactor; - } - - btConvexShape* getChildShape() - { - return m_childConvexShape; - } - - const btConvexShape* getChildShape() const - { - return m_childConvexShape; - } - - 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; - - virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) 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; - - -}; - -#endif //BT_UNIFORM_SCALING_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/Doxyfile b/WickedEngine/BULLET/BulletCollision/Doxyfile deleted file mode 100644 index 4ecb6acb6..000000000 --- a/WickedEngine/BULLET/BulletCollision/Doxyfile +++ /dev/null @@ -1,746 +0,0 @@ -# Doxyfile 1.2.4 - -# This file describes the settings to be used by doxygen 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 (" ") - -#--------------------------------------------------------------------------- -# General configuration options -#--------------------------------------------------------------------------- - -# The PROJECT_NAME tag is a single word (or a sequence of words surrounded -# by quotes) that should identify the project. -PROJECT_NAME = "Bullet Continuous Collision Detection Library" - -# 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 = - -# 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: -# Dutch, French, Italian, Czech, Swedish, German, Finnish, Japanese, -# Korean, Hungarian, Norwegian, Spanish, Romanian, Russian, Croatian, -# Polish, Portuguese and Slovene. - -OUTPUT_LANGUAGE = English - -# 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 = YES - -# If the EXTRACT_PRIVATE tag is set to YES all private members of a class -# will be included in the documentation. - -EXTRACT_PRIVATE = YES - -# If the EXTRACT_STATIC tag is set to YES all static members of a file -# will be included in the documentation. - -EXTRACT_STATIC = YES - -# 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 class will be included in the various -# overviews. This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_CLASSES = NO - -# 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 - -# 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 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 = NO - -# 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. It is allowed to use relative paths in the argument list. - -STRIP_FROM_PATH = - -# 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 CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will -# generate a class diagram (in Html and LaTeX) for classes with base or -# super classes. Setting the tag to NO turns the diagrams off. - -CLASS_DIAGRAMS = YES - -# 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. - -SOURCE_BROWSER = YES - -# 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 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 -# users are adviced 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 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 - -# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen -# will put list of the files that are included by a file in the documentation -# of that file. - -SHOW_INCLUDE_FILES = YES - -# 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 the Qt-style comments (thus requiring an -# explict @brief command for a brief description. - -JAVADOC_AUTOBRIEF = YES - -# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented -# member inherits the documentation from any documented member that it -# reimplements. - -INHERIT_DOCS = YES - -# 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 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 - -# 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 - -# The ENABLE_SECTIONS tag can be used to enable conditional -# documentation sections, marked by \if sectionname ... \endif. - -ENABLED_SECTIONS = - -# 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 - -# 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 = - -#--------------------------------------------------------------------------- -# 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 - -# 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. - -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 = . - - -# 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 all files are included. - -FILE_PATTERNS = *.h *.cpp *.c - -# 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 = YES - -# 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 = - -# 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. - -EXCLUDE_PATTERNS = - -# 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 = - -# 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. - -INPUT_FILTER = - -# 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. - -FILTER_SOURCE_FILES = NO - -#--------------------------------------------------------------------------- -# 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 = NO - -# 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_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 - -HTML_STYLESHEET = - -# 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 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 compressed HTML help file (.chm) -# of the generated HTML documentation. - -GENERATE_HTMLHELP = NO - -# 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 - -# If the GENERATE_TREEVIEW tag is set to YES, a side pannel 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 and frames is required (for instance Netscape 4.0+ -# or Internet explorer 4.0+). - -GENERATE_TREEVIEW = 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 - -#--------------------------------------------------------------------------- -# 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 - -# 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 = NO - -# 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 = NO - -# 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 - -#--------------------------------------------------------------------------- -# 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 optimised 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 a 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 assigments. You only have to provide -# replacements, missing definitions are set to their default value. - -RTF_STYLESHEET_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 - -#--------------------------------------------------------------------------- -# 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. Warning: This feature -# is still experimental and very incomplete. - -GENERATE_XML = NO - -#--------------------------------------------------------------------------- -# 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_PREDEFINED 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 = ../../generic/extern - -# 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. - -PREDEFINED = - -# If the MACRO_EXPANSION and EXPAND_PREDEF_ONLY 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 = - -#--------------------------------------------------------------------------- -# Configuration::addtions related to external references -#--------------------------------------------------------------------------- - -# The TAGFILES tag can be used to specify one or more tagfiles. - -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 - -# 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 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 = YES - -# 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 ENABLE_PREPROCESSING, 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, 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 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 - -# 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 on the path. - -DOT_PATH = - -# The MAX_DOT_GRAPH_WIDTH tag can be used to set the maximum allowed width -# (in pixels) of the graphs generated by dot. If a graph becomes larger than -# this value, doxygen will try to truncate the graph, so that it fits within -# the specified constraint. Beware that most browsers cannot cope with very -# large images. - -MAX_DOT_GRAPH_WIDTH = 1024 - -# The MAX_DOT_GRAPH_HEIGHT tag can be used to set the maximum allows height -# (in pixels) of the graphs generated by dot. If a graph becomes larger than -# this value, doxygen will try to truncate the graph, so that it fits within -# the specified constraint. Beware that most browsers cannot cope with very -# large images. - -MAX_DOT_GRAPH_HEIGHT = 1024 - -# 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 - -#--------------------------------------------------------------------------- -# Configuration::addtions related to the search engine -#--------------------------------------------------------------------------- - -# The SEARCHENGINE tag specifies whether or not a search engine should be -# used. If set to NO the values of all tags below this one will be ignored. - -SEARCHENGINE = NO - -# The CGI_NAME tag should be the name of the CGI script that -# starts the search engine (doxysearch) with the correct parameters. -# A script with this name will be generated by doxygen. - -CGI_NAME = search.cgi - -# The CGI_URL tag should be the absolute URL to the directory where the -# cgi binaries are located. See the documentation of your http daemon for -# details. - -CGI_URL = - -# The DOC_URL tag should be the absolute URL to the directory where the -# documentation is located. If left blank the absolute path to the -# documentation, with file:// prepended to it, will be used. - -DOC_URL = - -# The DOC_ABSPATH tag should be the absolute path to the directory where the -# documentation is located. If left blank the directory on the local machine -# will be used. - -DOC_ABSPATH = - -# The BIN_ABSPATH tag must point to the directory where the doxysearch binary -# is installed. - -BIN_ABSPATH = c:\program files\doxygen\bin - -# The EXT_DOC_PATHS tag can be used to specify one or more paths to -# documentation generated for other projects. This allows doxysearch to search -# the documentation for these projects as well. - -EXT_DOC_PATHS = diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btBoxCollision.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btBoxCollision.h deleted file mode 100644 index 0a0357e5a..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btBoxCollision.h +++ /dev/null @@ -1,645 +0,0 @@ -#ifndef BT_BOX_COLLISION_H_INCLUDED -#define BT_BOX_COLLISION_H_INCLUDED - -/*! \file gim_box_collision.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "LinearMath/btTransform.h" - - -///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)) - - - - - - -enum eBT_PLANE_INTERSECTION_TYPE -{ - BT_CONST_BACK_PLANE = 0, - BT_CONST_COLLIDE_PLANE, - BT_CONST_FRONT_PLANE -}; - -//SIMD_FORCE_INLINE bool test_cross_edge_box( -// const btVector3 & edge, -// const btVector3 & absolute_edge, -// const btVector3 & pointa, -// const btVector3 & pointb, const btVector3 & extend, -// int dir_index0, -// int dir_index1 -// int component_index0, -// int component_index1) -//{ -// // dir coords are -z and y -// -// const btScalar dir0 = -edge[dir_index0]; -// const btScalar dir1 = edge[dir_index1]; -// btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1; -// btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1; -// //find minmax -// if(pmin>pmax) -// { -// BT_SWAP_NUMBERS(pmin,pmax); -// } -// //find extends -// const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] + -// extend[component_index1] * absolute_edge[dir_index1]; -// -// if(pmin>rad || -rad>pmax) return false; -// return true; -//} -// -//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis( -// const btVector3 & edge, -// const btVector3 & absolute_edge, -// const btVector3 & pointa, -// const btVector3 & pointb, btVector3 & extend) -//{ -// -// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2); -//} -// -// -//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis( -// const btVector3 & edge, -// const btVector3 & absolute_edge, -// const btVector3 & pointa, -// const btVector3 & pointb, btVector3 & extend) -//{ -// -// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0); -//} -// -//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis( -// const btVector3 & edge, -// const btVector3 & absolute_edge, -// const btVector3 & pointa, -// const btVector3 & pointb, btVector3 & extend) -//{ -// -// 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_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);\ -}\ - - -//! 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) -{ - 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 -{ -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 - - 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(); - - 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) - { - - 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) - { - m_R1to0 = trans0.getBasis().inverse(); - m_T1to0 = m_R1to0 * (-trans0.getOrigin()); - - m_T1to0 += m_R1to0*trans1.getOrigin(); - m_R1to0 *= trans1.getBasis(); - - calc_absolute_matrix(); - } - - SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const - { - 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 -{ -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) - { - 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; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - m_max[2] += margin; - } - - 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) - { - m_min[0] -= margin; - m_min[1] -= margin; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - m_max[2] += margin; - } - - SIMD_FORCE_INLINE void invalidate() - { - m_min[0] = SIMD_INFINITY; - m_min[1] = SIMD_INFINITY; - m_min[2] = SIMD_INFINITY; - m_max[0] = -SIMD_INFINITY; - m_max[1] = -SIMD_INFINITY; - m_max[2] = -SIMD_INFINITY; - } - - SIMD_FORCE_INLINE void increment_margin(btScalar margin) - { - m_min[0] -= margin; - m_min[1] -= margin; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - m_max[2] += margin; - } - - SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin) - { - m_min[0] = other.m_min[0] - margin; - m_min[1] = other.m_min[1] - margin; - m_min[2] = other.m_min[2] - margin; - - m_max[0] = other.m_max[0] + margin; - m_max[1] = other.m_max[1] + margin; - m_max[2] = other.m_max[2] + margin; - } - - template - SIMD_FORCE_INLINE void calc_from_triangle( - 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_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 - SIMD_FORCE_INLINE void calc_from_triangle_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_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; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - m_max[2] += margin; - } - - //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) - { - 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()); - - 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) - { - 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()); - - m_min = center - textends; - m_max = center + textends; - } - - //! Merges a 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_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) - { - 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]); - } - - //! Gets the extend and center - SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const - { - 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 - { - 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]); - } - - - 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]) - { - return false; - } - return true; - } - - /*! \brief Finds the Ray intersection parameter. - \param aabb Aligned box - \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 - { - 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; - btScalar Dy = vorigin[1] - center[1]; - 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; - - - btScalar f = vdir[1] * Dz - vdir[2] * Dy; - 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; - f = vdir[0] * Dy - vdir[1] * Dx; - 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 - { - btVector3 center = (m_max+m_min)*0.5f; - btVector3 extend = m_max-center; - - btScalar _fOrigin = direction.dot(center); - btScalar _fMaximumExtent = extend.dot(direction.absolute()); - vmin = _fOrigin - _fMaximumExtent; - vmax = _fOrigin + _fMaximumExtent; - } - - SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const - { - btScalar _fmin,_fmax; - this->projection_interval(plane,_fmin,_fmax); - - if(plane[3] > _fmax + BOX_PLANE_EPSILON) - { - return BT_CONST_BACK_PLANE; // 0 - } - - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) - { - return BT_CONST_COLLIDE_PLANE; //1 - } - return BT_CONST_FRONT_PLANE;//2 - } - - 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 - { - btAABB tbox = box; - tbox.appy_transform_trans_cache(trans1_to_0); - return has_collision(tbox); - } - - //! 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 - { - - //Taken from OPCODE - btVector3 ea,eb;//extends - btVector3 ca,cb;//extends - get_center_extend(ca,ea); - box.get_center_extend(cb,eb); - - - btVector3 T; - btScalar t,t2; - int i; - - // Class I : A's basis vectors - for(i=0;i<3;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; - } - // Class II : B's basis vectors - 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; - } - // Class III : 9 cross products - if(fulltest) - { - 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++) - { - 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; - } - } - } - return true; - } - - //! Simple test for planes. - SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) const - { - eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane); - return (classify == BT_CONST_COLLIDE_PLANE); - } - - //! 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 - { - if(!collide_plane(triangle_plane)) return false; - - btVector3 center,extends; - this->get_center_extend(center,extends); - - const btVector3 v1(p1 - center); - const btVector3 v2(p2 - center); - const btVector3 v3(p3 - center); - - //First axis - 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 With Y axis - 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); - - - 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 With Y axis - 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); - - 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 With Y axis - 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); - - return true; - } -}; - - -//! Compairison of transformation objects -SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) -{ - 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; - return true; -} - - - -#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btClipPolygon.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btClipPolygon.h deleted file mode 100644 index de0a5231b..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btClipPolygon.h +++ /dev/null @@ -1,182 +0,0 @@ -#ifndef BT_CLIP_POLYGON_H_INCLUDED -#define BT_CLIP_POLYGON_H_INCLUDED - -/*! \file btClipPolygon.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "LinearMath/btTransform.h" -#include "LinearMath/btGeometryUtil.h" - - -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) -{ - 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) -{ - 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); - clipped_count++; - } - 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) -{ - int clipped_count = 0; - - - //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)) - { - clipped[clipped_count] = point0; - clipped_count++; - } - - // point 1 - btScalar olddist = firstdist; - btScalar dist = bt_distance_point_plane(plane,point1); - - bt_plane_clip_polygon_collect( - point0,point1, - olddist, - dist, - clipped, - clipped_count); - - olddist = dist; - - - // point 2 - dist = bt_distance_point_plane(plane,point2); - - bt_plane_clip_polygon_collect( - 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); - - return clipped_count; -} - - - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btCompoundFromGimpact.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btCompoundFromGimpact.h deleted file mode 100644 index 02f8b678a..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btCompoundFromGimpact.h +++ /dev/null @@ -1,93 +0,0 @@ -#ifndef BT_COMPOUND_FROM_GIMPACT -#define BT_COMPOUND_FROM_GIMPACT - -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "btGImpactShape.h" -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" - -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) - { - if (hitFraction < m_hitFraction) - return hitFraction; - } - - return m_hitFraction; - } - }; - struct MyInternalTriangleIndexCallback :public btInternalTriangleIndexCallback - { - const btGImpactMeshShape* m_gimpactShape; - btCompoundShape* m_colShape; - btScalar m_depth; - - 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); - } - }; - -btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) -{ - btCompoundShape* colShape = new btCompoundShape(); - - 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; -} - -#endif //BT_COMPOUND_FROM_GIMPACT \ No newline at end of file diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.cpp deleted file mode 100644 index eed31d839..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.cpp +++ /dev/null @@ -1,181 +0,0 @@ - -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#include "btContactProcessing.h" - -#define MAX_COINCIDENT 8 - -struct CONTACT_KEY_TOKEN -{ - unsigned int m_key; - int m_value; - CONTACT_KEY_TOKEN() - { - } - - CONTACT_KEY_TOKEN(unsigned int key,int token) - { - m_key = key; - m_value = token; - } - - CONTACT_KEY_TOKEN(const CONTACT_KEY_TOKEN& rtoken) - { - m_key = rtoken.m_key; - m_value = rtoken.m_value; - } - - inline bool operator <(const CONTACT_KEY_TOKEN& other) const - { - return (m_key < other.m_key); - } - - inline bool operator >(const CONTACT_KEY_TOKEN& other) const - { - return (m_key > other.m_key); - } - -}; - -class CONTACT_KEY_TOKEN_COMP -{ - public: - - bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const - { - return ( a < b ); - } -}; - - -void btContactArray::merge_contacts( - const btContactArray & contacts, bool normal_contact_average) -{ - clear(); - - int i; - if(contacts.size()==0) return; - - - if(contacts.size()==1) - { - push_back(contacts[0]); - return; - } - - btAlignedObjectArray keycontacts; - - keycontacts.reserve(contacts.size()); - - //fill key contacts - - for ( i = 0;im_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) - { - *pcontact = *scontact; - coincident_count = 0; - } - else if(normal_contact_average) - { - if(btFabs(pcontact->m_depth - scontact->m_depth)m_normal; - coincident_count++; - } - } - } - } - else - {//add new contact - - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } - - push_back(*scontact); - pcontact = &(*this)[this->size()-1]; - } - last_key = key; - } -} - -void btContactArray::merge_contacts_unique(const btContactArray & contacts) -{ - clear(); - - if(contacts.size()==0) return; - - if(contacts.size()==1) - { - push_back(contacts[0]); - return; - } - - GIM_CONTACT average_contact = contacts[0]; - - for (int i=1;i -{ -public: - btContactArray() - { - reserve(64); - } - - SIMD_FORCE_INLINE void push_contact( - const btVector3 &point,const btVector3 & normal, - btScalar depth, int feature1, int feature2) - { - push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) ); - } - - SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT & tricontact, - int feature1,int feature2) - { - for(int i = 0;i splitValue) - { - //swap - primitive_boxes.swap(i,splitIndex); - //swapLeafNodes(i,splitIndex); - splitIndex++; - } - } - - //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex - //otherwise the tree-building might fail due to stack-overflows in certain cases. - //unbalanced1 is unsafe: it can cause stack overflows - //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); - - //unbalanced2 should work too: always use center (perfect balanced trees) - //bool unbalanced2 = true; - - //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); - - if (unbalanced) - { - splitIndex = startIndex+ (numIndices>>1); - } - - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); - - return splitIndex; - -} - - -void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) -{ - int curIndex = m_num_nodes; - m_num_nodes++; - - btAssert((endIndex-startIndex)>0); - - if ((endIndex-startIndex)==1) - { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); - m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); - - return; - } - //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. - - //split axis - int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); - - splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - - - //calc this node bounding box - - btAABB node_bound; - node_bound.invalidate(); - - for (int i=startIndex;iget_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); - } - else - { - //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); - //get left bound - btAABB bound; - bound.invalidate(); - - btAABB temp_box; - - int child_node = getLeftNode(nodecount); - if(child_node) - { - getNodeBound(child_node,temp_box); - bound.merge(temp_box); - } - - child_node = getRightNode(nodecount); - if(child_node) - { - getNodeBound(child_node,temp_box); - bound.merge(temp_box); - } - - setNodeBound(nodecount,bound); - } - } -} - -//! this rebuild the entire set -void btGImpactBvh::buildSet() -{ - //obtain primitive boxes - GIM_BVH_DATA_ARRAY primitive_boxes; - primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - - for (int i = 0;iget_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 -{ - int curIndex = 0; - int numNodes = getNodeCount(); - - while (curIndex < numNodes) - { - btAABB bound; - getNodeBound(curIndex,bound); - - //catch bugs in tree data - - bool aabbOverlap = bound.has_collision(box); - bool isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData(curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getEscapeNodeIndex(curIndex); - } - } - if(collided_results.size()>0) return true; - return false; -} - - - -//! returns the indices of the primitives in the m_primitive_manager -bool btGImpactBvh::rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray & collided_results) const -{ - int curIndex = 0; - int numNodes = getNodeCount(); - - while (curIndex < numNodes) - { - btAABB bound; - getNodeBound(curIndex,bound); - - //catch bugs in tree data - - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); - bool isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData( curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getEscapeNodeIndex(curIndex); - } - } - if(collided_results.size()>0) return true; - return false; -} - - -SIMD_FORCE_INLINE bool _node_collision( - btGImpactBvh * boxset0, btGImpactBvh * boxset1, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0 ,int node1, bool complete_primitive_tests) -{ - btAABB box0; - boxset0->getNodeBound(node0,box0); - btAABB box1; - boxset1->getNodeBound(node1,box1); - - return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); -// box1.appy_transform_trans_cache(trans_cache_1to0); -// return box0.has_collision(box1); - -} - - -//stackless recursive collision routine -static void _find_collision_pairs_recursive( - btGImpactBvh * boxset0, btGImpactBvh * boxset1, - btPairSet * collision_pairs, - const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0, - int node0, int node1, bool complete_primitive_tests) -{ - - - - if( _node_collision( - boxset0,boxset1,trans_cache_1to0, - node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes - - if(boxset0->isLeafNode(node0)) - { - if(boxset1->isLeafNode(node1)) - { - // collision result - collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); - return; - } - else - { - - //collide left recursive - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getLeftNode(node1),false); - - //collide right recursive - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - - } - } - else - { - if(boxset1->isLeafNode(node1)) - { - - //collide left recursive - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - - - //collide right recursive - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),node1,false); - - - } - else - { - //collide left0 left1 - - - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); - - //collide left0 right1 - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); - - - //collide right0 left1 - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); - - //collide right0 right1 - - _find_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); - - }// else if node1 is not a leaf - }// else if node0 is not a leaf -} - - -void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0, - btGImpactBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) -{ - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; - - BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - - trans_cache_1to0.calc_from_homogenic(trans0,trans1); - -#ifdef TRI_COLLISION_PROFILING - bt_begin_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING - - _find_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); -#ifdef TRI_COLLISION_PROFILING - bt_end_gim02_tree_time(); -#endif //TRI_COLLISION_PROFILING - -} - diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.h deleted file mode 100644 index 6174ae97a..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.h +++ /dev/null @@ -1,396 +0,0 @@ -#ifndef GIM_BOX_SET_H_INCLUDED -#define GIM_BOX_SET_H_INCLUDED - -/*! \file gim_box_set.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "LinearMath/btAlignedObjectArray.h" - -#include "btBoxCollision.h" -#include "btTriangleShapeEx.h" - - - - - -//! Overlapping pair -struct GIM_PAIR -{ - int m_index1; - int m_index2; - GIM_PAIR() - {} - - GIM_PAIR(const GIM_PAIR & p) - { - m_index1 = p.m_index1; - m_index2 = p.m_index2; - } - - GIM_PAIR(int index1, int index2) - { - m_index1 = index1; - m_index2 = index2; - } -}; - -//! A pairset array -class btPairSet: public btAlignedObjectArray -{ -public: - btPairSet() - { - reserve(32); - } - inline void push_pair(int index1,int index2) - { - push_back(GIM_PAIR(index1,index2)); - } - - inline void push_pair_inv(int index1,int index2) - { - push_back(GIM_PAIR(index2,index1)); - } -}; - - -///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box -struct GIM_BVH_DATA -{ - btAABB m_bound; - int m_data; -}; - -//! Node Structure for trees -class GIM_BVH_TREE_NODE -{ -public: - btAABB m_bound; -protected: - int m_escapeIndexOrDataIndex; -public: - GIM_BVH_TREE_NODE() - { - m_escapeIndexOrDataIndex = 0; - } - - SIMD_FORCE_INLINE bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); - } - - SIMD_FORCE_INLINE int getEscapeIndex() const - { - //btAssert(m_escapeIndexOrDataIndex < 0); - return -m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setEscapeIndex(int index) - { - m_escapeIndexOrDataIndex = -index; - } - - SIMD_FORCE_INLINE int getDataIndex() const - { - //btAssert(m_escapeIndexOrDataIndex >= 0); - - return m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setDataIndex(int index) - { - m_escapeIndexOrDataIndex = index; - } - -}; - - -class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray -{ -}; - - -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); - - 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); -public: - btBvhTree() - { - m_num_nodes = 0; - } - - //! prototype functions for box tree management - //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); - - SIMD_FORCE_INLINE void clearNodes() - { - m_node_array.clear(); - m_num_nodes = 0; - } - - //! node count - SIMD_FORCE_INLINE int getNodeCount() const - { - return m_num_nodes; - } - - //! tells if the node is a leaf - SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const - { - return m_node_array[nodeindex].isLeafNode(); - } - - SIMD_FORCE_INLINE int getNodeData(int nodeindex) const - { - return m_node_array[nodeindex].getDataIndex(); - } - - 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) - { - m_node_array[nodeindex].m_bound = bound; - } - - SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const - { - 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(); - } - - SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const - { - return m_node_array[nodeindex].getEscapeIndex(); - } - - SIMD_FORCE_INLINE const GIM_BVH_TREE_NODE * get_node_pointer(int index = 0) const - { - return &m_node_array[index]; - } - - //!@} -}; - - -//! Prototype Base class for primitive classification -/*! -This class is a wrapper for primitive collections. -This tells relevant info for the Bounding Box set classes, which take care of space classification. -This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons. -*/ -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; - //! retrieves only the points of the triangle, and the collision margin - 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. -Requires a Primitive prototype (like btPrimitiveManagerBase ) -*/ -class btGImpactBvh -{ -protected: - btBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; - -protected: - //stackless refit - void refit(); -public: - - //! this constructor doesn't build the tree. you must call buildSet - btGImpactBvh() - { - m_primitive_manager = NULL; - } - - //! this constructor doesn't build the tree. you must call buildSet - btGImpactBvh(btPrimitiveManagerBase * primitive_manager) - { - m_primitive_manager = primitive_manager; - } - - SIMD_FORCE_INLINE btAABB getGlobalBox() const - { - btAABB totalbox; - getNodeBound(0, totalbox); - return totalbox; - } - - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) - { - m_primitive_manager = primitive_manager; - } - - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const - { - return m_primitive_manager; - } - - -//! node manager prototype functions -///@{ - - //! this attemps to refit the box set. - SIMD_FORCE_INLINE void update() - { - refit(); - } - - //! this rebuild the entire set - void buildSet(); - - //! returns the indices of the primitives in the m_primitive_manager - 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 - { - btAABB transbox=box; - transbox.appy_transform(transform); - 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; - - //! tells if this set has hierarcht - SIMD_FORCE_INLINE bool hasHierarchy() const - { - return true; - } - - //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const - { - return m_primitive_manager->is_trimesh(); - } - - //! node count - SIMD_FORCE_INLINE int getNodeCount() const - { - return m_box_tree.getNodeCount(); - } - - //! tells if the node is a leaf - SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const - { - return m_box_tree.isLeafNode(nodeindex); - } - - SIMD_FORCE_INLINE int getNodeData(int nodeindex) const - { - return m_box_tree.getNodeData(nodeindex); - } - - 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) - { - m_box_tree.setNodeBound(nodeindex, bound); - } - - - SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const - { - return m_box_tree.getLeftNode(nodeindex); - } - - SIMD_FORCE_INLINE int getRightNode(int nodeindex) const - { - return m_box_tree.getRightNode(nodeindex); - } - - SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const - { - return m_box_tree.getEscapeNodeIndex(nodeindex); - } - - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const - { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); - } - - - 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 - - static void find_collision(btGImpactBvh * boxset1, const btTransform & trans1, - btGImpactBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); -}; - - -#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp deleted file mode 100644 index 2e87475e3..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp +++ /dev/null @@ -1,932 +0,0 @@ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -/* -Author: Francisco Len Nßjera -Concave-Concave Collision - -*/ - -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "btGImpactCollisionAlgorithm.h" -#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) - { - } - - void get_plane_equation(btVector4 &equation) - { - equation[0] = m_planeNormal[0]; - equation[1] = m_planeNormal[1]; - equation[2] = m_planeNormal[2]; - equation[3] = m_planeConstant; - } - - - void get_plane_equation_transformed(const btTransform & trans,btVector4 &equation) 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; - } -}; - - - -////////////////////////////////////////////////////////////////////////////////////////////// -#ifdef TRI_COLLISION_PROFILING - -btClock g_triangle_clock; - -float g_accum_triangle_collision_time = 0; -int g_count_triangle_collision = 0; - -void bt_begin_gim02_tri_time() -{ - g_triangle_clock.reset(); -} - -void bt_end_gim02_tri_time() -{ - g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds(); - g_count_triangle_collision++; -} -#endif //TRI_COLLISION_PROFILING -//! Retrieving shapes shapes -/*! -Declared here due of insuficent space on Pool allocators -*/ -//!@{ -class GIM_ShapeRetriever -{ -public: - const btGImpactShapeInterface * m_gim_shape; - btTriangleShapeEx m_trishape; - btTetrahedronShapeEx m_tetrashape; - -public: - class ChildShapeRetriever - { - public: - GIM_ShapeRetriever * m_parent; - virtual const btCollisionShape * getChildShape(int index) - { - return m_parent->m_gim_shape->getChildShape(index); - } - virtual ~ChildShapeRetriever() {} - }; - - class TriangleShapeRetriever:public ChildShapeRetriever - { - public: - - virtual btCollisionShape * getChildShape(int index) - { - m_parent->m_gim_shape->getBulletTriangle(index,m_parent->m_trishape); - return &m_parent->m_trishape; - } - virtual ~TriangleShapeRetriever() {} - }; - - class TetraShapeRetriever:public ChildShapeRetriever - { - public: - - virtual btCollisionShape * getChildShape(int index) - { - m_parent->m_gim_shape->getBulletTetrahedron(index,m_parent->m_tetrashape); - return &m_parent->m_tetrashape; - } - }; -public: - ChildShapeRetriever m_child_retriever; - TriangleShapeRetriever m_tri_retriever; - TetraShapeRetriever m_tetra_retriever; - ChildShapeRetriever * m_current_retriever; - - GIM_ShapeRetriever(const btGImpactShapeInterface * gim_shape) - { - m_gim_shape = gim_shape; - //select retriever - if(m_gim_shape->needsRetrieveTriangles()) - { - m_current_retriever = &m_tri_retriever; - } - else if(m_gim_shape->needsRetrieveTetrahedrons()) - { - m_current_retriever = &m_tetra_retriever; - } - else - { - m_current_retriever = &m_child_retriever; - } - - m_current_retriever->m_parent = this; - } - - 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; - - float avgtime = g_accum_triangle_collision_time; - avgtime /= (float)g_count_triangle_collision; - - g_accum_triangle_collision_time = 0; - g_count_triangle_collision = 0; - - return avgtime; -} - -#endif //TRI_COLLISION_PROFILING - - - -btGImpactCollisionAlgorithm::btGImpactCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap) -{ - m_manifoldPtr = NULL; - m_convex_algorithm = NULL; -} - -btGImpactCollisionAlgorithm::~btGImpactCollisionAlgorithm() -{ - clearCache(); -} - - - - - -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); -} - - -void btGImpactCollisionAlgorithm::shape_vs_shape_collision( - const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btCollisionShape * shape0, - const btCollisionShape * shape1) -{ - - - { - - 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); - - algor->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(algor); - } - -} - -void btGImpactCollisionAlgorithm::convex_vs_convex_collision( - 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); - - 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) -{ - if(shape0->hasBoxSet() && shape1->hasBoxSet()) - { - btGImpactBoxSet::find_collision(shape0->getBoxSet(),trans0,shape1->getBoxSet(),trans1,pairset); - } - else - { - btAABB boxshape0; - btAABB boxshape1; - int i = shape0->getNumChildShapes(); - - while(i--) - { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); - - int j = shape1->getNumChildShapes(); - while(j--) - { - shape1->getChildAabb(i,trans1,boxshape1.m_min,boxshape1.m_max); - - if(boxshape1.has_collision(boxshape0)) - { - pairset.push_pair(i,j); - } - } - } - } - - -} - - -void btGImpactCollisionAlgorithm::gimpact_vs_shape_find_pairs( - const btTransform & trans0, - const btTransform & trans1, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1, - btAlignedObjectArray & collided_primitives) -{ - - btAABB boxshape; - - - if(shape0->hasBoxSet()) - { - btTransform trans1to0 = trans0.inverse(); - trans1to0 *= trans1; - - shape1->getAabb(trans1to0,boxshape.m_min,boxshape.m_max); - - shape0->getBoxSet()->boxQuery(boxshape, collided_primitives); - } - else - { - shape1->getAabb(trans1,boxshape.m_min,boxshape.m_max); - - btAABB boxshape0; - int i = shape0->getNumChildShapes(); - - while(i--) - { - shape0->getChildAabb(i,trans0,boxshape0.m_min,boxshape0.m_max); - - if(boxshape.has_collision(boxshape0)) - { - collided_primitives.push_back(i); - } - } - - } - -} - - -void btGImpactCollisionAlgorithm::collide_gjk_triangles(const btCollisionObjectWrapper * body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) -{ - btTriangleShapeEx tri0; - btTriangleShapeEx tri1; - - shape0->lockChildShapes(); - shape1->lockChildShapes(); - - const int * pair_pointer = pairs; - - while(pair_count--) - { - - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; - - - - shape0->getBulletTriangle(m_triface0,tri0); - shape1->getBulletTriangle(m_triface1,tri1); - - - //collide two convex shapes - if(tri0.overlap_test_conservative(tri1)) - { - convex_vs_convex_collision(body0Wrap,body1Wrap,&tri0,&tri1); - } - - } - - shape0->unlockChildShapes(); - shape1->unlockChildShapes(); -} - -void btGImpactCollisionAlgorithm::collide_sat_triangles(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactMeshShapePart * shape0, - const btGImpactMeshShapePart * shape1, - const int * pairs, int pair_count) -{ - btTransform orgtrans0 = body0Wrap->getWorldTransform(); - btTransform orgtrans1 = body1Wrap->getWorldTransform(); - - btPrimitiveTriangle ptri0; - btPrimitiveTriangle ptri1; - GIM_TRIANGLE_CONTACT contact_data; - - shape0->lockChildShapes(); - shape1->lockChildShapes(); - - const int * pair_pointer = pairs; - - while(pair_count--) - { - - m_triface0 = *(pair_pointer); - m_triface1 = *(pair_pointer+1); - pair_pointer+=2; - - - shape0->getPrimitiveTriangle(m_triface0,ptri0); - shape1->getPrimitiveTriangle(m_triface1,ptri1); - - #ifdef TRI_COLLISION_PROFILING - bt_begin_gim02_tri_time(); - #endif - - ptri0.applyTransform(orgtrans0); - ptri1.applyTransform(orgtrans1); - - - //build planes - ptri0.buildTriPlane(); - ptri1.buildTriPlane(); - // test conservative - - - - if(ptri0.overlap_test_conservative(ptri1)) - { - if(ptri0.find_triangle_collision_clip_method(ptri1,contact_data)) - { - - int j = contact_data.m_point_count; - while(j--) - { - - addContactPoint(body0Wrap, body1Wrap, - contact_data.m_points[j], - contact_data.m_separating_normal, - -contact_data.m_penetration_depth); - } - } - } - - #ifdef TRI_COLLISION_PROFILING - bt_end_gim02_tri_time(); - #endif - - } - - shape0->unlockChildShapes(); - shape1->unlockChildShapes(); - -} - - -void btGImpactCollisionAlgorithm::gimpact_vs_gimpact( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btGImpactShapeInterface * shape1) -{ - - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) - { - const btGImpactMeshShape * meshshape0 = static_cast(shape0); - m_part0 = meshshape0->getMeshPartCount(); - - while(m_part0--) - { - gimpact_vs_gimpact(body0Wrap,body1Wrap,meshshape0->getMeshPart(m_part0),shape1); - } - - return; - } - - if(shape1->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) - { - const btGImpactMeshShape * meshshape1 = static_cast(shape1); - m_part1 = meshshape1->getMeshPartCount(); - - while(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); - - if(pairset.size()== 0) return; - - 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 - - return; - } - - //general function - - shape0->lockChildShapes(); - shape1->lockChildShapes(); - - GIM_ShapeRetriever retriever0(shape0); - GIM_ShapeRetriever retriever1(shape1); - - bool child_has_transform0 = shape0->childrenHasTransform(); - bool child_has_transform1 = shape1->childrenHasTransform(); - - int i = pairset.size(); - while(i--) - { - GIM_PAIR * pair = &pairset[i]; - m_triface0 = pair->m_index1; - m_triface1 = pair->m_index2; - 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) - { - tr0 = orgtrans0*shape0->getChildTransform(m_triface0); - } - - if(child_has_transform1) - { - 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); - - //collide two convex shapes - convex_vs_convex_collision(&ob0,&ob1,colshape0,colshape1); - } - - shape0->unlockChildShapes(); - shape1->unlockChildShapes(); -} - -void btGImpactCollisionAlgorithm::gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btCollisionShape * shape1,bool swapped) -{ - if(shape0->getGImpactShapeType()==CONST_GIMPACT_TRIMESH_SHAPE) - { - const btGImpactMeshShape * meshshape0 = static_cast(shape0); - int& part = swapped ? m_part1 : m_part0; - part = meshshape0->getMeshPartCount(); - - while(part--) - { - - gimpact_vs_shape(body0Wrap, - body1Wrap, - meshshape0->getMeshPart(part), - shape1,swapped); - - } - - return; - } - - #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); - return; - } - - #endif - - - - if(shape1->isCompound()) - { - const btCompoundShape * compoundshape = static_cast(shape1); - gimpact_vs_compoundshape(body0Wrap,body1Wrap,shape0,compoundshape,swapped); - return; - } - else if(shape1->isConcave()) - { - 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; - - - shape0->lockChildShapes(); - - GIM_ShapeRetriever retriever0(shape0); - - - bool child_has_transform0 = shape0->childrenHasTransform(); - - - int i = collided_results.size(); - - while(i--) - { - int child_index = collided_results[i]; - if(swapped) - m_triface1 = child_index; - else - m_triface0 = child_index; - - const btCollisionShape * colshape0 = retriever0.getChildShape(child_index); - - btTransform tr0 = body0Wrap->getWorldTransform(); - - if(child_has_transform0) - { - 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()) - { - m_resultOut->setBody0Wrap(&ob0); - } else - { - m_resultOut->setBody1Wrap(&ob0); - } - - //collide two shapes - if(swapped) - { - - shape_vs_shape_collision(body1Wrap,&ob0,shape1,colshape0); - } - else - { - - shape_vs_shape_collision(&ob0,body1Wrap,colshape0,shape1); - } - m_resultOut->setBody0Wrap(prevObj0); - - } - - shape0->unlockChildShapes(); - -} - -void btGImpactCollisionAlgorithm::gimpact_vs_compoundshape(const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper* body1Wrap, - const btGImpactShapeInterface * shape0, - const btCompoundShape * shape1,bool swapped) -{ - btTransform orgtrans1 = body1Wrap->getWorldTransform(); - - int i = shape1->getNumChildShapes(); - while(i--) - { - - const btCollisionShape * colshape1 = shape1->getChildShape(i); - btTransform childtrans1 = orgtrans1*shape1->getChildTransform(i); - - btCollisionObjectWrapper ob1(body1Wrap,colshape1,body1Wrap->getCollisionObject(),childtrans1,-1,i); - - const btCollisionObjectWrapper* tmp = 0; - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) - { - tmp = m_resultOut->getBody0Wrap(); - m_resultOut->setBody0Wrap(&ob1); - } else - { - tmp = m_resultOut->getBody1Wrap(); - m_resultOut->setBody1Wrap(&ob1); - } - //collide child shape - gimpact_vs_shape(body0Wrap, &ob1, - shape0,colshape1,swapped); - - if (m_resultOut->getBody0Wrap()->getCollisionObject()==ob1.getCollisionObject()) - { - m_resultOut->setBody0Wrap(tmp); - } else - { - m_resultOut->setBody1Wrap(tmp); - } - } -} - -void btGImpactCollisionAlgorithm::gimpacttrimeshpart_vs_plane_collision( - 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); - btVector4 plane; - planeshape->get_plane_equation_transformed(orgtrans1,plane); - - //test box against plane - - btAABB tribox; - shape0->getAabb(orgtrans0,tribox.m_min,tribox.m_max); - tribox.increment_margin(planeshape->getMargin()); - - if( tribox.plane_classify(plane)!= BT_CONST_COLLIDE_PLANE) return; - - shape0->lockChildShapes(); - - btScalar margin = shape0->getMargin() + planeshape->getMargin(); - - btVector3 vertex; - int vi = shape0->getVertexCount(); - while(vi--) - { - shape0->getVertex(vi,vertex); - vertex = orgtrans0(vertex); - - btScalar distance = vertex.dot(plane) - plane[3] - margin; - - if(distance<0.0)//add contact - { - if(swapped) - { - addContactPoint(body1Wrap, body0Wrap, - vertex, - -plane, - distance); - } - else - { - addContactPoint(body0Wrap, body1Wrap, - vertex, - plane, - distance); - } - } - } - - shape0->unlockChildShapes(); -} - - - - -class btGImpactTriangleCallback: public btTriangleCallback -{ -public: - 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]); - tri1.setMargin(margin); - 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; - - if (algorithm->internalGetResultOut()->getBody0Wrap()->getCollisionObject()==ob1Wrap.getCollisionObject()) - { - tmp = algorithm->internalGetResultOut()->getBody0Wrap(); - algorithm->internalGetResultOut()->setBody0Wrap(&ob1Wrap); - } 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->internalGetResultOut()->setBody0Wrap(tmp); - } else - { - algorithm->internalGetResultOut()->setBody1Wrap(tmp); - } - - } -}; - - - - -void btGImpactCollisionAlgorithm::gimpact_vs_concave( - const btCollisionObjectWrapper* body0Wrap, - const btCollisionObjectWrapper * body1Wrap, - const btGImpactShapeInterface * shape0, - const btConcaveShape * shape1,bool swapped) -{ - //create the callback - btGImpactTriangleCallback tricallback; - tricallback.algorithm = this; - tricallback.body0Wrap = body0Wrap; - tricallback.body1Wrap = body1Wrap; - tricallback.gimpactshape0 = shape0; - tricallback.swapped = swapped; - tricallback.margin = shape1->getMargin(); - - //getting the trimesh AABB - btTransform gimpactInConcaveSpace; - - gimpactInConcaveSpace = body1Wrap->getWorldTransform().inverse() * body0Wrap->getWorldTransform(); - - 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) -{ - clearCache(); - - m_resultOut = resultOut; - m_dispatchInfo = &dispatchInfo; - const btGImpactShapeInterface * gimpactshape0; - const btGImpactShapeInterface * gimpactshape1; - - if (body0Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE) - { - gimpactshape0 = static_cast(body0Wrap->getCollisionShape()); - - if( body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) - { - gimpactshape1 = static_cast(body1Wrap->getCollisionShape()); - - gimpact_vs_gimpact(body0Wrap,body1Wrap,gimpactshape0,gimpactshape1); - } - else - { - gimpact_vs_shape(body0Wrap,body1Wrap,gimpactshape0,body1Wrap->getCollisionShape(),false); - } - - } - else if (body1Wrap->getCollisionShape()->getShapeType()==GIMPACT_SHAPE_PROXYTYPE ) - { - gimpactshape1 = static_cast(body1Wrap->getCollisionShape()); - - gimpact_vs_shape(body1Wrap,body0Wrap,gimpactshape1,body0Wrap->getCollisionShape(),true); - } -} - - -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) -{ - - static btGImpactCollisionAlgorithm::CreateFunc s_gimpact_cf; - - int i; - - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) - { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,&s_gimpact_cf); - } - - for ( i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) - { - dispatcher->registerCollisionCreateFunc(i,GIMPACT_SHAPE_PROXYTYPE ,&s_gimpact_cf); - } - -} diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h deleted file mode 100644 index f85a94cb4..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ /dev/null @@ -1,310 +0,0 @@ -/*! \file btGImpactShape.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H -#define BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H - -#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" - -#include "LinearMath/btAlignedObjectArray.h" - -#include "btGImpactShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - - -//! Collision Algorithm for GImpact Shapes -/*! -For register this algorithm in Bullet, proceed as following: - \code -btCollisionDispatcher * dispatcher = static_cast(m_dynamicsWorld ->getDispatcher()); -btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); - \endcode -*/ -class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ -protected: - btCollisionAlgorithm * m_convex_algorithm; - btPersistentManifold * m_manifoldPtr; - btManifoldResult* m_resultOut; - 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) - { - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); - return m_manifoldPtr; - } - - SIMD_FORCE_INLINE void destroyConvexAlgorithm() - { - if(m_convex_algorithm) - { - m_convex_algorithm->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm); - m_convex_algorithm = NULL; - } - } - - SIMD_FORCE_INLINE void destroyContactManifolds() - { - if(m_manifoldPtr == NULL) return; - m_dispatcher->releaseManifold(m_manifoldPtr); - m_manifoldPtr = NULL; - } - - SIMD_FORCE_INLINE void clearCache() - { - destroyContactManifolds(); - destroyConvexAlgorithm(); - - m_triface0 = -1; - m_part0 = -1; - m_triface1 = -1; - m_part1 = -1; - } - - SIMD_FORCE_INLINE btPersistentManifold* getLastManifold() - { - return m_manifoldPtr; - } - - - // Call before process collision - SIMD_FORCE_INLINE void checkManifold(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - if(getLastManifold() == 0) - { - newContactManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); - } - - m_resultOut->setPersistentManifold(getLastManifold()); - } - - // Call before process collision - SIMD_FORCE_INLINE btCollisionAlgorithm * newAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { - checkManifold(body0Wrap,body1Wrap); - - btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( - body0Wrap,body1Wrap,getLastManifold()); - return convex_algorithm ; - } - - // Call before process collision - SIMD_FORCE_INLINE void checkConvexAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* 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); - -//! Collision routines -//!@{ - - void collide_gjk_triangles(const btCollisionObjectWrapper* body0Wrap, - 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); - - - - - void shape_vs_shape_collision( - 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); - - - - void gimpact_vs_gimpact_find_pairs( - 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); - - - void gimpacttrimeshpart_vs_plane_collision( - 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); - - virtual ~btGImpactCollisionAlgorithm(); - - 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); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr) - manifoldArray.push_back(m_manifoldPtr); - } - - btManifoldResult* internalGetResultOut() - { - return m_resultOut; - } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - - //! Use this function for register the algorithm externally - 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 - - //! 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); - - void gimpact_vs_shape(const btCollisionObjectWrapper* body0Wrap, - 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_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; - } - -}; - - -//algorithm details -//#define BULLET_TRIANGLE_COLLISION 1 -#define GIMPACT_VS_PLANE_COLLISION 1 - - - -#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactMassUtil.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactMassUtil.h deleted file mode 100644 index 2543aefcf..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactMassUtil.h +++ /dev/null @@ -1,60 +0,0 @@ -/*! \file btGImpactMassUtil.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#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) -{ - btMatrix3x3 rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose(); - - btScalar x2 = transform.getOrigin()[0]; - x2*= x2; - btScalar y2 = transform.getOrigin()[1]; - y2*= y2; - btScalar z2 = transform.getOrigin()[2]; - z2*= z2; - - 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); -} - -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)); -} - - -#endif //GIMPACT_MESH_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp deleted file mode 100644 index 4528758c3..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp +++ /dev/null @@ -1,528 +0,0 @@ -/*! \file gim_box_set.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGImpactQuantizedBvh.h" -#include "LinearMath/btQuickprof.h" - -#ifdef TRI_COLLISION_PROFILING -btClock g_q_tree_clock; - - -float g_q_accum_tree_collision_time = 0; -int g_q_count_traversing = 0; - - -void bt_begin_gim02_q_tree_time() -{ - g_q_tree_clock.reset(); -} - -void bt_end_gim02_q_tree_time() -{ - g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds(); - g_q_count_traversing++; -} - - -//! Gets the average time in miliseconds of tree collisions -float btGImpactQuantizedBvh::getAverageTreeCollisionTime() -{ - if(g_q_count_traversing == 0) return 0; - - float avgtime = g_q_accum_tree_collision_time; - avgtime /= (float)g_q_count_traversing; - - g_q_accum_tree_collision_time = 0; - g_q_count_traversing = 0; - return avgtime; - -// float avgtime = g_q_count_traversing; -// g_q_count_traversing = 0; -// return avgtime; - -} - -#endif //TRI_COLLISION_PROFILING - -/////////////////////// btQuantizedBvhTree ///////////////////////////////// - -void btQuantizedBvhTree::calc_quantization( - GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin) -{ - //calc globa box - btAABB global_bound; - global_bound.invalidate(); - - for (int i=0;i splitValue) - { - //swap - primitive_boxes.swap(i,splitIndex); - //swapLeafNodes(i,splitIndex); - splitIndex++; - } - } - - //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex - //otherwise the tree-building might fail due to stack-overflows in certain cases. - //unbalanced1 is unsafe: it can cause stack overflows - //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); - - //unbalanced2 should work too: always use center (perfect balanced trees) - //bool unbalanced2 = true; - - //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); - - if (unbalanced) - { - splitIndex = startIndex+ (numIndices>>1); - } - - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); - - return splitIndex; - -} - - -void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex) -{ - int curIndex = m_num_nodes; - m_num_nodes++; - - btAssert((endIndex-startIndex)>0); - - if ((endIndex-startIndex)==1) - { - //We have a leaf node - setNodeBound(curIndex,primitive_boxes[startIndex].m_bound); - m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data); - - return; - } - //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. - - //split axis - int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex); - - splitIndex = _sort_and_calc_splitting_index( - primitive_boxes,startIndex,endIndex, - splitIndex//split axis - ); - - - //calc this node bounding box - - btAABB node_bound; - node_bound.invalidate(); - - for (int i=startIndex;iget_primitive_box(getNodeData(nodecount),leafbox); - setNodeBound(nodecount,leafbox); - } - else - { - //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount); - //get left bound - btAABB bound; - bound.invalidate(); - - btAABB temp_box; - - int child_node = getLeftNode(nodecount); - if(child_node) - { - getNodeBound(child_node,temp_box); - bound.merge(temp_box); - } - - child_node = getRightNode(nodecount); - if(child_node) - { - getNodeBound(child_node,temp_box); - bound.merge(temp_box); - } - - setNodeBound(nodecount,bound); - } - } -} - -//! this rebuild the entire set -void btGImpactQuantizedBvh::buildSet() -{ - //obtain primitive boxes - GIM_BVH_DATA_ARRAY primitive_boxes; - primitive_boxes.resize(m_primitive_manager->get_primitive_count()); - - for (int i = 0;iget_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 -{ - int curIndex = 0; - int numNodes = getNodeCount(); - - //quantize box - - unsigned short quantizedMin[3]; - unsigned short quantizedMax[3]; - - m_box_tree.quantizePoint(quantizedMin,box.m_min); - m_box_tree.quantizePoint(quantizedMax,box.m_max); - - - while (curIndex < numNodes) - { - - //catch bugs in tree data - - bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax); - bool isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData(curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getEscapeNodeIndex(curIndex); - } - } - if(collided_results.size()>0) return true; - return false; -} - - - -//! returns the indices of the primitives in the m_primitive_manager -bool btGImpactQuantizedBvh::rayQuery( - const btVector3 & ray_dir,const btVector3 & ray_origin , - btAlignedObjectArray & collided_results) const -{ - int curIndex = 0; - int numNodes = getNodeCount(); - - while (curIndex < numNodes) - { - btAABB bound; - getNodeBound(curIndex,bound); - - //catch bugs in tree data - - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); - bool isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData( curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getEscapeNodeIndex(curIndex); - } - } - if(collided_results.size()>0) return true; - return false; -} - - -SIMD_FORCE_INLINE bool _quantized_node_collision( - 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); - btAABB box1; - boxset1->getNodeBound(node1,box1); - - return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests ); -// box1.appy_transform_trans_cache(trans_cache_1to0); -// return box0.has_collision(box1); - -} - - -//stackless recursive collision routine -static void _find_quantized_collision_pairs_recursive( - 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(boxset0->isLeafNode(node0)) - { - if(boxset1->isLeafNode(node1)) - { - // collision result - collision_pairs->push_pair( - boxset0->getNodeData(node0),boxset1->getNodeData(node1)); - return; - } - else - { - - //collide left recursive - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getLeftNode(node1),false); - - //collide right recursive - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - node0,boxset1->getRightNode(node1),false); - - - } - } - else - { - if(boxset1->isLeafNode(node1)) - { - - //collide left recursive - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),node1,false); - - - //collide right recursive - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),node1,false); - - - } - else - { - //collide left0 left1 - - - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false); - - //collide left0 right1 - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false); - - - //collide right0 left1 - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false); - - //collide right0 right1 - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - collision_pairs,trans_cache_1to0, - boxset0->getRightNode(node0),boxset1->getRightNode(node1),false); - - }// else if node1 is not a leaf - }// else if node0 is not a leaf -} - - -void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0, - const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - btPairSet & collision_pairs) -{ - - if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return; - - BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - - trans_cache_1to0.calc_from_homogenic(trans0,trans1); - -#ifdef TRI_COLLISION_PROFILING - bt_begin_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - - _find_quantized_collision_pairs_recursive( - boxset0,boxset1, - &collision_pairs,trans_cache_1to0,0,0,true); -#ifdef TRI_COLLISION_PROFILING - bt_end_gim02_q_tree_time(); -#endif //TRI_COLLISION_PROFILING - -} - - diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.h deleted file mode 100644 index e6e52fff4..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ /dev/null @@ -1,372 +0,0 @@ -#ifndef GIM_QUANTIZED_SET_H_INCLUDED -#define GIM_QUANTIZED_SET_H_INCLUDED - -/*! \file btGImpactQuantizedBvh.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGImpactBvh.h" -#include "btQuantization.h" - - - - - -///btQuantizedBvhNode is a compressed aabb node, 16 bytes. -///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). -ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE -{ - //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; - //4 bytes - int m_escapeIndexOrDataIndex; - - BT_QUANTIZED_BVH_NODE() - { - m_escapeIndexOrDataIndex = 0; - } - - SIMD_FORCE_INLINE bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); - } - - SIMD_FORCE_INLINE int getEscapeIndex() const - { - //btAssert(m_escapeIndexOrDataIndex < 0); - return -m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setEscapeIndex(int index) - { - m_escapeIndexOrDataIndex = -index; - } - - SIMD_FORCE_INLINE int getDataIndex() const - { - //btAssert(m_escapeIndexOrDataIndex >= 0); - - return m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setDataIndex(int index) - { - m_escapeIndexOrDataIndex = index; - } - - SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( - unsigned short * quantizedMin,unsigned short * quantizedMax) const - { - if(m_quantizedAabbMin[0] > quantizedMax[0] || - m_quantizedAabbMax[0] < quantizedMin[0] || - m_quantizedAabbMin[1] > quantizedMax[1] || - m_quantizedAabbMax[1] < quantizedMin[1] || - m_quantizedAabbMin[2] > quantizedMax[2] || - m_quantizedAabbMax[2] < quantizedMin[2]) - { - return false; - } - return true; - } - -}; - - - -class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray -{ -}; - - - - -//! Basic Box tree structure -class btQuantizedBvhTree -{ -protected: - int m_num_nodes; - 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) ); - - int _sort_and_calc_splitting_index( - 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); - - void _build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex); -public: - btQuantizedBvhTree() - { - m_num_nodes = 0; - } - - //! prototype functions for box tree management - //!@{ - void build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes); - - SIMD_FORCE_INLINE void quantizePoint( - unsigned short * quantizedpoint, const btVector3 & point) const - { - 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 - { - return m_node_array[node_index].testQuantizedBoxOverlapp(quantizedMin,quantizedMax); - } - - SIMD_FORCE_INLINE void clearNodes() - { - m_node_array.clear(); - m_num_nodes = 0; - } - - //! node count - SIMD_FORCE_INLINE int getNodeCount() const - { - return m_num_nodes; - } - - //! tells if the node is a leaf - SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const - { - return m_node_array[nodeindex].isLeafNode(); - } - - SIMD_FORCE_INLINE int getNodeData(int nodeindex) const - { - return m_node_array[nodeindex].getDataIndex(); - } - - 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); - - bound.m_max = bt_unquantize( - m_node_array[nodeindex].m_quantizedAabbMax, - m_global_bound.m_min,m_bvhQuantization); - } - - 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_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; - } - - 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(); - } - - SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const - { - return m_node_array[nodeindex].getEscapeIndex(); - } - - SIMD_FORCE_INLINE const BT_QUANTIZED_BVH_NODE * get_node_pointer(int index = 0) const - { - return &m_node_array[index]; - } - - //!@} -}; - - - -//! Structure for containing Boxes -/*! -This class offers an structure for managing a box tree of primitives. -Requires a Primitive prototype (like btPrimitiveManagerBase ) -*/ -class btGImpactQuantizedBvh -{ -protected: - btQuantizedBvhTree m_box_tree; - btPrimitiveManagerBase * m_primitive_manager; - -protected: - //stackless refit - void refit(); -public: - - //! this constructor doesn't build the tree. you must call buildSet - btGImpactQuantizedBvh() - { - m_primitive_manager = NULL; - } - - //! this constructor doesn't build the tree. you must call buildSet - btGImpactQuantizedBvh(btPrimitiveManagerBase * primitive_manager) - { - m_primitive_manager = primitive_manager; - } - - SIMD_FORCE_INLINE btAABB getGlobalBox() const - { - btAABB totalbox; - getNodeBound(0, totalbox); - return totalbox; - } - - SIMD_FORCE_INLINE void setPrimitiveManager(btPrimitiveManagerBase * primitive_manager) - { - m_primitive_manager = primitive_manager; - } - - SIMD_FORCE_INLINE btPrimitiveManagerBase * getPrimitiveManager() const - { - return m_primitive_manager; - } - - -//! node manager prototype functions -///@{ - - //! this attemps to refit the box set. - SIMD_FORCE_INLINE void update() - { - refit(); - } - - //! this rebuild the entire set - void buildSet(); - - //! returns the indices of the primitives in the m_primitive_manager - 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 - { - btAABB transbox=box; - transbox.appy_transform(transform); - 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; - - //! tells if this set has hierarcht - SIMD_FORCE_INLINE bool hasHierarchy() const - { - return true; - } - - //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const - { - return m_primitive_manager->is_trimesh(); - } - - //! node count - SIMD_FORCE_INLINE int getNodeCount() const - { - return m_box_tree.getNodeCount(); - } - - //! tells if the node is a leaf - SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const - { - return m_box_tree.isLeafNode(nodeindex); - } - - SIMD_FORCE_INLINE int getNodeData(int nodeindex) const - { - return m_box_tree.getNodeData(nodeindex); - } - - 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) - { - m_box_tree.setNodeBound(nodeindex, bound); - } - - - SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const - { - return m_box_tree.getLeftNode(nodeindex); - } - - SIMD_FORCE_INLINE int getRightNode(int nodeindex) const - { - return m_box_tree.getRightNode(nodeindex); - } - - SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const - { - return m_box_tree.getEscapeNodeIndex(nodeindex); - } - - SIMD_FORCE_INLINE void getNodeTriangle(int nodeindex,btPrimitiveTriangle & triangle) const - { - m_primitive_manager->get_primitive_triangle(getNodeData(nodeindex),triangle); - } - - - 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 - - static void find_collision(const btGImpactQuantizedBvh * boxset1, const btTransform & trans1, - const btGImpactQuantizedBvh * boxset2, const btTransform & trans2, - btPairSet & collision_pairs); -}; - - -#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.cpp deleted file mode 100644 index ac8efdf38..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btGImpactShape.h" -#include "btGImpactMassUtil.h" - - -#define CALC_EXACT_INERTIA 1 - - -void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - lockChildShapes(); -#ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); - - int i = this->getNumChildShapes(); - btScalar shapemass = mass/btScalar(i); - - while(i--) - { - btVector3 temp_inertia; - m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia); - if(childrenHasTransform()) - { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]); - } - else - { - inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity()); - } - - } - -#else - - // Calc box inertia - - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * btScalar(0.08333333); - - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); - -#endif - unlockChildShapes(); -} - - - -void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - lockChildShapes(); - - -#ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); - - int i = this->getVertexCount(); - btScalar pointmass = mass/btScalar(i); - - while(i--) - { - btVector3 pointintertia; - this->getVertex(i,pointintertia); - pointintertia = gim_get_point_inertia(pointintertia,pointmass); - inertia+=pointintertia; - } - -#else - - // Calc box inertia - - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * btScalar(0.08333333); - - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); - -#endif - - unlockChildShapes(); -} - -void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - -#ifdef CALC_EXACT_INERTIA - inertia.setValue(0.f,0.f,0.f); - - int i = this->getMeshPartCount(); - btScalar partmass = mass/btScalar(i); - - while(i--) - { - btVector3 partinertia; - getMeshPart(i)->calculateLocalInertia(partmass,partinertia); - inertia+=partinertia; - } - -#else - - // Calc box inertia - - btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0]; - btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1]; - btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2]; - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * btScalar(0.08333333); - - inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); - -#endif -} - -void btGImpactMeshShape::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btCollisionWorld::RayResultCallback& resultCallback) const -{ -} - -void btGImpactMeshShapePart::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const -{ - lockChildShapes(); - - btAlignedObjectArray collided; - btVector3 rayDir(rayTo - rayFrom); - rayDir.normalize(); - m_box_set.rayQuery(rayDir, rayFrom, collided); - - if(collided.size()==0) - { - unlockChildShapes(); - return; - } - - int part = (int)getPart(); - btPrimitiveTriangle triangle; - int i = collided.size(); - while(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 -{ - lockChildShapes(); - btAABB box; - box.m_min = aabbMin; - box.m_max = aabbMax; - - btAlignedObjectArray collided; - m_box_set.boxQuery(box,collided); - - if(collided.size()==0) - { - unlockChildShapes(); - return; - } - - int part = (int)getPart(); - btPrimitiveTriangle triangle; - int i = collided.size(); - while(i--) - { - this->getPrimitiveTriangle(collided[i],triangle); - callback->processTriangle(triangle.m_vertices,part,collided[i]); - } - unlockChildShapes(); - -} - -void btGImpactMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const -{ - int i = m_mesh_parts.size(); - while(i--) - { - m_mesh_parts[i]->processAllTriangles(callback,aabbMin,aabbMax); - } -} - -void btGImpactMeshShape::processAllTrianglesRay(btTriangleCallback* callback,const btVector3& rayFrom, const btVector3& rayTo) const -{ - int i = m_mesh_parts.size(); - 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 -{ - btGImpactMeshShapeData* trimeshData = (btGImpactMeshShapeData*) dataBuffer; - - btCollisionShape::serialize(&trimeshData->m_collisionShapeData,serializer); - - m_meshInterface->serialize(&trimeshData->m_meshInterface, serializer); - - trimeshData->m_collisionMargin = float(m_collisionMargin); - - localScaling.serializeFloat(trimeshData->m_localScaling); - - trimeshData->m_gimpactSubType = int(getGImpactShapeType()); - - return "btGImpactMeshShapeData"; -} - diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.h deleted file mode 100644 index 3d1f48d47..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.h +++ /dev/null @@ -1,1184 +0,0 @@ -/*! \file btGImpactShape.h -\author Francisco Len Nßjera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef GIMPACT_SHAPE_H -#define GIMPACT_SHAPE_H - -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btAlignedObjectArray.h" - -#include "btGImpactQuantizedBvh.h" // box tree class - - -//! declare Quantized trees, (you can change to float based trees) -typedef btGImpactQuantizedBvh btGImpactBoxSet; - -enum eGIMPACT_SHAPE_TYPE -{ - CONST_GIMPACT_COMPOUND_SHAPE = 0, - CONST_GIMPACT_TRIMESH_SHAPE_PART, - CONST_GIMPACT_TRIMESH_SHAPE -}; - - - -//! Helper class for tetrahedrons -class btTetrahedronShapeEx:public btBU_Simplex1to4 -{ -public: - btTetrahedronShapeEx() - { - m_numVertices = 4; - } - - - SIMD_FORCE_INLINE void setVertices( - const btVector3 & v0,const btVector3 & v1, - const btVector3 & v2,const btVector3 & v3) - { - m_vertices[0] = v0; - m_vertices[1] = v1; - m_vertices[2] = v2; - m_vertices[3] = v3; - recalcLocalAabb(); - } -}; - - -//! 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 - - //! use this function for perfofm refit in bounding boxes - //! 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(); - } - - -public: - btGImpactShapeInterface() - { - m_shapeType=GIMPACT_SHAPE_PROXYTYPE; - m_localAABB.invalidate(); - m_needs_update = true; - localScaling.setValue(1.f,1.f,1.f); - } - - - //! performs refit operation - /*! - Updates the entire Box set of this shape. - \pre postUpdate() must be called for attemps to calculating the box set, else this function - 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; - } - - //! 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; - } - - //! 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() - { - return m_localAABB; - } - - - virtual int getShapeType() const - { - return GIMPACT_SHAPE_PROXYTYPE; - } - - /*! - \post You must call updateBound() for update the box set. - */ - virtual void setLocalScaling(const btVector3& scaling) - { - localScaling = scaling; - postUpdate(); - } - - virtual const btVector3& getLocalScaling() const - { - return localScaling; - } - - - virtual void setMargin(btScalar margin) - { - 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 ; - - //! gets boxset - 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 - { - if(m_box_set.getNodeCount() == 0) return false; - return true; - } - - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const = 0; - - - //! Gets the number of children - virtual int getNumChildShapes() const = 0; - - //! if true, then its children must get transforms. - virtual bool childrenHasTransform() const = 0; - - //! Determines if this shape has triangles - virtual bool needsRetrieveTriangles() const = 0; - - //! 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; - - - - //! call when reading child shapes - virtual void lockChildShapes() const - { - } - - virtual void unlockChildShapes() const - { - } - - //! if this trimesh - SIMD_FORCE_INLINE void getPrimitiveTriangle(int index,btPrimitiveTriangle & triangle) const - { - 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; - } - - //! 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; - - //! Sets the children transform - /*! - \post You must call updateBound() for update the box set. - */ - 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 - { - (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 - { - (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 - { - - } - - //!@} - -}; - - -//! btGImpactCompoundShape allows to handle multiple btCollisionShape objects at once -/*! -This class only can manage Convex subshapes -*/ -class btGImpactCompoundShape : public btGImpactShapeInterface -{ -public: - //! compound primitive manager - class CompoundPrimitiveManager:public btPrimitiveManagerBase - { - public: - virtual ~CompoundPrimitiveManager() {} - btGImpactCompoundShape * m_compoundShape; - - - CompoundPrimitiveManager(const CompoundPrimitiveManager& compound) - : btPrimitiveManagerBase() - { - m_compoundShape = compound.m_compoundShape; - } - - CompoundPrimitiveManager(btGImpactCompoundShape * compoundShape) - { - m_compoundShape = compoundShape; - } - - CompoundPrimitiveManager() - { - m_compoundShape = NULL; - } - - virtual bool is_trimesh() const - { - return false; - } - - virtual int get_primitive_count() const - { - return (int )m_compoundShape->getNumChildShapes(); - } - - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const - { - btTransform prim_trans; - if(m_compoundShape->childrenHasTransform()) - { - prim_trans = m_compoundShape->getChildTransform(prim_index); - } - else - { - prim_trans.setIdentity(); - } - const btCollisionShape* shape = m_compoundShape->getChildShape(prim_index); - shape->getAabb(prim_trans,primbox.m_min,primbox.m_max); - } - - virtual void get_primitive_triangle(int prim_index,btPrimitiveTriangle & triangle) const - { - btAssert(0); - (void) prim_index; (void) triangle; - } - - }; - - - -protected: - CompoundPrimitiveManager m_primitive_manager; - btAlignedObjectArray m_childTransforms; - btAlignedObjectArray m_childShapes; - - -public: - - btGImpactCompoundShape(bool children_has_transform = true) - { - (void) children_has_transform; - m_primitive_manager.m_compoundShape = this; - m_box_set.setPrimitiveManager(&m_primitive_manager); - } - - virtual ~btGImpactCompoundShape() - { - } - - - //! if true, then its children must get transforms. - virtual bool childrenHasTransform() const - { - if(m_childTransforms.size()==0) return false; - return true; - } - - - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const - { - return &m_primitive_manager; - } - - //! Obtains the compopund primitive manager - SIMD_FORCE_INLINE CompoundPrimitiveManager * getCompoundPrimitiveManager() - { - return &m_primitive_manager; - } - - //! Gets the number of children - 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) - { - btAssert(shape->isConvex()); - m_childTransforms.push_back(localTransform); - m_childShapes.push_back(shape); - } - - //! Use this method for adding children. Only Convex shapes are allowed. - void addChildShape(btCollisionShape* shape) - { - btAssert(shape->isConvex()); - m_childShapes.push_back(shape); - } - - //! Gets the children - virtual btCollisionShape* getChildShape(int index) - { - return m_childShapes[index]; - } - - //! Gets the children - virtual const btCollisionShape* getChildShape(int index) const - { - return m_childShapes[index]; - } - - //! 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); - } - } - - - //! Gets the children transform - virtual btTransform getChildTransform(int index) const - { - btAssert(m_childTransforms.size() == m_childShapes.size()); - return m_childTransforms[index]; - } - - //! Sets the children transform - /*! - \post You must call updateBound() for update the box set. - */ - virtual void setChildTransform(int index, const btTransform & transform) - { - btAssert(m_childTransforms.size() == m_childShapes.size()); - m_childTransforms[index] = transform; - postUpdate(); - } - - //! Determines if this shape has triangles - virtual bool needsRetrieveTriangles() const - { - return false; - } - - //! Determines if this shape has tetrahedrons - virtual bool needsRetrieveTetrahedrons() const - { - return false; - } - - - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const - { - (void) prim_index; (void) triangle; - btAssert(0); - } - - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const - { - (void) prim_index; (void) tetrahedron; - btAssert(0); - } - - - //! Calculates the exact inertia tensor for this shape - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual const char* getName()const - { - return "GImpactCompound"; - } - - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const - { - 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 -- When making operations with this shape, you must call lock before accessing to the trimesh primitives, and then call unlock -- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices. - -*/ -class btGImpactMeshShapePart : public btGImpactShapeInterface -{ -public: - //! Trimesh primitive manager - /*! - Manages the info from btStridingMeshInterface object and controls the Lock/Unlock mechanism - */ - class TrimeshPrimitiveManager:public btPrimitiveManagerBase - { - public: - btScalar m_margin; - btStridingMeshInterface * m_meshInterface; - btVector3 m_scale; - int m_part; - int m_lock_count; - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - TrimeshPrimitiveManager() - { - m_meshInterface = NULL; - m_part = 0; - m_margin = 0.01f; - m_scale = btVector3(1.f,1.f,1.f); - m_lock_count = 0; - vertexbase = 0; - numverts = 0; - stride = 0; - indexbase = 0; - indexstride = 0; - numfaces = 0; - } - - TrimeshPrimitiveManager(const TrimeshPrimitiveManager & manager) - : btPrimitiveManagerBase() - { - m_meshInterface = manager.m_meshInterface; - m_part = manager.m_part; - m_margin = manager.m_margin; - m_scale = manager.m_scale; - m_lock_count = 0; - vertexbase = 0; - numverts = 0; - stride = 0; - indexbase = 0; - indexstride = 0; - numfaces = 0; - - } - - TrimeshPrimitiveManager( - btStridingMeshInterface * meshInterface, int part) - { - m_meshInterface = meshInterface; - m_part = part; - m_scale = m_meshInterface->getScaling(); - m_margin = 0.1f; - m_lock_count = 0; - vertexbase = 0; - numverts = 0; - stride = 0; - indexbase = 0; - indexstride = 0; - numfaces = 0; - - } - - virtual ~TrimeshPrimitiveManager() {} - - void lock() - { - if(m_lock_count>0) - { - m_lock_count++; - return; - } - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &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) - { - --m_lock_count; - return; - } - m_meshInterface->unLockReadOnlyVertexBase(m_part); - vertexbase = NULL; - m_lock_count = 0; - } - - virtual bool is_trimesh() const - { - return true; - } - - virtual int get_primitive_count() const - { - return (int )numfaces; - } - - SIMD_FORCE_INLINE int get_vertex_count() const - { - return (int )numverts; - } - - SIMD_FORCE_INLINE void get_indices(int face_index,unsigned int &i0,unsigned int &i1,unsigned int &i2) const - { - if(indicestype == PHY_SHORT) - { - 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); - 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 - { - 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]); - } - 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]; - } - } - - virtual void get_primitive_box(int prim_index ,btAABB & primbox) const - { - 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); - } - - 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]); - triangle.m_margin = m_margin; - } - - 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]); - triangle.setMargin(m_margin); - } - - }; - - -protected: - TrimeshPrimitiveManager m_primitive_manager; -public: - - btGImpactMeshShapePart() - { - m_box_set.setPrimitiveManager(&m_primitive_manager); - } - - - btGImpactMeshShapePart(btStridingMeshInterface * meshInterface, int part) - { - m_primitive_manager.m_meshInterface = meshInterface; - m_primitive_manager.m_part = part; - m_box_set.setPrimitiveManager(&m_primitive_manager); - } - - virtual ~btGImpactMeshShapePart() - { - } - - //! if true, then its children must get transforms. - virtual bool childrenHasTransform() const - { - return false; - } - - - //! call when reading child shapes - virtual void lockChildShapes() const - { - void * dummy = (void*)(m_box_set.getPrimitiveManager()); - TrimeshPrimitiveManager * dummymanager = static_cast(dummy); - dummymanager->lock(); - } - - virtual void unlockChildShapes() const - { - void * dummy = (void*)(m_box_set.getPrimitiveManager()); - TrimeshPrimitiveManager * dummymanager = static_cast(dummy); - dummymanager->unlock(); - } - - //! Gets the number of children - virtual int getNumChildShapes() const - { - return m_primitive_manager.get_primitive_count(); - } - - - //! Gets the children - virtual btCollisionShape* getChildShape(int index) - { - (void) index; - btAssert(0); - return NULL; - } - - - - //! Gets the child - virtual const btCollisionShape* getChildShape(int index) const - { - (void) index; - btAssert(0); - return NULL; - } - - //! Gets the children transform - virtual btTransform getChildTransform(int index) const - { - (void) index; - btAssert(0); - return btTransform(); - } - - //! Sets the children transform - /*! - \post You must call updateBound() for update the box set. - */ - virtual void setChildTransform(int index, const btTransform & transform) - { - (void) index; - (void) transform; - btAssert(0); - } - - - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const - { - return &m_primitive_manager; - } - - SIMD_FORCE_INLINE TrimeshPrimitiveManager * getTrimeshPrimitiveManager() - { - return &m_primitive_manager; - } - - - - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - - - virtual const char* getName()const - { - return "GImpactMeshShapePart"; - } - - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const - { - return CONST_GIMPACT_TRIMESH_SHAPE_PART; - } - - //! Determines if this shape has triangles - virtual bool needsRetrieveTriangles() const - { - return true; - } - - //! Determines if this shape has tetrahedrons - virtual bool needsRetrieveTetrahedrons() const - { - return false; - } - - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const - { - m_primitive_manager.get_bullet_triangle(prim_index,triangle); - } - - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const - { - (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 - { - m_primitive_manager.get_vertex(vertex_index,vertex); - } - - SIMD_FORCE_INLINE void setMargin(btScalar margin) - { - m_primitive_manager.m_margin = margin; - postUpdate(); - } - - 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 const btVector3& getLocalScaling() const - { - return m_primitive_manager.m_scale; - } - - 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; -}; - - -//! This class manages a mesh supplied by the btStridingMeshInterface interface. -/*! -Set of btGImpactMeshShapePart parts -- Simply create this shape by passing the btStridingMeshInterface to the constructor btGImpactMeshShape, then you must call updateBound() after creating the mesh - -- You can handle deformable meshes with this shape, by calling postUpdate() every time when changing the mesh vertices. - -*/ -class btGImpactMeshShape : public btGImpactShapeInterface -{ - btStridingMeshInterface* m_meshInterface; - -protected: - btAlignedObjectArray m_mesh_parts; - void buildMeshParts(btStridingMeshInterface * meshInterface) - { - for (int i=0;igetNumSubParts() ;++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()); - } - } - -public: - btGImpactMeshShape(btStridingMeshInterface * meshInterface) - { - m_meshInterface = meshInterface; - buildMeshParts(meshInterface); - } - - virtual ~btGImpactMeshShape() - { - int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; - delete part; - } - m_mesh_parts.clear(); - } - - - btStridingMeshInterface* getMeshInterface() - { - return m_meshInterface; - } - - const btStridingMeshInterface* getMeshInterface() const - { - return m_meshInterface; - } - - int getMeshPartCount() const - { - return m_mesh_parts.size(); - } - - btGImpactMeshShapePart * getMeshPart(int index) - { - return m_mesh_parts[index]; - } - - - - const btGImpactMeshShapePart * getMeshPart(int index) const - { - return m_mesh_parts[index]; - } - - - virtual void setLocalScaling(const btVector3& scaling) - { - localScaling = scaling; - - int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; - part->setLocalScaling(scaling); - } - - m_needs_update = true; - } - - virtual void setMargin(btScalar margin) - { - m_collisionMargin = margin; - - int i = m_mesh_parts.size(); - 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() - { - int i = m_mesh_parts.size(); - while(i--) - { - btGImpactMeshShapePart * part = m_mesh_parts[i]; - part->postUpdate(); - } - - m_needs_update = true; - } - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - - //! Obtains the primitive manager - virtual const btPrimitiveManagerBase * getPrimitiveManager() const - { - btAssert(0); - return NULL; - } - - - //! Gets the number of children - virtual int getNumChildShapes() const - { - btAssert(0); - return 0; - } - - - //! if true, then its children must get transforms. - virtual bool childrenHasTransform() const - { - btAssert(0); - return false; - } - - //! Determines if this shape has triangles - virtual bool needsRetrieveTriangles() const - { - btAssert(0); - return false; - } - - //! Determines if this shape has tetrahedrons - virtual bool needsRetrieveTetrahedrons() const - { - btAssert(0); - return false; - } - - virtual void getBulletTriangle(int prim_index,btTriangleShapeEx & triangle) const - { - (void) prim_index; (void) triangle; - btAssert(0); - } - - virtual void getBulletTetrahedron(int prim_index,btTetrahedronShapeEx & tetrahedron) const - { - (void) prim_index; (void) tetrahedron; - btAssert(0); - } - - //! call when reading child shapes - virtual void lockChildShapes() const - { - btAssert(0); - } - - virtual void unlockChildShapes() const - { - 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); - } - - //! Gets the children - virtual btCollisionShape* getChildShape(int index) - { - (void) index; - btAssert(0); - return NULL; - } - - - //! Gets the child - virtual const btCollisionShape* getChildShape(int index) const - { - (void) index; - btAssert(0); - return NULL; - } - - //! Gets the children transform - virtual btTransform getChildTransform(int index) const - { - (void) index; - btAssert(0); - return btTransform(); - } - - //! Sets the children transform - /*! - \post You must call updateBound() for update the box set. - */ - virtual void setChildTransform(int index, const btTransform & transform) - { - (void) index; (void) transform; - btAssert(0); - } - - - virtual eGIMPACT_SHAPE_TYPE getGImpactShapeType() const - { - return CONST_GIMPACT_TRIMESH_SHAPE; - } - - - virtual const char* getName()const - { - return "GImpactMesh"; - } - - 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 processAllTrianglesRay (btTriangleCallback* callback,const btVector3& rayFrom,const btVector3& rayTo) 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; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGImpactMeshShapeData -{ - btCollisionShapeData m_collisionShapeData; - - btStridingMeshInterfaceData m_meshInterface; - - btVector3FloatData m_localScaling; - - float m_collisionMargin; - - int m_gimpactSubType; -}; - -SIMD_FORCE_INLINE int btGImpactMeshShape::calculateSerializeBufferSize() const -{ - return sizeof(btGImpactMeshShapeData); -} - - -#endif //GIMPACT_MESH_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.cpp deleted file mode 100644 index 5d07d1adb..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.cpp +++ /dev/null @@ -1,283 +0,0 @@ -/*! \file btGenericPoolAllocator.cpp -\author Francisco Leon Najera. email projectileman@yahoo.com - -General purpose allocator class -*/ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGenericPoolAllocator.h" - - - -/// *************** btGenericMemoryPool ******************/////////// - -size_t btGenericMemoryPool::allocate_from_free_nodes(size_t num_elements) -{ - size_t ptr = BT_UINT_MAX; - - if(m_free_nodes_count == 0) return BT_UINT_MAX; - // find an avaliable free node with the correct size - size_t revindex = m_free_nodes_count; - - while(revindex-- && ptr == BT_UINT_MAX) - { - if(m_allocated_sizes[m_free_nodes[revindex]]>=num_elements) - { - ptr = revindex; - } - } - if(ptr == BT_UINT_MAX) return BT_UINT_MAX; // not found - - - revindex = ptr; - ptr = m_free_nodes[revindex]; - // post: ptr contains the node index, and revindex the index in m_free_nodes - - size_t finalsize = m_allocated_sizes[ptr]; - finalsize -= num_elements; - - m_allocated_sizes[ptr] = num_elements; - - // post: finalsize>=0, m_allocated_sizes[ptr] has the requested size - - if(finalsize>0) // preserve free node, there are some free memory - { - m_free_nodes[revindex] = ptr + num_elements; - m_allocated_sizes[ptr + num_elements] = finalsize; - } - else // delete free node - { - // swap with end - m_free_nodes[revindex] = m_free_nodes[m_free_nodes_count-1]; - m_free_nodes_count--; - } - - return ptr; -} - -size_t btGenericMemoryPool::allocate_from_pool(size_t num_elements) -{ - if(m_allocated_count+num_elements>m_max_element_count) return BT_UINT_MAX; - - size_t ptr = m_allocated_count; - - m_allocated_sizes[m_allocated_count] = num_elements; - m_allocated_count+=num_elements; - - return ptr; -} - - -void btGenericMemoryPool::init_pool(size_t element_size, size_t element_count) -{ - m_allocated_count = 0; - m_free_nodes_count = 0; - - m_element_size = element_size; - m_max_element_count = element_count; - - - - - m_pool = (unsigned char *) btAlignedAlloc(m_element_size*m_max_element_count,16); - m_free_nodes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); - m_allocated_sizes = (size_t *) btAlignedAlloc(sizeof(size_t)*m_max_element_count,16); - - for (size_t i = 0;i< m_max_element_count;i++ ) - { - m_allocated_sizes[i] = 0; - } -} - -void btGenericMemoryPool::end_pool() -{ - btAlignedFree(m_pool); - btAlignedFree(m_free_nodes); - btAlignedFree(m_allocated_sizes); - m_allocated_count = 0; - m_free_nodes_count = 0; -} - - -//! Allocates memory in pool -/*! -\param size_bytes size in bytes of the buffer -*/ -void * btGenericMemoryPool::allocate(size_t size_bytes) -{ - - size_t module = size_bytes%m_element_size; - size_t element_count = size_bytes/m_element_size; - if(module>0) element_count++; - - size_t alloc_pos = allocate_from_free_nodes(element_count); - // a free node is found - if(alloc_pos != BT_UINT_MAX) - { - return get_element_data(alloc_pos); - } - // allocate directly on pool - alloc_pos = allocate_from_pool(element_count); - - if(alloc_pos == BT_UINT_MAX) return NULL; // not space - return get_element_data(alloc_pos); -} - -bool btGenericMemoryPool::freeMemory(void * pointer) -{ - unsigned char * pointer_pos = (unsigned char *)pointer; - unsigned char * pool_pos = (unsigned char *)m_pool; - // calc offset - if(pointer_pos=get_pool_capacity()) return false;// far away - - // find free position - m_free_nodes[m_free_nodes_count] = offset/m_element_size; - m_free_nodes_count++; - return true; -} - - -/// *******************! btGenericPoolAllocator *******************!/// - - -btGenericPoolAllocator::~btGenericPoolAllocator() -{ - // destroy pools - size_t i; - for (i=0;iend_pool(); - btAlignedFree(m_pools[i]); - } -} - - -// creates a pool -btGenericMemoryPool * btGenericPoolAllocator::push_new_pool() -{ - if(m_pool_count >= BT_DEFAULT_MAX_POOLS) return NULL; - - btGenericMemoryPool * newptr = (btGenericMemoryPool *)btAlignedAlloc(sizeof(btGenericMemoryPool),16); - - m_pools[m_pool_count] = newptr; - - m_pools[m_pool_count]->init_pool(m_pool_element_size,m_pool_element_count); - - m_pool_count++; - return newptr; -} - -void * btGenericPoolAllocator::failback_alloc(size_t size_bytes) -{ - - btGenericMemoryPool * pool = NULL; - - - if(size_bytes<=get_pool_capacity()) - { - pool = push_new_pool(); - } - - if(pool==NULL) // failback - { - return btAlignedAlloc(size_bytes,16); - } - - return pool->allocate(size_bytes); -} - -bool btGenericPoolAllocator::failback_free(void * pointer) -{ - btAlignedFree(pointer); - return true; -} - - -//! Allocates memory in pool -/*! -\param size_bytes size in bytes of the buffer -*/ -void * btGenericPoolAllocator::allocate(size_t size_bytes) -{ - void * ptr = NULL; - - size_t i = 0; - while(iallocate(size_bytes); - ++i; - } - - if(ptr) return ptr; - - return failback_alloc(size_bytes); -} - -bool btGenericPoolAllocator::freeMemory(void * pointer) -{ - bool result = false; - - size_t i = 0; - while(ifreeMemory(pointer); - ++i; - } - - if(result) return true; - - return failback_free(pointer); -} - -/// ************** STANDARD ALLOCATOR ***************************/// - - -#define BT_DEFAULT_POOL_SIZE 32768 -#define BT_DEFAULT_POOL_ELEMENT_SIZE 8 - -// main allocator -class GIM_STANDARD_ALLOCATOR: public btGenericPoolAllocator -{ -public: - GIM_STANDARD_ALLOCATOR():btGenericPoolAllocator(BT_DEFAULT_POOL_ELEMENT_SIZE,BT_DEFAULT_POOL_SIZE) - { - } -}; - -// global allocator -GIM_STANDARD_ALLOCATOR g_main_allocator; - - -void * btPoolAlloc(size_t size) -{ - return g_main_allocator.allocate(size); -} - -void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize) -{ - void * newptr = btPoolAlloc(newsize); - size_t copysize = oldsize -#include -#include -#include "LinearMath/btAlignedAllocator.h" - -#define BT_UINT_MAX UINT_MAX -#define BT_DEFAULT_MAX_POOLS 16 - - -//! Generic Pool class -class btGenericMemoryPool -{ -public: - unsigned char * m_pool; //[m_element_size*m_max_element_count]; - size_t * m_free_nodes; //[m_max_element_count];//! free nodes - size_t * m_allocated_sizes;//[m_max_element_count];//! Number of elements allocated per node - size_t m_allocated_count; - size_t m_free_nodes_count; -protected: - size_t m_element_size; - size_t m_max_element_count; - - size_t allocate_from_free_nodes(size_t num_elements); - size_t allocate_from_pool(size_t num_elements); - -public: - - void init_pool(size_t element_size, size_t element_count); - - void end_pool(); - - - btGenericMemoryPool(size_t element_size, size_t element_count) - { - init_pool(element_size, element_count); - } - - ~btGenericMemoryPool() - { - end_pool(); - } - - - inline size_t get_pool_capacity() - { - return m_element_size*m_max_element_count; - } - - inline size_t gem_element_size() - { - return m_element_size; - } - - inline size_t get_max_element_count() - { - return m_max_element_count; - } - - inline size_t get_allocated_count() - { - return m_allocated_count; - } - - inline size_t get_free_positions_count() - { - return m_free_nodes_count; - } - - inline void * get_element_data(size_t element_index) - { - return &m_pool[element_index*m_element_size]; - } - - //! Allocates memory in pool - /*! - \param size_bytes size in bytes of the buffer - */ - void * allocate(size_t size_bytes); - - bool freeMemory(void * pointer); -}; - - - - -//! Generic Allocator with pools -/*! -General purpose Allocator which can create Memory Pools dynamiacally as needed. -*/ -class btGenericPoolAllocator -{ -protected: - size_t m_pool_element_size; - size_t m_pool_element_count; -public: - btGenericMemoryPool * m_pools[BT_DEFAULT_MAX_POOLS]; - size_t m_pool_count; - - - inline size_t get_pool_capacity() - { - return m_pool_element_size*m_pool_element_count; - } - - -protected: - // creates a pool - btGenericMemoryPool * push_new_pool(); - - void * failback_alloc(size_t size_bytes); - - bool failback_free(void * pointer); -public: - - btGenericPoolAllocator(size_t pool_element_size, size_t pool_element_count) - { - m_pool_count = 0; - m_pool_element_size = pool_element_size; - m_pool_element_count = pool_element_count; - } - - virtual ~btGenericPoolAllocator(); - - //! Allocates memory in pool - /*! - \param size_bytes size in bytes of the buffer - */ - void * allocate(size_t size_bytes); - - bool freeMemory(void * pointer); -}; - - - -void * btPoolAlloc(size_t size); -void * btPoolRealloc(void *ptr, size_t oldsize, size_t newsize); -void btPoolFree(void *ptr); - - -#endif diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btGeometryOperations.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btGeometryOperations.h deleted file mode 100644 index 60f06510a..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btGeometryOperations.h +++ /dev/null @@ -1,212 +0,0 @@ -#ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED -#define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED - -/*! \file btGeometryOperations.h -*\author Francisco Leon Najera - -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btBoxCollision.h" - - - - - -#define PLANEDIREPSILON 0.0000001f -#define PARALELENORMALS 0.000001f - - -#define BT_CLAMP(number,minval,maxval) (numbermaxval?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) -{ - btVector3 planenormal = (e2-e1).cross(normal); - planenormal.normalize(); - 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 n = e2-e1; - cp = v - e1; - btScalar _scalar = cp.dot(n)/n.dot(n); - if(_scalar <0.0f) - { - cp = e1; - } - else if(_scalar >1.0f) - { - cp = e2; - } - else - { - cp = _scalar*n + e1; - } -} - - -//! line plane collision -/*! -*\return - -0 if the ray never intersects - -1 if the ray collides in front - -2 if the ray collides in back -*/ - -SIMD_FORCE_INLINE int bt_line_plane_collision( - 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) - { - returnvalue = 0; - tparam = tmax; - } - 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) -{ - btVector3 AD = vA2 - vA1; - btVector3 BD = vB2 - vB1; - btVector3 N = AD.cross(BD); - btScalar tp = N.length2(); - - 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(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++; - } - } - } - - for ( _k=0;_k0.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; - - 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; - - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; - - return true; -} - -int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle & other, btVector3 * clipped_points ) -{ - // edge 0 - - btVector3 temp_points[MAX_TRI_CLIPPING]; - - - btVector4 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; - - btVector3 temp_points1[MAX_TRI_CLIPPING]; - - - // edge 1 - get_edge_plane(1,edgeplane); - - - 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; -} - -bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle & other, GIM_TRIANGLE_CONTACT & contacts) -{ - btScalar margin = m_margin + other.m_margin; - - btVector3 clipped_points[MAX_TRI_CLIPPING]; - int clipped_count; - //create planes - // plane v vs U points - - GIM_TRIANGLE_CONTACT contacts1; - - contacts1.m_separating_normal = m_plane; - - - clipped_count = clip_triangle(other,clipped_points); - - 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; - - - //Clip tri1 by tri2 edges - GIM_TRIANGLE_CONTACT contacts2; - contacts2.m_separating_normal = other.m_plane; - - clipped_count = other.clip_triangle(*this,clipped_points); - - 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; - - // 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; - - dis2 = bt_distance_point_plane(plane1,m_vertices1[2]) - total_margin; - - if (dis0>0.0f&&dis1>0.0f&&dis2>0.0f) return false; - - return true; -} - - diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.h b/WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.h deleted file mode 100644 index 973c2ed12..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.h +++ /dev/null @@ -1,180 +0,0 @@ -/*! \file btGImpactShape.h -\author Francisco Leon Najera -*/ -/* -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef GIMPACT_TRIANGLE_SHAPE_EX_H -#define GIMPACT_TRIANGLE_SHAPE_EX_H - -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "btBoxCollision.h" -#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]; - - SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT& other) - { - m_penetration_depth = other.m_penetration_depth; - m_separating_normal = other.m_separating_normal; - m_point_count = other.m_point_count; - int i = m_point_count; - while(i--) - { - m_points[i] = other.m_points[i]; - } - } - - GIM_TRIANGLE_CONTACT() - { - } - - GIM_TRIANGLE_CONTACT(const GIM_TRIANGLE_CONTACT& other) - { - copy_from(other); - } - - //! classify points that are closer - void merge_points(const btVector4 & plane, - btScalar margin, const btVector3 * points, int point_count); - -}; - - - -class btPrimitiveTriangle -{ -public: - btVector3 m_vertices[3]; - btVector4 m_plane; - btScalar m_margin; - btScalar m_dummy; - 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]); - normal.normalize(); - m_plane.setValue(normal[0],normal[1],normal[2],m_vertices[0].dot(normal)); - } - - //! Test if triangles could collide - bool overlap_test_conservative(const btPrimitiveTriangle& other); - - //! Calcs the plane which is paralele to the edge and perpendicular to the triangle plane - /*! - \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); - } - - void applyTransform(const btTransform& t) - { - m_vertices[0] = t(m_vertices[0]); - m_vertices[1] = t(m_vertices[1]); - m_vertices[2] = t(m_vertices[2]); - } - - //! Clips the triangle against this - /*! - \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 ); - - //! 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); -}; - - - -//! Helper class for colliding Bullet Triangle Shapes -/*! -This class implements a better getAabb method than the previous btTriangleShape class -*/ -class btTriangleShapeEx: public btTriangleShape -{ -public: - - 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 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 - { - 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); - aabbMin = trianglebox.m_min; - aabbMax = trianglebox.m_max; - } - - void applyTransform(const btTransform& t) - { - m_vertices1[0] = t(m_vertices1[0]); - m_vertices1[1] = t(m_vertices1[1]); - m_vertices1[2] = t(m_vertices1[2]); - } - - SIMD_FORCE_INLINE void buildTriPlane(btVector4 & plane) const - { - 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)); - } - - bool overlap_test_conservative(const btTriangleShapeEx& other); -}; - - -#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_array.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_array.h deleted file mode 100644 index 27e6f32fc..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_array.h +++ /dev/null @@ -1,324 +0,0 @@ -#ifndef GIM_ARRAY_H_INCLUDED -#define GIM_ARRAY_H_INCLUDED -/*! \file gim_array.h -\author Francisco Leon Najera -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - -#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 -class gim_array -{ -public: -//! properties -//!@{ - T *m_data; - GUINT m_size; - GUINT m_allocated_size; -//!@} -//! protected operations -//!@{ - - inline void destroyData() - { - m_allocated_size = 0; - if(m_data==NULL) return; - gim_free(m_data); - m_data = NULL; - } - - inline bool resizeData(GUINT newsize) - { - if(newsize==0) - { - destroyData(); - return true; - } - - if(m_size>0) - { - m_data = (T*)gim_realloc(m_data,m_size*sizeof(T),newsize*sizeof(T)); - } - else - { - m_data = (T*)gim_alloc(newsize*sizeof(T)); - } - m_allocated_size = newsize; - return true; - } - - inline bool growingCheck() - { - 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; - } - return true; - } - -//!@} -//! 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() - { - if(m_size==0)return; - clear_range(0); - } - - inline void clear_memory() - { - clear(); - destroyData(); - } - - gim_array() - { - m_data = 0; - m_size = 0; - m_allocated_size = 0; - } - - gim_array(GUINT reservesize) - { - m_data = 0; - m_size = 0; - - m_allocated_size = 0; - reserve(reservesize); - } - - ~gim_array() - { - clear_memory(); - } - - inline GUINT size() const - { - return m_size; - } - - inline GUINT max_size() const - { - return m_allocated_size; - } - - inline T & operator[](size_t i) - { - return m_data[i]; - } - 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 * get_pointer_at(GUINT i) - { - return m_data + i; - } - - inline const T * get_pointer_at(GUINT i) const - { - return m_data + i; - } - - inline T & at(GUINT i) - { - return m_data[i]; - } - - inline const T & at(GUINT i) const - { - return m_data[i]; - } - - inline T & front() - { - return *m_data; - } - - inline const T & front() const - { - return *m_data; - } - - inline T & back() - { - return m_data[m_size-1]; - } - - inline const T & back() const - { - return m_data[m_size-1]; - } - - - inline void swap(GUINT i, GUINT j) - { - gim_swap_elements(m_data,i,j); - } - - inline void push_back(const T & obj) - { - 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++; - } - - inline void push_back_memcpy(const T & obj) - { - this->growingCheck(); - irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); - m_size++; - } - - inline void pop_back() - { - 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--; - } - - //! 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++; - } - - inline void insert(const T & obj,GUINT index) - { - insert_mem(index); - m_data[index] = obj; - } - - 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 -SIMD_FORCE_INLINE bool POINT_IN_HULL( - const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count) -{ - GREAL _dis; - for (GUINT _i = 0;_i< plane_count;++_i) - { - _dis = DISTANCE_PLANE_POINT(planes[_i],point); - if(_dis>0.0f) return false; - } - return true; -} - -template -SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT( - 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); -} - -enum ePLANE_INTERSECTION_TYPE -{ - G_BACK_PLANE = 0, - G_COLLIDE_PLANE, - G_FRONT_PLANE -}; - -enum eLINE_PLANE_INTERSECTION_TYPE -{ - G_FRONT_PLANE_S1 = 0, - G_FRONT_PLANE_S2, - G_BACK_PLANE_S1, - G_BACK_PLANE_S2, - G_COLLIDE_PLANE_S1, - G_COLLIDE_PLANE_S2 -}; - -//! Confirms if the plane intersect the edge or nor -/*! -intersection type must have the following values -
    -
  • 0 : Segment in front of plane, s1 closest -
  • 1 : Segment in front of plane, s2 closest -
  • 2 : Segment in back of plane, s1 closest -
  • 3 : Segment in back of plane, s2 closest -
  • 4 : Segment collides plane, s1 in back -
  • 5 : Segment collides plane, s2 in back -
-*/ - -template -SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2( - const CLASS_POINT& s1, - const CLASS_POINT &s2, - 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) - { - 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; - } - - 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; -} - -//! Confirms if the plane intersect the edge or not -/*! -clipped1 and clipped2 are the vertices behind the plane. -clipped1 is the closest - -intersection_type must have the following values -
    -
  • 0 : Segment in front of plane, s1 closest -
  • 1 : Segment in front of plane, s2 closest -
  • 2 : Segment in back of plane, s1 closest -
  • 3 : Segment in back of plane, s2 closest -
  • 4 : Segment collides plane, s1 in back -
  • 5 : Segment collides plane, s2 in back -
-*/ -template -SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST( - const CLASS_POINT& s1, - const CLASS_POINT &s2, - const CLASS_PLANE &plane, - CLASS_POINT &clipped1,CLASS_POINT &clipped2) -{ - 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; - } - 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) - -//! Ray plane collision in one way -/*! -Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).
-It uses the PLANEDIREPSILON constant. -*/ -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) -{ - GREAL _dis,_dotdir; - _dotdir = VEC_DOT(plane,vDir); - if(_dotdir -SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION( - 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) - { - returnvalue = 0; - tparam = tmax; - } - - VEC_SCALE(pout,tparam,vDir); - VEC_SUM(pout,vPoint,pout); - return returnvalue; -} - -/*! \brief Returns the Ray on which 2 planes intersect if they do. - Written by Rodrigo Hernandez on ODE convex collision - - \param p1 Plane 1 - \param p2 Plane 2 - \param p Contains the origin of the ray upon returning if planes intersect - \param d Contains the direction of the ray upon returning if planes intersect - \return true if the planes intersect, 0 if paralell. - -*/ -template -SIMD_FORCE_INLINE bool INTERSECT_PLANES( - 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; - 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; - return true; -} - -//***************** SEGMENT and LINE FUNCTIONS **********************************/// - -/*! Finds the closest point(cp) to (v) on a segment (e1,e2) - */ -template -SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT( - 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); - GREAL _scalar = VEC_DOT(cp, _n); - _scalar/= VEC_DOT(_n, _n); - if(_scalar <0.0f) - { - VEC_COPY(cp,e1); - } - else if(_scalar >1.0f) - { - VEC_COPY(cp,e2); - } - else - { - 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 -\param point1 Point of line 1 -\param dir2 Direction of line 2 -\param point2 Point of line 2 -\param t1 Result Parameter for line 1 -\param t2 Result Parameter for line 2 -\param dointersect 0 if the lines won't intersect, else 1 - -*/ -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) -{ - 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; - return true; -} - -//! Find closest points on segments -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) -{ - 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; - - if(_N[0]<_N[1]) - { - if(_M[1]<_M[2]) - { - vPointB = invert_b_order?vB1:vB2; - vPointA = vA1; - } - else if(_M[1]<_M[3]) - { - vPointB = invert_b_order?vB1:vB2; - CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); - } - else - { - vPointA = vA2; - CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); - } - } - else - { - if(_M[3]<_M[0]) - { - vPointB = invert_b_order?vB2:vB1; - vPointA = vA2; - } - else if(_M[3]<_M[1]) - { - vPointA = vA2; - CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2); - } - else - { - vPointB = invert_b_order?vB1:vB2; - CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2); - } - } - return; - } - - - VEC_CROSS(_M,_N,_BD); - _M[3] = VEC_DOT(_M,vB1); - - LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1)); - /*Closest point on segment*/ - VEC_DIFF(vPointB,vPointA,vB1); - _tp = VEC_DOT(vPointB, _BD); - _tp/= VEC_DOT(_BD, _BD); - _tp = GIM_CLAMP(_tp,0.0f,1.0f); - VEC_SCALE(vPointB,_tp,_BD); - VEC_SUM(vPointB,vPointB,vB1); -} - - - - -//! Line box intersection in one dimension -/*! - -*\param pos Position of the ray -*\param dir Projection of the Direction of the ray -*\param bmin Minimum bound of the box -*\param bmax Maximum bound of the box -*\param tfirst the minimum projection. Assign to 0 at first. -*\param tlast the maximum projection. Assign to INFINITY at first. -*\return true if there is an intersection. -*/ -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)) - { - return !(pos < bmin || pos > bmax); - } - GREAL a0 = (bmin - pos) / dir; - GREAL a1 = (bmax - pos) / dir; - 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 -SIMD_FORCE_INLINE void SORT_3_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; - - if(values[i0] < values[i1]) - { - order_indices[1] = i0; - order_indices[2] = i1; - } - else - { - order_indices[1] = i1; - order_indices[2] = i0; - } -} - - - - - -#endif // GIM_VECTOR_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_bitset.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_bitset.h deleted file mode 100644 index 7dee48a4c..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_bitset.h +++ /dev/null @@ -1,123 +0,0 @@ -#ifndef GIM_BITSET_H_INCLUDED -#define GIM_BITSET_H_INCLUDED -/*! \file gim_bitset.h -\author Francisco Leon Najera -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - -#include "gim_array.h" - - -#define GUINT_BIT_COUNT 32 -#define GUINT_EXPONENT 5 - -class gim_bitset -{ -public: - gim_array m_container; - - gim_bitset() - { - - } - - gim_bitset(GUINT bits_count) - { - resize(bits_count); - } - - ~gim_bitset() - { - } - - inline bool resize(GUINT newsize) - { - GUINT oldsize = m_container.size(); - m_container.resize(newsize/GUINT_BIT_COUNT + 1,false); - while(oldsize=size()) - { - resize(bit_index); - } - 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()) - { - return 0; - } - char value = m_container[bit_index >> GUINT_EXPONENT] & - (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))); - } -}; - - - - - -#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_collision.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_collision.h deleted file mode 100644 index 9c572638a..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_collision.h +++ /dev/null @@ -1,588 +0,0 @@ -#ifndef GIM_BOX_COLLISION_H_INCLUDED -#define GIM_BOX_COLLISION_H_INCLUDED - -/*! \file gim_box_collision.h -\author Francisco Leon Najera -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ -#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, -// const btVector3 & pointa, -// const btVector3 & pointb, const btVector3 & extend, -// int dir_index0, -// int dir_index1 -// int component_index0, -// int component_index1) -//{ -// // dir coords are -z and y -// -// const btScalar dir0 = -edge[dir_index0]; -// const btScalar dir1 = edge[dir_index1]; -// btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1; -// btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1; -// //find minmax -// if(pmin>pmax) -// { -// GIM_SWAP_NUMBERS(pmin,pmax); -// } -// //find extends -// const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] + -// extend[component_index1] * absolute_edge[dir_index1]; -// -// if(pmin>rad || -rad>pmax) return false; -// return true; -//} -// -//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis( -// const btVector3 & edge, -// const btVector3 & absolute_edge, -// const btVector3 & pointa, -// const btVector3 & pointb, btVector3 & extend) -//{ -// -// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2); -//} -// -// -//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis( -// const btVector3 & edge, -// const btVector3 & absolute_edge, -// const btVector3 & pointa, -// const btVector3 & pointb, btVector3 & extend) -//{ -// -// return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0); -//} -// -//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis( -// const btVector3 & edge, -// const btVector3 & absolute_edge, -// const btVector3 & pointa, -// const btVector3 & pointb, btVector3 & extend) -//{ -// -// 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)\ - {\ - 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_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 - - 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(); - } - - GIM_BOX_BOX_TRANSFORM_CACHE() - { - } - - - 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) - 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) - { - - m_R1to0 = trans0.getBasis().transpose(); - m_T1to0 = m_R1to0 * (-trans0.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) - { - m_R1to0 = trans0.getBasis().inverse(); - m_T1to0 = m_R1to0 * (-trans0.getOrigin()); - - m_T1to0 += m_R1to0*trans1.getOrigin(); - m_R1to0 *= trans1.getBasis(); - - calc_absolute_matrix(); - } - - SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) - { - return point.dot3(m_R1to0[0], m_R1to0[1], m_R1to0[2]) + m_T1to0; - } -}; - - -#define BOX_PLANE_EPSILON 0.000001f - -//! Axis aligned box -class GIM_AABB -{ -public: - btVector3 m_min; - 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, - 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_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; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - 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,btScalar margin ): - m_min(other.m_min),m_max(other.m_max) - { - m_min[0] -= margin; - m_min[1] -= margin; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - m_max[2] += margin; - } - - SIMD_FORCE_INLINE void invalidate() - { - m_min[0] = G_REAL_INFINITY; - m_min[1] = G_REAL_INFINITY; - m_min[2] = G_REAL_INFINITY; - m_max[0] = -G_REAL_INFINITY; - m_max[1] = -G_REAL_INFINITY; - m_max[2] = -G_REAL_INFINITY; - } - - SIMD_FORCE_INLINE void increment_margin(btScalar margin) - { - m_min[0] -= margin; - m_min[1] -= margin; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - m_max[2] += margin; - } - - SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin) - { - m_min[0] = other.m_min[0] - margin; - m_min[1] = other.m_min[1] - margin; - m_min[2] = other.m_min[2] - margin; - - m_max[0] = other.m_max[0] + margin; - m_max[1] = other.m_max[1] + margin; - m_max[2] = other.m_max[2] + margin; - } - - template - SIMD_FORCE_INLINE void calc_from_triangle( - 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_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 - SIMD_FORCE_INLINE void calc_from_triangle_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_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; - m_min[2] -= margin; - m_max[0] += margin; - m_max[1] += margin; - m_max[2] += margin; - } - - //! Apply a transform to an AABB - SIMD_FORCE_INLINE void appy_transform(const btTransform & trans) - { - 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()); - - m_min = center - textends; - m_max = center + textends; - } - - //! Merges a 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_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) - { - 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]); - } - - //! Gets the extend and center - SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend) const - { - 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 - { - 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]); - } - - - 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]) - { - return false; - } - return true; - } - - /*! \brief Finds the Ray intersection parameter. - \param aabb Aligned box - \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) - { - 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; - btScalar Dy = vorigin[1] - center[1]; - 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; - - - btScalar f = vdir[1] * Dz - vdir[2] * Dy; - 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; - f = vdir[0] * Dy - vdir[1] * Dx; - 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 - { - btVector3 center = (m_max+m_min)*0.5f; - btVector3 extend = m_max-center; - - btScalar _fOrigin = direction.dot(center); - btScalar _fMaximumExtent = extend.dot(direction.absolute()); - vmin = _fOrigin - _fMaximumExtent; - vmax = _fOrigin + _fMaximumExtent; - } - - SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const - { - btScalar _fmin,_fmax; - this->projection_interval(plane,_fmin,_fmax); - - if(plane[3] > _fmax + BOX_PLANE_EPSILON) - { - return G_BACK_PLANE; // 0 - } - - if(plane[3]+BOX_PLANE_EPSILON >=_fmin) - { - return G_COLLIDE_PLANE; //1 - } - return G_FRONT_PLANE;//2 - } - - 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); - return has_collision(tbox); - } - - //! 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) - { - - //Taken from OPCODE - btVector3 ea,eb;//extends - btVector3 ca,cb;//extends - get_center_extend(ca,ea); - box.get_center_extend(cb,eb); - - - btVector3 T; - btScalar t,t2; - int i; - - // Class I : A's basis vectors - for(i=0;i<3;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; - } - // Class II : B's basis vectors - 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; - } - // Class III : 9 cross products - if(fulltest) - { - 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++) - { - 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; - } - } - } - return true; - } - - //! Simple test for planes. - SIMD_FORCE_INLINE bool collide_plane( - const btVector4 & plane) - { - ePLANE_INTERSECTION_TYPE classify = plane_classify(plane); - return (classify == G_COLLIDE_PLANE); - } - - //! 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) - { - if(!collide_plane(triangle_plane)) return false; - - btVector3 center,extends; - this->get_center_extend(center,extends); - - const btVector3 v1(p1 - center); - const btVector3 v2(p2 - center); - const btVector3 v3(p3 - center); - - //First axis - 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 With Y axis - 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); - - - 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 With Y axis - 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); - - 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 With Y axis - 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); - - return true; - } -}; - - -//! Compairison of transformation objects -SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) -{ - 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; - return true; -} - - - -#endif // GIM_BOX_COLLISION_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.cpp deleted file mode 100644 index 0c3d7ba8d..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.cpp +++ /dev/null @@ -1,182 +0,0 @@ - -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - - -#include "gim_box_set.h" - - -GUINT GIM_BOX_TREE::_calc_splitting_axis( - 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; - - for (i=startIndex;i & primitive_boxes, GUINT startIndex, - GUINT endIndex, GUINT splitAxis) -{ - GUINT i; - 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); - 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: - GUINT rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); - - if (unbalanced) - { - splitIndex = startIndex+ (numIndices>>1); - } - - btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex)))); - - return splitIndex; -} - - -void GIM_BOX_TREE::_build_sub_tree(gim_array & primitive_boxes, GUINT startIndex, GUINT endIndex) -{ - GUINT current_index = m_num_nodes++; - - btAssert((endIndex-startIndex)>0); - - 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; - - m_node_array[current_index].m_bound = primitive_boxes[startIndex].m_bound; - m_node_array[current_index].m_data = primitive_boxes[startIndex].m_data; - return; - } - - //configure inner node - - GUINT splitIndex; - - //calc this node bounding box - m_node_array[current_index].m_bound.invalidate(); - for (splitIndex=startIndex;splitIndex & primitive_boxes) -{ - // initialize node count to 0 - m_num_nodes = 0; - // allocate nodes - m_node_array.resize(primitive_boxes.size()*2); - - _build_sub_tree(primitive_boxes, 0, primitive_boxes.size()); -} - - diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.h deleted file mode 100644 index 61d190a7d..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.h +++ /dev/null @@ -1,674 +0,0 @@ -#ifndef GIM_BOX_SET_H_INCLUDED -#define GIM_BOX_SET_H_INCLUDED - -/*! \file gim_box_set.h -\author Francisco Leon Najera -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - - -#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; - } -}; - -//! A pairset array -class gim_pair_set: public gim_array -{ -public: - gim_pair_set():gim_array(32) - { - } - inline void push_pair(GUINT index1,GUINT index2) - { - push_back(GIM_PAIR(index1,index2)); - } - - inline void push_pair_inv(GUINT index1,GUINT index2) - { - push_back(GIM_PAIR(index2,index1)); - } -}; - - -//! Prototype Base class for primitive classification -/*! -This class is a wrapper for primitive collections. -This tells relevant info for the Bounding Box set classes, which take care of space classification. -This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons. -*/ -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; -}; - - -struct GIM_AABB_DATA -{ - GIM_AABB m_bound; - GUINT m_data; -}; - -//! Node Structure for trees -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 - - GIM_BOX_TREE_NODE() - { - 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); - } -}; - -//! Basic Box tree structure -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); - - GUINT _calc_splitting_axis(gim_array & primitive_boxes, GUINT startIndex, GUINT endIndex); - - void _build_sub_tree(gim_array & primitive_boxes, GUINT startIndex, GUINT endIndex); -public: - GIM_BOX_TREE() - { - m_num_nodes = 0; - } - - //! prototype functions for box tree management - //!@{ - void build_tree(gim_array & primitive_boxes); - - SIMD_FORCE_INLINE void clearNodes() - { - m_node_array.clear(); - m_num_nodes = 0; - } - - //! node count - SIMD_FORCE_INLINE GUINT getNodeCount() const - { - return m_num_nodes; - } - - //! tells if the node is a leaf - SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const - { - return m_node_array[nodeindex].is_leaf_node(); - } - - SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const - { - return m_node_array[nodeindex].m_data; - } - - 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) - { - m_node_array[nodeindex].m_bound = bound; - } - - SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const - { - return m_node_array[nodeindex].m_left; - } - - SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const - { - return m_node_array[nodeindex].m_right; - } - - SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const - { - return m_node_array[nodeindex].m_escapeIndex; - } - - //!@} -}; - - -//! 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 -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--) - { - if(isLeafNode(nodecount)) - { - GIM_AABB 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); - //get right bound - childindex = getRightNodeIndex(nodecount); - GIM_AABB bound2; - getNodeBound(childindex,bound2); - bound.merge(bound2); - - setNodeBound(nodecount,bound); - } - } - } -public: - - GIM_BOX_TREE_TEMPLATE_SET() - { - } - - 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) - { - m_primitive_manager = primitive_manager; - } - - const _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() const - { - return m_primitive_manager; - } - - _GIM_PRIMITIVE_MANAGER_PROTOTYPE & getPrimitiveManager() - { - return m_primitive_manager; - } - -//! node manager prototype functions -///@{ - - //! this attemps to refit the box set. - SIMD_FORCE_INLINE void update() - { - refit(); - } - - //! this rebuild the entire set - SIMD_FORCE_INLINE void buildSet() - { - //obtain primitive boxes - gim_array primitive_boxes; - primitive_boxes.resize(m_primitive_manager.get_primitive_count(),false); - - for (GUINT i = 0;i & collided_results) const - { - GUINT curIndex = 0; - GUINT numNodes = getNodeCount(); - - while (curIndex < numNodes) - { - GIM_AABB bound; - getNodeBound(curIndex,bound); - - //catch bugs in tree data - - bool aabbOverlap = bound.has_collision(box); - bool isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData(curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getScapeNodeIndex(curIndex); - } - } - 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 - { - GIM_AABB transbox=box; - transbox.appy_transform(transform); - 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 - { - GUINT curIndex = 0; - GUINT numNodes = getNodeCount(); - - while (curIndex < numNodes) - { - GIM_AABB bound; - getNodeBound(curIndex,bound); - - //catch bugs in tree data - - bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir); - bool isleafnode = isLeafNode(curIndex); - - if (isleafnode && aabbOverlap) - { - collided_results.push_back(getNodeData( curIndex)); - } - - if (aabbOverlap || isleafnode) - { - //next subnode - curIndex++; - } - else - { - //skip node - curIndex+= getScapeNodeIndex(curIndex); - } - } - if(collided_results.size()>0) return true; - return false; - } - - //! tells if this set has hierarcht - SIMD_FORCE_INLINE bool hasHierarchy() const - { - return true; - } - - //! tells if this set is a trimesh - SIMD_FORCE_INLINE bool isTrimesh() const - { - return m_primitive_manager.is_trimesh(); - } - - //! node count - SIMD_FORCE_INLINE GUINT getNodeCount() const - { - return m_box_tree.getNodeCount(); - } - - //! tells if the node is a leaf - SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const - { - return m_box_tree.isLeafNode(nodeindex); - } - - SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const - { - return m_box_tree.getNodeData(nodeindex); - } - - 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) - { - m_box_tree.setNodeBound(nodeindex, bound); - } - - SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const - { - return m_box_tree.getLeftNodeIndex(nodeindex); - } - - SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const - { - return m_box_tree.getRightNodeIndex(nodeindex); - } - - SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const - { - return m_box_tree.getScapeNodeIndex(nodeindex); - } - - SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex,GIM_TRIANGLE & triangle) const - { - 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> -{ -public: - -}; - - - - - -/// GIM_BOX_SET collision methods -template -class GIM_TREE_TREE_COLLIDER -{ -public: - 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; - bool node1_is_leaf; - bool t0_is_trimesh; - bool t1_is_trimesh; - bool node0_has_triangle; - bool node1_has_triangle; - GIM_AABB m_box0; - GIM_AABB m_box1; - GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0; - btTransform trans_cache_0to1; - GIM_TRIANGLE m_tri0; - btVector4 m_tri0_plane; - 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); - //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]); - m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]); - m_tri0.get_plane(m_tri0_plane); - - node0_has_triangle = true; - } - - SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1) - { - 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]); - m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]); - m_tri1.get_plane(m_tri1_plane); - - node1_has_triangle = true; - } - - SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0) - { - 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; - } - - SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1) - { - 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) - { - 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; - - if(t0_is_trimesh && node0_is_leaf) - { - //perform primitive vs box collision - retrieve_node0_triangle(node0); - //do triangle vs box collision - 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_box1.increment_margin(-m_tri0.m_margin); - - if(!result) return false; - return true; - } - else if(t1_is_trimesh && node1_is_leaf) - { - //perform primitive vs box collision - retrieve_node1_triangle(node1); - //do triangle vs box collision - 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_box0.increment_margin(-m_tri1.m_margin); - - if(!result) return false; - return true; - } - return true; - } - - //stackless collision routine - void find_collision_pairs() - { - gim_pair_set stack_collisions; - stack_collisions.reserve(32); - - //add the first pair - stack_collisions.push_pair(0,0); - - - 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(node0_is_leaf) - { - if(node1_is_leaf) - { - 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)); - - //collide right - stack_collisions.push_pair(node0,m_boxset1->getRightNodeIndex(node1)); - } - } - else - { - if(node1_is_leaf) - { - //collide left - stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0),node1); - //collide right - stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0),node1); - } - else - { - GUINT left0 = m_boxset0->getLeftNodeIndex(node0); - GUINT right0 = m_boxset0->getRightNodeIndex(node0); - GUINT left1 = m_boxset1->getLeftNodeIndex(node1); - GUINT right1 = m_boxset1->getRightNodeIndex(node1); - //collide left - stack_collisions.push_pair(left0,left1); - //collide right - stack_collisions.push_pair(left0,right1); - //collide left - stack_collisions.push_pair(right0,left1); - //collide right - stack_collisions.push_pair(right0,right1); - - }// else if node1 is not a leaf - }// else if node0 is not a leaf - - }// 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) - { - m_collision_pairs = &collision_pairs; - m_boxset0 = boxset1; - m_boxset1 = boxset2; - - trans_cache_1to0.calc_from_homogenic(trans1,trans2); - - trans_cache_0to1 = trans2.inverse(); - trans_cache_0to1 *= trans1; - - - if(complete_primitive_tests) - { - t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh(); - t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh(); - } - else - { - t0_is_trimesh = false; - t1_is_trimesh = false; - } - - find_collision_pairs(); - } -}; - - -#endif // GIM_BOXPRUNING_H_INCLUDED - - diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_clip_polygon.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_clip_polygon.h deleted file mode 100644 index e342459ce..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_clip_polygon.h +++ /dev/null @@ -1,210 +0,0 @@ -#ifndef GIM_CLIP_POLYGON_H_INCLUDED -#define GIM_CLIP_POLYGON_H_INCLUDED - -/*! \file gim_tri_collision.h -\author Francisco Leon Najera -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - - -//! 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) - { - return DISTANCE_PLANE_POINT(plane, point); - } -}; - - - -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) -{ - 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); - clipped_count++; - } - if(!_classif) - { - VEC_COPY(clipped[clipped_count],point1); - clipped_count++; - } -} - - -//! Clips a polygon by a plane -/*! -*\return The count of the clipped counts -*/ -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) -{ - GUINT clipped_count = 0; - - - //clip first point - GREAL firstdist = distance_func(plane,polygon_points[0]);; - if(!(firstdist>G_EPSILON)) - { - VEC_COPY(clipped[clipped_count],polygon_points[0]); - clipped_count++; - } - - GREAL olddist = firstdist; - for(GUINT _i=1;_i -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) -{ - GUINT clipped_count = 0; - - //clip first point - GREAL firstdist = distance_func(plane,point0);; - if(!(firstdist>G_EPSILON)) - { - VEC_COPY(clipped[clipped_count],point0); - clipped_count++; - } - - // point 1 - GREAL olddist = firstdist; - GREAL dist = distance_func(plane,point1); - - PLANE_CLIP_POLYGON_COLLECT( - point0,point1, - olddist, - dist, - clipped, - clipped_count); - - olddist = dist; - - - // point 2 - dist = distance_func(plane,point2); - - PLANE_CLIP_POLYGON_COLLECT( - point1,point2, - olddist, - dist, - clipped, - clipped_count); - olddist = dist; - - - - //RETURN TO FIRST point - PLANE_CLIP_POLYGON_COLLECT( - point2,point0, - olddist, - firstdist, - clipped, - clipped_count); - - return clipped_count; -} - - -template -SIMD_FORCE_INLINE GUINT PLANE_CLIP_POLYGON3D( - 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()); -} - - -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) -{ - return PLANE_CLIP_TRIANGLE_GENERIC(plane,point0,point1,point2,clipped,DISTANCE_PLANE_3D_FUNC()); -} - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.cpp deleted file mode 100644 index 20e41de08..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.cpp +++ /dev/null @@ -1,146 +0,0 @@ - -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - -#include "gim_contact.h" - -#define MAX_COINCIDENT 8 - -void gim_contact_array::merge_contacts( - const gim_contact_array & contacts, bool normal_contact_average) -{ - clear(); - - if(contacts.size()==1) - { - push_back(contacts.back()); - return; - } - - gim_array keycontacts(contacts.size()); - keycontacts.resize(contacts.size(),false); - - //fill key contacts - - GUINT i; - - for (i = 0;im_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//) - { - *pcontact = *scontact; - coincident_count = 0; - } - else if(normal_contact_average) - { - if(btFabs(pcontact->m_depth - scontact->m_depth)m_normal; - coincident_count++; - } - } - } - } - else - {//add new contact - - if(normal_contact_average && coincident_count>0) - { - pcontact->interpolate_normals(coincident_normals,coincident_count); - coincident_count = 0; - } - - push_back(*scontact); - pcontact = &back(); - } - last_key = key; - } -} - -void gim_contact_array::merge_contacts_unique(const gim_contact_array & contacts) -{ - clear(); - - if(contacts.size()==1) - { - push_back(contacts.back()); - return; - } - - GIM_CONTACT average_contact = contacts.back(); - - for (GUINT i=1;i -{ -public: - gim_contact_array():gim_array(64) - { - } - - 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(); - newele.m_point = point; - newele.m_normal = normal; - newele.m_depth = depth; - newele.m_feature1 = feature1; - newele.m_feature2 = feature2; - } - - SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT_DATA & tricontact, - GUINT feature1,GUINT feature2) - { - for(GUINT i = 0;i -struct 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(GUINT key, const T & data) - { - m_key = key; - m_data = data; - } - - bool operator <(const GIM_HASH_TABLE_NODE & other) const - { - ///inverse order, further objects are first - if(m_key < other.m_key) return true; - return false; - } - - bool operator >(const GIM_HASH_TABLE_NODE & other) const - { - ///inverse order, further objects are first - if(m_key > other.m_key) return true; - return false; - } - - bool operator ==(const GIM_HASH_TABLE_NODE & other) const - { - ///inverse order, further objects are first - if(m_key == other.m_key) return true; - return false; - } -}; - -///Macro for getting the key -class GIM_HASH_NODE_GET_KEY -{ -public: - 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) - { - return ((int)(a.m_key - key)); - } -}; - -///Macro for comparing Hash nodes -class GIM_HASH_NODE_CMP_MACRO -{ -public: - 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) -{ - if(array_count - -
    -
  • 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 -
- -*/ -template -class gim_hash_table -{ -protected: - 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; - - ///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= 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); - - m_hash_table[cell_index] = GIM_INVALID_HASH; - } - - 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; - - //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; - - return this->_erase_unsorted(index); - } - - - - //! 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; - } - - GUINT cell_index = _assign_hash_table_cell(hashkey); - - GUINT value_key = m_hash_table[cell_index]; - - if(value_key!= GIM_INVALID_HASH) return value_key;// Not overrited - - m_hash_table[cell_index] = m_nodes.size(); - - _insert_unsorted(hashkey,value); - return GIM_INVALID_HASH; - } - - //! 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; - } - - GUINT cell_index = _assign_hash_table_cell(hashkey); - - 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 - } - - m_hash_table[cell_index] = m_nodes.size(); - - _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; - } - - //! 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(); - } - - //! 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(); - - 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(); - - 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; - } - - - -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; - - 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(); - } - - 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; - } - - 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; - } - - 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; - - if(!(m_nodes.size()< m_min_hash_table_size)) - { - if(m_node_size == 0) - { - m_node_size = GIM_DEFAULT_HASH_TABLE_NODE_SIZE; - } - - _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 - /*! - */ - 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 T& operator[](GUINT index) - { - return m_nodes[index].m_data; - } - - //! 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]; - } - 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; - last_index--; - _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()); - - - if(found) return result_ind; - } - return GIM_INVALID_HASH; - } - - //! 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 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; - } - - - - 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; - - if(m_hash_table) - { - return this->_erase_hash_table(hashkey); - } - //Binary search - - 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; - } - - 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); - } - - //! 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(); - } - - - - //! 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); - } - - -}; - - - -#endif // GIM_CONTAINERS_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_linear_math.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_linear_math.h deleted file mode 100644 index 64f11b495..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_linear_math.h +++ /dev/null @@ -1,1573 +0,0 @@ -#ifndef GIM_LINEAR_H_INCLUDED -#define GIM_LINEAR_H_INCLUDED - -/*! \file gim_linear_math.h -*\author Francisco Leon Najera -Type Independant Vector and matrix operations. -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - - -#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; \ -}\ - - -//! Zero out a 3D vector -#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; \ -}\ - - -/// Vector copy -#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]; \ -}\ - - -/// 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]; \ -}\ - -/// 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]);\ -}\ - -/// Vector difference -#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]; \ -}\ - - -/// 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]; \ -}\ - - -/// Vector sum -#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]; \ -}\ - - -/// 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]; \ -}\ - - -/// scalar times vector -#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]; \ -}\ - - -/// 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]; \ -}\ - - -/// accumulate scaled vector -#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]; \ -}\ - - -/// 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]; \ -}\ - - -/// Vector dot product -#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]) - -/// 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]) - -/// vector impact parameter (squared) -#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); \ -}\ - -/// Vector length -#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);\ -}\ - - -/// Vector length -#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);\ -}\ - - -/// Vector inv length -#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);\ -}\ - - - -/// distance between two points -#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);\ -}\ - - -/// 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);\ -}\ - -//! 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_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])) - - -/// 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; \ -}\ - - - - - - -/// 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; \ -}\ - -/*! 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; \ -}\ - -/*! 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; \ -}\ - -/*! 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; \ -}\ - -/*! 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; \ -}\ - -/*! 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; \ -}\ - -/*! 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]; \ - \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - -/*! 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]; \ - \ -}\ - -/*! 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]; \ -}\ - - -/*! 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];\ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - - -/*! 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]; \ -}\ - -//! 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];\ -}\ - - - -/** inverse transpose of matrix times vector - * - * This macro computes inverse transpose of matrix m, - * and multiplies vector v into it, to yeild vector p - * - * DANGER !!! Do Not use this on normal vectors!!! - * 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; \ - } \ -}\ - - -/** transform normal vector by inverse transpose of matrix - * and then renormalize the vector - * - * This macro computes inverse transpose of matrix m, - * 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); \ - } \ -}\ - - -/** 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]; \ -}\ - - -/** 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]; \ -}\ - - -/** 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]; \ -}\ - - -/** 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]; \ -}\ - - -/** 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]; \ -}\ - - -/** 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]; \ -}\ - - -/** 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]; \ -}\ - - -/** 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]); \ -}\ - - -/** 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); \ -}\ - - - -#endif // GIM_VECTOR_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_math.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_math.h deleted file mode 100644 index 939079e10..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_math.h +++ /dev/null @@ -1,157 +0,0 @@ -#ifndef GIM_MATH_H_INCLUDED -#define GIM_MATH_H_INCLUDED -/*! \file gim_math.h -\author Francisco Leon Najera -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - -#include "LinearMath/btScalar.h" - - - -#define GREAL btScalar -#define GREAL2 double -#define GINT int -#define GUINT unsigned int -#define GSHORT short -#define GUSHORT unsigned short -#define GINT64 long long -#define GUINT64 unsigned long long - - - -#define G_PI 3.14159265358979f -#define G_HALF_PI 1.5707963f -//267948966 -#define G_TWO_PI 6.28318530f -//71795864 -#define G_ROOT3 1.73205f -#define G_ROOT2 1.41421f -#define G_UINT_INFINITY 0xffffffff //!< A very very high value -#define G_REAL_INFINITY FLT_MAX -#define G_SIGN_BITMASK 0x80000000 -#define G_EPSILON SIMD_EPSILON - - - -enum GIM_SCALAR_TYPES -{ - G_STYPE_REAL =0, - G_STYPE_REAL2, - G_STYPE_SHORT, - G_STYPE_USHORT, - G_STYPE_INT, - G_STYPE_UINT, - G_STYPE_INT64, - G_STYPE_UINT64 -}; - - - -#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)) - -//! Signed integer representation of a floating-point value. -#define GIM_SIR(x) ((GINT&)(x)) - -//! Absolute integer representation of a floating-point value -#define GIM_AIR(x) (GIM_IR(x)&0x7fffffff) - -//! Floating-point representation of an integer value. -#define GIM_FR(x) ((GREAL&)(x)) - -#define GIM_MAX(a,b) (ab?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_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)) - -///returns a clamped number -#define GIM_CLAMP(number,minval,maxval) (numbermaxval?maxval:number)) - -#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_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;\ -}\ - -//! 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; -} - -inline GREAL gim_sqrt(GREAL f) -{ - GREAL r; - GIM_SQRT(f,r); - return r; -} - - - -#endif // GIM_MATH_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.cpp deleted file mode 100644 index 1636eb786..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - - -#include "gim_memory.h" -#include "stdlib.h" - -#ifdef GIM_SIMD_MEMORY -#include "LinearMath/btAlignedAllocator.h" -#endif - -static gim_alloc_function *g_allocfn = 0; -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) -{ - g_allocfn = fn; -} - -void gim_set_alloca_handler (gim_alloca_function *fn) -{ - g_allocafn = fn; -} - -void gim_set_realloc_handler (gim_realloc_function *fn) -{ - g_reallocfn = fn; -} - -void gim_set_free_handler (gim_free_function *fn) -{ - g_freefn = fn; -} - -gim_alloc_function *gim_get_alloc_handler() -{ - return g_allocfn; -} - -gim_alloca_function *gim_get_alloca_handler() -{ - return g_allocafn; -} - - -gim_realloc_function *gim_get_realloc_handler () -{ - return g_reallocfn; -} - - -gim_free_function *gim_get_free_handler () -{ - return g_freefn; -} - - -void * gim_alloc(size_t size) -{ - void * ptr; - if (g_allocfn) - { - ptr = g_allocfn(size); - } - else - { -#ifdef GIM_SIMD_MEMORY - ptr = btAlignedAlloc(size,16); -#else - ptr = malloc(size); -#endif - } - return ptr; -} - -void * gim_alloca(size_t 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 * newptr = gim_alloc(newsize); - size_t copysize = oldsize - -#ifdef PREFETCH -#include // for prefetch -#define pfval 64 -#define pfval2 128 -//! Prefetch 64 -#define pf(_x,_i) _mm_prefetch((void *)(_x + _i + pfval), 0) -//! Prefetch 128 -#define pf2(_x,_i) _mm_prefetch((void *)(_x + _i + pfval2), 0) -#else -//! Prefetch 64 -#define pf(_x,_i) -//! Prefetch 128 -#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_=SIMD_T_SIZE) - { - *(ui_dst_ptr++) = *(ui_src_ptr++); - copysize-=SIMD_T_SIZE; - } - if(copysize==0) 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); -#endif -} - - - -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) -{ - 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)); -} - -template -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); -} - -#endif // GIM_MEMORY_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_radixsort.h b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_radixsort.h deleted file mode 100644 index c246ef125..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_radixsort.h +++ /dev/null @@ -1,406 +0,0 @@ -#ifndef GIM_RADIXSORT_H_INCLUDED -#define GIM_RADIXSORT_H_INCLUDED -/*! \file gim_radixsort.h -\author Francisco Leon Najera. -Based on the work of Michael Herf : "fast floating-point radix sort" -Avaliable on http://www.stereopsis.com/radix.html -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - -#include "gim_memory.h" - -///Macros for sorting. -//! Prototype for comparators -class less_comparator -{ - public: - - template - inline int operator() ( const T& a, const Z& b ) - { - return ( ab?1:0)); - } -}; - -//! Prototype for comparators -class integer_comparator -{ - public: - - template - inline int operator() ( const T& a, const T& b ) - { - return (int)(a-b); - } -}; - -//!Prototype for getting the integer representation of an object -class uint_key_func -{ -public: - 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) - { - a = b; - } -}; - -//!Prototype for copying elements -class memcopy_elements_func -{ -public: - template - inline void operator()(T& a,T& b) - { - 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; - } - - inline bool operator <(const GIM_RSORT_TOKEN& other) const - { - return (m_key < other.m_key); - } - - inline bool operator >(const GIM_RSORT_TOKEN& other) const - { - return (m_key > other.m_key); - } -}; - -//! Prototype for comparators -class GIM_RSORT_TOKEN_COMPARATOR -{ - 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 ) - - - -///Radix sort for unsigned integer keys -inline void gim_radix_sort_rtokens( - 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; - for (i = 0; i < kHist * 3; ++i) - { - b0[i] = 0; - } - GUINT fi; - 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)] ++; - } - { - GUINT sum0 = 0, sum1 = 0, sum2 = 0; - GUINT tsum; - for (i = 0; i < kHist; ++i) - { - tsum = b0[i] + sum0; - b0[i] = sum0 - 1; - sum0 = tsum; - tsum = b1[i] + sum1; - b1[i] = sum1 - 1; - sum1 = tsum; - tsum = b2[i] + sum2; - b2[i] = sum2 - 1; - sum2 = tsum; - } - } - for (i = 0; i < element_count; ++i) - { - fi = array[i].m_key; - pos = D11_0(fi); - pos = ++b0[pos]; - sorted[pos].m_key = array[i].m_key; - sorted[pos].m_value = array[i].m_value; - } - for (i = 0; i < element_count; ++i) - { - fi = sorted[i].m_key; - pos = D11_1(fi); - pos = ++b1[pos]; - array[pos].m_key = sorted[i].m_key; - array[pos].m_value = sorted[i].m_value; - } - for (i = 0; i < element_count; ++i) - { - fi = array[i].m_key; - pos = D11_2(fi); - pos = ++b2[pos]; - sorted[pos].m_key = array[i].m_key; - sorted[pos].m_value = array[i].m_value; - } -} - - - - -/// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN -/*! -*\param array Array of elements to sort -*\param sorted_tokens Tokens of sorted elements -*\param element_count element count -*\param uintkey_macro Functor which retrieves the integer representation of an array element -*/ -template -void gim_radix_sort_array_tokens( - 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 -void gim_radix_sort( - 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) -{ - GUINT _k; - int _comp_result; - GUINT _i = _start_i; - GUINT _j = _end_i+1; - while (_i < _j) - { - _k = (_j+_i-1)/2; - _comp_result = _comp_macro(_array[_k], _search_key); - if (_comp_result == 0) - { - _result_index = _k; - return true; - } - else if (_comp_result < 0) - { - _i = _k+1; - } - else - { - _j = _k; - } - } - _result_index = _i; - 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. -\param _array -\param _start_i the beginning of the array -\param _end_i the ending index of the array -\param _search_key Value to find -\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 -bool gim_binary_search( - 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 _k; - while(_i < _j) - { - _k = (_j+_i-1)/2; - if(_array[_k]==_search_key) - { - _result_index = _k; - return true; - } - else if (_array[_k]<_search_key) - { - _i = _k+1; - } - else - { - _j = _k; - } - } - _result_index = _i; - 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) -{ - /* 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 < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0) - { - child++; - } - /* pick larger child */ - if (CompareFunc(temp , pArr[child - 1])<0) - { - /* move child up */ - pArr[k - 1] = pArr[child - 1]; - k = child; - } - else - { - break; - } - } - pArr[k - 1] = temp; -} /*downHeap*/ - - -template -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--) - { - gim_down_heap(pArr, k, n, CompareFunc); - } - - /* a[1..N] is now a heap */ - while ( n>=2 ) - { - 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 diff --git a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.cpp b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.cpp deleted file mode 100644 index f9727e1d5..000000000 --- a/WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.cpp +++ /dev/null @@ -1,640 +0,0 @@ - -/*! \file gim_tri_collision.h -\author Francisco Leon Najera -*/ -/* ------------------------------------------------------------------------------ -This source file is part of GIMPACT Library. - -For the latest info, see http://gimpact.sourceforge.net/ - -Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. -email: projectileman@yahoo.com - - This library is free software; you can redistribute it and/or - modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file GIMPACT-LICENSE-LGPL.TXT. - (2) The BSD-style license that is included with this library in - the file GIMPACT-LICENSE-BSD.TXT. - (3) The zlib/libpng license that is included with this library in - the file GIMPACT-LICENSE-ZLIB.TXT. - - 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 files - GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. - ------------------------------------------------------------------------------ -*/ - -#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; - btVector3 tu_vertices[3]; - btVector3 tv_vertices[3]; - btVector4 tu_plane; - btVector4 tv_plane; - btVector3 closest_point_u; - btVector3 closest_point_v; - btVector3 edge_edge_dir; - btVector3 distances; - GREAL du[4]; - GREAL du0du1; - GREAL du0du2; - GREAL dv[4]; - GREAL dv0dv1; - 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) - { - 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; - } - 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; - } - 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; - } - else - { - return false; - } - return true; - } - - - //! clip triangle - /*! - */ - SIMD_FORCE_INLINE GUINT clip_triangle( - 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); - - GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( - edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); - - if(clipped_count == 0) return 0; - - // edge 1 - - EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); - - clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points,clipped_count,temp_points1); - - if(clipped_count == 0) return 0; - - // edge 2 - - EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); - - clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points1,clipped_count,clip_points); - - return clipped_count; - - - /*GUINT i0 = (tri_plane.closestAxis()+1)%3; - GUINT i1 = (i0+1)%3; - // edge 0 - btVector3 temp_points[MAX_TRI_CLIPPING]; - btVector3 temp_points1[MAX_TRI_CLIPPING]; - - GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC( - 0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points, - DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1)); - - - if(clipped_count == 0) return 0; - - // edge 1 - clipped_count = PLANE_CLIP_POLYGON_GENERIC( - 0,temp_points,clipped_count,temp_points1, - DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1)); - - if(clipped_count == 0) return 0; - - // edge 2 - clipped_count = PLANE_CLIP_POLYGON_GENERIC( - 0,temp_points1,clipped_count,clipped_points, - DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1)); - - return clipped_count;*/ - } - - SIMD_FORCE_INLINE void sort_isect( - GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1) - { - if(isect1=isect_v[1]) // face U casts face V - { - return 1; - } - else if(isect_v[0]<=isect_u[0]) // face V casts face U - { - return 2; - } - // closest points - closest_point_u = up_e1; - 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 - { - return 2; - } - else if(isect_u[0]<=isect_v[0]) // face U casts face V - { - return 1; - } - // closest points - closest_point_u = up_e0; - 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(du[0]<0) //we need test behind the triangle plane - { - distances[0] = GIM_MAX3(du[0],du[1],du[2]); - distances[0] = -distances[0]; - 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); - } - else - { - 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 - - 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); - - 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]); - } - } - - - // plane U vs V points - - 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]); - - 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(dv[0]<0) //we need test behind the triangle plane - { - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); - distances[1] = -distances[1]; - 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); - } - else - { - 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 - - 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); - - 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]); - } - } - - GUINT bl; - /* bl = cross_line_intersection_test(); - if(bl==3) - { - //take edge direction too - bl = distances.maxAxis(); - } - else - {*/ - bl = 0; - if(distances[0]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); - - return true; - } - - //clip face against other - - - GUINT point_count; - //TODO - 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); - } - 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); - contacts.m_separating_normal *= -1.f; - } - if(contacts.m_point_count == 0) return false; - return true; - } - -}; - - -/*class GIM_TRIANGLE_CALCULATION_CACHE -{ -public: - GREAL margin; - GUINT clipped_count; - btVector3 tu_vertices[3]; - btVector3 tv_vertices[3]; - btVector3 temp_points[MAX_TRI_CLIPPING]; - btVector3 temp_points1[MAX_TRI_CLIPPING]; - btVector3 clipped_points[MAX_TRI_CLIPPING]; - GIM_TRIANGLE_CONTACT_DATA contacts1; - GIM_TRIANGLE_CONTACT_DATA contacts2; - - - //! clip triangle - GUINT clip_triangle( - const btVector4 & tri_plane, - const btVector3 * tripoints, - const btVector3 * srcpoints, - btVector3 * clipped_points) - { - // edge 0 - - btVector4 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); - - if(clipped_count == 0) return 0; - - // edge 1 - - EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); - - clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points,clipped_count,temp_points1); - - if(clipped_count == 0) return 0; - - // edge 2 - - EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); - - clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points1,clipped_count,clipped_points); - - return clipped_count; - } - - - - - //! collides only on one side - bool triangle_collision( - const btVector3 & u0, - const btVector3 & u1, - const btVector3 & u2, - GREAL margin_u, - const btVector3 & v0, - const btVector3 & v1, - const btVector3 & v2, - GREAL margin_v, - GIM_TRIANGLE_CONTACT_DATA & contacts) - { - - margin = margin_u + margin_v; - - - tu_vertices[0] = u0; - tu_vertices[1] = u1; - tu_vertices[2] = u2; - - tv_vertices[0] = v0; - tv_vertices[1] = v1; - tv_vertices[2] = v2; - - //create planes - // plane v vs U points - - - TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal); - - clipped_count = clip_triangle( - contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points); - - 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 triangle1 - //contacts1.m_separating_normal *= -1.f; - - //Clip tri1 by tri2 edges - - TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal); - - clipped_count = clip_triangle( - contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points); - - 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 - - contacts2.m_separating_normal *= -1.f; - - ////check most dir for contacts - if(contacts2.m_penetration_depth - 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>m_penetration_depth) - { - m_penetration_depth = _dist; - point_indices[0] = _k; - m_point_count=1; - } - else if((_dist+G_EPSILON)>=m_penetration_depth) - { - point_indices[m_point_count] = _k; - m_point_count++; - } - } - } - - for( _k=0;_k u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1] - - --> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1] - - --> u*(axe1[i1] - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2] - - --> u*((axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2] - - --> u*(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1] - - --> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) - -if 0.0<= u+v <=1.0 then they are inside of triangle - - \return false if the point is outside of triangle.This function doesn't take the margin - */ - SIMD_FORCE_INLINE bool get_uv_parameters( - const btVector3 & point, - const btVector3 & tri_plane, - GREAL & u, GREAL & v) const - { - btVector3 _axe1 = m_vertices[1]-m_vertices[0]; - btVector3 _axe2 = m_vertices[2]-m_vertices[0]; - btVector3 _vecproj = point - m_vertices[0]; - GUINT _i1 = (tri_plane.closestAxis()+1)%3; - GUINT _i2 = (_i1+1)%3; - if(btFabs(_axe2[_i2])G_EPSILON) - { - return false; - } - } - return true; - } - - //! is point in triangle beam? - /*! - 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 - { - //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(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 - 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) - { - 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); - } - - 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 - { - triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); - } - else - { - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); - } - - VEC_NORMALIZE(triangle_normal); - - 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) - { - 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); - } - - 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; - - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); - - VEC_NORMALIZE(triangle_normal); - - return true; - } - -}; - - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp deleted file mode 100644 index 940282f57..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btContinuousConvexCollision.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -#include "LinearMath/btTransformUtil.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -#include "btGjkPairDetector.h" -#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 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) -{ - 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::ClosestPointInput input; - input.m_transformA = transA; - input.m_transformB = transB; - 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; - btTransform planeInConvex; - 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 vtxInPlaneWorld = transB * vtxInPlaneProjected; - btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal; - - pointCollector.addContactPoint( - normalOnSurfaceB, - vtxInPlaneWorld, - distance); - } -} - -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); - - - btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); - btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f; - - btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; - btVector3 relLinVel = (linVelB-linVelA); - - btScalar relLinVelocLength = (linVelB-linVelA).length(); - - if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) - return false; - - - - btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); - - int maxIter = MAX_ITERATIONS; - - btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - bool hasResult = false; - btVector3 c; - - btScalar lastLambda = lambda; - //btScalar epsilon = btScalar(0.001); - - int numIter = 0; - //first solution, using GJK - - - btScalar radius = 0.001f; -// result.drawCoordSystem(sphereTr); - - btPointCollector pointCollector1; - - { - - computeClosestPoints(fromA,fromB,pointCollector1); - - hasResult = pointCollector1.m_hasResult; - c = pointCollector1.m_pointInWorld; - } - - if (hasResult) - { - btScalar dist; - dist = pointCollector1.m_distance + result.m_allowedPenetration; - n = pointCollector1.m_normalOnBInWorld; - btScalar projectedLinearVelocity = relLinVel.dot(n); - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) - return false; - - //not close enough - while (dist > radius) - { - if (result.m_debugDrawer) - { - 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.)) - return false; - - if (lambda < btScalar(0.)) - return false; - - - //todo: next check with relative epsilon - if (lambda <= lastLambda) - { - return false; - //n.setValue(0,0,0); - break; - } - lastLambda = lambda; - - - - //interpolate to next lambda - btTransform interpolatedTransA,interpolatedTransB,relativeTrans; - - btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); - btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); - relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); - - if (result.m_debugDrawer) - { - result.m_debugDrawer->drawSphere(interpolatedTransA.getOrigin(),0.2f,btVector3(1,0,0)); - } - - result.DebugDraw( lambda ); - - btPointCollector pointCollector; - computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector); - - if (pointCollector.m_hasResult) - { - dist = pointCollector.m_distance+result.m_allowedPenetration; - c = pointCollector.m_pointInWorld; - n = pointCollector.m_normalOnBInWorld; - } else - { - result.reportFailure(-1, numIter); - return false; - } - - numIter++; - if (numIter > maxIter) - { - result.reportFailure(-2, numIter); - return false; - } - } - - result.m_fraction = lambda; - result.m_normal = n; - result.m_hitPoint = c; - return true; - } - - return false; - -} - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h deleted file mode 100644 index bdc0572f7..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ /dev/null @@ -1,59 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H -#define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H - -#include "btConvexCast.h" -#include "btSimplexSolverInterface.h" -class btConvexPenetrationDepthSolver; -class btConvexShape; -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. -/// 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; - //second object is either a convex or a plane (code sharing) - const btConvexShape* m_convexB1; - const btStaticPlaneShape* m_planeShape; - - 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 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 - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp deleted file mode 100644 index d2a1310b2..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConvexCast.h" - -btConvexCast::~btConvexCast() -{ -} diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h deleted file mode 100644 index bfd79d03b..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ /dev/null @@ -1,73 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_CONVEX_CAST_H -#define BT_CONVEX_CAST_H - -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" -class btMinkowskiSumShape; -#include "LinearMath/btIDebugDraw.h" - -/// 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 - { - //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;} - CastResult() - :m_fraction(btScalar(BT_LARGE_FLOAT)), - m_debugDrawer(0), - m_allowedPenetration(btScalar(0)) - { - } - - - virtual ~CastResult() {}; - - btTransform m_hitTransformA; - btTransform m_hitTransformB; - btVector3 m_normal; - btVector3 m_hitPoint; - btScalar m_fraction; //input and output - btIDebugDraw* m_debugDrawer; - btScalar m_allowedPenetration; - - }; - - - /// 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; -}; - -#endif //BT_CONVEX_CAST_H diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h deleted file mode 100644 index 29620abff..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ /dev/null @@ -1,40 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_CONVEX_PENETRATION_DEPTH_H -#define BT_CONVEX_PENETRATION_DEPTH_H - -class btVector3; -#include "btSimplexSolverInterface.h" -class btConvexShape; -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; - - -}; -#endif //BT_CONVEX_PENETRATION_DEPTH_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h deleted file mode 100644 index 46ce1ab75..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ /dev/null @@ -1,88 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H -#define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H - -#include "LinearMath/btTransform.h" -#include "LinearMath/btVector3.h" - -/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations -/// This interface allows to query for closest points and penetration depth between two (convex) objects -/// the closest point is on the second object (B), and the normal points from the surface on B towards A. -/// distance is between closest points on B and closest point on A. So you can calculate closest point on A -/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB -struct btDiscreteCollisionDetectorInterface -{ - - struct 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; - }; - - struct ClosestPointInput - { - ClosestPointInput() - :m_maximumDistanceSquared(btScalar(BT_LARGE_FLOAT)) - { - } - - btTransform m_transformA; - btTransform m_transformB; - btScalar m_maximumDistanceSquared; - }; - - 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; - -}; - -struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result -{ - btVector3 m_normalOnSurfaceB; - btVector3 m_closestPointInB; - btScalar m_distance; //negative means penetration ! - - btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) - { - - } - virtual ~btStorageResult() {}; - - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) - { - if (depth < m_distance) - { - m_normalOnSurfaceB = normalOnBInWorld; - m_closestPointInB = pointInWorld; - m_distance = depth; - } - } -}; - -#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp deleted file mode 100644 index bef697a0a..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btGjkConvexCast.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "btGjkPairDetector.h" -#include "btPointCollector.h" -#include "LinearMath/btTransformUtil.h" - -#ifdef BT_USE_DOUBLE_PRECISION -#define MAX_ITERATIONS 64 -#else -#define MAX_ITERATIONS 32 -#endif - -btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) -:m_simplexSolver(simplexSolver), -m_convexA(convexA), -m_convexB(convexB) -{ -} - -bool btGjkConvexCast::calcTimeOfImpact( - const btTransform& fromA, - const btTransform& toA, - const btTransform& fromB, - const btTransform& toB, - CastResult& result) -{ - - - m_simplexSolver->reset(); - - /// compute linear velocity for this interval, to interpolate - //assume no rotation/angular velocity, assert here? - btVector3 linVelA,linVelB; - linVelA = toA.getOrigin()-fromA.getOrigin(); - linVelB = toB.getOrigin()-fromB.getOrigin(); - - btScalar radius = btScalar(0.001); - btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); - - int maxIter = MAX_ITERATIONS; - - btVector3 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - bool hasResult = false; - btVector3 c; - btVector3 r = (linVelA-linVelB); - - btScalar lastLambda = lambda; - //btScalar epsilon = btScalar(0.001); - - int numIter = 0; - //first solution, using GJK - - - btTransform identityTrans; - identityTrans.setIdentity(); - - -// result.drawCoordSystem(sphereTr); - - btPointCollector pointCollector; - - - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver); - btGjkPairDetector::ClosestPointInput input; - - //we don't use margins during CCD - // gjk.setIgnoreMargin(true); - - input.m_transformA = fromA; - input.m_transformB = fromB; - gjk.getClosestPoints(input,pointCollector,0); - - hasResult = pointCollector.m_hasResult; - c = pointCollector.m_pointInWorld; - - if (hasResult) - { - btScalar dist; - dist = pointCollector.m_distance; - n = pointCollector.m_normalOnBInWorld; - - - - //not close enough - while (dist > radius) - { - numIter++; - if (numIter > maxIter) - { - return false; //todo: report a failure - } - btScalar dLambda = btScalar(0.); - - btScalar projectedLinearVelocity = r.dot(n); - - dLambda = dist / (projectedLinearVelocity); - - lambda = lambda - dLambda; - - if (lambda > btScalar(1.)) - return false; - - if (lambda < btScalar(0.)) - return false; - - //todo: next check with relative epsilon - if (lambda <= lastLambda) - { - return false; - //n.setValue(0,0,0); - break; - } - lastLambda = lambda; - - //interpolate to next lambda - result.DebugDraw( lambda ); - input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda); - input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda); - - gjk.getClosestPoints(input,pointCollector,0); - if (pointCollector.m_hasResult) - { - if (pointCollector.m_distance < btScalar(0.)) - { - result.m_fraction = lastLambda; - n = pointCollector.m_normalOnBInWorld; - result.m_normal=n; - result.m_hitPoint = pointCollector.m_pointInWorld; - return true; - } - c = pointCollector.m_pointInWorld; - n = pointCollector.m_normalOnBInWorld; - dist = pointCollector.m_distance; - } else - { - //?? - return false; - } - - } - - //is n normalized? - //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (n.dot(r)>=-result.m_allowedPenetration) - return false; - - result.m_fraction = lambda; - result.m_normal = n; - result.m_hitPoint = c; - return true; - } - - return false; - - -} - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h deleted file mode 100644 index 6a42ee63b..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef BT_GJK_CONVEX_CAST_H -#define BT_GJK_CONVEX_CAST_H - -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -#include "LinearMath/btVector3.h" -#include "btConvexCast.h" -class btConvexShape; -class btMinkowskiSumShape; -#include "btSimplexSolverInterface.h" - -///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; - -public: - - 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); - -}; - -#endif //BT_GJK_CONVEX_CAST_H diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp deleted file mode 100644 index 3268f06c2..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ /dev/null @@ -1,1031 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the -use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not -claim that you wrote the original software. If you use this software in a -product, an acknowledgment in the product documentation would be appreciated -but is not required. -2. Altered source versions must be plainly marked as such, and must not be -misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson, 2008 -*/ -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "btGjkEpa2.h" - -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -#endif //__SPU__ -#endif - -namespace gjkepa2_impl -{ - - // Config - - /* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((btScalar)0.0001) -#define GJK_MIN_DISTANCE ((btScalar)0.0001) -#define GJK_DUPLICATED_EPS ((btScalar)0.0001) -#define GJK_SIMPLEX2_EPS ((btScalar)0.0) -#define GJK_SIMPLEX3_EPS ((btScalar)0.0) -#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - - /* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((btScalar)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((btScalar)0.00001) -#define EPA_INSIDE_EPS ((btScalar)0.01) - - - // Shorthands - typedef unsigned int U; - typedef unsigned char U1; - - // MinkowskiDiff - struct MinkowskiDiff - { - const btConvexShape* m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; -#ifdef __SPU__ - bool m_enableMargin; -#else - btVector3 (btConvexShape::*Ls)(const btVector3&) const; -#endif//__SPU__ - - - MinkowskiDiff() - { - - } -#ifdef __SPU__ - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - if (m_enableMargin) - { - return m_shapes[0]->localGetSupportVertexNonVirtual(d); - } else - { - return m_shapes[0]->localGetSupportVertexWithoutMarginNonVirtual(d); - } - } - inline btVector3 Support1(const btVector3& d) const - { - if (m_enableMargin) - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexNonVirtual(m_toshape1*d)); - } else - { - return m_toshape0*(m_shapes[1]->localGetSupportVertexWithoutMarginNonVirtual(m_toshape1*d)); - } - } -#else - void EnableMargin(bool enable) - { - if(enable) - Ls=&btConvexShape::localGetSupportVertexNonVirtual; - else - Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; - } - inline btVector3 Support0(const btVector3& d) const - { - return(((m_shapes[0])->*(Ls))(d)); - } - inline btVector3 Support1(const btVector3& d) const - { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); - } -#endif //__SPU__ - - inline btVector3 Support(const btVector3& d) const - { - return(Support0(d)-Support1(-d)); - } - btVector3 Support(const btVector3& d,U index) const - { - if(index) - return(Support1(d)); - else - return(Support0(d)); - } - }; - - typedef MinkowskiDiff tShape; - - - // GJK - struct GJK - { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - struct eStatus { enum _ { - Valid, - Inside, - Failed };}; - /* Fields */ - tShape m_shape; - btVector3 m_ray; - btScalar m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eStatus::_ m_status; - /* Methods */ - GJK() - { - Initialize(); - } - void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } - eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(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<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); - } - - }; - - // - static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver2::sResults::Separated; - /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); - shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); - shape.EnableMargin(withmargins); - } - -} - -// -// Api -// - -using namespace gjkepa2_impl; - -// -int btGjkEpaSolver2::StackSizeRequirement() -{ - return(sizeof(GJK)+sizeof(EPA)); -} - -// -bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) -{ - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==GJK::eStatus::Valid) - { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;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] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); - } - else - { - results.status = gjk_status==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); - } -} - -// -bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - bool usemargins) -{ - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) - { - case GJK::eStatus::Inside: - { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) - { - btVector3 w0=btVector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; - } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=sResults::EPA_Failed; - } - break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; - default: - { - } - } - return(false); -} - -#ifndef __SPU__ -// -btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, - btScalar margin, - const btConvexShape* shape0, - const btTransform& wtrs0, - sResults& results) -{ - tShape shape; - btSphereShape shape1(margin); - btTransform wtrs1(btQuaternion(0,0,0,1),position); - Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); - if(gjk_status==GJK::eStatus::Valid) - { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;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] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - const btVector3 delta= results.witnesses[1]- - results.witnesses[0]; - const btScalar margin= shape0->getMarginNonVirtual()+ - shape1.getMarginNonVirtual(); - const btScalar length= delta.length(); - results.normal = delta/length; - results.witnesses[0] += results.normal*margin; - return(length-margin); - } - else - { - if(gjk_status==GJK::eStatus::Inside) - { - if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) - { - const btVector3 delta= results.witnesses[0]- - results.witnesses[1]; - const btScalar length= delta.length(); - if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-length); - } - } - } - return(SIMD_INFINITY); -} - -// -bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) -{ - if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) - return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); - else - return(true); -} -#endif //__SPU__ - -/* Symbols cleanup */ - -#undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY -#undef GJK_MIN_DISTANCE -#undef GJK_DUPLICATED_EPS -#undef GJK_SIMPLEX2_EPS -#undef GJK_SIMPLEX3_EPS -#undef GJK_SIMPLEX4_EPS - -#undef EPA_MAX_VERTICES -#undef EPA_MAX_FACES -#undef EPA_MAX_ITERATIONS -#undef EPA_ACCURACY -#undef EPA_FALLBACK -#undef EPA_PLANE_EPS -#undef EPA_INSIDE_EPS diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h deleted file mode 100644 index ac501d5ec..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ /dev/null @@ -1,75 +0,0 @@ -/* -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 BT_GJK_EPA2_H -#define BT_GJK_EPA2_H - -#include "BulletCollision/CollisionShapes/btConvexShape.h" - -///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct btGjkEpaSolver2 -{ -struct sResults - { - enum eStatus - { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - btVector3 witnesses[2]; - btVector3 normal; - btScalar distance; - }; - -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 Penetration(const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - 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__ - -}; - -#endif //BT_GJK_EPA2_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp deleted file mode 100644 index 572ec36f5..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -EPA Copyright (c) Ricardo Padrela 2006 - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "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) -{ - - (void)debugDraw; - (void)v; - (void)simplexSolver; - -// const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); - btGjkEpaSolver2::sResults results; - - - if(btGjkEpaSolver2::Penetration(pConvexA,transformA, - pConvexB,transformB, - guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } else - { - if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) - { - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return false; - } - } - - return false; -} - - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h deleted file mode 100644 index 1ed6340af..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -EPA Copyright (c) Ricardo Padrela 2006 - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#ifndef BT_GJP_EPA_PENETRATION_DEPTH_H -#define BT_GJP_EPA_PENETRATION_DEPTH_H - -#include "btConvexPenetrationDepthSolver.h" - -///EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to -///calculate the penetration depth between two convex shapes. -class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver -{ - public : - - 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 : - -}; - -#endif // BT_GJP_EPA_PENETRATION_DEPTH_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp deleted file mode 100644 index 887757949..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ /dev/null @@ -1,480 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGjkPairDetector.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" - - - -#if defined(DEBUG) || defined (_DEBUG) -//#define TEST_NON_VIRTUAL 1 -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -//#define DEBUG_SPU_COLLISION_DETECTION 1 -#endif //__SPU__ -#endif - -//must be above the machine epsilon -#define REL_ERROR2 btScalar(1.0e-6) - -//temp globals, to improve GJK/EPA/penetration calculations -int gNumDeepPenetrationChecks = 0; -int gNumGjkChecks = 0; - - -btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), -m_penetrationDepthSolver(penetrationDepthSolver), -m_simplexSolver(simplexSolver), -m_minkowskiA(objectA), -m_minkowskiB(objectB), -m_shapeTypeA(objectA->getShapeType()), -m_shapeTypeB(objectB->getShapeType()), -m_marginA(objectA->getMargin()), -m_marginB(objectB->getMargin()), -m_ignoreMargin(false), -m_lastUsedMethod(-1), -m_catchDegeneracies(1), -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)swapResults; - - getClosestPointsNonVirtual(input,output,debugDraw); -} - -#ifdef __SPU__ -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) -#else -void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) -#endif -{ - m_cachedSeparatingDistance = 0.f; - - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 pointOnA,pointOnB; - btTransform localTransA = input.m_transformA; - btTransform localTransB = input.m_transformB; - btVector3 positionOffset = (localTransA.getOrigin() + localTransB.getOrigin()) * btScalar(0.5); - localTransA.getOrigin() -= positionOffset; - localTransB.getOrigin() -= positionOffset; - - bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); - - btScalar marginA = m_marginA; - btScalar marginB = m_marginB; - - gNumGjkChecks++; - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("inside gjk\n"); -#endif - //for CCD we don't use margins - if (m_ignoreMargin) - { - marginA = btScalar(0.); - marginB = btScalar(0.); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("ignoring margin\n"); -#endif - } - - m_curIter = 0; - int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? - m_cachedSeparatingAxis.setValue(0,1,0); - - bool isValid = false; - bool checkSimplex = false; - bool checkPenetration = true; - m_degenerateSimplex = 0; - - m_lastUsedMethod = -1; - - { - btScalar squaredDistance = BT_LARGE_FLOAT; - btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - - m_simplexSolver->reset(); - - for ( ; ; ) - //while (true) - { - - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); - -#if 1 - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - -// btVector3 pInA = localGetSupportingVertexWithoutMargin(m_shapeTypeA, m_minkowskiA, seperatingAxisInA,input.m_convexVertexData[0]);//, &featureIndexA); -// btVector3 qInB = localGetSupportingVertexWithoutMargin(m_shapeTypeB, m_minkowskiB, seperatingAxisInB,input.m_convexVertexData[1]);//, &featureIndexB); - -#else -#ifdef __SPU__ - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); -#else - btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); -#ifdef TEST_NON_VIRTUAL - btVector3 pInAv = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); - btVector3 qInBv = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); - btAssert((pInAv-pInA).length() < 0.0001); - btAssert((qInBv-qInB).length() < 0.0001); -#endif // -#endif //__SPU__ -#endif - - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("got local supporting vertices\n"); -#endif - - if (check2d) - { - pWorld[2] = 0.f; - qWorld[2] = 0.f; - } - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) - { - m_degenerateSimplex = 10; - checkSimplex=true; - //checkPenetration = false; - break; - } - - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (m_simplexSolver->inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * REL_ERROR2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } else - { - m_degenerateSimplex = 11; - } - checkSimplex = true; - break; - } - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("addVertex 1\n"); -#endif - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("addVertex 2\n"); -#endif - btVector3 newCachedSeparatingAxis; - - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(newCachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - if(newCachedSeparatingAxis.length2()previousSquaredDistance) - { - m_degenerateSimplex = 7; - squaredDistance = previousSquaredDistance; - checkSimplex = false; - break; - } -#endif // - - - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); - - //are we getting any closer ? - if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) - { -// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - checkSimplex = true; - m_degenerateSimplex = 12; - - break; - } - - m_cachedSeparatingAxis = newCachedSeparatingAxis; - - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { - #if defined(DEBUG) || defined (_DEBUG) || defined (DEBUG_SPU_COLLISION_DETECTION) - - printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance, - m_minkowskiA->getShapeType(), - m_minkowskiB->getShapeType()); - - #endif - break; - - } - - - bool check = (!m_simplexSolver->fullSimplex()); - //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); - - if (!check) - { - //do we need this backup_closest here ? -// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - m_degenerateSimplex = 13; - break; - } - } - - if (checkSimplex) - { - m_simplexSolver->compute_points(pointOnA, pointOnB); - normalInB = 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_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); - - //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) - { - //penetration case - - //if there is no way to handle penetrations, bail out - if (m_penetrationDepthSolver) - { - // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - gNumDeepPenetrationChecks++; - m_cachedSeparatingAxis.setZero(); - - bool isValid2 = m_penetrationDepthSolver->calcPenDepth( - *m_simplexSolver, - m_minkowskiA,m_minkowskiB, - localTransA,localTransB, - m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, - debugDraw - ); - - - if (isValid2) - { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB = m_cachedSeparatingAxis; - lenSqr = m_cachedSeparatingAxis.length2(); - } - - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - isValid = true; - m_lastUsedMethod = 3; - } else - { - m_lastUsedMethod = 8; - } - } else - { - m_lastUsedMethod = 9; - } - } else - - { - ///this is another degenerate case, where the initial GJK calculation reports a degenerate case - ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) - ///reports a valid positive distance. Use the results of the second GJK instead of failing. - ///thanks to Jacob.Langford for the reproduction case - ///http://code.google.com/p/bullet/issues/detail?id=250 - - - if (m_cachedSeparatingAxis.length2() > btScalar(0.)) - { - btScalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; - //only replace valid distances when the distance is less - if (!isValid || (distance2 < distance)) - { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - pointOnA -= m_cachedSeparatingAxis * marginA ; - pointOnB += m_cachedSeparatingAxis * marginB ; - normalInB = m_cachedSeparatingAxis; - normalInB.normalize(); - isValid = true; - m_lastUsedMethod = 6; - } else - { - m_lastUsedMethod = 5; - } - } - } - - } - - } - } - - - - if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) - { -#if 0 -///some debugging -// if (check2d) - { - printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]); - printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex); - } -#endif - - if (m_fixContactNormalDirection) - { - ///@workaround for sticky convex collisions - //in some degenerate cases (usually when the use uses very small margins) - //the contact normal is pointing the wrong direction - //so fix it now (until we can deal with all degenerate cases in GJK and EPA) - //contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A - //We like to use a dot product of the normal against the difference of the centroids, - //once the centroid is available in the API - //until then we use the center of the aabb to approximate the centroid - btVector3 aabbMin,aabbMax; - m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax); - btVector3 posA = (aabbMax+aabbMin)*btScalar(0.5); - - m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax); - btVector3 posB = (aabbMin+aabbMax)*btScalar(0.5); - - btVector3 diff = posA-posB; - if (diff.dot(normalInB) < 0.f) - normalInB *= -1.f; - } - m_cachedSeparatingAxis = normalInB; - m_cachedSeparatingDistance = distance; - - output.addContactPoint( - normalInB, - pointOnB+positionOffset, - distance); - - } - - -} - - - - - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h deleted file mode 100644 index feeae6862..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ /dev/null @@ -1,103 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - - -#ifndef BT_GJK_PAIR_DETECTOR_H -#define BT_GJK_PAIR_DETECTOR_H - -#include "btDiscreteCollisionDetectorInterface.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -class btConvexShape; -#include "btSimplexSolverInterface.h" -class btConvexPenetrationDepthSolver; - -/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface -class btGjkPairDetector : public btDiscreteCollisionDetectorInterface -{ - - - btVector3 m_cachedSeparatingAxis; - btConvexPenetrationDepthSolver* m_penetrationDepthSolver; - btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_minkowskiA; - const btConvexShape* m_minkowskiB; - int m_shapeTypeA; - int m_shapeTypeB; - btScalar m_marginA; - btScalar m_marginB; - - 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; - - 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); - - void getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); - - - void setMinkowskiA(const btConvexShape* minkA) - { - m_minkowskiA = minkA; - } - - void setMinkowskiB(const btConvexShape* minkB) - { - m_minkowskiB = minkB; - } - void setCachedSeperatingAxis(const btVector3& seperatingAxis) - { - m_cachedSeparatingAxis = seperatingAxis; - } - - const btVector3& getCachedSeparatingAxis() const - { - return m_cachedSeparatingAxis; - } - btScalar getCachedSeparatingDistance() const - { - return m_cachedSeparatingDistance; - } - - void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) - { - m_penetrationDepthSolver = penetrationDepthSolver; - } - - ///don't use setIgnoreMargin, it's for Bullet's internal use - void setIgnoreMargin(bool ignoreMargin) - { - m_ignoreMargin = ignoreMargin; - } - - -}; - -#endif //BT_GJK_PAIR_DETECTOR_H diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h deleted file mode 100644 index e40fb1d3d..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ /dev/null @@ -1,156 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MANIFOLD_CONTACT_POINT_H -#define BT_MANIFOLD_CONTACT_POINT_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransformUtil.h" - -#ifdef PFX_USE_FREE_VECTORMATH - #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 - - - -/// ManifoldContactPoint collects and maintains persistent contactpoints. -/// used to improve stability and performance of rigidbody dynamics response. -class btManifoldPoint - { - public: - btManifoldPoint() - :m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), - m_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_combinedRestitution(btScalar(0.)), - m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), - m_appliedImpulseLateral1(0.f), - m_appliedImpulseLateral2(0.f), - m_contactMotion1(0.f), - m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), - m_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; - - btScalar m_distance1; - btScalar m_combinedFriction; - btScalar m_combinedRollingFriction; - 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; - - btScalar m_appliedImpulse; - btScalar m_appliedImpulseLateral1; - btScalar m_appliedImpulseLateral2; - btScalar m_contactMotion1; - btScalar m_contactMotion2; - btScalar m_contactCFM1; - btScalar m_contactCFM2; - - 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/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp deleted file mode 100644 index fa45f4903..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ /dev/null @@ -1,361 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMinkowskiPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" - -#define NUM_UNITSPHERE_POINTS 42 - - -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 - ) -{ - - (void)v; - - bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); - - struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result - { - - btIntermediateResult():m_hasResult(false) - { - } - - btVector3 m_normalOnBInWorld; - btVector3 m_pointInWorld; - btScalar m_depth; - bool m_hasResult; - - virtual void setShapeIdentifiersA(int partId0,int index0) - { - (void)partId0; - (void)index0; - } - virtual void setShapeIdentifiersB(int partId1,int index1) - { - (void)partId1; - (void)index1; - } - void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) - { - m_normalOnBInWorld = normalOnBInWorld; - m_pointInWorld = pointInWorld; - m_depth = depth; - m_hasResult = true; - } - }; - - //just take fixed number of orientation, and sample the penetration depth in that direction - btScalar minProj = btScalar(BT_LARGE_FLOAT); - btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.)); - btVector3 minA,minB; - btVector3 seperatingAxisInA,seperatingAxisInB; - btVector3 pInA,qInB,pWorld,qWorld,w; - -#ifndef __SPU__ -#define USE_BATCHED_SUPPORT 1 -#endif -#ifdef USE_BATCHED_SUPPORT - - btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - int i; - - int numSampleDirections = NUM_UNITSPHERE_POINTS; - - for (i=0;igetNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; - getPenetrationDirections()[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); - numSampleDirections++; - } - } - } - - { - int numPDB = convexB->getNumPreferredPenetrationDirections(); - if (numPDB) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; - getPenetrationDirections()[numSampleDirections] = norm; - seperatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis(); - seperatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis(); - numSampleDirections++; - } - } - } - - - - - convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); - convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); - - for (i=0;i0.01) - { - - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; - - pInA = supportVerticesABatch[i]; - qInB = supportVerticesBBatch[i]; - - pWorld = transA(pInA); - qWorld = transB(qInB); - if (check2d) - { - pWorld[2] = 0.f; - qWorld[2] = 0.f; - } - - w = qWorld - pWorld; - btScalar delta = norm.dot(w); - //find smallest delta - if (delta < minProj) - { - minProj = delta; - minNorm = norm; - minA = pWorld; - minB = qWorld; - } - } - } -#else - - int numSampleDirections = NUM_UNITSPHERE_POINTS; - -#ifndef __SPU__ - { - int numPDA = convexA->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transA.getBasis() * norm; - getPenetrationDirections()[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - - { - int numPDB = convexB->getNumPreferredPenetrationDirections(); - if (numPDB) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - norm = transB.getBasis() * norm; - getPenetrationDirections()[numSampleDirections] = norm; - numSampleDirections++; - } - } - } -#endif // __SPU__ - - for (int i=0;ilocalGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - pWorld = transA(pInA); - qWorld = transB(qInB); - w = qWorld - pWorld; - btScalar delta = norm.dot(w); - //find smallest delta - if (delta < minProj) - { - minProj = delta; - minNorm = norm; - minA = pWorld; - minB = qWorld; - } - } -#endif //USE_BATCHED_SUPPORT - - //add the margins - - minA += minNorm*convexA->getMarginNonVirtual(); - minB -= minNorm*convexB->getMarginNonVirtual(); - //no penetration - if (minProj < btScalar(0.)) - return false; - - btScalar extraSeparation = 0.5f;///scale dependent - minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); - - - - - -//#define DEBUG_DRAW 1 -#ifdef DEBUG_DRAW - if (debugDraw) - { - btVector3 color(0,1,0); - debugDraw->drawLine(minA,minB,color); - color = btVector3 (1,1,1); - btVector3 vec = minB-minA; - btScalar prj2 = minNorm.dot(vec); - debugDraw->drawLine(minA,minA+(minNorm*minProj),color); - - } -#endif //DEBUG_DRAW - - - - btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); - - btScalar offsetDist = minProj; - btVector3 offset = minNorm * offsetDist; - - - - btGjkPairDetector::ClosestPointInput input; - - btVector3 newOrg = transA.getOrigin() + offset; - - btTransform displacedTrans = transA; - displacedTrans.setOrigin(newOrg); - - input.m_transformA = displacedTrans; - input.m_transformB = transB; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; - - btIntermediateResult res; - gjkdet.setCachedSeperatingAxis(-minNorm); - gjkdet.getClosestPoints(input,res,debugDraw); - - btScalar correctedMinNorm = minProj - res.m_depth; - - - //the penetration depth is over-estimated, relax it - btScalar penetration_relaxation= btScalar(1.); - minNorm*=penetration_relaxation; - - - if (res.m_hasResult) - { - - pa = res.m_pointInWorld - minNorm * correctedMinNorm; - pb = res.m_pointInWorld; - v = minNorm; - -#ifdef DEBUG_DRAW - if (debugDraw) - { - btVector3 color(1,0,0); - debugDraw->drawLine(pa,pb,color); - } -#endif//DEBUG_DRAW - - - } - return res.m_hasResult; -} - -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)) - }; - - return sPenetrationDirections; -} - - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h deleted file mode 100644 index fd533b4fc..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ /dev/null @@ -1,40 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H -#define BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H - -#include "btConvexPenetrationDepthSolver.h" - -///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. -///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. -class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver -{ -protected: - - 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 - ); -}; - -#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp deleted file mode 100644 index 4d92e853d..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ /dev/null @@ -1,305 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btPersistentManifold.h" -#include "LinearMath/btTransform.h" - - -btScalar gContactBreakingThreshold = btScalar(0.02); -ContactDestroyedCallback gContactDestroyedCallback = 0; -ContactProcessedCallback gContactProcessedCallback = 0; -///gContactCalcArea3Points will approximate the convex hull area using 3 points -///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower -bool gContactCalcArea3Points = true; - - -btPersistentManifold::btPersistentManifold() -:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), -m_body0(0), -m_body1(0), -m_cachedPoints (0), -m_index1a(0) -{ -} - - - - -#ifdef DEBUG_PERSISTENCY -#include -void btPersistentManifold::DebugPersistency() -{ - int i; - printf("DebugPersistency : numPoints %d\n",m_cachedPoints); - for (i=0;i1) - printf("error in clearUserCache\n"); - } - } - btAssert(occurance<=0); -#endif //DEBUG_PERSISTENCY - - if (pt.m_userPersistentData && gContactDestroyedCallback) - { - (*gContactDestroyedCallback)(pt.m_userPersistentData); - pt.m_userPersistentData = 0; - } - -#ifdef DEBUG_PERSISTENCY - DebugPersistency(); -#endif - } - - -} - -static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3) -{ - // It calculates possible 3 area constructed from random 4 points and returns the biggest one. - - btVector3 a[3],b[3]; - a[0] = p0 - p1; - a[1] = p0 - p2; - a[2] = p0 - p3; - b[0] = p2 - p3; - b[1] = p1 - p3; - b[2] = p1 - p2; - - //todo: Following 3 cross production can be easily optimized by SIMD. - btVector3 tmp0 = a[0].cross(b[0]); - btVector3 tmp1 = a[1].cross(b[1]); - btVector3 tmp2 = a[2].cross(b[2]); - - return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); -} - -int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) -{ - //calculate 4 possible cases areas, and take biggest area - //also need to keep 'deepest' - - int maxPenetrationIndex = -1; -#define KEEP_DEEPEST_POINT 1 -#ifdef KEEP_DEEPEST_POINT - btScalar maxPenetration = pt.getDistance(); - for (int i=0;i<4;i++) - { - if (m_pointCache[i].getDistance() < maxPenetration) - { - maxPenetrationIndex = i; - maxPenetration = m_pointCache[i].getDistance(); - } - } -#endif //KEEP_DEEPEST_POINT - - btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); - - if (gContactCalcArea3Points) - { - if (maxPenetrationIndex != 0) - { - btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; - btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; - btVector3 cross = a0.cross(b0); - res0 = cross.length2(); - } - if (maxPenetrationIndex != 1) - { - btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; - btVector3 cross = a1.cross(b1); - res1 = cross.length2(); - } - - if (maxPenetrationIndex != 2) - { - btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; - btVector3 cross = a2.cross(b2); - res2 = cross.length2(); - } - - if (maxPenetrationIndex != 3) - { - btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; - btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; - btVector3 cross = a3.cross(b3); - res3 = cross.length2(); - } - } - else - { - if(maxPenetrationIndex != 0) { - res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); - } - - if(maxPenetrationIndex != 1) { - res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); - } - - if(maxPenetrationIndex != 2) { - res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); - } - - if(maxPenetrationIndex != 3) { - res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); - } - } - btVector4 maxvec(res0,res1,res2,res3); - int biggestarea = maxvec.closestAxis4(); - return biggestarea; - -} - - -int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const -{ - btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); - int size = getNumContacts(); - int nearestPoint = -1; - for( int i = 0; i < size; i++ ) - { - const btManifoldPoint &mp = m_pointCache[i]; - - btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; - const btScalar distToManiPoint = diffA.dot(diffA); - if( distToManiPoint < shortestDist ) - { - shortestDist = distToManiPoint; - nearestPoint = i; - } - } - return nearestPoint; -} - -int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive) -{ - if (!isPredictive) - { - btAssert(validContactDistance(newPoint)); - } - - int insertIndex = getNumContacts(); - if (insertIndex == MANIFOLD_CACHE_SIZE) - { -#if MANIFOLD_CACHE_SIZE >= 4 - //sort cache so best points come first, based on area - insertIndex = sortCachedPoints(newPoint); -#else - insertIndex = 0; -#endif - clearUserCache(m_pointCache[insertIndex]); - - } else - { - m_cachedPoints++; - - - } - if (insertIndex<0) - insertIndex=0; - - btAssert(m_pointCache[insertIndex].m_userPersistentData==0); - m_pointCache[insertIndex] = newPoint; - return insertIndex; -} - -btScalar btPersistentManifold::getContactBreakingThreshold() const -{ - return m_contactBreakingThreshold; -} - - - -void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) -{ - int i; -#ifdef DEBUG_PERSISTENCY - printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n", - trA.getOrigin().getX(), - trA.getOrigin().getY(), - trA.getOrigin().getZ(), - trB.getOrigin().getX(), - trB.getOrigin().getY(), - trB.getOrigin().getZ()); -#endif //DEBUG_PERSISTENCY - /// first refresh worldspace positions and distance - for (i=getNumContacts()-1;i>=0;i--) - { - btManifoldPoint &manifoldPoint = m_pointCache[i]; - manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA ); - manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB ); - manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB); - manifoldPoint.m_lifeTime++; - } - - /// then - btScalar distance2d; - btVector3 projectedDifference,projectedPoint; - for (i=getNumContacts()-1;i>=0;i--) - { - - btManifoldPoint &manifoldPoint = m_pointCache[i]; - //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) - if (!validContactDistance(manifoldPoint)) - { - removeContactPoint(i); - } else - { - //contact also becomes invalid when relative movement orthogonal to normal exceeds margin - projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; - projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; - distance2d = projectedDifference.dot(projectedDifference); - if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() ) - { - removeContactPoint(i); - } else - { - //contact point processed callback - if (gContactProcessedCallback) - (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1); - } - } - } -#ifdef DEBUG_PERSISTENCY - DebugPersistency(); -#endif // -} - - - - - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h deleted file mode 100644 index 2ceaab750..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ /dev/null @@ -1,240 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_PERSISTENT_MANIFOLD_H -#define BT_PERSISTENT_MANIFOLD_H - - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" -#include "btManifoldPoint.h" -class btCollisionObject; -#include "LinearMath/btAlignedAllocator.h" - -struct btCollisionResult; - -///maximum contact breaking and merging threshold -extern btScalar gContactBreakingThreshold; - -typedef bool (*ContactDestroyedCallback)(void* userPersistentData); -typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); -extern ContactDestroyedCallback gContactDestroyedCallback; -extern ContactProcessedCallback gContactProcessedCallback; - -//the enum starts at 1024 to avoid type conflicts with btTypedConstraint -enum btContactManifoldTypes -{ - MIN_CONTACT_MANIFOLD_TYPE = 1024, - BT_PERSISTENT_MANIFOLD_TYPE -}; - -#define MANIFOLD_CACHE_SIZE 4 - -///btPersistentManifold 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. - - -ATTRIBUTE_ALIGNED128( 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; - - btScalar m_contactBreakingThreshold; - btScalar m_contactProcessingThreshold; - - - /// sort cached points so most isolated points come first - int sortCachedPoints(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_index1a; - - btPersistentManifold(); - - 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) - { - } - - 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) - { - m_body0 = body0; - m_body1 = body1; - } - - void clearUserCache(btManifoldPoint& pt); - -#ifdef DEBUG_PERSISTENCY - 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); - return m_pointCache[index]; - } - - SIMD_FORCE_INLINE btManifoldPoint& getContactPoint(int index) - { - btAssert(index < m_cachedPoints); - return m_pointCache[index]; - } - - ///@todo: get this margin from the current physics / collision environment - btScalar getContactBreakingThreshold() const; - - btScalar getContactProcessingThreshold() const - { - return m_contactProcessingThreshold; - } - - void setContactBreakingThreshold(btScalar contactBreakingThreshold) - { - m_contactBreakingThreshold = contactBreakingThreshold; - } - - void setContactProcessingThreshold(btScalar contactProcessingThreshold) - { - m_contactProcessingThreshold = contactProcessingThreshold; - } - - - - - int getCacheEntry(const btManifoldPoint& newPoint) const; - - int addManifoldPoint( const btManifoldPoint& newPoint, bool isPredictive=false); - - 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]; - //get rid of duplicated userPersistentData pointer - m_pointCache[lastUsedIndex].m_userPersistentData = 0; - m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; - m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; - m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; - m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f; - m_pointCache[lastUsedIndex].m_lifeTime = 0; - } - - btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); - m_cachedPoints--; - } - void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) - { - btAssert(validContactDistance(newPoint)); - -#define MAINTAIN_PERSISTENCY 1 -#ifdef MAINTAIN_PERSISTENCY - int lifeTime = m_pointCache[insertIndex].getLifeTime(); - btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; -// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; - - - - btAssert(lifeTime>=0); - void* cache = m_pointCache[insertIndex].m_userPersistentData; - - m_pointCache[insertIndex] = newPoint; - - m_pointCache[insertIndex].m_userPersistentData = cache; - m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; - m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; - m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - - m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; - m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; - m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - - - m_pointCache[insertIndex].m_lifeTime = lifeTime; -#else - clearUserCache(m_pointCache[insertIndex]); - m_pointCache[insertIndex] = newPoint; - -#endif - } - - - bool validContactDistance(const btManifoldPoint& pt) const - { - return pt.m_distance1 <= getContactBreakingThreshold(); - } - /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin - void refreshContactPoints( const btTransform& trA,const btTransform& trB); - - - SIMD_FORCE_INLINE void clearManifold() - { - int i; - for (i=0;i //for FLT_MAX - -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) -{ - - int ve; - btScalar ds, de; - int numVerts = pVtxIn.size(); - if (numVerts < 2) - return; - - btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; - btVector3 endVertex = pVtxIn[0]; - - ds = planeNormalWS.dot(firstVertex)+planeEqWS; - - for (ve = 0; ve < numVerts; ve++) - { - endVertex=pVtxIn[ve]; - - de = planeNormalWS.dot(endVertex)+planeEqWS; - - if (ds<0) - { - if (de<0) - { - // Start < 0, end < 0, so output endVertex - ppVtxOut.push_back(endVertex); - } - else - { - // Start < 0, end >= 0, so output intersection - ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); - } - } - else - { - 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(endVertex); - } - } - firstVertex = endVertex; - ds = de; - } -} - - -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; - - hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); - hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); - - if(Max0=0.0f); - btScalar d1 = Max1 - Min0; - btAssert(d1>=0.0f); - if (d01e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false; - return true; -} - -#ifdef TEST_INTERNAL_OBJECTS - -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); - 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]; -} - -void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr) -{ - const btMatrix3x3& rot = tr.getBasis(); - const btVector3& r0 = rot[0]; - 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(); - - 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) -{ - const btScalar dp = delta_c.dot(axis); - - btVector3 localAxis0; - InverseTransformPoint3x3(localAxis0, axis,trans0); - btVector3 localAxis1; - 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 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) - return false; - return true; -} -#endif //TEST_INTERNAL_OBJECTS - - - - 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 ) -{ - // compute the parameters of the closest points on each line segment - - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); - - btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - - if ( denom == 0.0f ) { - tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - tB = tA * dirA_dot_dirB - dirB_dot_trans; - - if ( tB < -hlenB ) { - tB = -hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } else if ( tB > hlenB ) { - tB = hlenB; - tA = tB * dirA_dot_dirB + dirA_dot_trans; - - if ( tA < -hlenA ) - tA = -hlenA; - else if ( tA > hlenA ) - tA = hlenA; - } - - // compute the closest points relative to segment centers. - - offsetA = dirA * tA; - offsetB = dirB * tB; - - ptsVector = translation - offsetA + offsetB; -} - - - -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 - const btVector3 c0 = transA * hullA.m_localCenter; - const btVector3 c1 = transB * hullB.m_localCenter; - const btVector3 DeltaC2 = c0 - c1; -//#endif - - btScalar dmin = FLT_MAX; - int curPlaneTests=0; - - int numFacesA = hullA.m_faces.size(); - // Test normals from hullA - for(int i=0;i=0&&edgeB>=0) - { -// printf("edge-edge\n"); - //add an edge-edge contact - - btVector3 ptsVector; - btVector3 offsetA; - btVector3 offsetB; - btScalar tA; - btScalar tB; - - 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); - - btScalar nlSqrt = ptsVector.length2(); - if (nlSqrt>SIMD_EPSILON) - { - btScalar nl = btSqrt(nlSqrt); - ptsVector *= 1.f/nl; - if (ptsVector.dot(DeltaC2)<0.f) - { - ptsVector*=-1.f; - } - btVector3 ptOnB = witnessPointB + offsetB; - btScalar distance = nl; - resultOut.addContactPoint(ptsVector, ptOnB,-distance); - } - - } - - - if((DeltaC2.dot(sep))<0.0f) - sep = -sep; - - return true; -} - -void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) -{ - btVertexArray worldVertsB2; - btVertexArray* pVtxIn = &worldVertsB1; - btVertexArray* pVtxOut = &worldVertsB2; - pVtxOut->reserve(pVtxIn->size()); - - int closestFaceA=-1; - { - btScalar dmin = FLT_MAX; - for(int face=0;faceresize(0); - } - - - -//#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]); - 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 vtx = pVtxIn->at(i); - btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; - if (depth <=minDist) - { -// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); - depth = minDist; - } - - if (depth <=maxDist) - { - btVector3 point = pVtxIn->at(i); -#ifdef ONLY_REPORT_DEEPEST_POINT - curMaxDist = depth; -#else -#if 0 - if (depth<-3) - { - printf("error in btPolyhedralContactClipping depth = %f\n", depth); - printf("likely wrong separatingNormal passed in\n"); - } -#endif - resultOut.addContactPoint(separatingNormal,point,depth); -#endif - } - } - } -#ifdef ONLY_REPORT_DEEPEST_POINT - if (curMaxDist dmax) - { - dmax = d; - closestFaceB = face; - } - } - } - btVertexArray worldVertsB1; - { - const btFace& polyB = hullB.m_faces[closestFaceB]; - const int numVertices = polyB.m_indices.size(); - for(int e0=0;e0=0) - clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); - -} diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h deleted file mode 100644 index b87bd4f32..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -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. -*/ - - -///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" - -class btConvexPolyhedron; - -typedef btAlignedObjectArray btVertexArray; - -// Clips a face to the back of a plane -struct btPolyhedralContactClipping -{ - static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); - static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); - - static 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); - -}; - -#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp deleted file mode 100644 index 786efd182..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -//#include - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "btRaycastCallback.h" - -btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags) - : - m_from(from), - m_to(to), - //@BP Mod - m_flags(flags), - m_hitFraction(btScalar(1.)) -{ - -} - - - -void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) -{ - const btVector3 &vert0=triangle[0]; - const btVector3 &vert1=triangle[1]; - const btVector3 &vert2=triangle[2]; - - btVector3 v10; v10 = vert1 - vert0 ; - btVector3 v20; v20 = vert2 - vert0 ; - - btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); - - const btScalar dist = vert0.dot(triangleNormal); - btScalar dist_a = triangleNormal.dot(m_from) ; - dist_a-= dist; - btScalar dist_b = triangleNormal.dot(m_to); - dist_b -= dist; - - if ( dist_a * dist_b >= btScalar(0.0) ) - { - return ; // same sign - } - - if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0))) - { - // Backface, skip check - return; - } - - - const btScalar proj_length=dist_a-dist_b; - const btScalar distance = (dist_a)/(proj_length); - // Now we have the intersection point on the plane, we'll see if it's inside the triangle - // Add an epsilon as a tolerance for the raycast, - // in case the ray hits exacly on the edge of the triangle. - // It must be scaled for the triangle size. - - if(distance < m_hitFraction) - { - - - btScalar edge_tolerance =triangleNormal.length2(); - edge_tolerance *= btScalar(-0.0001); - btVector3 point; point.setInterpolate3( m_from, m_to, distance); - { - btVector3 v0p; v0p = vert0 - point; - btVector3 v1p; v1p = vert1 - point; - btVector3 cp0; cp0 = v0p.cross( v1p ); - - if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) - { - - - btVector3 v2p; v2p = vert2 - point; - btVector3 cp1; - cp1 = v1p.cross( v2p); - if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) - { - btVector3 cp2; - cp2 = v2p.cross(v0p); - - if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) - { - //@BP Mod - // Triangle normal isn't normalized - triangleNormal.normalize(); - - //@BP Mod - Allow for unflipped normal when raycasting against backfaces - if (((m_flags & kF_KeepUnflippedNormal) == 0) && (dist_a <= btScalar(0.0))) - { - m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex); - } - else - { - m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); - } - } - } - } - } - } -} - - -btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin) -{ - m_convexShape = convexShape; - m_convexShapeFrom = convexShapeFrom; - m_convexShapeTo = convexShapeTo; - m_triangleToWorld = triangleToWorld; - m_hitFraction = 1.0f; - m_triangleCollisionMargin = triangleCollisionMargin; - m_allowedPenetration = 0.f; -} - -void -btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex) -{ - btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]); - triangleShape.setMargin(m_triangleCollisionMargin); - - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; - -//#define USE_SUBSIMPLEX_CONVEX_CAST 1 -//if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below -#ifdef USE_SUBSIMPLEX_CONVEX_CAST - btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver); -#else - //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); - btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver); -#endif //#USE_SUBSIMPLEX_CONVEX_CAST - - btConvexCast::CastResult castResult; - castResult.m_fraction = btScalar(1.); - castResult.m_allowedPenetration = m_allowedPenetration; - if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult)) - { - //add hit - if (castResult.m_normal.length2() > btScalar(0.0001)) - { - if (castResult.m_fraction < m_hitFraction) - { -/* btContinuousConvexCast's normal is already in world space */ -/* -#ifdef USE_SUBSIMPLEX_CONVEX_CAST - //rotate normal into worldspace - castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal; -#endif //USE_SUBSIMPLEX_CONVEX_CAST -*/ - castResult.m_normal.normalize(); - - reportHit (castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction, - partId, - triangleIndex); - } - } - } -} diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h deleted file mode 100644 index 3999d4005..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ /dev/null @@ -1,72 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_RAYCAST_TRI_CALLBACK_H -#define BT_RAYCAST_TRI_CALLBACK_H - -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "LinearMath/btTransform.h" -struct btBroadphaseProxy; -class btConvexShape; - -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 - kF_UseSubSimplexConvexCastRaytest = 1 << 2, // Uses an approximate but faster ray versus convex intersection algorithm - kF_Terminator = 0xFFFFFFFF - }; - unsigned int m_flags; - - btScalar m_hitFraction; - - 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; - -}; - -class btTriangleConvexcastCallback : public btTriangleCallback -{ -public: - const btConvexShape* m_convexShape; - btTransform m_convexShapeFrom; - btTransform m_convexShapeTo; - btTransform m_triangleToWorld; - btScalar m_hitFraction; - btScalar m_triangleCollisionMargin; - btScalar m_allowedPenetration; - - 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 btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; -}; - -#endif //BT_RAYCAST_TRI_CALLBACK_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h deleted file mode 100644 index da8a13914..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef BT_SIMPLEX_SOLVER_INTERFACE_H -#define BT_SIMPLEX_SOLVER_INTERFACE_H - -#include "LinearMath/btVector3.h" - -#define NO_VIRTUAL_INTERFACE 1 -#ifdef NO_VIRTUAL_INTERFACE -#include "btVoronoiSimplexSolver.h" -#define btSimplexSolverInterface btVoronoiSimplexSolver -#else - -/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices -/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on -/// voronoi regions or barycentric coordinates -class 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 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; - - -}; -#endif -#endif //BT_SIMPLEX_SOLVER_INTERFACE_H - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp deleted file mode 100644 index 18eb662de..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSubSimplexConvexCast.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" - -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -#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) -{ -} - -///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) -{ - - m_simplexSolver->reset(); - - btVector3 linVelA,linVelB; - linVelA = toA.getOrigin()-fromA.getOrigin(); - linVelB = toB.getOrigin()-fromB.getOrigin(); - - btScalar lambda = btScalar(0.); - - btTransform interpolatedTransA = fromA; - btTransform interpolatedTransB = fromB; - - ///take relative motion - 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 n; - n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - bool hasResult = false; - btVector3 c; - - btScalar lastLambda = lambda; - - - 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; - btScalar VdotR; - - while ( (dist2 > epsilon) && maxIter--) - { - supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis())); - supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis())); - w = supVertexA-supVertexB; - - btScalar VdotW = v.dot(w); - - if (lambda > btScalar(1.0)) - { - return false; - } - - if ( VdotW > btScalar(0.)) - { - VdotR = v.dot(r); - - 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); - //m_simplexSolver->reset(); - //check next line - w = supVertexA-supVertexB; - lastLambda = lambda; - n = v; - hasResult = true; - } - } - ///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); - - if (m_simplexSolver->closest(v)) - { - dist2 = v.length2(); - hasResult = true; - //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 - { - dist2 = btScalar(0.); - } - } - - //int numiter = MAX_ITERATIONS - maxIter; -// 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)) - 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) - return false; - - btVector3 hitA,hitB; - m_simplexSolver->compute_points(hitA,hitB); - result.m_hitPoint=hitB; - return true; -} - - - - diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h deleted file mode 100644 index 6c8127983..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_SUBSIMPLEX_CONVEX_CAST_H -#define BT_SUBSIMPLEX_CONVEX_CAST_H - -#include "btConvexCast.h" -#include "btSimplexSolverInterface.h" -class btConvexShape; - -/// btSubsimplexConvexCast implements Gino van den Bergens' paper -///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" -/// GJK based Ray Cast, optimized version -/// Objects should not start in overlap, otherwise results are not defined. -class btSubsimplexConvexCast : public btConvexCast -{ - btSimplexSolverInterface* m_simplexSolver; - const btConvexShape* m_convexA; - const btConvexShape* m_convexB; - -public: - - 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); - -}; - -#endif //BT_SUBSIMPLEX_CONVEX_CAST_H diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp deleted file mode 100644 index a775198ab..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ /dev/null @@ -1,609 +0,0 @@ - -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - - 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 "btVoronoiSimplexSolver.h" - -#define VERTA 0 -#define VERTB 1 -#define VERTC 2 -#define VERTD 3 - -#define CATCH_DEGENERATE_TETRAHEDRON 1 -void btVoronoiSimplexSolver::removeVertex(int index) -{ - - 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) -{ - 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 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_cachedBC.reset(); -} - - - - //add a vertex -void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& 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 btVoronoiSimplexSolver::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(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); - m_cachedValidClosest = m_cachedBC.isValid(); - break; - }; - case 2: - { - //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 - { - 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 - 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.)); - - 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); - - 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(btScalar(0.),btScalar(0.),btScalar(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 btVoronoiSimplexSolver::closest(btVector3& v) -{ - bool succes = updateClosestVectorAndPoints(); - v = m_cachedV; - return succes; -} - - - -btScalar btVoronoiSimplexSolver::maxVertex() -{ - int i, numverts = numVertices(); - btScalar maxV = btScalar(0.); - for (i=0;i= btScalar(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 - 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); - return true; - //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) - { - 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 - 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); - 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 - 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) - } - - // 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); - - return true; -// 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); - - 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))) - { - return -1; - } -#else - if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) - { -// printf("affine dependent/degenerate\n");// - return -1; - } -#endif - -#endif - // Points on opposite sides if expression signs are opposite - 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) -{ - btSubSimplexClosestResult 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; - } - - - 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) { - 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); - btVector3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - - 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.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); - btVector3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - - 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] - ); - - } - } - // Repeat test for face bdc - - - if (pointOutsideBDC) - { - closestPtPointTriangle(p, b, d, c,tempResult); - btVector3 q = tempResult.m_closestPointOnSimplex; - //convert result bitmask! - btScalar 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/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h deleted file mode 100644 index 2f389e27e..000000000 --- a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ /dev/null @@ -1,181 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef BT_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 -#define BT_USE_EQUAL_VERTEX_THRESHOLD -#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f - - -struct btUsageBitfield{ - btUsageBitfield() - { - 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 btSubSimplexClosestResult -{ - btVector3 m_closestPointOnSimplex; - //MASK for m_usedVertices - //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]; - bool m_degenerate; - - void reset() - { - m_degenerate = false; - setBarycentricCoordinates(); - m_usedVertices.reset(); - } - bool isValid() - { - bool valid = (m_barycentricCoords[0] >= 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.)) - { - 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 -#else -ATTRIBUTE_ALIGNED16(class) btVoronoiSimplexSolver : public btSimplexSolverInterface -#endif -{ -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - 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_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 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) - { - } - void reset(); - - void addVertex(const btVector3& w, const btVector3& p, const btVector3& q); - - void setEqualVertexThreshold(btScalar threshold) - { - m_equalVertexThreshold = threshold; - } - - btScalar getEqualVertexThreshold() const - { - return m_equalVertexThreshold; - } - - bool closest(btVector3& v); - - btScalar maxVertex(); - - bool fullSimplex() const - { - return (m_numVertices == 4); - } - - int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const; - - bool inSimplex(const btVector3& w); - - void backup_closest(btVector3& v) ; - - bool emptySimplex() const ; - - void compute_points(btVector3& p1, btVector3& p2) ; - - int numVertices() const - { - return m_numVertices; - } - - -}; - -#endif //BT_VORONOI_SIMPLEX_SOLVER_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h b/WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h deleted file mode 100644 index dffb06dfe..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h +++ /dev/null @@ -1,47 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CHARACTER_CONTROLLER_INTERFACE_H -#define BT_CHARACTER_CONTROLLER_INTERFACE_H - -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btActionInterface.h" - -class btCollisionShape; -class btRigidBody; -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; - - virtual void preStep ( btCollisionWorld* collisionWorld) = 0; - virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; - virtual bool canJump () const = 0; - virtual void jump () = 0; - - virtual bool onGround () const = 0; - virtual void setUpInterpolate (bool value) = 0; -}; - -#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.cpp b/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.cpp deleted file mode 100644 index 8f1cd20bf..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.cpp +++ /dev/null @@ -1,770 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/CollisionDispatch/btGhostObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "LinearMath/btDefaultMotionState.h" -#include "btKinematicCharacterController.h" - - -// static helper method -static btVector3 -getNormalizedVector(const btVector3& v) -{ - btVector3 n = v.normalized(); - if (n.length() < SIMD_EPSILON) { - n.setValue(0, 0, 0); - } - return n; -} - - -///@todo Interact with dynamic objects, -///Ride kinematicly animated platforms properly -///More realistic (or maybe just a config option) falling -/// -> Should integrate falling velocity manually and use that in stepDown() -///Support jumping -///Support ducking -class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback -{ -public: - btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) - { - m_me = me; - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) - { - if (rayResult.m_collisionObject == m_me) - return 1.0; - - return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); - } -protected: - btCollisionObject* m_me; -}; - -class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback -{ -public: - btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me, 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) - { - if (convexResult.m_hitCollisionObject == m_me) - return btScalar(1.0); - - if (!convexResult.m_hitCollisionObject->hasContactResponse()) - return btScalar(1.0); - - btVector3 hitNormalWorld; - if (normalInWorldSpace) - { - hitNormalWorld = convexResult.m_hitNormalLocal; - } else - { - ///need to transform normal into worldspace - hitNormalWorld = convexResult.m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; - } - - btScalar dotUp = m_up.dot(hitNormalWorld); - if (dotUp < m_minSlopeDot) { - return btScalar(1.0); - } - - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); - } -protected: - btCollisionObject* m_me; - const btVector3 m_up; - btScalar m_minSlopeDot; -}; - -/* - * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal' - * - * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html - */ -btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal) -{ - return direction - (btScalar(2.0) * direction.dot(normal)) * normal; -} - -/* - * Returns the portion of 'direction' that is parallel to 'normal' - */ -btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal) -{ - btScalar magnitude = direction.dot(normal); - return normal * magnitude; -} - -/* - * Returns the portion of 'direction' that is perpindicular to 'normal' - */ -btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) -{ - return direction - parallelComponent(direction, normal); -} - -btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) -{ - m_upAxis = upAxis; - m_addedMargin = 0.02; - m_walkDirection.setValue(0,0,0); - m_useGhostObjectSweepTest = true; - m_ghostObject = ghostObject; - m_stepHeight = stepHeight; - m_turnAngle = btScalar(0.0); - m_convexShape=convexShape; - m_useWalkDirection = true; // use walk direction by default, legacy behavior - m_velocityTimeInterval = 0.0; - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; - m_gravity = 9.8 * 3 ; // 3G acceleration. - m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. - m_jumpSpeed = 10.0; // ? - m_wasOnGround = false; - m_wasJumping = false; - m_interpolateUp = true; - setMaxSlope(btRadians(45.0)); - m_currentStepOffset = 0; - full_drop = false; - bounce_fix = false; -} - -btKinematicCharacterController::~btKinematicCharacterController () -{ -} - -btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() -{ - return m_ghostObject; -} - -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 - // that is not in the previous cache contents from the last timestep, as will happen if we - // are pushed into a new AABB overlap. Unhandled this means the next convex sweep gets stuck. - // - // Do this by calling the broadphase's setAabb with the moved AABB, this will update the broadphase - // 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()); - - bool penetration = false; - - collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); - - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - - btScalar maxPen = btScalar(0.0); - for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) - { - m_manifoldArray.resize(0); - - btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; - - btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); - - if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) - 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++) - { - const btManifoldPoint&pt = manifold->getContactPoint(p); - - btScalar dist = pt.getDistance(); - - if (dist < 0.0) - { - if (dist < maxPen) - { - maxPen = dist; - m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? - - } - m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); - penetration = true; - } 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]); - return penetration; -} - -void btKinematicCharacterController::stepUp ( btCollisionWorld* world) -{ - // phase 1: up - btTransform start, end; - m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f)); - - start.setIdentity (); - end.setIdentity (); - - /* FIXME: Handle penetration properly */ - start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin)); - end.setOrigin (m_targetPosition); - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071)); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); - } - else - { - world->convexSweepTest (m_convexShape, start, end, callback); - } - - if (callback.hasHit()) - { - // Only modify the position if the hit was a slope and not a wall or ceiling. - if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0) - { - // we moved up only a fraction of the step height - m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; - if (m_interpolateUp == true) - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - else - m_currentPosition = m_targetPosition; - } - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; - } else { - m_currentStepOffset = m_stepHeight; - m_currentPosition = m_targetPosition; - } -} - -void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) -{ - btVector3 movementDirection = m_targetPosition - m_currentPosition; - btScalar movementLength = movementDirection.length(); - if (movementLength>SIMD_EPSILON) - { - movementDirection.normalize(); - - btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal); - reflectDir.normalize(); - - btVector3 parallelDir, perpindicularDir; - - parallelDir = parallelComponent (reflectDir, hitNormal); - perpindicularDir = perpindicularComponent (reflectDir, hitNormal); - - m_targetPosition = m_currentPosition; - if (0)//tangentMag != 0.0) - { - btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength); -// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); - m_targetPosition += parComponent; - } - - if (normalMag != 0.0) - { - btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength); -// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); - m_targetPosition += perpComponent; - } - } else - { -// printf("movementLength don't normalize a zero vector\n"); - } -} - -void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove) -{ - // printf("m_normalizedDirection=%f,%f,%f\n", - // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]); - // phase 2: forward and strafe - btTransform start, end; - m_targetPosition = m_currentPosition + walkMove; - - start.setIdentity (); - end.setIdentity (); - - btScalar fraction = 1.0; - btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); -// printf("distance2=%f\n",distance2); - - if (m_touchingContact) - { - if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) - { - //interferes with step movement - //updateTargetPositionBasedOnCollision (m_touchingNormal); - } - } - - int maxIter = 10; - - while (fraction > btScalar(0.01) && maxIter-- > 0) - { - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); - btVector3 sweepDirNegative(m_currentPosition - m_targetPosition); - - 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); - - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } - - m_convexShape->setMargin(margin); - - - fraction -= callback.m_closestHitFraction; - - if (callback.hasHit()) - { - // we moved only a fraction - btScalar hitDistance; - hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); - -// m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - - updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); - btVector3 currentDir = m_targetPosition - m_currentPosition; - distance2 = currentDir.length2(); - if (distance2 > SIMD_EPSILON) - { - currentDir.normalize(); - /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ - if (currentDir.dot(m_normalizedDirection) <= btScalar(0.0)) - { - break; - } - } else - { -// printf("currentDir: don't normalize a zero vector\n"); - break; - } - - } else { - // we moved whole way - m_currentPosition = m_targetPosition; - } - - // if (callback.m_closestHitFraction == 0.f) - // break; - - } -} - -void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) -{ - btTransform start, end, end_double; - bool runonce = false; - - // phase 3: down - /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); - btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; - btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; - m_targetPosition -= (step_drop + gravity_drop);*/ - - btVector3 orig_position = m_targetPosition; - - btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; - - if(downVelocity > 0.0 && downVelocity > m_fallSpeed - && (m_wasOnGround || !m_wasJumping)) - downVelocity = m_fallSpeed; - - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); - m_targetPosition -= step_drop; - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); - callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - while (1) - { - start.setIdentity (); - end.setIdentity (); - - end_double.setIdentity (); - - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); - - //set double test for 2x the step drop, to check for a large drop vs small drop - end_double.setOrigin (m_targetPosition - step_drop); - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - - if (!callback.hasHit()) - { - //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) - m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - - if (!callback.hasHit()) - { - //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) - collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } - } - - btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; - bool has_hit = false; - if (bounce_fix == true) - has_hit = callback.hasHit() || callback2.hasHit(); - else - has_hit = callback2.hasHit(); - - if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false - && (m_wasOnGround || !m_wasJumping)) - { - //redo the velocity calculation when falling a small amount, for fast stairs motion - //for larger falls, use the smoother/slower interpolated movement by not touching the target position - - m_targetPosition = orig_position; - downVelocity = m_stepHeight; - - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); - m_targetPosition -= step_drop; - runonce = true; - continue; //re-run previous tests - } - break; - } - - if (callback.hasHit() || runonce == true) - { - // we dropped a fraction of the height -> hit floor - - btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; - - //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); - - if (bounce_fix == true) - { - if (full_drop == true) - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - else - //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); - } - else - 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 { - // we dropped the full height - - full_drop = true; - - if (bounce_fix == true) - { - 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 - downVelocity = m_fallSpeed; - step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); - m_targetPosition -= step_drop; - } - } - //printf("full drop - %g, %g\n", m_currentPosition.getY(), m_targetPosition.getY()); - - m_currentPosition = m_targetPosition; - } -} - - - -void btKinematicCharacterController::setWalkDirection -( -const btVector3& walkDirection -) -{ - m_useWalkDirection = true; - m_walkDirection = walkDirection; - m_normalizedDirection = getNormalizedVector(m_walkDirection); -} - - - -void btKinematicCharacterController::setVelocityForTimeInterval -( -const btVector3& velocity, -btScalar timeInterval -) -{ -// printf("setVelocity!\n"); -// printf(" interval: %f\n", timeInterval); -// printf(" velocity: (%f, %f, %f)\n", -// velocity.x(), velocity.y(), velocity.z()); - - m_useWalkDirection = false; - m_walkDirection = velocity; - m_normalizedDirection = getNormalizedVector(m_walkDirection); - m_velocityTimeInterval += timeInterval; -} - -void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld ) -{ - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; - m_wasOnGround = false; - m_wasJumping = false; - m_walkDirection.setValue(0,0,0); - m_velocityTimeInterval = 0.0; - - //clear pair cache - btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); - while (cache->getOverlappingPairArray().size() > 0) - { - cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); - } -} - -void btKinematicCharacterController::warp (const btVector3& origin) -{ - btTransform xform; - xform.setIdentity(); - xform.setOrigin (origin); - m_ghostObject->setWorldTransform (xform); -} - - -void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) -{ - - int numPenetrationLoops = 0; - m_touchingContact = false; - while (recoverFromPenetration (collisionWorld)) - { - numPenetrationLoops++; - m_touchingContact = true; - if (numPenetrationLoops > 4) - { - //printf("character could not recover from penetration = %d\n", numPenetrationLoops); - break; - } - } - - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - m_targetPosition = m_currentPosition; -// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); - - -} - -#include - -void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) -{ -// printf("playerStep(): "); -// printf(" dt = %f", dt); - - // quick check... - if (!m_useWalkDirection && m_velocityTimeInterval <= 0.0) { -// printf("\n"); - return; // no motion - } - - m_wasOnGround = onGround(); - - // Update fall velocity. - m_verticalVelocity -= m_gravity * dt; - if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) - { - m_verticalVelocity = m_jumpSpeed; - } - if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) - { - m_verticalVelocity = -btFabs(m_fallSpeed); - } - m_verticalOffset = m_verticalVelocity * dt; - - - btTransform xform; - xform = m_ghostObject->getWorldTransform (); - -// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); -// printf("walkSpeed=%f\n",walkSpeed); - - stepUp (collisionWorld); - if (m_useWalkDirection) { - stepForwardAndStrafe (collisionWorld, m_walkDirection); - } else { - //printf(" time: %f", m_velocityTimeInterval); - // still have some time left for moving! - btScalar dtMoving = - (dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval; - m_velocityTimeInterval -= dt; - - // how far will we move while we are moving? - btVector3 move = m_walkDirection * dtMoving; - - //printf(" dtMoving: %f", dtMoving); - - // okay, step - stepForwardAndStrafe(collisionWorld, move); - } - stepDown (collisionWorld, dt); - - // printf("\n"); - - xform.setOrigin (m_currentPosition); - m_ghostObject->setWorldTransform (xform); -} - -void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) -{ - m_fallSpeed = fallSpeed; -} - -void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) -{ - m_jumpSpeed = jumpSpeed; -} - -void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) -{ - m_maxJumpHeight = maxJumpHeight; -} - -bool btKinematicCharacterController::canJump () const -{ - return onGround(); -} - -void btKinematicCharacterController::jump () -{ - if (!canJump()) - return; - - m_verticalVelocity = m_jumpSpeed; - m_wasJumping = true; - -#if 0 - currently no jumping. - btTransform xform; - m_rigidBody->getMotionState()->getWorldTransform (xform); - btVector3 up = xform.getBasis()[1]; - up.normalize (); - btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0); - m_rigidBody->applyCentralImpulse (up * magnitude); -#endif -} - -void btKinematicCharacterController::setGravity(btScalar gravity) -{ - m_gravity = gravity; -} - -btScalar btKinematicCharacterController::getGravity() const -{ - return m_gravity; -} - -void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians) -{ - m_maxSlopeRadians = slopeRadians; - m_maxSlopeCosine = btCos(slopeRadians); -} - -btScalar btKinematicCharacterController::getMaxSlope() const -{ - return m_maxSlopeRadians; -} - -bool btKinematicCharacterController::onGround () const -{ - return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; -} - - -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) }; - - return sUpAxisDirection; -} - -void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer) -{ -} - -void btKinematicCharacterController::setUpInterpolate(bool value) -{ - m_interpolateUp = value; -} diff --git a/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.h b/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.h deleted file mode 100644 index add6f30a6..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.h +++ /dev/null @@ -1,170 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_KINEMATIC_CHARACTER_CONTROLLER_H -#define BT_KINEMATIC_CHARACTER_CONTROLLER_H - -#include "LinearMath/btVector3.h" - -#include "btCharacterControllerInterface.h" - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - - -class btCollisionShape; -class btConvexShape; -class btRigidBody; -class btCollisionWorld; -class btCollisionDispatcher; -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 -{ -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 - - btScalar m_verticalVelocity; - btScalar m_verticalOffset; - btScalar m_fallSpeed; - btScalar m_jumpSpeed; - 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_gravity; - - btScalar m_turnAngle; - - btScalar m_stepHeight; - - 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; - - //some internal variables - btVector3 m_currentPosition; - btScalar m_currentStepOffset; - btVector3 m_targetPosition; - - ///keep track of the contact manifolds - btManifoldArray m_manifoldArray; - - bool m_touchingContact; - btVector3 m_touchingNormal; - - bool m_wasOnGround; - bool m_wasJumping; - bool m_useGhostObjectSweepTest; - bool m_useWalkDirection; - btScalar m_velocityTimeInterval; - int m_upAxis; - - static btVector3* getUpAxisDirections(); - 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); - - 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); -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); - ~btKinematicCharacterController (); - - - ///btActionInterface interface - virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime) - { - preStep ( collisionWorld); - playerStep (collisionWorld, deltaTime); - } - - ///btActionInterface interface - void debugDraw(btIDebugDraw* debugDrawer); - - void setUpAxis (int axis) - { - if (axis < 0) - axis = 0; - if (axis > 2) - axis = 2; - m_upAxis = axis; - } - - /// This should probably be called setPositionIncrementPerSimulatorStep. - /// This is neither a direction nor a velocity, but the amount to - /// increment the position each simulation iteration, regardless - /// of dt. - /// This call will reset any velocity set by setVelocityForTimeInterval(). - 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 - /// to zero. - /// 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); - - void reset ( btCollisionWorld* collisionWorld ); - void warp (const btVector3& origin); - - void preStep ( btCollisionWorld* collisionWorld); - void playerStep ( btCollisionWorld* collisionWorld, btScalar dt); - - void setFallSpeed (btScalar fallSpeed); - void setJumpSpeed (btScalar jumpSpeed); - void setMaxJumpHeight (btScalar maxJumpHeight); - bool canJump () const; - - void jump (); - - void setGravity(btScalar gravity); - btScalar getGravity() const; - - /// The max slope determines the maximum angle that the controller can walk up. - /// The slope angle is measured in radians. - void setMaxSlope(btScalar slopeRadians); - btScalar getMaxSlope() const; - - btPairCachingGhostObject* getGhostObject(); - void setUseGhostSweepTest(bool useGhostObjectSweepTest) - { - m_useGhostObjectSweepTest = useGhostObjectSweepTest; - } - - bool onGround () const; - void setUpInterpolate (bool value); -}; - -#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp deleted file mode 100644 index 15a4c92de..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ /dev/null @@ -1,1141 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - - -#include "btConeTwistConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMinMax.h" -#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) -{ - init(); -} - -btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) - :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), - m_angularOnly(false), - m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) -{ - m_rbBFrame = m_rbAFrame; - m_rbBFrame.setOrigin(btVector3(0., 0., 0.)); - init(); -} - - -void btConeTwistConstraint::init() -{ - m_angularOnly = false; - m_solveTwistLimit = false; - m_solveSwingLimit = false; - m_bMotorEnabled = false; - m_maxMotorImpulse = btScalar(-1); - - setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); - m_damping = btScalar(0.01); - m_fixThresh = CONETWIST_DEF_FIX_THRESH; - m_flags = 0; - m_linCFM = btScalar(0.f); - m_linERP = btScalar(0.7f); - m_angCFM = btScalar(0.f); -} - - -void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 3; - info->nub = 3; - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - if(m_solveSwingLimit) - { - info->m_numConstraintRows++; - info->nub--; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - if(m_solveTwistLimit) - { - info->m_numConstraintRows++; - info->nub--; - } - } -} - -void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - //always reserve 6 rows: object transform is not available on SPU - info->m_numConstraintRows = 6; - info->nub = 0; - -} - - -void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); -} - -void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) -{ - calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); - - btAssert(!m_useSolveConstraintObsolete); - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - 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); - } - // 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) - { - info->cfm[j*info->rowskip] = m_linCFM; - } - } - int row = 3; - int srow = row * info->rowskip; - btVector3 ax1; - // angular limits - if(m_solveSwingLimit) - { - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { - btTransform trA = transA*m_rbAFrame; - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - int srow1 = srow + info->rowskip; - J1[srow+0] = p[0]; - J1[srow+1] = p[1]; - J1[srow+2] = p[2]; - J1[srow1+0] = q[0]; - J1[srow1+1] = q[1]; - J1[srow1+2] = q[2]; - J2[srow+0] = -p[0]; - J2[srow+1] = -p[1]; - J2[srow+2] = -p[2]; - J2[srow1+0] = -q[0]; - J2[srow1+1] = -q[1]; - J2[srow1+2] = -q[2]; - btScalar fact = info->fps * m_relaxationFactor; - info->m_constraintError[srow] = fact * m_swingAxis.dot(p); - info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - info->m_lowerLimit[srow1] = -SIMD_INFINITY; - info->m_upperLimit[srow1] = SIMD_INFINITY; - srow = srow1 + info->rowskip; - } - else - { - ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps * m_biasFactor; - - info->m_constraintError[srow] = k * m_swingCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) - { - info->cfm[srow] = m_angCFM; - } - // m_swingCorrection is always positive or 0 - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - srow += info->rowskip; - } - } - if(m_solveTwistLimit) - { - ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; - btScalar *J1 = info->m_J1angularAxis; - btScalar *J2 = info->m_J2angularAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - btScalar k = info->fps * m_biasFactor; - info->m_constraintError[srow] = k * m_twistCorrection; - if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) - { - info->cfm[srow] = m_angCFM; - } - if(m_twistSpan > 0.0f) - { - - if(m_twistCorrection > 0.0f) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - srow += info->rowskip; - } -} - - - -void btConeTwistConstraint::buildJacobian() -{ - if (m_useSolveConstraintObsolete) - { - m_appliedImpulse = btScalar(0.); - m_accTwistLimitImpulse = btScalar(0.); - m_accSwingLimitImpulse = btScalar(0.); - m_accMotorImpulse = btVector3(0.,0.,0.); - - if (!m_angularOnly) - { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; - - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) - { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } - - btPlaneSpace1(normal[0], normal[1], normal[2]); - - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - } - } - - calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); - } -} - - - -void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - #ifndef __SPU__ - if (m_useSolveConstraintObsolete) - { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - - btScalar tau = btScalar(0.3); - - //linear part - if (!m_angularOnly) - { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1; - bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); - btVector3 vel2; - bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - - btVector3 ftorqueAxis1 = rel_pos1.cross(normal); - btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); - - } - } - - // apply motor - if (m_bMotorEnabled) - { - // compute current and predicted transforms - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btVector3 omegaA; bodyA.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(); - btTransformUtil::integrateTransform( - trBCur, zerovec, omegaB, timeStep, trBPred); - - // compute desired transforms in world - btTransform trPose(m_qTarget); - btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); - btTransform trADes = trBPred * trABDes; - btTransform trBDes = trAPred * trABDes.inverse(); - - // compute desired omegas in world - btVector3 omegaADes, omegaBDes; - - btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); - btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); - - // compute delta omegas - btVector3 dOmegaA = omegaADes - omegaA; - btVector3 dOmegaB = omegaBDes - omegaB; - - // compute weighted avg axis of dOmega (weighting based on inertias) - btVector3 axisA, axisB; - btScalar kAxisAInv = 0, kAxisBInv = 0; - - if (dOmegaA.length2() > SIMD_EPSILON) - { - axisA = dOmegaA.normalized(); - kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); - } - - if (dOmegaB.length2() > SIMD_EPSILON) - { - axisB = dOmegaB.normalized(); - kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); - } - - btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; - - static bool bDoTorque = true; - if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) - { - avgAxis.normalize(); - kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); - kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); - btScalar kInvCombined = kAxisAInv + kAxisBInv; - - btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / - (kInvCombined * kInvCombined); - - if (m_maxMotorImpulse >= 0) - { - btScalar fMaxImpulse = m_maxMotorImpulse; - if (m_bNormalizedMotorStrength) - fMaxImpulse = fMaxImpulse/kAxisAInv; - - btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; - btScalar newUnclampedMag = newUnclampedAccImpulse.length(); - if (newUnclampedMag > fMaxImpulse) - { - newUnclampedAccImpulse.normalize(); - newUnclampedAccImpulse *= fMaxImpulse; - impulse = newUnclampedAccImpulse - m_accMotorImpulse; - } - m_accMotorImpulse += impulse; - } - - btScalar impulseMag = impulse.length(); - btVector3 impulseAxis = impulse / impulseMag; - - bodyA.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 - { - 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)); - btVector3 impulse = m_damping * m_kDamping * relVel; - - 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); - } - } - - // joint limits - { - ///solve angular part - btVector3 angVelA; - bodyA.internalGetAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.internalGetAngularVelocity(angVelB); - - // solve swing limit - if (m_solveSwingLimit) - { - btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; - btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); - if (relSwingVel > 0) - amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; - btScalar impulseMag = amplitude * m_kSwing; - - // Clamp the accumulated impulse - btScalar temp = m_accSwingLimitImpulse; - m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accSwingLimitImpulse - temp; - - btVector3 impulse = m_swingAxis * impulseMag; - - // don't let cone response affect twist - // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) - { - btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; - btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; - impulse = impulseNoTwistCouple; - } - - impulseMag = impulse.length(); - btVector3 noTwistSwingAxis = impulse / impulseMag; - - bodyA.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) - amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; - btScalar impulseMag = amplitude * m_kTwist; - - // Clamp the accumulated impulse - btScalar temp = m_accTwistLimitImpulse; - m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); - impulseMag = m_accTwistLimitImpulse - temp; - - // btVector3 impulse = m_twistAxis * impulseMag; - - bodyA.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__ -} - - - - -void btConeTwistConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - -#ifndef __SPU__ -void btConeTwistConstraint::calcAngleInfo() -{ - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - - btVector3 b1Axis1,b1Axis2,b1Axis3; - btVector3 b2Axis1,b2Axis2; - - b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); - b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); - - btScalar swing1=btScalar(0.),swing2 = btScalar(0.); - - btScalar swx=btScalar(0.),swy = btScalar(0.); - btScalar thresh = btScalar(10.); - btScalar fact; - - // Get Frame into world space - if (m_swingSpan1 >= btScalar(0.05f)) - { - b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis2); - swing1 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing1 *= fact; - } - - if (m_swingSpan2 >= btScalar(0.05f)) - { - b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); - swx = b2Axis1.dot(b1Axis1); - swy = b2Axis1.dot(b1Axis3); - swing2 = btAtan2Fast(swy, swx); - fact = (swy*swy + swx*swx) * thresh * thresh; - fact = fact / (fact + btScalar(1.0)); - swing2 *= fact; - } - - btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); - btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); - btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; - - if (EllipseAngle > 1.0f) - { - m_swingCorrection = EllipseAngle-1.0f; - m_solveSwingLimit = true; - // Calculate necessary axis & factors - m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); - m_swingAxis.normalize(); - btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; - m_swingAxis *= swingAxisSign; - } - - // Twist limits - if (m_twistSpan >= btScalar(0.)) - { - btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); - btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); - btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); - btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); - m_twistAngle = twist; - -// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); - btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); - if (twist <= -m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = -(twist + m_twistSpan); - m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - m_twistAxis *= -1.0f; - } - else if (twist > m_twistSpan*lockedFreeFactor) - { - m_twistCorrection = (twist - m_twistSpan); - m_solveTwistLimit = true; - m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; - m_twistAxis.normalize(); - } - } -} -#endif //__SPU__ - -static btVector3 vTwist(1,0,0); // twist axis in constraint's space - - - -void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) -{ - m_swingCorrection = btScalar(0.); - m_twistLimitSign = btScalar(0.); - m_solveTwistLimit = false; - m_solveSwingLimit = false; - // compute rotation of A wrt B (in constraint space) - if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) - { // it is assumed that setMotorTarget() was alredy called - // and motor target m_qTarget is within constraint limits - // TODO : split rotation to pure swing and pure twist - // compute desired transforms in world - btTransform trPose(m_qTarget); - btTransform trA = transA * m_rbAFrame; - btTransform trB = transB * m_rbBFrame; - btTransform trDeltaAB = trB * trPose * trA.inverse(); - btQuaternion qDeltaAB = trDeltaAB.getRotation(); - btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); - float swingAxisLen2 = swingAxis.length2(); - if(btFuzzyZero(swingAxisLen2)) - { - return; - } - m_swingAxis = swingAxis; - m_swingAxis.normalize(); - m_swingCorrection = qDeltaAB.getAngle(); - if(!btFuzzyZero(m_swingCorrection)) - { - m_solveSwingLimit = true; - } - return; - } - - - { - // compute rotation of A wrt B (in constraint space) - btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); - btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation(); - btQuaternion qAB = qB.inverse() * qA; - // split rotation into cone and twist - // (all this is done from B's perspective. Maybe I should be averaging axes...) - btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); - btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); - btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); - - if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) - { - btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; - computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); - - if (swingAngle > swingLimit * m_limitSoftness) - { - m_solveSwingLimit = true; - - // compute limit ratio: 0->1, where - // 0 == beginning of soft limit - // 1 == hard/real limit - m_swingLimitRatio = 1.f; - if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) - { - m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ - (swingLimit - swingLimit * m_limitSoftness); - } - - // swing correction tries to get back to soft limit - m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); - - // adjustment of swing axis (based on ellipse normal) - adjustSwingAxisToUseEllipseNormal(swingAxis); - - // Calculate necessary axis & factors - m_swingAxis = quatRotate(qB, -swingAxis); - - m_twistAxisA.setValue(0,0,0); - - m_kSwing = btScalar(1.) / - (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); - } - } - else - { - // you haven't set any limits; - // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) - // anyway, we have either hinge or fixed joint - btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); - btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); - btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0); - btVector3 target; - btScalar x = ivB.dot(ivA); - btScalar y = ivB.dot(jvA); - btScalar z = ivB.dot(kvA); - if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) - { // fixed. We'll need to add one more row to constraint - if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) - { - m_solveSwingLimit = true; - m_swingAxis = -ivB.cross(ivA); - } - } - else - { - if(m_swingSpan1 < m_fixThresh) - { // hinge around Y axis -// if(!(btFuzzyZero(y))) - if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) - { - m_solveSwingLimit = true; - if(m_swingSpan2 >= m_fixThresh) - { - y = btScalar(0.f); - btScalar span2 = btAtan2(z, x); - if(span2 > m_swingSpan2) - { - x = btCos(m_swingSpan2); - z = btSin(m_swingSpan2); - } - else if(span2 < -m_swingSpan2) - { - x = btCos(m_swingSpan2); - z = -btSin(m_swingSpan2); - } - } - } - } - else - { // hinge around Z axis -// if(!btFuzzyZero(z)) - if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) - { - m_solveSwingLimit = true; - if(m_swingSpan1 >= m_fixThresh) - { - z = btScalar(0.f); - btScalar span1 = btAtan2(y, x); - if(span1 > m_swingSpan1) - { - x = btCos(m_swingSpan1); - y = btSin(m_swingSpan1); - } - else if(span1 < -m_swingSpan1) - { - x = btCos(m_swingSpan1); - y = -btSin(m_swingSpan1); - } - } - } - } - target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; - target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; - target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; - target.normalize(); - m_swingAxis = -ivB.cross(target); - m_swingCorrection = m_swingAxis.length(); - m_swingAxis.normalize(); - } - } - - if (m_twistSpan >= btScalar(0.f)) - { - btVector3 twistAxis; - computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); - - if (m_twistAngle > m_twistSpan*m_limitSoftness) - { - m_solveTwistLimit = true; - - m_twistLimitRatio = 1.f; - if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) - { - m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ - (m_twistSpan - m_twistSpan * m_limitSoftness); - } - - // twist correction tries to get back to soft limit - m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); - - m_twistAxis = quatRotate(qB, -twistAxis); - - m_kTwist = btScalar(1.) / - (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + - computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); - } - - if (m_solveSwingLimit) - m_twistAxisA = quatRotate(qA, -twistAxis); - } - else - { - m_twistAngle = btScalar(0.f); - } - } -} - - - -// given a cone rotation in constraint space, (pre: twist must already be removed) -// this method computes its corresponding swing angle and axis. -// more interestingly, it computes the cone/swing limit (angle) for this cone "pose". -void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, - btScalar& swingAngle, // out - btVector3& vSwingAxis, // out - btScalar& swingLimit) // out -{ - swingAngle = qCone.getAngle(); - if (swingAngle > SIMD_EPSILON) - { - vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); - vSwingAxis.normalize(); -#if 0 - // 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.) - - // For starters, compute the direction from center to surface of ellipse. - // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. - // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) - btScalar xEllipse = vSwingAxis.y(); - btScalar yEllipse = -vSwingAxis.z(); - - // Now, we use the slope of the vector (using x/yEllipse) and find the length - // of the line that intersects the ellipse: - // x^2 y^2 - // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) - // a^2 b^2 - // Do the math and it should be clear. - - swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 - if (fabs(xEllipse) > SIMD_EPSILON) - { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); - btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); - norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); - btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); - } - - // test! - /*swingLimit = m_swingSpan2; - if (fabs(vSwingAxis.z()) > SIMD_EPSILON) - { - btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; - btScalar sinphi = m_swingSpan2 / sqrt(mag_2); - btScalar phi = asin(sinphi); - btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); - btScalar alpha = 3.14159f - theta - phi; - btScalar sinalpha = sin(alpha); - swingLimit = m_swingSpan1 * sinphi/sinalpha; - }*/ - } - else if (swingAngle < 0) - { - // this should never happen! -#if 0 - btAssert(0); -#endif - } -} - -btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const -{ - // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) - btScalar xEllipse = btCos(fAngleInRadians); - btScalar yEllipse = btSin(fAngleInRadians); - - // Use the slope of the vector (using x/yEllipse) and find the length - // of the line that intersects the ellipse: - // x^2 y^2 - // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) - // a^2 b^2 - // Do the math and it should be clear. - - float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) - if (fabs(xEllipse) > SIMD_EPSILON) - { - btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); - btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); - norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); - btScalar swingLimit2 = (1 + surfaceSlope2) / norm; - swingLimit = sqrt(swingLimit2); - } - - // convert into point in constraint space: - // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively - btVector3 vSwingAxis(0, xEllipse, -yEllipse); - btQuaternion qSwing(vSwingAxis, swingLimit); - btVector3 vPointInConstraintSpace(fLength,0,0); - return quatRotate(qSwing, vPointInConstraintSpace); -} - -// given a twist rotation in constraint space, (pre: cone must already be removed) -// this method computes its corresponding angle and axis. -void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, - btScalar& twistAngle, // out - btVector3& vTwistAxis) // out -{ - btQuaternion qMinTwist = qTwist; - twistAngle = qTwist.getAngle(); - - if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qMinTwist = -(qTwist); - twistAngle = qMinTwist.getAngle(); - } - if (twistAngle < 0) - { - // this should never happen -#if 0 - btAssert(0); -#endif - } - - vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); - if (twistAngle > SIMD_EPSILON) - vTwistAxis.normalize(); -} - - -void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const -{ - // the swing axis is computed as the "twist-free" cone rotation, - // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). - // so, if we're outside the limits, the closest way back inside the cone isn't - // along the vector back to the center. better (and more stable) to use the ellipse normal. - - // convert swing axis to direction from center to surface of ellipse - // (ie. rotate 2D vector by PI/2) - btScalar y = -vSwingAxis.z(); - btScalar z = vSwingAxis.y(); - - // do the math... - if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. - { - // compute gradient/normal of ellipse surface at current "point" - btScalar grad = y/z; - grad *= m_swingSpan2 / m_swingSpan1; - - // adjust y/z to represent normal at point (instead of vector to point) - if (y > 0) - y = fabs(grad * z); - else - y = -fabs(grad * z); - - // convert ellipse direction back to swing axis - vSwingAxis.setZ(-y); - vSwingAxis.setY( z); - vSwingAxis.normalize(); - } -} - - - -void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) -{ - btTransform trACur = m_rbA.getCenterOfMassTransform(); - btTransform trBCur = m_rbB.getCenterOfMassTransform(); -// btTransform trABCur = trBCur.inverse() * trACur; -// btQuaternion qABCur = trABCur.getRotation(); -// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); - //btQuaternion qConstraintCur = trConstraintCur.getRotation(); - - btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); - setMotorTargetInConstraintSpace(qConstraint); -} - - -void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) -{ - m_qTarget = q; - - // clamp motor target to within limits - { - btScalar softness = 1.f;//m_limitSoftness; - - // split into twist and cone - btVector3 vTwisted = quatRotate(m_qTarget, vTwist); - btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); - btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); - - // clamp cone - if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) - { - btScalar swingAngle, swingLimit; btVector3 swingAxis; - computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); - - if (fabs(swingAngle) > SIMD_EPSILON) - { - if (swingAngle > swingLimit*softness) - swingAngle = swingLimit*softness; - else if (swingAngle < -swingLimit*softness) - swingAngle = -swingLimit*softness; - qTargetCone = btQuaternion(swingAxis, swingAngle); - } - } - - // clamp twist - if (m_twistSpan >= btScalar(0.05f)) - { - btScalar twistAngle; btVector3 twistAxis; - computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); - - if (fabs(twistAngle) > SIMD_EPSILON) - { - // eddy todo: limitSoftness used here??? - if (twistAngle > m_twistSpan*softness) - twistAngle = m_twistSpan*softness; - else if (twistAngle < -m_twistSpan*softness) - twistAngle = -m_twistSpan*softness; - qTargetTwist = btQuaternion(twistAxis, twistAngle); - } - } - - m_qTarget = qTargetCone * qTargetTwist; - } -} - -///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) - { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - if((axis >= 0) && (axis < 3)) - { - m_linERP = value; - m_flags |= BT_CONETWIST_FLAGS_LIN_ERP; - } - else - { - m_biasFactor = value; - } - break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - if((axis >= 0) && (axis < 3)) - { - m_linCFM = value; - m_flags |= BT_CONETWIST_FLAGS_LIN_CFM; - } - else - { - m_angCFM = value; - m_flags |= BT_CONETWIST_FLAGS_ANG_CFM; - } - break; - default: - btAssertConstrParams(0); - break; - } -} - -///return the local value of parameter -btScalar btConeTwistConstraint::getParam(int num, int axis) const -{ - btScalar retVal = 0; - switch(num) - { - 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)) - { - retVal = m_biasFactor; - } - else - { - btAssertConstrParams(0); - } - break; - 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)) - { - btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM); - retVal = m_angCFM; - } - else - { - btAssertConstrParams(0); - } - break; - default : - btAssertConstrParams(0); - } - return retVal; -} - - -void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) -{ - m_rbAFrame = frameA; - m_rbBFrame = frameB; - buildJacobian(); - //calculateTransforms(); -} - - - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h deleted file mode 100644 index 1735b524d..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ /dev/null @@ -1,381 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -Written by: Marcus Hennix -*/ - - - -/* -Overview: - -btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc). -It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint". -It divides the 3 rotational DOFs into swing (movement within a cone) and twist. -Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape. -(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.) - -In the contraint's frame of reference: -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 - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -#ifdef BT_USE_DOUBLE_PRECISION -#define btConeTwistConstraintData2 btConeTwistConstraintDoubleData -#define btConeTwistConstraintDataName "btConeTwistConstraintDoubleData" -#else -#define btConeTwistConstraintData2 btConeTwistConstraintData -#define btConeTwistConstraintDataName "btConeTwistConstraintData" -#endif //BT_USE_DOUBLE_PRECISION - - -class btRigidBody; - -enum btConeTwistFlags -{ - BT_CONETWIST_FLAGS_LIN_CFM = 1, - BT_CONETWIST_FLAGS_LIN_ERP = 2, - BT_CONETWIST_FLAGS_ANG_CFM = 4 -}; - -///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) -ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint -{ -#ifdef IN_PARALLELL_SOLVER -public: -#endif - btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - - btTransform m_rbAFrame; - btTransform m_rbBFrame; - - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; - - btScalar m_damping; - - btScalar m_swingSpan1; - btScalar m_swingSpan2; - btScalar m_twistSpan; - - btScalar m_fixThresh; - - btVector3 m_swingAxis; - btVector3 m_twistAxis; - - btScalar m_kSwing; - btScalar m_kTwist; - - btScalar m_twistLimitSign; - btScalar m_swingCorrection; - btScalar m_twistCorrection; - - btScalar m_twistAngle; - - btScalar m_accSwingLimitImpulse; - btScalar m_accTwistLimitImpulse; - - bool m_angularOnly; - bool m_solveTwistLimit; - bool m_solveSwingLimit; - - bool m_useSolveConstraintObsolete; - - // not yet used... - btScalar m_swingLimitRatio; - btScalar m_twistLimitRatio; - btVector3 m_twistAxisA; - - // motor - bool m_bMotorEnabled; - bool m_bNormalizedMotorStrength; - btQuaternion m_qTarget; - btScalar m_maxMotorImpulse; - btVector3 m_accMotorImpulse; - - // 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 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); - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - 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 solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); - - - void updateRHS(btScalar timeStep); - - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - void setAngularOnly(bool angularOnly) - { - m_angularOnly = angularOnly; - } - - void setLimit(int limitIndex,btScalar limitValue) - { - switch (limitIndex) - { - case 3: - { - m_twistSpan = limitValue; - break; - } - case 4: - { - m_swingSpan2 = limitValue; - break; - } - case 5: - { - m_swingSpan1 = limitValue; - break; - } - default: - { - } - }; - } - - // setLimit(), a few notes: - // _softness: - // 0->1, recommend ~0.8->1. - // describes % of limits where movement is free. - // beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached. - // _biasFactor: - // 0->1?, recommend 0.3 +/-0.3 or so. - // strength with which constraint resists zeroth order (angular, not angular velocity) limit violation. - // __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) - { - m_swingSpan1 = _swingSpan1; - m_swingSpan2 = _swingSpan2; - m_twistSpan = _twistSpan; - - m_limitSoftness = _softness; - m_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; - } - - const btTransform& getAFrame() { return m_rbAFrame; }; - const btTransform& getBFrame() { return m_rbBFrame; }; - - inline int getSolveTwistLimit() - { - return m_solveTwistLimit; - } - - inline int getSolveSwingLimit() - { - return m_solveTwistLimit; - } - - inline btScalar getTwistLimitSign() - { - return m_twistLimitSign; - } - - void calcAngleInfo(); - void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - - inline btScalar getSwingSpan1() - { - return m_swingSpan1; - } - inline btScalar getSwingSpan2() - { - return m_swingSpan2; - } - inline btScalar getTwistSpan() - { - return m_twistSpan; - } - inline btScalar getTwistAngle() - { - return m_twistAngle; - } - bool isPastSwingLimit() { return m_solveSwingLimit; } - - void setDamping(btScalar damping) { m_damping = damping; } - - void enableMotor(bool b) { m_bMotorEnabled = b; } - void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; } - void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; } - - btScalar getFixThresh() { return m_fixThresh; } - void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; } - - // setMotorTarget: - // q: the desired rotation of bodyA wrt bodyB. - // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability) - // note: don't forget to enableMotor() - void setMotorTarget(const btQuaternion &q); - - // same as above, but q is the desired rotation of frameA wrt frameB in constraint space - void setMotorTargetInConstraintSpace(const btQuaternion &q); - - btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const; - - ///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 setFrames(const btTransform& frameA, const btTransform& frameB); - - const btTransform& getFrameOffsetA() const - { - return m_rbAFrame; - } - - const btTransform& getFrameOffsetB() const - { - return m_rbBFrame; - } - - - ///return the local value of parameter - virtual btScalar 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, btSerializer* serializer) const; - -}; - - - -struct btConeTwistConstraintDoubleData -{ - 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; - - - -}; - -#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION -///this structure is not used, except for loading pre-2.82 .bullet files -struct btConeTwistConstraintData -{ - 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_damping; - - char m_pad[4]; - -}; -#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION -// - -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 -{ - 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; - cone->m_limitSoftness = m_limitSoftness; - cone->m_biasFactor = m_biasFactor; - cone->m_relaxationFactor = m_relaxationFactor; - cone->m_damping = m_damping; - - return btConeTwistConstraintDataName; -} - - -#endif //BT_CONETWISTCONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConstraintSolver.h deleted file mode 100644 index 1ba1cd1e8..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ /dev/null @@ -1,64 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONSTRAINT_SOLVER_H -#define BT_CONSTRAINT_SOLVER_H - -#include "LinearMath/btScalar.h" - -class btPersistentManifold; -class btRigidBody; -class btCollisionObject; -class btTypedConstraint; -struct btContactSolverInfo; -struct btBroadphaseProxy; -class btIDebugDraw; -class btStackAlloc; -class btDispatcher; -/// btConstraintSolver provides solver interface - - -enum btConstraintSolverType -{ - BT_SEQUENTIAL_IMPULSE_SOLVER=1, - BT_MLCP_SOLVER=2 -}; - -class btConstraintSolver -{ - -public: - - virtual ~btConstraintSolver() {} - - 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 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; - - -}; - - - - -#endif //BT_CONSTRAINT_SOLVER_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.cpp deleted file mode 100644 index 9d60d9957..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btContactConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btContactSolverInfo.h" -#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() -{ - -} - -void btContactConstraint::setContactManifold(btPersistentManifold* contactManifold) -{ - m_contactManifold = *contactManifold; -} - -void btContactConstraint::getInfo1 (btConstraintInfo1* info) -{ - -} - -void btContactConstraint::getInfo2 (btConstraintInfo2* info) -{ - -} - -void btContactConstraint::buildJacobian() -{ - -} - - - - - -#include "btContactConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btContactSolverInfo.h" -#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* 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; - - 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 penetrationImpulse = positionalError * jacDiagABInv; - btScalar velocityImpulse = velocityError * jacDiagABInv; - - 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; -} - - -//bilateral constraint between two dynamic objects -void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, - btRigidBody& body2, const btVector3& pos2, - btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) -{ - (void)timeStep; - (void)distance; - - - btScalar normalLenSqr = normal.length2(); - btAssert(btFabs(normalLenSqr) < btScalar(1.1)); - if (normalLenSqr > btScalar(1.1)) - { - impulse = btScalar(0.); - return; - } - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - - btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), - body2.getCenterOfMassTransform().getBasis().transpose(), - rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), - body2.getInvInertiaDiagLocal(),body2.getInvMass()); - - btScalar jacDiagAB = jac.getDiagonal(); - btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; - - btScalar rel_vel = jac.getRelativeVelocity( - body1.getLinearVelocity(), - body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), - body2.getLinearVelocity(), - body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); - btScalar a; - a=jacDiagABInv; - - - rel_vel = normal.dot(vel); - - //todo: move this into proper structure - btScalar contactDamping = btScalar(0.2); - -#ifdef ONLY_USE_LINEAR_MASS - btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); - impulse = - contactDamping * rel_vel * massTerm; -#else - btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; - impulse = velocityImpulse; -#endif -} - - - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.h deleted file mode 100644 index 477c79d17..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ /dev/null @@ -1,71 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONTACT_CONSTRAINT_H -#define BT_CONTACT_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" -#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 -{ -protected: - - btPersistentManifold m_contactManifold; - -public: - - - btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB); - - void setContactManifold(btPersistentManifold* contactManifold); - - btPersistentManifold* getContactManifold() - { - return &m_contactManifold; - } - - const btPersistentManifold* getContactManifold() const - { - return &m_contactManifold; - } - - virtual ~btContactConstraint(); - - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - ///obsolete methods - 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); - - -///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); - - - -#endif //BT_CONTACT_CONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactSolverInfo.h deleted file mode 100644 index c07e9bbd8..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ /dev/null @@ -1,159 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONTACT_SOLVER_INFO -#define BT_CONTACT_SOLVER_INFO - -#include "LinearMath/btScalar.h" - -enum btSolverMode -{ - SOLVER_RANDMIZE_ORDER = 1, - SOLVER_FRICTION_SEPARATE = 2, - SOLVER_USE_WARMSTARTING = 4, - SOLVER_USE_2_FRICTION_DIRECTIONS = 16, - SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, - SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, - SOLVER_CACHE_FRIENDLY = 128, - SOLVER_SIMD = 256, - SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, - SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 -}; - -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; - btScalar m_erp;//used as Baumgarte factor - btScalar m_erp2;//used in Split Impulse - btScalar m_globalCfm;//constraint force mixing - int m_splitImpulse; - btScalar m_splitImpulsePenetrationThreshold; - btScalar m_splitImpulseTurnErp; - btScalar m_linearSlop; - btScalar m_warmstartingFactor; - - int m_solverMode; - int m_restingContactRestitutionThreshold; - int m_minimumSolverBatchSize; - btScalar m_maxGyroscopicForce; - btScalar m_singleAxisRollingFrictionThreshold; - - -}; - -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_restitution = btScalar(0.); - m_maxErrorReduction = btScalar(20.); - m_numIterations = 10; - m_erp = btScalar(0.2); - m_erp2 = btScalar(0.8); - m_globalCfm = 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_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; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::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 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; - 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_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 //BT_CONTACT_SOLVER_INFO diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp deleted file mode 100644 index f93a3280f..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#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) -:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) -{ - m_pivotInA = frameInA.getOrigin(); - m_pivotInB = frameInB.getOrigin(); - m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); - -} - -btFixedConstraint::~btFixedConstraint () -{ -} - - -void btFixedConstraint::getInfo1 (btConstraintInfo1* info) -{ - info->m_numConstraintRows = 6; - info->nub = 6; -} - -void btFixedConstraint::getInfo2 (btConstraintInfo2* info) -{ - //fix the 3 linear degrees of freedom - - - const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); - const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); - const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis(); - - - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - - btVector3 a1 = worldOrnA*m_pivotInA; - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - if (info->m_J2linearAxis) - { - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; - } - - btVector3 a2 = worldOrnB*m_pivotInB; - - { - // btVector3 a2n = -a2; - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - // set right hand side for the linear dofs - btScalar k = info->fps * info->erp; - - btVector3 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 - - btVector3 diff; - btScalar angle; - btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse(); - btQuaternion qrelCur; - mrelCur.getRotation(qrelCur); - btTransformUtil::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/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.h deleted file mode 100644 index 697e319e2..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_FIXED_CONSTRAINT_H -#define BT_FIXED_CONSTRAINT_H - -#include "btTypedConstraint.h" - -ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint -{ - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btQuaternion m_relTargetAB; - -public: - btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); - - virtual ~btFixedConstraint(); - - - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - virtual void setParam(int num, btScalar value, int axis = -1) - { - btAssert(0); - } - virtual btScalar getParam(int num, int axis = -1) const - { - btAssert(0); - return 0.f; - } - -}; - -#endif //BT_FIXED_CONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.cpp deleted file mode 100644 index bcd457b67..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2012 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. -*/ - -/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou. - -#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 () -{ -} - -void btGearConstraint::getInfo1 (btConstraintInfo1* info) -{ - info->m_numConstraintRows = 1; - info->nub = 1; -} - -void btGearConstraint::getInfo2 (btConstraintInfo2* info) -{ - btVector3 globalAxisA, globalAxisB; - - 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]; - -} - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.h deleted file mode 100644 index f9afcb912..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.h +++ /dev/null @@ -1,152 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2012 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 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" -#else -#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; - -public: - 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); - - ///internal method used by the constraint solver, don't use them directly - virtual void getInfo2 (btConstraintInfo2* info); - - void setAxisA(btVector3& axisA) - { - m_axisInA = axisA; - } - void setAxisB(btVector3& axisB) - { - m_axisInB = axisB; - } - void setRatio(btScalar ratio) - { - m_ratio = ratio; - } - const btVector3& getAxisA() const - { - return m_axisInA; - } - const btVector3& getAxisB() const - { - return m_axisInB; - } - btScalar getRatio() const - { - return m_ratio; - } - - - virtual void setParam(int num, btScalar value, int axis = -1) - { - (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; - btAssert(0); - return 0.f; - } - - 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; -}; - - - - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btGearConstraintFloatData -{ - btTypedConstraintFloatData m_typeConstraintData; - - btVector3FloatData m_axisInA; - btVector3FloatData m_axisInB; - - float m_ratio; - char m_padding[4]; -}; - -struct btGearConstraintDoubleData -{ - btTypedConstraintDoubleData m_typeConstraintData; - - btVector3DoubleData m_axisInA; - btVector3DoubleData m_axisInB; - - double m_ratio; -}; - -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 -{ - btGearConstraintData* gear = (btGearConstraintData*)dataBuffer; - btTypedConstraint::serialize(&gear->m_typeConstraintData,serializer); - - m_axisInA.serialize( gear->m_axisInA ); - m_axisInB.serialize( gear->m_axisInB ); - - gear->m_ratio = m_ratio; - - return btGearConstraintDataName; -} - - - - - - -#endif //BT_GEAR_CONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp deleted file mode 100644 index bc2b5a85d..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ /dev/null @@ -1,1063 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -/* -2007-09-09 -Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#include "btGeneric6DofConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#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) -{ - 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) -{ - ///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; - return mat[i][j]; -} - - - -///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); -bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz) -{ - // // rot = cy*cz -cy*sz sy - // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx - // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy - // - - btScalar fi = btGetMatrixElem(mat,2); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); - xyz[1] = btAsin(btGetMatrixElem(mat,2)); - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); - return true; - } - else - { - // WARNING. Not unique. XA - ZA = -atan2(r10,r11) - xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = -SIMD_HALF_PI; - xyz[2] = btScalar(0.0); - return false; - } - } - else - { - // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) - xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - xyz[1] = SIMD_HALF_PI; - xyz[2] = 0.0; - } - return false; -} - -//////////////////////////// btRotationalLimitMotor //////////////////////////////////// - -int btRotationalLimitMotor::testLimitValue(btScalar test_value) -{ - if(m_loLimit>m_hiLimit) - { - m_currentLimit = 0;//Free from violation - return 0; - } - if (test_value < m_loLimit) - { - m_currentLimit = 1;//low limit violation - m_currentLimitError = test_value - m_loLimit; - 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) - { - 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; - return 2; - }; - - m_currentLimit = 0;//Free from violation - return 0; - -} - - - -btScalar btRotationalLimitMotor::solveAngularLimits( - btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btRigidBody * body1 ) -{ - if (needApplyTorques()==false) return 0.0f; - - btScalar target_velocity = m_targetVelocity; - btScalar maxMotorForce = m_maxMotorForce; - - //current error correction - if (m_currentLimit!=0) - { - target_velocity = -m_stopERP*m_currentLimitError/(timeStep); - maxMotorForce = m_maxLimitForce; - } - - maxMotorForce *= timeStep; - - // current velocity difference - - btVector3 angVelA = body0->getAngularVelocity(); - btVector3 angVelB = body1->getAngularVelocity(); - - btVector3 vel_diff; - vel_diff = angVelA-angVelB; - - - - btScalar rel_vel = axis.dot(vel_diff); - - // correction velocity - btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel); - - - if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON ) - { - return 0.0f;//no need for applying force - } - - - // correction impulse - btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv; - - // clip correction impulse - btScalar clippedMotorImpulse; - - ///@todo: should clip against accumulated impulse - if (unclippedMotorImpulse>0.0f) - { - clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse; - } - else - { - clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse; - } - - - // sort with accumulated impulses - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); - - btScalar oldaccumImpulse = m_accumulatedImpulse; - btScalar sum = oldaccumImpulse + clippedMotorImpulse; - m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - - clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse; - - btVector3 motorImp = clippedMotorImpulse * axis; - - body0->applyTorqueImpulse(motorImp); - body1->applyTorqueImpulse(-motorImp); - - return clippedMotorImpulse; - - -} - -//////////////////////////// End btRotationalLimitMotor //////////////////////////////////// - - - - -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - - -int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value) -{ - btScalar loLimit = m_lowerLimit[limitIndex]; - btScalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) - { - m_currentLimit[limitIndex] = 0;//Free from violation - m_currentLimitError[limitIndex] = btScalar(0.f); - return 0; - } - - if (test_value < loLimit) - { - m_currentLimit[limitIndex] = 2;//low limit violation - m_currentLimitError[limitIndex] = test_value - loLimit; - return 2; - } - else if (test_value> hiLimit) - { - m_currentLimit[limitIndex] = 1;//High limit violation - m_currentLimitError[limitIndex] = test_value - hiLimit; - return 1; - }; - - m_currentLimit[limitIndex] = 0;//Free from violation - m_currentLimitError[limitIndex] = btScalar(0.f); - return 0; -} - - - -btScalar btTranslationalLimitMotor::solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos) -{ - - ///find relative velocity - // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition(); - // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition(); - btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); - - btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel = axis_normal_on_a.dot(vel); - - - - /// apply displacement correction - - //positional error (zeroth order error) - btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a); - btScalar lo = btScalar(-BT_LARGE_FLOAT); - btScalar hi = btScalar(BT_LARGE_FLOAT); - - btScalar minLimit = m_lowerLimit[limit_index]; - btScalar maxLimit = m_upperLimit[limit_index]; - - //handle the limits - if (minLimit < maxLimit) - { - { - if (depth > maxLimit) - { - depth -= maxLimit; - lo = btScalar(0.); - - } - else - { - if (depth < minLimit) - { - depth -= minLimit; - hi = btScalar(0.); - } - else - { - return 0.0f; - } - } - } - } - - btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv; - - - - - btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index]; - btScalar sum = oldNormalImpulse + normalImpulse; - m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum; - normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; - - btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - body1.applyImpulse( impulse_vector, rel_pos1); - body2.applyImpulse(-impulse_vector, rel_pos2); - - - - return normalImpulse; -} - -//////////////////////////// btTranslationalLimitMotor //////////////////////////////////// - -void btGeneric6DofConstraint::calculateAngleInfo() -{ - btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. - btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); - btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); - - m_calculatedAxis[1] = axis2.cross(axis0); - m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2); - m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]); - - m_calculatedAxis[0].normalize(); - m_calculatedAxis[1].normalize(); - m_calculatedAxis[2].normalize(); - -} - -void btGeneric6DofConstraint::calculateTransforms() -{ - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - m_calculatedTransformA = transA * m_frameInA; - m_calculatedTransformB = transB * m_frameInB; - calculateLinearInfo(); - calculateAngleInfo(); - 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)) - { - m_factA = miB / miS; - } - else - { - m_factA = btScalar(0.5f); - } - m_factB = btScalar(1.0f) - m_factA; - } -} - - - -void btGeneric6DofConstraint::buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW) -{ - new (&jacLinear) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normalWorld, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); -} - - - -void btGeneric6DofConstraint::buildAngularJacobian( - btJacobianEntry & jacAngular,const btVector3 & jointAxisW) -{ - new (&jacAngular) btJacobianEntry(jointAxisW, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - -} - - - -bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) -{ - btScalar angle = m_calculatedAxisAngleDiff[axis_index]; - angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); - m_angularLimits[axis_index].m_currentPosition = angle; - //test limits - m_angularLimits[axis_index].testLimitValue(angle); - return m_angularLimits[axis_index].needApplyTorques(); -} - - - -void btGeneric6DofConstraint::buildJacobian() -{ -#ifndef __SPU__ - if (m_useSolveConstraintObsolete) - { - - // Clear accumulated impulses for the next simulation step - m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - int i; - for(i = 0; i < 3; i++) - { - m_angularLimits[i].m_accumulatedImpulse = btScalar(0.); - } - //calculates transform - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - - // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin(); - // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin(); - calcAnchorPos(); - btVector3 pivotAInW = m_AnchorPos; - btVector3 pivotBInW = m_AnchorPos; - - // not used here - // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 normalWorld; - //linear part - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) - { - if (m_useLinearReferenceFrameA) - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - else - normalWorld = m_calculatedTransformB.getBasis().getColumn(i); - - buildLinearJacobian( - m_jacLinear[i],normalWorld , - pivotAInW,pivotBInW); - - } - } - - // angular part - for (i=0;i<3;i++) - { - //calculates error angle - if (testAngularLimitMotor(i)) - { - normalWorld = this->getAxis(i); - // Create angular atom - buildAngularJacobian(m_jacAng[i],normalWorld); - } - } - - } -#endif //__SPU__ - -} - - -void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - info->m_numConstraintRows = 0; - info->nub = 6; - int i; - //test linear limits - for(i = 0; i < 3; i++) - { - if(m_linearLimits.needApplyForce(i)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - //test angular limits - for (i=0;i<3 ;i++ ) - { - if(testAngularLimitMotor(i)) - { - info->m_numConstraintRows++; - info->nub--; - } - } - } -} - -void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - //pre-allocate all 6 - info->m_numConstraintRows = 6; - info->nub = 0; - } -} - - -void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info) -{ - btAssert(!m_useSolveConstraintObsolete); - - const btTransform& transA = m_rbA.getCenterOfMassTransform(); - const btTransform& transB = m_rbB.getCenterOfMassTransform(); - const btVector3& linVelA = m_rbA.getLinearVelocity(); - const btVector3& linVelB = m_rbB.getLinearVelocity(); - const btVector3& angVelA = m_rbA.getAngularVelocity(); - const btVector3& angVelB = m_rbB.getAngularVelocity(); - - 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 btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ - - btAssert(!m_useSolveConstraintObsolete); - //prepare constraint - calculateTransforms(transA,transB); - - int i; - 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 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; - //solve linear limits - btRotationalLimitMotor limot; - for (int i=0;i<3 ;i++ ) - { - if(m_linearLimits.needApplyForce(i)) - { // re-use rotational motor code - limot.m_bounce = btScalar(0.f); - limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; - limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; - limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; - limot.m_damping = m_linearLimits.m_damping; - limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; - limot.m_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) - { - 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 btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ - btGeneric6DofConstraint * d6constraint = this; - int row = row_offset; - //solve angular limits - for (int i=0;i<3 ;i++ ) - { - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) - { - btVector3 axis = d6constraint->getAxis(i); - int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); - if(!(flags & BT_6DOF_FLAGS_CFM_NORM)) - { - m_angularLimits[i].m_normalCFM = info->cfm[0]; - } - if(!(flags & BT_6DOF_FLAGS_CFM_STOP)) - { - m_angularLimits[i].m_stopCFM = info->cfm[0]; - } - 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); - } - } - - return row; -} - - - - -void btGeneric6DofConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - -void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB) -{ - m_frameInA = frameA; - m_frameInB = frameB; - buildJacobian(); - calculateTransforms(); -} - - - -btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const -{ - return m_calculatedAxis[axis_index]; -} - - -btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const -{ - return m_calculatedLinearDiff[axisIndex]; -} - - -btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const -{ - return m_calculatedAxisAngleDiff[axisIndex]; -} - - - -void btGeneric6DofConstraint::calcAnchorPos(void) -{ - btScalar imA = m_rbA.getInvMass(); - btScalar imB = m_rbB.getInvMass(); - btScalar weight; - if(imB == btScalar(0.0)) - { - weight = btScalar(1.0); - } - else - { - weight = imA / (imA + imB); - } - const btVector3& pA = m_calculatedTransformA.getOrigin(); - const btVector3& pB = m_calculatedTransformB.getOrigin(); - m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight); - return; -} - - - -void btGeneric6DofConstraint::calculateLinearInfo() -{ - m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin(); - m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff; - for(int i = 0; i < 3; i++) - { - m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i]; - m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]); - } -} - - - -int btGeneric6DofConstraint::get_limit_motor_info2( - btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) -{ - int srow = row * info->rowskip; - int powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - if (powered || limit) - { // if the joint is powered, or has joint limits, add in the extra row - btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - - if((!rotational)) - { - if (m_useOffsetForConstraintFrame) - { - btVector3 tmpA, tmpB, relA, relB; - // get vector from bodyB to frameB in WCS - relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); - // get its projection to constraint axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to constraint axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - // get desired offset between frames A and B along constraint axis - btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; - // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis - btVector3 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 - { - btVector3 ltd; // Linear Torque Decoupling vector - btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin(); - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = ltd[0]; - info->m_J1angularAxis[srow+1] = ltd[1]; - info->m_J1angularAxis[srow+2] = ltd[2]; - - c = m_calculatedTransformB.getOrigin() - transB.getOrigin(); - ltd = -c.cross(ax1); - info->m_J2angularAxis[srow+0] = ltd[0]; - info->m_J2angularAxis[srow+1] = ltd[1]; - info->m_J2angularAxis[srow+2] = ltd[2]; - } - } - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; - info->m_constraintError[srow] = btScalar(0.f); - if (powered) - { - info->cfm[srow] = limot->m_normalCFM; - 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); - 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_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] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - if (limit == 1) - { - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = angVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= angVelB.dot(ax1); - } - else - { - vel = linVelA.dot(ax1); -//make sure that if no body -> angVelB == zero vec -// if (body1) - vel -= linVelB.dot(ax1); - } - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - else - { - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->m_constraintError[srow]) - info->m_constraintError[srow] = newc; - } - } - } - } - } - return 1; - } - else return 0; -} - - - - - - - ///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)) - { - switch(num) - { - 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 : - m_linearLimits.m_stopCFM[axis] = value; - m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - 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 : - btAssertConstrParams(0); - } - } - else if((axis >=3) && (axis < 6)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - m_angularLimits[axis - 3].m_stopERP = value; - m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - 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 : - m_angularLimits[axis - 3].m_normalCFM = value; - m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT); - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } -} - - ///return the local value of parameter -btScalar btGeneric6DofConstraint::getParam(int num, int axis) const -{ - btScalar retVal = 0; - if((axis >= 0) && (axis < 3)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_stopERP[axis]; - break; - 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 : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_linearLimits.m_normalCFM[axis]; - break; - default : - btAssertConstrParams(0); - } - } - else if((axis >=3) && (axis < 6)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_stopERP; - break; - 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 : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT))); - retVal = m_angularLimits[axis - 3].m_normalCFM; - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } - return retVal; -} - - - -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 - - btTransform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - - // now get constraint frame in local coordinate systems - m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; - - calculateTransforms(); -} diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h deleted file mode 100644 index 431a52416..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ /dev/null @@ -1,640 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev -/// Added support for generic constraint solver through getInfo1/getInfo2 methods - -/* -2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - - -#ifndef BT_GENERIC_6DOF_CONSTRAINT_H -#define BT_GENERIC_6DOF_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - - - -#ifdef BT_USE_DOUBLE_PRECISION -#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2 -#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2" -#else -#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; - - //!@} - - //! 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; - 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; - } - - 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; - } - - - - //! 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(btScalar test_value); - - //! apply the correction impulses for two bodies - 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 - - 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_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; - - 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, 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); - - -}; - -enum bt6DofFlags -{ - BT_6DOF_FLAGS_CFM_NORM = 1, - BT_6DOF_FLAGS_CFM_STOP = 2, - BT_6DOF_FLAGS_ERP_STOP = 4 -}; -#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 -/*! -btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. -currently this limit supports rotational motors
    -
      -
    • For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method. -At this moment translational motors are not supported. May be in the future.
      • - -
    • For Angular limits, use the btRotationalLimitMotor structure for configuring the limit. -This is accessible through btGeneric6DofConstraint.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
      -
      • -
    - -*/ -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]; - //!@} - - -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 - - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; - - //!@} - - btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) - { - btAssert(0); - (void) other; - return *this; - } - - - int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - void 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(); - - - -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); - - //! 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(); - - //! Gets the global transform of the offset for body A - /*! - \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. - */ - const btTransform & getCalculatedTransformA() const - { - return m_calculatedTransformA; - } - - //! 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 & getFrameOffsetA() const - { - return m_frameInA; - } - - const btTransform & getFrameOffsetB() const - { - return m_frameInB; - } - - - btTransform & getFrameOffsetA() - { - return m_frameInA; - } - - btTransform & getFrameOffsetB() - { - return m_frameInB; - } - - - //! performs Jacobian calculation, and also calculates angle differences and axis - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual (btConstraintInfo1* 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 updateRHS(btScalar timeStep); - - //! Get the rotation axis in global coordinates - /*! - \pre btGeneric6DofConstraint.buildJacobian must be called previously. - */ - btVector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - 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); - - //! 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); - - void setLinearLowerLimit(const btVector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } - - void getLinearLowerLimit(btVector3& linearLower) - { - linearLower = m_linearLimits.m_lowerLimit; - } - - void setLinearUpperLimit(const btVector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } - - void getLinearUpperLimit(btVector3& linearUpper) - { - linearUpper = m_linearLimits.m_upperLimit; - } - - void setAngularLowerLimit(const btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); - } - - void getAngularLowerLimit(btVector3& angularLower) - { - 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) - { - 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]; - } - - //! 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 - { - lo = btNormalizeAngle(lo); - hi = btNormalizeAngle(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(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); - - // 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, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - void setAxis( const btVector3& axis1, const btVector3& axis2); - - - 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 btGeneric6DofConstraintData -{ - 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; - int m_useOffsetForConstraintFrame; -}; - -struct btGeneric6DofConstraintDoubleData2 -{ - 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; - int m_useOffsetForConstraintFrame; -}; - -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 -{ - - btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer; - btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); - - m_frameInA.serialize(dof->m_rbAFrame); - m_frameInB.serialize(dof->m_rbBFrame); - - - int i; - for (i=0;i<3;i++) - { - dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; - dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; - dof->m_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_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; - - return btGeneric6DofConstraintDataName; -} - - - - - -#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp deleted file mode 100644 index 6f765884e..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGeneric6DofSpringConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - -btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) - : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) -{ - init(); -} - - -btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) - : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB) -{ - init(); -} - - -void btGeneric6DofSpringConstraint::init() -{ - m_objectType = D6_SPRING_CONSTRAINT_TYPE; - - for(int i = 0; i < 6; i++) - { - m_springEnabled[i] = false; - m_equilibriumPoint[i] = btScalar(0.f); - m_springStiffness[i] = btScalar(0.f); - m_springDamping[i] = btScalar(1.f); - } -} - - -void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff) -{ - btAssert((index >= 0) && (index < 6)); - m_springEnabled[index] = onOff; - if(index < 3) - { - m_linearLimits.m_enableMotor[index] = onOff; - } - else - { - m_angularLimits[index - 3].m_enableMotor = onOff; - } -} - - - -void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) -{ - btAssert((index >= 0) && (index < 6)); - m_springStiffness[index] = stiffness; -} - - -void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) -{ - btAssert((index >= 0) && (index < 6)); - m_springDamping[index] = damping; -} - - -void btGeneric6DofSpringConstraint::setEquilibriumPoint() -{ - calculateTransforms(); - int i; - - for( i = 0; i < 3; i++) - { - m_equilibriumPoint[i] = m_calculatedLinearDiff[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) - { - m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; - } - else - { - m_equilibriumPoint[index] = m_calculatedAxisAngleDiff[index - 3]; - } -} - -void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index, btScalar val) -{ - btAssert((index >= 0) && (index < 6)); - 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++) - { - if(m_springEnabled[i]) - { - // get current position of constraint - btScalar currPos = m_calculatedLinearDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i]; - // spring force is (delta * m_stiffness) according to Hooke's Law - btScalar force = delta * m_springStiffness[i]; - btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); - m_linearLimits.m_targetVelocity[i] = velFactor * force; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; - } - } - for(i = 0; i < 3; i++) - { - if(m_springEnabled[i + 3]) - { - // get current position of constraint - btScalar currPos = m_calculatedAxisAngleDiff[i]; - // calculate difference - btScalar delta = currPos - m_equilibriumPoint[i+3]; - // spring force is (-delta * m_stiffness) according to Hooke's Law - btScalar force = -delta * m_springStiffness[i+3]; - btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); - m_angularLimits[i].m_targetVelocity = velFactor * force; - m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; - } - } -} - - -void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info) -{ - // this will be called by constraint solver at the constraint setup stage - // set current motor parameters - internalUpdateSprings(info); - // do the rest of job for constraint setup - btGeneric6DofConstraint::getInfo2(info); -} - - -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 - - btTransform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - - // now get constraint frame in local coordinate systems - m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; - - calculateTransforms(); -} - - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h deleted file mode 100644 index 1b2e0f62c..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ /dev/null @@ -1,121 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#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" -#else -#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 - -/// DOF index used in enableSpring() and setStiffness() means: -/// 0 : translation X -/// 1 : translation Y -/// 2 : translation Z -/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) -/// 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 -{ -protected: - 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: - - 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); - 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(int index); // set the current constraint position/orientation as an equilibrium point for given DOF - void setEquilibriumPoint(int index, btScalar val); - - virtual void setAxis( const btVector3& axis1, const btVector3& axis2); - - virtual void getInfo2 (btConstraintInfo2* info); - - 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 btGeneric6DofSpringConstraintData -{ - 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]; -}; - - -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 -{ - btGeneric6DofSpringConstraintData2* dof = (btGeneric6DofSpringConstraintData2*)dataBuffer; - btGeneric6DofConstraint::serialize(&dof->m_6dofData,serializer); - - int 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_springStiffness[i] = m_springStiffness[i]; - } - return btGeneric6DofSpringConstraintDataName; -} - -#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp deleted file mode 100644 index 29123d526..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btHinge2Constraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" - - - -// constructor -// anchor, axis1 and axis2 are in world coordinate system -// axis1 must be orthogonal to axis2 -btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) -: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), - m_anchor(anchor), - m_axis1(axis1), - m_axis2(axis2) -{ - // build frame basis - // 6DOF constraint uses Euler angles and to define limits - // it is assumed that rotational order is : - // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number - // used to prevent constraint from instability on poles; - // new position of X, allowed limits are (-PI,PI); - // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs - // Build the frame in world coordinate system first - btVector3 zAxis = axis1.normalize(); - btVector3 xAxis = axis2.normalize(); - btVector3 yAxis = zAxis.cross(xAxis); // we want right coordinate system - btTransform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - frameInW.setOrigin(anchor); - // now get constraint frame in local coordinate systems - m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; - // sei limits - setLinearLowerLimit(btVector3(0.f, 0.f, -1.f)); - setLinearUpperLimit(btVector3(0.f, 0.f, 1.f)); - // like front wheels of a car - setAngularLowerLimit(btVector3(1.f, 0.f, -SIMD_HALF_PI * 0.5f)); - setAngularUpperLimit(btVector3(-1.f, 0.f, SIMD_HALF_PI * 0.5f)); - // enable suspension - enableSpring(2, true); - setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) - setDamping(2, 0.01f); - setEquilibriumPoint(); -} - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.h deleted file mode 100644 index 9a0049869..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_HINGE2_CONSTRAINT_H -#define BT_HINGE2_CONSTRAINT_H - - - -#include "LinearMath/btVector3.h" -#include "btTypedConstraint.h" -#include "btGeneric6DofSpringConstraint.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 btGeneric6DofSpringConstraint -{ -protected: - 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 - 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(); } - const btVector3& getAxis1() { return m_axis1; } - const btVector3& getAxis2() { return m_axis2; } - btScalar getAngle1() { return getAngle(2); } - 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)); } -}; - - - -#endif // BT_HINGE2_CONSTRAINT_H - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp deleted file mode 100644 index c18974130..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ /dev/null @@ -1,1046 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btHingeConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMinMax.h" -#include -#include "btSolverBody.h" - - - -//#define HINGE_USE_OBSOLETE_SOLVER false -#define HINGE_USE_OBSOLETE_SOLVER false - -#define HINGE_USE_FRAME_OFFSET true - -#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), -#ifdef _BT_USE_CENTER_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_rbAFrame.getOrigin() = pivotInA; - - // since no frame is given, assume this to be zero angle and just pick rb transform axis - btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); - - btVector3 rbAxisA2; - btScalar projection = axisInA.dot(rbAxisA1); - if (projection >= 1.0f - SIMD_EPSILON) { - rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); - rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else if (projection <= -1.0f + SIMD_EPSILON) { - rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); - rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); - } else { - rbAxisA2 = axisInA.cross(rbAxisA1); - rbAxisA1 = rbAxisA2.cross(axisInA); - } - - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - m_rbBFrame.getOrigin() = pivotInB; - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); - -#ifndef _BT_USE_CENTER_LIMIT_ - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; -#endif - 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), -#ifdef _BT_USE_CENTER_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) -{ - - // since no frame is given, assume this to be zero angle and just pick rb transform axis - // fixed axis in worldspace - btVector3 rbAxisA1, rbAxisA2; - btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); - - m_rbAFrame.getOrigin() = pivotInA; - m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), - rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), - rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); - - btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; - - btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); - btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); - btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); - - - m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); - m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), - rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), - rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); - -#ifndef _BT_USE_CENTER_LIMIT_ - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; -#endif - 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), -#ifdef _BT_USE_CENTER_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) -{ -#ifndef _BT_USE_CENTER_LIMIT_ - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; -#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), -#ifdef _BT_USE_CENTER_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) -{ - ///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_ - //start with free - m_lowerLimit = btScalar(1.0f); - m_upperLimit = btScalar(-1.0f); - m_biasFactor = 0.3f; - m_relaxationFactor = 1.0f; - m_limitSoftness = 0.9f; - m_solveLimit = false; -#endif - m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); -} - - - -void btHingeConstraint::buildJacobian() -{ - if (m_useSolveConstraintObsolete) - { - m_appliedImpulse = btScalar(0.); - m_accMotorImpulse = btScalar(0.); - - if (!m_angularOnly) - { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 relPos = pivotBInW - pivotAInW; - - btVector3 normal[3]; - if (relPos.length2() > SIMD_EPSILON) - { - normal[0] = relPos.normalized(); - } - else - { - normal[0].setValue(btScalar(1.0),0,0); - } - - btPlaneSpace1(normal[0], normal[1], normal[2]); - - for (int i=0;i<3;i++) - { - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - pivotAInW - m_rbA.getCenterOfMassPosition(), - pivotBInW - m_rbB.getCenterOfMassPosition(), - normal[i], - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - } - } - - //calculate two perpendicular jointAxis, orthogonal to hingeAxis - //these two jointAxis require equal angular velocities for both bodies - - //this is unused for now, it's a todo - btVector3 jointAxis0local; - btVector3 jointAxis1local; - - btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - - btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; - btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - - new (&m_jacAng[0]) btJacobianEntry(jointAxis0, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[1]) btJacobianEntry(jointAxis1, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getInvInertiaDiagLocal(), - m_rbB.getInvInertiaDiagLocal()); - - // clear accumulator - m_accLimitImpulse = btScalar(0.); - - // test angular limit - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - - //Compute K = J*W*J' for hinge axis - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA)); - - } -} - - -#endif //__SPU__ - - -void btHingeConstraint::getInfo1(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular - info->nub = 1; - //always add the row, to avoid computation (data is not available yet) - //prepare constraint - testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - if(getSolveLimit() || getEnableAngularMotor()) - { - info->m_numConstraintRows++; // limit 3rd anguar as well - info->nub--; - } - - } -} - -void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - //always add the 'limit' row, to avoid computation (data is not available yet) - info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular - info->nub = 0; - } -} - -void btHingeConstraint::getInfo2 (btConstraintInfo2* info) -{ - if(m_useOffsetForConstraintFrame) - { - 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()); - } -} - - -void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) -{ - ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now - testLimit(transA,transB); - - getInfo2Internal(info,transA,transB,angVelA,angVelB); -} - - -void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) -{ - - btAssert(!m_useSolveConstraintObsolete); - int i, skip = info->rowskip; - // transforms in world space - btTransform trA = transA*m_rbAFrame; - btTransform trB = transB*m_rbBFrame; - // pivot point - btVector3 pivotAInW = trA.getOrigin(); - btVector3 pivotBInW = trB.getOrigin(); -#if 0 - if (0) - { - for (i=0;i<6;i++) - { - info->m_J1linearAxis[i*skip]=0; - info->m_J1linearAxis[i*skip+1]=0; - info->m_J1linearAxis[i*skip+2]=0; - - info->m_J1angularAxis[i*skip]=0; - info->m_J1angularAxis[i*skip+1]=0; - info->m_J1angularAxis[i*skip+2]=0; - - info->m_J2linearAxis[i*skip]=0; - info->m_J2linearAxis[i*skip+1]=0; - info->m_J2linearAxis[i*skip+2]=0; - - info->m_J2angularAxis[i*skip]=0; - info->m_J2angularAxis[i*skip+1]=0; - info->m_J2angularAxis[i*skip+2]=0; - - info->m_constraintError[i*skip]=0.f; - } - } -#endif //#if 0 - // linear (all fixed) - - if (!m_angularOnly) - { - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[skip + 1] = 1; - info->m_J1linearAxis[2 * skip + 2] = 1; - - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[skip + 1] = -1; - info->m_J2linearAxis[2 * skip + 2] = -1; - } - - - - - btVector3 a1 = pivotAInW - transA.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - btVector3 a2 = pivotBInW - transB.getOrigin(); - { - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - // linear RHS - btScalar k = info->fps * info->erp; - if (!m_angularOnly) - { - for(i = 0; i < 3; i++) - { - info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); - } - } - // make rotations around X and Y equal - // the hinge axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the hinge axis should be equal. thus the constraint equations are - // p*w1 - p*w2 = 0 - // q*w1 - q*w2 = 0 - // where p and q are unit vectors normal to the hinge axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - // get hinge axis (Z) - btVector3 ax1 = trA.getBasis().getColumn(2); - // get 2 orthos to hinge axis (X, Y) - btVector3 p = trA.getBasis().getColumn(0); - btVector3 q = trA.getBasis().getColumn(1); - // set the two hinge angular rows - int s3 = 3 * info->rowskip; - int s4 = 4 * info->rowskip; - - info->m_J1angularAxis[s3 + 0] = p[0]; - info->m_J1angularAxis[s3 + 1] = p[1]; - info->m_J1angularAxis[s3 + 2] = p[2]; - info->m_J1angularAxis[s4 + 0] = q[0]; - info->m_J1angularAxis[s4 + 1] = q[1]; - info->m_J1angularAxis[s4 + 2] = q[2]; - - info->m_J2angularAxis[s3 + 0] = -p[0]; - info->m_J2angularAxis[s3 + 1] = -p[1]; - info->m_J2angularAxis[s3 + 2] = -p[2]; - info->m_J2angularAxis[s4 + 0] = -q[0]; - info->m_J2angularAxis[s4 + 1] = -q[1]; - info->m_J2angularAxis[s4 + 2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the hinge back into alignment. - // if ax1,ax2 are the unit length hinge axes as computed from body1 and - // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). - // if `theta' is the angle between ax1 and ax2, we need an angular velocity - // along u to cover angle erp*theta in one step : - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (erp*fps) * (ax1 x ax2) - // ax1 x ax2 is in the plane space of ax1, so we project the angular - // velocity to p and q to find the right hand side. - btVector3 ax2 = trB.getBasis().getColumn(2); - btVector3 u = ax1.cross(ax2); - info->m_constraintError[s3] = k * u.dot(p); - info->m_constraintError[s4] = k * u.dot(q); - // check angular limits - int nrow = 4; // last filled row - int srow; - btScalar limit_err = btScalar(0.0); - int limit = 0; - if(getSolveLimit()) - { -#ifdef _BT_USE_CENTER_LIMIT_ - limit_err = m_limit.getCorrection() * m_referenceSign; -#else - limit_err = m_correction * m_referenceSign; -#endif - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - - } - // if the hinge has joint limits or motor, add in the extra row - int powered = 0; - if(getEnableAngularMotor()) - { - powered = 1; - } - if(limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; - - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; - - btScalar lostop = getLowerLimit(); - btScalar histop = getUpperLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - info->m_constraintError[srow] = btScalar(0.0f); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; - if(powered) - { - 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; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) - { - info->cfm[srow] = m_stopCFM; - } - if(lostop == histop) - { - // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else if(limit == 1) - { // low limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { // high limit - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) -#ifdef _BT_USE_CENTER_LIMIT_ - btScalar bounce = m_limit.getRelaxationFactor(); -#else - btScalar bounce = m_relaxationFactor; -#endif - if(bounce > btScalar(0.0)) - { - btScalar vel = angVelA.dot(ax1); - vel -= angVelB.dot(ax1); - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) - { - btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - else - { // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - } -#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 -} - - -void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) -{ - m_rbAFrame = frameA; - m_rbBFrame = frameB; - buildJacobian(); -} - - -void btHingeConstraint::updateRHS(btScalar timeStep) -{ - (void)timeStep; - -} - - -btScalar btHingeConstraint::getHingeAngle() -{ - return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) -{ - const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); - const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); - const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); -// btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); - btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); - return m_referenceSign * angle; -} - - - -void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) -{ - // Compute limit information - m_hingeAngle = getHingeAngle(transA,transB); -#ifdef _BT_USE_CENTER_LIMIT_ - m_limit.test(m_hingeAngle); -#else - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - if (m_lowerLimit <= m_upperLimit) - { - m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); - if (m_hingeAngle <= m_lowerLimit) - { - m_correction = (m_lowerLimit - m_hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } - else if (m_hingeAngle >= m_upperLimit) - { - m_correction = m_upperLimit - m_hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; - } - } -#endif - return; -} - - -static btVector3 vHinge(0, 0, btScalar(1)); - -void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) -{ - // convert target from body to constraint space - btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); - qConstraint.normalize(); - - // extract "pure" hinge component - btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); - btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); - btQuaternion qHinge = qNoHinge.inverse() * qConstraint; - qHinge.normalize(); - - // compute angular target, clamped to limits - btScalar targetAngle = qHinge.getAngle(); - if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. - { - qHinge = -(qHinge); - targetAngle = qHinge.getAngle(); - } - if (qHinge.getZ() < 0) - targetAngle = -targetAngle; - - setMotorTarget(targetAngle, dt); -} - -void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) -{ -#ifdef _BT_USE_CENTER_LIMIT_ - m_limit.fit(targetAngle); -#else - if (m_lowerLimit < m_upperLimit) - { - if (targetAngle < m_lowerLimit) - targetAngle = m_lowerLimit; - else if (targetAngle > m_upperLimit) - targetAngle = m_upperLimit; - } -#endif - // compute angular velocity - 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) -{ - btAssert(!m_useSolveConstraintObsolete); - int i, s = info->rowskip; - // transforms in world space - btTransform trA = transA*m_rbAFrame; - btTransform trB = transB*m_rbBFrame; - // pivot point -// btVector3 pivotAInW = trA.getOrigin(); -// btVector3 pivotBInW = trB.getOrigin(); -#if 1 - // difference between frames in WCS - btVector3 ofs = trB.getOrigin() - trA.getOrigin(); - // now get weight factors depending on masses - btScalar miA = getRigidBodyA().getInvMass(); - btScalar miB = getRigidBodyB().getInvMass(); - bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); - btScalar miS = miA + miB; - btScalar factA, factB; - if(miS > btScalar(0.f)) - { - factA = miB / miS; - } - else - { - factA = btScalar(0.5f); - } - factB = btScalar(1.0f) - factA; - // get the desired direction of hinge axis - // as weighted sum of Z-orthos of frameA and frameB in WCS - btVector3 ax1A = trA.getBasis().getColumn(2); - btVector3 ax1B = trB.getBasis().getColumn(2); - btVector3 ax1 = ax1A * factA + ax1B * factB; - ax1.normalize(); - // fill first 3 rows - // we want: velA + wA x relA == velB + wB x relB - btTransform bodyA_trans = transA; - btTransform bodyB_trans = transB; - int s0 = 0; - int s1 = s; - int s2 = s * 2; - int nrow = 2; // last filled row - btVector3 tmpA, tmpB, relA, relB, p, q; - // get vector from bodyB to frameB in WCS - relB = trB.getOrigin() - bodyB_trans.getOrigin(); - // get its projection to hinge axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to hinge axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = trA.getOrigin() - bodyA_trans.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - btVector3 totalDist = projA - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * factA; - relB = orthoB - totalDist * factB; - // now choose average ortho to hinge axis - p = orthoB * factA + orthoA * factB; - btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) - { - p /= btSqrt(len2); - } - else - { - p = trA.getBasis().getColumn(1); - } - // make one more ortho - q = ax1.cross(p); - // 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]; - tmpA = relA.cross(q); - tmpB = relB.cross(q); - 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]; - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - 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]; - - btScalar k = info->fps * info->erp; - - 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_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 - - btScalar rhs = k * p.dot(ofs); - info->m_constraintError[s0] = rhs; - rhs = k * q.dot(ofs); - info->m_constraintError[s1] = rhs; - rhs = k * ax1.dot(ofs); - info->m_constraintError[s2] = rhs; - } - // the hinge axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the hinge axis should be equal. thus the constraint equations are - // p*w1 - p*w2 = 0 - // q*w1 - q*w2 = 0 - // where p and q are unit vectors normal to the hinge axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - int s3 = 3 * s; - int s4 = 4 * s; - info->m_J1angularAxis[s3 + 0] = p[0]; - info->m_J1angularAxis[s3 + 1] = p[1]; - info->m_J1angularAxis[s3 + 2] = p[2]; - info->m_J1angularAxis[s4 + 0] = q[0]; - info->m_J1angularAxis[s4 + 1] = q[1]; - info->m_J1angularAxis[s4 + 2] = q[2]; - - info->m_J2angularAxis[s3 + 0] = -p[0]; - info->m_J2angularAxis[s3 + 1] = -p[1]; - info->m_J2angularAxis[s3 + 2] = -p[2]; - info->m_J2angularAxis[s4 + 0] = -q[0]; - info->m_J2angularAxis[s4 + 1] = -q[1]; - info->m_J2angularAxis[s4 + 2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the hinge back into alignment. - // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and - // bodyB, 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. - k = info->fps * info->erp; - 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 - int srow; - btScalar limit_err = btScalar(0.0); - int limit = 0; - if(getSolveLimit()) - { -#ifdef _BT_USE_CENTER_LIMIT_ - limit_err = m_limit.getCorrection() * m_referenceSign; -#else - limit_err = m_correction * m_referenceSign; -#endif - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - - } - // if the hinge has joint limits or motor, add in the extra row - int powered = 0; - if(getEnableAngularMotor()) - { - powered = 1; - } - if(limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; - - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; - - btScalar lostop = getLowerLimit(); - btScalar histop = getUpperLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - info->m_constraintError[srow] = btScalar(0.0f); - btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; - if(powered) - { - 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; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_HINGE_FLAGS_CFM_STOP) - { - info->cfm[srow] = m_stopCFM; - } - if(lostop == histop) - { - // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else if(limit == 1) - { // low limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { // high limit - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) -#ifdef _BT_USE_CENTER_LIMIT_ - btScalar bounce = m_limit.getRelaxationFactor(); -#else - btScalar bounce = m_relaxationFactor; -#endif - if(bounce > btScalar(0.0)) - { - btScalar vel = angVelA.dot(ax1); - vel -= angVelB.dot(ax1); - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) - { - btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - else - { // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - } -#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 -} - - -///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)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - m_stopERP = value; - m_flags |= BT_HINGE_FLAGS_ERP_STOP; - break; - case BT_CONSTRAINT_STOP_CFM : - m_stopCFM = value; - m_flags |= BT_HINGE_FLAGS_CFM_STOP; - break; - case BT_CONSTRAINT_CFM : - m_normalCFM = value; - m_flags |= BT_HINGE_FLAGS_CFM_NORM; - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } -} - -///return the local value of parameter -btScalar btHingeConstraint::getParam(int num, int axis) const -{ - btScalar retVal = 0; - if((axis == -1) || (axis == 5)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); - retVal = m_stopERP; - break; - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); - retVal = m_stopCFM; - break; - case BT_CONSTRAINT_CFM : - btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); - retVal = m_normalCFM; - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } - return retVal; -} - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.h deleted file mode 100644 index 7c33ac24e..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ /dev/null @@ -1,412 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ - -#ifndef BT_HINGECONSTRAINT_H -#define BT_HINGECONSTRAINT_H - -#define _BT_USE_CENTER_LIMIT_ 1 - - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -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" -#else -#define btHingeConstraintData btHingeConstraintFloatData -#define btHingeConstraintDataName "btHingeConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - - -enum btHingeFlags -{ - BT_HINGE_FLAGS_CFM_STOP = 1, - BT_HINGE_FLAGS_ERP_STOP = 2, - BT_HINGE_FLAGS_CFM_NORM = 4 -}; - - -/// 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 -{ -#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 - - btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransform m_rbBFrame; - - btScalar m_motorTargetVelocity; - btScalar m_maxMotorImpulse; - - -#ifdef _BT_USE_CENTER_LIMIT_ - btAngularLimit m_limit; -#else - btScalar m_lowerLimit; - btScalar m_upperLimit; - btScalar m_limitSign; - btScalar m_correction; - - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; - - bool m_solveLimit; -#endif - - btScalar m_kHinge; - - - btScalar m_accLimitImpulse; - btScalar m_hingeAngle; - btScalar m_referenceSign; - - bool m_angularOnly; - bool m_enableAngularMotor; - bool m_useSolveConstraintObsolete; - bool m_useOffsetForConstraintFrame; - bool m_useReferenceFrameA; - - btScalar m_accMotorImpulse; - - int m_flags; - btScalar m_normalCFM; - 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,const btTransform& rbAFrame, bool useReferenceFrameA = false); - - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); - - 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 - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - btRigidBody& getRigidBodyA() - { - return m_rbA; - } - - btRigidBody& getRigidBodyB() - { - return m_rbB; - } - - btTransform& getFrameOffsetA() - { - return m_rbAFrame; - } - - btTransform& getFrameOffsetB() - { - return m_rbBFrame; - } - - void setFrames(const btTransform& frameA, const btTransform& frameB); - - void setAngularOnly(bool angularOnly) - { - m_angularOnly = angularOnly; - } - - void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse) - { - m_enableAngularMotor = enableMotor; - m_motorTargetVelocity = targetVelocity; - m_maxMotorImpulse = maxMotorImpulse; - } - - // 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 setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; } - 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) - { -#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_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; -#endif - } - - 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() ); - - btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA; - - 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() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis(); - - } - - btScalar getLowerLimit() const - { -#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 - } - - - btScalar getHingeAngle(); - - btScalar getHingeAngle(const btTransform& transA,const btTransform& transB); - - void testLimit(const btTransform& transA,const btTransform& transB); - - - const btTransform& getAFrame() const { return m_rbAFrame; }; - const btTransform& getBFrame() const { return m_rbBFrame; }; - - btTransform& getAFrame() { return m_rbAFrame; }; - btTransform& getBFrame() { return m_rbBFrame; }; - - inline int getSolveLimit() - { -#ifdef _BT_USE_CENTER_LIMIT_ - return m_limit.isLimit(); -#else - return m_solveLimit; -#endif - } - - inline btScalar getLimitSign() - { -#ifdef _BT_USE_CENTER_LIMIT_ - return m_limit.getSign(); -#else - return m_limitSign; -#endif - } - - inline bool getAngularOnly() - { - return m_angularOnly; - } - inline bool getEnableAngularMotor() - { - return m_enableAngularMotor; - } - inline btScalar getMotorTargetVelosity() - { - return m_motorTargetVelocity; - } - inline btScalar getMaxMotorImpulse() - { - return m_maxMotorImpulse; - } - // 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, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar 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, 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 -{ - 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; - -}; -#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION - - -struct btHingeConstraintFloatData -{ - 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; - - 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 -{ - 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]; - -}; - - - - -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 -{ - btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer; - btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer); - - m_rbAFrame.serialize(hingeData->m_rbAFrame); - m_rbBFrame.serialize(hingeData->m_rbBFrame); - - hingeData->m_angularOnly = m_angularOnly; - hingeData->m_enableAngularMotor = m_enableAngularMotor; - hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse); - hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity); - hingeData->m_useReferenceFrameA = m_useReferenceFrameA; -#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()); - hingeData->m_biasFactor = float(m_limit.getBiasFactor()); - hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor()); -#else - hingeData->m_lowerLimit = float(m_lowerLimit); - hingeData->m_upperLimit = float(m_upperLimit); - hingeData->m_limitSoftness = float(m_limitSoftness); - hingeData->m_biasFactor = float(m_biasFactor); - hingeData->m_relaxationFactor = float(m_relaxationFactor); -#endif - - return btHingeConstraintDataName; -} - -#endif //BT_HINGECONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btJacobianEntry.h deleted file mode 100644 index 125580d19..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ /dev/null @@ -1,155 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_JACOBIAN_ENTRY_H -#define BT_JACOBIAN_ENTRY_H - -#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 -// 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 -ATTRIBUTE_ALIGNED16(class) btJacobianEntry -{ -public: - btJacobianEntry() {}; - //constraint between two different rigidbodies - btJacobianEntry( - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - const btVector3& rel_pos1,const btVector3& rel_pos2, - const btVector3& jointAxis, - const btVector3& inertiaInvA, - const btScalar massInvA, - const btVector3& inertiaInvB, - const btScalar 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); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //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.))) - { - 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); - - 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) - { - m_0MinvJt = inertiaInvA * m_aJ; - m_1MinvJt = inertiaInvB * m_bJ; - m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); - - btAssert(m_Adiag > btScalar(0.0)); - } - - //constraint on one rigidbody - btJacobianEntry( - const btMatrix3x3& world2A, - const btVector3& rel_pos1,const btVector3& rel_pos2, - const btVector3& jointAxis, - const btVector3& inertiaInvA, - const btScalar 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 = 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; } - - // for two constraints on the same rigidbody (for example vehicle friction) - btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const - { - const btJacobianEntry& jacA = *this; - btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); - btScalar 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) - 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 lin1 = massInvB * lin; - 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) - { - btVector3 linrel = linvelA - linvelB; - 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]; - return rel_vel2 + SIMD_EPSILON; - } -//private: - - 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; - -}; - -#endif //BT_JACOBIAN_ENTRY_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp deleted file mode 100644 index 3c0430b90..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ /dev/null @@ -1,229 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "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,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() -{ - - ///we need it for both methods - { - m_appliedImpulse = btScalar(0.); - - btVector3 normal(0,0,0); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - new (&m_jac[i]) btJacobianEntry( - m_rbA.getCenterOfMassTransform().getBasis().transpose(), - m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, - m_rbA.getInvInertiaDiagLocal(), - m_rbA.getInvMass(), - m_rbB.getInvInertiaDiagLocal(), - m_rbB.getInvMass()); - normal[i] = 0; - } - } - - -} - -void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info) -{ - getInfo1NonVirtual(info); -} - -void btPoint2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { - info->m_numConstraintRows = 3; - info->nub = 3; - } -} - - - - -void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info) -{ - getInfo2NonVirtual(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans) -{ - btAssert(!m_useSolveConstraintObsolete); - - //retrieve matrices - - // anchor points in global coordinates with respect to body PORs. - - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; - - btVector3 a1 = body0_trans.getBasis()*getPivotInA(); - { - btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); - btVector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; - - btVector3 a2 = body1_trans.getBasis()*getPivotInB(); - - { - // btVector3 a2n = -a2; - btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); - btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); - btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); - } - - - - // set right hand side - btScalar 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) - { - for (j=0; j<3; j++) - { - info->cfm[j*info->rowskip] = m_cfm; - } - } - - 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_damping = m_setting.m_damping; - -} - - - -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). -///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) - { - btAssertConstrParams(0); - } - else - { - switch(num) - { - 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; - m_flags |= BT_P2P_FLAGS_CFM; - break; - default: - btAssertConstrParams(0); - } - } -} - -///return the local value of parameter -btScalar btPoint2PointConstraint::getParam(int num, int axis) const -{ - btScalar retVal(SIMD_INFINITY); - if(axis != -1) - { - btAssertConstrParams(0); - } - else - { - switch(num) - { - case BT_CONSTRAINT_ERP : - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP); - retVal = m_erp; - break; - case BT_CONSTRAINT_CFM : - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM); - retVal = m_cfm; - break; - default: - btAssertConstrParams(0); - } - } - return retVal; -} - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h deleted file mode 100644 index 912189494..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ /dev/null @@ -1,175 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_POINT2POINTCONSTRAINT_H -#define BT_POINT2POINTCONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - - -#ifdef BT_USE_DOUBLE_PRECISION -#define btPoint2PointConstraintData2 btPoint2PointConstraintDoubleData2 -#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData2" -#else -#define btPoint2PointConstraintData2 btPoint2PointConstraintFloatData -#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData" -#endif //BT_USE_DOUBLE_PRECISION - -struct btConstraintSetting -{ - btConstraintSetting() : - m_tau(btScalar(0.3)), - m_damping(btScalar(1.)), - m_impulseClamp(btScalar(0.)) - { - } - btScalar m_tau; - btScalar m_damping; - btScalar m_impulseClamp; -}; - -enum btPoint2PointFlags -{ - BT_P2P_FLAGS_ERP = 1, - BT_P2P_FLAGS_CFM = 2 -}; - -/// 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 -{ -#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: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - - btConstraintSetting m_setting; - - btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); - - btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA); - - - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans); - - void updateRHS(btScalar timeStep); - - void setPivotA(const btVector3& pivotA) - { - m_pivotInA = pivotA; - } - - void setPivotB(const btVector3& pivotB) - { - m_pivotInB = pivotB; - } - - const btVector3& getPivotInA() const - { - return m_pivotInA; - } - - const btVector3& 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, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar 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, btSerializer* serializer) const; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btPoint2PointConstraintFloatData -{ - btTypedConstraintData m_typeConstraintData; - btVector3FloatData m_pivotInA; - btVector3FloatData m_pivotInB; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btPoint2PointConstraintDoubleData2 -{ - 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 -{ - btTypedConstraintData m_typeConstraintData; - btVector3DoubleData m_pivotInA; - btVector3DoubleData m_pivotInB; -}; -#endif //BT_BACKWARDS_COMPATIBLE_SERIALIZATION - - -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 -{ - btPoint2PointConstraintData2* p2pData = (btPoint2PointConstraintData2*)dataBuffer; - - btTypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); - m_pivotInA.serialize(p2pData->m_pivotInA); - m_pivotInB.serialize(p2pData->m_pivotInB); - - return btPoint2PointConstraintDataName; -} - -#endif //BT_POINT2POINTCONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp deleted file mode 100644 index be93e3543..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ /dev/null @@ -1,1739 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -//#define COMPUTE_IMPULSE_DENOM 1 -//#define BT_ADDITIONAL_DEBUG - -//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. - -#include "btSequentialImpulseConstraintSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - -#include "LinearMath/btIDebugDraw.h" -//#include "btJacobianEntry.h" -#include "LinearMath/btMinMax.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include -#include "LinearMath/btStackAlloc.h" -#include "LinearMath/btQuickprof.h" -//#include "btSolverBody.h" -//#include "btSolverConstraint.h" -#include "LinearMath/btAlignedObjectArray.h" -#include //for memset - -int gNumSplitImpulseRecoveries = 0; - -#include "BulletDynamics/Dynamics/btRigidBody.h" - -btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_btSeed2(0) -{ - -} - -btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() -{ -} - -#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 ) -{ - __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); -} -#endif//USE_SIMD - -// Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(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)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).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_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)); -#else - resolveSingleConstraintRowGeneric(body1,body2,c); -#endif -} - -// Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) -{ - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_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()); - -// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); -} - - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(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)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.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_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)); -#else - resolveSingleConstraintRowLowerLimit(body1,body2,c); -#endif -} - -// Projected Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,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()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedImpulse = sum; - } - body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); -} - - -void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, - const btSolverConstraint& c) -{ - if (c.m_rhsPenetration) - { - gNumSplitImpulseRecoveries++; - btScalar 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()); - - 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); - } -} - - void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) -{ -#ifdef USE_SIMD - if (!c.m_rhsPenetration) - return; - - 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 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)); -#else - resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); -#endif -} - - - -unsigned long btSequentialImpulseConstraintSolver::btRand2() -{ - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; - return m_btSeed2; -} - - - -//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) -int btSequentialImpulseConstraintSolver::btRandInt2 (int n) -{ - // seems good; xor-fold and modulus - const unsigned long un = static_cast(n); - unsigned long r = btRand2(); - - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { - r ^= (r >> 16); - if (un <= 0x00000100UL) { - r ^= (r >> 8); - if (un <= 0x00000010UL) { - r ^= (r >> 4); - if (un <= 0x00000004UL) { - r ^= (r >> 2); - if (un <= 0x00000002UL) { - r ^= (r >> 1); - } - } - } - } - } - - return (int) (r % un); -} - - - -void btSequentialImpulseConstraintSolver::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); - } - - -} - - - - - - -btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) -{ - btScalar rest = restitution * -rel_vel; - return rest; -} - - - -void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) -{ - - - if (colObj && colObj->hasAnisotropicFriction(frictionMode)) - { - // transform to local coordinates - btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); - const btVector3& friction_scaling = colObj->getAnisotropicFriction(); - //apply anisotropic friction - loc_lateral *= friction_scaling; - // ... and transform it back to global coordinates - frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; - } - -} - - - - -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, btScalar desiredVelocity, btScalar cfmSlip) -{ - - - btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - - btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; - - 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; - - if (body0) - { - 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_contactNormal1.setZero(); - solverConstraint.m_relpos1CrossNormal.setZero(); - solverConstraint.m_angularComponentA .setZero(); - } - - if (body1) - { - 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_contactNormal2.setZero(); - solverConstraint.m_relpos2CrossNormal.setZero(); - solverConstraint.m_angularComponentB.setZero(); - } - - { - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (body0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = body0->getInvMass() + normalAxis.dot(vec); - } - if (body1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); - } - 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)); - - rel_vel = vel1Dotn+vel2Dotn; - -// btScalar positionalError = 0.f; - - 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; - - } -} - -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, 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, desiredVelocity, cfmSlip); - return solverConstraint; -} - - -void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity, btScalar cfmSlip) - -{ - btVector3 normalAxis(0,0,0); - - - solverConstraint.m_contactNormal1 = normalAxis; - solverConstraint.m_contactNormal2 = -normalAxis; - btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - - btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; - btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; - - 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; - - { - btVector3 ftorqueAxis1 = -normalAxis1; - solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - 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); - } - - - { - btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); - btVector3 iMJaB = body1?body1->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; - } - - { - - - 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)); - - rel_vel = vel1Dotn+vel2Dotn; - -// btScalar positionalError = 0.f; - - 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; - - } -} - - - - - - - - -btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) -{ - btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); - solverConstraint.m_frictionIndex = frictionIndex; - setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); - return solverConstraint; -} - - -int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep) -{ - - 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; - -} -#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) -{ - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - 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(); - - relaxation = 1.f; - - 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); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - } - - 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; - - { - 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); - 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; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - if (penetration>0) - { - positionalError = 0; - - velocityError -= penetration / infoGlobal.m_timeStep; - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_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 = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } - - - - -} - - - -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) -{ - - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - btRigidBody* rb0 = bodyA->m_originalBody; - btRigidBody* rb1 = bodyB->m_originalBody; - - { - btSolverConstraint& frictionConstraint1 = m_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); - if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - 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 - { - frictionConstraint2.m_appliedImpulse = 0.f; - } - } -} - - - - -void btSequentialImpulseConstraintSolver::convertContact(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]; - - - - ///avoid collision response between two static objects - if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) - return; - - int rollingFriction=1; - for (int j=0;jgetNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - if (cp.getDistance() <= manifold->getContactProcessingThreshold()) - { - btVector3 rel_pos1; - btVector3 rel_pos2; - btScalar relaxation; - - - int frictionIndex = m_tmpSolverContactConstraintPool.size(); - btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - - solverConstraint.m_originalContactPoint = &cp; - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - - btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - - 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); - - - -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints - - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) - { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - } - - ///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 - /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 *= 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); - - 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); - } - - } 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); - - 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); - } - - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - { - cp.m_lateralFrictionInitialized = true; - } - } - - } else - { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); - - } - setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - - - - - } - } -} - -void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) -{ - int i; - btPersistentManifold* manifold = 0; -// btCollisionObject* colObj0=0,*colObj1=0; - - - for (i=0;iisEnabled()) - { - if (!constraint->getRigidBodyA().isStaticOrKinematicObject()) - { - bool found=false; - for (int b=0;bgetRigidBodyA()==bodies[b]) - { - found = true; - break; - } - } - btAssert(found); - } - if (!constraint->getRigidBodyB().isStaticOrKinematicObject()) - { - bool found=false; - for (int b=0;bgetRigidBodyB()==bodies[b]) - { - found = true; - break; - } - } - btAssert(found); - } - } - } - //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++) - { - bodies[i]->setCompanionId(-1); - } - - - m_tmpSolverBodyPool.reserve(numBodies+1); - m_tmpSolverBodyPool.resize(0); - - //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - //initSolverBody(&fixedBody,0); - - //convert all bodies - - for (int i=0;igetInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - btVector3 gyroForce (0,0,0); - if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE) - { - gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); - solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; - } - } - } - - if (1) - { - int j; - for (j=0;jbuildJacobian(); - constraint->internalSetAppliedImpulse(0.0f); - } - } - - //btRigidBody* rb0=0,*rb1=0; - - //if (1) - { - { - - int totalNumRows = 0; - int i; - - m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); - //calculate the total number of contraint rows - for (i=0;igetJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA.setZero(); - fb->m_appliedTorqueBodyA.setZero(); - fb->m_appliedForceBodyB.setZero(); - fb->m_appliedTorqueBodyB.setZero(); - } - - if (constraints[i]->isEnabled()) - { - } - if (constraints[i]->isEnabled()) - { - constraints[i]->getInfo1(&info1); - } else - { - info1.m_numConstraintRows = 0; - info1.nub = 0; - } - totalNumRows += info1.m_numConstraintRows; - } - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); - - - ///setup the btSolverConstraints - int currentRow = 0; - - for (i=0;igetRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); - - int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); - - btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; - - - - - int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) - m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - - - int j; - for ( j=0;jinternalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - - - btTypedConstraint::btConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; - info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; - info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; - info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this - ///the size of btSolverConstraint needs be a multiple of btScalar - btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); - info2.m_constraintError = ¤tConstraintRow->m_rhs; - currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; - info2.m_damping = infoGlobal.m_damping; - info2.cfm = ¤tConstraintRow->m_cfm; - info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; - info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; - info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2); - - ///finalize the constraint setup - for ( j=0;j=constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); - } - - if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); - } - - solverConstraint.m_originalContactPoint = constraint; - - { - const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); - } - { - const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); - } - - { - btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); - btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; - btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? - btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; - - btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); - sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - sum += iMJlB.dot(solverConstraint.m_contactNormal2); - sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - btScalar fsum = btFabs(sum); - btAssert(fsum > SIMD_EPSILON); - solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f; - } - - - - { - btScalar rel_vel; - btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) - + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); - - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) - + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); - - rel_vel = vel1Dotn+vel2Dotn; - btScalar restitution = 0.f; - btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - btScalar velocityError = restitution - rel_vel * info2.m_damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_appliedImpulse = 0.f; - - - } - } - } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; - } - } - - convertContacts(manifoldPtr,numManifolds,infoGlobal); - - } - -// btContactSolverInfo info = infoGlobal; - - - 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 & SOLVER_USE_2_FRICTION_DIRECTIONS)) - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); - else - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); - - m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); - { - int i; - for (i=0;iisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - - ///solve all contact constraints using SIMD, if available - 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;cbtScalar(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 & SOLVER_USE_2_FRICTION_DIRECTIONS) - { - - 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; - - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - } - } - } - } - - } - 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; - - //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - } - } - - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (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; - - resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - } - } - - - } - } - } else - { - //non-SIMD version - ///solve all joint constraints - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - } - } - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - } - } - } - } - return 0.f; -} - - -void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) -{ - int iteration; - if (infoGlobal.m_splitImpulse) - { - if (infoGlobal.m_solverMode & SOLVER_SIMD) - { - for ( iteration = 0;iteration infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - - for ( int iteration = 0 ; iteration< maxIterations ; iteration++) - //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) - { - solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); - } - - } - return 0.f; -} - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) -{ - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i,j; - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - for (j=0;jm_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? - } - } - - 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 ???? */ - - } - - constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); - if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) - { - constr->setEnabled(false); - } - } - - - - for ( i=0;isetLinearVelocity( - m_tmpSolverBodyPool[i].m_linearVelocity+ - m_tmpSolverBodyPool[i].m_externalForceImpulse); - - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( - m_tmpSolverBodyPool[i].m_angularVelocity+ - m_tmpSolverBodyPool[i].m_externalTorqueImpulse); - - if (infoGlobal.m_splitImpulse) - m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); - - m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); - } - } - - m_tmpSolverContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); - - m_tmpSolverBodyPool.resizeNoInitialize(0); - return 0.f; -} - - - -/// 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*/) -{ - - BT_PROFILE("solveGroup"); - //you need to provide at least some bodies - - solveGroupCacheFriendlySetup( 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() -{ - m_btSeed2 = 0; -} - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h deleted file mode 100644 index 180d2a385..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ /dev/null @@ -1,148 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H -#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - -class btIDebugDraw; -class btPersistentManifold; -class btDispatcher; -class btCollisionObject; -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h" -#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" -#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" - -///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. -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; - btAlignedObjectArray m_tmpConstraintSizesPool; - int m_maxOverrideNumSolverIterations; - int m_fixedBodyId; - 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, - btScalar desiredVelocity=0., btScalar cfmSlip=0.); - - void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity=0., btScalar cfmSlip=0.); - - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); - - - void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, - const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); - - static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); - - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); - - ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction - unsigned long m_btSeed2; - - - btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); - - virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); - - void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); - - - void resolveSplitPenetrationSIMD( - btSolverBody& bodyA,btSolverBody& bodyB, - const btSolverConstraint& contactConstraint); - - void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& bodyA,btSolverBody& bodyB, - const btSolverConstraint& contactConstraint); - - //internal method - int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); - void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep); - - void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - - void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& 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); - - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btSequentialImpulseConstraintSolver(); - virtual ~btSequentialImpulseConstraintSolver(); - - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - - - - ///clear internal cached data and reset random seed - virtual void reset(); - - unsigned long btRand2(); - - int btRandInt2 (int n); - - void setRandSeed(unsigned long seed) - { - m_btSeed2 = seed; - } - unsigned long getRandSeed() const - { - return m_btSeed2; - } - - - virtual btConstraintSolverType getSolverType() const - { - return BT_SEQUENTIAL_IMPULSE_SOLVER; - } -}; - - - - -#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp deleted file mode 100644 index aff9f27f5..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ /dev/null @@ -1,864 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 -*/ - - - -#include "btSliderConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include - -#define USE_OFFSET_FOR_CONSTANT_FRAME true - -void btSliderConstraint::initParams() -{ - 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.); - m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = btScalar(0.); - m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM; - - m_poweredLinMotor = false; - 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_accumulatedAngMotorImpulse = btScalar(0.0); - - m_flags = 0; - m_flags = 0; - - m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME; - - 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) -{ - 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) -{ - ///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()); - - initParams(); -} - - - - - - -void btSliderConstraint::getInfo1(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - testAngLimits(); - testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) - { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; - } - if(getSolveAngLimit() || getPoweredAngMotor()) - { - 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; -} - -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()); -} - - - - - - - -void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) - { - m_calculatedTransformA = transA * m_frameInA; - m_calculatedTransformB = transB * m_frameInB; - } - else - { - m_calculatedTransformA = transB * m_frameInB; - m_calculatedTransformB = transA * m_frameInA; - } - 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_delta = m_realPivotBInW - m_realPivotAInW; - } - else - { - m_delta = m_realPivotAInW - m_realPivotBInW; - } - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //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_depth[0] > m_upperLinLimit) - { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; - } - else if(m_depth[0] < m_lowerLinLimit) - { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; - } - else - { - m_depth[0] = btScalar(0.); - } - } - else - { - m_depth[0] = btScalar(0.); - } -} - - - -void btSliderConstraint::testAngLimits(void) -{ - m_angDepth = btScalar(0.); - m_solveAngLim = false; - 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)); - rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); - m_angPos = rot; - if(rot < m_lowerAngLimit) - { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) - { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; - } - } -} - -btVector3 btSliderConstraint::getAncorInA(void) -{ - btVector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; - ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; - return ancorInA; -} - - - -btVector3 btSliderConstraint::getAncorInB(void) -{ - btVector3 ancorInB; - ancorInB = m_frameInB.getOrigin(); - return ancorInB; -} - - -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 - btScalar miA = rbAinvMass; - btScalar miB = rbBinvMass; - bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); - btScalar miS = miA + miB; - btScalar factA, factB; - if(miS > btScalar(0.f)) - { - factA = miB / miS; - } - else - { - factA = btScalar(0.5f); - } - factB = btScalar(1.0f) - factA; - btVector3 ax1, p, q; - btVector3 ax1A = trA.getBasis().getColumn(0); - btVector3 ax1B = trB.getBasis().getColumn(0); - 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); - } - else - { // old way - use frameA - ax1 = trA.getBasis().getColumn(0); - // get 2 orthos to slider axis (Y, Z) - p = trA.getBasis().getColumn(1); - q = trA.getBasis().getColumn(2); - } - // make rotations around these orthos equal - // the slider axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the slider axis should be equal. thus the constraint equations are - // p*w1 - p*w2 = 0 - // q*w1 - q*w2 = 0 - // where p and q are unit vectors normal to the slider axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - info->m_J1angularAxis[0] = p[0]; - info->m_J1angularAxis[1] = p[1]; - info->m_J1angularAxis[2] = p[2]; - info->m_J1angularAxis[s+0] = q[0]; - info->m_J1angularAxis[s+1] = q[1]; - info->m_J1angularAxis[s+2] = q[2]; - - info->m_J2angularAxis[0] = -p[0]; - info->m_J2angularAxis[1] = -p[1]; - info->m_J2angularAxis[2] = -p[2]; - info->m_J2angularAxis[s+0] = -q[0]; - info->m_J2angularAxis[s+1] = -q[1]; - info->m_J2angularAxis[s+2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the slider back into alignment. - // if ax1A,ax1B are the unit length slider axes as computed from bodyA and - // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). - // if "theta" is the angle between ax1 and ax2, we need an angular velocity - // along u to cover angle erp*theta in one step : - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (erp*fps) * (ax1 x ax2) - // ax1 x ax2 is in the plane space of ax1, so we project the angular - // velocity to p and q to find the right hand side. -// btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); - 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) - { - info->cfm[0] = m_cfmOrthoAng; - info->cfm[s] = m_cfmOrthoAng; - } - - int nrow = 1; // last filled row - int srow; - btScalar limit_err; - int limit; - int powered; - - // next two rows. - // we want: velA + wA x relA == velB + wB x relB ... but this would - // result in three equations, so we project along two orthos to the slider axis - - btTransform bodyA_trans = transA; - btTransform bodyB_trans = transB; - nrow++; - 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) - { - // get vector from bodyB to frameB in WCS - relB = trB.getOrigin() - bodyB_trans.getOrigin(); - // get its projection to slider axis - btVector3 projB = ax1 * relB.dot(ax1); - // get vector directed from bodyB to slider axis (and orthogonal to it) - btVector3 orthoB = relB - projB; - // same for bodyA - relA = trA.getOrigin() - bodyA_trans.getOrigin(); - btVector3 projA = ax1 * relA.dot(ax1); - btVector3 orthoA = relA - projA; - // get desired offset between frames A and B along slider axis - btScalar sliderOffs = m_linPos - m_depth[0]; - // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis - btVector3 totalDist = projA + ax1 * sliderOffs - projB; - // get offset vectors relA and relB - relA = orthoA + totalDist * factA; - relB = orthoB - totalDist * factB; - // now choose average ortho to slider axis - p = orthoB * factA + orthoA * factB; - btScalar len2 = p.length2(); - if(len2 > SIMD_EPSILON) - { - p /= btSqrt(len2); - } - else - { - p = trA.getBasis().getColumn(1); - } - // make one more ortho - q = ax1.cross(p); - // 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]; - tmpA = relA.cross(q); - tmpB = relB.cross(q); - 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]; - } - else - { // 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]; - tmp = c.cross(q); - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; - - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - 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 - - // k = info->fps * info->erp * getSoftnessOrthoLin(); - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp; - k = info->fps * currERP; - - btScalar rhs = k * p.dot(ofs); - info->m_constraintError[s2] = rhs; - rhs = k * q.dot(ofs); - info->m_constraintError[s3] = rhs; - 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()) - { - limit_err = getLinDepth() * signFact; - limit = (limit_err > btScalar(0.0)) ? 2 : 1; - } - powered = 0; - if(getPoweredLinMotor()) - { - powered = 1; - } - // if the slider has joint limits or motor, add in the extra row - if (limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1linearAxis[srow+0] = ax1[0]; - info->m_J1linearAxis[srow+1] = ax1[1]; - info->m_J1linearAxis[srow+2] = ax1[2]; - 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 - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in limited slider-jointed free - // bodies from gaining angular momentum. - if(m_useOffsetForConstraintFrame) - { - // this is needed only when bodyA and bodyB are both dynamic. - 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]; - } - } - else - { // 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]; - } - // right-hand part - btScalar lostop = getLowerLinLimit(); - btScalar histop = getUpperLinLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - info->m_constraintError[srow] = 0.; - info->m_lowerLimit[srow] = 0.; - info->m_upperLimit[srow] = 0.; - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) - { - info->cfm[srow] = m_cfmDirLin; - } - btScalar tag_vel = getTargetLinMotorVelocity(); - btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); - info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); - info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; - info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN) - { - info->cfm[srow] = m_cfmLimLin; - } - if(lostop == histop) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else if(limit == 1) - { // low limit - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - else - { // high limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) - btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); - if(bounce > btScalar(0.0)) - { - btScalar vel = 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) - { - btScalar newc = -bounce * vel; - if (newc > info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - else - { // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - } - info->m_constraintError[srow] *= getSoftnessLimLin(); - } // if(limit) - } // if linear limit - // check angular limits - limit_err = btScalar(0.0); - limit = 0; - if(getSolveAngLimit()) - { - limit_err = getAngDepth(); - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - } - // if the slider has joint limits, add in the extra row - powered = 0; - if(getPoweredAngMotor()) - { - powered = 1; - } - if(limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; - - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; - - btScalar lostop = getLowerAngLimit(); - btScalar histop = getUpperAngLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; - if(powered) - { - if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG) - { - info->cfm[srow] = m_cfmDirAng; - } - btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP); - info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); - info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; - info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * currERP; - info->m_constraintError[srow] += k * limit_err; - if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) - { - info->cfm[srow] = m_cfmLimAng; - } - if(lostop == histop) - { - // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else if(limit == 1) - { // low limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { // high limit - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) - btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); - if(bounce > btScalar(0.0)) - { - btScalar vel = m_rbA.getAngularVelocity().dot(ax1); - vel -= m_rbB.getAngularVelocity().dot(ax1); - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) - { - btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - else - { // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - } - info->m_constraintError[srow] *= getSoftnessLimAng(); - } // if(limit) - } // if angular limit or powered -} - - -///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) - { - 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 retVal(SIMD_INFINITY); - 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; - } - return retVal; -} - - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.h deleted file mode 100644 index 57ebb47d8..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ /dev/null @@ -1,361 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -TODO: - - add clamping od accumulated impulse to improve stability - - add conversion for ODE constraint solver -*/ - -#ifndef BT_SLIDER_CONSTRAINT_H -#define BT_SLIDER_CONSTRAINT_H - -#ifdef BT_USE_DOUBLE_PRECISION -#define btSliderConstraintData2 btSliderConstraintDoubleData -#define btSliderConstraintDataName "btSliderConstraintDoubleData" -#else -#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)) - - -enum btSliderFlags -{ - BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0), - BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1), - BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2), - BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3), - BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4), - BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5), - BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6), - BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7), - BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8), - BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9), - BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10), - BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11) -}; - - -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; - // use frameA fo define limits, if true - bool m_useLinearReferenceFrameA; - // linear limits - btScalar m_lowerLinLimit; - btScalar m_upperLinLimit; - // angular limits - btScalar m_lowerAngLimit; - btScalar m_upperAngLimit; - // softness, restitution and damping for different cases - // DirLin - moving inside linear limits - // LimLin - hitting linear limit - // DirAng - moving inside angular limits - // LimAng - hitting angular limit - // OrthoLin, OrthoAng - against constraint axis - btScalar m_softnessDirLin; - btScalar m_restitutionDirLin; - btScalar m_dampingDirLin; - btScalar m_cfmDirLin; - - btScalar m_softnessDirAng; - btScalar m_restitutionDirAng; - btScalar m_dampingDirAng; - btScalar m_cfmDirAng; - - btScalar m_softnessLimLin; - btScalar m_restitutionLimLin; - btScalar m_dampingLimLin; - btScalar m_cfmLimLin; - - btScalar m_softnessLimAng; - btScalar m_restitutionLimAng; - btScalar m_dampingLimAng; - btScalar m_cfmLimAng; - - btScalar m_softnessOrthoLin; - btScalar m_restitutionOrthoLin; - btScalar m_dampingOrthoLin; - btScalar m_cfmOrthoLin; - - btScalar m_softnessOrthoAng; - 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_jacAng[3]; - - btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - - btVector3 m_sliderAxis; - btVector3 m_realPivotAInW; - btVector3 m_realPivotBInW; - btVector3 m_projPivotInW; - btVector3 m_delta; - btVector3 m_depth; - btVector3 m_relPosA; - btVector3 m_relPosB; - - btScalar m_linPos; - btScalar m_angPos; - - 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; - - //------------------------ - 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); - - // overrides - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual(btConstraintInfo1* info); - - 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); } - bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } - btScalar getSoftnessDirLin() { return m_softnessDirLin; } - btScalar getRestitutionDirLin() { return m_restitutionDirLin; } - btScalar getDampingDirLin() { return m_dampingDirLin ; } - btScalar getSoftnessDirAng() { return m_softnessDirAng; } - btScalar getRestitutionDirAng() { return m_restitutionDirAng; } - btScalar getDampingDirAng() { return m_dampingDirAng; } - btScalar getSoftnessLimLin() { return m_softnessLimLin; } - btScalar getRestitutionLimLin() { return m_restitutionLimLin; } - btScalar getDampingLimLin() { return m_dampingLimLin; } - btScalar getSoftnessLimAng() { return m_softnessLimAng; } - btScalar getRestitutionLimAng() { return m_restitutionLimAng; } - btScalar getDampingLimAng() { return m_dampingLimAng; } - btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } - btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } - btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } - btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } - btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } - btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } - void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } - void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } - void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } - void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } - void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } - void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } - void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } - void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } - void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } - void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } - void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } - void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } - void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } - void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } - void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } - void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } - void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } - void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } - void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } - bool getPoweredLinMotor() { return m_poweredLinMotor; } - void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } - btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } - void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } - btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } - void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } - bool getPoweredAngMotor() { return m_poweredAngMotor; } - void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } - btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } - void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } - btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } - - btScalar getLinearPos() const { return m_linPos; } - btScalar getAngularPos() const { return m_angPos; } - - - - // access for ODE solver - bool getSolveLinLimit() { return m_solveLinLim; } - btScalar getLinDepth() { return m_depth[0]; } - 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(); - // access for PE Solver - btVector3 getAncorInA(); - btVector3 getAncorInB(); - // access for UseFrameOffset - 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()); - buildJacobian(); - } - - - ///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); - ///return the local value of parameter - virtual btScalar 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, 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. - btTransformFloatData m_rbBFrame; - - float m_linearUpperLimit; - float m_linearLowerLimit; - - 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. - btTransformDoubleData m_rbBFrame; - - double m_linearUpperLimit; - double m_linearLowerLimit; - - double m_angularUpperLimit; - double m_angularLowerLimit; - - int m_useLinearReferenceFrameA; - int m_useOffsetForConstraintFrame; - -}; - -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 -{ - - btSliderConstraintData2* sliderData = (btSliderConstraintData2*) dataBuffer; - btTypedConstraint::serialize(&sliderData->m_typeConstraintData,serializer); - - m_frameInA.serialize(sliderData->m_rbAFrame); - m_frameInB.serialize(sliderData->m_rbBFrame); - - sliderData->m_linearUpperLimit = m_upperLinLimit; - sliderData->m_linearLowerLimit = m_lowerLinLimit; - - sliderData->m_angularUpperLimit = m_upperAngLimit; - sliderData->m_angularLowerLimit = m_lowerAngLimit; - - sliderData->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA; - sliderData->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame; - - return btSliderConstraintDataName; -} - - - -#endif //BT_SLIDER_CONSTRAINT_H - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp deleted file mode 100644 index 0c7dbd668..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ /dev/null @@ -1,255 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btSolve2LinearConstraint.h" - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" - - -void btSolve2LinearConstraint::resolveUnilateralPairConstraint( - 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, - - 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.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - 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); - - //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)); - -// 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; - - - // 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 ); - - //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; - - //[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, - - 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.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - 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); - - //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)); - - // calculate rhs (or error) terms - 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 ); - - //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; - - //[a b] [d -c] - //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) - - //[jA nD] * [imp0] = [dv0] - //[nD jB] [imp1] [dv1] - - if ( imp0 > btScalar(0.0)) - { - if ( imp1 > btScalar(0.0) ) - { - //both positive - } - else - { - imp1 = btScalar(0.); - - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } - } - else - { - imp0 = btScalar(0.); - - imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= btScalar(0.0) ) - { - imp1 = btScalar(0.); - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } else - { - } - } -} - - -/* -void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, - const btScalar invMassA, - const btVector3& linvelA,const btVector3& angvelA, - const btVector3& rel_posA1, - const btMatrix3x3& invInertiaBWS, - 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) -{ - -} -*/ - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h deleted file mode 100644 index e8bfabf86..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOLVE_2LINEAR_CONSTRAINT_H -#define BT_SOLVE_2LINEAR_CONSTRAINT_H - -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btVector3.h" - - -class btRigidBody; - - - -/// constraint class used for lateral tyre friction. -class btSolve2LinearConstraint -{ - btScalar m_tau; - btScalar m_damping; - -public: - - btSolve2LinearConstraint(btScalar tau,btScalar damping) - { - m_tau = tau; - m_damping = damping; - } - // - // solve unilateral constraint (equality, direct method) - // - 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, - - 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, - 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, - - 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, - const btVector3& rel_posA1, - const btMatrix3x3& invInertiaBWS, - 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); - -*/ - -}; - -#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverBody.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverBody.h deleted file mode 100644 index 27ccefe41..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverBody.h +++ /dev/null @@ -1,306 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOLVER_BODY_H -#define BT_SOLVER_BODY_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" - -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btTransformUtil.h" - -///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision -#ifdef BT_USE_SSE -#define USE_SIMD 1 -#endif // - - -#ifdef USE_SIMD - -struct btSimdScalar -{ - SIMD_FORCE_INLINE btSimdScalar() - { - - } - - SIMD_FORCE_INLINE btSimdScalar(float fl) - :m_vec128 (_mm_set1_ps(fl)) - { - } - - SIMD_FORCE_INLINE btSimdScalar(__m128 v128) - :m_vec128(v128) - { - } - union - { - __m128 m_vec128; - float m_floats[4]; - int m_ints[4]; - btScalar m_unusedPadding; - }; - SIMD_FORCE_INLINE __m128 get128() - { - return m_vec128; - } - - SIMD_FORCE_INLINE const __m128 get128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE void set128(__m128 v128) - { - m_vec128 = v128; - } - - SIMD_FORCE_INLINE operator __m128() - { - return m_vec128; - } - SIMD_FORCE_INLINE operator const __m128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE operator float() const - { - return m_floats[0]; - } - -}; - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator*(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); -} - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator+(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); -} - - -#else -#define btSimdScalar btScalar -#endif - -///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED16 (struct) btSolverBody -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - 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) - { - m_worldTransform = worldTransform; - } - - const btTransform& getWorldTransform() const - { - return m_worldTransform; - } - - - - 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); - else - velocity.setValue(0,0,0); - } - - - 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); - else - velocity.setValue(0,0,0); - } - - SIMD_FORCE_INLINE void getAngularVelocity(btVector3& 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 - 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); - } - } - - 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); - } - } - - - - const btVector3& getDeltaLinearVelocity() const - { - return m_deltaLinearVelocity; - } - - const btVector3& getDeltaAngularVelocity() const - { - return m_deltaAngularVelocity; - } - - const btVector3& getPushVelocity() const - { - return m_pushVelocity; - } - - const btVector3& getTurnVelocity() const - { - return m_turnVelocity; - } - - - //////////////////////////////////////////////// - ///some internal methods, don't use them - - btVector3& internalGetDeltaLinearVelocity() - { - return m_deltaLinearVelocity; - } - - btVector3& internalGetDeltaAngularVelocity() - { - return m_deltaAngularVelocity; - } - - const btVector3& internalGetAngularFactor() const - { - return m_angularFactor; - } - - const btVector3& internalGetInvMass() const - { - return m_invMass; - } - - void internalSetInvMass(const btVector3& invMass) - { - m_invMass = invMass; - } - - btVector3& internalGetPushVelocity() - { - return m_pushVelocity; - } - - btVector3& internalGetTurnVelocity() - { - return m_turnVelocity; - } - - SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const - { - velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); - } - - SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const - { - 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) - { - 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); - } - } - - - 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) - { - // 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 - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverConstraint.h deleted file mode 100644 index 5515e6b31..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ /dev/null @@ -1,80 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOLVER_CONSTRAINT_H -#define BT_SOLVER_CONSTRAINT_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#include "btJacobianEntry.h" -#include "LinearMath/btAlignedObjectArray.h" - -//#define NO_FRICTION_TANGENTIALS 1 -#include "btSolverBody.h" - - -///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_relpos1CrossNormal; - btVector3 m_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_numRowsForNonContactConstraint; - }; - - int m_overrideNumSolverIterations; - int m_frictionIndex; - int m_solverBodyIdA; - int m_solverBodyIdB; - - - enum btSolverConstraintType - { - BT_SOLVER_CONTACT_1D = 0, - BT_SOLVER_FRICTION_1D - }; -}; - -typedef btAlignedObjectArray btConstraintArray; - - -#endif //BT_SOLVER_CONSTRAINT_H - - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp deleted file mode 100644 index 27fdd9d3d..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ /dev/null @@ -1,222 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btTypedConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btSerializer.h" - - -#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) - -btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) -:btTypedObject(type), -m_userConstraintType(-1), -m_userConstraintId(-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_userConstraintId(-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) - { - return btScalar(1.0f); - } - else if(lowLim == uppLim) - { - return btScalar(0.0f); - } - btScalar lim_fact = btScalar(1.0f); - btScalar delta_max = vel / timeFact; - if(delta_max < btScalar(0.0f)) - { - if((pos >= lowLim) && (pos < (lowLim - delta_max))) - { - lim_fact = (lowLim - pos) / delta_max; - } - else if(pos < lowLim) - { - lim_fact = btScalar(0.0f); - } - else - { - lim_fact = btScalar(1.0f); - } - } - else if(delta_max > btScalar(0.0f)) - { - if((pos <= uppLim) && (pos > (uppLim - delta_max))) - { - lim_fact = (uppLim - pos) / delta_max; - } - else if(pos > uppLim) - { - lim_fact = btScalar(0.0f); - } - else - { - lim_fact = btScalar(1.0f); - } - } - else - { - lim_fact = btScalar(0.0f); - } - return lim_fact; -} - -///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - 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); - tcd->m_name = (char*)serializer->getUniquePointer(name); - if (tcd->m_name) - { - serializer->serializeName(name); - } - - tcd->m_objectType = m_objectType; - 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_appliedImpulse = m_appliedImpulse; - tcd->m_dbgDrawSize = m_dbgDrawSize; - - tcd->m_disableCollisionsBetweenLinkedBodies = false; - - int i; - for (i=0;im_disableCollisionsBetweenLinkedBodies = true; - for (i=0;im_disableCollisionsBetweenLinkedBodies = true; - - return btTypedConstraintDataName; -} - -btRigidBody& btTypedConstraint::getFixedBody() -{ - 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_biasFactor = _biasFactor; - m_relaxationFactor = _relaxationFactor; -} - -void btAngularLimit::test(const btScalar angle) -{ - m_correction = 0.0f; - m_sign = 0.0f; - m_solveLimit = false; - - if (m_halfRange >= 0.0f) - { - btScalar deviation = btNormalizeAngle(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; - } - } -} - - -btScalar btAngularLimit::getError() const -{ - return m_correction * m_sign; -} - -void btAngularLimit::fit(btScalar& angle) const -{ - if (m_halfRange > 0.0f) - { - btScalar relativeAngle = btNormalizeAngle(angle - m_center); - if (!btEqual(relativeAngle, m_halfRange)) - { - if (relativeAngle > 0.0f) - { - angle = getHigh(); - } - else - { - angle = getLow(); - } - } - } -} - -btScalar btAngularLimit::getLow() const -{ - return btNormalizeAngle(m_center - m_halfRange); -} - -btScalar btAngularLimit::getHigh() const -{ - return btNormalizeAngle(m_center + m_halfRange); -} diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.h deleted file mode 100644 index b58f984d0..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ /dev/null @@ -1,544 +0,0 @@ -/* -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 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" -#else -#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, - HINGE_CONSTRAINT_TYPE, - CONETWIST_CONSTRAINT_TYPE, - D6_CONSTRAINT_TYPE, - SLIDER_CONSTRAINT_TYPE, - CONTACT_CONSTRAINT_TYPE, - D6_SPRING_CONSTRAINT_TYPE, - GEAR_CONSTRAINT_TYPE, - FIXED_CONSTRAINT_TYPE, - MAX_CONSTRAINT_TYPE -}; - - -enum btConstraintParams -{ - BT_CONSTRAINT_ERP=1, - BT_CONSTRAINT_STOP_ERP, - BT_CONSTRAINT_CFM, - BT_CONSTRAINT_STOP_CFM -}; - -#if 1 - #define btAssertConstrParams(_par) btAssert(_par) -#else - #define btAssertConstrParams(_par) -#endif - - -ATTRIBUTE_ALIGNED16(struct) btJointFeedback -{ - 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 -{ - int m_userConstraintType; - - union - { - int m_userConstraintId; - void* m_userConstraintPtr; - }; - - btScalar m_breakingImpulseThreshold; - bool m_isEnabled; - bool m_needsFeedback; - int m_overrideNumSolverIterations; - - - btTypedConstraint& operator=(btTypedConstraint& other) - { - btAssert(0); - (void) other; - return *this; - } - -protected: - 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); - - struct btConstraintInfo1 { - int m_numConstraintRows,nub; - }; - - static btRigidBody& getFixedBody(); - - struct btConstraintInfo2 { - // integrator parameters: frames per second (1/stepsize), default error - // reduction parameter (0..1). - btScalar fps,erp; - - // for the first and second body, pointers to two (linear and angular) - // n*3 jacobian sub matrices, stored by rows. these matrices will have - // been initialized to 0 on entry. if the second body is zero then the - // J2xx pointers may be 0. - btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; - - // elements to jump from one row to the next in J's - int rowskip; - - // right hand sides of the equation J*v = c + cfm * lambda. cfm is the - // "constraint force mixing" vector. c is set to zero on entry, cfm is - // set to a constant value (typically very small or zero) value on entry. - btScalar *m_constraintError,*cfm; - - // lo and hi limits for variables (set to -/+ infinity on entry). - btScalar *m_lowerLimit,*m_upperLimit; - - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; - // number of solver iterations - int m_numIterations; - - //damping of the velocity - btScalar 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 buildJacobian() {}; - - ///internal method used by the constraint solver, don't use them directly - virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar 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; - - ///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() - { - return m_appliedImpulse; - } - - - btScalar getBreakingImpulseThreshold() const - { - return m_breakingImpulseThreshold; - } - - void setBreakingImpulseThreshold(btScalar 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(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {}; - - - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - - btRigidBody& getRigidBodyA() - { - return m_rbA; - } - btRigidBody& 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(btJointFeedback* jointFeedback) - { - m_jointFeedback = jointFeedback; - } - - const btJointFeedback* getJointFeedback() const - { - return m_jointFeedback; - } - - btJointFeedback* 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. - btScalar getAppliedImpulse() const - { - btAssert(m_needsFeedback); - return m_appliedImpulse; - } - - btTypedConstraintType getConstraintType () const - { - return btTypedConstraintType(m_objectType); - } - - void setDbgDrawSize(btScalar dbgDrawSize) - { - m_dbgDrawSize = dbgDrawSize; - } - btScalar 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, 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; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - 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 -// 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) - { - return angleInRadians; - } - 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) - { - btScalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians)); - btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians)); - return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians; - } - else - { - return angleInRadians; - } -} - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btTypedConstraintFloatData -{ - btRigidBodyFloatData *m_rbA; - btRigidBodyFloatData *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; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 - -#define BT_BACKWARDS_COMPATIBLE_SERIALIZATION -#ifdef BT_BACKWARDS_COMPATIBLE_SERIALIZATION -///this structure is not used, except for loading pre-2.82 .bullet files -struct btTypedConstraintData -{ - btRigidBodyData *m_rbA; - btRigidBodyData *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; - -}; -#endif //BACKWARDS_COMPATIBLE - -struct btTypedConstraintDoubleData -{ - btRigidBodyDoubleData *m_rbA; - btRigidBodyDoubleData *m_rbB; - char *m_name; - - int m_objectType; - int m_userConstraintType; - int m_userConstraintId; - int m_needsFeedback; - - double m_appliedImpulse; - double m_dbgDrawSize; - - int m_disableCollisionsBetweenLinkedBodies; - int m_overrideNumSolverIterations; - - double m_breakingImpulseThreshold; - int m_isEnabled; - char padding[4]; - -}; - - -SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btTypedConstraintData2); -} - - - -class btAngularLimit -{ -private: - btScalar - 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 - 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) - {} - - /// 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(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f); - - /// Checks conastaint angle against limit. If limit is active and the angle violates the limit - /// correction is calculated. - void test(const btScalar angle); - - /// Returns limit's softness - inline btScalar getSoftness() const - { - return m_softness; - } - - /// Returns limit's bias factor - inline btScalar getBiasFactor() const - { - return m_biasFactor; - } - - /// Returns limit's relaxation factor - inline btScalar getRelaxationFactor() const - { - return m_relaxationFactor; - } - - /// Returns correction value evaluated when test() was invoked - inline btScalar getCorrection() const - { - return m_correction; - } - - /// Returns sign value evaluated when test() was invoked - inline btScalar getSign() const - { - return m_sign; - } - - /// Gives half of the distance between min and max limit angle - inline btScalar 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(btScalar& angle) const; - - /// Returns correction value multiplied by sign value - btScalar getError() const; - - btScalar getLow() const; - - btScalar getHigh() const; - -}; - - - -#endif //BT_TYPED_CONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp deleted file mode 100644 index b009f41ae..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "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) -{ - // build frame basis - // 6DOF constraint uses Euler angles and to define limits - // it is assumed that rotational order is : - // Z - first, allowed limits are (-PI,PI); - // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number - // used to prevent constraint from instability on poles; - // new position of X, allowed limits are (-PI,PI); - // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs - // Build the frame in world coordinate system first - btVector3 zAxis = m_axis1.normalize(); - btVector3 yAxis = m_axis2.normalize(); - 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.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., 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)); -} - -void btUniversalConstraint::setAxis(const btVector3& axis1,const btVector3& 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 - - btTransform frameInW; - frameInW.setIdentity(); - frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], - xAxis[1], yAxis[1], zAxis[1], - xAxis[2], yAxis[2], zAxis[2]); - frameInW.setOrigin(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(); -} - - diff --git a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.h deleted file mode 100644 index 9e7084104..000000000 --- a/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.h +++ /dev/null @@ -1,65 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org -Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#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. -/// 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 -{ -protected: - 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); - // access - const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } - const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } - const btVector3& getAxis1() { return m_axis1; } - const btVector3& getAxis2() { return m_axis2; } - btScalar getAngle1() { return getAngle(2); } - btScalar getAngle2() { return getAngle(1); } - // limits - 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); -}; - - - -#endif // BT_UNIVERSAL_CONSTRAINT_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/Bullet-C-API.cpp b/WickedEngine/BULLET/BulletDynamics/Dynamics/Bullet-C-API.cpp deleted file mode 100644 index bd8e27483..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ /dev/null @@ -1,405 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* - Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's. - Work in progress, functionality will be added on demand. - - If possible, use the richer Bullet C++ API, by including -*/ - -#include "Bullet-C-Api.h" -#include "btBulletDynamicsCommon.h" -#include "LinearMath/btAlignedAllocator.h" - - - -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btTransform.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" - -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" - - -/* - Create and Delete a Physics SDK -*/ - -struct btPhysicsSdk -{ - -// btDispatcher* m_dispatcher; -// btOverlappingPairCache* m_pairCache; -// btConstraintSolver* m_constraintSolver - - btVector3 m_worldAabbMin; - btVector3 m_worldAabbMax; - - - //todo: version, hardware/optimization settings etc? - btPhysicsSdk() - :m_worldAabbMin(-1000,-1000,-1000), - m_worldAabbMax(1000,1000,1000) - { - - } - - -}; - -plPhysicsSdkHandle plNewBulletSdk() -{ - void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16); - return (plPhysicsSdkHandle)new (mem)btPhysicsSdk; -} - -void plDeletePhysicsSdk(plPhysicsSdkHandle physicsSdk) -{ - btPhysicsSdk* phys = reinterpret_cast(physicsSdk); - btAlignedFree(phys); -} - - -/* Dynamics World */ -plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle) -{ - btPhysicsSdk* physicsSdk = reinterpret_cast(physicsSdkHandle); - void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16); - btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration(); - mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16); - btDispatcher* dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration); - mem = btAlignedAlloc(sizeof(btAxisSweep3),16); - btBroadphaseInterface* pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax); - mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); - btConstraintSolver* constraintSolver = new(mem) btSequentialImpulseConstraintSolver(); - - mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16); - return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration); -} -void plDeleteDynamicsWorld(plDynamicsWorldHandle world) -{ - //todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAlignedFree(dynamicsWorld); -} - -void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - dynamicsWorld->stepSimulation(timeStep); -} - -void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - - dynamicsWorld->addRigidBody(body); -} - -void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - - dynamicsWorld->removeRigidBody(body); -} - -/* Rigid Body */ - -plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionShapeHandle cshape ) -{ - btTransform trans; - trans.setIdentity(); - btVector3 localInertia(0,0,0); - btCollisionShape* shape = reinterpret_cast( cshape); - btAssert(shape); - if (mass) - { - shape->calculateLocalInertia(mass,localInertia); - } - void* mem = btAlignedAlloc(sizeof(btRigidBody),16); - btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia); - btRigidBody* body = new (mem)btRigidBody(rbci); - body->setWorldTransform(trans); - body->setUserPointer(user_data); - return (plRigidBodyHandle) body; -} - -void plDeleteRigidBody(plRigidBodyHandle cbody) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody); - btAssert(body); - btAlignedFree( body); -} - - -/* Collision Shape definition */ - -plCollisionShapeHandle plNewSphereShape(plReal radius) -{ - void* mem = btAlignedAlloc(sizeof(btSphereShape),16); - return (plCollisionShapeHandle) new (mem)btSphereShape(radius); - -} - -plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z) -{ - void* mem = btAlignedAlloc(sizeof(btBoxShape),16); - return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z)); -} - -plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height) -{ - //capsule is convex hull of 2 spheres, so use btMultiSphereShape - - const int numSpheres = 2; - btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)}; - btScalar radi[numSpheres] = {radius,radius}; - void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16); - return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres); -} -plCollisionShapeHandle plNewConeShape(plReal radius, plReal height) -{ - void* mem = btAlignedAlloc(sizeof(btConeShape),16); - return (plCollisionShapeHandle) new (mem)btConeShape(radius,height); -} - -plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height) -{ - void* mem = btAlignedAlloc(sizeof(btCylinderShape),16); - return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius)); -} - -/* Convex Meshes */ -plCollisionShapeHandle plNewConvexHullShape() -{ - void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16); - return (plCollisionShapeHandle) new (mem)btConvexHullShape(); -} - - -/* Concave static triangle meshes */ -plMeshInterfaceHandle plNewMeshInterface() -{ - return 0; -} - -plCollisionShapeHandle plNewCompoundShape() -{ - void* mem = btAlignedAlloc(sizeof(btCompoundShape),16); - return (plCollisionShapeHandle) new (mem)btCompoundShape(); -} - -void plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn) -{ - btCollisionShape* colShape = reinterpret_cast(compoundShapeHandle); - btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE); - btCompoundShape* compoundShape = reinterpret_cast(colShape); - btCollisionShape* childShape = reinterpret_cast(childShapeHandle); - btTransform localTrans; - localTrans.setIdentity(); - localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2])); - localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3])); - compoundShape->addChildShape(localTrans,childShape); -} - -void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient) -{ - btQuaternion orn; - orn.setEuler(yaw,pitch,roll); - orient[0] = orn.getX(); - orient[1] = orn.getY(); - orient[2] = orn.getZ(); - orient[3] = orn.getW(); - -} - - -// extern void plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2); -// extern plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle); - - -void plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z) -{ - btCollisionShape* colShape = reinterpret_cast( cshape); - (void)colShape; - btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE); - btConvexHullShape* convexHullShape = reinterpret_cast( cshape); - convexHullShape->addPoint(btVector3(x,y,z)); - -} - -void plDeleteShape(plCollisionShapeHandle cshape) -{ - btCollisionShape* shape = reinterpret_cast( cshape); - btAssert(shape); - btAlignedFree(shape); -} -void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling) -{ - btCollisionShape* shape = reinterpret_cast( cshape); - btAssert(shape); - btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]); - shape->setLocalScaling(scaling); -} - - - -void plSetPosition(plRigidBodyHandle object, const plVector3 position) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btVector3 pos(position[0],position[1],position[2]); - btTransform worldTrans = body->getWorldTransform(); - worldTrans.setOrigin(pos); - body->setWorldTransform(worldTrans); -} - -void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]); - btTransform worldTrans = body->getWorldTransform(); - worldTrans.setRotation(orn); - body->setWorldTransform(worldTrans); -} - -void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btTransform& worldTrans = body->getWorldTransform(); - worldTrans.setFromOpenGLMatrix(matrix); -} - -void plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - body->getWorldTransform().getOpenGLMatrix(matrix); - -} - -void plGetPosition(plRigidBodyHandle object,plVector3 position) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - const btVector3& pos = body->getWorldTransform().getOrigin(); - position[0] = pos.getX(); - position[1] = pos.getY(); - position[2] = pos.getZ(); -} - -void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - const btQuaternion& orn = body->getWorldTransform().getRotation(); - orientation[0] = orn.getX(); - orientation[1] = orn.getY(); - orientation[2] = orn.getZ(); - orientation[3] = orn.getW(); -} - - - -//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); - -// extern plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); - -double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]) -{ - btVector3 vp(p1[0], p1[1], p1[2]); - btTriangleShape trishapeA(vp, - btVector3(p2[0], p2[1], p2[2]), - btVector3(p3[0], p3[1], p3[2])); - trishapeA.setMargin(0.000001f); - btVector3 vq(q1[0], q1[1], q1[2]); - btTriangleShape trishapeB(vq, - btVector3(q2[0], q2[1], q2[2]), - btVector3(q3[0], q3[1], q3[2])); - trishapeB.setMargin(0.000001f); - - // btVoronoiSimplexSolver sGjkSimplexSolver; - // btGjkEpaPenetrationDepthSolver penSolverPtr; - - static btSimplexSolverInterface sGjkSimplexSolver; - sGjkSimplexSolver.reset(); - - static btGjkEpaPenetrationDepthSolver Solver0; - static btMinkowskiPenetrationDepthSolver Solver1; - - btConvexPenetrationDepthSolver* Solver = NULL; - - Solver = &Solver1; - - btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver); - - convexConvex.m_catchDegeneracies = 1; - - // btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0); - - btPointCollector gjkOutput; - btGjkPairDetector::ClosestPointInput input; - - - btTransform tr; - tr.setIdentity(); - - input.m_transformA = tr; - input.m_transformB = tr; - - convexConvex.getClosestPoints(input, gjkOutput, 0); - - - if (gjkOutput.m_hasResult) - { - - pb[0] = pa[0] = gjkOutput.m_pointInWorld[0]; - pb[1] = pa[1] = gjkOutput.m_pointInWorld[1]; - pb[2] = pa[2] = gjkOutput.m_pointInWorld[2]; - - pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance; - pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance; - pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance; - - normal[0] = gjkOutput.m_normalOnBInWorld[0]; - normal[1] = gjkOutput.m_normalOnBInWorld[1]; - normal[2] = gjkOutput.m_normalOnBInWorld[2]; - - return gjkOutput.m_distance; - } - return -1.0f; -} - diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btActionInterface.h b/WickedEngine/BULLET/BulletDynamics/Dynamics/btActionInterface.h deleted file mode 100644 index e1fea3a49..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btActionInterface.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef _BT_ACTION_INTERFACE_H -#define _BT_ACTION_INTERFACE_H - -class btIDebugDraw; -class btCollisionWorld; - -#include "LinearMath/btScalar.h" -#include "btRigidBody.h" - -///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld -class btActionInterface -{ -protected: - - static btRigidBody& getFixedBody(); - - -public: - - virtual ~btActionInterface() - { - } - - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0; - - virtual void debugDraw(btIDebugDraw* debugDrawer) = 0; - -}; - -#endif //_BT_ACTION_INTERFACE_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp deleted file mode 100644 index fb8a4068e..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ /dev/null @@ -1,1459 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btDiscreteDynamicsWorld.h" - -//collision detection -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btQuickprof.h" - -//rigidbody & constraints -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" -#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" -#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" - -#if 0 -btAlignedObjectArray debugContacts; -btAlignedObjectArray debugNormals; -int startHit=2; -int firstHit=startHit; -#endif - -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - -} - - -class btSortConstraintOnIslandPredicate -{ - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) 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; - - btAlignedObjectArray m_bodies; - btAlignedObjectArray m_manifolds; - btAlignedObjectArray m_constraints; - - - InplaceSolverIslandCallback( - btConstraintSolver* solver, - btStackAlloc* stackAlloc, - btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_sortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) - { - - } - - InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - - 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); - } - - - 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->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 - btTypedConstraint** startConstraint = 0; - int numCurConstraints = 0; - int i; - - //find the first constraint for this island - for (i=0;im_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - - for (i=0;im_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - 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; - - 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_synchronizeAllMotionStates(false), -m_applySpeculativeContactRestitution(false), -m_profileTimings(0), -m_fixedTimeStep(0), -m_latencyMotionStateInterpolation(true) - -{ - if (!m_constraintSolver) - { - void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); - m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; - m_ownsConstraintSolver = true; - } else - { - m_ownsConstraintSolver = false; - } - - { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); - m_islandManager = new (mem) btSimulationIslandManager(); - } - - m_ownsIslandManager = true; - - { - 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); - } - if (m_solverIslandCallback) - { - m_solverIslandCallback->~InplaceSolverIslandCallback(); - btAlignedFree(m_solverIslandCallback); - } - if (m_ownsConstraintSolver) - { - - m_constraintSolver->~btConstraintSolver(); - btAlignedFree(m_constraintSolver); - } -} - -void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) -{ -///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows -///to switch status _after_ adding kinematic objects to the world -///fix it for Bullet 3.x release - for (int i=0;igetActivationState() != ISLAND_SLEEPING) - { - if (body->isKinematicObject()) - { - //to calculate velocities next frame - body->saveKinematicState(timeStep); - } - } - } - -} - -void btDiscreteDynamicsWorld::debugDrawWorld() -{ - BT_PROFILE("debugDrawWorld"); - - btCollisionWorld::debugDrawWorld(); - - bool drawConstraints = false; - if (getDebugDrawer()) - { - int mode = getDebugDrawer()->getDebugMode(); - if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) - { - drawConstraints = true; - } - } - if(drawConstraints) - { - for(int i = getNumConstraints()-1; i>=0 ;i--) - { - btTypedConstraint* constraint = getConstraint(i); - debugDrawConstraint(constraint); - } - } - - - - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) - { - int i; - - if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) - { - for (i=0;idebugDraw(m_debugDrawer); - } - } - } -} - -void btDiscreteDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;iclearForces(); - } -} - -///apply gravity, call this once per timestep -void btDiscreteDynamicsWorld::applyGravity() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;iisActive()) - { - body->applyGravity(); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) -{ - btAssert(body); - - if (body->getMotionState() && !body->isStaticOrKinematicObject()) - { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - ///@todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(), - (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(), - interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeMotionStates() -{ - BT_PROFILE("synchronizeMotionStates"); - if (m_synchronizeAllMotionStates) - { - //iterate over all collision objects - for ( int i=0;iisActive()) - synchronizeSingleMotionState(body); - } - } -} - - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - startProfiling(timeStep); - - BT_PROFILE("stepSimulation"); - - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_fixedTimeStep = fixedTimeStep; - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep; - m_fixedTimeStep = 0; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (getDebugDrawer()) - { - btIDebugDraw* debugDrawer = getDebugDrawer (); - gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; - } - if (numSimulationSubSteps) - { - - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; - - saveKinematicState(fixedTimeStep*clampedSimulationSteps); - - applyGravity(); - - - - for (int i=0;iisActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(gravity); - } - } -} - -btVector3 btDiscreteDynamicsWorld::getGravity () const -{ - return m_gravity; -} - -void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) -{ - btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); -} - -void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btRigidBody* body = btRigidBody::upcast(collisionObject); - if (body) - removeRigidBody(body); - else - btCollisionWorld::removeCollisionObject(collisionObject); -} - -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - m_nonStaticRigidBodies.remove(body); - btCollisionWorld::removeCollisionObject(body); -} - - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) -{ - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - if (!body->isStaticObject()) - { - m_nonStaticRigidBodies.push_back(body); - } else - { - body->setActivationState(ISLAND_SLEEPING); - } - - bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); - } -} - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) -{ - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - if (!body->isStaticObject()) - { - m_nonStaticRigidBodies.push_back(body); - } - else - { - body->setActivationState(ISLAND_SLEEPING); - } - addCollisionObject(body,group,mask); - } -} - - -void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) -{ - BT_PROFILE("updateActions"); - - for ( int i=0;iupdateAction( this, timeStep); - } -} - - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BT_PROFILE("updateActivationState"); - - for ( int i=0;iupdateDeactivation(timeStep); - - if (body->wantsSleeping()) - { - if (body->isStaticOrKinematicObject()) - { - body->setActivationState(ISLAND_SLEEPING); - } else - { - if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); - if (body->getActivationState() == ISLAND_SLEEPING) - { - body->setAngularVelocity(btVector3(0,0,0)); - body->setLinearVelocity(btVector3(0,0,0)); - } - - } - } else - { - if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); - } - } - } -} - -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) -{ - m_constraints.push_back(constraint); - if (disableCollisionsBetweenLinkedBodies) - { - constraint->getRigidBodyA().addConstraintRef(constraint); - constraint->getRigidBodyB().addConstraintRef(constraint); - } -} - -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) -{ - m_constraints.remove(constraint); - constraint->getRigidBodyA().removeConstraintRef(constraint); - constraint->getRigidBodyB().removeConstraintRef(constraint); -} - -void btDiscreteDynamicsWorld::addAction(btActionInterface* action) -{ - m_actions.push_back(action); -} - -void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) -{ - m_actions.remove(action); -} - - -void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) -{ - addAction(vehicle); -} - -void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) -{ - removeAction(vehicle); -} - -void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) -{ - addAction(character); -} - -void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) -{ - removeAction(character); -} - - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - BT_PROFILE("solveConstraints"); - - m_sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;isetup(&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_solverIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); -} - - -void btDiscreteDynamicsWorld::calculateSimulationIslands() -{ - BT_PROFILE("calculateSimulationIslands"); - - 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]; - - 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++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - if (constraint->isEnabled()) - { - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } - } - - //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) - { - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) - { - if (convexResult.m_hitCollisionObject == m_me) - return 1.0f; - - //ignore result if there is no contact response - if(!convexResult.m_hitCollisionObject->hasContactResponse()) - return 1.0f; - - btVector3 linVelA,linVelB; - linVelA = m_convexToWorld-m_convexFromWorld; - linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); - - btVector3 relativeVelocity = (linVelA-linVelB); - //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) - return 1.f; - - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - //don't collide with itself - if (proxy0->m_clientObject == m_me) - return false; - - ///don't do CCD when the collision filters are not matching - if (!ClosestConvexResultCallback::needsCollision(proxy0)) - return false; - - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; - - //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 - if (m_dispatcher->needsResponse(m_me,otherObj)) - { -#if 0 - ///don't do CCD when there are already contact points (touching contact/penetration) - btAlignedObjectArray manifoldArray; - btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); - if (collisionPair) - { - if (collisionPair->m_algorithm) - { - manifoldArray.resize(0); - collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j=0;jgetNumContacts()>0) - return false; - } - } - } -#endif - return true; - } - - return false; - } - - -}; - -///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; - - -void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) -{ - BT_PROFILE("createPredictiveContacts"); - - { - BT_PROFILE("release predictive contact manifolds"); - - for (int i=0;im_dispatcher1->releaseManifold(manifold); - } - m_predictiveManifolds.clear(); - } - - btTransform predictedTrans; - for ( int i=0;isetHitFraction(1.f); - - if (body->isActive() && (!body->isStaticOrKinematicObject())) - { - - body->predictIntegratedTransform(timeStep, predictedTrans); - - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("predictive convexSweepTest"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; -#ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY - class StaticOnlyCallback : public btClosestNotMeConvexResultCallback - { - public: - - 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 - { - 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()); -#else - 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; - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - btTransform modifiedPredictedTrans = predictedTrans; - modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); - if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) - { - - 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); - m_predictiveManifolds.push_back(manifold); - - 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); - - 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_positionWorldOnA = body->getWorldTransform().getOrigin(); - pt.m_positionWorldOnB = worldPointB; - - } - } - } - } - } -} -void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - BT_PROFILE("integrateTransforms"); - 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(); - - - - if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("CCD motion clamping"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; -#ifdef USE_STATIC_ONLY - class StaticOnlyCallback : public btClosestNotMeConvexResultCallback - { - public: - - 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 - { - 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()); -#else - 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; - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - btTransform modifiedPredictedTrans = predictedTrans; - modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - - 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->setHitFraction(0.f); - body->proceedToTransform( predictedTrans); - -#if 0 - btVector3 linVel = body->getLinearVelocity(); - - btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep; - btScalar maxSpeedSqr = maxSpeed*maxSpeed; - if (linVel.length2()>maxSpeedSqr) - { - linVel.normalize(); - linVel*= maxSpeed; - body->setLinearVelocity(linVel); - btScalar ms2 = body->getLinearVelocity().length2(); - body->predictIntegratedTransform(timeStep, predictedTrans); - - btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - btScalar smt = body->getCcdSquareMotionThreshold(); - printf("sm2=%f\n",sm2); - } -#else - - //don't apply the collision response right now, it will happen next frame - //if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution. - //btScalar appliedImpulse = 0.f; - //btScalar depth = 0.f; - //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); - - -#endif - - continue; - } - } - } - - - body->proceedToTransform( predictedTrans); - - } - - } - - ///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++) - { - const btManifoldPoint& pt = manifold->getContactPoint(p); - btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1); - - 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; - - const btVector3& pos1 = pt.getPositionWorldOnA(); - const btVector3& pos2 = pt.getPositionWorldOnB(); - - btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin(); - btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin(); - - if (body0) - body0->applyImpulse(imp,rel_pos0); - if (body1) - body1->applyImpulse(-imp,rel_pos1); - } - } - } - } - -} - - - - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;iisStaticOrKinematicObject()) - { - //don't integrate/update velocities here, it happens in the constraint solver - - body->applyDamping(timeStep); - - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } - } -} - - -void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) -{ - (void)timeStep; - -#ifndef BT_NO_PROFILE - CProfileManager::Reset(); -#endif //BT_NO_PROFILE - -} - - - - - - -void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) -{ - bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; - bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; - btScalar dbgDrawSize = constraint->getDbgDrawSize(); - if(dbgDrawSize <= btScalar(0.f)) - { - return; - } - - switch(constraint->getConstraintType()) - { - case POINT2POINT_CONSTRAINT_TYPE: - { - btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; - btTransform tr; - tr.setIdentity(); - btVector3 pivot = p2pC->getPivotInA(); - pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - getDebugDrawer()->drawTransform(tr, dbgDrawSize); - // that ideally should draw the same frame - pivot = p2pC->getPivotInB(); - pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - } - break; - case HINGE_CONSTRAINT_TYPE: - { - btHingeConstraint* pHinge = (btHingeConstraint*)constraint; - btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - btScalar minAng = pHinge->getLowerLimit(); - btScalar maxAng = pHinge->getUpperLimit(); - if(minAng == maxAng) - { - break; - } - bool drawSect = true; - if(minAng > maxAng) - { - minAng = btScalar(0.f); - maxAng = SIMD_2_PI; - drawSect = false; - } - if(drawLimits) - { - btVector3& center = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); - } - } - break; - case CONETWIST_CONSTRAINT_TYPE: - { - btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; - btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - //const btScalar length = btScalar(5); - const btScalar length = dbgDrawSize; - static int nSegments = 8*4; - btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); - btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); - pPrev = tr * pPrev; - for (int i=0; iGetPointForAngle(fAngleInRadians, length); - pCur = tr * pCur; - getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); - - if (i%(nSegments/8) == 0) - getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); - - pPrev = pCur; - } - btScalar tws = pCT->getTwistSpan(); - btScalar twa = pCT->getTwistAngle(); - bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); - if(useFrameB) - { - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - } - else - { - tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - } - btVector3 pivot = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis1 = tr.getBasis().getColumn(1); - getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); - - } - } - break; - case D6_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) - { - tr = p6DOF->getCalculatedTransformA(); - const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); - btVector3 up = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; - btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); - axis = tr.getBasis().getColumn(1); - btScalar ay = p6DOF->getAngle(1); - btScalar az = p6DOF->getAngle(2); - btScalar cy = btCos(ay); - btScalar sy = btSin(ay); - btScalar cz = btCos(az); - btScalar sz = btSin(az); - btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; - tr = p6DOF->getCalculatedTransformB(); - btVector3 normal = -tr.getBasis().getColumn(0); - btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; - btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; - if(minFi > maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); - } - else if(minFi < maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); - } - tr = p6DOF->getCalculatedTransformA(); - btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; - btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; - getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); - } - } - break; - case SLIDER_CONSTRAINT_TYPE: - { - btSliderConstraint* pSlider = (btSliderConstraint*)constraint; - btTransform tr = pSlider->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pSlider->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - btTransform tr = pSlider->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; - } - return; -} - - - - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree( m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; - m_solverIslandCallback->m_solver = solver; -} - -btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} - - -int btDiscreteDynamicsWorld::getNumConstraints() const -{ - return int(m_constraints.size()); -} -btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) -{ - return m_constraints[index]; -} -const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const -{ - return m_constraints[index]; -} - - - -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); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - 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); - } -} - - - - -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 - - 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; - - 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_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_splitImpulse = getSolverInfo().m_splitImpulse; - -#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); -} - -void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) -{ - - serializer->startSerialization(); - - serializeDynamicsWorldInfo(serializer); - - serializeRigidBodies(serializer); - - serializeCollisionObjects(serializer); - - serializer->finishSerialization(); -} - diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h deleted file mode 100644 index d8a34b7da..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ /dev/null @@ -1,234 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_DISCRETE_DYNAMICS_WORLD_H -#define BT_DISCRETE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; -class btSimulationIslandManager; -class btTypedConstraint; -class btActionInterface; -class btPersistentManifold; -class btIDebugDraw; -struct InplaceSolverIslandCallback; - -#include "LinearMath/btAlignedObjectArray.h" - - -///btDiscreteDynamicsWorld provides discrete rigid body simulation -///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld -{ -protected: - - btAlignedObjectArray m_sortedConstraints; - InplaceSolverIslandCallback* m_solverIslandCallback; - - btConstraintSolver* m_constraintSolver; - - btSimulationIslandManager* m_islandManager; - - btAlignedObjectArray m_constraints; - - btAlignedObjectArray m_nonStaticRigidBodies; - - btVector3 m_gravity; - - //for variable timesteps - btScalar m_localTime; - btScalar m_fixedTimeStep; - //for variable timesteps - - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - bool m_synchronizeAllMotionStates; - bool m_applySpeculativeContactRestitution; - - btAlignedObjectArray m_actions; - - int m_profileTimings; - - bool m_latencyMotionStateInterpolation; - - btAlignedObjectArray m_predictiveManifolds; - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void integrateTransforms(btScalar timeStep); - - virtual void calculateSimulationIslands(); - - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void updateActivationState(btScalar timeStep); - - void updateActions(btScalar timeStep); - - void startProfiling(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - 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); - - 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 void synchronizeMotionStates(); - - ///this can be useful to synchronize a single rigid body -> graphics object - void synchronizeSingleMotionState(btRigidBody* body); - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - - virtual void removeConstraint(btTypedConstraint* constraint); - - virtual void addAction(btActionInterface*); - - virtual void removeAction(btActionInterface*); - - btSimulationIslandManager* getSimulationIslandManager() - { - return m_islandManager; - } - - const btSimulationIslandManager* getSimulationIslandManager() const - { - return m_islandManager; - } - - btCollisionWorld* getCollisionWorld() - { - return this; - } - - virtual void setGravity(const btVector3& gravity); - - virtual btVector3 getGravity () const; - - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - virtual void addRigidBody(btRigidBody* body); - - virtual void addRigidBody(btRigidBody* body, short group, short mask); - - 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); - - - void debugDrawConstraint(btTypedConstraint* constraint); - - virtual void debugDrawWorld(); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - - virtual btTypedConstraint* getConstraint(int index) ; - - virtual const btTypedConstraint* getConstraint(int index) 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(); - - ///apply gravity, call this once per timestep - virtual void applyGravity(); - - virtual void setNumTasks(int numTasks) - { - (void) numTasks; - } - - ///obsolete, use updateActions instead - virtual void updateVehicles(btScalar timeStep) - { - updateActions(timeStep); - } - - ///obsolete, use addAction instead - virtual void addVehicle(btActionInterface* vehicle); - ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle); - ///obsolete, use addAction instead - virtual void addCharacter(btActionInterface* character); - ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character); - - void setSynchronizeAllMotionStates(bool synchronizeAll) - { - m_synchronizeAllMotionStates = synchronizeAll; - } - bool getSynchronizeAllMotionStates() const - { - return m_synchronizeAllMotionStates; - } - - void setApplySpeculativeContactRestitution(bool enable) - { - 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); - - ///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 ) - { - m_latencyMotionStateInterpolation = latencyInterpolation; - } - bool getLatencyMotionStateInterpolation() const - { - return m_latencyMotionStateInterpolation; - } -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btDynamicsWorld.h b/WickedEngine/BULLET/BulletDynamics/Dynamics/btDynamicsWorld.h deleted file mode 100644 index 35dd1400f..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btDynamicsWorld.h +++ /dev/null @@ -1,167 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_DYNAMICS_WORLD_H -#define BT_DYNAMICS_WORLD_H - -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -class btTypedConstraint; -class btActionInterface; -class btConstraintSolver; -class btDynamicsWorld; - - -/// Type for the callback for each tick -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 -}; - -///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; - - btContactSolverInfo m_solverInfo; - -public: - - - 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, short group, short 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; - } - - - ///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; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btDynamicsWorldFloatData -{ - btContactSolverInfoFloatData m_solverInfo; - btVector3FloatData m_gravity; -}; - - -#endif //BT_DYNAMICS_WORLD_H - - diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.cpp b/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.cpp deleted file mode 100644 index 222f90066..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.cpp +++ /dev/null @@ -1,400 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btRigidBody.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "LinearMath/btMinMax.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btMotionState.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "LinearMath/btSerializer.h" - -//'temporarily' global variables -btScalar gDeactivationTime = btScalar(2.); -bool gDisableDeactivation = false; -static int uniqueId = 0; - - -btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) -{ - setupRigidBody(constructionInfo); -} - -btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) -{ - btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); - setupRigidBody(cinfo); -} - -void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) -{ - - m_internalType=CO_RIGID_BODY; - - m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - m_angularFactor.setValue(1,1,1); - m_linearFactor.setValue(1,1,1); - m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); - - m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; - m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; - m_optionalMotionState = constructionInfo.m_motionState; - m_contactSolverType = 0; - m_frictionSolverType = 0; - m_additionalDamping = constructionInfo.m_additionalDamping; - m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor; - m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr; - m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr; - m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor; - - if (m_optionalMotionState) - { - m_optionalMotionState->getWorldTransform(m_worldTransform); - } else - { - m_worldTransform = constructionInfo.m_startWorldTransform; - } - - m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - - //moved to btCollisionObject - m_friction = constructionInfo.m_friction; - m_rollingFriction = constructionInfo.m_rollingFriction; - m_restitution = constructionInfo.m_restitution; - - setCollisionShape( constructionInfo.m_collisionShape ); - m_debugBodyId = uniqueId++; - - setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); - updateInertiaTensor(); - - m_rigidbodyFlags = 0; - - - m_deltaLinearVelocity.setZero(); - m_deltaAngularVelocity.setZero(); - m_invMass = m_inverseMass*m_linearFactor; - m_pushVelocity.setZero(); - m_turnVelocity.setZero(); - - - -} - - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) -{ - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); -} - -void btRigidBody::saveKinematicState(btScalar timeStep) -{ - //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities - if (timeStep != btScalar(0.)) - { - //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform - if (getMotionState()) - getMotionState()->getWorldTransform(m_worldTransform); - btVector3 linVel,angVel; - - btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); - m_interpolationLinearVelocity = m_linearVelocity; - m_interpolationAngularVelocity = m_angularVelocity; - m_interpolationWorldTransform = m_worldTransform; - //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); - } -} - -void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const -{ - getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); -} - - - - -void btRigidBody::setGravity(const btVector3& acceleration) -{ - if (m_inverseMass != btScalar(0.0)) - { - m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); - } - m_gravity_acceleration = acceleration; -} - - - - - - -void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) -{ - m_linearDamping = 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) -{ - //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 - //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway - -//#define USE_OLD_DAMPING_METHOD 1 -#ifdef USE_OLD_DAMPING_METHOD - m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); -#else - m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); - m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); -#endif - - if (m_additionalDamping) - { - //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. - //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete - if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) && - (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr)) - { - m_angularVelocity *= m_additionalDampingFactor; - m_linearVelocity *= m_additionalDampingFactor; - } - - - btScalar speed = m_linearVelocity.length(); - if (speed < m_linearDamping) - { - btScalar dampVel = btScalar(0.005); - if (speed > dampVel) - { - btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else - { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } - - btScalar angSpeed = m_angularVelocity.length(); - if (angSpeed < m_angularDamping) - { - btScalar angDampVel = btScalar(0.005); - if (angSpeed > angDampVel) - { - btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else - { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - } - } - } -} - - -void btRigidBody::applyGravity() -{ - if (isStaticOrKinematicObject()) - return; - - applyCentralForce(m_gravity); - -} - -void btRigidBody::proceedToTransform(const btTransform& newTrans) -{ - setCenterOfMassTransform( newTrans ); -} - - -void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) -{ - if (mass == btScalar(0.)) - { - m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; - m_inverseMass = btScalar(0.); - } else - { - m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - m_inverseMass = btScalar(1.0) / mass; - } - - //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; -} - - -void btRigidBody::updateInertiaTensor() -{ - m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); -} - - -btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const -{ - btVector3 inertiaLocal; - inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0]; - inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1]; - inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2]; - btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); - btVector3 gf = getAngularVelocity().cross(tmp); - btScalar l2 = gf.length2(); - if (l2>maxGyroscopicForce*maxGyroscopicForce) - { - gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; - } - return gf; -} - -void btRigidBody::integrateVelocities(btScalar step) -{ - if (isStaticOrKinematicObject()) - return; - - m_linearVelocity += m_totalForce * (m_inverseMass * step); - m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; - -#define MAX_ANGVEL SIMD_HALF_PI - /// clamp angular velocity. collision calculations will fail on higher angular velocities - btScalar angvel = m_angularVelocity.length(); - if (angvel*step > MAX_ANGVEL) - { - m_angularVelocity *= (MAX_ANGVEL/step) /angvel; - } - -} - -btQuaternion btRigidBody::getOrientation() const -{ - btQuaternion orn; - m_worldTransform.getBasis().getRotation(orn); - return orn; -} - - -void btRigidBody::setCenterOfMassTransform(const btTransform& xform) -{ - - if (isKinematicObject()) - { - m_interpolationWorldTransform = m_worldTransform; - } else - { - m_interpolationWorldTransform = xform; - } - m_interpolationLinearVelocity = getLinearVelocity(); - m_interpolationAngularVelocity = getAngularVelocity(); - m_worldTransform = xform; - updateInertiaTensor(); -} - - -bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const -{ - const btRigidBody* otherRb = btRigidBody::upcast(co); - if (!otherRb) - return true; - - for (int i = 0; i < m_constraintRefs.size(); ++i) - { - const btTypedConstraint* c = m_constraintRefs[i]; - if (c->isEnabled()) - if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb) - return false; - } - - return true; -} - - - -void btRigidBody::addConstraintRef(btTypedConstraint* c) -{ - int index = m_constraintRefs.findLinearSearch(c); - if (index == m_constraintRefs.size()) - m_constraintRefs.push_back(c); - - m_checkCollideWith = true; -} - -void btRigidBody::removeConstraintRef(btTypedConstraint* c) -{ - m_constraintRefs.remove(c); - m_checkCollideWith = m_constraintRefs.size() > 0; -} - -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 -{ - btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; - - btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); - - m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld); - m_linearVelocity.serialize(rbd->m_linearVelocity); - m_angularVelocity.serialize(rbd->m_angularVelocity); - rbd->m_inverseMass = m_inverseMass; - m_angularFactor.serialize(rbd->m_angularFactor); - m_linearFactor.serialize(rbd->m_linearFactor); - m_gravity.serialize(rbd->m_gravity); - m_gravity_acceleration.serialize(rbd->m_gravity_acceleration); - m_invInertiaLocal.serialize(rbd->m_invInertiaLocal); - m_totalForce.serialize(rbd->m_totalForce); - m_totalTorque.serialize(rbd->m_totalTorque); - rbd->m_linearDamping = m_linearDamping; - rbd->m_angularDamping = m_angularDamping; - rbd->m_additionalDamping = m_additionalDamping; - rbd->m_additionalDampingFactor = m_additionalDampingFactor; - rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; - rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; - rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; - rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; - rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; - - return btRigidBodyDataName; -} - - - -void btRigidBody::serializeSingleObject(class btSerializer* serializer) const -{ - btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); - const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); -} - - diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.h b/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.h deleted file mode 100644 index ed90fb441..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.h +++ /dev/null @@ -1,604 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_RIGIDBODY_H -#define BT_RIGIDBODY_H - -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btTransform.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -class btCollisionShape; -class btMotionState; -class btTypedConstraint; - - -extern btScalar gDeactivationTime; -extern bool gDisableDeactivation; - -#ifdef BT_USE_DOUBLE_PRECISION -#define btRigidBodyData btRigidBodyDoubleData -#define btRigidBodyDataName "btRigidBodyDoubleData" -#else -#define btRigidBodyData btRigidBodyFloatData -#define btRigidBodyDataName "btRigidBodyFloatData" -#endif //BT_USE_DOUBLE_PRECISION - - -enum btRigidBodyFlags -{ - BT_DISABLE_WORLD_GRAVITY = 1, - ///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability - ///So generally it is best to not enable it. - ///If really needed, run at a high frequency like 1000 Hertz: ///See Demos/GyroscopicDemo for an example use - BT_ENABLE_GYROPSCOPIC_FORCE = 2 -}; - - -///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. -///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. -///There are 3 types of rigid bodies: -///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. -///- 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. -///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 -{ - - 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; - - btScalar m_linearDamping; - btScalar m_angularDamping; - - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; - - - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; - - //m_optionalMotionState allows to automatic synchronize the world transform for active objects - btMotionState* m_optionalMotionState; - - //keep track of typed constraints referencing this rigid body - btAlignedObjectArray m_constraintRefs; - - 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; - - -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. - ///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 - { - 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; - - btCollisionShape* m_collisionShape; - btVector3 m_localInertia; - btScalar m_linearDamping; - btScalar m_angularDamping; - - ///best simulation results when friction is non-zero - 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; - ///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; - - 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_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 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)); - - - 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); - } - -protected: - - ///setupRigidBody is only used internally by the constructor - void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); - -public: - - 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) - { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) - return (const btRigidBody*)colObj; - return 0; - } - static btRigidBody* upcast(btCollisionObject* colObj) - { - 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 - { - return m_gravity_acceleration; - } - - void setDamping(btScalar lin_damping, btScalar ang_damping); - - btScalar getLinearDamping() const - { - return m_linearDamping; - } - - btScalar getAngularDamping() const - { - return m_angularDamping; - } - - btScalar getLinearSleepingThreshold() const - { - return m_linearSleepingThreshold; - } - - btScalar getAngularSleepingThreshold() const - { - return m_angularSleepingThreshold; - } - - void applyDamping(btScalar timeStep); - - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { - return m_collisionShape; - } - - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { - return m_collisionShape; - } - - void setMassProps(btScalar mass, const btVector3& inertia); - - const btVector3& getLinearFactor() const - { - return m_linearFactor; - } - void setLinearFactor(const btVector3& linearFactor) - { - m_linearFactor = linearFactor; - 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) - { - m_totalForce += force*m_linearFactor; - } - - const btVector3& getTotalForce() const - { - return m_totalForce; - }; - - const btVector3& getTotalTorque() const - { - return m_totalTorque; - }; - - const btVector3& getInvInertiaDiagLocal() const - { - return m_invInertiaLocal; - }; - - void setInvInertiaDiagLocal(const btVector3& diagInvInertia) - { - m_invInertiaLocal = diagInvInertia; - } - - void setSleepingThresholds(btScalar linear,btScalar angular) - { - m_linearSleepingThreshold = linear; - m_angularSleepingThreshold = angular; - } - - void applyTorque(const btVector3& torque) - { - m_totalTorque += torque*m_angularFactor; - } - - void applyForce(const btVector3& force, const btVector3& rel_pos) - { - applyCentralForce(force); - applyTorque(rel_pos.cross(force*m_linearFactor)); - } - - void applyCentralImpulse(const btVector3& impulse) - { - m_linearVelocity += impulse *m_linearFactor * m_inverseMass; - } - - void applyTorqueImpulse(const btVector3& torque) - { - m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; - } - - 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)); - } - } - } - - 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(); - } - btQuaternion getOrientation() const; - - const btTransform& getCenterOfMassTransform() const { - return m_worldTransform; - } - const btVector3& getLinearVelocity() const { - return m_linearVelocity; - } - const btVector3& getAngularVelocity() const { - return m_angularVelocity; - } - - - inline void setLinearVelocity(const btVector3& lin_vel) - { - m_updateRevision++; - m_linearVelocity = lin_vel; - } - - inline void setAngularVelocity(const btVector3& ang_vel) - { - m_updateRevision++; - m_angularVelocity = ang_vel; - } - - btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const - { - //we also calculate lin/ang velocity for kinematic objects - return m_linearVelocity + m_angularVelocity.cross(rel_pos); - - //for kinematic objects, we could also use use: - // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; - } - - void translate(const btVector3& v) - { - m_worldTransform.getOrigin() += v; - } - - - void getAabb(btVector3& aabbMin,btVector3& aabbMax) const; - - - - - - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const - { - btVector3 r0 = pos - getCenterOfMassPosition(); - - btVector3 c0 = (r0).cross(normal); - - btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); - - return m_inverseMass + normal.dot(vec); - - } - - SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const - { - btVector3 vec = axis * getInvInertiaTensorWorld(); - return axis.dot(vec); - } - - SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) - { - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) - return; - - if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) && - (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold)) - { - m_deactivationTime += timeStep; - } else - { - m_deactivationTime=btScalar(0.); - setActivationState(0); - } - - } - - SIMD_FORCE_INLINE bool wantsSleeping() - { - - if (getActivationState() == DISABLE_DEACTIVATION) - return false; - - //disable deactivation - if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) - return false; - - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) - return true; - - if (m_deactivationTime> gDeactivationTime) - { - return true; - } - return false; - } - - - - const btBroadphaseProxy* getBroadphaseProxy() const - { - return m_broadphaseHandle; - } - btBroadphaseProxy* getBroadphaseProxy() - { - return m_broadphaseHandle; - } - void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) - { - m_broadphaseHandle = broadphaseProxy; - } - - //btMotionState allows to automatic synchronize the world transform for active objects - btMotionState* getMotionState() - { - return m_optionalMotionState; - } - const btMotionState* getMotionState() const - { - return m_optionalMotionState; - } - void setMotionState(btMotionState* motionState) - { - m_optionalMotionState = motionState; - if (m_optionalMotionState) - motionState->getWorldTransform(m_worldTransform); - } - - //for experimental overriding of friction/contact solver func - int m_contactSolverType; - int m_frictionSolverType; - - void setAngularFactor(const btVector3& angFac) - { - m_updateRevision++; - m_angularFactor = angFac; - } - - void setAngularFactor(btScalar angFac) - { - m_updateRevision++; - m_angularFactor.setValue(angFac,angFac,angFac); - } - const btVector3& getAngularFactor() const - { - return m_angularFactor; - } - - //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? - bool isInWorld() const - { - return (getBroadphaseProxy() != 0); - } - - virtual bool checkCollideWithOverride(const btCollisionObject* co) const; - - void addConstraintRef(btTypedConstraint* c); - void removeConstraintRef(btTypedConstraint* c); - - btTypedConstraint* getConstraintRef(int index) - { - return m_constraintRefs[index]; - } - - int getNumConstraintRefs() const - { - return m_constraintRefs.size(); - } - - void setFlags(int flags) - { - m_rigidbodyFlags = flags; - } - - int getFlags() const - { - return m_rigidbodyFlags; - } - - btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) 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; - -}; - -//@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -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; -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -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]; -}; - - - -#endif //BT_RIGIDBODY_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp deleted file mode 100644 index 35dd38840..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ /dev/null @@ -1,280 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSimpleDynamicsWorld.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#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" -{ - 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() -{ - if (m_ownsConstraintSolver) - btAlignedFree( m_constraintSolver); -} - -int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - (void)fixedTimeStep; - (void)maxSubSteps; - - - ///apply gravity, predict motion - predictUnconstraintMotion(timeStep); - - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = getDebugDrawer(); - - ///perform collision detection - performDiscreteCollisionDetection(); - - ///solve contact constraints - int numManifolds = m_dispatcher1->getNumManifolds(); - 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); - } - - ///integrate transforms - integrateTransforms(timeStep); - - updateAabbs(); - - synchronizeMotionStates(); - - clearForces(); - - return 1; - -} - -void btSimpleDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;iclearForces(); - } - } -} - - -void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) -{ - m_gravity = gravity; - for ( int i=0;isetGravity(gravity); - } - } -} - -btVector3 btSimpleDynamicsWorld::getGravity () const -{ - return m_gravity; -} - -void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - btCollisionWorld::removeCollisionObject(body); -} - -void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btRigidBody* body = btRigidBody::upcast(collisionObject); - if (body) - removeRigidBody(body); - else - btCollisionWorld::removeCollisionObject(collisionObject); -} - - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) -{ - body->setGravity(m_gravity); - - if (body->getCollisionShape()) - { - addCollisionObject(body); - } -} - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) -{ - body->setGravity(m_gravity); - - if (body->getCollisionShape()) - { - addCollisionObject(body,group,mask); - } -} - - -void btSimpleDynamicsWorld::debugDrawWorld() -{ - -} - -void btSimpleDynamicsWorld::addAction(btActionInterface* action) -{ - -} - -void btSimpleDynamicsWorld::removeAction(btActionInterface* action) -{ - -} - - -void btSimpleDynamicsWorld::updateAabbs() -{ - btTransform predictedTrans; - for ( int i=0;iisActive() && (!body->isStaticObject())) - { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } - } - } -} - -void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btTransform predictedTrans; - for ( int i=0;iisActive() && (!body->isStaticObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); - } - } - } -} - - - -void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - for ( int i=0;iisStaticObject()) - { - if (body->isActive()) - { - body->applyGravity(); - body->integrateVelocities( timeStep); - body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } - } - } - } -} - - -void btSimpleDynamicsWorld::synchronizeMotionStates() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;igetMotionState()) - { - if (body->getActivationState() != ISLAND_SLEEPING) - { - body->getMotionState()->setWorldTransform(body->getWorldTransform()); - } - } - } - -} - - -void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree(m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; -} - -btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} diff --git a/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h deleted file mode 100644 index d48d2e39c..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ /dev/null @@ -1,89 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SIMPLE_DYNAMICS_WORLD_H -#define BT_SIMPLE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; - -///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. -///Please use btDiscreteDynamicsWorld instead -class btSimpleDynamicsWorld : public btDynamicsWorld -{ -protected: - - btConstraintSolver* m_constraintSolver; - - bool m_ownsConstraintSolver; - - void predictUnconstraintMotion(btScalar timeStep); - - void integrateTransforms(btScalar timeStep); - - btVector3 m_gravity; - -public: - - - - ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver - btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btSimpleDynamicsWorld(); - - ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - - virtual void setGravity(const btVector3& gravity); - - virtual btVector3 getGravity () const; - - virtual void addRigidBody(btRigidBody* body); - - virtual void addRigidBody(btRigidBody* body, short group, short mask); - - virtual void removeRigidBody(btRigidBody* body); - - virtual void debugDrawWorld(); - - virtual void addAction(btActionInterface* action); - - virtual void removeAction(btActionInterface* action); - - ///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 updateAabbs(); - - virtual void synchronizeMotionStates(); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual btDynamicsWorldType getWorldType() const - { - return BT_SIMPLE_DYNAMICS_WORLD; - } - - virtual void clearForces(); - -}; - -#endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.cpp b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.cpp deleted file mode 100644 index 56a1c55d9..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.cpp +++ /dev/null @@ -1,1009 +0,0 @@ -/* - * PURPOSE: - * Class representing an articulated rigid body. Stores the body's - * current state, allows forces and torques to be set, handles - * timestepping and implements Featherstone's algorithm. - * - * COPYRIGHT: - * Copyright (C) Stephen Thompson, , 2011-2013 - * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) - - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the use of this software. - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it freely, - subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - - */ - - -#include "btMultiBody.h" -#include "btMultiBodyLink.h" -#include "btMultiBodyLinkCollider.h" - -// #define INCLUDE_GYRO_TERM - -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 -} - - - - -// -// Various spatial helper functions -// - -namespace { - void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame - const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates - const btVector3 &top_in, // top part of input vector - const btVector3 &bottom_in, // bottom part of input vector - btVector3 &top_out, // top part of output vector - btVector3 &bottom_out) // bottom part of output vector - { - top_out = rotation_matrix * top_in; - bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; - } - - void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, - const btVector3 &displacement, - const btVector3 &top_in, - const btVector3 &bottom_in, - btVector3 &top_out, - btVector3 &bottom_out) - { - top_out = rotation_matrix.transpose() * top_in; - bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); - } - - btScalar SpatialDotProduct(const btVector3 &a_top, - const btVector3 &a_bottom, - const btVector3 &b_top, - const btVector3 &b_bottom) - { - return a_bottom.dot(b_top) + a_top.dot(b_bottom); - } -} - - -// -// Implementation of class btMultiBody -// - -btMultiBody::btMultiBody(int n_links, - btScalar mass, - const btVector3 &inertia, - bool fixed_base_, - bool can_sleep_) - : base_quat(0, 0, 0, 1), - base_mass(mass), - base_inertia(inertia), - - fixed_base(fixed_base_), - awake(true), - can_sleep(can_sleep_), - sleep_timer(0), - m_baseCollider(0), - m_linearDamping(0.04f), - m_angularDamping(0.04f), - m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), - m_hasSelfCollision(true) -{ - links.resize(n_links); - - vector_buf.resize(2*n_links); - matrix_buf.resize(n_links + 1); - m_real_buf.resize(6 + 2*n_links); - base_pos.setValue(0, 0, 0); - base_force.setValue(0, 0, 0); - base_torque.setValue(0, 0, 0); -} - -btMultiBody::~btMultiBody() -{ -} - -void btMultiBody::setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rot_parent_to_this, - const btVector3 &joint_axis, - const btVector3 &r_vector_when_q_zero, - bool disableParentCollision) -{ - links[i].mass = mass; - links[i].inertia = inertia; - links[i].parent = parent; - links[i].zero_rot_parent_to_this = rot_parent_to_this; - links[i].axis_top.setValue(0,0,0); - links[i].axis_bottom = joint_axis; - links[i].e_vector = r_vector_when_q_zero; - links[i].is_revolute = false; - links[i].cached_rot_parent_to_this = rot_parent_to_this; - if (disableParentCollision) - links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - - links[i].updateCache(); -} - -void btMultiBody::setupRevolute(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &zero_rot_parent_to_this, - const btVector3 &joint_axis, - const btVector3 &parent_axis_position, - const btVector3 &my_axis_position, - bool disableParentCollision) -{ - links[i].mass = mass; - links[i].inertia = inertia; - links[i].parent = parent; - links[i].zero_rot_parent_to_this = zero_rot_parent_to_this; - links[i].axis_top = joint_axis; - links[i].axis_bottom = joint_axis.cross(my_axis_position); - links[i].d_vector = my_axis_position; - links[i].e_vector = parent_axis_position; - links[i].is_revolute = true; - if (disableParentCollision) - links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - links[i].updateCache(); -} - - - - - -int btMultiBody::getParent(int i) const -{ - return links[i].parent; -} - -btScalar btMultiBody::getLinkMass(int i) const -{ - return links[i].mass; -} - -const btVector3 & btMultiBody::getLinkInertia(int i) const -{ - return links[i].inertia; -} - -btScalar btMultiBody::getJointPos(int i) const -{ - return links[i].joint_pos; -} - -btScalar btMultiBody::getJointVel(int i) const -{ - return m_real_buf[6 + i]; -} - -void btMultiBody::setJointPos(int i, btScalar q) -{ - links[i].joint_pos = q; - links[i].updateCache(); -} - -void btMultiBody::setJointVel(int i, btScalar qdot) -{ - m_real_buf[6 + i] = qdot; -} - -const btVector3 & btMultiBody::getRVector(int i) const -{ - return links[i].cached_r_vector; -} - -const btQuaternion & btMultiBody::getParentToLocalRot(int i) const -{ - return links[i].cached_rot_parent_to_this; -} - -btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const -{ - btVector3 result = local_pos; - while (i != -1) { - // 'result' is in frame i. transform it to frame parent(i) - result += getRVector(i); - result = quatRotate(getParentToLocalRot(i).inverse(),result); - i = getParent(i); - } - - // 'result' is now in the base frame. transform it to world frame - result = quatRotate(getWorldToBaseRot().inverse() ,result); - result += getBasePos(); - - return result; -} - -btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const -{ - if (i == -1) { - // world to base - return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); - } else { - // find position in parent frame, then transform to current frame - return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i); - } -} - -btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const -{ - btVector3 result = local_dir; - while (i != -1) { - result = quatRotate(getParentToLocalRot(i).inverse() , result); - i = getParent(i); - } - result = quatRotate(getWorldToBaseRot().inverse() , result); - return result; -} - -btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const -{ - if (i == -1) { - return quatRotate(getWorldToBaseRot(), world_dir); - } else { - return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir)); - } -} - -void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const -{ - int num_links = getNumLinks(); - // Calculates the velocities of each link (and the base) in its local frame - omega[0] = quatRotate(base_quat ,getBaseOmega()); - vel[0] = quatRotate(base_quat ,getBaseVel()); - - for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - - // transform parent vel into this frame, store in omega[i+1], vel[i+1] - SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector, - omega[parent+1], vel[parent+1], - omega[i+1], vel[i+1]); - - // now add qidot * shat_i - omega[i+1] += getJointVel(i) * links[i].axis_top; - vel[i+1] += getJointVel(i) * links[i].axis_bottom; - } -} - -btScalar btMultiBody::getKineticEnergy() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray omega;omega.resize(num_links+1); - btAlignedObjectArray vel;vel.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - // we will do the factor of 0.5 at the end - btScalar result = base_mass * vel[0].dot(vel[0]); - result += omega[0].dot(base_inertia * omega[0]); - - for (int i = 0; i < num_links; ++i) { - result += links[i].mass * vel[i+1].dot(vel[i+1]); - result += omega[i+1].dot(links[i].inertia * omega[i+1]); - } - - return 0.5f * result; -} - -btVector3 btMultiBody::getAngularMomentum() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray omega;omega.resize(num_links+1); - btAlignedObjectArray vel;vel.resize(num_links+1); - btAlignedObjectArray rot_from_world;rot_from_world.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - rot_from_world[0] = base_quat; - btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0])); - - for (int i = 0; i < num_links; ++i) { - rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1]; - result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1]))); - } - - return result; -} - - -void btMultiBody::clearForcesAndTorques() -{ - base_force.setValue(0, 0, 0); - base_torque.setValue(0, 0, 0); - - for (int i = 0; i < getNumLinks(); ++i) { - links[i].applied_force.setValue(0, 0, 0); - links[i].applied_torque.setValue(0, 0, 0); - links[i].joint_torque = 0; - } -} - -void btMultiBody::clearVelocities() -{ - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - m_real_buf[i] = 0.f; - } -} -void btMultiBody::addLinkForce(int i, const btVector3 &f) -{ - links[i].applied_force += f; -} - -void btMultiBody::addLinkTorque(int i, const btVector3 &t) -{ - links[i].applied_torque += t; -} - -void btMultiBody::addJointTorque(int i, btScalar Q) -{ - links[i].joint_torque += Q; -} - -const btVector3 & btMultiBody::getLinkForce(int i) const -{ - return links[i].applied_force; -} - -const btVector3 & btMultiBody::getLinkTorque(int i) const -{ - return links[i].applied_torque; -} - -btScalar btMultiBody::getJointTorque(int i) const -{ - return links[i].joint_torque; -} - - -inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed) -{ - btVector3 row0 = btVector3( - v0.x() * v1Transposed.x(), - v0.x() * v1Transposed.y(), - v0.x() * v1Transposed.z()); - btVector3 row1 = btVector3( - v0.y() * v1Transposed.x(), - v0.y() * v1Transposed.y(), - v0.y() * v1Transposed.z()); - btVector3 row2 = btVector3( - v0.z() * v1Transposed.x(), - v0.z() * v1Transposed.y(), - v0.z() * v1Transposed.z()); - - btMatrix3x3 m(row0[0],row0[1],row0[2], - row1[0],row1[1],row1[2], - row2[0],row2[1],row2[2]); - return m; -} - - -void btMultiBody::stepVelocities(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) -{ - // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) - // and the base linear & angular accelerations. - - // We apply damping forces in this routine as well as any external forces specified by the - // caller (via addBaseForce etc). - - // output should point to an array of 6 + num_links reals. - // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), - // num_links joint acceleration values. - - int num_links = getNumLinks(); - - const btScalar DAMPING_K1_LINEAR = m_linearDamping; - const btScalar DAMPING_K2_LINEAR = m_linearDamping; - - const btScalar DAMPING_K1_ANGULAR = m_angularDamping; - const btScalar DAMPING_K2_ANGULAR= m_angularDamping; - - btVector3 base_vel = getBaseVel(); - btVector3 base_omega = getBaseOmega(); - - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame - - scratch_r.resize(2*num_links + 6); - scratch_v.resize(8*num_links + 6); - scratch_m.resize(4*num_links + 4); - - btScalar * r_ptr = &scratch_r[0]; - btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results - btVector3 * v_ptr = &scratch_v[0]; - - // vhat_i (top = angular, bottom = linear part) - btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // zhat_i^A - btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // chat_i (note NOT defined for the base) - btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links; - btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links; - - // top left, top right and bottom left blocks of Ihat_i^A. - // bottom right block = transpose of top left block and is not stored. - // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently. - btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1]; - btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2]; - btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; - - // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3 * rot_from_parent = &matrix_buf[0]; - btMatrix3x3 * rot_from_world = &scratch_m[0]; - - // hhat_i, ahat_i - // hhat is NOT stored for the base (but ahat is) - btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; - btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; - btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; - btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; - - // Y_i, D_i - btScalar * Y = r_ptr; r_ptr += num_links; - btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; - - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; - - - // Start of the algorithm proper. - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - rot_from_parent[0] = btMatrix3x3(base_quat); - - vel_top_angular[0] = rot_from_parent[0] * base_omega; - vel_bottom_linear[0] = rot_from_parent[0] * base_vel; - - if (fixed_base) { - zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); - } else { - zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force - - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); - - zero_acc_bottom_linear[0] = - - (rot_from_parent[0] * base_torque); - - if (m_useGyroTerm) - zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] ); - - zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); - - } - - - - inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - - - inertia_top_right[0].setValue(base_mass, 0, 0, - 0, base_mass, 0, - 0, 0, base_mass); - inertia_bottom_left[0].setValue(base_inertia[0], 0, 0, - 0, base_inertia[1], 0, - 0, 0, base_inertia[2]); - - rot_from_world[0] = rot_from_parent[0]; - - for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this); - - - rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; - - // vhat_i = i_xhat_p(i) * vhat_p(i) - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - vel_top_angular[parent+1], vel_bottom_linear[parent+1], - vel_top_angular[i+1], vel_bottom_linear[i+1]); - - // we can now calculate chat_i - // remember vhat_i is really vhat_p(i) (but in current frame) at this point - coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector)) - + 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i); - if (links[i].is_revolute) { - coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i); - coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom); - } else { - coriolis_top_angular[i] = btVector3(0,0,0); - } - - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top; - vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom; - - // calculate zhat_i^A - zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force)); - zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; - - zero_acc_bottom_linear[i+1] = - - (rot_from_world[i+1] * links[i].applied_torque); - if (m_useGyroTerm) - { - zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] ); - } - - zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); - - // calculate Ihat_i^A - inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); - inertia_top_right[i+1].setValue(links[i].mass, 0, 0, - 0, links[i].mass, 0, - 0, 0, links[i].mass); - inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0, - 0, links[i].inertia[1], 0, - 0, 0, links[i].inertia[2]); - } - - - // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) { - - h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom; - h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom; - btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]); - D[i] = val; - Y[i] = links[i].joint_torque - - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]) - - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]); - - const int parent = links[i].parent; - - - // Ip += pXi * (Ii - hi hi' / Di) * iXp - const btScalar one_over_di = 1.0f / D[i]; - - - - - const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]); - const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]); - const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]); - - - btMatrix3x3 r_cross; - r_cross.setValue( - 0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1], - links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0], - -links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0); - - inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1]; - inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1]; - inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() * - (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1]; - - - // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di) - btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar Y_over_D = Y[i] * one_over_di; - in_top = zero_acc_top_angular[i+1] - + inertia_top_left[i+1] * coriolis_top_angular[i] - + inertia_top_right[i+1] * coriolis_bottom_linear[i] - + Y_over_D * h_top[i]; - in_bottom = zero_acc_bottom_linear[i+1] - + inertia_bottom_left[i+1] * coriolis_top_angular[i] - + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i] - + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - in_top, in_bottom, out_top, out_bottom); - zero_acc_top_angular[parent+1] += out_top; - zero_acc_bottom_linear[parent+1] += out_bottom; - } - - - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) - - if (fixed_base) - { - accel_top[0] = accel_bottom[0] = btVector3(0,0,0); - } - else - { - if (num_links > 0) - { - //Matrix Imatrix; - //Imatrix.block<3,3>(0,0) = inertia_top_left[0]; - //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0]; - //Imatrix.block<3,3>(0,3) = inertia_top_right[0]; - //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose(); - //cached_imatrix_lu.reset(new Eigen::LU >(Imatrix)); // TODO: Avoid memory allocation here? - - cached_inertia_top_left = inertia_top_left[0]; - cached_inertia_top_right = inertia_top_right[0]; - cached_inertia_lower_left = inertia_bottom_left[0]; - cached_inertia_lower_right= inertia_top_left[0].transpose(); - - } - btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); - btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); - float result[6]; - - solveImatrix(rhs_top, rhs_bot, result); -// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); - for (int i = 0; i < 3; ++i) { - accel_top[0][i] = -result[i]; - accel_bottom[0][i] = -result[i+3]; - } - - } - - // now do the loop over the links - for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - accel_top[parent+1], accel_bottom[parent+1], - accel_top[i+1], accel_bottom[i+1]); - joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top; - accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom; - } - - // transform base accelerations back to the world frame. - btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; - output[0] = omegadot_out[0]; - output[1] = omegadot_out[1]; - output[2] = omegadot_out[2]; - - btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; - // Final step: add the accelerations (times dt) to the velocities. - applyDeltaVee(output, dt); - - -} - - - -void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const -{ - int num_links = getNumLinks(); - ///solve I * x = rhs, so the result = invI * rhs - if (num_links == 0) - { - // in the case of 0 links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result[0] = rhs_bot[0] / base_inertia[0]; - result[1] = rhs_bot[1] / base_inertia[1]; - result[2] = rhs_bot[2] / base_inertia[2]; - result[3] = rhs_top[0] / base_mass; - result[4] = rhs_top[1] / base_mass; - result[5] = rhs_top[2] / base_mass; - } else - { - /// Special routine for calculating the inverse of a spatial inertia matrix - ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = cached_inertia_top_right.inverse()*-1.f; - btMatrix3x3 tmp = cached_inertia_lower_right * Binv; - btMatrix3x3 invIupper_right = (tmp * cached_inertia_top_left + cached_inertia_lower_left).inverse(); - tmp = invIupper_right * cached_inertia_lower_right; - btMatrix3x3 invI_upper_left = (tmp * Binv); - btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = cached_inertia_top_left * invI_upper_left; - tmp[0][0]-= 1.0; - tmp[1][1]-= 1.0; - tmp[2][2]-= 1.0; - btMatrix3x3 invI_lower_left = (Binv * tmp); - - //multiply result = invI * rhs - { - btVector3 vtop = invI_upper_left*rhs_top; - btVector3 tmp; - tmp = invIupper_right * rhs_bot; - vtop += tmp; - btVector3 vbot = invI_lower_left*rhs_top; - tmp = invI_lower_right * rhs_bot; - vbot += tmp; - result[0] = vtop[0]; - result[1] = vtop[1]; - result[2] = vtop[2]; - result[3] = vbot[0]; - result[4] = vbot[1]; - result[5] = vbot[2]; - } - - } -} - - -void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output, - btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const -{ - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame - int num_links = getNumLinks(); - scratch_r.resize(num_links); - scratch_v.resize(4*num_links + 4); - - btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0]; - btVector3 * v_ptr = &scratch_v[0]; - - // zhat_i^A (scratch space) - btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; - btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; - - // rot_from_parent (cached from calcAccelerations) - const btMatrix3x3 * rot_from_parent = &matrix_buf[0]; - - // hhat (cached), accel (scratch) - const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; - const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; - btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; - btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; - - // Y_i (scratch), D_i (cached) - btScalar * Y = r_ptr; r_ptr += num_links; - const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; - - btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size()); - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - btVector3 input_force(force[3],force[4],force[5]); - btVector3 input_torque(force[0],force[1],force[2]); - - // Fill in zero_acc - // -- set to force/torque on the base, zero otherwise - if (fixed_base) - { - zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); - } else - { - zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force); - zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque); - } - for (int i = 0; i < num_links; ++i) - { - zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0); - } - - // 'Downward' loop. - for (int i = num_links - 1; i >= 0; --i) - { - - Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); - Y[i] += force[6 + i]; // add joint torque - - const int parent = links[i].parent; - - // Zp += pXi * (Zi + hi*Yi/Di) - btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar Y_over_D = Y[i] / D[i]; - in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i]; - in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i]; - InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - in_top, in_bottom, out_top, out_bottom); - zero_acc_top_angular[parent+1] += out_top; - zero_acc_bottom_linear[parent+1] += out_bottom; - } - - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; - - // Second 'upward' loop - if (fixed_base) - { - accel_top[0] = accel_bottom[0] = btVector3(0,0,0); - } else - { - btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); - btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); - - float result[6]; - solveImatrix(rhs_top,rhs_bot, result); - // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); - - for (int i = 0; i < 3; ++i) { - accel_top[0][i] = -result[i]; - accel_bottom[0][i] = -result[i+3]; - } - - } - - // now do the loop over the links - for (int i = 0; i < num_links; ++i) { - const int parent = links[i].parent; - SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - accel_top[parent+1], accel_bottom[parent+1], - accel_top[i+1], accel_bottom[i+1]); - joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += joint_accel[i] * links[i].axis_top; - accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom; - } - - // transform base accelerations back to the world frame. - btVector3 omegadot_out; - omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; - 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() * accel_bottom[0]; - - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; -} - -void btMultiBody::stepPositions(btScalar dt) -{ - int num_links = getNumLinks(); - // step position by adding dt * velocity - btVector3 v = getBaseVel(); - base_pos += dt * v; - - // "exponential map" method for the rotation - btVector3 base_omega = getBaseOmega(); - const btScalar omega_norm = base_omega.norm(); - const btScalar omega_times_dt = omega_norm * dt; - const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156 - if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) - { - const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2 - const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega| - const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|) - base_quat = base_quat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term); - } else - { - base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt); - } - - // Make sure the quaternion represents a valid rotation. - // (Not strictly necessary, but helps prevent any round-off errors from building up.) - base_quat.normalize(); - - // Finally we can update joint_pos for each of the links - for (int i = 0; i < num_links; ++i) - { - float jointVel = getJointVel(i); - links[i].joint_pos += dt * jointVel; - links[i].updateCache(); - } -} - -void btMultiBody::fillContactJacobian(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const -{ - // temporary space - int num_links = getNumLinks(); - scratch_v.resize(2*num_links + 2); - scratch_m.resize(num_links + 1); - - btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local = v_ptr; v_ptr += num_links + 1; - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - scratch_r.resize(num_links); - btScalar * results = num_links > 0 ? &scratch_r[0] : 0; - - btMatrix3x3 * rot_from_world = &scratch_m[0]; - - const btVector3 p_minus_com_world = contact_point - base_pos; - - rot_from_world[0] = btMatrix3x3(base_quat); - - p_minus_com[0] = rot_from_world[0] * p_minus_com_world; - n_local[0] = rot_from_world[0] * normal; - - // omega coeffients first. - btVector3 omega_coeffs; - omega_coeffs = p_minus_com_world.cross(normal); - jac[0] = omega_coeffs[0]; - jac[1] = omega_coeffs[1]; - jac[2] = omega_coeffs[2]; - // then v coefficients - jac[3] = normal[0]; - jac[4] = normal[1]; - jac[5] = normal[2]; - - // Set remaining jac values to zero for now. - for (int i = 6; i < 6 + num_links; ++i) { - jac[i] = 0; - } - - // Qdot coefficients, if necessary. - if (num_links > 0 && link > -1) { - - // TODO: speed this up -- don't calculate for links we don't need. - // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, - // which is resulting in repeated work being done...) - - // calculate required normals & positions in the local frames. - for (int i = 0; i < num_links; ++i) { - - // transform to local frame - const int parent = links[i].parent; - const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this); - rot_from_world[i+1] = mtx * rot_from_world[parent+1]; - - n_local[i+1] = mtx * n_local[parent+1]; - p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector; - - // calculate the jacobian entry - if (links[i].is_revolute) { - results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom ); - } else { - results[i] = n_local[i+1].dot( links[i].axis_bottom ); - } - } - - // Now copy through to output. - while (link != -1) { - jac[6 + link] = results[link]; - link = links[link].parent; - } - } -} - -void btMultiBody::wakeUp() -{ - awake = true; -} - -void btMultiBody::goToSleep() -{ - awake = false; -} - -void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) -{ - int num_links = getNumLinks(); - extern bool gDisableDeactivation; - if (!can_sleep || gDisableDeactivation) - { - awake = true; - sleep_timer = 0; - return; - } - - // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) - btScalar motion = 0; - for (int i = 0; i < 6 + num_links; ++i) { - motion += m_real_buf[i] * m_real_buf[i]; - } - - if (motion < SLEEP_EPSILON) { - sleep_timer += timestep; - if (sleep_timer > SLEEP_TIMEOUT) { - goToSleep(); - } - } else { - sleep_timer = 0; - if (!awake) - wakeUp(); - } -} diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.h b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.h deleted file mode 100644 index 7177bebbf..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.h +++ /dev/null @@ -1,466 +0,0 @@ -/* - * PURPOSE: - * Class representing an articulated rigid body. Stores the body's - * current state, allows forces and torques to be set, handles - * timestepping and implements Featherstone's algorithm. - * - * COPYRIGHT: - * Copyright (C) Stephen Thompson, , 2011-2013 - * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) - - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the use of this software. - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it freely, - subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - - */ - - -#ifndef BT_MULTIBODY_H -#define BT_MULTIBODY_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btAlignedObjectArray.h" - - -#include "btMultiBodyLink.h" -class btMultiBodyLinkCollider; - -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 fixed_base_, // whether the base is fixed (true) or can move (false) - bool can_sleep_); - - ~btMultiBody(); - - void setupPrismatic(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, // in my frame; assumed diagonal - int parent, - const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame. - const btVector3 &joint_axis, // in my frame - const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0. - bool disableParentCollision=false - ); - - void setupRevolute(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &joint_axis, // in my frame - const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - const btMultibodyLink& getLink(int index) const - { - return links[index]; - } - - btMultibodyLink& getLink(int index) - { - return links[index]; - } - - - void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base - { - m_baseCollider = collider; - } - const btMultiBodyLinkCollider* getBaseCollider() const - { - return m_baseCollider; - } - btMultiBodyLinkCollider* getBaseCollider() - { - return m_baseCollider; - } - - // - // get parent - // input: link num from 0 to num_links-1 - // output: link num from 0 to num_links-1, OR -1 to mean the base. - // - int getParent(int link_num) const; - - - // - // get number of links, masses, moments of inertia - // - - int getNumLinks() const { return links.size(); } - btScalar getBaseMass() const { return base_mass; } - const btVector3 & getBaseInertia() const { return base_inertia; } - 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) { base_mass = mass; } - void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; } - - - // - // get/set pos/vel/rot/omega for the base link - // - - const btVector3 & getBasePos() const { return base_pos; } // in world frame - const btVector3 getBaseVel() const - { - return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); - } // in world frame - const btQuaternion & getWorldToBaseRot() const - { - return base_quat; - } // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame - - void setBasePos(const btVector3 &pos) - { - base_pos = pos; - } - void setBaseVel(const btVector3 &vel) - { - - m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; - } - void setWorldToBaseRot(const btQuaternion &rot) - { - base_quat = rot; - } - void setBaseOmega(const btVector3 &omega) - { - m_real_buf[0]=omega[0]; - m_real_buf[1]=omega[1]; - m_real_buf[2]=omega[2]; - } - - - // - // get/set pos/vel for child links (i = 0 to num_links-1) - // - - btScalar getJointPos(int i) const; - btScalar getJointVel(int i) const; - - void setJointPos(int i, btScalar q); - void setJointVel(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_real_buf[0]; - } -/* btScalar * getVelocityVector() - { - return &real_buf[0]; - } - */ - - // - // get the frames of reference (positions and orientations) of the child links - // (i = 0 to num_links-1) - // - - const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords - const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. - - - // - // transform vectors in local frame of link i to world frame (or vice versa) - // - btVector3 localPosToWorld(int i, const btVector3 &vec) const; - btVector3 localDirToWorld(int i, const btVector3 &vec) const; - btVector3 worldPosToLocal(int i, const btVector3 &vec) const; - btVector3 worldDirToLocal(int i, const btVector3 &vec) const; - - - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // - - btScalar getKineticEnergy() const; - btVector3 getAngularMomentum() const; - - - // - // set external forces and torques. Note all external forces/torques are given in the WORLD frame. - // - - void clearForcesAndTorques(); - void clearVelocities(); - - void addBaseForce(const btVector3 &f) - { - base_force += f; - } - void addBaseTorque(const btVector3 &t) { base_torque += t; } - void addLinkForce(int i, const btVector3 &f); - void addLinkTorque(int i, const btVector3 &t); - void addJointTorque(int i, btScalar Q); - - const btVector3 & getBaseForce() const { return base_force; } - const btVector3 & getBaseTorque() const { return base_torque; } - const btVector3 & getLinkForce(int i) const; - const btVector3 & getLinkTorque(int i) const; - btScalar getJointTorque(int i) const; - - - // - // dynamics routines. - // - - // timestep the velocities (given the external forces/torques set using addBaseForce etc). - // also sets up caches for calcAccelerationDeltas. - // - // Note: the caller must provide three vectors which are used as - // temporary scratch space. The idea here is to reduce dynamic - // memory allocation: the same scratch vectors can be re-used - // again and again for different Multibodies, instead of each - // btMultiBody allocating (and then deallocating) their own - // individual scratch buffers. This gives a considerable speed - // improvement, at least on Windows (where dynamic memory - // allocation appears to be fairly slow). - // - void stepVelocities(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m); - - // calcAccelerationDeltas - // 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) - // stepVelocities must have been called first. - void calcAccelerationDeltas(const btScalar *force, btScalar *output, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v) const; - - // apply a delta-vee directly. used in sequential impulses code. - void applyDeltaVee(const btScalar * delta_vee) - { - - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - m_real_buf[i] += delta_vee[i]; - } - - } - void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) - { - btScalar sum = 0; - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; - } - btScalar l = btSqrt(sum); - /* - static btScalar maxl = -1e30f; - if (l>maxl) - { - maxl=l; - // printf("maxl=%f\n",maxl); - } - */ - if (l>m_maxAppliedImpulse) - { -// printf("exceeds 100: l=%f\n",maxl); - multiplier *= m_maxAppliedImpulse/l; - } - - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; - m_real_buf[i] += delta_vee[i] * multiplier; - } - } - - // timestep the positions (given current velocities). - void stepPositions(btScalar dt); - - - // - // contacts - // - - // This routine fills out a contact constraint jacobian for this body. - // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. - // 'normal' & 'contact_point' are both given in world coordinates. - void fillContactJacobian(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const; - - - // - // sleeping - // - void setCanSleep(bool canSleep) - { - can_sleep = canSleep; - } - - bool isAwake() const { return awake; } - void wakeUp(); - void goToSleep(); - void checkMotionAndSleepIfRequired(btScalar timestep); - - bool hasFixedBase() const - { - return fixed_base; - } - - int getCompanionId() const - { - return m_companionId; - } - void setCompanionId(int id) - { - //printf("for %p setCompanionId(%d)\n",this, id); - m_companionId = id; - } - - void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links - { - links.resize(numLinks); - } - - btScalar getLinearDamping() const - { - return m_linearDamping; - } - void setLinearDamping( btScalar damp) - { - m_linearDamping = damp; - } - btScalar getAngularDamping() const - { - return m_angularDamping; - } - - bool getUseGyroTerm() const - { - return m_useGyroTerm; - } - void setUseGyroTerm(bool useGyro) - { - m_useGyroTerm = useGyro; - } - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } - - void setHasSelfCollision(bool hasSelfCollision) - { - m_hasSelfCollision = hasSelfCollision; - } - bool hasSelfCollision() const - { - return m_hasSelfCollision; - } - -private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented - - void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; - - void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; - - -private: - - btMultiBodyLinkCollider* m_baseCollider;//can be NULL - - btVector3 base_pos; // position of COM of base (world frame) - btQuaternion base_quat; // rotates world points into base frame - - btScalar base_mass; // mass of the base - btVector3 base_inertia; // inertia of the base (in local frame; diagonal) - - btVector3 base_force; // external force applied to base. World frame. - btVector3 base_torque; // external torque applied to base. World frame. - - btAlignedObjectArray links; // array of links, excluding the base. index from 0 to num_links-1. - btAlignedObjectArray m_colliders; - - // - // real_buf: - // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) - // 6+num_links num_links D - // - // vector_buf: - // offset size array - // 0 num_links h_top - // num_links num_links h_bottom - // - // matrix_buf: - // offset size array - // 0 num_links+1 rot_from_parent - // - - btAlignedObjectArray m_real_buf; - btAlignedObjectArray vector_buf; - btAlignedObjectArray matrix_buf; - - //std::auto_ptr > > cached_imatrix_lu; - - btMatrix3x3 cached_inertia_top_left; - btMatrix3x3 cached_inertia_top_right; - btMatrix3x3 cached_inertia_lower_left; - btMatrix3x3 cached_inertia_lower_right; - - bool fixed_base; - - // Sleep parameters. - bool awake; - bool can_sleep; - btScalar sleep_timer; - - int m_companionId; - btScalar m_linearDamping; - btScalar m_angularDamping; - bool m_useGyroTerm; - btScalar m_maxAppliedImpulse; - bool m_hasSelfCollision; -}; - -#endif diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp deleted file mode 100644 index 44e04c3a1..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ /dev/null @@ -1,527 +0,0 @@ -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - -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_num_rows(numRows), - m_isUnilateral(isUnilateral), - m_maxAppliedImpulse(100) -{ - m_jac_size_A = (6 + bodyA->getNumLinks()); - m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0)); - m_pos_offset = ((1 + m_jac_size_both)*m_num_rows); - m_data.resize((2 + m_jac_size_both) * m_num_rows); -} - -btMultiBodyConstraint::~btMultiBodyConstraint() -{ -} - - - -btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit) -{ - - - - constraintRow.m_multiBodyA = m_bodyA; - constraintRow.m_multiBodyB = m_bodyB; - - btMultiBody* multiBodyA = constraintRow.m_multiBodyA; - btMultiBody* multiBodyB = constraintRow.m_multiBodyB; - - if (multiBodyA) - { - - const int ndofA = multiBodyA->getNumLinks() + 6; - - constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (constraintRow.m_deltaVelAindex <0) - { - constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); - } - - constraintRow.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - for (int i=0;icalcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } - - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 6; - - constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (constraintRow.m_deltaVelBindex <0) - { - constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - constraintRow.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - - for (int i=0;icalcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); - } - { - - 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; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 6; - jacA = &data.m_jacobians[constraintRow.m_jacAindex]; - lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 6; - jacB = &data.m_jacobians[constraintRow.m_jacBindex]; - lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - constraintRow.m_jacDiagABInv = 1.f/(d); - } else - { - constraintRow.m_jacDiagABInv = 1.f; - } - - } - - - //compute rhs and remaining constraintRow fields - - - - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 6; - btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumLinks() + 6; - btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - - constraintRow.m_friction = 0.f; - - constraintRow.m_appliedImpulse = 0.f; - constraintRow.m_appliedPushImpulse = 0.f; - - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - btScalar erp = infoGlobal.m_erp2; - - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse) - { - //combine position and velocity into rhs - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - } - - - constraintRow.m_cfm = 0.f; - constraintRow.m_lowerLimit = lowerLimit; - constraintRow.m_upperLimit = upperLimit; - - } - return rel_vel; -} - - -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; -} - - -void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar position, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - - - btVector3 rel_pos1 = posAworld; - btVector3 rel_pos2 = posBworld; - - 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; - - const btVector3& pos1 = posAworld; - const btVector3& pos2 = posBworld; - - 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; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - if (multiBodyA) - { - const int ndofA = multiBodyA->getNumLinks() + 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); - } - - solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - - btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } else - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 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); - } - - solverConstraint.m_jacBindex = data.m_jacobians.size(); - - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); - } else - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - } - - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &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) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } - } - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &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) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); - } - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - - } - - - //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 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->getNumLinks() + 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; - - - restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; - solverConstraint.m_upperLimit = m_maxAppliedImpulse; - } - -} diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.h deleted file mode 100644 index 9fa317330..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ /dev/null @@ -1,166 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_H -#define BT_MULTIBODY_CONSTRAINT_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btMultiBody.h" - -class btMultiBody; -struct btSolverInfo; - -#include "btMultiBodySolverConstraint.h" - -struct btMultiBodyJacobianData -{ - btAlignedObjectArray m_jacobians; - btAlignedObjectArray m_deltaVelocitiesUnitImpulse; - btAlignedObjectArray m_deltaVelocities; - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; - btAlignedObjectArray* m_solverBodyPool; - int m_fixedBodyId; - -}; - - -class btMultiBodyConstraint -{ -protected: - - btMultiBody* m_bodyA; - btMultiBody* m_bodyB; - int m_linkA; - int m_linkB; - - int m_num_rows; - int m_jac_size_A; - int m_jac_size_both; - int m_pos_offset; - - bool m_isUnilateral; - - btScalar m_maxAppliedImpulse; - - - // 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); - - void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar position, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - - btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit); - -public: - - btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); - virtual ~btMultiBodyConstraint(); - - - - virtual int getIslandIdA() const =0; - virtual int getIslandIdB() const =0; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal)=0; - - int getNumRows() const - { - return m_num_rows; - } - - btMultiBody* getMultiBodyA() - { - return m_bodyA; - } - btMultiBody* getMultiBodyB() - { - return m_bodyB; - } - - // 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 - { - return m_data[m_pos_offset + row]; - } - - void setPosition(int row, btScalar pos) - { - m_data[m_pos_offset + 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) - { - return &m_data[m_num_rows + row * m_jac_size_both]; - } - const btScalar* jacobianA(int row) const - { - return &m_data[m_num_rows + (row * m_jac_size_both)]; - } - btScalar* jacobianB(int row) - { - return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; - } - const btScalar* jacobianB(int row) const - { - return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; - } - - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } - - -}; - -#endif //BT_MULTIBODY_CONSTRAINT_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp deleted file mode 100644 index 577f84622..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ /dev/null @@ -1,795 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyConstraintSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "btMultiBodyLinkCollider.h" - -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -#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 val = 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;jm_multiBodyFrictionContactConstraints.size();j++) - { - if (iteration < infoGlobal.m_numIterations) - { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; - 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; - resolveSingleConstraintRowGeneric(frictionConstraint); - } - } - } - return val; -} - -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_data.m_jacobians.resize(0); - m_data.m_deltaVelocitiesUnitImpulse.resize(0); - m_data.m_deltaVelocities.resize(0); - - for (int i=0;im_multiBody->setCompanionId(-1); - } - } - - btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); - - return val; -} - -void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) -{ - for (int i = 0; i < ndof; ++i) - m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; -} - -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) -{ - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; - btSolverBody* bodyA = 0; - btSolverBody* bodyB = 0; - int ndofA=0; - int ndofB=0; - - if (c.m_multiBodyA) - { - ndofA = c.m_multiBodyA->getNumLinks() + 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 - { - bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; - deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); - } - - if (c.m_multiBodyB) - { - ndofB = c.m_multiBodyB->getNumLinks() + 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 - { - bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; - 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; - const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } else - { - 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); - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } else - { - bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } - -} - - -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c) -{ - - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; - int ndofA=0; - int ndofB=0; - - if (c.m_multiBodyA) - { - ndofA = c.m_multiBodyA->getNumLinks() + 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]; - } - - if (c.m_multiBodyB) - { - ndofB = c.m_multiBodyB->getNumLinks() + 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]; - } - - - 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; - c.m_appliedImpulse = c.m_lowerLimit; - } - else if (sum > c.m_upperLimit) - { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; - c.m_appliedImpulse = c.m_upperLimit; - } - else - { - c.m_appliedImpulse = sum; - } - - if (c.m_multiBodyA) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - } - if (c.m_multiBodyB) - { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - } -} - - -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; - - 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 = 1.f; - - if (multiBodyA) - { - const int ndofA = multiBodyA->getNumLinks() + 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->fillContactJacobian(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->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - } else - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormal; - } - - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 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->fillContactJacobian(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->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - } else - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormal; - } - - { - - 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; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 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) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormal.dot(vec); - } - } - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 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) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormal.dot(vec); - } - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - - } - - - //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 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) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumLinks() + 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 += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - } - - solverConstraint.m_friction = cp.m_combinedFriction; - - - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } - - - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(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); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(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); - } - } - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } - -} - - - - -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_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; - - setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, 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; - - 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]; - - - ///avoid collision response between two static objects -// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) - // return; - - int rollingFriction=1; - - for (int j=0;jgetNumContacts();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_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; - - bool isFriction = false; - setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); - -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); - - /////setup the friction constraints -#define ENABLE_FRICTION -#ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = frictionIndex; -#if ROLLING_FRICTION - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) - { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - } -#endif //ROLLING_FRICTION - - ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame - ///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 - /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) - {/* - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) - { - cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); - 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); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - */ - { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - - 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_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) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - { - cp.m_lateralFrictionInitialized = true; - } - } - - } else - { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); - - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); - - //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - //todo: - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - } - - -#endif //ENABLE_FRICTION - - } - } -} - -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 - { - convertMultiBodyContact(manifold,infoGlobal); - } - } - - //also convert the multibody constraints, if any - - - for (int i=0;icreateConstraintRows(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) -{ - return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,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 start\n"); - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; - - -} diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h deleted file mode 100644 index 0f4cd69c0..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ /dev/null @@ -1,85 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H -#define BT_MULTIBODY_CONSTRAINT_SOLVER_H - -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "btMultiBodySolverConstraint.h" - - -class btMultiBody; - -#include "btMultiBodyConstraint.h" - - - -ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver -{ - -protected: - - btMultiBodyConstraintArray m_multiBodyNonContactConstraints; - - btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - - btMultiBodyJacobianData m_data; - - //temp storage for multi body constraints for a specific island/group called by 'solveGroup' - btMultiBodyConstraint** m_tmpMultiBodyConstraints; - int m_tmpNumMultiBodyConstraints; - - void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); - void resolveSingleConstraintRowGenericMultiBody(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); - - - 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); - - 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); - -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 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 - diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp deleted file mode 100644 index 0910f8f6a..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ /dev/null @@ -1,578 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiBodyDynamicsWorld.h" -#include "btMultiBodyConstraintSolver.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btQuickprof.h" -#include "btMultiBodyConstraint.h" - - - - -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) -{ - m_multiBodies.push_back(body); - -} - -void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) -{ - m_multiBodies.remove(body); -} - -void btMultiBodyDynamicsWorld::calculateSimulationIslands() -{ - BT_PROFILE("calculateSimulationIslands"); - - 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]; - - 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++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - if (constraint->isEnabled()) - { - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - 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++) - { - btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; - - if (((cur) && (!(cur)->isStaticOrKinematicObject())) && - ((prev) && (!(prev)->isStaticOrKinematicObject()))) - { - int tagPrev = prev->getIslandTag(); - int tagCur = cur->getIslandTag(); - getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); - } - if (cur && !cur->isStaticOrKinematicObject()) - prev = cur; - - } - } - } - - //merge islands linked by multibody constraints - { - for (int i=0;im_multiBodyConstraints.size();i++) - { - btMultiBodyConstraint* c = m_multiBodyConstraints[i]; - int tagA = c->getIslandIdA(); - int tagB = c->getIslandIdB(); - 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) -{ - BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); - - - - for ( int i=0;icheckMotionAndSleepIfRequired(timeStep); - if (!body->isAwake()) - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() == ACTIVE_TAG) - { - col->setActivationState( WANTS_DEACTIVATION); - col->setDeactivationTime(0.f); - } - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* col = body->getLink(b).m_collider; - if (col && col->getActivationState() == ACTIVE_TAG) - { - col->setActivationState( WANTS_DEACTIVATION); - col->setDeactivationTime(0.f); - } - } - } else - { - btMultiBodyLinkCollider* col = body->getBaseCollider(); - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); - - for (int b=0;bgetNumLinks();b++) - { - btMultiBodyLinkCollider* col = body->getLink(b).m_collider; - if (col && col->getActivationState() != DISABLE_DEACTIVATION) - col->setActivationState( ACTIVE_TAG ); - } - } - - } - } - - btDiscreteDynamicsWorld::updateActivationState(timeStep); -} - - -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(); - 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; - } -}; - - - -SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) -{ - int islandId; - - int islandTagA = lhs->getIslandIdA(); - int islandTagB = lhs->getIslandIdB(); - 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; - } -}; - -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; - - btAlignedObjectArray m_bodies; - btAlignedObjectArray m_manifolds; - btAlignedObjectArray m_constraints; - btAlignedObjectArray m_multiBodyConstraints; - - - 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) - { - btAssert(0); - (void)other; - return *this; - } - - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) - { - btAssert(solverInfo); - m_solverInfo = solverInfo; - - m_multiBodySortedConstraints = sortedMultiBodyConstraints; - m_numMultiBodyConstraints = numMultiBodyConstraints; - m_sortedConstraints = sortedConstraints; - m_numConstraints = numConstraints; - - m_debugDrawer = debugDrawer; - 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) - { - 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 - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - btMultiBodyConstraint** startMultiBodyConstraint = 0; - - int numCurConstraints = 0; - int numCurMultiBodyConstraints = 0; - - int i; - - //find the first constraint for this island - - for (i=0;im_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else - { - - for (i=0;im_solverInfo->m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - 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; - btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - - 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_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) -{ - //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::solveConstraints(btContactSolverInfo& solverInfo) -{ - - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; - - - 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_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); - - - { - BT_PROFILE("btMultiBody addForce and stepVelocities"); - for (int i=0;im_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++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - scratch_r.resize(bod->getNumLinks()+1); - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); - - bod->clearForcesAndTorques(); - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - - bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - } - } - } - - m_solverMultiBodyIslandCallback->processConstraints(); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer); - -} - -void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btDiscreteDynamicsWorld::integrateTransforms(timeStep); - - { - BT_PROFILE("btMultiBody stepPositions"); - //integrate and update the Featherstone hierarchies - btAlignedObjectArray world_to_local; - btAlignedObjectArray local_origin; - - for (int b=0;bgetBaseCollider() && 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 (!isSleeping) - { - int nLinks = bod->getNumLinks(); - - ///base + num links - world_to_local.resize(nLinks+1); - local_origin.resize(nLinks+1); - - bod->stepPositions(timeStep); - - - - world_to_local[0] = bod->getWorldToBaseRot(); - local_origin[0] = bod->getBasePos(); - - if (bod->getBaseCollider()) - { - btVector3 posr = local_origin[0]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float 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])); - - bod->getBaseCollider()->setWorldTransform(tr); - - } - - for (int k=0;kgetNumLinks();k++) - { - const int parent = bod->getParent(k); - world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; - local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); - } - - - for (int m=0;mgetNumLinks();m++) - { - btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; - if (col) - { - int link = col->m_link; - btAssert(link == m); - - int index = link+1; - - btVector3 posr = local_origin[index]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - - col->setWorldTransform(tr); - } - } - } else - { - bod->clearVelocities(); - } - } - } -} - - - -void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.push_back(constraint); -} - -void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) -{ - m_multiBodyConstraints.remove(constraint); -} diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h deleted file mode 100644 index ad57a346d..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ /dev/null @@ -1,56 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H -#define BT_MULTIBODY_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" - - -class btMultiBody; -class btMultiBodyConstraint; -class btMultiBodyConstraintSolver; -struct MultiBodyInplaceSolverIslandCallback; - -///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet -///This implementation is still preliminary/experimental. -class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld -{ -protected: - btAlignedObjectArray m_multiBodies; - btAlignedObjectArray m_multiBodyConstraints; - btAlignedObjectArray m_sortedMultiBodyConstraints; - btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; - MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; - - virtual void calculateSimulationIslands(); - virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); - virtual void integrateTransforms(btScalar timeStep); -public: - - btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btMultiBodyDynamicsWorld (); - - virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); - - virtual void removeMultiBody(btMultiBody* body); - - virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); - - virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); -}; -#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp deleted file mode 100644 index ea309e885..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyJointLimitConstraint.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - -btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) - :btMultiBodyConstraint(body,body,link,link,2,true), - m_lowerBound(lower), - m_upperBound(upper) -{ - // the data.m_jacobians never change, so may as well - // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - - // row 0: the lower bound - jacobianA(0)[6 + link] = 1; - - // row 1: the upper bound - jacobianB(1)[6 + link] = -1; -} -btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() -{ -} - -int btMultiBodyJointLimitConstraint::getIslandIdA() const -{ - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - return -1; -} - -int btMultiBodyJointLimitConstraint::getIslandIdB() const -{ - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - return -1; -} - - -void btMultiBodyJointLimitConstraint::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. - - // row 0: the lower bound - setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); - - // row 1: the upper bound - setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - - for (int row=0;row infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - 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_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = penetrationImpulse; - } - } - } - -} - - - - diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h deleted file mode 100644 index 0c7fc1708..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ /dev/null @@ -1,44 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H -#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - -#include "btMultiBodyConstraint.h" -struct btSolverInfo; - -class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint -{ -protected: - - btScalar m_lowerBound; - btScalar m_upperBound; -public: - - btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); - virtual ~btMultiBodyJointLimitConstraint(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - -}; - -#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp deleted file mode 100644 index ab5a43023..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyJointMotor.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - -btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) - :btMultiBodyConstraint(body,body,link,link,1,true), - m_desiredVelocity(desiredVelocity) -{ - m_maxAppliedImpulse = maxMotorImpulse; - // the data.m_jacobians never change, so may as well - // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - - // row 0: the lower bound - jacobianA(0)[6 + link] = 1; -} -btMultiBodyJointMotor::~btMultiBodyJointMotor() -{ -} - -int btMultiBodyJointMotor::getIslandIdA() const -{ - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - return -1; -} - -int btMultiBodyJointMotor::getIslandIdB() const -{ - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - return -1; -} - - -void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. - - - - for (int row=0;row=0 || (multiBody && !multiBody->hasFixedBase())) - { - m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); - } - // else - //{ - // m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); - //} - - m_internalType = CO_FEATHERSTONE_LINK; - } - static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) - { - 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) - return (btMultiBodyLinkCollider*)colObj; - return 0; - } - - virtual bool checkCollideWithOverride(const btCollisionObject* co) const - { - const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); - if (!other) - return true; - if (other->m_multiBody != this->m_multiBody) - return true; - if (!m_multiBody->hasSelfCollision()) - return false; - - //check if 'link' has collision disabled - if (m_link>=0) - { - const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link) - return false; - } - - if (other->m_link>=0) - { - const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link) - return false; - } - return true; - } -}; - -#endif //BT_FEATHERSTONE_LINK_COLLIDER_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp deleted file mode 100644 index f66900491..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#include "btMultiBodyPoint2Point.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - -btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,3,false), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) -{ -} - -btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) -{ -} - - -btMultiBodyPoint2Point::~btMultiBodyPoint2Point() -{ -} - - -int btMultiBodyPoint2Point::getIslandIdA() const -{ - if (m_rigidBodyA) - return m_rigidBodyA->getIslandTag(); - - if (m_bodyA) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyPoint2Point::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - } - return -1; -} - - - -void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - -// int i=1; - for (int i=0;i<3;i++) - { - - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - - constraintRow.m_solverBodyIdA = data.m_fixedBodyId; - constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - - - btVector3 contactNormalOnB(0,0,0); - contactNormalOnB[i] = -1; - - btScalar penetration = 0; - - // Convert local points back to world - btVector3 pivotAworld = m_pivotInA; - if (m_rigidBodyA) - { - - constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; - } else - { - if (m_bodyA) - pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - } - btVector3 pivotBworld = m_pivotInB; - if (m_rigidBodyB) - { - constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; - } else - { - if (m_bodyB) - pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - - } - btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); - btScalar relaxation = 1.f; - fillMultiBodyConstraintMixed(constraintRow, data, - contactNormalOnB, - pivotAworld, pivotBworld, - position, - infoGlobal, - relaxation, - false); - constraintRow.m_lowerLimit = -m_maxAppliedImpulse; - constraintRow.m_upperLimit = m_maxAppliedImpulse; - - } -} diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h deleted file mode 100644 index 26ca12b40..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was written by Erwin Coumans - -#ifndef BT_MULTIBODY_POINT2POINT_H -#define BT_MULTIBODY_POINT2POINT_H - -#include "btMultiBodyConstraint.h" - -class btMultiBodyPoint2Point : public btMultiBodyConstraint -{ -protected: - - btRigidBody* m_rigidBodyA; - btRigidBody* m_rigidBodyB; - btVector3 m_pivotInA; - btVector3 m_pivotInB; - - -public: - - 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(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - const btVector3& getPivotInB() const - { - return m_pivotInB; - } - - void setPivotInB(const btVector3& pivotInB) - { - m_pivotInB = pivotInB; - } - - -}; - -#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h deleted file mode 100644 index cf06dfb9e..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H -#define BT_MULTIBODY_SOLVER_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedObjectArray.h" - -class btMultiBody; -#include "BulletDynamics/ConstraintSolver/btSolverBody.h" -#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 -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - - int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 - btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal1; - int m_jacAindex; - - int m_deltaVelBindex; - btVector3 m_relpos2CrossNormal; - btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always - int m_jacBindex; - - 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_solverBodyIdA; - btMultiBody* m_multiBodyA; - int m_linkA; - - int m_solverBodyIdB; - btMultiBody* m_multiBodyB; - int m_linkB; - - enum btSolverConstraintType - { - BT_SOLVER_CONTACT_1D = 0, - BT_SOLVER_FRICTION_1D - }; -}; - -typedef btAlignedObjectArray btMultiBodyConstraintArray; - -#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp deleted file mode 100644 index 3bf7b5c13..000000000 --- a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ /dev/null @@ -1,2079 +0,0 @@ -/************************************************************************* -* * -* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * -* All rights reserved. Email: russ@q12.org Web: www.q12.org * -* * -* This library is free software; you can redistribute it and/or * -* modify it under the terms of EITHER: * -* (1) The GNU Lesser General Public License as published by the Free * -* Software Foundation; either version 2.1 of the License, or (at * -* your option) any later version. The text of the GNU Lesser * -* General Public License is included with this library in the * -* file LICENSE.TXT. * -* (2) The BSD-style license that is included with this library in * -* the file LICENSE-BSD.TXT. * -* * -* 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 files * -* LICENSE.TXT and LICENSE-BSD.TXT for more details. * -* * -*************************************************************************/ - -/* - - -THE ALGORITHM -------------- - -solve A*x = b+w, with x and w subject to certain LCP conditions. -each x(i),w(i) must lie on one of the three line segments in the following -diagram. each line segment corresponds to one index set : - - w(i) - /|\ | : - | | : - | |i in N : - w>0 | |state[i]=0 : - | | : - | | : i in C - w=0 + +-----------------------+ - | : | - | : | - w<0 | : |i in N - | : |state[i]=1 - | : | - | : | - +-------|-----------|-----------|----------> x(i) - lo 0 hi - -the Dantzig algorithm proceeds as follows: - for i=1:n - * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or - negative towards the line. as this is done, the other (x(j),w(j)) - for j= 0. this makes the algorithm a bit -simpler, because the starting point for x(i),w(i) is always on the dotted -line x=0 and x will only ever increase in one direction, so it can only hit -two out of the three line segments. - - -NOTES ------ - -this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m". -the implementation is split into an LCP problem object (btLCP) and an LCP -driver function. most optimization occurs in the btLCP object. - -a naive implementation of the algorithm requires either a lot of data motion -or a lot of permutation-array lookup, because we are constantly re-ordering -rows and columns. to avoid this and make a more optimized algorithm, a -non-trivial data structure is used to represent the matrix A (this is -implemented in the fast version of the btLCP object). - -during execution of this algorithm, some indexes in A are clamped (set C), -some are non-clamped (set N), and some are "don't care" (where x=0). -A,x,b,w (and other problem vectors) are permuted such that the clamped -indexes are first, the unclamped indexes are next, and the don't-care -indexes are last. this permutation is recorded in the array `p'. -initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped, -the corresponding elements of p are swapped. - -because the C and N elements are grouped together in the rows of A, we can do -lots of work with a fast dot product function. if A,x,etc were not permuted -and we only had a permutation array, then those dot products would be much -slower as we would have a permutation array lookup in some inner loops. - -A is accessed through an array of row pointers, so that element (i,j) of the -permuted matrix is A[i][j]. this makes row swapping fast. for column swapping -we still have to actually move the data. - -during execution of this algorithm we maintain an L*D*L' factorization of -the clamped submatrix of A (call it `AC') which is the top left nC*nC -submatrix of A. there are two ways we could arrange the rows/columns in AC. - -(1) AC is always permuted such that L*D*L' = AC. this causes a problem -when a row/column is removed from C, because then all the rows/columns of A -between the deleted index and the end of C need to be rotated downward. -this results in a lot of data motion and slows things down. -(2) L*D*L' is actually a factorization of a *permutation* of AC (which is -itself a permutation of the underlying A). this is what we do - the -permutation is recorded in the vector C. call this permutation A[C,C]. -when a row/column is removed from C, all we have to do is swap two -rows/columns and manipulate C. - -*/ - - -#include "btDantzigLCP.h" - -#include //memcpy - -bool s_error = false; - -//*************************************************************************** -// code generation parameters - - -#define btLCP_FAST // use fast btLCP object - -// option 1 : matrix row pointers (less data copying) -#define BTROWPTRS -#define BTATYPE btScalar ** -#define BTAROW(i) (m_A[i]) - -// option 2 : no matrix row pointers (slightly faster inner loops) -//#define NOROWPTRS -//#define BTATYPE btScalar * -//#define BTAROW(i) (m_A+(i)*m_nskip) - -#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. - * B is an n*1 matrix that contains the right hand sides. - * B is stored by columns and its leading dimension is also lskip. - * B is overwritten with X. - * this processes blocks of 2*2. - * 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 */ - } -} - -/* solve L*X=B, with B containing 2 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. - * B is an n*2 matrix that contains the right hand sides. - * B is stored by columns and its leading dimension is also lskip. - * B is overwritten with X. - * this processes blocks of 2*2. - * 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 */ - } -} - - -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! */ - } -} - -/* 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. - * B is an n*1 matrix that contains the right hand sides. - * B is stored by columns and its leading dimension is also lskip. - * B is overwritten with X. - * this processes blocks of 4*4. - * 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; - } -} - -/* solve L^T * x=b, with b containing 1 right hand side. - * L is an n*n lower triangular matrix with ones on the diagonal. - * L is stored by rows and its leading dimension is lskip. - * b is an n*1 matrix that contains the right hand side. - * b is overwritten with x. - * 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,lskip3,i,j; - /* 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) -{ - btAssert (a && d && n >= 0); - for (int i=0; i 0 && nskip >= n); - btSolveL1 (L,b,n,nskip); - btVectorScale (b,d,n); - btSolveL1T (L,b,n,nskip); -} - - - -//*************************************************************************** - -// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of -// A is nskip. this only references and swaps the lower triangle. -// if `do_fast_row_swaps' is nonzero and row pointers are being used, then -// 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) -{ - 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; - - 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. -// -// two index sets C and N are kept. each set holds a subset of -// the variable indexes 0..n-1. an index can only be in one set. -// initially both sets are empty. -// -// the index set C is special: solutions to A(C,C)\A(C,i) can be generated. - -//*************************************************************************** -// fast implementation of btLCP. see the above definition of btLCP for -// interface comments. -// -// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is -// permuted as the other vectors/matrices are permuted. -// -// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have -// contiguous indexes. the don't-care indexes follow N. -// -// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are -// added or removed from the set C the factorization is updated. -// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A. -// the leading dimension of the matrix L is always `nskip'. -// -// at the start there may be other indexes that are unbounded but are not -// included in `nub'. btLCP will permute the matrix so that absolutely all -// unbounded vectors are at the start. thus there may be some initial -// permutation. -// -// the algorithms here assume certain patterns, particularly with respect to -// index transfer. - -#ifdef btLCP_FAST - -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 - 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); - 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); - 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); - 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), -#else - 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) -{ - { - 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; - const int nub = m_nub; - if (nub < n) { - for (int k=0; k<100; k++) { - int i1,i2; - do { - i1 = dRandInt(n-nub)+nub; - i2 = dRandInt(n-nub)+nub; - } - while (i1 > i2); - //printf ("--> %d %d\n",i1,i2); - btSwapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,n,i1,i2,m_nskip,0); - } - } - */ - - // 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++; - } - } - } - - // 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 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 - /* - { - const int n = m_n; - const int nub = m_nub; - for (int k=0; k 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) { - { - 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 && 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& scratch) -{ - btAssert (L && d && a && n > 0 && nskip >= n); - - if (n < 2) return; - scratch.resize(2*nskip); - btScalar *W1 = &scratch[0]; - - btScalar *W2 = W1 + nskip; - - W1[0] = btScalar(0.0); - W2[0] = btScalar(0.0); - for (int j=1; j j) ? _BTGETA(i,j) : _BTGETA(j,i)) - -inline size_t btEstimateLDLTAddTLTmpbufSize(int nskip) -{ - 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) -{ - 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 && p[i] < n1); - #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) -{ - { - 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) { - { - 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) { - int *C = m_C; - btScalar *tmp = m_tmp; - const int nC = m_nC; - for (int j=0; j0 && 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 - (2) x = hi, w <= 0 - (3) lo < x < hi, w = 0 -A is a matrix of dimension n*n, everything else is a vector of size n*1. -lo and hi can be +/- dInfinity as needed. the first `nub' variables are -unbounded, i.e. hi and lo are assumed to be +/- dInfinity. - -we restrict lo(i) <= 0 and hi(i) >= 0. - -the original data (A,b) may be modified by this function. - -if the `findex' (friction index) parameter is nonzero, it points to an array -of index values. in this case constraints that have findex[i] >= 0 are -special. all non-special constraints are solved for, then the lo and hi values -for the special constraints are set: - hi[i] = abs( hi[i] * x[findex[i]] ) - lo[i] = -hi[i] -and the solution continues. this mechanism allows a friction approximation -to be implemented. the first `nub' variables are assumed to have findex < 0. - -*/ - - -#ifndef _BT_LCP_H_ -#define _BT_LCP_H_ - -#include -#include -#include - - -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" - -struct btDantzigScratchMemory -{ - btAlignedObjectArray m_scratch; - btAlignedObjectArray L; - btAlignedObjectArray d; - btAlignedObjectArray delta_w; - btAlignedObjectArray delta_x; - btAlignedObjectArray Dell; - btAlignedObjectArray ell; - 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); - - - -#endif //_BT_LCP_H_ diff --git a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigSolver.h deleted file mode 100644 index 2a2f2d3d3..000000000 --- a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigSolver.h +++ /dev/null @@ -1,112 +0,0 @@ -/* -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. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_DANTZIG_SOLVER_H -#define BT_DANTZIG_SOLVER_H - -#include "btMLCPSolverInterface.h" -#include "btDantzigLCP.h" - - -class btDantzigSolver : public btMLCPSolverInterface -{ -protected: - - btScalar m_acceptableUpperLimitSolution; - - btAlignedObjectArray m_tempBuffer; - - btAlignedObjectArray m_A; - btAlignedObjectArray m_b; - btAlignedObjectArray m_x; - btAlignedObjectArray m_lo; - btAlignedObjectArray m_hi; - btAlignedObjectArray m_dependencies; - btDantzigScratchMemory m_scratchMemory; -public: - - btDantzigSolver() - :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) - { - bool result = true; - int n = b.rows(); - if (n) - { - int nub = 0; - btAlignedObjectArray ww; - ww.resize(n); - - - const btScalar* Aptr = A.getBufferPointer(); - m_A.resize(n*n); - for (int i=0;i= m_acceptableUpperLimitSolution) - { - return false; - } - - if (x[i] <= -m_acceptableUpperLimitSolution) - { - return false; - } - } - - 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 ); - 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 ); - } - 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 constraintRowIndex; -}; - - - -void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) -{ - int numContactRows = interleaveContactAndFriction ? 3 : 1; - - int numConstraintRows = m_allConstraintArray.size(); - int n = numConstraintRows; - { - BT_PROFILE("init b (rhs)"); - m_b.resize(numConstraintRows); - m_bSplit.resize(numConstraintRows); - //m_b.setZero(); - for (int i=0;i=0) - { - m_lo[i] = -BT_INFINITY; - m_hi[i] = BT_INFINITY; - } else - { - m_lo[i] = m_allConstraintArray[i].m_lowerLimit; - m_hi[i] = m_allConstraintArray[i].m_upperLimit; - } - } - } - - // - int m=m_allConstraintArray.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_allConstraintArray.size()); - } - - static btMatrixXu J3; - { - BT_PROFILE("J3.resize"); - J3.resize(2*m,8); - } - static btMatrixXu JinvM3; - { - BT_PROFILE("JinvM3.resize/setZero"); - - JinvM3.resize(2*m,8); - JinvM3.setZero(); - J3.setZero(); - } - int cur=0; - int rowOffset = 0; - static btAlignedObjectArray ofs; - { - BT_PROFILE("ofs resize"); - ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintArray.size()); - } - { - BT_PROFILE("Compute J and JinvM"); - int c=0; - - int numRows = 0; - - for (int i=0;igetInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); - - for (int r=0;r<3;r++) - { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintArray[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;rowgetInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); - - for (int r=0;r<3;r++) - { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintArray[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=0) - { - int j0 = jointNodeArray[startJointNodeA].jointIndex; - int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; - if (j0=0) - { - int j1 = jointNodeArray[startJointNodeB].jointIndex; - int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; - - if (j1m_tmpSolverBodyPool.size(); - int numConstraintRows = m_allConstraintArray.size(); - - m_b.resize(numConstraintRows); - if (infoGlobal.m_splitImpulse) - m_bSplit.resize(numConstraintRows); - - for (int i=0;igetInvInertiaTensorWorld()[r][c] : 0); - } - - static btMatrixXu J; - J.resize(numConstraintRows,6*numBodies); - J.setZero(); - - m_lo.resize(numConstraintRows); - m_hi.resize(numConstraintRows); - - for (int i=0;i m_limitDependencies; - btConstraintArray m_allConstraintArray; - btMLCPSolverInterface* m_solver; - int m_fallback; - - 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); - - //return true is it solves the problem successfully - virtual bool solveMLCP(const btContactSolverInfo& infoGlobal); - -public: - - btMLCPSolver( btMLCPSolverInterface* solver); - virtual ~btMLCPSolver(); - - void setMLCPSolver(btMLCPSolverInterface* solver) - { - m_solver = solver; - } - - int getNumFallbacks() const - { - return m_fallback; - } - void setNumFallbacks(int num) - { - m_fallback = num; - } - - virtual btConstraintSolverType getSolverType() const - { - return BT_MLCP_SOLVER; - } - -}; - - -#endif //BT_MLCP_SOLVER_H diff --git a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h deleted file mode 100644 index 25bb3f6d3..000000000 --- a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h +++ /dev/null @@ -1,33 +0,0 @@ -/* -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. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_MLCP_SOLVER_INTERFACE_H -#define BT_MLCP_SOLVER_INTERFACE_H - -#include "LinearMath/btMatrixX.h" - -class btMLCPSolverInterface -{ -public: - virtual ~btMLCPSolverInterface() - { - } - - //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; -}; - -#endif //BT_MLCP_SOLVER_INTERFACE_H diff --git a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btPATHSolver.h b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btPATHSolver.h deleted file mode 100644 index 9ec31a6d4..000000000 --- a/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btPATHSolver.h +++ /dev/null @@ -1,151 +0,0 @@ -/* -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. -*/ -///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" { -#include "PATH/SimpleLCP.h" -#include "PATH/License.h" -#include "PATH/Error_Interface.h" -}; - void __stdcall MyError(Void *data, Char *msg) -{ - printf("Path Error: %s\n",msg); -} - void __stdcall MyWarning(Void *data, Char *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"); - e.error_data = 0; - e.warning = MyWarning; - e.error = MyError; - 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) - { - MCP_Termination status; - - - int numVariables = b.rows(); - if (0==numVariables) - return true; - - /* - 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 - - m_ij - a vector of size m_nnz containing the data for M - - q - a vector of size variables - - lb - a vector of size variables containing the lower bounds on x - - ub - a vector of size variables containing the upper bounds on x - */ - btAlignedObjectArray values; - btAlignedObjectArray rowIndices; - btAlignedObjectArray colIndices; - - for (int i=0;i zResult; - zResult.resize(numVariables); - btAlignedObjectArray rhs; - btAlignedObjectArray upperBounds; - btAlignedObjectArray lowerBounds; - for (int i=0;i& limitDependency, int numIterations, bool useSparsity = true) - { - //A is a m-n matrix, m rows, n columns - btAssert(A.rows() == b.rows()); - - int i, j, numRows = A.rows(); - - float delta; - - for (int k = 0; k =0) - { - s = x[limitDependency[i]]; - if (s<0) - s=1; - } - - if (x[i]hi[i]*s) - x[i]=hi[i]*s; - } - } - return true; - } - -}; - -#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H diff --git a/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.cpp deleted file mode 100644 index 77b475b96..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ /dev/null @@ -1,771 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ - -#include "LinearMath/btVector3.h" -#include "btRaycastVehicle.h" - -#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" -#include "LinearMath/btQuaternion.h" -#include "BulletDynamics/Dynamics/btDynamicsWorld.h" -#include "btVehicleRaycaster.h" -#include "btWheelInfo.h" -#include "LinearMath/btMinMax.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - -#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.))); - return s_fixed; -} - -btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) -:m_vehicleRaycaster(raycaster), -m_pitchControl(btScalar(0.)) -{ - m_chassisBody = chassis; - m_indexRightAxis = 0; - m_indexUpAxis = 2; - m_indexForwardAxis = 1; - defaultInit(tuning); -} - - -void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) -{ - (void)tuning; - m_currentVehicleSpeedKmHour = btScalar(0.); - m_steeringValue = btScalar(0.); - -} - - - -btRaycastVehicle::~btRaycastVehicle() -{ -} - - -// -// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed -// -btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel) -{ - - btWheelInfoConstructionInfo ci; - - ci.m_chassisConnectionCS = connectionPointCS; - ci.m_wheelDirectionCS = wheelDirectionCS0; - ci.m_wheelAxleCS = wheelAxleCS; - ci.m_suspensionRestLength = suspensionRestLength; - ci.m_wheelRadius = wheelRadius; - ci.m_suspensionStiffness = tuning.m_suspensionStiffness; - ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; - ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; - ci.m_frictionSlip = tuning.m_frictionSlip; - ci.m_bIsFrontWheel = isFrontWheel; - ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; - 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); - return wheel; -} - - - - -const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const -{ - btAssert(wheelIndex < getNumWheels()); - const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; - return wheel.m_worldTransform; - -} - -void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) -{ - - btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; - updateWheelTransformsWS(wheel,interpolatedTransform); - btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; - const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; - btVector3 fwd = up.cross(right); - fwd = fwd.normalize(); -// up = right.cross(fwd); -// up.normalize(); - - //rotate around steering over de wheelAxleWS - btScalar steering = wheel.m_steering; - - btQuaternion steeringOrn(up,steering);//wheel.m_steering); - btMatrix3x3 steeringMat(steeringOrn); - - btQuaternion rotatingOrn(right,-wheel.m_rotation); - btMatrix3x3 rotatingMat(rotatingOrn); - - btMatrix3x3 basis2( - right[0],fwd[0],up[0], - right[1],fwd[1],up[1], - right[2],fwd[2],up[2] - ); - - wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2); - wheel.m_worldTransform.setOrigin( - wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength - ); -} - -void btRaycastVehicle::resetSuspension() -{ - - int i; - for (i=0;igetMotionState())) - { - getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); - } - - wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); - wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; - wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; -} - -btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) -{ - updateWheelTransformsWS( wheel,false); - - - btScalar depth = -1; - - btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; - - btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); - const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; - wheel.m_raycastInfo.m_contactPointWS = source + rayvector; - const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; - - btScalar param = btScalar(0.); - - btVehicleRaycaster::btVehicleRaycasterResult rayResults; - - btAssert(m_vehicleRaycaster); - - void* object = m_vehicleRaycaster->castRay(source,target,rayResults); - - wheel.m_raycastInfo.m_groundObject = 0; - - if (object) - { - param = rayResults.m_distFraction; - depth = raylen * rayResults.m_distFraction; - wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; - wheel.m_raycastInfo.m_isInContact = true; - - wheel.m_raycastInfo.m_groundObject = &getFixedBody();///@todo for driving on dynamic/movable objects!; - //wheel.m_raycastInfo.m_groundObject = object; - - - btScalar hitDistance = param*raylen; - wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; - //clamp on max suspension travel - - btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); - btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); - if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) - { - wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; - } - if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) - { - wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; - } - - wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; - - btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); - - btVector3 chassis_velocity_at_contactPoint; - btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); - - chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); - - btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - - if ( denominator >= btScalar(-0.1)) - { - wheel.m_suspensionRelativeVelocity = btScalar(0.0); - wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); - } - else - { - btScalar inv = btScalar(-1.) / denominator; - wheel.m_suspensionRelativeVelocity = projVel * inv; - wheel.m_clippedInvContactDotSuspension = inv; - } - - } else - { - //put wheel info as in rest position - wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); - wheel.m_suspensionRelativeVelocity = btScalar(0.0); - wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; - wheel.m_clippedInvContactDotSuspension = btScalar(1.0); - } - - return depth; -} - - -const btTransform& btRaycastVehicle::getChassisWorldTransform() const -{ - /*if (getRigidBody()->getMotionState()) - { - btTransform chassisWorldTrans; - getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); - return chassisWorldTrans; - } - */ - - - return getRigidBody()->getCenterOfMassTransform(); -} - - -void btRaycastVehicle::updateVehicle( btScalar step ) -{ - { - for (int i=0;igetLinearVelocity().length(); - - const btTransform& chassisTrans = getChassisWorldTransform(); - - btVector3 forwardW ( - chassisTrans.getBasis()[0][m_indexForwardAxis], - chassisTrans.getBasis()[1][m_indexForwardAxis], - chassisTrans.getBasis()[2][m_indexForwardAxis]); - - if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) - { - m_currentVehicleSpeedKmHour *= btScalar(-1.); - } - - // - // simulate suspension - // - - int i=0; - for (i=0;i 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); - - - for (i=0;igetCenterOfMassPosition(); - btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); - - if (wheel.m_raycastInfo.m_isInContact) - { - const btTransform& chassisWorldTransform = getChassisWorldTransform(); - - btVector3 fwd ( - chassisWorldTransform.getBasis()[0][m_indexForwardAxis], - chassisWorldTransform.getBasis()[1][m_indexForwardAxis], - chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); - - btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); - fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; - - btScalar proj2 = fwd.dot(vel); - - wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius); - wheel.m_rotation += wheel.m_deltaRotation; - - } else - { - wheel.m_rotation += wheel.m_deltaRotation; - } - - wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact - - } - - - -} - - -void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) -{ - btAssert(wheel>=0 && wheel < getNumWheels()); - - btWheelInfo& wheelInfo = getWheelInfo(wheel); - wheelInfo.m_steering = steering; -} - - - -btScalar btRaycastVehicle::getSteeringValue(int wheel) const -{ - return getWheelInfo(wheel).m_steering; -} - - -void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) -{ - btAssert(wheel>=0 && wheel < getNumWheels()); - btWheelInfo& wheelInfo = getWheelInfo(wheel); - wheelInfo.m_engineForce = force; -} - - -const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const -{ - btAssert((index >= 0) && (index < getNumWheels())); - - return m_wheelInfo[index]; -} - -btWheelInfo& btRaycastVehicle::getWheelInfo(int index) -{ - btAssert((index >= 0) && (index < getNumWheels())); - - return m_wheelInfo[index]; -} - -void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) -{ - btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); - getWheelInfo(wheelIndex).m_brake = brake; -} - - -void btRaycastVehicle::updateSuspension(btScalar deltaTime) -{ - (void)deltaTime; - - btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); - - for (int w_it=0; w_itcomputeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar relaxation = 1.f; - m_jacDiagABInv = relaxation/(denom0+denom1); - } - - - -}; - -btScalar calcRollingFriction(btWheelContactPoint& contactPoint); -btScalar calcRollingFriction(btWheelContactPoint& contactPoint) -{ - - btScalar j1=0.f; - - const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; - - btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); - btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); - - btScalar maxImpulse = contactPoint.m_maxImpulse; - - btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); - - // calculate j that moves us to zero relative velocity - j1 = -vrel * contactPoint.m_jacDiagABInv; - btSetMin(j1, maxImpulse); - btSetMax(j1, -maxImpulse); - - return j1; -} - - - - -btScalar sideFrictionStiffness2 = btScalar(1.0); -void btRaycastVehicle::updateFriction(btScalar timeStep) -{ - - //calculate the impulse, so that the wheels don't move sidewards - int numWheel = getNumWheels(); - if (!numWheel) - return; - - m_forwardWS.resize(numWheel); - m_axle.resize(numWheel); - m_forwardImpulse.resize(numWheel); - m_sideImpulse.resize(numWheel); - - int numWheelsOnGround = 0; - - - //collapse all those loops into one! - for (int i=0;i maximpSquared) - { - sliding = true; - - btScalar factor = maximp / btSqrt(impulseSquared); - - m_wheelInfo[wheel].m_skidInfo *= factor; - } - } - - } - } - - - - - if (sliding) - { - for (int wheel = 0;wheel < getNumWheels(); wheel++) - { - if (m_sideImpulse[wheel] != btScalar(0.)) - { - if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) - { - m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - } - } - } - } - - // apply the impulses - { - for (int wheel = 0;wheelgetCenterOfMassPosition(); - - if (m_forwardImpulse[wheel] != btScalar(0.)) - { - m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); - } - if (m_sideImpulse[wheel] != btScalar(0.)) - { - class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; - - btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - - groundObject->getCenterOfMassPosition(); - - - btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - -#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; -#endif - m_chassisBody->applyImpulse(sideImp,rel_pos); - - //apply friction impulse on the ground - groundObject->applyImpulse(-sideImp,rel_pos2); - } - } - } - - -} - - - -void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer) -{ - - for (int v=0;vgetNumWheels();v++) - { - btVector3 wheelColor(0,1,1); - if (getWheelInfo(v).m_raycastInfo.m_isInContact) - { - wheelColor.setValue(0,0,1); - } else - { - wheelColor.setValue(1,0,1); - } - - btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin(); - - btVector3 axle = btVector3( - getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()], - getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()], - getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]); - - //debug wheels (cylinders) - debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); - debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); - - } -} - - -void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) -{ -// RayResultCallback& resultCallback; - - btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); - - m_dynamicsWorld->rayTest(from, to, rayCallback); - - if (rayCallback.hasHit()) - { - - const btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject); - if (body && body->hasContactResponse()) - { - result.m_hitPointInWorld = rayCallback.m_hitPointWorld; - result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld; - result.m_hitNormalInWorld.normalize(); - result.m_distFraction = rayCallback.m_closestHitFraction; - return (void*)body; - } - } - return 0; -} - diff --git a/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.h b/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.h deleted file mode 100644 index f59555f94..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.h +++ /dev/null @@ -1,236 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#ifndef BT_RAYCASTVEHICLE_H -#define BT_RAYCASTVEHICLE_H - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "btVehicleRaycaster.h" -class btDynamicsWorld; -#include "LinearMath/btAlignedObjectArray.h" -#include "btWheelInfo.h" -#include "BulletDynamics/Dynamics/btActionInterface.h" - -class btVehicleTuning; - -///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; - - ///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.)) - { - } - btScalar m_suspensionStiffness; - btScalar m_suspensionCompression; - btScalar m_suspensionDamping; - btScalar m_maxSuspensionTravelCm; - btScalar m_frictionSlip; - btScalar m_maxSuspensionForce; - - }; -private: - - btScalar m_tau; - btScalar m_damping; - btVehicleRaycaster* m_vehicleRaycaster; - btScalar m_pitchControl; - btScalar m_steeringValue; - btScalar m_currentVehicleSpeedKmHour; - - btRigidBody* m_chassisBody; - - int m_indexRightAxis; - int m_indexUpAxis; - 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() ; - - - ///btActionInterface interface - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step) - { - (void) collisionWorld; - updateVehicle(step); - } - - - ///btActionInterface interface - void debugDraw(btIDebugDraw* debugDrawer); - - const btTransform& getChassisWorldTransform() const; - - btScalar rayCast(btWheelInfo& wheel); - - virtual void updateVehicle(btScalar step); - - - void resetSuspension(); - - btScalar getSteeringValue(int wheel) const; - - void setSteeringValue(btScalar steering,int wheel); - - - void applyEngineForce(btScalar force, int wheel); - - const btTransform& getWheelTransformWS( int wheelIndex ) const; - - void updateWheelTransform( int wheelIndex, bool interpolatedTransform = true ); - -// 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); - - inline int getNumWheels() const { - return int (m_wheelInfo.size()); - } - - btAlignedObjectArray m_wheelInfo; - - - const btWheelInfo& getWheelInfo(int index) const; - - btWheelInfo& getWheelInfo(int index); - - void updateWheelTransformsWS(btWheelInfo& wheel , bool interpolatedTransform = true); - - - void setBrake(btScalar brake,int wheelIndex); - - void setPitchControl(btScalar pitch) - { - m_pitchControl = pitch; - } - - void updateSuspension(btScalar deltaTime); - - virtual void updateFriction(btScalar timeStep); - - - - inline btRigidBody* getRigidBody() - { - return m_chassisBody; - } - - const btRigidBody* getRigidBody() const - { - return m_chassisBody; - } - - inline int getRightAxis() const - { - return m_indexRightAxis; - } - inline int getUpAxis() const - { - return m_indexUpAxis; - } - - inline int getForwardAxis() const - { - return m_indexForwardAxis; - } - - - ///Worldspace forward vector - btVector3 getForwardVector() const - { - const btTransform& chassisTrans = getChassisWorldTransform(); - - 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 - { - return m_currentVehicleSpeedKmHour; - } - - 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 ; - } - - void setUserConstraintType(int userConstraintType) - { - m_userConstraintType = userConstraintType; - }; - - void setUserConstraintId(int uid) - { - m_userConstraintId = uid; - } - - int getUserConstraintId() const - { - return m_userConstraintId; - } - -}; - -class btDefaultVehicleRaycaster : public btVehicleRaycaster -{ - btDynamicsWorld* m_dynamicsWorld; -public: - btDefaultVehicleRaycaster(btDynamicsWorld* world) - :m_dynamicsWorld(world) - { - } - - virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result); - -}; - - -#endif //BT_RAYCASTVEHICLE_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Vehicle/btVehicleRaycaster.h b/WickedEngine/BULLET/BulletDynamics/Vehicle/btVehicleRaycaster.h deleted file mode 100644 index 3cc909c65..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://bulletphysics.org - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#ifndef BT_VEHICLE_RAYCASTER_H -#define BT_VEHICLE_RAYCASTER_H - -#include "LinearMath/btVector3.h" - -/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting -struct btVehicleRaycaster -{ -virtual ~btVehicleRaycaster() -{ -} - struct btVehicleRaycasterResult - { - 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; - -}; - -#endif //BT_VEHICLE_RAYCASTER_H - diff --git a/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.cpp b/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.cpp deleted file mode 100644 index ef93c16ff..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#include "btWheelInfo.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity - - -btScalar btWheelInfo::getSuspensionRestLength() const -{ - - return m_suspensionRestLength1; - -} - -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; - 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)) - { - m_suspensionRelativeVelocity = btScalar(0.0); - m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); - } - else - { - btScalar inv = btScalar(-1.) / project; - m_suspensionRelativeVelocity = projVel * inv; - m_clippedInvContactDotSuspension = inv; - } - - } - - else // Not in contact : position wheel in a nice (rest length) position - { - m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); - m_suspensionRelativeVelocity = btScalar(0.0); - m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; - m_clippedInvContactDotSuspension = btScalar(1.0); - } -} diff --git a/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.h b/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.h deleted file mode 100644 index f916053ec..000000000 --- a/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#ifndef BT_WHEEL_INFO_H -#define BT_WHEEL_INFO_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btTransform.h" - -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; - bool m_bIsFrontWheel; - -}; - -/// btWheelInfo contains information per wheel about friction and suspension. -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 - }; - - 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; - 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_engineForce; - - btScalar m_brake; - - bool m_bIsFrontWheel; - - void* m_clientInfo;//can be used to store pointer to sync transforms... - - btWheelInfo(btWheelInfoConstructionInfo& ci) - - { - - m_suspensionRestLength1 = ci.m_suspensionRestLength; - m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; - - m_wheelsRadius = ci.m_wheelRadius; - m_suspensionStiffness = ci.m_suspensionStiffness; - m_wheelsDampingCompression = ci.m_wheelsDampingCompression; - m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; - m_chassisConnectionPointCS = ci.m_chassisConnectionCS; - m_wheelDirectionCS = ci.m_wheelDirectionCS; - m_wheelAxleCS = ci.m_wheelAxleCS; - m_frictionSlip = ci.m_frictionSlip; - m_steering = btScalar(0.); - m_engineForce = btScalar(0.); - m_rotation = btScalar(0.); - m_deltaRotation = btScalar(0.); - m_brake = btScalar(0.); - m_rollInfluence = btScalar(0.1); - m_bIsFrontWheel = ci.m_bIsFrontWheel; - m_maxSuspensionForce = ci.m_maxSuspensionForce; - - } - - void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); - - btScalar m_clippedInvContactDotSuspension; - btScalar m_suspensionRelativeVelocity; - //calculated by suspension - btScalar m_wheelsSuspensionForce; - btScalar m_skidInfo; - -}; - -#endif //BT_WHEEL_INFO_H - diff --git a/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp b/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp deleted file mode 100644 index e90d24e6e..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" - -#include "btDefaultSoftBodySolver.h" -#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 performance in future once we understand more clearly when constants need to be updated - m_updateSolverConstants = true; -} - -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) -{ - m_softBodySet.copyFromArray( softBodies ); -} - -void btDefaultSoftBodySolver::updateSoftBodies( ) -{ - for ( int i=0; i < m_softBodySet.size(); i++) - { - btSoftBody* psb=(btSoftBody*)m_softBodySet[i]; - if (psb->isActive()) - { - psb->integrateMotion(); - } - } -} // updateSoftBodies - -bool btDefaultSoftBodySolver::checkInitialized() -{ - return true; -} - -void btDefaultSoftBodySolver::solveConstraints( float solverdt ) -{ - // Solve constraints for non-solver softbodies - for(int i=0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = static_cast(m_softBodySet[i]); - if (psb->isActive()) - { - psb->solveConstraints(); - } - } -} // btDefaultSoftBodySolver::solveConstraints - - -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 ) - { - const btAlignedObjectArray &clothVertices( softBody->m_nodes ); - int numVertices = clothVertices.size(); - - const btCPUVertexBufferDescriptor *cpuVertexBuffer = static_cast< btCPUVertexBufferDescriptor* >(vertexBuffer); - float *basePointer = cpuVertexBuffer->getBasePointer(); - - if( vertexBuffer->hasVertexPositions() ) - { - const int vertexOffset = cpuVertexBuffer->getVertexOffset(); - const int vertexStride = cpuVertexBuffer->getVertexStride(); - float *vertexPointer = basePointer + vertexOffset; - - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) - { - btVector3 position = clothVertices[vertexIndex].m_x; - *(vertexPointer + 0) = position.getX(); - *(vertexPointer + 1) = position.getY(); - *(vertexPointer + 2) = position.getZ(); - vertexPointer += vertexStride; - } - } - if( vertexBuffer->hasNormals() ) - { - const int normalOffset = cpuVertexBuffer->getNormalOffset(); - const int normalStride = cpuVertexBuffer->getNormalStride(); - float *normalPointer = basePointer + normalOffset; - - for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) - { - btVector3 normal = clothVertices[vertexIndex].m_n; - *(normalPointer + 0) = normal.getX(); - *(normalPointer + 1) = normal.getY(); - *(normalPointer + 2) = normal.getZ(); - normalPointer += normalStride; - } - } - } -} // btDefaultSoftBodySolver::copySoftBodyToVertexBuffer - -void btDefaultSoftBodySolver::processCollision( btSoftBody* softBody, btSoftBody* 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 ) -{ - softBody->defaultCollisionHandler( collisionObjectWrap ); -} // btDefaultSoftBodySolver::processCollision - - -void btDefaultSoftBodySolver::predictMotion( float timeStep ) -{ - for ( int i=0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = m_softBodySet[i]; - - if (psb->isActive()) - { - psb->predictMotion(timeStep); - } - } -} - diff --git a/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.h b/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.h deleted file mode 100644 index 1c17ffcbb..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_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: - /** Variable to define whether we need to update solver constants on the next iteration */ - bool m_updateSolverConstants; - - btAlignedObjectArray< btSoftBody * > m_softBodySet; - - -public: - btDefaultSoftBodySolver(); - - virtual ~btDefaultSoftBodySolver(); - - virtual SolverTypes getSolverType() const - { - return DEFAULT_SOLVER; - } - - virtual bool checkInitialized(); - - virtual void updateSoftBodies( ); - - virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies,bool forceUpdate=false ); - - virtual void copyBackToSoftBodies(bool bMove = true); - - virtual void solveConstraints( float solverdt ); - - virtual void predictMotion( float solverdt ); - - virtual void copySoftBodyToVertexBuffer( const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer ); - - virtual void processCollision( btSoftBody *, const btCollisionObjectWrapper* ); - - virtual void processCollision( btSoftBody*, btSoftBody* ); - -}; - -#endif // #ifndef BT_ACCELERATED_SOFT_BODY_CPU_SOLVER_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBody.cpp b/WickedEngine/BULLET/BulletSoftBody/btSoftBody.cpp deleted file mode 100644 index a0c8cca4a..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBody.cpp +++ /dev/null @@ -1,3655 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#include "btSoftBodyInternals.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "btSoftBodyData.h" -#include "LinearMath/btSerializer.h" - - -// -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; - - /* 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; - } - updateBounds(); - -} - -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) -:m_worldInfo(worldInfo) -{ - 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_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_worldTransform.setIdentity(); - setSolver(eSolverPresets::Positions); - - /* 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_restLengthScale = btScalar(1.0); -} - -// -btSoftBody::~btSoftBody() -{ - //for now, delete the internal shape - delete m_collisionShape; - int i; - - releaseClusters(); - for(i=0;i0) - *pm=*m_materials[0]; - else - ZeroInitialize(*pm); - m_materials.push_back(pm); - return(pm); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) -{ - Note n; - ZeroInitialize(n); - n.m_rank = 0; - n.m_text = text; - n.m_offset = o; - n.m_coords[0] = c.x(); - n.m_coords[1] = c.y(); - n.m_coords[2] = c.z(); - n.m_coords[3] = c.w(); - n.m_nodes[0] = n0;n.m_rank+=n0?1:0; - n.m_nodes[1] = n1;n.m_rank+=n1?1:0; - n.m_nodes[2] = n2;n.m_rank+=n2?1:0; - n.m_nodes[3] = n3;n.m_rank+=n3?1:0; - m_notes.push_back(n); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) -{ - appendNote(text,o,btVector4(1,0,0,0),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]); -} - -// -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]); -} - -// -void btSoftBody::appendNode( const btVector3& x,btScalar m) -{ - if(m_nodes.capacity()==m_nodes.size()) - { - pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); - indicesToPointers(); - } - const btScalar margin=getCollisionShape()->getMargin(); - m_nodes.push_back(Node()); - Node& n=m_nodes[m_nodes.size()-1]; - ZeroInitialize(n); - n.m_x = x; - n.m_q = n.m_x; - n.m_im = m>0?1/m:0; - n.m_material = m_materials[0]; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); -} - -// -void btSoftBody::appendLink(int model,Material* mat) -{ - Link l; - if(model>=0) - l=m_links[model]; - else - { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } - m_links.push_back(l); -} - -// -void btSoftBody::appendLink( int node0, - int node1, - Material* mat, - bool bcheckexist) -{ - appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); -} - -// -void btSoftBody::appendLink( Node* node0, - Node* node1, - Material* mat, - bool bcheckexist) -{ - 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; - } -} - -// -void btSoftBody::appendFace(int model,Material* mat) -{ - Face f; - if(model>=0) - { f=m_faces[model]; } - else - { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } - m_faces.push_back(f); -} - -// -void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) -{ - if (node0==node1) - return; - if (node1==node2) - return; - 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; -} - -// -void btSoftBody::appendTetra(int model,Material* mat) -{ -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); -} - -// -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; -} - -// - -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); -} - -// -void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence) -{ - if (disableCollisionBetweenLinkedBodies) - { - 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; - a.m_influence = influence; - m_anchors.push_back(a); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) -{ - LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; - m_joints.push_back(pj); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) -{ - appendLinearJoint(specs,m_clusters[0],body); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) -{ - appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) -{ - AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; - pj->m_icontrol = specs.icontrol; - m_joints.push_back(pj); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) -{ - appendAngularJoint(specs,m_clusters[0],body); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) -{ - appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); -} - -// -void btSoftBody::addForce(const btVector3& force) -{ - for(int i=0,ni=m_nodes.size();i0) - { - n.m_f += force; - } -} - -void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex) -{ - btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size()); - - 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_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 ) - { - 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; - const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2 = rel_v.length2(); - - if(rel_v2>SIMD_EPSILON) - { - const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = n.m_n; - - if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag) - { - 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)); - - // 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; - btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); - btScalar v_len2 = n.m_v.length2(); - - if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0) - { - 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); - } - - n.m_f += fDrag; - n.m_f += fLift; - } - else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) - { - if (btSoftBody::eAeroModel::V_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); - - 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); - ApplyClampedForce(n, force, dt); - } - } - } - } - } -} - -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 bool as_aero = as_lift || as_drag; - const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); - - if(as_faero) - { - 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); - medium.m_velocity = windVelocity; - medium.m_density = m_worldInfo->air_density; - const btVector3 rel_v=v-medium.m_velocity; - const btScalar rel_v_len = rel_v.length(); - const btScalar rel_v2=rel_v.length2(); - - if(rel_v2>SIMD_EPSILON) - { - const btVector3 rel_v_nrm = rel_v.normalized(); - btVector3 nrm = f.m_normal; - - if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag) - { - 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)); - - fDrag /= 3; - fLift /= 3; - - for(int j=0;j<3;++j) - { - 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; - btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2(); - btScalar v_len2 = f.m_n[j]->m_v.length2(); - - if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0) - { - 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); - } - - f.m_n[j]->m_f += fDrag; - f.m_n[j]->m_f += fLift; - } - } - } - else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) - { - if (btSoftBody::eAeroModel::F_TwoSided) - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); - - 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); - } - } - } - } - -} - -// -void btSoftBody::addVelocity(const btVector3& velocity) -{ - for(int i=0,ni=m_nodes.size();i0) - { - n.m_v = velocity; - } - } -} - - -// -void btSoftBody::addVelocity(const btVector3& velocity,int node) -{ - Node& n=m_nodes[node]; - if(n.m_im>0) - { - n.m_v += velocity; - } -} - -// -void btSoftBody::setMass(int node,btScalar mass) -{ - m_nodes[node].m_im=mass>0?1/mass:0; - m_bUpdateRtCst=true; -} - -// -btScalar btSoftBody::getMass(int node) const -{ - return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); -} - -// -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) - { - f.m_n[j]->m_im+=twicearea; - } - } - for( i=0;i 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; - } - } -for( i=0;i0) - { - m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im; - } - } -setTotalMass(mass,false); -} - -// -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();i0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; - } - /* 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; - } -} - -// -btScalar btSoftBody::getVolume() const -{ - btScalar vol=0; - if(m_nodes.size()>0) - { - 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)); - } - vol/=(btScalar)6; - } - return(vol); -} - -// -int btSoftBody::clusterCount() const -{ - return(m_clusters.size()); -} - -// -btVector3 btSoftBody::clusterCom(const Cluster* cluster) -{ - btVector3 com(0,0,0); - for(int i=0,ni=cluster->m_nodes.size();im_nodes[i]->m_x*cluster->m_masses[i]; - } - return(com*cluster->m_imass); -} - -// -btVector3 btSoftBody::clusterCom(int cluster) const -{ - return(clusterCom(m_clusters[cluster])); -} - -// -btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) -{ - return(cluster->m_lv+btCross(cluster->m_av,rpos)); -} - -// -void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) -{ - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*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) -{ - 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) -{ - if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); - if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); -} - -// -void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) -{ - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; - cluster->m_nvimpulses++; -} - -// -void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) -{ - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_dimpulses[1]+=ai; - cluster->m_ndimpulses++; -} - -// -void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) -{ - if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); - if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); -} - -// -void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) -{ - cluster->m_dimpulses[0]+=impulse*cluster->m_imass; - cluster->m_ndimpulses++; -} - -struct NodeLinks -{ - btAlignedObjectArray m_links; -}; - - - -// -int btSoftBody::generateBendingConstraints(int distance,Material* mat) -{ - int i,j; - - if(distance>1) - { - /* 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) - { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; - } - } - - } - } - } - } else - { - ///generic Floyd's algorithm - for(int k=0;ksum) - { - 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); - c->~Cluster(); - btAlignedFree(c); - m_clusters.remove(c); -} - -// -void btSoftBody::releaseClusters() -{ - while(m_clusters.size()>0) releaseCluster(0); -} - -// -int btSoftBody::generateClusters(int k,int maxiterations) -{ - int i; - releaseClusters(); - m_clusters.resize(btMin(k,m_nodes.size())); - for(i=0;im_collide= true; - } - k=m_clusters.size(); - if(k>0) - { - /* Initialize */ - btAlignedObjectArray centers; - btVector3 cog(0,0,0); - int i; - for(i=0;im_nodes.push_back(&m_nodes[i]); - } - cog/=(btScalar)m_nodes.size(); - centers.resize(k,cog); - /* Iterate */ - const btScalar slope=16; - bool changed; - int iterations=0; - do { - const btScalar w=2-btMin(1,iterations/slope); - changed=false; - iterations++; - int i; - - for(i=0;im_nodes.size();++j) - { - c+=m_clusters[i]->m_nodes[j]->m_x; - } - if(m_clusters[i]->m_nodes.size()) - { - c /= (btScalar)m_clusters[i]->m_nodes.size(); - c = centers[i]+(c-centers[i])*w; - changed |= ((c-centers[i]).length2()>SIMD_EPSILON); - centers[i] = c; - m_clusters[i]->m_nodes.resize(0); - } - } - for(i=0;im_nodes.push_back(&m_nodes[i]); - } - } while(changed&&(iterations cids; - cids.resize(m_nodes.size(),-1); - for(i=0;im_nodes.size();++j) - { - 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()) - { - m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); - } - } - } - } - } - /* Master */ - if(m_clusters.size()>1) - { - Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - pmaster->m_collide = false; - pmaster->m_nodes.reserve(m_nodes.size()); - for(int i=0;im_nodes.push_back(&m_nodes[i]); - m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); - } - /* Terminate */ - for(i=0;im_nodes.size()==0) - { - releaseCluster(i--); - } - } - } 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; - } - for (i=0;im_nodes.push_back(m_tetras[i].m_n[j]); - } - } - - } else - { - m_clusters.resize(m_faces.size()); - for(i=0;im_collide= true; - } - - for(i=0;im_nodes.push_back(m_faces[i].m_n[j]); - } - } - } - } - - if (m_clusters.size()) - { - initializeClusters(); - updateClusters(); - - - //for self-collision - m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size()); - { - for (int c0=0;c0m_clusterIndex=c0; - for (int c1=0;c1m_nodes.size();i++) - { - for (int j=0;jm_nodes.size();j++) - { - if (cla->m_nodes[i] == clb->m_nodes[j]) - { - connected=true; - break; - } - } - } - m_clusterConnectivity[c0+c1*m_clusters.size()]=connected; - } - } - } - } - - return(m_clusters.size()); -} - -// -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; - - /* Filter out */ - for(i=0;iEval(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; - } - } - } - /* Fill edges */ - for(i=0;i0) - { - 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) - { - 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; } - } - else - { - if(b.m_im>0) - { b.m_im/=0.5f;m=1/b.m_im; } - else - m=0; - } - 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) - { - appendLink(i); - Link* pft[]={ &m_links[i], - &m_links[m_links.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[0]]; - pft[0]->m_n[1]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[1]]; - } - } - } - /* Refine faces */ - for(i=0;i0) - { - appendFace(i); - const int l=(k+1)%3; - Face* pft[]={ &m_faces[i], - &m_faces[m_faces.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[l]]; - pft[0]->m_n[1]=&m_nodes[idx[j]]; - pft[0]->m_n[2]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[k]]; - pft[1]->m_n[2]=&m_nodes[idx[l]]; - appendLink(ni,idx[l],pft[0]->m_material); - --i;break; - } - } - } - } - /* Cut */ - if(cut) - { - btAlignedObjectArray cnodes; - const int pcount=ncount; - int i; - ncount=m_nodes.size(); - cnodes.resize(ncount,0); - /* Nodes */ - for(i=0;i=pcount)||(btFabs(ifn->Eval(x))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(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x) ranks; - btAlignedObjectArray todelete; - ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();i=0;--i) - { - if(!ranks[i]) todelete.push_back(i); - } - if(todelete.size()) - { - btAlignedObjectArray& map=ranks; - for(int i=0;im_v=v; - pn[1]->m_v=v; - for(i=0,ni=m_links.size();im_n[1]=pn[mtch]; - pft[1]->m_n[0]=pn[1-mtch]; - done=true; - } - } - for(i=0,ni=m_faces.size();im_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) - { - m_ndbvt.remove(pn[0]->m_leaf); - m_ndbvt.remove(pn[1]->m_leaf); - m_nodes.pop_back(); - m_nodes.pop_back(); - } - return(done); -} - -// -bool btSoftBody::rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) -{ - if(m_faces.size()&&m_fdbvt.empty()) - initializeFaceTree(); - - results.body = this; - results.fraction = 1.f; - results.feature = eFeature::None; - results.index = -1; - - return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); -} - -// -void btSoftBody::setSolver(eSolverPresets::_ preset) -{ - m_cfg.m_vsequence.clear(); - m_cfg.m_psequence.clear(); - m_cfg.m_dsequence.clear(); - switch(preset) - { - 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_dsequence.push_back(ePSolver::Linear); - break; - } -} - -// -void btSoftBody::predictMotion(btScalar dt) -{ - - int i,ni; - - /* Update */ - if(m_bUpdateRtCst) - { - m_bUpdateRtCst=false; - updateConstants(); - m_fdbvt.clear(); - if(m_cfg.collisions&fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt*m_cfg.timescale; - m_sst.isdt = 1/m_sst.sdt; - 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++) - { - if (deltaV[c]>clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (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); - } - /* 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); - } - } - /* Pose */ - updatePose(); - /* Match */ - if(m_pose.m_bframe&&(m_cfg.kMT>0)) - { - const btMatrix3x3 posetrs=m_pose.m_rot; - for(int i=0,ni=m_nodes.size();i0) - { - const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; - n.m_x=Lerp(n.m_x,x,m_cfg.kMT); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); -} - -// -void btSoftBody::solveConstraints() -{ - - /* Apply clusters */ - applyClusters(false); - /* Prepare links */ - - 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); - } - /* Prepare anchors */ - for(i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; - a.m_body->activate(); - } - /* Solve velocities */ - if(m_cfg.viterations>0) - { - /* Solve */ - for(int isolve=0;isolve0) - { - for(int isolve=0;isolve0) - { - const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i& bodies) -{ - const int nb=bodies.size(); - int iterations=0; - int i; - - for(i=0;im_cfg.citerations); - } - for(i=0;iprepareClusters(iterations); - } - for(i=0;isolveClusters(sor); - } - } - for(i=0;icleanupClusters(); - } -} - -// -void btSoftBody::integrateMotion() -{ - /* Update */ - updateNormals(); -} - -// -btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) -{ - m_rayFrom = rayFrom; - m_rayNormalizedDirection = (rayTo-rayFrom); - m_rayTo = rayTo; - m_mint = mxt; - m_face = 0; - m_tests = 0; -} - -// -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)&&(tteps)&&(tceps) && - (btDot(n,btCross(b-hit,c-hit))>ceps) && - (btDot(n,btCross(c-hit,a-hit))>ceps)) - { - return(t); - } - } - } - return(-1); -} - -// -void btSoftBody::pointersToIndices() -{ -#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; - } - } - for(i=0,ni=m_links.size();idata=*(void**)&i; - } - } - for(i=0,ni=m_anchors.size();idata=&m_nodes[i]; - } - } - for(i=0,ni=m_links.size();idata=&m_faces[i]; - } - } - for(i=0,ni=m_anchors.size();im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) - { - ++cnt; - if(!bcountonly) - { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; - } - } - } - } - else - {/* Use dbvt */ - RayFromToCaster collider(rayFrom,rayTo,mint); - - btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); - if(collider.m_face) - { - mint=collider.m_mint; - feature=btSoftBody::eFeature::Face; - index=(int)(collider.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; - - - const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, - v0,v1,v2, - mint); - if(t>0) - { - ++cnt; - if(!bcountonly) - { - feature=btSoftBody::eFeature::Tetra; - index=i; - mint=t; - } - } - } - } - return(cnt); -} - -// -void btSoftBody::initializeFaceTree() -{ - m_fdbvt.clear(); - for(int i=0;igetCollisionShape(); -// const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); - //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform &wtr = colObjWrap->getWorldTransform(); - //todo: check which transform is needed here - - btScalar dst = - m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(x), - shp, - nrm, - margin); - 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); - } - return(false); -} - -// -void btSoftBody::updateNormals() -{ - - 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; - } - for(i=0,ni=m_nodes.size();iSIMD_EPSILON) - m_nodes[i].m_n /= len; - } -} - -// -void btSoftBody::updateBounds() -{ - /*if( m_acceleratedSoftBody ) - { - // If we have an accelerated softbody we need to obtain the bounds correctly - // For now (slightly hackily) just have a very large AABB - // TODO: Write get bounds kernel - // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could - // probably do a test and exchange reasonably efficiently. - - m_bounds[0] = btVector3(-1000, -1000, -1000); - m_bounds[1] = btVector3(1000, 1000, 1000); - - } else {*/ - 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()) - { - 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); - } - //} -} - - -// -void btSoftBody::updatePose() -{ - 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) - { - 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) -{ - 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); - } - - /* Node area */ - - if (averageArea) - { - btAlignedObjectArray counts; - counts.resize(m_nodes.size(),0); - for(i=0,ni=m_nodes.size();im_area+=btFabs(f.m_ra); - } - } - for(i=0,ni=m_nodes.size();i0) - m_nodes[i].m_area/=(btScalar)counts[i]; - else - 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; - } -} - -void btSoftBody::updateConstants() -{ - resetLinkRestLengths(); - updateLinkConstants(); - updateArea(); -} - - - -// -void btSoftBody::initializeClusters() -{ - int i; - - for( i=0;im_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_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); - { - int i,ni; - - for(i=0,ni=c.m_nodes.size();im_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=c.m_masses[i]; - ii[0][0] += m*(q[1]+q[2]); - ii[1][1] += m*(q[0]+q[2]); - ii[2][2] += m*(q[0]+q[1]); - ii[0][1] -= m*k[0]*k[1]; - ii[0][2] -= m*k[0]*k[2]; - ii[1][2] -= m*k[1]*k[2]; - } - } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - - ii = ii.inverse(); - - /* Frame */ - c.m_framexform.setIdentity(); - c.m_framexform.setOrigin(c.m_com); - c.m_framerefs.resize(c.m_nodes.size()); - { - int i; - for(i=0;im_x-c.m_com; - } - } - } -} - -// -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; - } - PolarDecompose(m,r,s); - c.m_framexform.setOrigin(c.m_com); - c.m_framexform.setBasis(r); - /* Inertia */ -#if 1/* Constant */ - c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); -#else -#if 0/* Sphere */ - const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); - const btVector3 inertia(rk,rk,rk); - const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, - btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, - btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); - - 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]; - } - 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); - { - 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); - } - } - 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); - 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); - else - c.m_leaf=m_cdbvt.insert(bounds,&c); - } - } - } - - -} - - - - -// -void btSoftBody::cleanupClusters() -{ - for(int i=0;iTerminate(m_sst.sdt); - if(m_joints[i]->m_delete) - { - btAlignedFree(m_joints[i]); - m_joints.remove(m_joints[i--]); - } - } -} - -// -void btSoftBody::prepareClusters(int iterations) -{ - for(int i=0;iPrepare(m_sst.sdt,iterations); - } -} - - -// -void btSoftBody::solveClusters(btScalar sor) -{ - for(int i=0,ni=m_joints.size();iSolve(m_sst.sdt,sor); - } -} - -// -void btSoftBody::applyClusters(bool drift) -{ - BT_PROFILE("ApplyClusters"); -// const btScalar f0=m_sst.sdt; - //const btScalar f1=f0/2; - btAlignedObjectArray deltas; - btAlignedObjectArray weights; - deltas.resize(m_nodes.size(),btVector3(0,0,0)); - weights.resize(m_nodes.size(),0); - int i; - - 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; - } - } - } - for(i=0;i0) - { - m_nodes[i].m_x+=deltas[i]/weights[i]; - } - } -} - -// -void btSoftBody::dampClusters() -{ - int i; - - for(i=0;i0) - { - for(int j=0;j0) - { - 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); - } - } - } - } - } -} - -// -void btSoftBody::Joint::Prepare(btScalar dt,int) -{ - m_bodies[0].activate(); - m_bodies[1].activate(); -} - -// -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) - { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; - } - m_drift /=(btScalar)iterations; -} - -// -void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) -{ - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vr=va-vb; - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); -} - -// -void btSoftBody::LJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); - } -} - -// -void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) -{ - static const btScalar maxdrift=SIMD_PI/16; - m_icontrol->Prepare(this); - Joint::Prepare(dt,iterations); - m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; - m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; - m_drift = NormalizeAny(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_drift /=(btScalar)iterations; -} - -// -void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) -{ - const btVector3 va=m_bodies[0].angularVelocity(); - const btVector3 vb=m_bodies[1].angularVelocity(); - const btVector3 vr=va-vb; - const btScalar sp=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); -} - -// -void btSoftBody::AJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); - } -} - -// -void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) -{ - Joint::Prepare(dt,iterations); - const bool dodrift=(m_life==0); - m_delete=(++m_life)>m_maxlife; - if(dodrift) - { - 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_drift/=(btScalar)iterations; - } - else - { - m_drift=m_sdrift=btVector3(0,0,0); - } -} - -// -void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) -{ - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vrel=va-vb; - const btScalar rvac=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; - } - 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_asVelocity) - { - if (impulse.m_velocity.length() m_maxSelfCollisionImpulse) - { - - } 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]); - } - } - } - } else - { - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); - } -} - -// -void btSoftBody::CJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); - } -} - -// -void btSoftBody::applyForces() -{ - - BT_PROFILE("SoftBody applyForces"); -// const btScalar dt = m_sst.sdt; - const btScalar kLF = m_cfg.kLF; - 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) - { - volume = getVolume(); - ivolumetp = 1/btFabs(volume)*kPR; - dvolumetv = (m_pose.m_volume-volume)*kVC; - } - /* Per vertex forces */ - int i,ni; - - for(i=0,ni=m_nodes.size();i0) - { - if(use_medium) - { - /* Aerodynamics */ - addAeroForceToNode(m_windVelocity, i); - } - /* Pressure */ - if(as_pressure) - { - n.m_f += n.m_n*(n.m_area*ivolumetp); - } - /* Volume */ - if(as_volume) - { - 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_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) -{ - const btScalar dt = psb->m_sst.sdt; - const btScalar mrg = psb->getCollisionShape()->getMargin(); - for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; - const sCti& cti = c.m_cti; - btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - - const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb = c.m_node->m_x-c.m_node->m_q; - 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 ); - c.m_node->m_x -= impulse * c.m_c2; - if (tmpRigid) - tmpRigid->applyImpulse(impulse,c.m_c1); - } - } -} - -// -void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) -{ - for(int i=0,ni=psb->m_scontacts.size();im_scontacts[i]; - const btVector3& nr=c.m_normal; - Node& n=*c.m_node; - Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - 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; - } - 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) -{ - for(int i=0,ni=psb->m_links.size();im_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) - { - 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) -{ - 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); - } -} - -// -btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ 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); - default: - { - } - } - return(0); -} - -// -btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) -{ - switch(solver) - { - case eVSolver::Linear: return(&btSoftBody::VSolve_Links); - default: - { - } - } - return(0); -} - -// -void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) -{ - - switch(m_cfg.collisions&fCollision::RVSmask) - { - case fCollision::SDF_RS: - { - 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; - docollide.m_colObj1Wrap = pcoWrap; - docollide.m_rigidBody = prb1; - - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); - } - break; - case fCollision::CL_RS: - { - btSoftColliders::CollideCL_RS collider; - collider.ProcessColObj(this,pcoWrap); - } - break; - } -} - -// -void btSoftBody::defaultCollisionHandler(btSoftBody* psb) -{ - const int cf=m_cfg.collisions&psb->m_cfg.collisions; - switch(cf&fCollision::SVSmask) - { - case fCollision::CL_SS: - { - - //support self-collision if CL_SELF flag set - if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF) - { - btSoftColliders::CollideCL_SS docollide; - docollide.ProcessSoftSoft(this,psb); - } - - } - break; - case fCollision::VF_SS: - { - //only self-collision for Cluster, not Vertex-Face yet - if (this!=psb) - { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - } - } - break; - default: - { - - } - } -} - - - -void btSoftBody::setWindVelocity( const btVector3 &velocity ) -{ - m_windVelocity = velocity; -} - - -const btVector3& btSoftBody::getWindVelocity() -{ - return m_windVelocity; -} - - - -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 -{ - btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer; - - btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer); - - btHashMap m_nodeIndexMap; - - sbd->m_numMaterials = m_materials.size(); - 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); - //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); - 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_ARRAY_CODE,(void*) &m_materials); - } - - - - - sbd->m_numNodes = m_nodes.size(); - 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); - SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr; - for (int i=0;im_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; - 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); - } - 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; - if (sbd->m_links) - { - int sz = sizeof(SoftBodyLinkData); - int numElem = sbd->m_numLinks; - 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_restLength = m_links[i].m_rl; - } - 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; - if (sbd->m_faces) - { - int sz = sizeof(SoftBodyFaceData); - int numElem = sbd->m_numFaces; - 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_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]); - } - - - sbd->m_numTetrahedra = m_tetras.size(); - 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); - 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; - } - 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_restVolume = m_tetras[i].m_rv; - } - 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; - if (sbd->m_anchors) - { - int sz = sizeof(SoftRigidAnchorData); - int numElem = sbd->m_numAnchors; - 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; - btAssert(memPtr->m_nodeIndex < m_nodes.size()); - } - 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; - sbd->m_config.m_pressure = m_cfg.kPR; - sbd->m_config.m_aeroModel = this->m_cfg.aeromodel; - sbd->m_config.m_lift = m_cfg.kLF; - sbd->m_config.m_drag = m_cfg.kDG; - sbd->m_config.m_positionIterations = m_cfg.piterations; - sbd->m_config.m_driftIterations = m_cfg.diterations; - sbd->m_config.m_clusterIterations = m_cfg.citerations; - sbd->m_config.m_velocityIterations = m_cfg.viterations; - sbd->m_config.m_maxVolume = m_cfg.maxvolume; - sbd->m_config.m_damping = m_cfg.kDP; - sbd->m_config.m_poseMatch = m_cfg.kMT; - sbd->m_config.m_collisionFlags = m_cfg.collisions; - sbd->m_config.m_volume = m_cfg.kVC; - sbd->m_config.m_rigidContactHardness = m_cfg.kCHR; - sbd->m_config.m_kineticContactHardness = m_cfg.kKHR; - sbd->m_config.m_softContactHardness = m_cfg.kSHR; - sbd->m_config.m_anchorHardness = m_cfg.kAHR; - sbd->m_config.m_timeScale = m_cfg.timescale; - sbd->m_config.m_maxVolume = m_cfg.maxvolume; - sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL; - sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL; - sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL; - sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL; - sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL; - sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL; - - //pose for shape matching - { - sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose); - - int sz = sizeof(SoftBodyPoseData); - 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; - if (memPtr->m_numPositions) - { - int numElem = memPtr->m_numPositions; - int sz = sizeof(btVector3Data); - 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]); - } - 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; - 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;ifinalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]); - } - - 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; - 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;im_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; - m_clusters[i]->m_com.serializeFloat(memPtr->m_com); - memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor; - m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]); - m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]); - m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform); - memPtr->m_idmass = m_clusters[i]->m_idmass; - memPtr->m_imass = m_clusters[i]->m_imass; - m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi); - memPtr->m_ldamping = m_clusters[i]->m_ldamping; - m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii); - m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv); - memPtr->m_matching = m_clusters[i]->m_matching; - memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse; - memPtr->m_ndamping = m_clusters[i]->m_ndamping; - memPtr->m_ldamping = m_clusters[i]->m_ldamping; - memPtr->m_adamping = m_clusters[i]->m_adamping; - memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor; - - memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size(); - memPtr->m_numMasses = m_clusters[i]->m_masses.size(); - memPtr->m_numNodes = m_clusters[i]->m_nodes.size(); - - memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses; - m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]); - 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; - 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;jm_framerefs[j].serializeFloat(*memPtr); - } - 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; - 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;jm_masses[j]; - } - 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 ) - { - 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;jm_nodes[j]); - btAssert(indexPtr); - *memPtr = *indexPtr; - } - serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes); - } - } - 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; - - if (sbd->m_joints) - { - int sz = sizeof(btSoftBodyJointData); - int numElem = m_joints.size(); - 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]); - m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]); - memPtr->m_cfm = m_joints[i]->m_cfm; - memPtr->m_erp = m_joints[i]->m_erp; - memPtr->m_split = m_joints[i]->m_split; - memPtr->m_delete = m_joints[i]->m_delete; - - 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; - } - memPtr->m_bodyA = 0; - memPtr->m_bodyB = 0; - if (m_joints[i]->m_bodies[0].m_soft) - { - memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER; - memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft); - } - if (m_joints[i]->m_bodies[0].m_collisionObject) - { - memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT; - memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject); - } - if (m_joints[i]->m_bodies[0].m_rigid) - { - memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY; - memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid); - } - - if (m_joints[i]->m_bodies[1].m_soft) - { - memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER; - memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft); - } - if (m_joints[i]->m_bodies[1].m_collisionObject) - { - memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT; - memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject); - } - if (m_joints[i]->m_bodies[1].m_rigid) - { - memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY; - 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]); - } - - - return btSoftBodyDataName; -} - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBody.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBody.h deleted file mode 100644 index ee1a3d952..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBody.h +++ /dev/null @@ -1,1000 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#ifndef _BT_SOFT_BODY_H -#define _BT_SOFT_BODY_H - -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "btSparseSDF.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" - -//#ifdef BT_USE_DOUBLE_PRECISION -//#define btRigidBodyData btRigidBodyDoubleData -//#define btRigidBodyDataName "btRigidBodyDoubleData" -//#else -#define btSoftBodyData btSoftBodyFloatData -#define btSoftBodyDataName "btSoftBodyFloatData" -//#endif //BT_USE_DOUBLE_PRECISION - -class btBroadphaseInterface; -class btDispatcher; -class btSoftBodySolver; - -/* 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; - - 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) - { - } -}; - - -///The btSoftBody is an class to simulate cloth and volumetric soft bodies. -///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. -class btSoftBody : public btCollisionObject -{ -public: - btAlignedObjectArray m_collisionDisabledObjects; - - // The solver object that handles this soft body - 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 - };}; - - ///eVSolver : velocities solvers - 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 - };}; - - ///eSolverPresets - struct eSolverPresets { enum _ { - Positions, - Velocities, - Default = Positions, - END - };}; - - ///eFeature - struct eFeature { enum _ { - None, - Node, - Link, - Face, - Tetra, - END - };}; - - typedef btAlignedObjectArray tVSolverArray; - typedef btAlignedObjectArray tPSolverArray; - - // - // Flags - // - - ///fCollision - struct fCollision { enum _ { - RVSmask = 0x000f, ///Rigid versus soft mask - SDF_RS = 0x0001, ///SDF based rigid vs soft - CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - - SVSmask = 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 - };}; - - // - // API Types - // - - /* 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) - }; - - /* ImplicitFn */ - struct ImplicitFn - { - virtual btScalar Eval(const btVector3& x)=0; - }; - - // - // Internal types - // - - typedef btAlignedObjectArray tScalarArray; - typedef btAlignedObjectArray tVector3Array; - - /* 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 */ - }; - - /* Base type */ - struct Element - { - void* m_tag; // User data - Element() : m_tag(0) {} - }; - /* 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 - }; - - /* Feature */ - struct Feature : Element - { - Material* m_material; // Material - }; - /* 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 - }; - /* Link */ - struct Link : Feature - { - 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 - }; - /* Face */ - struct Face : Feature - { - Node* m_n[3]; // Node pointers - btVector3 m_normal; // Normal - btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data - }; - /* 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) - }; - /* 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 - }; - /* 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 - }; - /* 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 - }; - /* 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 - }; - /* Cluster */ - struct Cluster - { - 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 */ - struct Impulse - { - 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) - { - m_rigid = (btRigidBody*)btRigidBody::upcast(m_collisionObject); - } - - void activate() const - { - if(m_rigid) - m_rigid->activate(); - if (m_collisionObject) - m_collisionObject->activate(); - - } - const btMatrix3x3& invWorldInertia() const - { - static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); - if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); - if(m_soft) return(m_soft->m_invwi); - return(iwi); - } - btScalar invMass() const - { - if(m_rigid) return(m_rigid->getInvMass()); - if(m_soft) return(m_soft->m_imass); - return(0); - } - const btTransform& xform() const - { - static const btTransform identity=btTransform::getIdentity(); - if(m_collisionObject) return(m_collisionObject->getWorldTransform()); - if(m_soft) return(m_soft->m_framexform); - return(identity); - } - btVector3 linearVelocity() const - { - if(m_rigid) return(m_rigid->getLinearVelocity()); - if(m_soft) return(m_soft->m_lv); - return(btVector3(0,0,0)); - } - btVector3 angularVelocity(const btVector3& rpos) const - { - if(m_rigid) return(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 - { - return(linearVelocity()+angularVelocity(rpos)); - } - void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const - { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); - } - void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const - { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); - } - void applyImpulse(const Impulse& impulse,const btVector3& rpos) const - { - if(impulse.m_asVelocity) - { -// 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) - { -// 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 - { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); - } - void applyDAImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); - } - void applyAImpulse(const Impulse& impulse) const - { - if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); - if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); - } - void applyDCImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyCentralImpulse(impulse); - if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); - } - }; - /* Joint */ - struct Joint - { - struct eType { enum _ { - Linear=0, - Angular, - Contact - };}; - struct Specs - { - 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() {} - 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; - }; - /* 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); } - }; - /* AJoint */ - struct AJoint : Joint - { - struct IControl - { - virtual void Prepare(AJoint*) {} - virtual btScalar Speed(AJoint*,btScalar current) { return(current); } - static IControl* Default() { static IControl def;return(&def); } - }; - struct Specs : Joint::Specs - { - Specs() : icontrol(IControl::Default()) {} - 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 - { - 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 - { - 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); - }; - - // - // 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; - - // - // 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 - - btAlignedObjectArraym_clusterConnectivity;//cluster connectivity, for self-collision - - btTransform m_initialWorldTransform; - - btVector3 m_windVelocity; - - btScalar m_restLengthScale; - - // - // Api - // - - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m); - - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo); - - void initDefaults(); - - /* dtor */ - virtual ~btSoftBody(); - /* Check for existing link */ - - btAlignedObjectArray m_userIndexMapping; - - btSoftBodyWorldInfo* getWorldInfo() - { - return m_worldInfo; - } - - ///@todo: avoid internal softbody shape hack and move collision code to collision library - 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); - // - 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); - /* Add aero force to a node of the body */ - void addAeroForceToNode(const btVector3& windVelocity,int nodeIndex); - - /* Add aero force to a face of the body */ - void addAeroForceToFace(const btVector3& windVelocity,int faceIndex); - - /* Add velocity to the entire body */ - void addVelocity( 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); - /* Set volume mass (using tetrahedrons) */ - 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); - /* Get link resting lengths scale */ - 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) */ - ///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); - - ///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); - - - - // - // Functionality to deal with new accelerated solvers. - // - - /** - * Set a wind velocity for interaction with the air. - */ - void setWindVelocity( const btVector3 &velocity ); - - - /** - * Return the wind velocity for interaction with the air. - */ - const btVector3& getWindVelocity(); - - // - // 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 ) - { - m_softBodySolver = softBodySolver; - } - - // - // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() - { - return m_softBodySolver; - } - - // - // Return the solver that handles this soft body - // - btSoftBodySolver *getSoftBodySolver() const - { - return m_softBodySolver; - } - - - // - // Cast - // - - static const btSoftBody* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (const btSoftBody*)colObj; - return 0; - } - static btSoftBody* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (btSoftBody*)colObj; - return 0; - } - - // - // ::btCollisionObject - // - - virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin = m_bounds[0]; - aabbMax = m_bounds[1]; - } - // - // Private - // - 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); - - - 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; - - -}; - - - - -#endif //_BT_SOFT_BODY_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp deleted file mode 100644 index 9f0d44526..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ /dev/null @@ -1,357 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSoftBodyConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" -#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 - -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) -{ - m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject()); - m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject(); - - // - // create the manifold from the dispatcher 'manifold pool' - // - // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); - - clearCache(); -} - -btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() -{ - clearCache(); - // m_dispatcher->releaseManifold( m_manifoldPtr ); - -} - - -void btSoftBodyTriangleCallback::clearCache() -{ - for (int i=0;im_childShape); - 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) -{ - //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)) - { - 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); - } - - btTriIndex triIndex(partId,triangleIndex,0); - btHashKey triKey(triIndex.getUid()); - - - btTriIndex* shapeIndex = m_shapeCache[triKey]; - if (shapeIndex) - { - btCollisionShape* tm = shapeIndex->m_childShape; - btAssert(tm); - - //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, ob, btTransform::getIdentity());//ob->getWorldTransform());//?? - btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex); - - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); - - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); - colAlgo->~btCollisionAlgorithm(); - ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - return; - } - - //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]); - normal.normalize(); - normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; - // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; - // other+=normal*22.f; - btVector3 pts[6] = {triangle[0]+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]); - // 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());//?? - - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); - - colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); - colAlgo->~btCollisionAlgorithm(); - ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - triIndex.m_childShape = tm; - m_shapeCache.insert(triKey,triIndex); - - } - - - -} - - - -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_resultOut = resultOut; - - - 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(); - softTransform.setOrigin(softBodyCenter); - - btTransform convexInTriangleSpace; - convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform; - 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) -{ - - - //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()); - - // if (convexBody->getCollisionShape()->isConvex()) - { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); - m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut); - - - concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); - - // resultOut->refreshContactPoints(); - - } - - } - -} - - -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 - //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); - if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) - { - return btScalar(1.); - } - - //const btVector3& from = convexbody->m_worldTransform.getOrigin(); - //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); - //todo: only do if the motion exceeds the 'radius' - - btTransform triInv = triBody->getWorldTransform().inverse(); - btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); - btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - - struct LocalTriangleSphereCastCallback : public btTriangleCallback - { - btTransform m_ccdSphereFromTrans; - btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; - - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - - - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { - } - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - //do a swept sphere for now - btTransform ident; - ident.setIdentity(); - btConvexCast::CastResult castResult; - castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - //local space? - - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) - { - if (m_hitFraction > castResult.m_fraction) - m_hitFraction = castResult.m_fraction; - } - - } - - }; - - - - - - if (triBody->getCollisionShape()->isConcave()) - { - btVector3 rayAabbMin = convexFromLocal.getOrigin(); - rayAabbMin.setMin(convexToLocal.getOrigin()); - btVector3 rayAabbMax = convexFromLocal.getOrigin(); - rayAabbMax.setMax(convexToLocal.getOrigin()); - btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); - - raycastCallback.m_hitFraction = convexbody->getHitFraction(); - - btCollisionObject* concavebody = triBody; - - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - - if (triangleMesh) - { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); - } - - - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) - { - convexbody->setHitFraction( raycastCallback.m_hitFraction); - return raycastCallback.m_hitFraction; - } - } - - return btScalar(1.); - -} diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h deleted file mode 100644 index 11c7b88f9..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ /dev/null @@ -1,155 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H -#define BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btSoftBody; -class btCollisionShape; - -#include "LinearMath/btHashMap.h" - -#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS - -struct btTriIndex -{ - int m_PartIdTriangleIndex; - class btCollisionShape* m_childShape; - - btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) - { - m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; - m_childShape = shape; - } - - 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)); - } - int getPartId() const - { - // Get only the highest bits where the part index is stored - return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); - } - 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 ; - - btManifoldResult* m_resultOut; - - btDispatcher* m_dispatcher; - const btDispatcherInfo* m_dispatchInfoPtr; - btScalar m_collisionMarginTriangle; - - btHashMap,btTriIndex> m_shapeCache; - -public: - int m_triangleCount; - - // btPersistentManifold* m_manifoldPtr; - - btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); - - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triObjWrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual ~btSoftBodyTriangleCallback(); - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - void clearCache(); - - SIMD_FORCE_INLINE const btVector3& getAabbMin() const - { - return m_aabbMin; - } - SIMD_FORCE_INLINE const btVector3& getAabbMax() const - { - return m_aabbMax; - } - -}; - - - - -/// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes. -class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm -{ - - bool m_isSwapped; - - btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback; - -public: - - 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); - - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - //we don't add any manifolds - } - - void clearCache(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyData.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyData.h deleted file mode 100644 index 87d8841cf..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyData.h +++ /dev/null @@ -1,217 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFTBODY_FLOAT_DATA -#define BT_SOFTBODY_FLOAT_DATA - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - - -struct SoftBodyMaterialData -{ - float m_linearStiffness; - float m_angularStiffness; - float m_volumeStiffness; - int m_flags; -}; - -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; -}; - -struct SoftBodyLinkData -{ - SoftBodyMaterialData *m_material; - int m_nodeIndices[2]; // Node pointers - float m_restLength; // Rest length - int m_bbending; // Bending link -}; - -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; -}; - -struct SoftRigidAnchorData -{ - 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 SoftBodyConfigData -{ - 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 SoftBodyPoseData -{ - 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; -}; - -struct SoftBodyClusterData -{ - 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 -{ - 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 -{ - 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; - - 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 - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.cpp b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.cpp deleted file mode 100644 index 36f675a6c..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.cpp +++ /dev/null @@ -1,1055 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBodyHelpers.cpp by Nathanael Presson - -#include "btSoftBodyInternals.h" -#include -#include -#include "btSoftBodyHelpers.h" -#include "LinearMath/btConvexHull.h" -#include "LinearMath/btConvexHullComputer.h" - - -// -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); -} - -// -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); -} - -// -static void drawTree( btIDebugDraw* idraw, - const btDbvtNode* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) -{ - if(node) - { - if(node->isinternal()&&((depthchilds[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); - } - 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); - } - } -} - -// -template -static inline T sum(const btAlignedObjectArray& items) -{ - T v; - if(items.size()) - { - v=items[0]; - for(int i=1,ni=items.size();i -static inline void add(btAlignedObjectArray& items,const Q& value) -{ - for(int i=0,ni=items.size();i -static inline void mul(btAlignedObjectArray& items,const Q& value) -{ - for(int i=0,ni=items.size();i -static inline T average(const btAlignedObjectArray& items) -{ - const btScalar n=(btScalar)(items.size()>0?items.size():1); - return(sum(items)/n); -} - -// -static inline btScalar tetravolume(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))); -} - -// -#if 0 -static btVector3 stresscolor(btScalar stress) -{ - static const btVector3 spectrum[]= { btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0)}; - static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; - static const btScalar one=1; - stress=btMax(0,btMin(1,stress))*ncolors; - const int sel=(int)stress; - const btScalar frc=stress-sel; - return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); -} -#endif - -// -void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) -{ - 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)) - { - srand(1806); - for(i=0;im_clusters.size();++i) - { - if(psb->m_clusters[i]->m_collide) - { - btVector3 color( rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX); - 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; - } -#define USE_NEW_CONVEX_HULL_COMPUTER -#ifdef USE_NEW_CONVEX_HULL_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(); - - int v0 = firstEdge->getSourceVertex(); - int v1 = firstEdge->getTargetVertex(); - while (edge!=firstEdge) - { - int v2 = edge->getTargetVertex(); - idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1); - edge = edge->getNextEdgeOfFace(); - 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) - { - const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; - idraw->drawTriangle(hres.m_OutputVertices[idx[0]], - hres.m_OutputVertices[idx[1]], - hres.m_OutputVertices[idx[2]], - color,1); - } - hlib.ReleaseResult(hres); -#endif - - } - /* Velocities */ -#if 0 - for(int j=0;jm_clusters[i].m_nodes.size();++j) - { - const btSoftBody::Cluster& c=psb->m_clusters[i]; - const btVector3 r=c.m_nodes[j]->m_x-c.m_com; - const btVector3 v=c.m_lv+btCross(c.m_av,r); - 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)); - } - } - else - { - /* Nodes */ - if(0!=(drawflags&fDrawFlags::Nodes)) - { - for(i=0;im_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)); - } - } - /* Links */ - if(0!=(drawflags&fDrawFlags::Links)) - { - for(i=0;im_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); - } - } - /* Normals */ - if(0!=(drawflags&fDrawFlags::Normals)) - { - for(i=0;im_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); - } - } - /* 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)); - } - } - /* 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;im_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;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)); - } - for(i=0;im_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) - { - drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); - } - } - } - - - /* Notes */ - if(0!=(drawflags&fDrawFlags::Notes)) - { - for(i=0;im_notes.size();++i) - { - const btSoftBody::Note& n=psb->m_notes[i]; - btVector3 p=n.m_offset; - for(int j=0;jm_x*n.m_coords[j]; - } - idraw->draw3dText(p,n.m_text); - } - } - /* 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) - { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) - { - case btSoftBody::Joint::eType::Linear: - { - const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; - const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; - idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); - idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); - drawVertex(idraw,a0,0.25,btVector3(1,1,0)); - drawVertex(idraw,a1,0.25,btVector3(0,1,1)); - } - break; - case btSoftBody::Joint::eType::Angular: - { - //const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; - const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); - const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); - const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; - idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); - idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); - idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); - idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); - break; - } - default: - { - } - - } - } - } -} - -// -void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) -{ - for(int i=0;im_nodes.size();++i) - { - 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); - } - if(areas) - { - sprintf(buff," A(%.2f)",n.m_area); - strcat(text,buff); - } - if(text[0]) idraw->draw3dText(n.m_x,text); - } -} - -// -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); -} - -// -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); -} - -// -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); -} - -// -void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) -{ - 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) - { - 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) -{ - /* 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); - delete[] x; - delete[] m; - /* Create links */ - for(i=1;iappendLink(i-1,i); - } - /* 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) -{ -#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); - 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) - { - 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->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->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); - } - } - } - } - } - /* Finished */ -#undef IDX - return(psb); -} - -// -btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords) -{ - - /* - * - * corners: - * - * [0][0] corner00 ------- corner01 [resx][0] - * | | - * | | - * [0][resy] corner10 -------- corner11 [resx][resy] - * - * - * - * - * - * - * "fixedgs" map: - * - * corner00 --> +1 - * corner01 --> +2 - * corner10 --> +4 - * corner11 --> +8 - * upper middle --> +16 - * left middle --> +32 - * right middle --> +64 - * lower middle --> +128 - * center --> +256 - * - * - * tex_coords size (resx-1)*(resy-1)*12 - * - * - * - * SINGLE QUAD INTERNALS - * - * 1) btSoftBody's nodes and links, - * diagonal link is optional ("gendiags") - * - * - * node00 ------ node01 - * | . - * | . - * | . - * | . - * | . - * node10 node11 - * - * - * - * 2) Faces: - * two triangles, - * UV Coordinates (hier example for single quad) - * - * (0,1) (0,1) (1,1) - * 1 |\ 3 \-----| 2 - * | \ \ | - * | \ \ | - * | \ \ | - * | \ \ | - * 2 |-----\ 3 \| 1 - * (0,0) (1,0) (1,0) - * - * - * - * - * - * - */ - -#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) - /* Create nodes */ - if((resx<2)||(resy<2)) return(0); - const int rx=resx; - const int ry=resy; - const int tot=rx*ry; - btVector3* x=new btVector3[tot]; - btScalar* m=new btScalar[tot]; - - int iy; - - for(iy=0;iysetMass(IDX(0,0),0); - if(fixeds&2) psb->setMass(IDX(rx-1,0),0); - if(fixeds&4) psb->setMass(IDX(0,ry-1),0); - if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); - if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); - if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); - if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); - if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); - if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); - delete[] x; - delete[] m; - - - int z = 0; - /* Create links and faces */ - for(iy=0;iyappendLink(node00,node01); - if(mdy) psb->appendLink(node00,node10); - if(mdx&&mdy) - { - psb->appendFace(node00,node10,node11); - if (tex_coords) { - tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); - } - psb->appendFace(node11,node01,node00); - if (tex_coords) { - tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); - } - if (gendiags) psb->appendLink(node00,node11); - z += 12; - } - } - } - /* Finished */ -#undef IDX - return(psb); -} - -float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) -{ - - /* - * - * - * node00 --- node01 - * | | - * node10 --- node11 - * - * - * ID map: - * - * node00 s --> 0 - * node00 t --> 1 - * - * node01 s --> 3 - * node01 t --> 1 - * - * node10 s --> 0 - * node10 t --> 2 - * - * node11 s --> 3 - * node11 t --> 2 - * - * - */ - - float tc=0.0f; - if (id == 0) { - tc = (1.0f/((resx-1))*ix); - } - else if (id==1) { - tc = (1.0f/((resy-1))*(resy-1-iy)); - } - else if (id==2) { - tc = (1.0f/((resy-1))*(resy-1-iy-1)); - } - else if (id==3) { - tc = (1.0f/((resx-1))*(ix+1)); - } - return tc; -} -// -btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) -{ - struct Hammersley - { - 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); - } - } - }; - btAlignedObjectArray vtx; - vtx.resize(3+res); - Hammersley::Generate(&vtx[0],vtx.size()); - for(int i=0;i chks; - btAlignedObjectArray vtx; - chks.resize(maxidx*maxidx,false); - vtx.resize(maxidx); - for(i=0,j=0,ni=maxidx*3;iappendLink(idx[j],idx[k]); - } - } -#undef IDX - psb->appendFace(idx[0],idx[1],idx[2]); - } - - if (randomizeConstraints) - { - psb->randomizeConstraints(); - } - - return(psb); -} - -// -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) - { - 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]); - } - hlib.ReleaseResult(hres); - if (randomizeConstraints) - { - psb->randomizeConstraints(); - } - return(psb); -} - - - - -static int nextLine(const char* buffer) -{ - int numBytesRead=0; - - while (*buffer != '\n') - { - buffer++; - numBytesRead++; - } - - - if (buffer[0]==0x0a) - { - buffer++; - numBytesRead++; - } - return numBytesRead; -} - -/* Create from TetGen .ele, .face, .node data */ -btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo, - const char* ele, - const char* face, - const char* node, - bool bfacelinks, - 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; - node += nextLine(node); - - //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); -#if 0 -if(face&&face[0]) - { - int nface=0; - sf>>nface;sf>>hasbounds; - for(int i=0;i>index; - sf>>ni[0];sf>>ni[1];sf>>ni[2]; - sf>>bound; - psb->appendFace(ni[0],ni[1],ni[2]); - 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); - } - } - } -#endif - -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) - { - 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); -} - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.h deleted file mode 100644 index 620a52fe3..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.h +++ /dev/null @@ -1,143 +0,0 @@ -/* -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. -*/ - -#ifndef BT_SOFT_BODY_HELPERS_H -#define BT_SOFT_BODY_HELPERS_H - -#include "btSoftBody.h" - -// -// 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 -{ - /* 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); - -}; - -#endif //BT_SOFT_BODY_HELPERS_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyInternals.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyInternals.h deleted file mode 100644 index 19d0543ef..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyInternals.h +++ /dev/null @@ -1,908 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#ifndef _BT_SOFT_BODY_INTERNALS_H -#define _BT_SOFT_BODY_INTERNALS_H - -#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 -// -// 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; -}; - -// -// btSoftBodyCollisionShape -// -class btSoftBodyCollisionShape : public btConcaveShape -{ -public: - btSoftBody* m_body; - - btSoftBodyCollisionShape(btSoftBody* backptr) - { - m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; - m_body=backptr; - } - - virtual ~btSoftBodyCollisionShape() - { - - } - - 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 - { - /* 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) - { - aabbMin.setMin(crns[i]); - aabbMax.setMax(crns[i]); - } - } - - - virtual void setLocalScaling(const btVector3& /*scaling*/) - { - ///na - } - virtual const btVector3& getLocalScaling() const - { - static const btVector3 dummy(1,1,1); - return dummy; - } - virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const - { - ///not yet - btAssert(0); - } - virtual const char* getName()const - { - return "SoftBody"; - } - -}; - -// -// btSoftClusterCollisionShape -// -class btSoftClusterCollisionShape : public btConvexInternalShape -{ -public: - const btSoftBody::Cluster* m_cluster; - - btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { - btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; - btScalar d=btDot(vec,n[0]->m_x); - int j=0; - for(int i=1,ni=m_cluster->m_nodes.size();im_x); - if(k>d) { d=k;j=i; } - } - return(n[j]->m_x); - } - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const - { - return(localGetSupportingVertex(vec)); - } - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const - {} - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const - {} - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - {} - - virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } - - //debugging - virtual const char* getName()const {return "SOFTCLUSTER";} - - virtual void setMargin(btScalar margin) - { - btConvexInternalShape::setMargin(margin); - } - virtual btScalar getMargin() const - { - return getMargin(); - } -}; - -// -// Inline's -// - -// -template -static inline void ZeroInitialize(T& value) -{ - 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) -{ - 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); -} -// -template -static inline T Clamp(const T& x,const T& l,const T& h) -{ return(xh?h:x); } -// -template -static inline T Sq(const T& x) -{ return(x*x); } -// -template -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)); } -// -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) -{ - const btVector3 d=x-y; - return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); -} -// -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) -{ - 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 Cross(const btVector3& v) -{ - 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 Diagonal(btScalar x) -{ - 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 Add(const btMatrix3x3& a, - const btMatrix3x3& b) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]+b[i]; - return(r); -} -// -static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) -{ - 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) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]*b; - return(r); -} -// -static inline void Orthogonalize(btMatrix3x3& m) -{ - 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) -{ - 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) -{ - return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); -} - -// -static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) -{ - return(Add(iia,iib).inverse()); -} - -// -static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) -{ - return(a*btDot(v,a)); -} -// -static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) -{ - return(v-ProjectOnAxis(v,a)); -} - -// -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(l2SIMD_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; - } - else - { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); - } - } - } -} - -// -template -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()); -} -// -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)); -} - -// -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]) - { - 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;iEval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)SIMD_EPSILON) - return(v/l); - else - return(btVector3(0,0,0)); -} - -// -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); -} - -// -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); -} - -// -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); -} - -// -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))); -} - -// -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 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; - } - else - {/* 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) -{ - if((a==ma)&&(b==mb)) return(0); - if((a==mb)&&(b==ma)) return(1); - return(-1); -} - -// -// btEigen : Extract eigen system, -// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html -// outputs are NOT sorted. -// -struct btEigen -{ - static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) - { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; - vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])accuracy) - { - 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)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) - return; - - 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* 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; - } - else - { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; - } - } - } - } - 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(); - ///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; - - 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 - { - 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; - - - 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]; - } - - if (!connected) - { - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) - { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) - { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; - } - } - } else - { - static int count=0; - count++; - //printf("count=%d\n",count); - - } - } - void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb) - { - 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); - } - }; - // - // CollideSDF_RS - // - struct CollideSDF_RS : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); - } - void DoNode(btSoftBody::Node& n) const - { - 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)) - { - 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; - 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; - }; - // - // CollideVF_SS - // - struct CollideVF_SS : btDbvt::ICollide - { - 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)) - { - 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; - } - const btScalar ms=ma+mb; - if(ms>0) - { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; - psb[0]->m_scontacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar mrg; - }; -}; - -#endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp deleted file mode 100644 index f5a67f6d8..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftBodyRigidBodyCollisionConfiguration.h" -#include "btSoftRigidCollisionAlgorithm.h" -#include "btSoftBodyConcaveCollisionAlgorithm.h" -#include "btSoftSoftCollisionAlgorithm.h" - -#include "LinearMath/btPoolAllocator.h" - -#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1 - -btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) -:btDefaultCollisionConfiguration(constructionInfo) -{ - void* mem; - - mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); - m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - m_swappedSoftRigidConvexCreateFunc->m_swapped=true; - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; - m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; -#endif - - //replace pool by a new one, with potential larger size - - if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) - { - int curElemSize = m_collisionAlgorithmPool->getElementSize(); - ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - - - int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); - int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); - int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); - - int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - - if (collisionAlgorithmMaxElementSize > curElemSize) - { - m_collisionAlgorithmPool->~btPoolAllocator(); - btAlignedFree(m_collisionAlgorithmPool); - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); - } - } - -} - -btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() -{ - m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softSoftCreateFunc); - - m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConvexCreateFunc); - - m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConvexCreateFunc); - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConcaveCreateFunc); - - m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - 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) -{ - - ///try to handle the softbody interactions first - - if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE)) - { - return m_softSoftCreateFunc; - } - - ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) - { - return m_softRigidConvexCreateFunc; - } - - ///convex versus soft body - if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) - { - return m_swappedSoftRigidConvexCreateFunc; - } - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) - { - return m_softRigidConcaveCreateFunc; - } - - ///convex versus soft body - if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) - { - return m_swappedSoftRigidConcaveCreateFunc; - } -#endif - - ///fallback to the regular rigid collision shape - return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1); -} diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h deleted file mode 100644 index 21addcfe2..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ /dev/null @@ -1,48 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION -#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - -#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" - -class btVoronoiSimplexSolver; -class btGjkEpaPenetrationDepthSolver; - - -///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of 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; - -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); - -}; - -#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolverVertexBuffer.h deleted file mode 100644 index c4733d640..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolverVertexBuffer.h +++ /dev/null @@ -1,165 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H -#define BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H - - -class btVertexBufferDescriptor -{ -public: - enum BufferTypes - { - CPU_BUFFER, - DX11_BUFFER, - OPENGL_BUFFER - }; - -protected: - - bool m_hasVertexPositions; - bool m_hasNormals; - - int m_vertexOffset; - int m_vertexStride; - - int m_normalOffset; - int m_normalStride; - -public: - btVertexBufferDescriptor() - { - m_hasVertexPositions = false; - m_hasNormals = false; - m_vertexOffset = 0; - m_vertexStride = 0; - m_normalOffset = 0; - m_normalStride = 0; - } - - virtual ~btVertexBufferDescriptor() - { - - } - - virtual bool hasVertexPositions() const - { - return m_hasVertexPositions; - } - - virtual bool hasNormals() const - { - return m_hasNormals; - } - - /** - * Return the type of the vertex buffer descriptor. - */ - virtual BufferTypes getBufferType() const = 0; - - /** - * Return the vertex offset in floats from the base pointer. - */ - virtual int getVertexOffset() const - { - return m_vertexOffset; - } - - /** - * Return the vertex stride in number of floats between vertices. - */ - virtual int getVertexStride() const - { - return m_vertexStride; - } - - /** - * Return the vertex offset in floats from the base pointer. - */ - virtual int getNormalOffset() const - { - return m_normalOffset; - } - - /** - * Return the vertex stride in number of floats between vertices. - */ - virtual int getNormalStride() const - { - return m_normalStride; - } -}; - - -class btCPUVertexBufferDescriptor : public btVertexBufferDescriptor -{ -protected: - float *m_basePointer; - -public: - /** - * vertexBasePointer is pointer to beginning of the buffer. - * 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 ) - { - m_basePointer = basePointer; - m_vertexOffset = vertexOffset; - m_vertexStride = vertexStride; - m_hasVertexPositions = true; - } - - /** - * vertexBasePointer is pointer to beginning of the buffer. - * 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 ) - { - m_basePointer = basePointer; - - m_vertexOffset = vertexOffset; - m_vertexStride = vertexStride; - m_hasVertexPositions = true; - - m_normalOffset = normalOffset; - m_normalStride = normalStride; - m_hasNormals = true; - } - - virtual ~btCPUVertexBufferDescriptor() - { - - } - - /** - * Return the type of the vertex buffer descriptor. - */ - virtual BufferTypes getBufferType() const - { - return CPU_BUFFER; - } - - /** - * Return the base pointer in memory to the first vertex. - */ - virtual float *getBasePointer() const - { - return m_basePointer; - } -}; - -#endif // #ifndef BT_SOFT_BODY_SOLVER_VERTEX_BUFFER_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolvers.h b/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolvers.h deleted file mode 100644 index 6947bc27d..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolvers.h +++ /dev/null @@ -1,154 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_BODY_SOLVERS_H -#define BT_SOFT_BODY_SOLVERS_H - -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" - - -class btSoftBodyTriangleData; -class btSoftBodyLinkData; -class btSoftBodyVertexData; -class btVertexBufferDescriptor; -class btCollisionObject; -class btSoftBody; - - -class btSoftBodySolver -{ -public: - enum SolverTypes - { - DEFAULT_SOLVER, - CPU_SOLVER, - CL_SOLVER, - CL_SIMD_SOLVER, - DX_SOLVER, - DX_SIMD_SOLVER - }; - - -protected: - int m_numberOfPositionIterations; - int m_numberOfVelocityIterations; - // Simulation timescale - float m_timeScale; - -public: - btSoftBodySolver() : - m_numberOfPositionIterations( 10 ), - m_timeScale( 1 ) - { - m_numberOfVelocityIterations = 0; - m_numberOfPositionIterations = 5; - } - - 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; - - /** 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; - - /** Solve constraints for a set of soft bodies */ - 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; - - /** Process a collision between two soft bodies */ - virtual void processCollision( btSoftBody*, btSoftBody* ) = 0; - - /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfPositionIterations( int iterations ) - { - m_numberOfPositionIterations = iterations; - } - - /** Get the number of velocity constraint solver iterations this solver uses. */ - virtual int getNumberOfPositionIterations() - { - return m_numberOfPositionIterations; - } - - /** Set the number of velocity constraint solver iterations this solver uses. */ - virtual void setNumberOfVelocityIterations( int iterations ) - { - m_numberOfVelocityIterations = iterations; - } - - /** Get the number of velocity constraint solver iterations this solver uses. */ - virtual int getNumberOfVelocityIterations() - { - return m_numberOfVelocityIterations; - } - - /** Return the timescale that the simulation is using */ - float getTimeScale() - { - return m_timeScale; - } - -#if 0 - /** - * Add a collision object to be used by the indicated softbody. - */ - virtual void addCollisionObjectForSoftBody( int clothIdentifier, btCollisionObject *collisionObject ) = 0; -#endif -}; - -/** - * Class to manage movement of data from a solver to a given target. - * This version is abstract. Subclasses will have custom pairings for different combinations. - */ -class btSoftBodySolverOutput -{ -protected: - -public: - btSoftBodySolverOutput() - { - } - - virtual ~btSoftBodySolverOutput() - { - } - - - /** 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; -}; - - -#endif // #ifndef BT_SOFT_BODY_SOLVERS_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp deleted file mode 100644 index 01c148a2c..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftRigidCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "btSoftBody.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -///TODO: include all the shapes that the softbody can collide with -///alternatively, implement special case collision algorithms (just like for rigid collision shapes) - -//#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() -{ - - //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); - - /*if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } - */ - -} - - -#include - -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()) - { - softBody->getSoftBodySolver()->processCollision(softBody, rigidCollisionObjectWrap); - } - - -} - -btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} - - - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.h deleted file mode 100644 index a9b513e36..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ /dev/null @@ -1,75 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_RIGID_COLLISION_ALGORITHM_H -#define BT_SOFT_RIGID_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" - -#include "LinearMath/btVector3.h" -class btSoftBody; - -/// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody -class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm -{ - // bool m_ownManifold; - // btPersistentManifold* m_manifoldPtr; - - btSoftBody* m_softBody; - btCollisionObject* m_rigidCollisionObject; - - ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean - bool m_isSwapped; - -public: - - btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - //we don't add any manifolds - } - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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,true); - } - } - }; - -}; - -#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H - - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.cpp deleted file mode 100644 index 5f3593545..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ /dev/null @@ -1,367 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSoftRigidDynamicsWorld.h" -#include "LinearMath/btQuickprof.h" - -//softbody & helpers -#include "btSoftBody.h" -#include "btSoftBodyHelpers.h" -#include "btSoftBodySolvers.h" -#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) -{ - if( !m_softBodySolver ) - { - void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); - m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); - m_ownsSolver = true; - } - - m_drawFlags = fDrawFlags::Std; - m_drawNodeTree = true; - m_drawFaceTree = false; - m_drawClusterTree = false; - m_sbi.m_broadphase = pairCache; - m_sbi.m_dispatcher = dispatcher; - m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.Reset(); - - m_sbi.air_density = (btScalar)1.2; - m_sbi.water_density = 0; - m_sbi.water_offset = 0; - m_sbi.water_normal = btVector3(0,0,0); - m_sbi.m_gravity.setValue(0,-10,0); - - m_sbi.m_sparsesdf.Initialize(); - - -} - -btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() -{ - if (m_ownsSolver) - { - m_softBodySolver->~btSoftBodySolver(); - btAlignedFree(m_softBodySolver); - } -} - -void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); - { - BT_PROFILE("predictUnconstraintMotionSoftBody"); - m_softBodySolver->predictMotion( timeStep ); - } -} - -void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) -{ - - // Let the solver grab the soft bodies and if necessary optimize for it - m_softBodySolver->optimize( getSoftBodyArray() ); - - if( !m_softBodySolver->checkInitialized() ) - { - btAssert( "Solver initialization failed\n" ); - } - - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); - - ///solve soft bodies constraints - solveSoftBodiesConstraints( timeStep ); - - //self collisions - for ( int i=0;idefaultCollisionHandler(psb); - } - - ///update soft bodies - m_softBodySolver->updateSoftBodies( ); - - // End solver-wise simulation step - // /////////////////////////////// - -} - -void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) -{ - BT_PROFILE("solveSoftConstraints"); - - if(m_softBodies.size()) - { - btSoftBody::solveClusters(m_softBodies); - } - - // Solve constraints solver-wise - m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); - -} - -void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask) -{ - m_softBodies.push_back(body); - - // Set the soft body solver that will deal with this body - // to be the world's solver - body->setSoftBodySolver( m_softBodySolver ); - - btCollisionWorld::addCollisionObject(body, - collisionFilterGroup, - collisionFilterMask); - -} - -void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) -{ - m_softBodies.remove(body); - - btCollisionWorld::removeCollisionObject(body); -} - -void btSoftRigidDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) -{ - btSoftBody* body = btSoftBody::upcast(collisionObject); - if (body) - removeSoftBody(body); - else - btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); -} - -void btSoftRigidDynamicsWorld::debugDrawWorld() -{ - btDiscreteDynamicsWorld::debugDrawWorld(); - - if (getDebugDrawer()) - { - int i; - for ( i=0;im_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) - { - btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); - } - - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); - if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); - if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); - } - } - } -} - - - - -struct btSoftSingleRayCallback : public btBroadphaseRayCallback -{ - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const 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) - { - m_rayFromTrans.setIdentity(); - m_rayFromTrans.setOrigin(m_rayFromWorld); - m_rayToTrans.setIdentity(); - m_rayToTrans.setOrigin(m_rayToWorld); - - btVector3 rayDir = (rayToWorld-rayFromWorld); - - rayDir.normalize (); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - m_signs[0] = m_rayDirectionInverse[0] < 0.0; - m_signs[1] = m_rayDirectionInverse[1] < 0.0; - m_signs[2] = m_rayDirectionInverse[2] < 0.0; - - m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); - - } - - - - virtual bool process(const btBroadphaseProxy* proxy) - { - ///terminate further ray tests, once the closestHitFraction reached zero - if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) - return false; - - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; - - //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) - { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; -#if 0 -#ifdef RECALCULATE_AABB - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); -#else - //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); - const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; - const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; -#endif -#endif - //btScalar hitLambda = m_resultCallback.m_closestHitFraction; - //culling already done by broadphase - //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) - { - m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); - } - } - return true; - } -}; - -void 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); - -#ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); -#else - for (int i=0;igetNumCollisionObjects();i++) - { - rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - -} - - -void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) -{ - if (collisionShape->isSoftBody()) { - btSoftBody* softBody = btSoftBody::upcast(collisionObject); - if (softBody) { - btSoftBody::sRayCast softResult; - if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) - { - - if (softResult.fraction<= resultCallback.m_closestHitFraction) - { - - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = 0; - shapeInfo.m_triangleIndex = softResult.index; - // get the normal - btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - btVector3 normal=-rayDir; - normal.normalize(); - - if (softResult.feature == btSoftBody::eFeature::Face) - { - normal = softBody->m_faces[softResult.index].m_normal; - if (normal.dot(rayDir) > 0) { - // normal always point toward origin of the ray - normal = -normal; - } - } - - btCollisionWorld::LocalRayResult rayResult - (collisionObject, - &shapeInfo, - normal, - softResult.fraction); - bool normalInWorldSpace = true; - resultCallback.addSingleResult(rayResult,normalInWorldSpace); - } - } - } - } - else { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); - } -} - - -void 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); - const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); - } - } - -} - -void btSoftRigidDynamicsWorld::serialize(btSerializer* serializer) -{ - - serializer->startSerialization(); - - serializeDynamicsWorldInfo( serializer); - - serializeSoftBodies(serializer); - - serializeRigidBodies(serializer); - - serializeCollisionObjects(serializer); - - serializer->finishSerialization(); -} - - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.h b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.h deleted file mode 100644 index 3e0efafd6..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H -#define BT_SOFT_RIGID_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" -#include "btSoftBody.h" - -typedef btAlignedObjectArray btSoftBodyArray; - -class btSoftBodySolver; - -class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld -{ - - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; - btSoftBodyWorldInfo m_sbi; - ///Solver classes that encapsulate multiple soft bodies for solving - btSoftBodySolver *m_softBodySolver; - bool m_ownsSolver; - -protected: - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - void solveSoftBodiesConstraints( btScalar timeStep ); - - void serializeSoftBodies(btSerializer* serializer); - -public: - - btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); - - virtual ~btSoftRigidDynamicsWorld(); - - virtual void debugDrawWorld(); - - void addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); - - void removeSoftBody(btSoftBody* body); - - ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - int getDrawFlags() const { return(m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags=f; } - - btSoftBodyWorldInfo& getWorldInfo() - { - return m_sbi; - } - const btSoftBodyWorldInfo& getWorldInfo() const - { - return m_sbi; - } - - virtual btDynamicsWorldType getWorldType() const - { - return BT_SOFT_RIGID_DYNAMICS_WORLD; - } - - btSoftBodyArray& getSoftBodyArray() - { - return m_softBodies; - } - - const btSoftBodyArray& getSoftBodyArray() const - { - return m_softBodies; - } - - - virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; - - /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. - /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. - /// This allows more customization. - static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback); - - virtual void serialize(btSerializer* serializer); - -}; - -#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp deleted file mode 100644 index 72043e69e..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftSoftCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletSoftBody/btSoftBodySolvers.h" -#include "btSoftBody.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -#define USE_PERSISTENT_CONTACTS 1 - -btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* /*obj0*/,const btCollisionObjectWrapper* /*obj1*/) -: btCollisionAlgorithm(ci) -//m_ownManifold(false), -//m_manifoldPtr(mf) -{ -} - -btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() -{ -} - -void btSoftSoftCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - 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*/) -{ - //not yet - return 1.f; -} diff --git a/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.h deleted file mode 100644 index 43b1439cc..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ /dev/null @@ -1,69 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_SOFT_COLLISION_ALGORITHM_H -#define BT_SOFT_SOFT_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" - -class btPersistentManifold; -class btSoftBody; - -///collision detection between two btSoftBody shapes -class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm -{ - 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - 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); - - virtual ~btSoftSoftCollisionAlgorithm(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - 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); - } - }; - -}; - -#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H - - diff --git a/WickedEngine/BULLET/BulletSoftBody/btSparseSDF.h b/WickedEngine/BULLET/BulletSoftBody/btSparseSDF.h deleted file mode 100644 index bcf0c7982..000000000 --- a/WickedEngine/BULLET/BulletSoftBody/btSparseSDF.h +++ /dev/null @@ -1,319 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSparseSdf implementation by Nathanael Presson - -#ifndef BT_SPARSE_SDF_H -#define BT_SPARSE_SDF_H - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" - -// Modified Paul Hsieh hash -template -unsigned int HsiehHash(const void* pdata) -{ - const unsigned short* data=(const unsigned short*)pdata; - unsigned hash=DWORDLEN<<2,tmp; - for(int i=0;i>11; - } - hash^=hash<<3;hash+=hash>>5; - hash^=hash<<4;hash+=hash>>17; - hash^=hash<<25;hash+=hash>>6; - return(hash); -} - -template -struct btSparseSdf -{ - // - // Inner types - // - struct IntFrac - { - int b; - int i; - btScalar f; - }; - struct Cell - { - btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; - int c[3]; - int puid; - unsigned hash; - const btCollisionShape* pclient; - Cell* next; - }; - // - // Fields - // - - 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) - { - //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); - Reset(); - } - // - void Reset() - { - for(int i=0,ni=cells.size();inext; - delete pc; - pc=pn; - } - } - voxelsz =0.25; - puid =0; - ncells =0; - nprobes =1; - nqueries =1; - } - // - void GarbageCollect(int lifetime=256) - { - const int life=puid-lifetime; - for(int i=0;inext; - if(pc->puidnext=pn; else root=pn; - delete pc;pc=pp;--ncells; - } - pp=pc;pc=pn; - } - } - //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... */ - } - // - int RemoveReferences(btCollisionShape* pcs) - { - int refcount=0; - for(int i=0;inext; - if(pc->pclient==pcs) - { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;++refcount; - } - pp=pc;pc=pn; - } - } - return(refcount); - } - // - 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; - ++nqueries; - while(c) - { - ++nprobes; - if( (c->hash==h) && - (c->c[0]==ix.b) && - (c->c[1]==iy.b) && - (c->c[2]==iz.b) && - (c->pclient==shape)) - { break; } - else - { c=c->next; } - } - if(!c) - { - ++nprobes; - ++ncells; - int sz = sizeof(Cell); - if (ncells>m_clampCells) - { - static int numResets=0; - 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; - 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 */ -#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(); -#else - 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); - } - // - 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 btScalar z=voxelsz*k+org.z(); - for(int j=0;j<=CELLSIZE;++j) - { - const btScalar y=voxelsz*j+org.y(); - for(int i=0;i<=CELLSIZE;++i) - { - const btScalar x=voxelsz*i+org.x(); - c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); - } - } - } - } - // - static inline btScalar DistanceToShape(const btVector3& x, - const btCollisionShape* shape) - { - btTransform unit; - unit.setIdentity(); - if(shape->isConvex()) - { - btGjkEpaSolver2::sResults res; - const btConvexShape* csh=static_cast(shape); - return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); - } - return(0); - } - // - 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); - } - // - static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) - { - return(a+(b-a)*t); - } - - - - // - static inline unsigned int Hash(int x,int y,int z,const btCollisionShape* shape) - { - struct btS - { - int x,y,z; - void* p; - }; - - btS myset; - - myset.x=x;myset.y=y;myset.z=z;myset.p=(void*)shape; - const void* ptr = &myset; - - unsigned int result = HsiehHash (ptr); - - - return result; - } -}; - - -#endif //BT_SPARSE_SDF_H diff --git a/WickedEngine/BULLET/CMakeLists.txt b/WickedEngine/BULLET/CMakeLists.txt deleted file mode 100644 index 341f1f3dc..000000000 --- a/WickedEngine/BULLET/CMakeLists.txt +++ /dev/null @@ -1,463 +0,0 @@ -project(BULLET) -include(GNUInstallDirs) - -set(Bullet_HEADERS - ${CMAKE_CURRENT_SOURCE_DIR}/Bullet-C-Api.h - ${CMAKE_CURRENT_SOURCE_DIR}/btBulletCollisionCommon.h - ${CMAKE_CURRENT_SOURCE_DIR}/btBulletDynamicsCommon.h -) -install(FILES ${Bullet_HEADERS} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/") - -set(Bullet_HEADERS_CollisionBroadphaseCollision - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btAxisSweep3.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btDbvt.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btDispatcher.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btQuantizedBvh.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h - ) -install(FILES ${Bullet_HEADERS_CollisionBroadphaseCollision} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/") - -set(Bullet_HEADERS_DynamicsCollisionDispatch - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/SphereTriangleDetector.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btBoxBoxDetector.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCollisionConfiguration.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCollisionDispatcher.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCollisionObject.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCollisionWorld.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btGhostObject.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btManifoldResult.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btSimulationIslandManager.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionDispatch/btUnionFind.h - ) -install(FILES ${Bullet_HEADERS_DynamicsCollisionDispatch} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletCollision/CollisionDispatch/") - -set(Bullet_HEADERS_DynamicsCollisionShapes - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btBox2dShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btBoxShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btCapsuleShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btCollisionMargin.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btCollisionShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btCompoundShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConcaveShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConeShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConvex2dShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConvexHullShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConvexInternalShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConvexPointCloudShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConvexPolyhedron.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConvexShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btCylinderShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btEmptyShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btMaterial.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btMinkowskiSumShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btMultiSphereShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btOptimizedBvh.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btShapeHull.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btSphereShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btStaticPlaneShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btStridingMeshInterface.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTetrahedronShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleBuffer.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleCallback.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleInfoMap.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleMesh.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleMeshShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btTriangleShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/CollisionShapes/btUniformScalingShape.h - ) -install(FILES ${Bullet_HEADERS_DynamicsCollisionShapes} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletCollision/CollisionShapes/") - - -set(Bullet_HEADERS_DynamicsGimpact - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btBoxCollision.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btClipPolygon.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btCompoundFromGimpact.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btContactProcessing.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btGImpactBvh.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btGImpactMassUtil.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btGImpactQuantizedBvh.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btGImpactShape.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btGenericPoolAllocator.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btGeometryOperations.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btQuantization.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/btTriangleShapeEx.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_array.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_basic_geometry_operations.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_bitset.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_box_collision.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_box_set.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_clip_polygon.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_contact.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_geom_types.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_geometry.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_hash_table.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_linear_math.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_math.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_memory.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_radixsort.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/Gimpact/gim_tri_collision.h - ) -install(FILES ${Bullet_HEADERS_DynamicsGimpact} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletCollision/Gimpact/") - - -set(Bullet_HEADERS_DynamicsNarrowPhaseCollision - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btConvexCast.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btPointCollector.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h - ) -install(FILES ${Bullet_HEADERS_DynamicsNarrowPhaseCollision} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/") - - -set(Bullet_HEADERS_DynamicsCharacter - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Character/btCharacterControllerInterface.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Character/btKinematicCharacterController.h - ) -install(FILES ${Bullet_HEADERS_DynamicsCharacter} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletDynamics/Character/") - - -set(Bullet_HEADERS_DynamicsConstraintSolver - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btConstraintSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btContactConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btContactSolverInfo.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btFixedConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btGearConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btHinge2Constraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btHingeConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btJacobianEntry.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btSliderConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btSolverBody.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btSolverConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btTypedConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/ConstraintSolver/btUniversalConstraint.h - ) -install(FILES ${Bullet_HEADERS_DynamicsConstraintSolver} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/") - - -set(Bullet_HEADERS_DynamicsDyn - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Dynamics/btActionInterface.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Dynamics/btDynamicsWorld.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Dynamics/btRigidBody.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h - ) -install(FILES ${Bullet_HEADERS_DynamicsDyn} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletDynamics/Dynamics/") - - -set(Bullet_HEADERS_DynamicsFeatherstone - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBody.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyJointMotor.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyLink.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h - ) -install(FILES ${Bullet_HEADERS_DynamicsFeatherstone} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletDynamics/Featherstone/") - - -set(Bullet_HEADERS_DynamicsMLCPSolvers - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/MLCPSolvers/btDantzigLCP.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/MLCPSolvers/btDantzigSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/MLCPSolvers/btMLCPSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/MLCPSolvers/btPATHSolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h - ) -install(FILES ${Bullet_HEADERS_DynamicsMLCPSolvers} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/") - - -set(Bullet_HEADERS_DynamicsVehicle - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Vehicle/btRaycastVehicle.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Vehicle/btVehicleRaycaster.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletDynamics/Vehicle/btWheelInfo.h - ) -install(FILES ${Bullet_HEADERS_DynamicsVehicle} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletDynamics/Vehicle/") - - -set(Bullet_HEADERS_SoftBody - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btDefaultSoftBodySolver.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBody.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBodyData.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBodyHelpers.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBodyInternals.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBodySolverVertexBuffer.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftBodySolvers.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftRigidCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftRigidDynamicsWorld.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSoftSoftCollisionAlgorithm.h - ${CMAKE_CURRENT_SOURCE_DIR}/BulletSoftBody/btSparseSDF.h - ) -install(FILES ${Bullet_HEADERS_SoftBody} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/BulletSoftBody/") - - -set(Bullet_HEADERS_LinearMath - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btAabbUtil2.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btAlignedAllocator.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btAlignedObjectArray.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btConvexHull.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btConvexHullComputer.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btDefaultMotionState.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btGeometryUtil.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btGrahamScan2dConvexHull.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btHashMap.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btIDebugDraw.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btList.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btMatrix3x3.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btMatrixX.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btMinMax.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btMotionState.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btPolarDecomposition.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btPoolAllocator.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btQuadWord.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btQuaternion.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btQuickprof.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btRandom.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btScalar.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btSerializer.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btStackAlloc.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btTransform.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btTransformUtil.h - ${CMAKE_CURRENT_SOURCE_DIR}/LinearMath/btVector3.h - ) -install(FILES ${Bullet_HEADERS_LinearMath} - DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WickedEngine/BULLET/LinearMath/") - - -add_library(Bullet STATIC - BulletCollision/BroadphaseCollision/btAxisSweep3.cpp - BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp - BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp - BulletCollision/BroadphaseCollision/btDbvt.cpp - BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp - BulletCollision/BroadphaseCollision/btDispatcher.cpp - BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp - BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp - BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp - BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp - BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp - BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp - BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp - BulletCollision/CollisionDispatch/btCollisionObject.cpp - BulletCollision/CollisionDispatch/btCollisionWorld.cpp - BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp - BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp - BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp - BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btGhostObject.cpp - BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp - BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp - BulletCollision/CollisionDispatch/btManifoldResult.cpp - BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp - BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp - BulletCollision/CollisionDispatch/btUnionFind.cpp - BulletCollision/CollisionShapes/btBox2dShape.cpp - BulletCollision/CollisionShapes/btBoxShape.cpp - BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp - BulletCollision/CollisionShapes/btCapsuleShape.cpp - BulletCollision/CollisionShapes/btCollisionShape.cpp - BulletCollision/CollisionShapes/btCompoundShape.cpp - BulletCollision/CollisionShapes/btConcaveShape.cpp - BulletCollision/CollisionShapes/btConeShape.cpp - BulletCollision/CollisionShapes/btConvex2dShape.cpp - BulletCollision/CollisionShapes/btConvexHullShape.cpp - BulletCollision/CollisionShapes/btConvexInternalShape.cpp - BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp - BulletCollision/CollisionShapes/btConvexPolyhedron.cpp - BulletCollision/CollisionShapes/btConvexShape.cpp - BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp - BulletCollision/CollisionShapes/btCylinderShape.cpp - BulletCollision/CollisionShapes/btEmptyShape.cpp - BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp - BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp - BulletCollision/CollisionShapes/btMultiSphereShape.cpp - BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp - BulletCollision/CollisionShapes/btOptimizedBvh.cpp - BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp - BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp - BulletCollision/CollisionShapes/btShapeHull.cpp - BulletCollision/CollisionShapes/btSphereShape.cpp - BulletCollision/CollisionShapes/btStaticPlaneShape.cpp - BulletCollision/CollisionShapes/btStridingMeshInterface.cpp - BulletCollision/CollisionShapes/btTetrahedronShape.cpp - BulletCollision/CollisionShapes/btTriangleBuffer.cpp - BulletCollision/CollisionShapes/btTriangleCallback.cpp - BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp - BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp - BulletCollision/CollisionShapes/btTriangleMesh.cpp - BulletCollision/CollisionShapes/btTriangleMeshShape.cpp - BulletCollision/CollisionShapes/btUniformScalingShape.cpp - BulletCollision/Gimpact/btContactProcessing.cpp - BulletCollision/Gimpact/btGImpactBvh.cpp - BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp - BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp - BulletCollision/Gimpact/btGImpactShape.cpp - BulletCollision/Gimpact/btGenericPoolAllocator.cpp - BulletCollision/Gimpact/btTriangleShapeEx.cpp - BulletCollision/Gimpact/gim_box_set.cpp - BulletCollision/Gimpact/gim_contact.cpp - BulletCollision/Gimpact/gim_memory.cpp - BulletCollision/Gimpact/gim_tri_collision.cpp - BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp - BulletCollision/NarrowPhaseCollision/btConvexCast.cpp - BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp - BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp - BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp - BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp - BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp - BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp - BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp - BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp - BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp - BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp - BulletDynamics/Character/btKinematicCharacterController.cpp - BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp - BulletDynamics/ConstraintSolver/btContactConstraint.cpp - BulletDynamics/ConstraintSolver/btFixedConstraint.cpp - BulletDynamics/ConstraintSolver/btGearConstraint.cpp - BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp - BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp - BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp - BulletDynamics/ConstraintSolver/btHingeConstraint.cpp - BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp - BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp - BulletDynamics/ConstraintSolver/btSliderConstraint.cpp - BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp - BulletDynamics/ConstraintSolver/btTypedConstraint.cpp - BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp - BulletDynamics/Dynamics/Bullet-C-API.cpp - BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp - BulletDynamics/Dynamics/btRigidBody.cpp - BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp - BulletDynamics/Featherstone/btMultiBody.cpp - BulletDynamics/Featherstone/btMultiBodyConstraint.cpp - BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp - BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp - BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp - BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp - BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp - BulletDynamics/MLCPSolvers/btDantzigLCP.cpp - BulletDynamics/MLCPSolvers/btMLCPSolver.cpp - BulletDynamics/Vehicle/btRaycastVehicle.cpp - BulletDynamics/Vehicle/btWheelInfo.cpp - BulletSoftBody/btDefaultSoftBodySolver.cpp - BulletSoftBody/btSoftBody.cpp - BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp - BulletSoftBody/btSoftBodyHelpers.cpp - BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp - BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp - BulletSoftBody/btSoftRigidDynamicsWorld.cpp - BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp - LinearMath/btAlignedAllocator.cpp - LinearMath/btConvexHull.cpp - LinearMath/btConvexHullComputer.cpp - LinearMath/btGeometryUtil.cpp - LinearMath/btPolarDecomposition.cpp - LinearMath/btQuickprof.cpp - LinearMath/btSerializer.cpp - LinearMath/btVector3.cpp - ${Bullet_HEADERS} - ${Bullet_HEADERS_CollisionBroadphaseCollision} - ${Bullet_HEADERS_DynamicsCollisionDispatch} - ${Bullet_HEADERS_DynamicsCollisionShapes} - ${Bullet_HEADERS_DynamicsGimpact} - ${Bullet_HEADERS_DynamicsNarrowPhaseCollision} - ${Bullet_HEADERS_DynamicsCharacter} - ${Bullet_HEADERS_DynamicsConstraintSolver} - ${Bullet_HEADERS_DynamicsDyn} - ${Bullet_HEADERS_DynamicsFeatherstone} - ${Bullet_HEADERS_DynamicsMLCPSolvers} - ${Bullet_HEADERS_DynamicsVehicle} - ${Bullet_HEADERS_SoftBody} - ${Bullet_HEADERS_LinearMath} -) - -target_include_directories(Bullet SYSTEM PUBLIC - $ - $ -) - -set_target_properties(Bullet PROPERTIES - FOLDER "ThirdParty" - POSITION_INDEPENDENT_CODE ${WICKED_DYNAMIC_LIBRARY} -) diff --git a/WickedEngine/BULLET/LinearMath/btAabbUtil2.h b/WickedEngine/BULLET/LinearMath/btAabbUtil2.h deleted file mode 100644 index d2997b4e6..000000000 --- a/WickedEngine/BULLET/LinearMath/btAabbUtil2.h +++ /dev/null @@ -1,232 +0,0 @@ -/* -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 BT_AABB_UTIL2 -#define BT_AABB_UTIL2 - -#include "btTransform.h" -#include "btVector3.h" -#include "btMinMax.h" - - - -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) -{ - bool overlap = true; - overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap; - overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap; - overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap; - return overlap; -} - - -/// conservative test for overlap between two aabbs -SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, - const btVector3 &aabbMin2, const btVector3 &aabbMax2) -{ - bool overlap = true; - overlap = (aabbMin1.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 -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]; - - 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) -{ - 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], - const btVector3 bounds[2], - btScalar& tmin, - btScalar lambda_min, - btScalar lambda_max) -{ - 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(); - 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) ); -} - -SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& aabbMin, - const btVector3& aabbMax, - 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); - if ((sourceOutcode & targetOutcode) == 0x0) - { - btScalar lambda_enter = btScalar(0.0); - btScalar lambda_exit = param; - btVector3 r = target - source; - int i; - btScalar normSign = 1; - btVector3 hitNormal(0,0,0); - int bit=1; - - 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]; - if (lambda_enter <= lambda) - { - lambda_enter = lambda; - hitNormal.setValue(0,0,0); - hitNormal[i] = normSign; - } - } - else if (targetOutcode & bit) - { - btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; - btSetMin(lambda_exit, lambda); - } - bit<<=1; - } - normSign = btScalar(-1.); - } - if (lambda_enter <= lambda_exit) - { - param = lambda_enter; - normal = hitNormal; - return true; - } - } - return false; -} - - - -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 center = t.getOrigin(); - 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) -{ - 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; -} - -#define USE_BANCHLESS 1 -#ifdef USE_BANCHLESS - //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) - SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) - { - return static_cast(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) - & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) - & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), - 1, 0)); - } -#else - SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) - { - bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } -#endif //USE_BANCHLESS - -#endif //BT_AABB_UTIL2 - - diff --git a/WickedEngine/BULLET/LinearMath/btAlignedAllocator.cpp b/WickedEngine/BULLET/LinearMath/btAlignedAllocator.cpp deleted file mode 100644 index a65296c6a..000000000 --- a/WickedEngine/BULLET/LinearMath/btAlignedAllocator.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btAlignedAllocator.h" - -int gNumAlignedAllocs = 0; -int gNumAlignedFree = 0; -int gTotalBytesAlignedAllocs = 0;//detect memory leaks - -static void *btAllocDefault(size_t size) -{ - return malloc(size); -} - -static void btFreeDefault(void *ptr) -{ - free(ptr); -} - -static btAllocFunc *sAllocFunc = btAllocDefault; -static btFreeFunc *sFreeFunc = btFreeDefault; - - - -#if defined (BT_HAS_ALIGNED_ALLOCATOR) -#include -static void *btAlignedAllocDefault(size_t size, int alignment) -{ - return _aligned_malloc(size, (size_t)alignment); -} - -static void btAlignedFreeDefault(void *ptr) -{ - _aligned_free(ptr); -} -#elif defined(__CELLOS_LV2__) -#include - -static inline void *btAlignedAllocDefault(size_t size, int alignment) -{ - return memalign(alignment, size); -} - -static inline void btAlignedFreeDefault(void *ptr) -{ - free(ptr); -} -#else - - - - - -static inline void *btAlignedAllocDefault(size_t size, int alignment) -{ - void *ret; - char *real; - 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; - - if (ptr) { - real = *((void **)(ptr)-1); - sFreeFunc(real); - } -} -#endif - - -static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault; -static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault; - -void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc) -{ - sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault; - sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault; -} - -void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc) -{ - sAllocFunc = allocFunc ? allocFunc : btAllocDefault; - sFreeFunc = freeFunc ? freeFunc : btFreeDefault; -} - -#ifdef BT_DEBUG_MEMORY_ALLOCATIONS -//this generic allocator provides the total allocated number of bytes -#include - -void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename) -{ - void *ret; - char *real; - - gTotalBytesAlignedAllocs += size; - gNumAlignedAllocs++; - - - real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1)); - if (real) { - ret = (void*) btAlignPointer(real + 2*sizeof(void *), alignment); - *((void **)(ret)-1) = (void *)(real); - *((int*)(ret)-2) = size; - - } else { - ret = (void *)(real);//?? - } - - printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size); - - int* ptr = (int*)ret; - *ptr = 12; - return (ret); -} - -void btAlignedFreeInternal (void* ptr,int line,char* filename) -{ - - void* real; - gNumAlignedFree++; - - if (ptr) { - real = *((void **)(ptr)-1); - int size = *((int*)(ptr)-2); - gTotalBytesAlignedAllocs -= size; - - printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size); - - sFreeFunc(real); - } else - { - printf("NULL ptr\n"); - } -} - -#else //BT_DEBUG_MEMORY_ALLOCATIONS - -void* btAlignedAllocInternal (size_t size, int alignment) -{ - gNumAlignedAllocs++; - void* ptr; - ptr = sAlignedAllocFunc(size, alignment); -// printf("btAlignedAllocInternal %d, %x\n",size,ptr); - return ptr; -} - -void btAlignedFreeInternal (void* ptr) -{ - if (!ptr) - { - return; - } - - gNumAlignedFree++; -// printf("btAlignedFreeInternal %x\n",ptr); - sAlignedFreeFunc(ptr); -} - -#endif //BT_DEBUG_MEMORY_ALLOCATIONS - diff --git a/WickedEngine/BULLET/LinearMath/btAlignedAllocator.h b/WickedEngine/BULLET/LinearMath/btAlignedAllocator.h deleted file mode 100644 index f168f3c66..000000000 --- a/WickedEngine/BULLET/LinearMath/btAlignedAllocator.h +++ /dev/null @@ -1,107 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_ALIGNED_ALLOCATOR -#define BT_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 "btScalar.h" -//#define BT_DEBUG_MEMORY_ALLOCATIONS 1 -#ifdef BT_DEBUG_MEMORY_ALLOCATIONS - -#define btAlignedAlloc(a,b) \ - btAlignedAllocInternal(a,b,__LINE__,__FILE__) - -#define btAlignedFree(ptr) \ - btAlignedFreeInternal(ptr,__LINE__,__FILE__) - -void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename); - -void btAlignedFreeInternal (void* ptr,int line,char* filename); - -#else - void* btAlignedAllocInternal (size_t size, int alignment); - void btAlignedFreeInternal (void* ptr); - - #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment) - #define btAlignedFree(ptr) btAlignedFreeInternal(ptr) - -#endif -typedef int size_type; - -typedef void *(btAlignedAllocFunc)(size_t size, int alignment); -typedef void (btAlignedFreeFunc)(void *memblock); -typedef void *(btAllocFunc)(size_t size); -typedef void (btFreeFunc)(void *memblock); - -///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom -void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc); -///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it. -void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); - - -///The btAlignedAllocator is a portable class for aligned memory allocations. -///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned. -template < typename T , unsigned Alignment > -class btAlignedAllocator { - - typedef btAlignedAllocator< T , Alignment > self_type; - -public: - - //just going down a list: - btAlignedAllocator() {} - /* - btAlignedAllocator( const self_type & ) {} - */ - - template < typename Other > - btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {} - - typedef const T* const_pointer; - typedef const T& const_reference; - typedef T* pointer; - typedef T& reference; - typedef T value_type; - - pointer address ( reference ref ) const { return &ref; } - const_pointer address ( const_reference ref ) const { return &ref; } - pointer allocate ( size_type n , const_pointer * hint = 0 ) { - (void)hint; - return reinterpret_cast< pointer >(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 destroy ( pointer ptr ) { ptr->~value_type(); } - - - template < typename O > struct rebind { - typedef btAlignedAllocator< O , Alignment > other; - }; - template < typename O > - self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; } - - friend bool operator==( const self_type & , const self_type & ) { return true; } -}; - - - -#endif //BT_ALIGNED_ALLOCATOR - diff --git a/WickedEngine/BULLET/LinearMath/btAlignedObjectArray.h b/WickedEngine/BULLET/LinearMath/btAlignedObjectArray.h deleted file mode 100644 index 24e59ab65..000000000 --- a/WickedEngine/BULLET/LinearMath/btAlignedObjectArray.h +++ /dev/null @@ -1,511 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_OBJECT_ARRAY__ -#define BT_OBJECT_ARRAY__ - -#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 -///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors -///You can enable BT_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 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 - -#ifdef BT_USE_MEMCPY -#include -#include -#endif //BT_USE_MEMCPY - -#ifdef BT_USE_PLACEMENT_NEW -#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 -class btAlignedObjectArray -{ - btAlignedAllocator m_allocator; - - int m_size; - int m_capacity; - T* m_data; - //PCK: added this line - bool m_ownsMemory; - -#ifdef BT_ALLOW_ARRAY_COPY_OPERATOR -public: - SIMD_FORCE_INLINE btAlignedObjectArray& operator=(const btAlignedObjectArray &other) - { - copyFromArray(other); - return *this; - } -#else//BT_ALLOW_ARRAY_COPY_OPERATOR -private: - 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=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) - { - int curSize = size(); - - if (newsize < curSize) - { - } else - { - if (newsize > size()) - { - reserve(newsize); - } - //leave this uninitialized - } - m_size = newsize; - } - - SIMD_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 BT_USE_PLACEMENT_NEW - for (int i=curSize;i - void quickSortInternal(const L& CompareFunc,int lo, int hi) - { - // lo is the lower index, hi is the upper index - // of the region of array a that is to be sorted - int i=lo, j=hi; - T x=m_data[(lo+hi)/2]; - - // partition - do - { - while (CompareFunc(m_data[i],x)) - i++; - while (CompareFunc(x,m_data[j])) - j--; - if (i<=j) - { - swap(i,j); - i++; j--; - } - } while (i<=j); - - // recursion - if (lo - void quickSort(const L& CompareFunc) - { - //don't sort 0 or 1 elements - if (size()>1) - { - quickSortInternal(CompareFunc,0,size()-1); - } - } - - - ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ - template - void downHeap(T *pArr, int k, int n, const L& CompareFunc) - { - /* PRE: a[k+1..N] is a heap */ - /* POST: a[k..N] is a heap */ - - T temp = pArr[k - 1]; - /* k has child(s) */ - while (k <= n/2) - { - int child = 2*k; - - if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child])) - { - child++; - } - /* pick larger child */ - if (CompareFunc(temp , pArr[child - 1])) - { - /* move child up */ - pArr[k - 1] = pArr[child - 1]; - k = child; - } - else - { - break; - } - } - pArr[k - 1] = temp; - } /*downHeap*/ - - void swap(int index0,int index1) - { -#ifdef 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)); -#else - 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) - { - /* sort a[0..N-1], N.B. 0 to N-1 */ - int k; - int n = m_size; - for (k = n/2; k > 0; k--) - { - downHeap(m_data, k, n, CompareFunc); - } - - /* a[1..N] is now a heap */ - while ( n>=1 ) - { - swap(0,n-1); /* largest of a[0..n-1] */ - - - n = n - 1; - /* restore a[1..i-1] heap */ - downHeap(m_data, 1, n, CompareFunc); - } - } - - ///non-recursive binary search, assumes sorted array - int findBinarySearch(const T& key) const - { - int first = 0; - int last = size()-1; - - //assume sorted array - while (first <= last) { - int mid = (first + last) / 2; // compute mid point. - if (key > m_data[mid]) - first = mid + 1; // repeat search in top half. - else if (key < m_data[mid]) - last = mid - 1; // repeat search in bottom half. - else - return mid; // found it. return position ///// - } - return size(); // failed to find key - } - - - int findLinearSearch(const T& key) const - { - int index=size(); - int i; - - for (i=0;i - -#include "btConvexHull.h" -#include "btAlignedObjectArray.h" -#include "btMinMax.h" -#include "btVector3.h" - - - - - -//---------------------------------- - -class int3 -{ -public: - int x,y,z; - int3(){}; - int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} - const int& operator[](int i) const {return (&x)[i];} - int& operator[](int i) {return (&x)[i];} -}; - - -//------- btPlane ---------- - - -inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} -inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); } -inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); } - - -//--------- Utility Functions ------ - -btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); -btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); - -btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2); -btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) -{ - btVector3 N1 = p0.normal; - btVector3 N2 = p1.normal; - btVector3 N3 = p2.normal; - - btVector3 n2n3; n2n3 = N2.cross(N3); - btVector3 n3n1; n3n1 = N3.cross(N1); - btVector3 n1n2; n1n2 = N1.cross(N2); - - btScalar quotient = (N1.dot(n2n3)); - - btAssert(btFabs(quotient) > btScalar(0.000001)); - - quotient = btScalar(-1.) / quotient; - n2n3 *= p0.dist; - n3n1 *= p1.dist; - n1n2 *= p2.dist; - btVector3 potentialVertex = n2n3; - potentialVertex += n3n1; - potentialVertex += n1n2; - potentialVertex *= quotient; - - btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); - return result; - -} - -btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); -btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); -btVector3 NormalOf(const btVector3 *vert, const int n); - - -btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) -{ - // returns the point where the line p0-p1 intersects the plane n&d - static btVector3 dif; - dif = p1-p0; - btScalar dn= btDot(plane.normal,dif); - btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn; - return p0 + (dif*t); -} - -btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) -{ - return point - plane.normal * (btDot(point,plane.normal)+plane.dist); -} - -btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) -{ - // return the normal of the triangle - // inscribed by v0, v1, and v2 - btVector3 cp=btCross(v1-v0,v2-v1); - btScalar m=cp.length(); - if(m==0) return btVector3(1,0,0); - return cp*(btScalar(1.0)/m); -} - - -btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) -{ - static btVector3 cp; - cp = btCross(udir,vdir).normalized(); - - btScalar distu = -btDot(cp,ustart); - btScalar distv = -btDot(cp,vstart); - btScalar dist = (btScalar)fabs(distu-distv); - if(upoint) - { - btPlane plane; - plane.normal = btCross(vdir,cp).normalized(); - plane.dist = -btDot(plane.normal,vstart); - *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); - } - if(vpoint) - { - btPlane plane; - plane.normal = btCross(udir,cp).normalized(); - plane.dist = -btDot(plane.normal,ustart); - *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); - } - return dist; -} - - - - - - - -#define COPLANAR (0) -#define UNDER (1) -#define OVER (2) -#define SPLIT (OVER|UNDER) -#define PAPERWIDTH (btScalar(0.001)) - -btScalar planetestepsilon = PAPERWIDTH; - - - -typedef ConvexH::HalfEdge HalfEdge; - -ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) -{ - vertices.resize(vertices_size); - edges.resize(edges_size); - facets.resize(facets_size); -} - - -int PlaneTest(const btPlane &p, const btVector3 &v); -int PlaneTest(const btPlane &p, const btVector3 &v) { - btScalar a = btDot(v,p.normal)+p.dist; - int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); - return flag; -} - -int SplitTest(ConvexH &convex,const btPlane &plane); -int SplitTest(ConvexH &convex,const btPlane &plane) { - int flag=0; - for(int i=0;i -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; - } - btAssert(m!=-1); - return m; -} - -btVector3 orth(const btVector3 &v); -btVector3 orth(const btVector3 &v) -{ - btVector3 a=btCross(v,btVector3(0,0,1)); - btVector3 b=btCross(v,btVector3(0,1,0)); - if (a.length() > b.length()) - { - return a.normalized(); - } else { - return b.normalized(); - } -} - - -template -int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &allow) -{ - int m=-1; - while(m==-1) - { - m = maxdirfiltered(p,count,dir,allow); - if(allow[m]==3) return m; - T u = orth(dir); - T v = btCross(u,dir); - int ma=-1; - for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) - { - btScalar s = btSin(SIMD_RADS_PER_DEG*(x)); - btScalar c = btCos(SIMD_RADS_PER_DEG*(x)); - int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); - if(ma==m && mb==m) - { - allow[m]=3; - return m; - } - if(ma!=-1 && ma!=mb) // Yuck - this is really ugly - { - int mc = ma; - for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) - { - btScalar s = btSin(SIMD_RADS_PER_DEG*(xx)); - btScalar c = btCos(SIMD_RADS_PER_DEG*(xx)); - int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); - if(mc==m && md==m) - { - allow[m]=3; - return m; - } - mc=md; - } - } - ma=mb; - } - allow[m]=0; - m=-1; - } - btAssert(0); - return m; -} - - - - -int operator ==(const int3 &a,const int3 &b); -int operator ==(const int3 &a,const int3 &b) -{ - for(int i=0;i<3;i++) - { - if(a[i]!=b[i]) return 0; - } - return 1; -} - - -int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); -int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) -{ - btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); - return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? -} -int hasedge(const int3 &t, int a,int b); -int hasedge(const int3 &t, int a,int b) -{ - for(int i=0;i<3;i++) - { - int i1= (i+1)%3; - if(t[i]==a && t[i1]==b) return 1; - } - return 0; -} -int hasvert(const int3 &t, int v); -int hasvert(const int3 &t, int v) -{ - return (t[0]==v || t[1]==v || t[2]==v) ; -} -int shareedge(const int3 &a,const int3 &b); -int shareedge(const int3 &a,const int3 &b) -{ - int i; - for(i=0;i<3;i++) - { - int i1= (i+1)%3; - if(hasedge(a,b[i1],b[i])) return 1; - } - return 0; -} - -class btHullTriangle; - - - -class btHullTriangle : public int3 -{ -public: - int3 n; - int id; - int vmax; - btScalar rise; - btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) - { - vmax=-1; - rise = btScalar(0.0); - } - ~btHullTriangle() - { - } - int &neib(int a,int b); -}; - - -int &btHullTriangle::neib(int a,int b) -{ - static int er=-1; - int i; - for(i=0;i<3;i++) - { - int i1=(i+1)%3; - int i2=(i+2)%3; - if((*this)[i]==a && (*this)[i1]==b) return n[i2]; - if((*this)[i]==b && (*this)[i1]==a) return n[i2]; - } - btAssert(0); - return er; -} -void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) -{ - int i; - for(i=0;i<3;i++) - { - int i1=(i+1)%3; - int i2=(i+2)%3; - int a = (*s)[i1]; - int b = (*s)[i2]; - btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); - btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); - m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); - m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); - } -} - -void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) -{ - b2bfix(s,t); - deAllocateTriangle(s); - - deAllocateTriangle(t); -} - -void HullLibrary::checkit(btHullTriangle *t) -{ - (void)t; - - int i; - btAssert(m_tris[t->id]==t); - for(i=0;i<3;i++) - { - int i1=(i+1)%3; - int i2=(i+2)%3; - int a = (*t)[i1]; - int b = (*t)[i2]; - - // release compile fix - (void)i1; - (void)i2; - (void)a; - (void)b; - - btAssert(a!=b); - btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); - } -} - -btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) -{ - void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); - btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); - tr->id = m_tris.size(); - m_tris.push_back(tr); - - return tr; -} - -void HullLibrary::deAllocateTriangle(btHullTriangle* tri) -{ - btAssert(m_tris[tri->id]==tri); - m_tris[tri->id]=NULL; - tri->~btHullTriangle(); - btAlignedFree(tri); -} - - -void HullLibrary::extrude(btHullTriangle *t0,int v) -{ - int3 t= *t0; - int n = m_tris.size(); - btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); - ta->n = int3(t0->n[0],n+1,n+2); - m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; - btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); - tb->n = int3(t0->n[1],n+2,n+0); - m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; - btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); - tc->n = int3(t0->n[2],n+0,n+1); - m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; - checkit(ta); - checkit(tb); - checkit(tc); - if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); - if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); - if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); - deAllocateTriangle(t0); - -} - -btHullTriangle* HullLibrary::extrudable(btScalar epsilon) -{ - int i; - btHullTriangle *t=NULL; - for(i=0;iriserise)) - { - t = m_tris[i]; - } - } - return (t->rise >epsilon)?t:NULL ; -} - - - - -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]); - if (basis[1].length() > basis[2].length()) - { - basis[1].normalize(); - } else { - basis[1] = basis[2]; - basis[1].normalize (); - } - int p2 = maxdirsterid(verts,verts_count,basis[1],allow); - if(p2 == p0 || p2 == p1) - { - p2 = maxdirsterid(verts,verts_count,-basis[1],allow); - } - if(p2 == p0 || p2 == p1) - return int4(-1,-1,-1,-1); - basis[1] = verts[p2] - verts[p0]; - basis[2] = btCross(basis[1],basis[0]).normalized(); - int p3 = maxdirsterid(verts,verts_count,basis[2],allow); - if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); - if(p3==p0||p3==p1||p3==p2) - return int4(-1,-1,-1,-1); - btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); - if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(p2,p3);} - return int4(p0,p1,p2,p3); -} - -int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) -{ - if(verts_count <4) return 0; - if(vlimit==0) vlimit=1000000000; - int j; - btVector3 bmin(*verts),bmax(*verts); - btAlignedObjectArray 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); - - 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]]); - } - btHullTriangle *te; - vlimit-=4; - while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) - { - //int3 ti=*te; - int v=te->vmax; - btAssert(v != -1); - btAssert(!isextreme[v]); // wtf we've already done this vertex - isextreme[v]=1; - //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already - j=m_tris.size(); - while(j--) { - if(!m_tris[j]) continue; - int3 t=*m_tris[j]; - if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) - { - extrude(m_tris[j],v); - } - } - // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle - j=m_tris.size(); - while(j--) - { - if(!m_tris[j]) continue; - if(!hasvert(*m_tris[j],v)) break; - int3 nt=*m_tris[j]; - if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) - { - btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; - btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->idvmax>=0) break; - btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); - t->vmax = maxdirsterid(verts,verts_count,n,allow); - if(isextreme[t->vmax]) - { - t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. - } - else - { - t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); - } - } - vlimit --; - } - return 1; -} - -int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) -{ - int rc=calchullgen(verts,verts_count, vlimit) ; - if(!rc) return 0; - btAlignedObjectArray ts; - int i; - - for(i=0;i(ts[i]); - } - m_tris.resize(0); - - return 1; -} - - - - - -bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) -{ - - int tris_count; - int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast(vlimit) ); - if(!ret) return false; - result.mIndexCount = (unsigned int) (tris_count*3); - result.mFaceCount = (unsigned int) tris_count; - result.mVertices = (btVector3*) vertices; - result.mVcount = (unsigned int) vcount; - return true; - -} - - -void ReleaseHull(PHullResult &result); -void ReleaseHull(PHullResult &result) -{ - if ( result.m_Indices.size() ) - { - result.m_Indices.clear(); - } - - result.mVcount = 0; - result.mIndexCount = 0; - result.mVertices = 0; -} - - -//********************************************************************* -//********************************************************************* -//******** HullLib header -//********************************************************************* -//********************************************************************* - -//********************************************************************* -//********************************************************************* -//******** HullLib implementation -//********************************************************************* -//********************************************************************* - -HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request - HullResult &result) // contains the resulst -{ - HullError ret = QE_FAIL; - - - PHullResult hr; - - unsigned int vcount = desc.mVcount; - if ( vcount < 8 ) vcount = 8; - - btAlignedObjectArray vertexSource; - vertexSource.resize(static_cast(vcount)); - - btVector3 scale; - - unsigned int ovcount; - - bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! - - if ( ok ) - { - - -// if ( 1 ) // scale vertices back to their original size. - { - for (unsigned int i=0; i(i)]; - v[0]*=scale[0]; - v[1]*=scale[1]; - v[2]*=scale[2]; - } - } - - ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); - - if ( ok ) - { - - // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. - btAlignedObjectArray vertexScratch; - vertexScratch.resize(static_cast(hr.mVcount)); - - BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); - - ret = QE_OK; - - if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! - { - result.mPolygons = false; - result.mNumOutputVertices = ovcount; - result.m_OutputVertices.resize(static_cast(ovcount)); - 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 ); - - if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) - { - - const unsigned int *source = &hr.m_Indices[0]; - unsigned int *dest = &result.m_Indices[0]; - - for (unsigned int i=0; i(ovcount)); - 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 ); - -// 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]; - } - } - } - - btScalar dx = bmax[0] - bmin[0]; - btScalar dy = bmax[1] - bmin[1]; - btScalar dz = bmax[2] - bmin[2]; - - btVector3 center; - - center[0] = dx*btScalar(0.5) + bmin[0]; - center[1] = dy*btScalar(0.5) + bmin[1]; - center[2] = dz*btScalar(0.5) + bmin[2]; - - if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) - { - - btScalar len = FLT_MAX; - - if ( dx > EPSILON && dx < len ) len = dx; - if ( dy > EPSILON && dy < len ) len = dy; - if ( dz > EPSILON && dz < len ) len = dz; - - if ( len == FLT_MAX ) - { - dx = dy = dz = btScalar(0.01); // one centimeter - } - else - { - if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * btScalar(0.05); - if ( dz < EPSILON ) dz = len * btScalar(0.05); - } - - btScalar x1 = center[0] - dx; - btScalar x2 = center[0] + dx; - - btScalar y1 = center[1] - dy; - btScalar y2 = center[1] + dy; - - btScalar z1 = center[2] - dz; - btScalar z2 = center[2] + dz; - - addPoint(vcount,vertices,x1,y1,z1); - addPoint(vcount,vertices,x2,y1,z1); - addPoint(vcount,vertices,x2,y2,z1); - addPoint(vcount,vertices,x1,y2,z1); - addPoint(vcount,vertices,x1,y1,z2); - addPoint(vcount,vertices,x2,y1,z2); - addPoint(vcount,vertices,x2,y2,z2); - addPoint(vcount,vertices,x1,y2,z2); - - return true; // return cube - - - } - else - { - if ( scale ) - { - scale[0] = dx; - scale[1] = dy; - scale[2] = dz; - - recip[0] = 1 / dx; - recip[1] = 1 / dy; - recip[2] = 1 / dz; - - center[0]*=recip[0]; - center[1]*=recip[1]; - center[2]*=recip[2]; - - } - - } - - - - vtx = (const char *) svertices; - - for (unsigned int i=0; igetX(); - btScalar py = p->getY(); - btScalar pz = p->getZ(); - - if ( scale ) - { - px = px*recip[0]; // normalize - py = py*recip[1]; // normalize - pz = pz*recip[2]; // normalize - } - -// if ( 1 ) - { - unsigned int j; - - for (j=0; j dist2 ) - { - v[0] = px; - v[1] = py; - v[2] = pz; - - } - - break; - } - } - - if ( j == vcount ) - { - btVector3& dest = vertices[vcount]; - dest[0] = px; - dest[1] = py; - dest[2] = pz; - vcount++; - } - m_vertexIndexMapping.push_back(j); - } - } - - // ok..now make sure we didn't prune so many vertices it is now invalid. -// if ( 1 ) - { - btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; - btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; - - for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; - } - } - - btScalar dx = bmax[0] - bmin[0]; - btScalar dy = bmax[1] - bmin[1]; - btScalar dz = bmax[2] - bmin[2]; - - if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) - { - btScalar cx = dx*btScalar(0.5) + bmin[0]; - btScalar cy = dy*btScalar(0.5) + bmin[1]; - btScalar cz = dz*btScalar(0.5) + bmin[2]; - - btScalar len = FLT_MAX; - - if ( dx >= EPSILON && dx < len ) len = dx; - if ( dy >= EPSILON && dy < len ) len = dy; - if ( dz >= EPSILON && dz < len ) len = dz; - - if ( len == FLT_MAX ) - { - dx = dy = dz = btScalar(0.01); // one centimeter - } - else - { - if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * btScalar(0.05); - if ( dz < EPSILON ) dz = len * btScalar(0.05); - } - - btScalar x1 = cx - dx; - btScalar x2 = cx + dx; - - btScalar y1 = cy - dy; - btScalar y2 = cy + dy; - - btScalar z1 = cz - dz; - btScalar z2 = cz + dz; - - vcount = 0; // add box - - addPoint(vcount,vertices,x1,y1,z1); - addPoint(vcount,vertices,x2,y1,z1); - addPoint(vcount,vertices,x2,y2,z1); - addPoint(vcount,vertices,x1,y2,z1); - addPoint(vcount,vertices,x1,y1,z2); - addPoint(vcount,vertices,x2,y1,z2); - addPoint(vcount,vertices,x2,y2,z2); - addPoint(vcount,vertices,x1,y2,z2); - - return true; - } - } - - return true; -} - -void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) -{ - btAlignedObjectArraytmpIndices; - tmpIndices.resize(m_vertexIndexMapping.size()); - int i; - - for (i=0;i(vcount)); - memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); - - ocount = 0; - - for (i=0; i= 0 && v < vcount ); - - if ( usedIndices[static_cast(v)] ) // if already remapped - { - indices[i] = usedIndices[static_cast(v)]-1; // index to new array - } - else - { - - indices[i] = ocount; // new index mapping - - overts[ocount][0] = verts[v][0]; // copy old vert to new vert array - overts[ocount][1] = verts[v][1]; - overts[ocount][2] = verts[v][2]; - - for (int k=0;k=0 && ocount <= vcount ); - - usedIndices[static_cast(v)] = ocount; // assign new index remapping - - - } - } - - -} diff --git a/WickedEngine/BULLET/LinearMath/btConvexHull.h b/WickedEngine/BULLET/LinearMath/btConvexHull.h deleted file mode 100644 index 69c52bc6f..000000000 --- a/WickedEngine/BULLET/LinearMath/btConvexHull.h +++ /dev/null @@ -1,241 +0,0 @@ - -/* -Stan Melax Convex Hull Computation -Copyright (c) 2008 Stan Melax http://www.melax.com/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///includes modifications/improvements by John Ratcliff, see BringOutYourDead below. - -#ifndef BT_CD_HULL_H -#define BT_CD_HULL_H - -#include "btVector3.h" -#include "btAlignedObjectArray.h" - -typedef btAlignedObjectArray TUIntArray; - -class HullResult -{ -public: - HullResult(void) - { - mPolygons = true; - mNumOutputVertices = 0; - 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. - -// 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 -}; - - -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; - }; - - HullDesc(HullFlag flag, - 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; - } - - bool HasHullFlag(HullFlag flag) const - { - if ( mFlags & flag ) return true; - return false; - } - - void SetHullFlag(HullFlag flag) - { - mFlags|=flag; - } - - void ClearHullFlag(HullFlag 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; -}; - -enum HullError -{ - 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){} - -}; - - - -class ConvexH -{ - public: - class HalfEdge - { - 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){} - }; - ConvexH() - { - } - ~ConvexH() - { - } - btAlignedObjectArray vertices; - btAlignedObjectArray edges; - btAlignedObjectArray facets; - ConvexH(int vertices_size,int edges_size,int facets_size); -}; - - -class int4 -{ -public: - 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];} -}; - -class PHullResult -{ -public: - - PHullResult(void) - { - mVcount = 0; - mIndexCount = 0; - mFaceCount = 0; - mVertices = 0; - } - - unsigned int mVcount; - unsigned int mIndexCount; - unsigned int mFaceCount; - 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. - -private: - - 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); - - void removeb2b(btHullTriangle* s,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 calchullgen(btVector3 *verts,int verts_count, int vlimit); - - int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow); - - class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); - - 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'. - //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); - - 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); -}; - - -#endif //BT_CD_HULL_H - diff --git a/WickedEngine/BULLET/LinearMath/btConvexHullComputer.cpp b/WickedEngine/BULLET/LinearMath/btConvexHullComputer.cpp deleted file mode 100644 index d58ac955f..000000000 --- a/WickedEngine/BULLET/LinearMath/btConvexHullComputer.cpp +++ /dev/null @@ -1,2755 +0,0 @@ -/* -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 "btConvexHullComputer.h" -#include "btAlignedObjectArray.h" -#include "btMinMax.h" -#include "btVector3.h" - -#ifdef __GNUC__ - #include -#elif defined(_MSC_VER) - 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; -#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 btConvexHullInternal -{ - public: - - class Point64 - { - 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); - - 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; - 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; - 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" ); - return result; - -#else - bool negative = a < 0; - if (negative) - { - a = -a; - } - if (b < 0) - { - negative = !negative; - b = -b; - } - DMul::mul((uint64_t) a, (uint64_t) b, result.low, result.high); - return negative ? -result : result; -#endif -} - -btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_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 btConvexHullInternal::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; - 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; - -#else - - return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator)); - -#endif -} - -int btConvexHullInternal::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 * (int64_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 btConvexHullInternal::Rational128::compare(int64_t b) const -{ - if (isInt64) - { - int64_t a = sign * (int64_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; -} - - -btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) -{ - btAssert(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 btConvexHullInternal::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)) - { - btAssert(v0->point.z < v1->point.z); - Vertex* v1p = v1->prev; - if (v1p == v1) - { - c0 = v0; - if (v1->edges) - { - btAssert(v1->edges->next == v1->edges); - v1 = v1->edges->target; - btAssert(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; - int32_t sign = 1; - - for (int side = 0; side <= 1; side++) - { - int32_t dx = (v1->point.x - v0->point.x) * sign; - if (dx > 0) - { - while (true) - { - int32_t dy = v1->point.y - v0->point.y; - - Vertex* w0 = side ? v0->next : v0->prev; - if (w0 != v0) - { - int32_t dx0 = (w0->point.x - v0->point.x) * sign; - int32_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) - { - int32_t dx1 = (w1->point.x - v1->point.x) * sign; - int32_t dy1 = w1->point.y - v1->point.y; - int32_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) - { - int32_t dy = v1->point.y - v0->point.y; - - Vertex* w1 = side ? v1->prev : v1->next; - if (w1 != v1) - { - int32_t dx1 = (w1->point.x - v1->point.x) * sign; - int32_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) - { - int32_t dx0 = (w0->point.x - v0->point.x) * sign; - int32_t dy0 = w0->point.y - v0->point.y; - int32_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 - { - int32_t x = v0->point.x; - int32_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; - - int32_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 btConvexHullInternal::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) - { - int32_t dx = v->point.x - w->point.x; - int32_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; - } - btAssert(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 - printf("\n\nMerge\n"); - result.print(); - hull1.print(); -#endif - merge(result, hull1); -#ifdef DEBUG_CONVEX_HULL - printf("\n Result\n"); - result.print(); -#endif -} - -#ifdef DEBUG_CONVEX_HULL -void btConvexHullInternal::IntermediateHull::print() -{ - printf(" Hull\n"); - for (Vertex* v = minXy; v; ) - { - printf(" "); - v->print(); - if (v == maxXy) - { - printf(" maxXy"); - } - if (v == minYx) - { - printf(" minYx"); - } - if (v == maxYx) - { - printf(" maxYx"); - } - if (v->next->prev != v) - { - printf(" Inconsistency"); - } - printf("\n"); - v = v->next; - if (v == minXy) - { - break; - } - } - if (minXy) - { - minXy->copy = (minXy->copy == -1) ? -2 : -1; - minXy->printGraph(); - } -} - -void btConvexHullInternal::Vertex::printGraph() -{ - print(); - printf("\nEdges\n"); - Edge* e = edges; - if (e) - { - do - { - e->print(); - printf("\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 - -btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t) -{ - btAssert(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); - btAssert(!m.isZero()); - int64_t dot = n.dot(m); - btAssert(dot != 0); - return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; - } - return COUNTER_CLOCKWISE; - } - else if (prev->prev == next) - { - return CLOCKWISE; - } - else - { - return NONE; - } -} - -btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot) -{ - Edge* minEdge = NULL; - -#ifdef DEBUG_CONVEX_HULL - printf("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 - printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN()); - e->print(); -#endif - if (cot.isNaN()) - { - btAssert(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 - printf("\n"); -#endif - } - e = e->next; - } while (e != start->edges); - } - return minEdge; -} - -void btConvexHullInternal::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); - int64_t dist = c0->point.dot(normal); - 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 - - int64_t maxDot0 = et0.dot(perp); - if (e0) - { - while (e0->target != stop0) - { - Edge* e = e0->reverse->prev; - if (e->target->point.dot(normal) < dist) - { - break; - } - btAssert(e->target->point.dot(normal) == dist); - if (e->copy == mergeStamp) - { - break; - } - int64_t dot = e->target->point.dot(perp); - if (dot <= maxDot0) - { - break; - } - maxDot0 = dot; - e0 = e; - et0 = e->target->point; - } - } - - int64_t maxDot1 = et1.dot(perp); - if (e1) - { - while (e1->target != stop1) - { - Edge* e = e1->reverse->next; - if (e->target->point.dot(normal) < dist) - { - break; - } - btAssert(e->target->point.dot(normal) == dist); - if (e->copy == mergeStamp) - { - break; - } - int64_t dot = e->target->point.dot(perp); - if (dot <= maxDot1) - { - break; - } - maxDot1 = dot; - e1 = e; - et1 = e->target->point; - } - } - -#ifdef DEBUG_CONVEX_HULL - printf(" Starting at %d %d\n", et0.index, et1.index); -#endif - - int64_t dx = maxDot1 - maxDot0; - if (dx > 0) - { - while (true) - { - int64_t dy = (et1 - et0).dot(s); - - if (e0 && (e0->target != stop0)) - { - Edge* f0 = e0->next->reverse; - if (f0->copy > mergeStamp) - { - int64_t dx0 = (f0->target->point - et0).dot(perp); - int64_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) - { - int64_t dx1 = d1.dot(perp); - int64_t dy1 = d1.dot(s); - int64_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 - { - btAssert((e1 == start1) && (d1.dot(normal) < 0)); - } - } - } - - break; - } - } - else if (dx < 0) - { - while (true) - { - int64_t dy = (et1 - et0).dot(s); - - if (e1 && (e1->target != stop1)) - { - Edge* f1 = e1->prev->reverse; - if (f1->copy > mergeStamp) - { - int64_t dx1 = (f1->target->point - et1).dot(perp); - int64_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) - { - int64_t dx0 = d0.dot(perp); - int64_t dy0 = d0.dot(s); - int64_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 - { - btAssert((e0 == start0) && (d0.dot(normal) < 0)); - } - } - } - - break; - } - } -#ifdef DEBUG_CONVEX_HULL - printf(" Advanced edges to %d %d\n", et0.index, et1.index); -#endif -} - - -void btConvexHullInternal::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); - btAssert(!t.isZero()); - - Edge* e = c0->edges; - Edge* start0 = NULL; - if (e) - { - do - { - int64_t dot = (*e->target - *c0).dot(normal); - btAssert(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 - { - int64_t dot = (*e->target - *c1).dot(normal); - btAssert(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 - printf("\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 - printf(" -> 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 - printf(" 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; - } -} - -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)))); - } -}; - -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; - 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]); - ptr += stride; - min.setMin(p); - max.setMax(p); - } - } - else - { - for (int i = 0; i < count; i++) - { - const float* v = (const float*) ptr; - btVector3 p(v[0], v[1], v[2]); - ptr += stride; - min.setMin(p); - max.setMax(p); - } - } - - btVector3 s = max - min; - maxAxis = s.maxAxis(); - minAxis = s.minAxis(); - if (minAxis == maxAxis) - { - minAxis = (maxAxis + 1) % 3; - } - medAxis = 3 - maxAxis - minAxis; - - s /= btScalar(10216); - if (((medAxis + 1) % 3) != maxAxis) - { - s *= -1; - } - scaling = s; - - if (s[0] != 0) - { - s[0] = btScalar(1) / s[0]; - } - if (s[1] != 0) - { - s[1] = btScalar(1) / s[1]; - } - if (s[2] != 0) - { - s[2] = btScalar(1) / s[2]; - } - - center = (min + max) * btScalar(0.5); - - btAlignedObjectArray points; - points.resize(count); - 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]); - 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].index = i; - } - } - else - { - for (int i = 0; i < count; i++) - { - 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].index = i; - } - } - points.quickSort(pointCmp()); - - 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 - printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count); -#endif -} - -btVector3 btConvexHullInternal::toBtVector(const Point32& v) -{ - btVector3 p; - p[medAxis] = btScalar(v.x); - p[maxAxis] = btScalar(v.y); - p[minAxis] = btScalar(v.z); - return p * scaling; -} - -btVector3 btConvexHullInternal::getBtNormal(Face* face) -{ - return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized(); -} - -btVector3 btConvexHullInternal::getCoordinates(const Vertex* v) -{ - btVector3 p; - p[medAxis] = v->xvalue(); - p[maxAxis] = v->yvalue(); - p[minAxis] = v->zvalue(); - return p * scaling + center; -} - -btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount) -{ - if (!vertexList) - { - return 0; - } - int stamp = --mergeStamp; - btAlignedObjectArray stack; - vertexList->copy = stamp; - stack.push_back(vertexList); - btAlignedObjectArray 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) - { - int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); - btAssert(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; - } - - btAssert(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; - } - - btVector3 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) - { - btScalar minDist = SIMD_INFINITY; - for (int i = 0; i < faceCount; i++) - { - btVector3 normal = getBtNormal(faces[i]); - btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); - if (dist < minDist) - { - minDist = dist; - } - } - - if (minDist <= 0) - { - return 0; - } - - amount = btMin(amount, minDist * clampAmount); - } - - unsigned int seed = 243703; - for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) - { - btSwap(faces[i], faces[seed % faceCount]); - } - - for (int i = 0; i < faceCount; i++) - { - if (!shiftFace(faces[i], amount, stack)) - { - return -amount; - } - } - - return amount; -} - -bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray stack) -{ - btVector3 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((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]); - if (shift.isZero()) - { - return true; - } - 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); -#endif - int64_t origDot = face->origin.dot(normal); - Point32 shiftedOrigin = face->origin + shift; - int64_t shiftedDot = shiftedOrigin.dot(normal); - btAssert(shiftedDot <= origDot); - if (shiftedDot >= origDot) - { - return false; - } - - Edge* intersection = NULL; - - Edge* startEdge = face->nearbyVertex->edges; -#ifdef DEBUG_CONVEX_HULL - printf("Start edge is "); - startEdge->print(); - printf(", 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); - btAssert(dot.compare(origDot) <= 0); -#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); -#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); - btAssert(dot.compare(origDot) <= 0); -#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); -#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 - printf("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 - printf("Checking for outwards edge, current edge is "); - e->print(); - printf("\n"); -#endif - } -#ifdef SHOW_ITERATIONS - printf("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 - printf("Intersecting edge is "); - intersection->print(); - printf("\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 - printf("Needed %d iterations to advance intersection\n", n); -#endif - } - -#ifdef DEBUG_CONVEX_HULL - printf("Advanced intersecting edge to "); - intersection->print(); - printf(", 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; - btAssert(e != intersection->reverse); - cmp = e->target->dot(normal).compare(shiftedDot); -#ifdef DEBUG_CONVEX_HULL - printf("Testing edge "); - e->print(); - printf(" -> cmp = %d\n", cmp); -#endif - if (cmp >= 0) - { - intersection = e; - break; - } - } -#ifdef SHOW_ITERATIONS - printf("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 - 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); - int64_t m01 = face->dir1.dot(n0); - int64_t m10 = face->dir0.dot(n1); - int64_t m11 = face->dir1.dot(n1); - int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); - int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); - Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); - btAssert(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 = (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; - - 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 - printf("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 - printf("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 - printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); -#endif - } - stack.push_back(NULL); - } - - btAssert(stack.size() > 0); - vertexList = stack[0]; - -#ifdef DEBUG_CONVEX_HULL - printf("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 - printf("Needed %d iterations to remove part\n", n); -#endif - - stack.resize(0); - face->origin = shiftedOrigin; - - return true; -} - - -static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray& vertices) -{ - int index = vertex->copy; - if (index < 0) - { - index = vertices.size(); - vertex->copy = index; - vertices.push_back(vertex); -#ifdef DEBUG_CONVEX_HULL - printf("Vertex %d gets index *%d\n", vertex->point.index, index); -#endif - } - return index; -} - -btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp) -{ - if (count <= 0) - { - vertices.clear(); - edges.clear(); - faces.clear(); - return 0; - } - - btConvexHullInternal hull; - hull.compute(coords, doubleCoords, stride, count); - - btScalar 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); - - btAlignedObjectArray oldVertices; - getVertexCopy(hull.vertexList, oldVertices); - int copied = 0; - while (copied < oldVertices.size()) - { - btConvexHullInternal::Vertex* v = oldVertices[copied]; - vertices.push_back(hull.getCoordinates(v)); - btConvexHullInternal::Edge* firstEdge = v->edges; - if (firstEdge) - { - int firstCopy = -1; - int prevCopy = -1; - btConvexHullInternal::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 - printf(" 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++) - { - btConvexHullInternal::Vertex* v = oldVertices[i]; - btConvexHullInternal::Edge* firstEdge = v->edges; - if (firstEdge) - { - btConvexHullInternal::Edge* e = firstEdge; - do - { - if (e->copy >= 0) - { -#ifdef DEBUG_CONVEX_HULL - printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex()); -#endif - faces.push_back(e->copy); - btConvexHullInternal::Edge* f = e; - do - { -#ifdef DEBUG_CONVEX_HULL - printf(" 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/WickedEngine/BULLET/LinearMath/btConvexHullComputer.h b/WickedEngine/BULLET/LinearMath/btConvexHullComputer.h deleted file mode 100644 index 7240ac4fb..000000000 --- a/WickedEngine/BULLET/LinearMath/btConvexHullComputer.h +++ /dev/null @@ -1,103 +0,0 @@ -/* -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 BT_CONVEX_HULL_COMPUTER_H -#define BT_CONVEX_HULL_COMPUTER_H - -#include "btVector3.h" -#include "btAlignedObjectArray.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 btConvexHullComputer -{ - private: - btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp); - - public: - - class Edge - { - private: - int next; - int reverse; - int targetVertex; - - friend class btConvexHullComputer; - - 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 - 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). - 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". - */ - 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); - } -}; - - -#endif //BT_CONVEX_HULL_COMPUTER_H - diff --git a/WickedEngine/BULLET/LinearMath/btDefaultMotionState.h b/WickedEngine/BULLET/LinearMath/btDefaultMotionState.h deleted file mode 100644 index c90b74923..000000000 --- a/WickedEngine/BULLET/LinearMath/btDefaultMotionState.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef BT_DEFAULT_MOTION_STATE_H -#define BT_DEFAULT_MOTION_STATE_H - -#include "btMotionState.h" - -///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets. -ATTRIBUTE_ALIGNED16(struct) btDefaultMotionState : public btMotionState -{ - btTransform m_graphicsWorldTrans; - btTransform m_centerOfMassOffset; - btTransform m_startWorldTrans; - void* m_userPointer; - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity()) - : m_graphicsWorldTrans(startTrans), - m_centerOfMassOffset(centerOfMassOffset), - m_startWorldTrans(startTrans), - m_userPointer(0) - - { - } - - ///synchronizes world transform from user to physics - virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const - { - centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ; - } - - ///synchronizes world transform from physics to user - ///Bullet only calls the update of worldtransform for active objects - virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) - { - m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; - } - - - -}; - -#endif //BT_DEFAULT_MOTION_STATE_H diff --git a/WickedEngine/BULLET/LinearMath/btGeometryUtil.cpp b/WickedEngine/BULLET/LinearMath/btGeometryUtil.cpp deleted file mode 100644 index 5ac230f71..000000000 --- a/WickedEngine/BULLET/LinearMath/btGeometryUtil.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* -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 "btGeometryUtil.h" - - -/* - Make sure this dummy function never changes so that it - can be used by probes that are checking whether the - library is actually installed. -*/ -extern "C" -{ - void btBulletMathProbe (); - - void btBulletMathProbe () {} -} - - -bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin) -{ - int numbrushes = planeEquations.size(); - for (int i=0;ibtScalar(0.)) - { - return false; - } - } - return true; - -} - - -bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin) -{ - int numvertices = vertices.size(); - for (int i=0;ibtScalar(0.)) - { - return false; - } - } - return true; -} - -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 ) -{ - const int numvertices = vertices.size(); - // brute force: - for (int i=0;i btScalar(0.0001)) - { - planeEquation.normalize(); - if (notExist(planeEquation,planeEquationsOut)) - { - planeEquation[3] = -planeEquation.dot(N1); - - //check if inside, and replace supportingVertexOut if needed - if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01))) - { - planeEquationsOut.push_back(planeEquation); - } - } - } - normalSign = btScalar(-1.); - } - - } - } - } - -} - -void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& 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) ) ) - { - //point P out of 3 plane equations: - - // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) - //P = ------------------------------------------------------------------------- - // N1 . ( N2 * N3 ) - - - btScalar quotient = (N1.dot(n2n3)); - if (btFabs(quotient) > btScalar(0.000001)) - { - quotient = btScalar(-1.) / quotient; - n2n3 *= N1[3]; - n3n1 *= N2[3]; - n1n2 *= N3[3]; - btVector3 potentialVertex = n2n3; - potentialVertex += n3n1; - potentialVertex += n1n2; - potentialVertex *= quotient; - - //check if inside, and replace supportingVertexOut if needed - if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01))) - { - verticesOut.push_back(potentialVertex); - } - } - } - } - } - } -} - diff --git a/WickedEngine/BULLET/LinearMath/btGeometryUtil.h b/WickedEngine/BULLET/LinearMath/btGeometryUtil.h deleted file mode 100644 index a4b13b456..000000000 --- a/WickedEngine/BULLET/LinearMath/btGeometryUtil.h +++ /dev/null @@ -1,42 +0,0 @@ -/* -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 BT_GEOMETRY_UTIL_H -#define BT_GEOMETRY_UTIL_H - -#include "btVector3.h" -#include "btAlignedObjectArray.h" - -///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices. -class btGeometryUtil -{ - 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 bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin); - -}; - - -#endif //BT_GEOMETRY_UTIL_H - diff --git a/WickedEngine/BULLET/LinearMath/btGrahamScan2dConvexHull.h b/WickedEngine/BULLET/LinearMath/btGrahamScan2dConvexHull.h deleted file mode 100644 index e658c5cf0..000000000 --- a/WickedEngine/BULLET/LinearMath/btGrahamScan2dConvexHull.h +++ /dev/null @@ -1,117 +0,0 @@ -/* -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 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) - { - } - btScalar m_angle; - int m_orgIndex; -}; - - -struct btAngleCompareFunc { - btVector3 m_anchor; - btAngleCompareFunc(const btVector3& anchor) - : m_anchor(anchor) - { - } - 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(); - if (al != bl) - return al < bl; - else - { - return a.m_orgIndex < b.m_orgIndex; - } - } - } -}; - -inline void GrahamScanConvexHull2D(btAlignedObjectArray& originalPoints, btAlignedObjectArray& hull, const btVector3& normalAxis) -{ - btVector3 axis0,axis1; - btPlaneSpace1(normalAxis,axis0,axis1); - - - 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; - if (!isConvex) - hull.pop_back(); - else - hull.push_back(originalPoints[i]); - } - } -} - -#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H diff --git a/WickedEngine/BULLET/LinearMath/btHashMap.h b/WickedEngine/BULLET/LinearMath/btHashMap.h deleted file mode 100644 index ce07db3ac..000000000 --- a/WickedEngine/BULLET/LinearMath/btHashMap.h +++ /dev/null @@ -1,450 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_HASH_MAP_H -#define BT_HASH_MAP_H - -#include "btAlignedObjectArray.h" - -///very basic hashable string implementation, compatible with btHashMap -struct btHashString -{ - const char* m_string; - unsigned int m_hash; - - SIMD_FORCE_INLINE unsigned int getHash()const - { - return m_hash; - } - - btHashString(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; - - for(int i = 0; m_string[i]; 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 btHashString& other) const - { - return (m_string == other.m_string) || - (0==portableStringCompare(m_string,other.m_string)); - - } - -}; - -const int BT_HASH_NULL=0xffffffff; - - -class btHashInt -{ - int m_uid; -public: - btHashInt(int uid) :m_uid(uid) - { - } - - int getUid1() const - { - return m_uid; - } - - void setUid1(int uid) - { - m_uid = uid; - } - - bool equals(const btHashInt& other) const - { - return getUid1() == other.getUid1(); - } - //to our success - SIMD_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 btHashPtr -{ - - union - { - const void* m_pointer; - int m_hashValues[2]; - }; - -public: - - btHashPtr(const void* ptr) - :m_pointer(ptr) - { - } - - const void* getPointer() const - { - return m_pointer; - } - - bool equals(const btHashPtr& other) const - { - return getPointer() == other.getPointer(); - } - - //to our success - SIMD_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 btHashKeyPtr -{ - int m_uid; -public: - - btHashKeyPtr(int uid) :m_uid(uid) - { - } - - int getUid1() const - { - return m_uid; - } - - bool equals(const btHashKeyPtr& other) const - { - return getUid1() == other.getUid1(); - } - - //to our success - SIMD_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 btHashKey -{ - int m_uid; -public: - - btHashKey(int uid) :m_uid(uid) - { - } - - int getUid1() const - { - return m_uid; - } - - bool equals(const btHashKey& other) const - { - return getUid1() == other.getUid1(); - } - //to our success - SIMD_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 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; - - 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] = BT_HASH_NULL; - } - for (i = 0; i < newCapacity; ++i) - { - m_next[i] = BT_HASH_NULL; - } - - for(i=0;i= (unsigned int)m_hashTable.size()) - { - return BT_HASH_NULL; - } - - int index = m_hashTable[hash]; - while ((index != BT_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 //BT_HASH_MAP_H diff --git a/WickedEngine/BULLET/LinearMath/btIDebugDraw.h b/WickedEngine/BULLET/LinearMath/btIDebugDraw.h deleted file mode 100644 index de97c3f87..000000000 --- a/WickedEngine/BULLET/LinearMath/btIDebugDraw.h +++ /dev/null @@ -1,445 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#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 -{ - public: - - enum DebugDrawModes - { - DBG_NoDebug=0, - DBG_DrawWireframe = 1, - DBG_DrawAabb=2, - DBG_DrawFeaturesText=4, - DBG_DrawContactPoints=8, - DBG_NoDeactivation=16, - DBG_NoHelpText = 32, - 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_MAX_DEBUG_DRAW_MODE - }; - - virtual ~btIDebugDraw() {}; - - 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); - } - - 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); - btScalar minTh = -SIMD_HALF_PI; - btScalar maxTh = SIMD_HALF_PI; - 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 ); - } - - 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); - } - - 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) - { - - 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++) - { - 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; - - drawLine(pa,pb,color); - } - 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(0.7f,0,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); - } - - 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)((maxAngle - minAngle) / step); - if(!nSteps) nSteps = 1; - btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle); - if(drawSect) - { - drawLine(center, prev, color); - } - for(int i = 1; i <= nSteps; i++) - { - btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps); - btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle); - drawLine(prev, next, color); - prev = next; - } - if(drawSect) - { - drawLine(center, prev, color); - } - } - 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]; - btVector3 *pvA = vA, *pvB = vB, *pT; - btVector3 npole = center + up * radius; - btVector3 spole = center - up * radius; - btVector3 arcStart; - btScalar step = stepDegrees * SIMD_RADS_PER_DEG; - const btVector3& kv = up; - const btVector3& iv = axis; - btVector3 jv = kv.cross(iv); - bool drawN = false; - bool drawS = false; - if(minTh <= -SIMD_HALF_PI) - { - minTh = -SIMD_HALF_PI + step; - drawN = true; - } - if(maxTh >= SIMD_HALF_PI) - { - maxTh = SIMD_HALF_PI - step; - drawS = true; - } - if(minTh > maxTh) - { - minTh = -SIMD_HALF_PI + step; - maxTh = SIMD_HALF_PI - step; - drawN = drawS = true; - } - int n_hor = (int)((maxTh - minTh) / step) + 1; - if(n_hor < 2) n_hor = 2; - btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1); - bool isClosed = false; - if(minPs > maxPs) - { - minPs = -SIMD_PI + step; - maxPs = SIMD_PI; - isClosed = true; - } - else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f)) - { - isClosed = true; - } - else - { - isClosed = false; - } - int n_vert = (int)((maxPs - minPs) / step) + 1; - if(n_vert < 2) n_vert = 2; - btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1); - for(int i = 0; i < n_hor; i++) - { - btScalar th = minTh + btScalar(i) * step_h; - btScalar sth = radius * btSin(th); - btScalar cth = radius * btCos(th); - for(int j = 0; j < n_vert; j++) - { - btScalar psi = minPs + btScalar(j) * step_v; - btScalar sps = btSin(psi); - btScalar cps = btCos(psi); - pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv; - if(i) - { - drawLine(pvA[j], pvB[j], color); - } - else if(drawS) - { - drawLine(spole, pvB[j], color); - } - if(j) - { - drawLine(pvB[j-1], pvB[j], color); - } - else - { - arcStart = pvB[j]; - } - if((i == (n_hor - 1)) && drawN) - { - drawLine(npole, pvB[j], color); - } - - if (drawCenter) - { - if(isClosed) - { - if(j == (n_vert-1)) - { - drawLine(arcStart, pvB[j], color); - } - } - else - { - if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1)))) - { - drawLine(center, pvB[j], color); - } - } - } - } - pT = pvA; pvA = pvB; pvB = pT; - } - } - - - 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); - drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color); - drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color); - drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color); - drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); - drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); - drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); - drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); - drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color); - drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color); - drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color); - drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color); - } - virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color) - { - drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color); - drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color); - drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color); - drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color); - drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); - drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); - drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); - drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); - drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color); - drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color); - drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color); - drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color); - } - - virtual void drawCapsule(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) - { - int stepDegrees = 30; - - btVector3 capStart(0.f,0.f,0.f); - capStart[upAxis] = -halfHeight; - - btVector3 capEnd(0.f,0.f,0.f); - capEnd[upAxis] = halfHeight; - - // Draw the ends - { - - btTransform childTransform = transform; - childTransform.getOrigin() = transform * capStart; - { - btVector3 center = childTransform.getOrigin(); - 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); - } - - - - } - - { - btTransform childTransform = transform; - childTransform.getOrigin() = transform * capEnd; - { - btVector3 center = childTransform.getOrigin(); - 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); - } - } - - // Draw some additional lines - btVector3 start = transform.getOrigin(); - - 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); - } - - } - - virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color) - { - btVector3 start = transform.getOrigin(); - btVector3 offsetHeight(0,0,0); - offsetHeight[upAxis] = halfHeight; - int stepDegrees=30; - btVector3 capStart(0.f,0.f,0.f); - capStart[upAxis] = -halfHeight; - btVector3 capEnd(0.f,0.f,0.f); - capEnd[upAxis] = halfHeight; - - 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); - } - // Drawing top and bottom caps of the cylinder - 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)); - } - - virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color) - { - int stepDegrees = 30; - btVector3 start = transform.getOrigin(); - - 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 capEnd(0.f,0.f,0.f); - capEnd[upAxis] = -halfHeight; - - 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); - } - - 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); - 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); - } - - 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); - 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); - } -}; - - -#endif //BT_IDEBUG_DRAW__H - diff --git a/WickedEngine/BULLET/LinearMath/btList.h b/WickedEngine/BULLET/LinearMath/btList.h deleted file mode 100644 index eec80a706..000000000 --- a/WickedEngine/BULLET/LinearMath/btList.h +++ /dev/null @@ -1,73 +0,0 @@ -/* -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 BT_GEN_LIST_H -#define BT_GEN_LIST_H - -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; } - - bool isHead() const { return m_prev == 0; } - bool isTail() const { return m_next == 0; } - - void insertBefore(btGEN_Link *link) { - m_next = link; - m_prev = link->m_prev; - m_next->m_prev = this; - m_prev->m_next = this; - } - - void insertAfter(btGEN_Link *link) { - m_next = link->m_next; - m_prev = link; - m_next->m_prev = this; - m_prev->m_next = this; - } - - void remove() { - m_next->m_prev = m_prev; - m_prev->m_next = m_next; - } - -private: - btGEN_Link *m_next; - btGEN_Link *m_prev; -}; - -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/WickedEngine/BULLET/LinearMath/btMatrix3x3.h b/WickedEngine/BULLET/LinearMath/btMatrix3x3.h deleted file mode 100644 index 14fe704f8..000000000 --- a/WickedEngine/BULLET/LinearMath/btMatrix3x3.h +++ /dev/null @@ -1,1367 +0,0 @@ -/* -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 BT_MATRIX3x3_H -#define BT_MATRIX3x3_H - -#include "btVector3.h" -#include "btQuaternion.h" -#include - -#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)) -#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)) -#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}; -const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f}; -#endif - -#ifdef BT_USE_DOUBLE_PRECISION -#define btMatrix3x3Data btMatrix3x3DoubleData -#else -#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 { - - ///Data storage for the matrix, each vector is a row of the matrix - btVector3 m_el[3]; - -public: - /** @brief No initializaion constructor */ - btMatrix3x3 () {} - - // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } - - /**@brief Constructor from Quaternion */ - explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } - /* - template - Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) - { - setEulerYPR(yaw, pitch, roll); - } - */ - /** @brief Constructor with row major formatting */ - btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, - const btScalar& yx, const btScalar& yy, const btScalar& yz, - const btScalar& zx, const btScalar& zy, const btScalar& zz) - { - 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 ) - { - 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 ) - { - m_el[0] = v0; - m_el[1] = v1; - m_el[2] = v2; - } - - // Copy constructor - SIMD_FORCE_INLINE btMatrix3x3(const btMatrix3x3& 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 - 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) - { - m_el[0] = other.m_el[0]; - m_el[1] = other.m_el[1]; - m_el[2] = other.m_el[2]; - } - - /** @brief Assignment Operator */ - SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) - { - m_el[0] = other.m_el[0]; - 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 */ - SIMD_FORCE_INLINE btVector3 getColumn(int i) const - { - return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); - } - - - /** @brief Get a row of the matrix as a vector - * @param i Row number 0 indexed */ - SIMD_FORCE_INLINE const btVector3& getRow(int i) const - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Get a mutable reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - SIMD_FORCE_INLINE btVector3& operator[](int i) - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Get a const reference to a row of the matrix as a vector - * @param i Row number 0 indexed */ - SIMD_FORCE_INLINE const btVector3& operator[](int i) const - { - btFullAssert(0 <= i && i < 3); - return m_el[i]; - } - - /** @brief Multiply by the target matrix on the right - * @param m Rotation matrix to be applied - * Equivilant to this = this * m */ - btMatrix3x3& operator*=(const btMatrix3x3& m); - - /** @brief Adds by the target matrix on the right - * @param m matrix to be applied - * Equivilant to this = this + 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); - - /** @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) - { - 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 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); - } - - /** @brief Set the matrix from a quaternion - * @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(); - __m128i Qi = btCastfTo128i(Q); - __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; // - - 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; - - 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, - xy + wz, btScalar(1.0) - (xx + zz), yz - wx, - xz - wy, yz + wx, btScalar(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 btScalar& yaw, const btScalar& pitch, const btScalar& 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(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 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(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)); -#endif - } - - 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); -#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)); -#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 - { -#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 - - 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 - - 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[10] = btScalar(m_el[2].z()); - 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 - { -#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); - - 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; } - } - - x = m_el[i][i] - m_el[j][j] - m_el[k][k] + 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); - } - - 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)) - { - btScalar s = btSqrt(trace + btScalar(1.0)); - 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 - { - 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)); - temp[i] = s * btScalar(0.5); - s = btScalar(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(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 (yaw>0) - yaw-=SIMD_PI; - else - yaw+=SIMD_PI; - - if (roll>0) - roll-=SIMD_PI; - else - roll+=SIMD_PI; - } - }; - - - /**@brief Get the matrix represented as euler angles around ZYX - * @param yaw Yaw around X axis - * @param pitch Pitch around Y axis - * @param roll around X axis - * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ - void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const - { - struct Euler - { - btScalar yaw; - btScalar pitch; - btScalar roll; - }; - - Euler euler_out; - Euler euler_out2; //second solution - //get the pointer to the raw data - - // Check that pitch is not at a singularity - if (btFabs(m_el[2].x()) >= 1) - { - euler_out.yaw = 0; - euler_out2.yaw = 0; - - // From difference of angles formula - btScalar delta = btAtan2(m_el[0].x(),m_el[0].z()); - if (m_el[2].x() > 0) //gimbal locked up - { - euler_out.pitch = SIMD_PI / btScalar(2.0); - euler_out2.pitch = SIMD_PI / btScalar(2.0); - euler_out.roll = euler_out.pitch + delta; - euler_out2.roll = euler_out.pitch + delta; - } - else // gimbal locked down - { - euler_out.pitch = -SIMD_PI / btScalar(2.0); - euler_out2.pitch = -SIMD_PI / btScalar(2.0); - euler_out.roll = -euler_out.pitch + delta; - euler_out2.roll = -euler_out.pitch + delta; - } - } - else - { - euler_out.pitch = - btAsin(m_el[2].x()); - euler_out2.pitch = SIMD_PI - euler_out.pitch; - - euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), - m_el[2].z()/btCos(euler_out.pitch)); - euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), - m_el[2].z()/btCos(euler_out2.pitch)); - - euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), - m_el[0].x()/btCos(euler_out.pitch)); - euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), - m_el[0].x()/btCos(euler_out2.pitch)); - } - - if (solution_number == 1) - { - yaw = euler_out.yaw; - pitch = euler_out.pitch; - roll = euler_out.roll; - } - else - { - yaw = euler_out2.yaw; - pitch = euler_out2.pitch; - roll = euler_out2.roll; - } - } - - /**@brief Create a scaled copy of the matrix - * @param s Scaling vector The elements of the vector will scale each column */ - - btMatrix3x3 scaled(const btVector3& s) const - { -#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 - return btMatrix3x3( - 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; - /**@brief Return the adjoint of the matrix */ - btMatrix3x3 adjoint() const; - /**@brief Return the matrix with all values non negative */ - btMatrix3x3 absolute() const; - /**@brief Return the transpose of the matrix */ - btMatrix3x3 transpose() const; - /**@brief Return the inverse of the matrix */ - btMatrix3x3 inverse() const; - - btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; - btMatrix3x3 timesTranspose(const btMatrix3x3& m) 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 - { - 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 - { - return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); - } - - - /**@brief diagonalizes this matrix by the Jacobi method. - * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original - * coordinate system, i.e., old_this = rot * new_this * rot^T. - * @param threshold See iteration - * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied - * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. - * - * Note that this matrix is assumed to be symmetric. - */ - void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) - { - rot.setIdentity(); - 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 - * @param r1 The second row to use for calculating the cofactor - * @param c1 The second column to use for calculating the cofactor - * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details - */ - btScalar cofac(int r1, int c1, int r2, int c2) const - { - return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; - } - - void serialize(struct btMatrix3x3Data& dataOut) const; - - void serializeFloat(struct btMatrix3x3FloatData& dataOut) const; - - void deSerialize(const struct btMatrix3x3Data& dataIn); - - void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn); - - void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn); - -}; - - -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; - - 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); - - 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; - - 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); - - 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; -} - -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; -#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; -} - -SIMD_FORCE_INLINE btMatrix3x3 -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)); -#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)); -#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); -#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], - - 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], - - 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; -#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; -} - - -SIMD_FORCE_INLINE btScalar -btMatrix3x3::determinant() const -{ - return btTriple((*this)[0], (*this)[1], (*this)[2]); -} - - -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 - 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 ); -#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)); -} - -SIMD_FORCE_INLINE btMatrix3x3 -btMatrix3x3::inverse() const -{ - btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); - btScalar det = (*this)[0].dot(co); - btFullAssert(det != btScalar(0.0)); - 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); -} - -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 ); - -#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 ); -#else - 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(), - m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(), - m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(), - m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(), - m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(), - m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(), - m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z()); -#endif -} - -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); - -#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 ); - -#else - return btMatrix3x3( - 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 -} - -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]); -#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)) - - 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); - - 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); - - 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 = 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 -operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) -{ -#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); - -#elif defined(BT_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, 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 - 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])); -#endif -} - -/* -SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { -return btMatrix3x3( -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. */ -SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) -{ -#if (defined (BT_USE_SSE_IN_API) && defined (BT_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 btMatrix3x3FloatData -{ - btVector3FloatData m_el[3]; -}; - -///for serialization -struct btMatrix3x3DoubleData -{ - btVector3DoubleData m_el[3]; -}; - - - - -SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const -{ - 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 -{ - 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) -{ - 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) -{ - 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) -{ - for (int i=0;i<3;i++) - m_el[i].deSerializeDouble(dataIn.m_el[i]); -} - -#endif //BT_MATRIX3x3_H - diff --git a/WickedEngine/BULLET/LinearMath/btMatrixX.h b/WickedEngine/BULLET/LinearMath/btMatrixX.h deleted file mode 100644 index 1c29632c5..000000000 --- a/WickedEngine/BULLET/LinearMath/btMatrixX.h +++ /dev/null @@ -1,504 +0,0 @@ -/* -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. -*/ -///original version written by Erwin Coumans, October 2013 - -#ifndef BT_MATRIX_X_H -#define BT_MATRIX_X_H - -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btAlignedObjectArray.h" - -class btIntSortPredicate -{ - public: - bool operator() ( const int& a, const int& b ) const - { - return a < b; - } -}; - - -template -struct btMatrixX -{ - int m_rows; - int m_cols; - int m_operations; - int m_resizeOperations; - int m_setElemOperations; - - btAlignedObjectArray m_storage; - btAlignedObjectArray< btAlignedObjectArray > m_rowNonZeroElements1; - btAlignedObjectArray< btAlignedObjectArray > m_colNonZeroElements; - - T* getBufferPointerWritable() - { - return m_storage.size() ? &m_storage[0] : 0; - } - - const T* getBufferPointer() const - { - return m_storage.size() ? &m_storage[0] : 0; - } - btMatrixX() - :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) - { - resize(rows,cols); - } - void resize(int rows, int cols) - { - m_resizeOperations++; - m_rows = rows; - m_cols = cols; - { - BT_PROFILE("m_storage.resize"); - m_storage.resize(rows*cols); - } - clearSparseInfo(); - } - int cols() const - { - return m_cols; - } - int rows() const - { - return m_rows; - } - ///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead - /*T& operator() (int row,int col) - { - return m_storage[col*m_rows+row]; - } - */ - - void addElem(int row,int col, T val) - { - if (val) - { - if (m_storage[col+row*m_cols]==0.f) - { - setElem(row,col,val); - } else - { - 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 -struct btVectorX -{ - btAlignedObjectArray m_storage; - - btVectorX() - { - } - btVectorX(int numRows) - { - m_storage.resize(numRows); - } - - void resize(int rows) - { - m_storage.resize(rows); - } - int cols() const - { - return 1; - } - int rows() const - { - return m_storage.size(); - } - int size() const - { - return rows(); - } - void setZero() - { - // for (int i=0;i -void setElem(btMatrixX& mat, int row, int col, T val) -{ - mat.setElem(row,col,val); -} -*/ - - -typedef btMatrixX btMatrixXf; -typedef btVectorX btVectorXf; - -typedef btMatrixX btMatrixXd; -typedef btVectorX btVectorXd; - - - -inline void setElem(btMatrixXd& mat, int row, int col, double val) -{ - mat.setElem(row,col,val); -} - -inline void setElem(btMatrixXf& mat, int row, int col, float val) -{ - mat.setElem(row,col,val); -} - -#ifdef BT_USE_DOUBLE_PRECISION - #define btVectorXu btVectorXd - #define btMatrixXu btMatrixXd -#else - #define btVectorXu btVectorXf - #define btMatrixXu btMatrixXf -#endif //BT_USE_DOUBLE_PRECISION - - - -#endif//BT_MATRIX_H_H diff --git a/WickedEngine/BULLET/LinearMath/btMinMax.h b/WickedEngine/BULLET/LinearMath/btMinMax.h deleted file mode 100644 index 5b436e9ba..000000000 --- a/WickedEngine/BULLET/LinearMath/btMinMax.h +++ /dev/null @@ -1,71 +0,0 @@ -/* -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 BT_GEN_MINMAX_H -#define BT_GEN_MINMAX_H - -#include "btScalar.h" - -template -SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) -{ - return a < b ? a : b ; -} - -template -SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) -{ - return a > b ? a : b; -} - -template -SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub) -{ - return a < lb ? lb : (ub < a ? ub : a); -} - -template -SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) -{ - if (b < a) - { - a = b; - } -} - -template -SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) -{ - if (a < b) - { - a = b; - } -} - -template -SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub) -{ - if (a < lb) - { - a = lb; - } - else if (ub < a) - { - a = ub; - } -} - -#endif //BT_GEN_MINMAX_H diff --git a/WickedEngine/BULLET/LinearMath/btMotionState.h b/WickedEngine/BULLET/LinearMath/btMotionState.h deleted file mode 100644 index 943181409..000000000 --- a/WickedEngine/BULLET/LinearMath/btMotionState.h +++ /dev/null @@ -1,40 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MOTIONSTATE_H -#define BT_MOTIONSTATE_H - -#include "btTransform.h" - -///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 -{ - public: - - virtual ~btMotionState() - { - - } - - 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 diff --git a/WickedEngine/BULLET/LinearMath/btPolarDecomposition.cpp b/WickedEngine/BULLET/LinearMath/btPolarDecomposition.cpp deleted file mode 100644 index a4dca7fdd..000000000 --- a/WickedEngine/BULLET/LinearMath/btPolarDecomposition.cpp +++ /dev/null @@ -1,99 +0,0 @@ -#include "btPolarDecomposition.h" -#include "btMinMax.h" - -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); - } -} - -const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001); -const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16; - -btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations) -: m_tolerance(tolerance) -, 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(); - - 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; - - // 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; - - // 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(); - - // 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; - - return m_maxIterations; -} - -unsigned int btPolarDecomposition::maxIterations() const -{ - return m_maxIterations; -} - -unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) -{ - static btPolarDecomposition polar; - return polar.decompose(a, u, h); -} - diff --git a/WickedEngine/BULLET/LinearMath/btPolarDecomposition.h b/WickedEngine/BULLET/LinearMath/btPolarDecomposition.h deleted file mode 100644 index 561566764..000000000 --- a/WickedEngine/BULLET/LinearMath/btPolarDecomposition.h +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef POLARDECOMPOSITION_H -#define POLARDECOMPOSITION_H - -#include "btMatrix3x3.h" - -/** - * This class is used to compute the polar decomposition of a matrix. In - * general, the polar decomposition factorizes a matrix, A, into two parts: a - * unitary matrix (U) and a positive, semi-definite Hermitian matrix (H). - * However, in this particular implementation the original matrix, A, is - * required to be a square 3x3 matrix with real elements. This means that U will - * be an orthogonal matrix and H with be a positive-definite, symmetric matrix. - */ -class btPolarDecomposition -{ - public: - static const btScalar DEFAULT_TOLERANCE; - static const unsigned int DEFAULT_MAX_ITERATIONS; - - /** - * Creates an instance with optional parameters. - * - * @param tolerance - the tolerance used to determine convergence of the - * algorithm - * @param maxIterations - the maximum number of iterations used to achieve - * convergence - */ - btPolarDecomposition(btScalar tolerance = DEFAULT_TOLERANCE, - unsigned int maxIterations = DEFAULT_MAX_ITERATIONS); - - /** - * 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. - * - * @param a - the original matrix - * @param u - the resulting orthogonal matrix - * @param h - the resulting symmetric matrix - * - * @return the number of iterations performed by the algorithm. - */ - 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; - - private: - btScalar m_tolerance; - unsigned int m_maxIterations; -}; - -/** - * This functions decomposes the matrix 'a' into two parts: an orthogonal matrix - * 'u' and a symmetric, positive-definite matrix 'h'. If the number of - * iterations returned by this function is equal to - * btPolarDecomposition::DEFAULT_MAX_ITERATIONS, the algorithm has failed to - * converge. - * - * @param a - the original matrix - * @param u - the resulting orthogonal matrix - * @param h - the resulting symmetric matrix - * - * @return the number of iterations performed by the algorithm. - */ -unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h); - -#endif // POLARDECOMPOSITION_H - diff --git a/WickedEngine/BULLET/LinearMath/btPoolAllocator.h b/WickedEngine/BULLET/LinearMath/btPoolAllocator.h deleted file mode 100644 index ef2084537..000000000 --- a/WickedEngine/BULLET/LinearMath/btPoolAllocator.h +++ /dev/null @@ -1,121 +0,0 @@ -/* -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 _BT_POOL_ALLOCATOR_H -#define _BT_POOL_ALLOCATOR_H - -#include "btScalar.h" -#include "btAlignedAllocator.h" - -///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; - -public: - - btPoolAllocator(int elemSize, int maxElements) - :m_elemSize(elemSize), - m_maxElements(maxElements) - { - 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; - } - - ~btPoolAllocator() - { - btAlignedFree( 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; - btAssert(!size || size<=m_elemSize); - btAssert(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) { - btAssert((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/WickedEngine/BULLET/LinearMath/btQuadWord.h b/WickedEngine/BULLET/LinearMath/btQuadWord.h deleted file mode 100644 index 11067ef47..000000000 --- a/WickedEngine/BULLET/LinearMath/btQuadWord.h +++ /dev/null @@ -1,244 +0,0 @@ -/* -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 BT_SIMD_QUADWORD_H -#define BT_SIMD_QUADWORD_H - -#include "btScalar.h" -#include "btMinMax.h" - - - - - -#if defined (__CELLOS_LV2) && defined (__SPU__) -#include -#endif - -/**@brief The btQuadWord class is base class for btVector3 and btQuaternion. - * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. - */ -#ifndef USE_LIBSPE2 -ATTRIBUTE_ALIGNED16(class) btQuadWord -#else -class btQuadWord -#endif -{ -protected: - -#if defined (__SPU__) && defined (__CELLOS_LV2__) - union { - vec_float4 mVec128; - btScalar m_floats[4]; - }; -public: - vec_float4 get128() const - { - return mVec128; - } -protected: -#else //__CELLOS_LV2__ __SPU__ - -#if defined(BT_USE_SSE) || defined(BT_USE_NEON) - union { - btSimdFloat4 mVec128; - btScalar m_floats[4]; - }; -public: - SIMD_FORCE_INLINE btSimdFloat4 get128() const - { - return mVec128; - } - SIMD_FORCE_INLINE void set128(btSimdFloat4 v128) - { - mVec128 = v128; - } -#else - btScalar m_floats[4]; -#endif // BT_USE_SSE - -#endif //__CELLOS_LV2__ __SPU__ - - public: - -#if defined(BT_USE_SSE) || defined(BT_USE_NEON) - - // Set Vector - SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec) - { - mVec128 = vec; - } - - // Copy constructor - SIMD_FORCE_INLINE btQuadWord(const btQuadWord& rhs) - { - mVec128 = rhs.mVec128; - } - - // Assignment Operator - 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]; } - - //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } - //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } - ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } - SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } - - SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const - { -#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])); -#endif - } - - SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const - { - return !(*this == other); - } - - /**@brief Set x,y,z and zero w - * @param x Value of x - * @param y Value of y - * @param z Value of z - */ - SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z) - { - m_floats[0]=_x; - m_floats[1]=_y; - m_floats[2]=_z; - m_floats[3] = 0.f; - } - -/* void getValue(btScalar *m) const - { - m[0] = m_floats[0]; - m[1] = m_floats[1]; - m[2] = m_floats[2]; - } -*/ -/**@brief Set the values - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) - { - m_floats[0]=_x; - m_floats[1]=_y; - m_floats[2]=_z; - m_floats[3]=_w; - } - /**@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; - } - -/**@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; - } - - /**@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 - * @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 - } - - - -}; - -#endif //BT_SIMD_QUADWORD_H diff --git a/WickedEngine/BULLET/LinearMath/btQuaternion.h b/WickedEngine/BULLET/LinearMath/btQuaternion.h deleted file mode 100644 index 665421de1..000000000 --- a/WickedEngine/BULLET/LinearMath/btQuaternion.h +++ /dev/null @@ -1,909 +0,0 @@ -/* -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 BT_SIMD__QUATERNION_H_ -#define BT_SIMD__QUATERNION_H_ - - -#include "btVector3.h" -#include "btQuadWord.h" - - - - - -#ifdef BT_USE_SSE - -//const __m128 ATTRIBUTE_ALIGNED16(vOnes) = {1.0f, 1.0f, 1.0f, 1.0f}; -#define vOnes (_mm_set_ps(1.0f, 1.0f, 1.0f, 1.0f)) - -#endif - -#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)) - -#elif defined(BT_USE_NEON) - -const btSimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f}; -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 { -public: - /**@brief No initialization constructor */ - btQuaternion() {} - -#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; - } - - // Copy constructor - SIMD_FORCE_INLINE btQuaternion(const btQuaternion& rhs) - { - mVec128 = rhs.mVec128; - } - - // Assignment Operator - 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 - * @param axis The axis which the rotation is around - * @param angle The magnitude of the rotation around the angle (Radians) */ - btQuaternion(const btVector3& _axis, const btScalar& _angle) - { - setRotation(_axis, _angle); - } - /**@brief Constructor from Euler angles - * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z - * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y - * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */ - btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) - { -#ifndef BT_EULER_DEFAULT_ZYX - setEuler(yaw, pitch, roll); -#else - setEulerZYX(yaw, pitch, roll); -#endif - } - /**@brief Set the rotation using axis angle notation - * @param axis The axis around which to rotate - * @param angle The magnitude of the rotation in Radians */ - void setRotation(const btVector3& axis, const btScalar& _angle) - { - btScalar d = axis.length(); - btAssert(d != btScalar(0.0)); - btScalar s = btSin(_angle * btScalar(0.5)) / d; - setValue(axis.x() * s, axis.y() * s, axis.z() * s, - btCos(_angle * btScalar(0.5))); - } - /**@brief Set the quaternion using Euler angles - * @param yaw Angle around Y - * @param pitch Angle around X - * @param roll Angle around Z */ - void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) - { - btScalar halfYaw = btScalar(yaw) * btScalar(0.5); - btScalar halfPitch = btScalar(pitch) * btScalar(0.5); - btScalar halfRoll = btScalar(roll) * btScalar(0.5); - btScalar cosYaw = btCos(halfYaw); - btScalar sinYaw = btSin(halfYaw); - btScalar cosPitch = btCos(halfPitch); - btScalar sinPitch = btSin(halfPitch); - btScalar cosRoll = btCos(halfRoll); - btScalar sinRoll = btSin(halfRoll); - setValue(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 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 cosYaw = btCos(halfYaw); - btScalar sinYaw = btSin(halfYaw); - btScalar cosPitch = btCos(halfPitch); - btScalar sinPitch = btSin(halfPitch); - btScalar cosRoll = btCos(halfRoll); - btScalar sinRoll = btSin(halfRoll); - setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x - cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y - cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z - cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx - } - /**@brief Add two quaternions - * @param q The quaternion to add to this one */ - SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) - { -#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]; -#endif - return *this; - } - - /**@brief Subtract out a quaternion - * @param q The quaternion to subtract from this one */ - btQuaternion& operator-=(const btQuaternion& q) - { -#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]; -#endif - return *this; - } - - /**@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) - 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; -#endif - return *this; - } - - /**@brief Multiply this quaternion by q on the right - * @param q The other quaternion - * Equivilant to this = this * q */ - btQuaternion& operator*=(const btQuaternion& q) - { -#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)); - - 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)); - - 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) - - 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 = (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.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 - * @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; - - 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(BT_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.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 */ - btScalar length2() const - { - return dot(*this); - } - - /**@brief Return the length of the quaternion */ - btScalar length() const - { - return btSqrt(length2()); - } - - /**@brief Normalize the quaternion - * Such that x^2 + y^2 + z^2 +w^2 = 1 */ - btQuaternion& normalize() - { -#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); - 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 - 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 */ - 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) - - return btQuaternion(_mm_mul_ps(mVec128, vs)); -#elif defined(BT_USE_NEON) - return btQuaternion(vmulq_n_f32(mVec128, s)); -#else - return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s); -#endif - } - - /**@brief Return an inversely scaled versionof this quaternion - * @param s The inverse scale factor */ - btQuaternion operator/(const btScalar& s) const - { - btAssert(s != btScalar(0.0)); - return *this * (btScalar(1.0) / s); - } - - /**@brief Inversely scale this quaternion - * @param s The scale factor */ - btQuaternion& operator/=(const btScalar& s) - { - btAssert(s != btScalar(0.0)); - return *this *= btScalar(1.0) / s; - } - - /**@brief Return a normalized version of this quaternion */ - btQuaternion normalized() const - { - return *this / length(); - } - /**@brief Return the ***half*** angle between this quaternion and the other - * @param q The other quaternion */ - 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 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 - return btAcos(dot(-q) / s) * btScalar(2.0); - else - return btAcos(dot(q) / s) * btScalar(2.0); - } - - /**@brief Return the angle of rotation represented by this quaternion */ - btScalar getAngle() const - { - btScalar s = btScalar(2.) * btAcos(m_floats[3]); - return s; - } - - /**@brief Return the angle of rotation represented by this quaternion along the shortest path*/ - btScalar getAngleShortestPath() const - { - btScalar s; - if (dot(*this) < 0) - s = btScalar(2.) * btAcos(m_floats[3]); - else - s = btScalar(2.) * btAcos(-m_floats[3]); - - 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); - 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) - 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(-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 */ - SIMD_FORCE_INLINE btQuaternion - operator+(const btQuaternion& q2) const - { -#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 - 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 - * @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) - return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128)); -#elif defined(BT_USE_NEON) - 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 - * This simply negates each element */ - SIMD_FORCE_INLINE btQuaternion operator-() const - { -#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 - const btQuaternion& q2 = *this; - 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 - { - btQuaternion 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 */ - SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const - { - btQuaternion 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. */ - btQuaternion slerp(const btQuaternion& q, const btScalar& t) const - { - btScalar magnitude = btSqrt(length2() * q.length2()); - btAssert(magnitude > btScalar(0)); - - btScalar product = dot(q) / magnitude; - if (btFabs(product) < btScalar(1)) - { - // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); - - const btScalar theta = btAcos(sign * product); - const btScalar s1 = btSin(sign * t * theta); - const btScalar d = btScalar(1.0) / btSin(theta); - const btScalar s0 = btSin((btScalar(1.0) - t) * theta); - - return btQuaternion( - (m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); - } - else - { - return *this; - } - } - - static const btQuaternion& getIdentity() - { - static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.)); - return identityQuat; - } - - SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } - - -}; - - - - - -/**@brief Return the product of two quaternions */ -SIMD_FORCE_INLINE btQuaternion -operator*(const btQuaternion& q1, const btQuaternion& q2) -{ -#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 = 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 = 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 - - 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 - - return btQuaternion(A0); - -#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]; - - 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 = (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.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()); -#endif -} - -SIMD_FORCE_INLINE btQuaternion -operator*(const btQuaternion& q, const btVector3& w) -{ -#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 = 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; - - 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 - - return btQuaternion(A1); - -#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; - - 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 = (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()); -#endif -} - -SIMD_FORCE_INLINE btQuaternion -operator*(const btVector3& w, const btQuaternion& q) -{ -#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 = 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; - - 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 - - return btQuaternion(A1); - -#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]; - - 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 = (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(), - +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()); -#endif -} - -/**@brief Calculate the dot product between two quaternions */ -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(); -} - -/**@brief Return the angle between two quaternions*/ -SIMD_FORCE_INLINE btScalar -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) -{ - return q.inverse(); -} - -/**@brief Return the result of spherical linear interpolation betwen two quaternions - * @param q1 The first quaternion - * @param q2 The second quaternion - * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 - * Slerp assumes constant velocity between positions. */ -SIMD_FORCE_INLINE btQuaternion -slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) -{ - return q1.slerp(q2, t); -} - -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) - 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()); -#endif -} - -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); - - 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 - } - - 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); -} - -SIMD_FORCE_INLINE btQuaternion -shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) -{ - v0.normalize(); - v1.normalize(); - return shortestArcQuat(v0,v1); -} - -#endif //BT_SIMD__QUATERNION_H_ - - - diff --git a/WickedEngine/BULLET/LinearMath/btQuickprof.cpp b/WickedEngine/BULLET/LinearMath/btQuickprof.cpp deleted file mode 100644 index 544aee89d..000000000 --- a/WickedEngine/BULLET/LinearMath/btQuickprof.cpp +++ /dev/null @@ -1,566 +0,0 @@ -/* - -*************************************************************************************************** -** -** profile.cpp -** -** Real-Time Hierarchical Profiling for Game Programming Gems 3 -** -** by Greg Hjelstrom & Byon Garrabrant -** -***************************************************************************************************/ - -// Credits: The Clock class was inspired by the Timer classes in -// Ogre (www.ogre3d.org). - -#include "btQuickprof.h" - -#ifndef BT_NO_PROFILE - - -static btClock gProfileClock; - - -#ifdef __CELLOS_LV2__ -#include -#include -#include -#endif - -#if defined (SUNOS) || defined (__SUNOS__) -#include -#endif - -#if defined(WIN32) || defined(_WIN32) - -#define BT_USE_WINDOWS_TIMERS -#define WIN32_LEAN_AND_MEAN -#define NOWINRES -#define NOMCX -#define NOIME - -#ifdef _XBOX - #include -#else //_XBOX - #include -#endif //_XBOX - -#include - - -#else //_WIN32 -#include -#endif //_WIN32 - -#define mymin(a,b) (a > b ? a : b) - -struct btClockData -{ - -#ifdef BT_USE_WINDOWS_TIMERS - LARGE_INTEGER mClockFrequency; - DWORD mStartTick; - LONGLONG mPrevElapsedTime; - LARGE_INTEGER mStartTime; -#else -#ifdef __CELLOS_LV2__ - uint64_t mStartTime; -#else - struct timeval mStartTime; -#endif -#endif //__CELLOS_LV2__ - -}; - -///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling. -btClock::btClock() -{ - m_data = new btClockData; -#ifdef BT_USE_WINDOWS_TIMERS - QueryPerformanceFrequency(&m_data->mClockFrequency); -#endif - reset(); -} - -btClock::~btClock() -{ - delete m_data; -} - -btClock::btClock(const btClock& other) -{ - m_data = new btClockData; - *m_data = *other.m_data; -} - -btClock& btClock::operator=(const btClock& other) -{ - *m_data = *other.m_data; - return *this; -} - - - /// Resets the initial reference time. -void btClock::reset() -{ -#ifdef BT_USE_WINDOWS_TIMERS - QueryPerformanceCounter(&m_data->mStartTime); - m_data->mStartTick = GetTickCount(); - m_data->mPrevElapsedTime = 0; -#else -#ifdef __CELLOS_LV2__ - - typedef uint64_t ClockSize; - ClockSize newTime; - //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); - SYS_TIMEBASE_GET( newTime ); - m_data->mStartTime = newTime; -#else - gettimeofday(&m_data->mStartTime, 0); -#endif -#endif -} - -/// Returns the time in ms since the last call to reset or since -/// the btClock was created. -unsigned long int btClock::getTimeMilliseconds() -{ -#ifdef BT_USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; - QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - - m_data->mStartTime.QuadPart; - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - - // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; - - return msecTicks; -#else - -#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"); - - 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__ -#endif -} - - /// Returns the time in us since the last call to reset or since - /// the Clock was created. -unsigned long int btClock::getTimeMicroseconds() -{ -#ifdef BT_USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; - QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - - m_data->mStartTime.QuadPart; - - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick; - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; - - // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - - return usecTicks; -#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 ); - - 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__ -#endif -} - - - - - -inline void Profile_Get_Ticks(unsigned long int * ticks) -{ - *ticks = gProfileClock.getTimeMicroseconds(); -} - -inline float Profile_Get_Tick_Rate(void) -{ -// return 1000000.f; - return 1000.f; - -} - - - -/*************************************************************************************************** -** -** CProfileNode -** -***************************************************************************************************/ - -/*********************************************************************************************** - * INPUT: * - * name - pointer to a static string which is the name of this profile node * - * parent - parent pointer * - * * - * WARNINGS: * - * The name is assumed to be a static pointer, only the pointer is stored and compared for * - * efficiency reasons. * - *=============================================================================================*/ -CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) : - Name( name ), - TotalCalls( 0 ), - TotalTime( 0 ), - StartTime( 0 ), - RecursionCounter( 0 ), - Parent( parent ), - Child( NULL ), - Sibling( NULL ), - m_userPtr(0) -{ - Reset(); -} - - -void CProfileNode::CleanupMemory() -{ - delete ( Child); - Child = NULL; - delete ( Sibling); - Sibling = NULL; -} - -CProfileNode::~CProfileNode( void ) -{ - delete ( Child); - delete ( Sibling); -} - - -/*********************************************************************************************** - * INPUT: * - * name - static string pointer to the name of the node we are searching for * - * * - * WARNINGS: * - * All profile names are assumed to be static strings so this function uses pointer compares * - * to find the named node. * - *=============================================================================================*/ -CProfileNode * CProfileNode::Get_Sub_Node( const char * name ) -{ - // Try to find this sub node - CProfileNode * child = Child; - while ( child ) { - if ( child->Name == name ) { - return child; - } - child = child->Sibling; - } - - // We didn't find it, so add it - - CProfileNode * node = new CProfileNode( name, this ); - node->Sibling = Child; - Child = node; - return node; -} - - -void CProfileNode::Reset( void ) -{ - TotalCalls = 0; - TotalTime = 0.0f; - - - if ( Child ) { - Child->Reset(); - } - if ( Sibling ) { - Sibling->Reset(); - } -} - - -void CProfileNode::Call( void ) -{ - TotalCalls++; - if (RecursionCounter++ == 0) { - Profile_Get_Ticks(&StartTime); - } -} - - -bool CProfileNode::Return( void ) -{ - if ( --RecursionCounter == 0 && TotalCalls != 0 ) { - unsigned long int time; - Profile_Get_Ticks(&time); - time-=StartTime; - TotalTime += (float)time / Profile_Get_Tick_Rate(); - } - return ( RecursionCounter == 0 ); -} - - -/*************************************************************************************************** -** -** CProfileIterator -** -***************************************************************************************************/ -CProfileIterator::CProfileIterator( CProfileNode * start ) -{ - CurrentParent = start; - CurrentChild = CurrentParent->Get_Child(); -} - - -void CProfileIterator::First(void) -{ - CurrentChild = CurrentParent->Get_Child(); -} - - -void CProfileIterator::Next(void) -{ - CurrentChild = CurrentChild->Get_Sibling(); -} - - -bool CProfileIterator::Is_Done(void) -{ - return CurrentChild == NULL; -} - - -void CProfileIterator::Enter_Child( int index ) -{ - CurrentChild = CurrentParent->Get_Child(); - while ( (CurrentChild != NULL) && (index != 0) ) { - index--; - CurrentChild = CurrentChild->Get_Sibling(); - } - - if ( CurrentChild != NULL ) { - CurrentParent = CurrentChild; - CurrentChild = CurrentParent->Get_Child(); - } -} - - -void CProfileIterator::Enter_Parent( void ) -{ - if ( CurrentParent->Get_Parent() != NULL ) { - CurrentParent = CurrentParent->Get_Parent(); - } - CurrentChild = CurrentParent->Get_Child(); -} - - -/*************************************************************************************************** -** -** CProfileManager -** -***************************************************************************************************/ - -CProfileNode CProfileManager::Root( "Root", NULL ); -CProfileNode * CProfileManager::CurrentNode = &CProfileManager::Root; -int CProfileManager::FrameCounter = 0; -unsigned long int CProfileManager::ResetTime = 0; - - -/*********************************************************************************************** - * CProfileManager::Start_Profile -- Begin a named profile * - * * - * Steps one level deeper into the tree, if a child already exists with the specified name * - * then it accumulates the profiling; otherwise a new child node is added to the profile tree. * - * * - * INPUT: * - * name - name of this profiling record * - * * - * WARNINGS: * - * The string used is assumed to be a static string; pointer compares are used throughout * - * the profiling code for efficiency. * - *=============================================================================================*/ -void CProfileManager::Start_Profile( const char * name ) -{ - if (name != CurrentNode->Get_Name()) { - CurrentNode = CurrentNode->Get_Sub_Node( name ); - } - - CurrentNode->Call(); -} - - -/*********************************************************************************************** - * CProfileManager::Stop_Profile -- Stop timing and record the results. * - *=============================================================================================*/ -void CProfileManager::Stop_Profile( void ) -{ - // Return will indicate whether we should back up to our parent (we may - // be profiling a recursive function) - if (CurrentNode->Return()) { - CurrentNode = CurrentNode->Get_Parent(); - } -} - - -/*********************************************************************************************** - * CProfileManager::Reset -- Reset the contents of the profiling system * - * * - * This resets everything except for the tree structure. All of the timing data is reset. * - *=============================================================================================*/ -void CProfileManager::Reset( void ) -{ - gProfileClock.reset(); - Root.Reset(); - Root.Call(); - FrameCounter = 0; - Profile_Get_Ticks(&ResetTime); -} - - -/*********************************************************************************************** - * CProfileManager::Increment_Frame_Counter -- Increment the frame counter * - *=============================================================================================*/ -void CProfileManager::Increment_Frame_Counter( void ) -{ - FrameCounter++; -} - - -/*********************************************************************************************** - * CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset * - *=============================================================================================*/ -float CProfileManager::Get_Time_Since_Reset( void ) -{ - unsigned long int time; - Profile_Get_Ticks(&time); - time -= ResetTime; - return (float)time / Profile_Get_Tick_Rate(); -} - -#include - -void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing) -{ - profileIterator->First(); - if (profileIterator->Is_Done()) - return; - - float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time(); - int i; - int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset(); - for (i=0;iGet_Current_Parent_Name(), parent_time ); - float totalTime = 0.f; - - - int numChildren = 0; - - for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next()) - { - numChildren++; - float current_total_time = profileIterator->Get_Current_Total_Time(); - accumulated_time += current_total_time; - float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f; - { - int i; for (i=0;iGet_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls()); - totalTime += current_total_time; - //recurse into children - } - - if (parent_time < accumulated_time) - { - printf("what's wrong\n"); - } - for (i=0;i SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time); - - for (i=0;iEnter_Child(i); - dumpRecursive(profileIterator,spacing+3); - profileIterator->Enter_Parent(); - } -} - - - -void CProfileManager::dumpAll() -{ - CProfileIterator* profileIterator = 0; - profileIterator = CProfileManager::Get_Iterator(); - - dumpRecursive(profileIterator,0); - - CProfileManager::Release_Iterator(profileIterator); -} - - - - -#endif //BT_NO_PROFILE diff --git a/WickedEngine/BULLET/LinearMath/btQuickprof.h b/WickedEngine/BULLET/LinearMath/btQuickprof.h deleted file mode 100644 index cb8be141a..000000000 --- a/WickedEngine/BULLET/LinearMath/btQuickprof.h +++ /dev/null @@ -1,203 +0,0 @@ - -/*************************************************************************************************** -** -** Real-Time Hierarchical Profiling for Game Programming Gems 3 -** -** by Greg Hjelstrom & Byon Garrabrant -** -***************************************************************************************************/ - -// Credits: The Clock class was inspired by the Timer classes in -// Ogre (www.ogre3d.org). - - - -#ifndef BT_QUICK_PROF_H -#define BT_QUICK_PROF_H - -//To disable built-in profiling, please comment out next line -#define BT_NO_PROFILE 1 -#ifndef BT_NO_PROFILE -#include //@todo remove this, backwards compatibility -#include "btScalar.h" -#include "btAlignedAllocator.h" -#include - - - - - -#define USE_BT_CLOCK 1 - -#ifdef USE_BT_CLOCK - -///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling. -class btClock -{ -public: - btClock(); - - btClock(const btClock& other); - btClock& operator=(const btClock& other); - - ~btClock(); - - /// Resets the initial reference time. - void reset(); - - /// Returns the time in ms since the last call to reset or since - /// the btClock was created. - unsigned long int getTimeMilliseconds(); - - /// Returns the time in us since the last call to reset or since - /// the Clock was created. - unsigned long int getTimeMicroseconds(); -private: - struct btClockData* m_data; -}; - -#endif //USE_BT_CLOCK - - - - -///A node in the Profile Hierarchy Tree -class CProfileNode { - -public: - CProfileNode( const char * name, CProfileNode * parent ); - ~CProfileNode( void ); - - 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; } - - 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;} -protected: - - const char * Name; - int TotalCalls; - float TotalTime; - unsigned long int StartTime; - int RecursionCounter; - - CProfileNode * Parent; - CProfileNode * Child; - CProfileNode * Sibling; - void* m_userPtr; -}; - -///An iterator to navigate through the tree -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 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(); } - - 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(); } - - - -protected: - - CProfileNode * CurrentParent; - CProfileNode * CurrentChild; - - - CProfileIterator( CProfileNode * start ); - friend class CProfileManager; -}; - - -///The Manager for the Profile system -class CProfileManager { -public: - static void Start_Profile( const char * name ); - static void Stop_Profile( void ); - - static void CleanupMemory(void) - { - Root.CleanupMemory(); - } - - static void Reset( void ); - static void Increment_Frame_Counter( void ); - static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; } - static float Get_Time_Since_Reset( void ); - - static CProfileIterator * Get_Iterator( void ) - { - - return new CProfileIterator( &Root ); - } - static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } - - static void dumpRecursive(CProfileIterator* profileIterator, int spacing); - - static void dumpAll(); - -private: - static CProfileNode Root; - static CProfileNode * CurrentNode; - static int FrameCounter; - static unsigned long int ResetTime; -}; - - -///ProfileSampleClass is a simple way to profile a function's scope -///Use the BT_PROFILE macro at the start of scope to time -class CProfileSample { -public: - CProfileSample( const char * name ) - { - CProfileManager::Start_Profile( name ); - } - - ~CProfileSample( void ) - { - CProfileManager::Stop_Profile(); - } -}; - - -#define BT_PROFILE( name ) CProfileSample __profile( name ) - -#else - -#define BT_PROFILE( name ) - -#endif //#ifndef BT_NO_PROFILE - - - -#endif //BT_QUICK_PROF_H - - diff --git a/WickedEngine/BULLET/LinearMath/btRandom.h b/WickedEngine/BULLET/LinearMath/btRandom.h deleted file mode 100644 index 4cbfc6bfe..000000000 --- a/WickedEngine/BULLET/LinearMath/btRandom.h +++ /dev/null @@ -1,42 +0,0 @@ -/* -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 BT_GEN_RANDOM_H -#define BT_GEN_RANDOM_H - -#ifdef MT19937 - -#include -#include - -#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(); } - -#else - -#include - -#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(); } - -#endif - -#endif //BT_GEN_RANDOM_H - diff --git a/WickedEngine/BULLET/LinearMath/btScalar.h b/WickedEngine/BULLET/LinearMath/btScalar.h deleted file mode 100644 index a531672df..000000000 --- a/WickedEngine/BULLET/LinearMath/btScalar.h +++ /dev/null @@ -1,735 +0,0 @@ -/* -Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef BT_SCALAR_H -#define BT_SCALAR_H - -#ifdef BT_MANAGED_CODE -//Aligned data types not supported in managed code -#pragma unmanaged -#endif - - -#include -#include //size_t for MSVC 6.0 -#include - -/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 282 - -inline int btGetVersion() -{ - return BT_BULLET_VERSION; -} - -#if defined(DEBUG) || defined (_DEBUG) -#define BT_DEBUG -#endif - - -#ifdef _WIN32 - - #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) - - #define SIMD_FORCE_INLINE inline - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #else - //#define BT_HAS_ALIGNED_ALLOCATOR - #pragma warning(disable : 4324) // disable padding warning -// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. -// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines -// #pragma warning(disable:4786) // Disable the "debug name too long" warning - - #define SIMD_FORCE_INLINE __forceinline - #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a - #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a - #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a - #ifdef _XBOX - #define BT_USE_VMX128 - - #include - #define BT_HAVE_NATIVE_FSEL - #define btFsel(a,b,c) __fsel((a),(b),(c)) - #else - -#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) - #if _MSC_VER>1400 - #define BT_USE_SIMD_VECTOR3 - #endif - - #define BT_USE_SSE - #ifdef BT_USE_SSE - //BT_USE_SSE_IN_API is disabled under Windows by default, because - //it makes it harder to integrate Bullet into your application under Windows - //(structured embedding Bullet structs/classes need to be 16-byte aligned) - //with relatively little performance gain - //If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries - //you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage) - //#define BT_USE_SSE_IN_API - #endif //BT_USE_SSE - #include -#endif - - #endif//_XBOX - - #endif //__MINGW32__ - -#ifdef BT_DEBUG - #if defined(_MSC_VER) && !defined(__clang__) - #include - #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }} - #else//_MSC_VER - #include - #define btAssert assert - #endif//_MSC_VER -#else - #define btAssert(x) -#endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - - #define btLikely(_c) _c - #define btUnlikely(_c) _c - -#else - -#if defined (__CELLOS_LV2__) - #define SIMD_FORCE_INLINE inline __attribute__((always_inline)) - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef BT_DEBUG -#ifdef __SPU__ -#include -#define printf spu_printf - #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} -#else - #define btAssert assert -#endif - -#else - #define btAssert(x) -#endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - - #define btLikely(_c) _c - #define btUnlikely(_c) _c - -#else - -#ifdef USE_LIBSPE2 - - #define SIMD_FORCE_INLINE __inline - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef BT_DEBUG - #define btAssert assert -#else - #define btAssert(x) -#endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - - - #define btLikely(_c) __builtin_expect((_c), 1) - #define btUnlikely(_c) __builtin_expect((_c), 0) - - -#else - //non-windows systems - -#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION))) - #if defined (__i386__) || defined (__x86_64__) - #define BT_USE_SIMD_VECTOR3 - #define BT_USE_SSE - //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries - //if apps run into issues, we will disable the next line - #define BT_USE_SSE_IN_API - #ifdef BT_USE_SSE - // include appropriate SSE level - #if defined (__SSE4_1__) - #include - #elif defined (__SSSE3__) - #include - #elif defined (__SSE3__) - #include - #else - #include - #endif - #endif //BT_USE_SSE - #elif defined( __ARM_NEON__ ) - #ifdef __clang__ - #define BT_USE_NEON 1 - #define BT_USE_SIMD_VECTOR3 - - #if defined BT_USE_NEON && defined (__clang__) - #include - #endif//BT_USE_NEON - #endif //__clang__ - #endif//__arm__ - - #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline)) -///@todo: check out alignment methods for other platforms/compilers - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif - - #if defined(DEBUG) || defined (_DEBUG) - #if defined (__i386__) || defined (__x86_64__) - #include - #define btAssert(x)\ - {\ - if(!(x))\ - {\ - printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\ - asm volatile ("int3");\ - }\ - } - #else//defined (__i386__) || defined (__x86_64__) - #define btAssert assert - #endif//defined (__i386__) || defined (__x86_64__) - #else//defined(DEBUG) || defined (_DEBUG) - #define btAssert(x) - #endif//defined(DEBUG) || defined (_DEBUG) - - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - #define btLikely(_c) _c - #define btUnlikely(_c) _c - -#else - - #define SIMD_FORCE_INLINE inline - ///@todo: check out alignment methods for other platforms/compilers - ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #ifndef assert - #include - #endif - -#if defined(DEBUG) || defined (_DEBUG) - #define btAssert assert -#else - #define btAssert(x) -#endif - - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - #define btLikely(_c) _c - #define btUnlikely(_c) _c -#endif //__APPLE__ - -#endif // LIBSPE2 - -#endif //__CELLOS_LV2__ -#endif - - -///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. -#if defined(BT_USE_DOUBLE_PRECISION) - -typedef double btScalar; -//this number could be bigger in double precision -#define BT_LARGE_FLOAT 1e30 -#else - -typedef float btScalar; -//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX -#define BT_LARGE_FLOAT 1e18f -#endif - -#ifdef BT_USE_SSE -typedef __m128 btSimdFloat4; -#endif//BT_USE_SSE - -#if defined (BT_USE_SSE) -//#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE) -#ifdef _WIN32 - -#ifndef BT_NAN -static int btNanMask = 0x7F800001; -#define BT_NAN (*(float*)&btNanMask) -#endif - -#ifndef BT_INFINITY -static int btInfinityMask = 0x7F800000; -#define BT_INFINITY (*(float*)&btInfinityMask) -#endif - -#if defined(__clang__) -#define BT_NO_SIMD_OPERATOR_OVERLOADS -#endif // clang - -//use this, in case there are clashes (such as xnamath.h) -#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS -inline __m128 operator + (const __m128 A, const __m128 B) -{ - return _mm_add_ps(A, B); -} - -inline __m128 operator - (const __m128 A, const __m128 B) -{ - return _mm_sub_ps(A, B); -} - -inline __m128 operator * (const __m128 A, const __m128 B) -{ - return _mm_mul_ps(A, B); -} -#endif //BT_NO_SIMD_OPERATOR_OVERLOADS - -#define btCastfTo128i(a) (_mm_castps_si128(a)) -#define btCastfTo128d(a) (_mm_castps_pd(a)) -#define btCastiTo128f(a) (_mm_castsi128_ps(a)) -#define btCastdTo128f(a) (_mm_castpd_ps(a)) -#define btCastdTo128i(a) (_mm_castpd_si128(a)) -#define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3) - -#else//_WIN32 - -#define btCastfTo128i(a) ((__m128i)(a)) -#define btCastfTo128d(a) ((__m128d)(a)) -#define btCastiTo128f(a) ((__m128) (a)) -#define btCastdTo128f(a) ((__m128) (a)) -#define btCastdTo128i(a) ((__m128i)(a)) -#define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3} -#define BT_INFINITY INFINITY -#define BT_NAN NAN -#endif//_WIN32 -#else - -#ifdef BT_USE_NEON - #include - - typedef float32x4_t btSimdFloat4; - #define BT_INFINITY INFINITY - #define BT_NAN NAN - #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} -#else//BT_USE_NEON - - #ifndef BT_INFINITY - static int btInfinityMask = 0x7F800000; - #define BT_INFINITY (*(float*)&btInfinityMask) - #endif -#endif//BT_USE_NEON - -#endif //BT_USE_SSE - -#ifdef BT_USE_NEON -#include - -typedef float32x4_t btSimdFloat4; -#define BT_INFINITY INFINITY -#define BT_NAN NAN -#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} -#endif - - - - - -#define BT_DECLARE_ALIGNED_ALLOCATOR() \ - SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ - SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \ - SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ - SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ - SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ - SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \ - SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ - SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ - - - -#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) - -SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } -SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } -SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (xbtScalar(1)) x=btScalar(1); return acos(x); } -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (xbtScalar(1)) x=btScalar(1); return asin(x); } -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } -SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } -SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } -SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } -SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } - -#else - -SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) -{ -#ifdef USE_APPROXIMATION - double x, z, tempf; - unsigned long *tfptr = ((unsigned long *)&tempf) + 1; - - tempf = y; - *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ - x = tempf; - z = y*btScalar(0.5); - x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - return x*y; -#else - return sqrtf(y); -#endif -} -SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } -SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { - if (xbtScalar(1)) - x=btScalar(1); - return acosf(x); -} -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { - if (xbtScalar(1)) - x=btScalar(1); - return asinf(x); -} -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } -SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } -SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } -SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } -SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } - -#endif - -#define SIMD_PI btScalar(3.1415926535897932384626433832795029) -#define SIMD_2_PI btScalar(2.0) * SIMD_PI -#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5)) -#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) -#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) -#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) - -#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ -#define btRecip(x) (btScalar(1.0)/btScalar(x)) - -#ifdef BT_USE_DOUBLE_PRECISION -#define SIMD_EPSILON DBL_EPSILON -#define SIMD_INFINITY DBL_MAX -#else -#define SIMD_EPSILON FLT_EPSILON -#define SIMD_INFINITY FLT_MAX -#endif - -SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) -{ - btScalar coeff_1 = SIMD_PI / 4.0f; - btScalar coeff_2 = 3.0f * coeff_1; - btScalar abs_y = btFabs(y); - btScalar angle; - if (x >= 0.0f) { - btScalar r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - btScalar r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } - -SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) { - return (((a) <= eps) && !((a) < -eps)); -} -SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) { - return (!((a) <= eps)); -} - - -SIMD_FORCE_INLINE int btIsNegative(btScalar x) { - return x < btScalar(0.0) ? 1 : 0; -} - -SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } -SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; } - -#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name - -#ifndef btFsel -SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) -{ - return a >= 0 ? b : c; -} -#endif -#define btFsels(a,b,c) (btScalar)btFsel(a,b,c) - - -SIMD_FORCE_INLINE bool btMachineIsLittleEndian() -{ - long int i = 1; - const char *p = (const char *) &i; - if (p[0] == 1) // Lowest address contains the least significant byte - return true; - else - return false; -} - - - -///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 -///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html -SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) -{ - // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero - // Rely on positive value or'ed with its negative having sign bit on - // and zero value or'ed with its negative (which is still zero) having sign bit off - // Use arithmetic shift right, shifting the sign bit through all 32 bits - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) -{ - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) -{ -#ifdef BT_HAVE_NATIVE_FSEL - return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); -#else - return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; -#endif -} - -template SIMD_FORCE_INLINE void btSwap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; -} - - -//PCK: endian swapping functions -SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val) -{ - return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); -} - -SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val) -{ - return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); -} - -SIMD_FORCE_INLINE unsigned btSwapEndian(int val) -{ - return btSwapEndian((unsigned)val); -} - -SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) -{ - return btSwapEndian((unsigned short) val); -} - -///btSwapFloat uses using char pointers to swap the endianness -////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values -///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. -///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. -///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. -///so instead of returning a float/double, we return integer/long long integer -SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d) -{ - unsigned int a = 0; - unsigned char *dst = (unsigned char *)&a; - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - return a; -} - -// unswap using char pointers -SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) -{ - float d = 0.0f; - unsigned char *src = (unsigned char *)&a; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - - return d; -} - - -// swap using char pointers -SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst) -{ - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - -} - -// unswap using char pointers -SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) -{ - double d = 0.0; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - - return d; -} - -template -SIMD_FORCE_INLINE void btSetZero(T* a, int n) -{ - T* acurr = a; - size_t ncurr = n; - while (ncurr > 0) - { - *(acurr++) = 0; - --ncurr; - } -} - - -SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n) -{ - btScalar p0,q0,m0,p1,q1,m1,sum; - sum = 0; - n -= 2; - while (n >= 0) { - p0 = a[0]; q0 = b[0]; - m0 = p0 * q0; - p1 = a[1]; q1 = b[1]; - m1 = p1 * q1; - sum += m0; - sum += m1; - a += 2; - b += 2; - n -= 2; - } - n += 2; - while (n > 0) { - sum += (*a) * (*b); - a++; - b++; - n--; - } - return sum; -} - - -// returns normalized value in range [-SIMD_PI, SIMD_PI] -SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) -{ - angleInRadians = btFmod(angleInRadians, SIMD_2_PI); - if(angleInRadians < -SIMD_PI) - { - return angleInRadians + SIMD_2_PI; - } - else if(angleInRadians > SIMD_PI) - { - return angleInRadians - SIMD_2_PI; - } - else - { - return angleInRadians; - } -} - - - -///rudimentary class to provide type info -struct btTypedObject -{ - btTypedObject(int objectType) - :m_objectType(objectType) - { - } - int m_objectType; - inline int getObjectType() const - { - return m_objectType; - } -}; - - - -///align a pointer to the provided alignment, upwards -template T* btAlignPointer(T* unalignedPtr, size_t alignment) -{ - - struct btConvertPointerSizeT - { - union - { - T* ptr; - size_t integer; - }; - }; - btConvertPointerSizeT converter; - - - const size_t bit_mask = ~(alignment - 1); - converter.ptr = unalignedPtr; - converter.integer += alignment-1; - converter.integer &= bit_mask; - return converter.ptr; -} - -#endif //BT_SCALAR_H diff --git a/WickedEngine/BULLET/LinearMath/btSerializer.cpp b/WickedEngine/BULLET/LinearMath/btSerializer.cpp deleted file mode 100644 index ba3449395..000000000 --- a/WickedEngine/BULLET/LinearMath/btSerializer.cpp +++ /dev/null @@ -1,991 +0,0 @@ -char sBulletDNAstr[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(69),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), -char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), -char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), -char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), -char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), -char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), -char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), -char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), -char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), -char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), -char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), -char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), -char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), -char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), -char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), -char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), -char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), -char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), -char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), -char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), -char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), -char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), -char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), -char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), -char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), -char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), -char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), -char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), -char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), -char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), -char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), -char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), -char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), -char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), -char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), -char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), -char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), -char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), -char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), -char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), -char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), -char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), -char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), -char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), -char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), -char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), -char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), -char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), -char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), -char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), -char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), -char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), -char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), -char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), -char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), -char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), -char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), -char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), -char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), -char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), -char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), -char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68), -char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122), -char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110), -char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), -char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), -char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121), -char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), -char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100), -char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117), -char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104), -char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105), -char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109),char(95),char(114),char(111),char(111),char(116),char(67),char(111),char(108),char(108),char(105), -char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97), -char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101), -char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), -char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116), -char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(114),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114), -char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99), -char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114), -char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116), -char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84), -char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95), -char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67), -char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73), -char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110), -char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0), -char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97), -char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103), -char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70), -char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101), -char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105), -char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0), -char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101), -char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76), -char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), -char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), -char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117), -char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111), -char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103), -char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110), -char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65), -char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101), -char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112), -char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100), -char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97), -char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68), -char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100), -char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98), -char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104), -char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100), -char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109), -char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101), -char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102), -char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101), -char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114), -char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116), -char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105), -char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116), -char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111), -char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119), -char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110), -char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), -char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110), -char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116), -char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109), -char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110), -char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), -char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83), -char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103), -char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65), -char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109), -char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97), -char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111), -char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111), -char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110), -char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119), -char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95), -char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109), -char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114), -char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117), -char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114), -char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116), -char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0), -char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95), -char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97), -char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95), -char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101), -char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110), -char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100), -char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114), -char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), -char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101), -char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111), -char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111), -char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95), -char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117), -char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105), -char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99), -char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116), -char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114), -char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100), -char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111), -char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103), -char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105), -char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102), -char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83), -char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116), -char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73), -char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), -char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97), -char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87), -char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102), -char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95), -char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112), -char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115), -char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97), -char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), -char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97), -char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95), -char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109), -char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0), -char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97), -char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109), -char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108), -char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109), -char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105), -char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109), -char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100), -char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50), -char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101), -char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50), -char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121), -char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109), -char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0), -char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109), -char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), -char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116), -char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77), -char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114), -char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74), -char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(84),char(89),char(80),char(69), -char(87),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(111), -char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108), -char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), -char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101), -char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115), -char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108), -char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108), -char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49), -char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110), -char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105), -char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111), -char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0), -char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119), -char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67), -char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50), -char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(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(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(60),char(0), -char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1),char(0),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1), -char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0),char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1), -char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(-52),char(0),char(108),char(1), -char(92),char(0),char(-116),char(0),char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0), -char(92),char(1),char(104),char(0),char(-84),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(76),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(78),char(0), -char(0),char(0),char(37),char(0),char(43),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(44),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(37),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),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(45),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(96),char(0),char(46),char(0),char(5),char(0),char(27),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(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), -char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0), -char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), -char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), -char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0), -char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), -char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0), -char(50),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0), -char(53),char(0),char(21),char(0),char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), -char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0), -char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), -char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0), -char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0), -char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0), -char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), -char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0), -char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0), -char(55),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0), -char(53),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), -char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0), -char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0), -char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0), -char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0), -char(4),char(0),char(-97),char(0),char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0), -char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0), -char(0),char(0),char(-96),char(0),char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), -char(61),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0), -char(57),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0), -char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), -char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), -char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), -char(7),char(0),char(-81),char(0),char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0), -char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), -char(0),char(0),char(-80),char(0),char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0), -char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0), -char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), -char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0), -char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0), -char(68),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(71),char(0),char(5),char(0),char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), -char(8),char(0),char(-65),char(0),char(72),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0), -char(7),char(0),char(-75),char(0),char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(73),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(8),char(0),char(-75),char(0),char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(74),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0), -char(7),char(0),char(-62),char(0),char(0),char(0),char(37),char(0),char(75),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-64),char(0), -char(14),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-76),char(0), -char(8),char(0),char(111),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(113),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(8),char(0),char(-51),char(0),char(8),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(4),char(0),char(-47),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(0),char(0),char(37),char(0), -char(52),char(0),char(22),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(113),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(7),char(0),char(-51),char(0), -char(7),char(0),char(-50),char(0),char(7),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),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(0),char(0),char(37),char(0),char(76),char(0),char(4),char(0),char(7),char(0),char(-43),char(0), -char(7),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(4),char(0),char(79),char(0),char(77),char(0),char(10),char(0),char(76),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(7),char(0),char(-120),char(0),char(7),char(0),char(-34),char(0),char(4),char(0),char(-33),char(0),char(4),char(0),char(53),char(0),char(78),char(0),char(4),char(0), -char(76),char(0),char(-40),char(0),char(4),char(0),char(-32),char(0),char(7),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(79),char(0),char(4),char(0), -char(13),char(0),char(-35),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(80),char(0),char(7),char(0), -char(13),char(0),char(-27),char(0),char(76),char(0),char(-40),char(0),char(4),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(4),char(0),char(53),char(0),char(81),char(0),char(6),char(0),char(15),char(0),char(-22),char(0),char(13),char(0),char(-24),char(0), -char(13),char(0),char(-21),char(0),char(58),char(0),char(-20),char(0),char(4),char(0),char(-19),char(0),char(7),char(0),char(-23),char(0),char(82),char(0),char(26),char(0), -char(4),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),char(7),char(0),char(-76),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(7),char(0),char(-4),char(0),char(7),char(0),char(-3),char(0),char(7),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(7),char(0),char(0),char(1), -char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(4),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(4),char(0),char(118),char(0),char(83),char(0),char(12),char(0),char(15),char(0),char(6),char(1),char(15),char(0),char(7),char(1),char(15),char(0),char(8),char(1), -char(13),char(0),char(9),char(1),char(13),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(4),char(0),char(12),char(1),char(4),char(0),char(13),char(1), -char(4),char(0),char(14),char(1),char(4),char(0),char(15),char(1),char(7),char(0),char(-25),char(0),char(4),char(0),char(53),char(0),char(84),char(0),char(27),char(0), -char(17),char(0),char(16),char(1),char(15),char(0),char(17),char(1),char(15),char(0),char(18),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(19),char(1), -char(13),char(0),char(20),char(1),char(13),char(0),char(21),char(1),char(13),char(0),char(22),char(1),char(13),char(0),char(23),char(1),char(4),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(4),char(0),char(31),char(1),char(4),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(7),char(0),char(37),char(1),char(7),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(41),char(1),char(85),char(0),char(12),char(0),char(9),char(0),char(42),char(1),char(9),char(0),char(43),char(1), -char(13),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(7),char(0),char(-57),char(0),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(13),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(4),char(0),char(51),char(1),char(4),char(0),char(53),char(0), -char(86),char(0),char(19),char(0),char(48),char(0),char(126),char(0),char(83),char(0),char(52),char(1),char(76),char(0),char(53),char(1),char(77),char(0),char(54),char(1), -char(78),char(0),char(55),char(1),char(79),char(0),char(56),char(1),char(80),char(0),char(57),char(1),char(81),char(0),char(58),char(1),char(84),char(0),char(59),char(1), -char(85),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(82),char(0),char(68),char(1), -}; -int sBulletDNAlen= sizeof(sBulletDNAstr); - -char sBulletDNAstr64[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(69),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), -char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), -char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), -char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), -char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), -char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), -char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), -char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), -char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), -char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), -char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), -char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), -char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), -char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), -char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), -char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), -char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), -char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), -char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), -char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), -char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), -char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), -char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), -char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), -char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), -char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), -char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), -char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), -char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), -char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), -char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), -char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), -char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), -char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), -char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), -char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), -char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), -char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), -char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), -char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), -char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), -char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), -char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), -char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), -char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), -char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), -char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), -char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), -char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), -char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), -char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), -char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), -char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), -char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), -char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), -char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), -char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), -char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), -char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), -char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), -char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), -char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68), -char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122), -char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110), -char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), -char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), -char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121), -char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), -char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100), -char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117), -char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104), -char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105), -char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109),char(95),char(114),char(111),char(111),char(116),char(67),char(111),char(108),char(108),char(105), -char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97), -char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101), -char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), -char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116), -char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(114),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114), -char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99), -char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114), -char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116), -char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84), -char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95), -char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67), -char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73), -char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110), -char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0), -char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97), -char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103), -char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70), -char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101), -char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105), -char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0), -char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101), -char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76), -char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), -char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), -char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117), -char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111), -char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103), -char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110), -char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65), -char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101), -char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112), -char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100), -char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97), -char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68), -char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100), -char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98), -char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104), -char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100), -char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109), -char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101), -char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102), -char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101), -char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114), -char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116), -char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105), -char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116), -char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111), -char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119), -char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110), -char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), -char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110), -char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116), -char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109), -char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110), -char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), -char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83), -char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103), -char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65), -char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109), -char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97), -char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111), -char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111), -char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110), -char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119), -char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95), -char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109), -char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114), -char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117), -char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114), -char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116), -char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115), -char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0), -char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95), -char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97), -char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95), -char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101), -char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110), -char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100), -char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114), -char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), -char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101), -char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111), -char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111), -char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95), -char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117), -char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105), -char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99), -char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99), -char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116), -char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114), -char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100), -char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111), -char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103), -char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105), -char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116), -char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102), -char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83), -char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116), -char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73), -char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), -char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116), -char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97), -char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87), -char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102), -char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95), -char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112), -char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115), -char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97), -char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), -char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97), -char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95), -char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109), -char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0), -char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97), -char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109), -char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108), -char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109), -char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105), -char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109), -char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100), -char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50), -char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101), -char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50), -char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121), -char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109), -char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0), -char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109), -char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), -char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116), -char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77), -char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114), -char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0), -char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74), -char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(0),char(84),char(89),char(80),char(69), -char(87),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(111), -char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108), -char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), -char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101), -char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115), -char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), -char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108), -char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108), -char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49), -char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100), -char(121),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110), -char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(50),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105), -char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111), -char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0), -char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119), -char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67), -char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50), -char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(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(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(64),char(0), -char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1),char(16),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1), -char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0),char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1), -char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-40),char(0),char(120),char(1), -char(104),char(0),char(-104),char(0),char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0), -char(104),char(1),char(112),char(0),char(-32),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(76),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(78),char(0), -char(0),char(0),char(37),char(0),char(43),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(44),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(37),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(44),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(45),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(96),char(0),char(46),char(0),char(5),char(0),char(27),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(47),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), -char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(18),char(0),char(104),char(0),char(18),char(0),char(105),char(0),char(14),char(0),char(106),char(0), -char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), -char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), -char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(25),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(25),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(17),char(0),char(104),char(0),char(17),char(0),char(105),char(0), -char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), -char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(49),char(0),char(2),char(0), -char(50),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(51),char(0),char(2),char(0),char(52),char(0),char(124),char(0),char(13),char(0),char(125),char(0), -char(53),char(0),char(21),char(0),char(48),char(0),char(126),char(0),char(15),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), -char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0), -char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), -char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0), -char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(54),char(0),char(22),char(0),char(47),char(0),char(126),char(0),char(16),char(0),char(127),char(0), -char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0), -char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), -char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0), -char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0), -char(55),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(56),char(0),char(13),char(0),char(53),char(0),char(-108),char(0), -char(53),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), -char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0), -char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(57),char(0),char(13),char(0),char(58),char(0),char(-108),char(0),char(58),char(0),char(-107),char(0), -char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0), -char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0), -char(4),char(0),char(-97),char(0),char(59),char(0),char(14),char(0),char(54),char(0),char(-108),char(0),char(54),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0), -char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0), -char(0),char(0),char(-96),char(0),char(60),char(0),char(3),char(0),char(57),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), -char(61),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(62),char(0),char(3),char(0), -char(57),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(63),char(0),char(13),char(0),char(57),char(0),char(-95),char(0), -char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), -char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(64),char(0),char(13),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), -char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), -char(7),char(0),char(-81),char(0),char(65),char(0),char(14),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0), -char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), -char(0),char(0),char(-80),char(0),char(66),char(0),char(10),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0), -char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(67),char(0),char(11),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0), -char(17),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(68),char(0),char(9),char(0), -char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), -char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(69),char(0),char(9),char(0), -char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(70),char(0),char(5),char(0), -char(68),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(71),char(0),char(5),char(0),char(69),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), -char(8),char(0),char(-65),char(0),char(72),char(0),char(9),char(0),char(57),char(0),char(-95),char(0),char(17),char(0),char(-92),char(0),char(17),char(0),char(-91),char(0), -char(7),char(0),char(-75),char(0),char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(73),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(18),char(0),char(-92),char(0),char(18),char(0),char(-91),char(0), -char(8),char(0),char(-75),char(0),char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0), -char(4),char(0),char(-70),char(0),char(74),char(0),char(5),char(0),char(56),char(0),char(-95),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0), -char(7),char(0),char(-62),char(0),char(0),char(0),char(37),char(0),char(75),char(0),char(4),char(0),char(59),char(0),char(-95),char(0),char(14),char(0),char(-64),char(0), -char(14),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(50),char(0),char(22),char(0),char(8),char(0),char(-61),char(0),char(8),char(0),char(-76),char(0), -char(8),char(0),char(111),char(0),char(8),char(0),char(-60),char(0),char(8),char(0),char(113),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(8),char(0),char(-51),char(0),char(8),char(0),char(-50),char(0),char(8),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0), -char(4),char(0),char(-47),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(0),char(0),char(37),char(0), -char(52),char(0),char(22),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-60),char(0), -char(7),char(0),char(113),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(7),char(0),char(-51),char(0), -char(7),char(0),char(-50),char(0),char(7),char(0),char(-49),char(0),char(4),char(0),char(-48),char(0),char(4),char(0),char(-47),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(0),char(0),char(37),char(0),char(76),char(0),char(4),char(0),char(7),char(0),char(-43),char(0), -char(7),char(0),char(-42),char(0),char(7),char(0),char(-41),char(0),char(4),char(0),char(79),char(0),char(77),char(0),char(10),char(0),char(76),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(7),char(0),char(-120),char(0),char(7),char(0),char(-34),char(0),char(4),char(0),char(-33),char(0),char(4),char(0),char(53),char(0),char(78),char(0),char(4),char(0), -char(76),char(0),char(-40),char(0),char(4),char(0),char(-32),char(0),char(7),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(79),char(0),char(4),char(0), -char(13),char(0),char(-35),char(0),char(76),char(0),char(-40),char(0),char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(80),char(0),char(7),char(0), -char(13),char(0),char(-27),char(0),char(76),char(0),char(-40),char(0),char(4),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(4),char(0),char(53),char(0),char(81),char(0),char(6),char(0),char(15),char(0),char(-22),char(0),char(13),char(0),char(-24),char(0), -char(13),char(0),char(-21),char(0),char(58),char(0),char(-20),char(0),char(4),char(0),char(-19),char(0),char(7),char(0),char(-23),char(0),char(82),char(0),char(26),char(0), -char(4),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0),char(7),char(0),char(-76),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(7),char(0),char(-4),char(0),char(7),char(0),char(-3),char(0),char(7),char(0),char(-2),char(0),char(7),char(0),char(-1),char(0),char(7),char(0),char(0),char(1), -char(7),char(0),char(1),char(1),char(4),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(4),char(0),char(4),char(1),char(4),char(0),char(5),char(1), -char(4),char(0),char(118),char(0),char(83),char(0),char(12),char(0),char(15),char(0),char(6),char(1),char(15),char(0),char(7),char(1),char(15),char(0),char(8),char(1), -char(13),char(0),char(9),char(1),char(13),char(0),char(10),char(1),char(7),char(0),char(11),char(1),char(4),char(0),char(12),char(1),char(4),char(0),char(13),char(1), -char(4),char(0),char(14),char(1),char(4),char(0),char(15),char(1),char(7),char(0),char(-25),char(0),char(4),char(0),char(53),char(0),char(84),char(0),char(27),char(0), -char(17),char(0),char(16),char(1),char(15),char(0),char(17),char(1),char(15),char(0),char(18),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(19),char(1), -char(13),char(0),char(20),char(1),char(13),char(0),char(21),char(1),char(13),char(0),char(22),char(1),char(13),char(0),char(23),char(1),char(4),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(4),char(0),char(31),char(1),char(4),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(7),char(0),char(37),char(1),char(7),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(41),char(1),char(85),char(0),char(12),char(0),char(9),char(0),char(42),char(1),char(9),char(0),char(43),char(1), -char(13),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(7),char(0),char(-57),char(0),char(7),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(13),char(0),char(48),char(1),char(4),char(0),char(49),char(1),char(4),char(0),char(50),char(1),char(4),char(0),char(51),char(1),char(4),char(0),char(53),char(0), -char(86),char(0),char(19),char(0),char(48),char(0),char(126),char(0),char(83),char(0),char(52),char(1),char(76),char(0),char(53),char(1),char(77),char(0),char(54),char(1), -char(78),char(0),char(55),char(1),char(79),char(0),char(56),char(1),char(80),char(0),char(57),char(1),char(81),char(0),char(58),char(1),char(84),char(0),char(59),char(1), -char(85),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(4),char(0),char(27),char(1),char(4),char(0),char(62),char(1),char(4),char(0),char(63),char(1), -char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(4),char(0),char(66),char(1),char(4),char(0),char(67),char(1),char(82),char(0),char(68),char(1), -}; -int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/WickedEngine/BULLET/LinearMath/btSerializer.h b/WickedEngine/BULLET/LinearMath/btSerializer.h deleted file mode 100644 index ff1dc574c..000000000 --- a/WickedEngine/BULLET/LinearMath/btSerializer.h +++ /dev/null @@ -1,639 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SERIALIZER_H -#define BT_SERIALIZER_H - -#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE -#include "btHashMap.h" - -#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) -#include -#endif -#include - - - -///only the 32bit versions for now -extern char sBulletDNAstr[]; -extern int sBulletDNAlen; -extern char sBulletDNAstr64[]; -extern int sBulletDNAlen64; - -SIMD_FORCE_INLINE int btStrLen(const char* str) -{ - if (!str) - return(0); - int len = 0; - - while (*str != 0) - { - str++; - len++; - } - - return len; -} - - -class btChunk -{ -public: - int m_chunkCode; - int m_length; - void *m_oldPtr; - int m_dna_nr; - int m_number; -}; - -enum btSerializationFlags -{ - BT_SERIALIZE_NO_BVH = 1, - BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2, - BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4 -}; - -class btSerializer -{ - -public: - - virtual ~btSerializer() {} - - virtual const unsigned char* getBufferPointer() const = 0; - - virtual int getCurrentBufferSize() const = 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* 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 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) ) -#else -# define BT_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) ) -#endif - -#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 -{ - union - { - void* m_ptr; - int m_uniqueIds[2]; - }; -}; - -///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 -{ - - - 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; - int m_currentSize; - void* m_dna; - int m_dnaLength; - - int m_serializationFlags; - - - btAlignedObjectArray m_chunkPtrs; - -protected: - - virtual void* findPointer(void* oldPtr) - { - void** ptr = m_chunkP.find(oldPtr); - if (ptr && *ptr) - return *ptr; - return 0; - } - - - - - - 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; - - 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) - - - */ - - if (strncmp((const char*)m_dna, "SDNA", 4)==0) - { - // skip ++ NAME - intPtr++; intPtr++; - } - - // Parse names - if (!littleEndian) - *intPtr = btSwapEndian(*intPtr); - - dataLen = *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++; - - if (!littleEndian) - *intPtr = btSwapEndian(*intPtr); - - dataLen = *intPtr; - intPtr++; - - - 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++; - - dataLen = (int)mTypes.size(); - - shtPtr = (short*)intPtr; - for (i=0; i amount of structs (int) - - - - - - - */ - - 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; im_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_chunkP.clear(); - m_nameMap.clear(); - m_uniquePointers.clear(); - m_chunkPtrs.clear(); - } - - virtual void* getUniquePointer(void*oldPtr) - { - if (!oldPtr) - return 0; - - btPointerUid* uptr = (btPointerUid*)m_uniquePointers.find(oldPtr); - if (uptr) - { - return uptr->m_ptr; - } - 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; - 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) - { - - 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(totalsize - usedsize); - } - - unsigned char* allocate(unsigned int size) - { - const unsigned int nus(usedsize+size); - if(nusprevious = current; - pb->address = data+usedsize; - current = pb; - return(pb); - } - SIMD_FORCE_INLINE void endBlock(btBlock* block) - { - btAssert(block==current); - //Raise(L"Unmatched blocks"); - if(block==current) - { - current = block->previous; - usedsize = (unsigned int)((block->address-data)-sizeof(btBlock)); - } - } - -private: - void ctor() - { - data = 0; - totalsize = 0; - usedsize = 0; - current = 0; - ischild = false; - } - unsigned char* data; - unsigned int totalsize; - unsigned int usedsize; - btBlock* current; - bool ischild; -}; - -#endif //BT_STACK_ALLOC diff --git a/WickedEngine/BULLET/LinearMath/btTransform.h b/WickedEngine/BULLET/LinearMath/btTransform.h deleted file mode 100644 index 907627379..000000000 --- a/WickedEngine/BULLET/LinearMath/btTransform.h +++ /dev/null @@ -1,305 +0,0 @@ -/* -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 BT_TRANSFORM_H -#define BT_TRANSFORM_H - - -#include "btMatrix3x3.h" - -#ifdef BT_USE_DOUBLE_PRECISION -#define btTransformData btTransformDoubleData -#else -#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 - btMatrix3x3 m_basis; - ///Storage for the translation - btVector3 m_origin; - -public: - - /**@brief No initialization constructor */ - btTransform() {} - /**@brief Constructor from btQuaternion (optional btVector3 ) - * @param q Rotation from quaternion - * @param c Translation from Vector (default 0,0,0) */ - explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, - const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) - : m_basis(q), - m_origin(c) - {} - - /**@brief Constructor from btMatrix3x3 (optional btVector3) - * @param b Rotation from Matrix - * @param c Translation from Vector default (0,0,0)*/ - explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, - const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) - : m_basis(b), - m_origin(c) - {} - /**@brief Copy constructor */ - SIMD_FORCE_INLINE btTransform (const btTransform& other) - : m_basis(other.m_basis), - m_origin(other.m_origin) - { - } - /**@brief Assignment Operator */ - SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other) - { - m_basis = other.m_basis; - 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 */ - 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) { - 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 */ - SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const - { - return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin; - } - - /**@brief Return the transform of the vector */ - SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const - { - return (*this)(x); - } - - /**@brief Return the transform of the btQuaternion */ - SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const - { - return getRotation() * q; - } - - /**@brief Return the basis matrix for the rotation */ - SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; } - /**@brief Return the basis matrix for the rotation */ - SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; } - - /**@brief Return the origin vector translation */ - SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; } - /**@brief Return the origin vector translation */ - SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; } - - /**@brief Return a quaternion representing the rotation */ - btQuaternion getRotation() const { - btQuaternion q; - m_basis.getRotation(q); - 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 btScalar *m) - { - m_basis.setFromOpenGLSubMatrix(m); - m_origin.setValue(m[12],m[13],m[14]); - } - - /**@brief Fill an array representation - * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ - void getOpenGLMatrix(btScalar *m) const - { - m_basis.getOpenGLSubMatrix(m); - m[12] = m_origin.x(); - m[13] = m_origin.y(); - m[14] = m_origin.z(); - m[15] = btScalar(1.0); - } - - /**@brief Set the translational element - * @param origin The vector to set the translation to */ - SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) - { - m_origin = origin; - } - - SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const; - - - /**@brief Set the rotational element by btMatrix3x3 */ - SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis) - { - m_basis = basis; - } - - /**@brief Set the rotational element by btQuaternion */ - SIMD_FORCE_INLINE void setRotation(const btQuaternion& q) - { - m_basis.setRotation(q); - } - - - /**@brief Set this transformation to the identity */ - void setIdentity() - { - m_basis.setIdentity(); - m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - } - - /**@brief Multiply this Transform by another(this = this * another) - * @param t The other transform */ - btTransform& operator*=(const btTransform& t) - { - m_origin += m_basis * t.m_origin; - m_basis *= t.m_basis; - return *this; - } - - /**@brief Return the inverse of this transform */ - btTransform inverse() const - { - btMatrix3x3 inv = m_basis.transpose(); - return btTransform(inv, inv * -m_origin); - } - - /**@brief Return the inverse of this transform times the other transform - * @param t The other transform - * return this.inverse() * the other */ - btTransform inverseTimes(const btTransform& t) const; - - /**@brief Return the product of this transform and the other */ - btTransform operator*(const btTransform& t) const; - - /**@brief Return an identity transform */ - static const btTransform& getIdentity() - { - static const btTransform identityTransform(btMatrix3x3::getIdentity()); - return identityTransform; - } - - void serialize(struct btTransformData& dataOut) const; - - void serializeFloat(struct btTransformFloatData& dataOut) const; - - void deSerialize(const struct btTransformData& dataIn); - - void deSerializeDouble(const struct btTransformDoubleData& dataIn); - - void deSerializeFloat(const struct btTransformFloatData& dataIn); - -}; - - -SIMD_FORCE_INLINE btVector3 -btTransform::invXform(const btVector3& inVec) const -{ - btVector3 v = inVec - m_origin; - return (m_basis.transpose() * v); -} - -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); -} - -SIMD_FORCE_INLINE btTransform -btTransform::operator*(const btTransform& t) const -{ - 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() ); -} - - -///for serialization -struct btTransformFloatData -{ - btMatrix3x3FloatData m_basis; - btVector3FloatData m_origin; -}; - -struct btTransformDoubleData -{ - btMatrix3x3DoubleData m_basis; - btVector3DoubleData m_origin; -}; - - - -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 -{ - m_basis.serializeFloat(dataOut.m_basis); - m_origin.serializeFloat(dataOut.m_origin); -} - - -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) -{ - m_basis.deSerializeFloat(dataIn.m_basis); - m_origin.deSerializeFloat(dataIn.m_origin); -} - -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 - - - - - - diff --git a/WickedEngine/BULLET/LinearMath/btTransformUtil.h b/WickedEngine/BULLET/LinearMath/btTransformUtil.h deleted file mode 100644 index 2303c2742..000000000 --- a/WickedEngine/BULLET/LinearMath/btTransformUtil.h +++ /dev/null @@ -1,228 +0,0 @@ -/* -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 BT_TRANSFORM_UTIL_H -#define BT_TRANSFORM_UTIL_H - -#include "btTransform.h" -#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI - - - - -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()); -} - - - - - - -/// Utils related to temporal transforms -class btTransformUtil -{ - -public: - - 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 - btQuaternion predictedOrn = curTrans.getRotation(); - predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); - predictedOrn.normalize(); - #else - //Exponential map - //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia - - btVector3 axis; - btScalar fAngle = angvel.length(); - //limit the angular motion - if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD) - { - fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; - } - - 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 ); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle ); - } - btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) )); - btQuaternion orn0 = curTrans.getRotation(); - - btQuaternion predictedOrn = dorn * orn0; - predictedOrn.normalize(); - #endif - predictedTransform.setRotation(predictedOrn); - } - - static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel) - { - linVel = (pos1 - pos0) / timeStep; - btVector3 axis; - btScalar angle; - if (orn0 != orn1) - { - calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle); - angVel = axis * angle / timeStep; - } else - { - angVel.setValue(0,0,0); - } - } - - static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle) - { - btQuaternion orn1 = orn0.nearest(orn1a); - btQuaternion dorn = orn1 * orn0.inverse(); - angle = dorn.getAngle(); - axis = btVector3(dorn.x(),dorn.y(),dorn.z()); - axis[3] = btScalar(0.); - //check for axis length - btScalar len = axis.length2(); - if (len < SIMD_EPSILON*SIMD_EPSILON) - axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); - else - axis /= btSqrt(len); - } - - static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) - { - linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; - btVector3 axis; - btScalar angle; - calculateDiffAxisAngle(transform0,transform1,axis,angle); - angVel = axis * angle / timeStep; - } - - 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 - dorn.normalize(); - - angle = dorn.getAngle(); - axis = btVector3(dorn.x(),dorn.y(),dorn.z()); - axis[3] = btScalar(0.); - //check for axis length - btScalar len = axis.length2(); - if (len < SIMD_EPSILON*SIMD_EPSILON) - axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.)); - else - axis /= btSqrt(len); - } - -}; - - -///The btConvexSeparatingDistanceUtil can help speed up convex collision detection -///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance -class btConvexSeparatingDistanceUtil -{ - btQuaternion m_ornA; - btQuaternion m_ornB; - btVector3 m_posA; - btVector3 m_posB; - - btVector3 m_separatingNormal; - - btScalar m_boundingRadiusA; - btScalar m_boundingRadiusB; - btScalar m_separatingDistance; - -public: - - btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB) - :m_boundingRadiusA(boundingRadiusA), - m_boundingRadiusB(boundingRadiusB), - m_separatingDistance(0.f) - { - } - - btScalar getConservativeSeparatingDistance() - { - return m_separatingDistance; - } - - void updateSeparatingDistance(const btTransform& transA,const btTransform& transB) - { - const btVector3& toPosA = transA.getOrigin(); - const btVector3& toPosB = transB.getOrigin(); - btQuaternion toOrnA = transA.getRotation(); - btQuaternion toOrnB = transB.getRotation(); - - if (m_separatingDistance>0.f) - { - - - btVector3 linVelA,angVelA,linVelB,angVelB; - btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA); - btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB); - btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; - btVector3 relLinVel = (linVelB-linVelA); - btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal); - if (relLinVelocLength<0.f) - { - relLinVelocLength = 0.f; - } - - btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength; - m_separatingDistance -= projectedMotion; - } - - m_posA = toPosA; - m_posB = toPosB; - m_ornA = toOrnA; - m_ornB = toOrnB; - } - - void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB) - { - m_separatingDistance = separatingDistance; - - if (m_separatingDistance>0.f) - { - m_separatingNormal = separatingVector; - - const btVector3& toPosA = transA.getOrigin(); - const btVector3& toPosB = transB.getOrigin(); - btQuaternion toOrnA = transA.getRotation(); - btQuaternion toOrnB = transB.getRotation(); - m_posA = toPosA; - m_posB = toPosB; - m_ornA = toOrnA; - m_ornB = toOrnB; - } - } - -}; - - -#endif //BT_TRANSFORM_UTIL_H - diff --git a/WickedEngine/BULLET/LinearMath/btVector3.cpp b/WickedEngine/BULLET/LinearMath/btVector3.cpp deleted file mode 100644 index 9389a25ca..000000000 --- a/WickedEngine/BULLET/LinearMath/btVector3.cpp +++ /dev/null @@ -1,1664 +0,0 @@ -/* - Copyright (c) 2011 Apple Inc. - 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. - - This source version has been altered. - */ - -#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 -#endif - - -#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 BT_USE_SSE || defined _WIN32 - -#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 ) -{ - 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]) ); -#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 ) - { -#ifdef __APPLE__ - float4 t0, t1, t2, t3, t4; - float4 * sap = &stack_array[index + localCount / 4]; - vertices += localCount; // counter the offset - size_t byteIndex = -(localCount) * sizeof(float); - //AT&T Code style assembly - asm volatile - ( ".align 4 \n\ - 0: movaps %[max], %[t2] // move max out of the way to avoid propagating NaNs in max \n\ - movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\ - movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\ - movaps %[t0], %[max] // vertices[0] \n\ - movlhps %[t1], %[max] // x0y0x1y1 \n\ - movaps 32(%[vertices], %[byteIndex], 4), %[t3] // vertices[2] \n\ - movaps 48(%[vertices], %[byteIndex], 4), %[t4] // vertices[3] \n\ - mulps %[vLo], %[max] // x0y0x1y1 * vLo \n\ - movhlps %[t0], %[t1] // z0w0z1w1 \n\ - movaps %[t3], %[t0] // vertices[2] \n\ - movlhps %[t4], %[t0] // x2y2x3y3 \n\ - mulps %[vLo], %[t0] // x2y2x3y3 * vLo \n\ - movhlps %[t3], %[t4] // z2w2z3w3 \n\ - shufps $0x88, %[t4], %[t1] // z0z1z2z3 \n\ - mulps %[vHi], %[t1] // z0z1z2z3 * vHi \n\ - movaps %[max], %[t3] // x0y0x1y1 * vLo \n\ - shufps $0x88, %[t0], %[max] // x0x1x2x3 * vLo.x \n\ - shufps $0xdd, %[t0], %[t3] // y0y1y2y3 * vLo.y \n\ - addps %[t3], %[max] // x + y \n\ - addps %[t1], %[max] // x + y + z \n\ - movaps %[max], (%[sap], %[byteIndex]) // record result for later scrutiny \n\ - maxps %[t2], %[max] // record max, restore max \n\ - add $16, %[byteIndex] // advance loop counter\n\ - jnz 0b \n\ - " - : [max] "+x" (max), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex) - : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap) - : "memory", "cc" - ); - index += localCount/4; -#else - { - for( unsigned int i=0; i 16) ) - { - for( ; index + 4 <= count / 4; index+=4 ) - { // do four dot products at a time. Carefully avoid touching the w element. - float4 v0 = vertices[0]; - float4 v1 = vertices[1]; - float4 v2 = vertices[2]; - float4 v3 = vertices[3]; vertices += 4; - - float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 - float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 - float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 - float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 - - lo0 = lo0*vLo; - lo1 = lo1*vLo; - float4 z = _mm_shuffle_ps(hi0, hi1, 0x88); - float4 x = _mm_shuffle_ps(lo0, lo1, 0x88); - float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd); - z = z*vHi; - x = x+y; - x = x+z; - stack_array[index] = x; - min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan - - v0 = vertices[0]; - v1 = vertices[1]; - v2 = vertices[2]; - v3 = vertices[3]; vertices += 4; - - lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 - hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 - lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 - hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 - - lo0 = lo0*vLo; - lo1 = lo1*vLo; - z = _mm_shuffle_ps(hi0, hi1, 0x88); - x = _mm_shuffle_ps(lo0, lo1, 0x88); - y = _mm_shuffle_ps(lo0, lo1, 0xdd); - z = z*vHi; - x = x+y; - x = x+z; - stack_array[index+1] = x; - min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan - - v0 = vertices[0]; - v1 = vertices[1]; - v2 = vertices[2]; - v3 = vertices[3]; vertices += 4; - - lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 - hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 - lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 - hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 - - lo0 = lo0*vLo; - lo1 = lo1*vLo; - z = _mm_shuffle_ps(hi0, hi1, 0x88); - x = _mm_shuffle_ps(lo0, lo1, 0x88); - y = _mm_shuffle_ps(lo0, lo1, 0xdd); - z = z*vHi; - x = x+y; - x = x+z; - stack_array[index+2] = x; - min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan - - v0 = vertices[0]; - v1 = vertices[1]; - v2 = vertices[2]; - v3 = vertices[3]; vertices += 4; - - lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 - hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 - lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 - hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 - - lo0 = lo0*vLo; - lo1 = lo1*vLo; - z = _mm_shuffle_ps(hi0, hi1, 0x88); - x = _mm_shuffle_ps(lo0, lo1, 0x88); - y = _mm_shuffle_ps(lo0, lo1, 0xdd); - z = z*vHi; - x = x+y; - x = x+z; - stack_array[index+3] = x; - min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan - - // It is too costly to keep the index of the min here. We will look for it again later. We save a lot of work this way. - } - } - - size_t localCount = (count & -4L) - 4*index; - if( localCount ) - { - - -#ifdef __APPLE__ - vertices += localCount; // counter the offset - float4 t0, t1, t2, t3, t4; - size_t byteIndex = -(localCount) * sizeof(float); - float4 * sap = &stack_array[index + localCount / 4]; - - asm volatile - ( ".align 4 \n\ - 0: movaps %[min], %[t2] // move min out of the way to avoid propagating NaNs in min \n\ - movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\ - movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\ - movaps %[t0], %[min] // vertices[0] \n\ - movlhps %[t1], %[min] // x0y0x1y1 \n\ - movaps 32(%[vertices], %[byteIndex], 4), %[t3] // vertices[2] \n\ - movaps 48(%[vertices], %[byteIndex], 4), %[t4] // vertices[3] \n\ - mulps %[vLo], %[min] // x0y0x1y1 * vLo \n\ - movhlps %[t0], %[t1] // z0w0z1w1 \n\ - movaps %[t3], %[t0] // vertices[2] \n\ - movlhps %[t4], %[t0] // x2y2x3y3 \n\ - movhlps %[t3], %[t4] // z2w2z3w3 \n\ - mulps %[vLo], %[t0] // x2y2x3y3 * vLo \n\ - shufps $0x88, %[t4], %[t1] // z0z1z2z3 \n\ - mulps %[vHi], %[t1] // z0z1z2z3 * vHi \n\ - movaps %[min], %[t3] // x0y0x1y1 * vLo \n\ - shufps $0x88, %[t0], %[min] // x0x1x2x3 * vLo.x \n\ - shufps $0xdd, %[t0], %[t3] // y0y1y2y3 * vLo.y \n\ - addps %[t3], %[min] // x + y \n\ - addps %[t1], %[min] // x + y + z \n\ - movaps %[min], (%[sap], %[byteIndex]) // record result for later scrutiny \n\ - minps %[t2], %[min] // record min, restore min \n\ - add $16, %[byteIndex] // advance loop counter\n\ - jnz 0b \n\ - " - : [min] "+x" (min), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex) - : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap) - : "memory", "cc" - ); - index += localCount/4; -#else - { - for( unsigned int i=0; i -#include -#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 ); - -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 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 == err && hasFeature) - capabilities |= 0x2000; - - testedCapabilities = true; - } - - return capabilities; -} - - - - -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; - - return _maxdot_large(vv, vec, count, 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; - - return _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 _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}; - - 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 _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); - - 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 ) -{ - 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); - } - - 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 ) -{ - 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 -#endif - -#endif /* __APPLE__ */ - - diff --git a/WickedEngine/BULLET/LinearMath/btVector3.h b/WickedEngine/BULLET/LinearMath/btVector3.h deleted file mode 100644 index 3bc299200..000000000 --- a/WickedEngine/BULLET/LinearMath/btVector3.h +++ /dev/null @@ -1,1352 +0,0 @@ -/* -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 BT_VECTOR3_H -#define BT_VECTOR3_H - -//#include -#include "btScalar.h" -#include "btMinMax.h" -#include "btAlignedAllocator.h" - -#ifdef BT_USE_DOUBLE_PRECISION -#define btVector3Data btVector3DoubleData -#define btVector3DataName "btVector3DoubleData" -#else -#define btVector3Data btVector3FloatData -#define btVector3DataName "btVector3FloatData" -#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' -#endif - - -#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 btv3AbsiMask (_mm_set_epi32(0x00000000, 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) -#define btvxyzMaskf btvFFF0fMask -#define btvAbsfMask btCastiTo128f(btvAbsMask) - -//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)) - -//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}; -//const __m128 ATTRIBUTE_ALIGNED16(vHalf) = {0.5f, 0.5f, 0.5f, 0.5f}; -//const __m128 ATTRIBUTE_ALIGNED16(v1_5) = {1.5f, 1.5f, 1.5f, 1.5f}; - -#endif - -#ifdef BT_USE_NEON - -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}; -const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF}; -const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0}; - -#endif - -/**@brief btVector3 can be used to represent 3D points and vectors. - * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user - * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers - */ -ATTRIBUTE_ALIGNED16(class) btVector3 -{ -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - -#if defined (__SPU__) && defined (__CELLOS_LV2__) - btScalar m_floats[4]; -public: - 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() - { - - } - - - - /**@brief Constructor from scalars - * @param x X value - * @param y Y value - * @param z Z value - */ - SIMD_FORCE_INLINE btVector3(const btScalar& _x, const btScalar& _y, const btScalar& _z) - { - m_floats[0] = _x; - m_floats[1] = _y; - m_floats[2] = _z; - m_floats[3] = btScalar(0.f); - } - -#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; - } - - // Copy constructor - SIMD_FORCE_INLINE btVector3(const btVector3& rhs) - { - mVec128 = rhs.mVec128; - } - - // Assignment Operator - 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 - * @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) - 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[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 */ - SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) - { -#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[1] -= v.m_floats[1]; - m_floats[2] -= v.m_floats[2]; -#endif - return *this; - } - - /**@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) - 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; -#endif - return *this; - } - - /**@brief Inversely scale the vector - * @param s Scale factor to divide by */ - SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) - { - btFullAssert(s != btScalar(0.0)); - -#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); - vs = bt_pshufd_ps(vs, 0x00); // (S S S S) - - mVec128 = _mm_mul_ps(mVec128, vs); - - return *this; -#else - return *this *= btScalar(1.0) / s; -#endif - } - - /**@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) - __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(BT_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 */ - SIMD_FORCE_INLINE btScalar length2() const - { - return dot(*this); - } - - /**@brief Return the length of the vector */ - SIMD_FORCE_INLINE btScalar length() const - { - return btSqrt(length2()); - } - - /**@brief Return the norm (length) of the vector */ - SIMD_FORCE_INLINE btScalar norm() const - { - return length(); - } - - /**@brief Return the distance squared between the ends of this and another vector - * This is symantically treating the vector like a point */ - SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; - - /**@brief Return the distance between the ends of this and another vector - * This is symantically treating the vector like a point */ - SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; - - SIMD_FORCE_INLINE btVector3& safeNormalize() - { - btVector3 absVec = this->absolute(); - int maxIndex = absVec.maxAxis(); - if (absVec[maxIndex]>0) - { - *this /= absVec[maxIndex]; - return *this /= length(); - } - setValue(1,0,0); - return *this; - } - - /**@brief Normalize this vector - * x^2 + y^2 + z^2 = 1 */ - SIMD_FORCE_INLINE btVector3& normalize() - { - - btAssert(length() != btScalar(0)); - -#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 - 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 - - 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 - - - return *this; -#else - return *this /= length(); -#endif - } - - /**@brief Return a normalized version of this vector */ - SIMD_FORCE_INLINE btVector3 normalized() const; - - /**@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; - - /**@brief Return the angle between this and another vector - * @param v The other vector */ - SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const - { - btScalar s = btSqrt(length2() * v.length2()); - btFullAssert(s != btScalar(0.0)); - return btAcos(dot(v) / s); - } - - /**@brief Return a vector will the absolute values of each element */ - SIMD_FORCE_INLINE btVector3 absolute() const - { - -#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 - return btVector3( - btFabs(m_floats[0]), - btFabs(m_floats[1]), - btFabs(m_floats[2])); -#endif - } - - /**@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) - - 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) - 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, btvFFF0Mask); - - return btVector3(V); -#else - return btVector3( - 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 - } - - 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) - // 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) - - 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: - 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(BT_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 */ - 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 - { -#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); -#endif - } - - /**@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) - 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]; - 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]; } - - //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; } - //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; } - ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. - SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; } - SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; } - - SIMD_FORCE_INLINE bool operator==(const btVector3& other) const - { -#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 - { - return !(*this == other); - } - - /**@brief Set each element to the max of the current values and the values of another btVector3 - * @param other The other btVector3 to compare with - */ - SIMD_FORCE_INLINE void setMax(const btVector3& other) - { -#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); -#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.w()); -#endif - } - - /**@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) - { -#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); -#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.w()); -#endif - } - - 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] = btScalar(0.f); - } - - 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); - __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); - 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.); -#endif - } - - void setZero() - { -#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); - mVec128 = vreinterpretq_f32_s32(vi); -#else - setValue(btScalar(0.),btScalar(0.),btScalar(0.)); -#endif - } - - 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 - { - return length2() < SIMD_EPSILON; - } - - SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const; - - SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn); - - SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const; - - SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn); - - 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; - - /**@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; - - /* 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); - -#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)); -#endif - } -}; - -/**@brief Return the sum of two vectors (Point symantics)*/ -SIMD_FORCE_INLINE btVector3 -operator+(const btVector3& v1, const btVector3& v2) -{ -#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]); -#endif -} - -/**@brief Return the elementwise product of two vectors */ -SIMD_FORCE_INLINE btVector3 -operator*(const btVector3& v1, const btVector3& v2) -{ -#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]); -#endif -} - -/**@brief Return the difference between two vectors */ -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)) - - // without _mm_and_ps this code causes slowdown in Concave moving - __m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128); - return btVector3(_mm_and_ps(r, btvFFF0fMask)); -#elif defined(BT_USE_NEON) - float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128); - 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]); -#endif -} - -/**@brief Return the negative of the vector */ -SIMD_FORCE_INLINE btVector3 -operator-(const btVector3& v) -{ -#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)); -#elif defined(BT_USE_NEON) - return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask)); -#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 -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) - return btVector3(_mm_mul_ps(v.mVec128, vs)); -#elif defined(BT_USE_NEON) - float32x4_t r = vmulq_n_f32(v.mVec128, s); - return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask)); -#else - return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); -#endif -} - -/**@brief Return the vector scaled by s */ -SIMD_FORCE_INLINE btVector3 -operator*(const btScalar& s, const btVector3& v) -{ - return v * s; -} - -/**@brief Return the vector inversely scaled by s */ -SIMD_FORCE_INLINE btVector3 -operator/(const btVector3& v, const btScalar& s) -{ - btFullAssert(s != btScalar(0.0)); -#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); - vs = bt_pshufd_ps(vs, 0x00); // (S S S S) - - return btVector3(_mm_mul_ps(v.mVec128, vs)); -#else - return v * (btScalar(1.0) / s); -#endif -} - -/**@brief Return the vector inversely scaled by 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)) - __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128); - vec = _mm_and_ps(vec, btvFFF0fMask); - 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 - - 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]); -#endif -} - -/**@brief Return the dot product between two vectors */ -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); -} - - -/**@brief Return the distance between two vectors */ -SIMD_FORCE_INLINE btScalar -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); -} - -/**@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 btScalar -btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3) -{ - return v1.triple(v2, v3); -} - -/**@brief Return the linear interpolation between two vectors - * @param v1 One vector - * @param v2 The other vector - * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ -SIMD_FORCE_INLINE btVector3 -lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) -{ - return v1.lerp(v2, t); -} - - - -SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const -{ - return (v - *this).length2(); -} - -SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const -{ - return (v - *this).length(); -} - -SIMD_FORCE_INLINE btVector3 btVector3::normalized() const -{ - btVector3 norm = *this; - - return norm.normalize(); -} - -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) - - __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) - O = _mm_add_ps(O, Y); - 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 = vcos * X; - O = O + vcos; - - return btVector3(O); -#else - btVector3 o = wAxis * wAxis.dot( *this ); - btVector3 _x = *this - o; - btVector3 _y; - - _y = wAxis.cross( *this ); - - return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) ); -#endif -} - -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 ) -#endif - { - btScalar maxDot = -SIMD_INFINITY; - int i = 0; - int ptIndex = -1; - for( i = 0; i < array_count; i++ ) - { - btScalar dot = array[i].dot(*this); - - if( dot > maxDot ) - { - maxDot = dot; - ptIndex = i; - } - } - - dotOut = maxDot; - 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 -{ -#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 ) -#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) - { - m_floats[3] = _w; - } - -#if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON) - SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec) - { - mVec128 = vec; - } - - SIMD_FORCE_INLINE btVector4(const btVector3& rhs) - { - mVec128 = rhs.mVec128; - } - - 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) - - SIMD_FORCE_INLINE btVector4 absolute4() const - { -#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 - return btVector4( - btFabs(m_floats[0]), - btFabs(m_floats[1]), - btFabs(m_floats[2]), - btFabs(m_floats[3])); -#endif - } - - - btScalar getW() const { return m_floats[3];} - - - SIMD_FORCE_INLINE int maxAxis4() const - { - int maxIndex = -1; - btScalar maxVal = btScalar(-BT_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; - maxVal = m_floats[3]; - } - - return maxIndex; - } - - - SIMD_FORCE_INLINE int minAxis4() const - { - int minIndex = -1; - btScalar minVal = btScalar(BT_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; - } - - - SIMD_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(btScalar *m) const - { - m[0] = m_floats[0]; - m[1] = m_floats[1]; - m[2] =m_floats[2]; - } -*/ -/**@brief Set the values - * @param x Value of x - * @param y Value of y - * @param z Value of z - * @param w Value of w - */ - SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) - { - m_floats[0]=_x; - m_floats[1]=_y; - m_floats[2]=_z; - m_floats[3]=_w; - } - - -}; - - -///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -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; - 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 //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) -{ - for (int i=0;i<4;i++) - { - btSwapScalarEndian(sourceVec[i],destVec[i]); - } - -} - -///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization -SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector) -{ - - btVector3 swappedVec; - for (int i=0;i<4;i++) - { - btSwapScalarEndian(vector[i],swappedVec[i]); - } - vector = swappedVec; -} - -template -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; - } -} - - -struct btVector3FloatData -{ - float m_floats[4]; -}; - -struct btVector3DoubleData -{ - double m_floats[4]; - -}; - -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++) - dataOut.m_floats[i] = float(m_floats[i]); -} - -SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn) -{ - 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 -{ - ///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]); -} - -SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn) -{ - 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 -{ - ///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]; -} - -SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn) -{ - for (int i=0;i<4;i++) - m_floats[i] = dataIn.m_floats[i]; -} - -#endif //BT_VECTOR3_H diff --git a/WickedEngine/BULLET/btBulletCollisionCommon.h b/WickedEngine/BULLET/btBulletCollisionCommon.h deleted file mode 100644 index af981b5d3..000000000 --- a/WickedEngine/BULLET/btBulletCollisionCommon.h +++ /dev/null @@ -1,68 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BULLET_COLLISION_COMMON_H -#define BULLET_COLLISION_COMMON_H - -///Common headerfile includes for Bullet Collision Detection - -///Bullet's btCollisionWorld and btCollisionObject definitions -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -///Collision Shapes -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" -#include "BulletCollision/CollisionShapes/btConvexHullShape.h" -#include "BulletCollision/CollisionShapes/btTriangleMesh.h" -#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" -#include "BulletCollision/CollisionShapes/btEmptyShape.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/CollisionShapes/btUniformScalingShape.h" - -///Narrowphase Collision Detector -#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" - -//#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" - -///Dispatching and generation of collision pairs (broadphase) -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" -#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h" - -///Math library & Utils -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btDefaultMotionState.h" -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btSerializer.h" - - -#endif //BULLET_COLLISION_COMMON_H - diff --git a/WickedEngine/BULLET/btBulletDynamicsCommon.h b/WickedEngine/BULLET/btBulletDynamicsCommon.h deleted file mode 100644 index 50282bf21..000000000 --- a/WickedEngine/BULLET/btBulletDynamicsCommon.h +++ /dev/null @@ -1,51 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BULLET_DYNAMICS_COMMON_H -#define BULLET_DYNAMICS_COMMON_H - -///Common headerfile includes for Bullet Dynamics, including Collision Detection -#include "btBulletCollisionCommon.h" - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" - -#include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" -#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h" -#include "BulletDynamics/ConstraintSolver/btUniversalConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHinge2Constraint.h" -#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 - diff --git a/WickedEngine/CMakeLists.txt b/WickedEngine/CMakeLists.txt index 65d306846..9e65ab357 100644 --- a/WickedEngine/CMakeLists.txt +++ b/WickedEngine/CMakeLists.txt @@ -48,10 +48,13 @@ else () endif() endif() -add_subdirectory(BULLET) add_subdirectory(LUA) add_subdirectory(Utility) +add_compile_definitions(JPH_DEBUG_RENDERER=1) +set(PHYSICS_REPO_ROOT ${CMAKE_CURRENT_SOURCE_DIR}) +include(${PHYSICS_REPO_ROOT}/Jolt/Jolt.cmake) + set(HEADER_FILES WickedEngine.h CommonInclude.h @@ -200,7 +203,7 @@ add_library(${TARGET_NAME} ${WICKED_LIBRARY_TYPE} wiNetwork_Linux.cpp wiNetwork_Windows.cpp wiOcean.cpp - wiPhysics_Bullet.cpp + wiPhysics_Jolt.cpp wiPhysics_BindLua.cpp wiProfiler.cpp wiRandom.cpp @@ -244,7 +247,7 @@ target_include_directories(${TARGET_NAME} SYSTEM PUBLIC ) set(WICKEDENGINE_STATIC_LIBRARIES - Bullet + Jolt LUA Utility ) diff --git a/WickedEngine/Jolt/AABBTree/AABBTreeBuilder.cpp b/WickedEngine/Jolt/AABBTree/AABBTreeBuilder.cpp new file mode 100644 index 000000000..e4b7ece74 --- /dev/null +++ b/WickedEngine/Jolt/AABBTree/AABBTreeBuilder.cpp @@ -0,0 +1,239 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +AABBTreeBuilder::Node::Node() +{ + mChild[0] = nullptr; + mChild[1] = nullptr; +} + +AABBTreeBuilder::Node::~Node() +{ + delete mChild[0]; + delete mChild[1]; +} + +uint AABBTreeBuilder::Node::GetMinDepth() const +{ + if (HasChildren()) + { + uint left = mChild[0]->GetMinDepth(); + uint right = mChild[1]->GetMinDepth(); + return min(left, right) + 1; + } + else + return 1; +} + +uint AABBTreeBuilder::Node::GetMaxDepth() const +{ + if (HasChildren()) + { + uint left = mChild[0]->GetMaxDepth(); + uint right = mChild[1]->GetMaxDepth(); + return max(left, right) + 1; + } + else + return 1; +} + +uint AABBTreeBuilder::Node::GetNodeCount() const +{ + if (HasChildren()) + return mChild[0]->GetNodeCount() + mChild[1]->GetNodeCount() + 1; + else + return 1; +} + +uint AABBTreeBuilder::Node::GetLeafNodeCount() const +{ + if (HasChildren()) + return mChild[0]->GetLeafNodeCount() + mChild[1]->GetLeafNodeCount(); + else + return 1; +} + +uint AABBTreeBuilder::Node::GetTriangleCountInTree() const +{ + if (HasChildren()) + return mChild[0]->GetTriangleCountInTree() + mChild[1]->GetTriangleCountInTree(); + else + return GetTriangleCount(); +} + +void AABBTreeBuilder::Node::GetTriangleCountPerNode(float &outAverage, uint &outMin, uint &outMax) const +{ + outMin = INT_MAX; + outMax = 0; + outAverage = 0; + uint avg_divisor = 0; + GetTriangleCountPerNodeInternal(outAverage, avg_divisor, outMin, outMax); + if (avg_divisor > 0) + outAverage /= avg_divisor; +} + +float AABBTreeBuilder::Node::CalculateSAHCost(float inCostTraversal, float inCostLeaf) const +{ + float surface_area = mBounds.GetSurfaceArea(); + return surface_area > 0.0f? CalculateSAHCostInternal(inCostTraversal / surface_area, inCostLeaf / surface_area) : 0.0f; +} + +void AABBTreeBuilder::Node::GetNChildren(uint inN, Array &outChildren) const +{ + JPH_ASSERT(outChildren.empty()); + + // Check if there is anything to expand + if (!HasChildren()) + return; + + // Start with the children of this node + outChildren.push_back(mChild[0]); + outChildren.push_back(mChild[1]); + + size_t next = 0; + bool all_triangles = true; + while (outChildren.size() < inN) + { + // If we have looped over all nodes, start over with the first node again + if (next >= outChildren.size()) + { + // If there only triangle nodes left, we have to terminate + if (all_triangles) + return; + next = 0; + all_triangles = true; + } + + // Try to expand this node into its two children + const Node *to_expand = outChildren[next]; + if (to_expand->HasChildren()) + { + outChildren.erase(outChildren.begin() + next); + outChildren.push_back(to_expand->mChild[0]); + outChildren.push_back(to_expand->mChild[1]); + all_triangles = false; + } + else + { + ++next; + } + } +} + +float AABBTreeBuilder::Node::CalculateSAHCostInternal(float inCostTraversalDivSurfaceArea, float inCostLeafDivSurfaceArea) const +{ + if (HasChildren()) + return inCostTraversalDivSurfaceArea * mBounds.GetSurfaceArea() + + mChild[0]->CalculateSAHCostInternal(inCostTraversalDivSurfaceArea, inCostLeafDivSurfaceArea) + + mChild[1]->CalculateSAHCostInternal(inCostTraversalDivSurfaceArea, inCostLeafDivSurfaceArea); + else + return inCostLeafDivSurfaceArea * mBounds.GetSurfaceArea() * GetTriangleCount(); +} + +void AABBTreeBuilder::Node::GetTriangleCountPerNodeInternal(float &outAverage, uint &outAverageDivisor, uint &outMin, uint &outMax) const +{ + if (HasChildren()) + { + mChild[0]->GetTriangleCountPerNodeInternal(outAverage, outAverageDivisor, outMin, outMax); + mChild[1]->GetTriangleCountPerNodeInternal(outAverage, outAverageDivisor, outMin, outMax); + } + else + { + outAverage += GetTriangleCount(); + outAverageDivisor++; + outMin = min(outMin, GetTriangleCount()); + outMax = max(outMax, GetTriangleCount()); + } +} + +AABBTreeBuilder::AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf) : + mTriangleSplitter(inSplitter), + mMaxTrianglesPerLeaf(inMaxTrianglesPerLeaf) +{ +} + +AABBTreeBuilder::Node *AABBTreeBuilder::Build(AABBTreeBuilderStats &outStats) +{ + TriangleSplitter::Range initial = mTriangleSplitter.GetInitialRange(); + Node *root = BuildInternal(initial); + + float avg_triangles_per_leaf; + uint min_triangles_per_leaf, max_triangles_per_leaf; + root->GetTriangleCountPerNode(avg_triangles_per_leaf, min_triangles_per_leaf, max_triangles_per_leaf); + + mTriangleSplitter.GetStats(outStats.mSplitterStats); + + outStats.mSAHCost = root->CalculateSAHCost(1.0f, 1.0f); + outStats.mMinDepth = root->GetMinDepth(); + outStats.mMaxDepth = root->GetMaxDepth(); + outStats.mNodeCount = root->GetNodeCount(); + outStats.mLeafNodeCount = root->GetLeafNodeCount(); + outStats.mMaxTrianglesPerLeaf = mMaxTrianglesPerLeaf; + outStats.mTreeMinTrianglesPerLeaf = min_triangles_per_leaf; + outStats.mTreeMaxTrianglesPerLeaf = max_triangles_per_leaf; + outStats.mTreeAvgTrianglesPerLeaf = avg_triangles_per_leaf; + + return root; +} + +AABBTreeBuilder::Node *AABBTreeBuilder::BuildInternal(const TriangleSplitter::Range &inTriangles) +{ + // Check if there are too many triangles left + if (inTriangles.Count() > mMaxTrianglesPerLeaf) + { + // Split triangles in two batches + TriangleSplitter::Range left, right; + if (!mTriangleSplitter.Split(inTriangles, left, right)) + { + // When the trace below triggers: + // + // This code builds a tree structure to accelerate collision detection. + // At top level it will start with all triangles in a mesh and then divides the triangles into two batches. + // This process repeats until until the batch size is smaller than mMaxTrianglePerLeaf. + // + // It uses a TriangleSplitter to find a good split. When this warning triggers, the splitter was not able + // to create a reasonable split for the triangles. This usually happens when the triangles in a batch are + // intersecting. They could also be overlapping when projected on the 3 coordinate axis. + // + // To solve this issue, you could try to pass your mesh through a mesh cleaning / optimization algorithm. + // You could also inspect the triangles that cause this issue and see if that part of the mesh can be fixed manually. + // + // When you do not fix this warning, the tree will be less efficient for collision detection, but it will still work. + JPH_IF_DEBUG(Trace("AABBTreeBuilder: Doing random split for %d triangles (max per node: %u)!", (int)inTriangles.Count(), mMaxTrianglesPerLeaf);) + int half = inTriangles.Count() / 2; + JPH_ASSERT(half > 0); + left = TriangleSplitter::Range(inTriangles.mBegin, inTriangles.mBegin + half); + right = TriangleSplitter::Range(inTriangles.mBegin + half, inTriangles.mEnd); + } + + // Recursively build + Node *node = new Node(); + node->mChild[0] = BuildInternal(left); + node->mChild[1] = BuildInternal(right); + node->mBounds = node->mChild[0]->mBounds; + node->mBounds.Encapsulate(node->mChild[1]->mBounds); + return node; + } + + // Create leaf node + Node *node = new Node(); + node->mTriangles.reserve(inTriangles.Count()); + for (uint i = inTriangles.mBegin; i < inTriangles.mEnd; ++i) + { + const IndexedTriangle &t = mTriangleSplitter.GetTriangle(i); + const VertexList &v = mTriangleSplitter.GetVertices(); + node->mTriangles.push_back(t); + node->mBounds.Encapsulate(v, t); + } + + return node; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/AABBTree/AABBTreeBuilder.h b/WickedEngine/Jolt/AABBTree/AABBTreeBuilder.h new file mode 100644 index 000000000..dacae5e07 --- /dev/null +++ b/WickedEngine/Jolt/AABBTree/AABBTreeBuilder.h @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct AABBTreeBuilderStats +{ + ///@name Splitter stats + TriangleSplitter::Stats mSplitterStats; ///< Stats returned by the triangle splitter algorithm + + ///@name Tree structure + float mSAHCost = 0.0f; ///< Surface Area Heuristic cost of this tree + int mMinDepth = 0; ///< Minimal depth of tree (number of nodes) + int mMaxDepth = 0; ///< Maximum depth of tree (number of nodes) + int mNodeCount = 0; ///< Number of nodes in the tree + int mLeafNodeCount = 0; ///< Number of leaf nodes (that contain triangles) + + ///@name Configured stats + int mMaxTrianglesPerLeaf = 0; ///< Configured max triangles per leaf + + ///@name Actual stats + int mTreeMinTrianglesPerLeaf = 0; ///< Minimal amount of triangles in a leaf + int mTreeMaxTrianglesPerLeaf = 0; ///< Maximal amount of triangles in a leaf + float mTreeAvgTrianglesPerLeaf = 0.0f; ///< Average amount of triangles in leaf nodes +}; + +/// Helper class to build an AABB tree +class JPH_EXPORT AABBTreeBuilder +{ +public: + /// A node in the tree, contains the AABox for the tree and any child nodes or triangles + class Node : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Node(); + ~Node(); + + /// Get number of triangles in this node + inline uint GetTriangleCount() const { return uint(mTriangles.size()); } + + /// Check if this node has any children + inline bool HasChildren() const { return mChild[0] != nullptr || mChild[1] != nullptr; } + + /// Min depth of tree + uint GetMinDepth() const; + + /// Max depth of tree + uint GetMaxDepth() const; + + /// Number of nodes in tree + uint GetNodeCount() const; + + /// Number of leaf nodes in tree + uint GetLeafNodeCount() const; + + /// Get triangle count in tree + uint GetTriangleCountInTree() const; + + /// Calculate min and max triangles per node + void GetTriangleCountPerNode(float &outAverage, uint &outMin, uint &outMax) const; + + /// Calculate the total cost of the tree using the surface area heuristic + float CalculateSAHCost(float inCostTraversal, float inCostLeaf) const; + + /// Recursively get children (breadth first) to get in total inN children (or less if there are no more) + void GetNChildren(uint inN, Array &outChildren) const; + + /// Bounding box + AABox mBounds; + + /// Triangles (if no child nodes) + IndexedTriangleList mTriangles; + + /// Child nodes (if no triangles) + Node * mChild[2]; + + private: + friend class AABBTreeBuilder; + + /// Recursive helper function to calculate cost of the tree + float CalculateSAHCostInternal(float inCostTraversalDivSurfaceArea, float inCostLeafDivSurfaceArea) const; + + /// Recursive helper function to calculate min and max triangles per node + void GetTriangleCountPerNodeInternal(float &outAverage, uint &outAverageDivisor, uint &outMin, uint &outMax) const; + }; + + /// Constructor + AABBTreeBuilder(TriangleSplitter &inSplitter, uint inMaxTrianglesPerLeaf = 16); + + /// Recursively build tree, returns the root node of the tree + Node * Build(AABBTreeBuilderStats &outStats); + +private: + Node * BuildInternal(const TriangleSplitter::Range &inTriangles); + + TriangleSplitter & mTriangleSplitter; + const uint mMaxTrianglesPerLeaf; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/AABBTree/AABBTreeToBuffer.h b/WickedEngine/Jolt/AABBTree/AABBTreeToBuffer.h new file mode 100644 index 000000000..1fa9bb7ae --- /dev/null +++ b/WickedEngine/Jolt/AABBTree/AABBTreeToBuffer.h @@ -0,0 +1,245 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +template using Deque = std::deque>; + +/// Conversion algorithm that converts an AABB tree to an optimized binary buffer +template +class AABBTreeToBuffer +{ +public: + /// Header for the tree + using NodeHeader = typename NodeCodec::Header; + + /// Size in bytes of the header of the tree + static const int HeaderSize = NodeCodec::HeaderSize; + + /// Maximum number of children per node in the tree + static const int NumChildrenPerNode = NodeCodec::NumChildrenPerNode; + + /// Header for the triangles + using TriangleHeader = typename TriangleCodec::TriangleHeader; + + /// Size in bytes of the header for the triangles + static const int TriangleHeaderSize = TriangleCodec::TriangleHeaderSize; + + /// Convert AABB tree. Returns false if failed. + bool Convert(const VertexList &inVertices, const AABBTreeBuilder::Node *inRoot, const char *&outError) + { + const typename NodeCodec::EncodingContext node_ctx; + typename TriangleCodec::EncodingContext tri_ctx(inVertices); + + // Estimate the amount of memory required + uint tri_count = inRoot->GetTriangleCountInTree(); + uint node_count = inRoot->GetNodeCount(); + uint nodes_size = node_ctx.GetPessimisticMemoryEstimate(node_count); + uint total_size = HeaderSize + TriangleHeaderSize + nodes_size + tri_ctx.GetPessimisticMemoryEstimate(tri_count); + mTree.reserve(total_size); + + // Reset counters + mNodesSize = 0; + + // Add headers + NodeHeader *header = HeaderSize > 0? mTree.Allocate() : nullptr; + TriangleHeader *triangle_header = TriangleHeaderSize > 0? mTree.Allocate() : nullptr; + + struct NodeData + { + const AABBTreeBuilder::Node * mNode = nullptr; // Node that this entry belongs to + Vec3 mNodeBoundsMin; // Quantized node bounds + Vec3 mNodeBoundsMax; + uint mNodeStart = uint(-1); // Start of node in mTree + uint mTriangleStart = uint(-1); // Start of the triangle data in mTree + uint mNumChildren = 0; // Number of children + uint mChildNodeStart[NumChildrenPerNode]; // Start of the children of the node in mTree + uint mChildTrianglesStart[NumChildrenPerNode]; // Start of the triangle data in mTree + uint * mParentChildNodeStart = nullptr; // Where to store mNodeStart (to patch mChildNodeStart of my parent) + uint * mParentTrianglesStart = nullptr; // Where to store mTriangleStart (to patch mChildTrianglesStart of my parent) + }; + + Deque to_process; + Deque to_process_triangles; + Array node_list; + + node_list.reserve(node_count); // Needed to ensure that array is not reallocated, so we can keep pointers in the array + + NodeData root; + root.mNode = inRoot; + root.mNodeBoundsMin = inRoot->mBounds.mMin; + root.mNodeBoundsMax = inRoot->mBounds.mMax; + node_list.push_back(root); + to_process.push_back(&node_list.back()); + + // Child nodes out of loop so we don't constantly realloc it + Array child_nodes; + child_nodes.reserve(NumChildrenPerNode); + + for (;;) + { + while (!to_process.empty()) + { + // Get the next node to process + NodeData *node_data = to_process.back(); + to_process.pop_back(); + + // Due to quantization box could have become bigger, not smaller + JPH_ASSERT(AABox(node_data->mNodeBoundsMin, node_data->mNodeBoundsMax).Contains(node_data->mNode->mBounds), "AABBTreeToBuffer: Bounding box became smaller!"); + + // Collect the first NumChildrenPerNode sub-nodes in the tree + child_nodes.clear(); // Won't free the memory + node_data->mNode->GetNChildren(NumChildrenPerNode, child_nodes); + node_data->mNumChildren = (uint)child_nodes.size(); + + // Fill in default child bounds + Vec3 child_bounds_min[NumChildrenPerNode], child_bounds_max[NumChildrenPerNode]; + for (size_t i = 0; i < NumChildrenPerNode; ++i) + if (i < child_nodes.size()) + { + child_bounds_min[i] = child_nodes[i]->mBounds.mMin; + child_bounds_max[i] = child_nodes[i]->mBounds.mMax; + } + else + { + child_bounds_min[i] = Vec3::sZero(); + child_bounds_max[i] = Vec3::sZero(); + } + + // Start a new node + uint old_size = (uint)mTree.size(); + node_data->mNodeStart = node_ctx.NodeAllocate(node_data->mNode, node_data->mNodeBoundsMin, node_data->mNodeBoundsMax, child_nodes, child_bounds_min, child_bounds_max, mTree, outError); + if (node_data->mNodeStart == uint(-1)) + return false; + mNodesSize += (uint)mTree.size() - old_size; + + if (node_data->mNode->HasChildren()) + { + // Insert in reverse order so we process left child first when taking nodes from the back + for (int idx = int(child_nodes.size()) - 1; idx >= 0; --idx) + { + // Due to quantization box could have become bigger, not smaller + JPH_ASSERT(AABox(child_bounds_min[idx], child_bounds_max[idx]).Contains(child_nodes[idx]->mBounds), "AABBTreeToBuffer: Bounding box became smaller!"); + + // Add child to list of nodes to be processed + NodeData child; + child.mNode = child_nodes[idx]; + child.mNodeBoundsMin = child_bounds_min[idx]; + child.mNodeBoundsMax = child_bounds_max[idx]; + child.mParentChildNodeStart = &node_data->mChildNodeStart[idx]; + child.mParentTrianglesStart = &node_data->mChildTrianglesStart[idx]; + NodeData *old = &node_list[0]; + node_list.push_back(child); + if (old != &node_list[0]) + { + outError = "Internal Error: Array reallocated, memory corruption!"; + return false; + } + + // Store triangles in separate list so we process them last + if (node_list.back().mNode->HasChildren()) + to_process.push_back(&node_list.back()); + else + to_process_triangles.push_back(&node_list.back()); + } + } + else + { + // Add triangles + node_data->mTriangleStart = tri_ctx.Pack(node_data->mNode->mTriangles, mTree, outError); + if (node_data->mTriangleStart == uint(-1)) + return false; + } + + // Patch offset into parent + if (node_data->mParentChildNodeStart != nullptr) + { + *node_data->mParentChildNodeStart = node_data->mNodeStart; + *node_data->mParentTrianglesStart = node_data->mTriangleStart; + } + } + + // If we've got triangles to process, loop again with just the triangles + if (to_process_triangles.empty()) + break; + else + to_process.swap(to_process_triangles); + } + + // Finalize all nodes + for (NodeData &n : node_list) + if (!node_ctx.NodeFinalize(n.mNode, n.mNodeStart, n.mNumChildren, n.mChildNodeStart, n.mChildTrianglesStart, mTree, outError)) + return false; + + // Finalize the triangles + tri_ctx.Finalize(inVertices, triangle_header, mTree); + + // Validate that we reserved enough memory + if (nodes_size < mNodesSize) + { + outError = "Internal Error: Not enough memory reserved for nodes!"; + return false; + } + if (total_size < (uint)mTree.size()) + { + outError = "Internal Error: Not enough memory reserved for triangles!"; + return false; + } + + // Finalize the nodes + if (!node_ctx.Finalize(header, inRoot, node_list[0].mNodeStart, node_list[0].mTriangleStart, outError)) + return false; + + // Shrink the tree, this will invalidate the header and triangle_header variables + mTree.shrink_to_fit(); + + return true; + } + + /// Get resulting data + inline const ByteBuffer & GetBuffer() const + { + return mTree; + } + + /// Get resulting data + inline ByteBuffer & GetBuffer() + { + return mTree; + } + + /// Get header for tree + inline const NodeHeader * GetNodeHeader() const + { + return mTree.Get(0); + } + + /// Get header for triangles + inline const TriangleHeader * GetTriangleHeader() const + { + return mTree.Get(HeaderSize); + } + + /// Get root of resulting tree + inline const void * GetRoot() const + { + return mTree.Get(HeaderSize + TriangleHeaderSize); + } + +private: + ByteBuffer mTree; ///< Resulting tree structure + uint mNodesSize; ///< Size in bytes of the nodes in the buffer +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h b/WickedEngine/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h new file mode 100644 index 000000000..e5376ea82 --- /dev/null +++ b/WickedEngine/Jolt/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h @@ -0,0 +1,287 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +template +class NodeCodecQuadTreeHalfFloat +{ +public: + /// Number of child nodes of this node + static constexpr int NumChildrenPerNode = 4; + + /// Header for the tree + struct Header + { + Float3 mRootBoundsMin; + Float3 mRootBoundsMax; + uint32 mRootProperties; + }; + + /// Size of the header (an empty struct is always > 0 bytes so this needs a separate variable) + static constexpr int HeaderSize = sizeof(Header); + + /// Stack size to use during DecodingContext::sWalkTree + static constexpr int StackSize = 128; + + /// Node properties + enum : uint32 + { + TRIANGLE_COUNT_BITS = 4, + TRIANGLE_COUNT_SHIFT = 28, + TRIANGLE_COUNT_MASK = (1 << TRIANGLE_COUNT_BITS) - 1, + OFFSET_BITS = 28, + OFFSET_MASK = (1 << OFFSET_BITS) - 1, + OFFSET_NON_SIGNIFICANT_BITS = 2, + OFFSET_NON_SIGNIFICANT_MASK = (1 << OFFSET_NON_SIGNIFICANT_BITS) - 1, + }; + + /// Node structure + struct Node + { + HalfFloat mBoundsMinX[4]; ///< 4 child bounding boxes + HalfFloat mBoundsMinY[4]; + HalfFloat mBoundsMinZ[4]; + HalfFloat mBoundsMaxX[4]; + HalfFloat mBoundsMaxY[4]; + HalfFloat mBoundsMaxZ[4]; + uint32 mNodeProperties[4]; ///< 4 child node properties + }; + + static_assert(sizeof(Node) == 64, "Node should be 64 bytes"); + + /// This class encodes and compresses quad tree nodes + class EncodingContext + { + public: + /// Get an upper bound on the amount of bytes needed for a node tree with inNodeCount nodes + uint GetPessimisticMemoryEstimate(uint inNodeCount) const + { + return inNodeCount * (sizeof(Node) + Alignment - 1); + } + + /// Allocate a new node for inNode. + /// Algorithm can modify the order of ioChildren to indicate in which order children should be compressed + /// Algorithm can enlarge the bounding boxes of the children during compression and returns these in outChildBoundsMin, outChildBoundsMax + /// inNodeBoundsMin, inNodeBoundsMax is the bounding box if inNode possibly widened by compressing the parent node + /// Returns uint(-1) on error and reports the error in outError + uint NodeAllocate(const AABBTreeBuilder::Node *inNode, Vec3Arg inNodeBoundsMin, Vec3Arg inNodeBoundsMax, Array &ioChildren, Vec3 outChildBoundsMin[NumChildrenPerNode], Vec3 outChildBoundsMax[NumChildrenPerNode], ByteBuffer &ioBuffer, const char *&outError) const + { + // We don't emit nodes for leafs + if (!inNode->HasChildren()) + return (uint)ioBuffer.size(); + + // Align the buffer + ioBuffer.Align(Alignment); + uint node_start = (uint)ioBuffer.size(); + + // Fill in bounds + Node *node = ioBuffer.Allocate(); + + for (size_t i = 0; i < 4; ++i) + { + if (i < ioChildren.size()) + { + const AABBTreeBuilder::Node *this_node = ioChildren[i]; + + // Copy bounding box + node->mBoundsMinX[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMin.GetX()); + node->mBoundsMinY[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMin.GetY()); + node->mBoundsMinZ[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMin.GetZ()); + node->mBoundsMaxX[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMax.GetX()); + node->mBoundsMaxY[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMax.GetY()); + node->mBoundsMaxZ[i] = HalfFloatConversion::FromFloat(this_node->mBounds.mMax.GetZ()); + + // Store triangle count + node->mNodeProperties[i] = this_node->GetTriangleCount() << TRIANGLE_COUNT_SHIFT; + if (this_node->GetTriangleCount() >= TRIANGLE_COUNT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Too many triangles"; + return uint(-1); + } + } + else + { + // Make this an invalid triangle node + node->mNodeProperties[i] = uint32(TRIANGLE_COUNT_MASK) << TRIANGLE_COUNT_SHIFT; + + // Make bounding box invalid + node->mBoundsMinX[i] = HALF_FLT_MAX; + node->mBoundsMinY[i] = HALF_FLT_MAX; + node->mBoundsMinZ[i] = HALF_FLT_MAX; + node->mBoundsMaxX[i] = HALF_FLT_MAX; + node->mBoundsMaxY[i] = HALF_FLT_MAX; + node->mBoundsMaxZ[i] = HALF_FLT_MAX; + } + } + + // Since we don't keep track of the bounding box while descending the tree, we keep the root bounds at all levels for triangle compression + for (int i = 0; i < NumChildrenPerNode; ++i) + { + outChildBoundsMin[i] = inNodeBoundsMin; + outChildBoundsMax[i] = inNodeBoundsMax; + } + + return node_start; + } + + /// Once all nodes have been added, this call finalizes all nodes by patching in the offsets of the child nodes (that were added after the node itself was added) + bool NodeFinalize(const AABBTreeBuilder::Node *inNode, uint inNodeStart, uint inNumChildren, const uint *inChildrenNodeStart, const uint *inChildrenTrianglesStart, ByteBuffer &ioBuffer, const char *&outError) const + { + if (!inNode->HasChildren()) + return true; + + Node *node = ioBuffer.Get(inNodeStart); + for (uint i = 0; i < inNumChildren; ++i) + { + // If there are triangles, use the triangle offset otherwise use the node offset + uint offset = node->mNodeProperties[i] != 0? inChildrenTrianglesStart[i] : inChildrenNodeStart[i]; + if (offset & OFFSET_NON_SIGNIFICANT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Internal Error: Offset has non-significant bits set"; + return false; + } + offset >>= OFFSET_NON_SIGNIFICANT_BITS; + if (offset & ~OFFSET_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Offset too large. Too much data."; + return false; + } + + // Store offset of next node / triangles + node->mNodeProperties[i] |= offset; + } + + return true; + } + + /// Once all nodes have been finalized, this will finalize the header of the nodes + bool Finalize(Header *outHeader, const AABBTreeBuilder::Node *inRoot, uint inRootNodeStart, uint inRootTrianglesStart, const char *&outError) const + { + uint offset = inRoot->HasChildren()? inRootNodeStart : inRootTrianglesStart; + if (offset & OFFSET_NON_SIGNIFICANT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Internal Error: Offset has non-significant bits set"; + return false; + } + offset >>= OFFSET_NON_SIGNIFICANT_BITS; + if (offset & ~OFFSET_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Offset too large. Too much data."; + return false; + } + + inRoot->mBounds.mMin.StoreFloat3(&outHeader->mRootBoundsMin); + inRoot->mBounds.mMax.StoreFloat3(&outHeader->mRootBoundsMax); + outHeader->mRootProperties = offset + (inRoot->GetTriangleCount() << TRIANGLE_COUNT_SHIFT); + if (inRoot->GetTriangleCount() >= TRIANGLE_COUNT_MASK) + { + outError = "NodeCodecQuadTreeHalfFloat: Too many triangles"; + return false; + } + + return true; + } + }; + + /// This class decodes and decompresses quad tree nodes + class DecodingContext + { + public: + /// Get the amount of bits needed to store an ID to a triangle block + inline static uint sTriangleBlockIDBits(const ByteBuffer &inTree) + { + return 32 - CountLeadingZeros((uint32)inTree.size()) - OFFSET_NON_SIGNIFICANT_BITS; + } + + /// Convert a triangle block ID to the start of the triangle buffer + inline static const void * sGetTriangleBlockStart(const uint8 *inBufferStart, uint inTriangleBlockID) + { + return inBufferStart + (inTriangleBlockID << OFFSET_NON_SIGNIFICANT_BITS); + } + + /// Constructor + JPH_INLINE explicit DecodingContext(const Header *inHeader) + { + // Start with the root node on the stack + mNodeStack[0] = inHeader->mRootProperties; + } + + /// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitTriangles for each triangle encountered + template + JPH_INLINE void WalkTree(const uint8 *inBufferStart, const TriangleContext &inTriangleContext, Visitor &ioVisitor) + { + do + { + // Test if node contains triangles + uint32 node_properties = mNodeStack[mTop]; + uint32 tri_count = node_properties >> TRIANGLE_COUNT_SHIFT; + if (tri_count == 0) + { + const Node *node = reinterpret_cast(inBufferStart + (node_properties << OFFSET_NON_SIGNIFICANT_BITS)); + + // Unpack bounds + UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast(&node->mBoundsMinX[0])); + Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy); + Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle()); + + UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast(&node->mBoundsMinZ[0])); + Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx); + Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle()); + + UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast(&node->mBoundsMaxY[0])); + Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz); + Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle()); + + // Load properties for 4 children + UVec4 properties = UVec4::sLoadInt4(&node->mNodeProperties[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop); + + // Push them onto the stack + JPH_ASSERT(mTop + 4 < StackSize); + properties.StoreInt4(&mNodeStack[mTop]); + mTop += num_results; + } + else if (tri_count != TRIANGLE_COUNT_MASK) // TRIANGLE_COUNT_MASK indicates a padding node, normally we shouldn't visit these nodes but when querying with a big enough box you could touch HALF_FLT_MAX (about 65K) + { + // Node contains triangles, do individual tests + uint32 triangle_block_id = node_properties & OFFSET_MASK; + const void *triangles = sGetTriangleBlockStart(inBufferStart, triangle_block_id); + + ioVisitor.VisitTriangles(inTriangleContext, triangles, tri_count, triangle_block_id); + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + + // Fetch next node until we find one that the visitor wants to see + do + --mTop; + while (mTop >= 0 && !ioVisitor.ShouldVisitNode(mTop)); + } + while (mTop >= 0); + } + + /// This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkTree() again) + bool IsDoneWalking() const + { + return mTop < 0; + } + + private: + uint32 mNodeStack[StackSize]; + int mTop = 0; + }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h b/WickedEngine/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h new file mode 100644 index 000000000..7d2972187 --- /dev/null +++ b/WickedEngine/Jolt/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h @@ -0,0 +1,456 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Store vertices in 64 bits and indices in 8 bits + 8 bit of flags per triangle like this: +/// +/// TriangleBlockHeader, +/// TriangleBlock (4 triangles and their flags in 16 bytes), +/// TriangleBlock... +/// +/// Vertices are stored: +/// +/// VertexData (1 vertex in 64 bits), +/// VertexData... +/// +/// They're compressed relative to the bounding box as provided by the node codec. +class TriangleCodecIndexed8BitPackSOA4Flags +{ +public: + class TriangleHeader + { + public: + Float3 mOffset; ///< Offset of all vertices + Float3 mScale; ///< Scale of all vertices, vertex_position = mOffset + mScale * compressed_vertex_position + }; + + /// Size of the header (an empty struct is always > 0 bytes so this needs a separate variable) + static constexpr int TriangleHeaderSize = sizeof(TriangleHeader); + + /// If this codec could return a different offset than the current buffer size when calling Pack() + static constexpr bool ChangesOffsetOnPack = false; + + /// Amount of bits per component + enum EComponentData : uint32 + { + COMPONENT_BITS = 21, + COMPONENT_MASK = (1 << COMPONENT_BITS) - 1, + }; + + /// Packed X and Y coordinate + enum EVertexXY : uint32 + { + COMPONENT_X = 0, + COMPONENT_Y1 = COMPONENT_BITS, + COMPONENT_Y1_BITS = 32 - COMPONENT_BITS, + }; + + /// Packed Z and Y coordinate + enum EVertexZY : uint32 + { + COMPONENT_Z = 0, + COMPONENT_Y2 = COMPONENT_BITS, + COMPONENT_Y2_BITS = 31 - COMPONENT_BITS, + }; + + /// A single packed vertex + struct VertexData + { + uint32 mVertexXY; + uint32 mVertexZY; + }; + + static_assert(sizeof(VertexData) == 8, "Compiler added padding"); + + /// A block of 4 triangles + struct TriangleBlock + { + uint8 mIndices[3][4]; ///< 8 bit indices to triangle vertices for 4 triangles in the form mIndices[vertex][triangle] where vertex in [0, 2] and triangle in [0, 3] + uint8 mFlags[4]; ///< Triangle flags (could contain material and active edges) + }; + + static_assert(sizeof(TriangleBlock) == 16, "Compiler added padding"); + + /// A triangle header, will be followed by one or more TriangleBlocks + struct TriangleBlockHeader + { + const VertexData * GetVertexData() const { return reinterpret_cast(reinterpret_cast(this) + mOffsetToVertices); } + const TriangleBlock * GetTriangleBlock() const { return reinterpret_cast(reinterpret_cast(this) + sizeof(TriangleBlockHeader)); } + + uint32 mOffsetToVertices; ///< Offset from current block to start of vertices in bytes + }; + + static_assert(sizeof(TriangleBlockHeader) == 4, "Compiler added padding"); + + /// This class is used to validate that the triangle data will not be degenerate after compression + class ValidationContext + { + public: + /// Constructor + ValidationContext(const IndexedTriangleList &inTriangles, const VertexList &inVertices) : + mVertices(inVertices) + { + // Only used the referenced triangles, just like EncodingContext::Finalize does + for (const IndexedTriangle &i : inTriangles) + for (uint32 idx : i.mIdx) + mBounds.Encapsulate(Vec3(inVertices[idx])); + } + + /// Test if a triangle will be degenerate after quantization + bool IsDegenerate(const IndexedTriangle &inTriangle) const + { + // Quantize the triangle in the same way as EncodingContext::Finalize does + UVec4 quantized_vertex[3]; + Vec3 compress_scale = Vec3::sReplicate(COMPONENT_MASK) / Vec3::sMax(mBounds.GetSize(), Vec3::sReplicate(1.0e-20f)); + for (int i = 0; i < 3; ++i) + quantized_vertex[i] = ((Vec3(mVertices[inTriangle.mIdx[i]]) - mBounds.mMin) * compress_scale + Vec3::sReplicate(0.5f)).ToInt(); + return quantized_vertex[0] == quantized_vertex[1] || quantized_vertex[1] == quantized_vertex[2] || quantized_vertex[0] == quantized_vertex[2]; + } + + private: + const VertexList & mVertices; + AABox mBounds; + }; + + /// This class is used to encode and compress triangle data into a byte buffer + class EncodingContext + { + public: + /// Construct the encoding context + explicit EncodingContext(const VertexList &inVertices) : + mVertexMap(inVertices.size(), 0xffffffff) // Fill vertex map with 'not found' + { + // Reserve for worst case to avoid allocating in the inner loop + mVertices.reserve(inVertices.size()); + } + + /// Get an upper bound on the amount of bytes needed to store inTriangleCount triangles + uint GetPessimisticMemoryEstimate(uint inTriangleCount) const + { + // Worst case each triangle is alone in a block, none of the vertices are shared and we need to add 3 bytes to align the vertices + return inTriangleCount * (sizeof(TriangleBlockHeader) + sizeof(TriangleBlock) + 3 * sizeof(VertexData)) + 3; + } + + /// Pack the triangles in inContainer to ioBuffer. This stores the mMaterialIndex of a triangle in the 8 bit flags. + /// Returns uint(-1) on error. + uint Pack(const IndexedTriangleList &inTriangles, ByteBuffer &ioBuffer, const char *&outError) + { + // Determine position of triangles start + uint offset = (uint)ioBuffer.size(); + + // Update stats + uint tri_count = (uint)inTriangles.size(); + mNumTriangles += tri_count; + + // Allocate triangle block header + TriangleBlockHeader *header = ioBuffer.Allocate(); + + // Compute first vertex that this batch will use (ensuring there's enough room if none of the vertices are shared) + uint start_vertex = Clamp((int)mVertices.size() - 256 + (int)tri_count * 3, 0, (int)mVertices.size()); + + // Store the start vertex offset, this will later be patched to give the delta offset relative to the triangle block + mOffsetsToPatch.push_back(uint((uint8 *)&header->mOffsetToVertices - &ioBuffer[0])); + header->mOffsetToVertices = start_vertex * sizeof(VertexData); + + // Pack vertices + uint padded_triangle_count = AlignUp(tri_count, 4); + for (uint t = 0; t < padded_triangle_count; t += 4) + { + TriangleBlock *block = ioBuffer.Allocate(); + for (uint vertex_nr = 0; vertex_nr < 3; ++vertex_nr) + for (uint block_tri_idx = 0; block_tri_idx < 4; ++block_tri_idx) + { + // Fetch vertex index. Create degenerate triangles for padding triangles. + bool triangle_available = t + block_tri_idx < tri_count; + uint32 src_vertex_index = triangle_available? inTriangles[t + block_tri_idx].mIdx[vertex_nr] : inTriangles[tri_count - 1].mIdx[0]; + + // Check if we've seen this vertex before and if it is in the range that we can encode + uint32 &vertex_index = mVertexMap[src_vertex_index]; + if (vertex_index == 0xffffffff || vertex_index < start_vertex) + { + // Add vertex + vertex_index = (uint32)mVertices.size(); + mVertices.push_back(src_vertex_index); + } + + // Store vertex index + uint32 vertex_offset = vertex_index - start_vertex; + if (vertex_offset > 0xff) + { + outError = "TriangleCodecIndexed8BitPackSOA4Flags: Offset doesn't fit in 8 bit"; + return uint(-1); + } + block->mIndices[vertex_nr][block_tri_idx] = (uint8)vertex_offset; + + // Store flags + uint32 flags = triangle_available? inTriangles[t + block_tri_idx].mMaterialIndex : 0; + if (flags > 0xff) + { + outError = "TriangleCodecIndexed8BitPackSOA4Flags: Material index doesn't fit in 8 bit"; + return uint(-1); + } + block->mFlags[block_tri_idx] = (uint8)flags; + } + } + + return offset; + } + + /// After all triangles have been packed, this finalizes the header and triangle buffer + void Finalize(const VertexList &inVertices, TriangleHeader *ioHeader, ByteBuffer &ioBuffer) const + { + // Check if anything to do + if (mVertices.empty()) + return; + + // Align buffer to 4 bytes + uint vertices_idx = (uint)ioBuffer.Align(4); + + // Patch the offsets + for (uint o : mOffsetsToPatch) + *ioBuffer.Get(o) += vertices_idx - o; + + // Calculate bounding box + AABox bounds; + for (uint32 v : mVertices) + bounds.Encapsulate(Vec3(inVertices[v])); + + // Compress vertices + VertexData *vertices = ioBuffer.Allocate(mVertices.size()); + Vec3 compress_scale = Vec3::sReplicate(COMPONENT_MASK) / Vec3::sMax(bounds.GetSize(), Vec3::sReplicate(1.0e-20f)); + for (uint32 v : mVertices) + { + UVec4 c = ((Vec3(inVertices[v]) - bounds.mMin) * compress_scale + Vec3::sReplicate(0.5f)).ToInt(); + JPH_ASSERT(c.GetX() <= COMPONENT_MASK); + JPH_ASSERT(c.GetY() <= COMPONENT_MASK); + JPH_ASSERT(c.GetZ() <= COMPONENT_MASK); + vertices->mVertexXY = c.GetX() + (c.GetY() << COMPONENT_Y1); + vertices->mVertexZY = c.GetZ() + ((c.GetY() >> COMPONENT_Y1_BITS) << COMPONENT_Y2); + ++vertices; + } + + // Store decompression information + bounds.mMin.StoreFloat3(&ioHeader->mOffset); + (bounds.GetSize() / Vec3::sReplicate(COMPONENT_MASK)).StoreFloat3(&ioHeader->mScale); + } + + private: + using VertexMap = Array; + + uint mNumTriangles = 0; + Array mVertices; ///< Output vertices as an index into the original vertex list (inVertices), sorted according to occurrence + VertexMap mVertexMap; ///< Maps from the original mesh vertex index (inVertices) to the index in our output vertices (mVertices) + Array mOffsetsToPatch; ///< Offsets to the vertex buffer that need to be patched in once all nodes have been packed + }; + + /// This class is used to decode and decompress triangle data packed by the EncodingContext + class DecodingContext + { + private: + /// Private helper functions to unpack the 1 vertex of 4 triangles (outX contains the x coordinate of triangle 0 .. 3 etc.) + JPH_INLINE void Unpack(const VertexData *inVertices, UVec4Arg inIndex, Vec4 &outX, Vec4 &outY, Vec4 &outZ) const + { + // Get compressed data + UVec4 c1 = UVec4::sGatherInt4<8>(&inVertices->mVertexXY, inIndex); + UVec4 c2 = UVec4::sGatherInt4<8>(&inVertices->mVertexZY, inIndex); + + // Unpack the x y and z component + UVec4 xc = UVec4::sAnd(c1, UVec4::sReplicate(COMPONENT_MASK)); + UVec4 yc = UVec4::sOr(c1.LogicalShiftRight(), c2.LogicalShiftRight().LogicalShiftLeft()); + UVec4 zc = UVec4::sAnd(c2, UVec4::sReplicate(COMPONENT_MASK)); + + // Convert to float + outX = Vec4::sFusedMultiplyAdd(xc.ToFloat(), mScaleX, mOffsetX); + outY = Vec4::sFusedMultiplyAdd(yc.ToFloat(), mScaleY, mOffsetY); + outZ = Vec4::sFusedMultiplyAdd(zc.ToFloat(), mScaleZ, mOffsetZ); + } + + public: + JPH_INLINE explicit DecodingContext(const TriangleHeader *inHeader) : + mOffsetX(Vec4::sReplicate(inHeader->mOffset.x)), + mOffsetY(Vec4::sReplicate(inHeader->mOffset.y)), + mOffsetZ(Vec4::sReplicate(inHeader->mOffset.z)), + mScaleX(Vec4::sReplicate(inHeader->mScale.x)), + mScaleY(Vec4::sReplicate(inHeader->mScale.y)), + mScaleZ(Vec4::sReplicate(inHeader->mScale.z)) + { + } + + /// Unpacks triangles in the format t1v1,t1v2,t1v3, t2v1,t2v2,t2v3, ... + JPH_INLINE void Unpack(const void *inTriangleStart, uint32 inNumTriangles, Vec3 *outTriangles) const + { + JPH_ASSERT(inNumTriangles > 0); + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const VertexData *vertices = header->GetVertexData(); + const TriangleBlock *t = header->GetTriangleBlock(); + const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2); + + int triangles_left = inNumTriangles; + + do + { + // Get the indices for the three vertices (reads 4 bytes extra, but these are the flags so that's ok) + UVec4 indices = UVec4::sLoadInt4(reinterpret_cast(&t->mIndices[0])); + UVec4 iv1 = indices.Expand4Byte0(); + UVec4 iv2 = indices.Expand4Byte4(); + UVec4 iv3 = indices.Expand4Byte8(); + + // Decompress the triangle data + Vec4 v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z; + Unpack(vertices, iv1, v1x, v1y, v1z); + Unpack(vertices, iv2, v2x, v2y, v2z); + Unpack(vertices, iv3, v3x, v3y, v3z); + + // Transpose it so we get normal vectors + Mat44 v1 = Mat44(v1x, v1y, v1z, Vec4::sZero()).Transposed(); + Mat44 v2 = Mat44(v2x, v2y, v2z, Vec4::sZero()).Transposed(); + Mat44 v3 = Mat44(v3x, v3y, v3z, Vec4::sZero()).Transposed(); + + // Store triangle data + for (int i = 0; i < 4 && triangles_left > 0; ++i, --triangles_left) + { + *outTriangles++ = v1.GetColumn3(i); + *outTriangles++ = v2.GetColumn3(i); + *outTriangles++ = v3.GetColumn3(i); + } + + ++t; + } + while (t < end); + } + + /// Tests a ray against the packed triangles + JPH_INLINE float TestRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, const void *inTriangleStart, uint32 inNumTriangles, float inClosest, uint32 &outClosestTriangleIndex) const + { + JPH_ASSERT(inNumTriangles > 0); + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const VertexData *vertices = header->GetVertexData(); + const TriangleBlock *t = header->GetTriangleBlock(); + const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2); + + Vec4 closest = Vec4::sReplicate(inClosest); + UVec4 closest_triangle_idx = UVec4::sZero(); + + UVec4 start_triangle_idx = UVec4::sZero(); + do + { + // Get the indices for the three vertices (reads 4 bytes extra, but these are the flags so that's ok) + UVec4 indices = UVec4::sLoadInt4(reinterpret_cast(&t->mIndices[0])); + UVec4 iv1 = indices.Expand4Byte0(); + UVec4 iv2 = indices.Expand4Byte4(); + UVec4 iv3 = indices.Expand4Byte8(); + + // Decompress the triangle data + Vec4 v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z; + Unpack(vertices, iv1, v1x, v1y, v1z); + Unpack(vertices, iv2, v2x, v2y, v2z); + Unpack(vertices, iv3, v3x, v3y, v3z); + + // Perform ray vs triangle test + Vec4 distance = RayTriangle4(inRayOrigin, inRayDirection, v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z); + + // Update closest with the smaller values + UVec4 smaller = Vec4::sLess(distance, closest); + closest = Vec4::sSelect(closest, distance, smaller); + + // Update triangle index with the smallest values + UVec4 triangle_idx = start_triangle_idx + UVec4(0, 1, 2, 3); + closest_triangle_idx = UVec4::sSelect(closest_triangle_idx, triangle_idx, smaller); + + // Next block + ++t; + start_triangle_idx += UVec4::sReplicate(4); + } + while (t < end); + + // Get the smallest component + Vec4::sSort4(closest, closest_triangle_idx); + outClosestTriangleIndex = closest_triangle_idx.GetX(); + return closest.GetX(); + } + + /// Decode a single triangle + inline void GetTriangle(const void *inTriangleStart, uint32 inTriangleIdx, Vec3 &outV1, Vec3 &outV2, Vec3 &outV3) const + { + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const VertexData *vertices = header->GetVertexData(); + const TriangleBlock *block = header->GetTriangleBlock() + (inTriangleIdx >> 2); + uint32 block_triangle_idx = inTriangleIdx & 0b11; + + // Get the 3 vertices + const VertexData &v1 = vertices[block->mIndices[0][block_triangle_idx]]; + const VertexData &v2 = vertices[block->mIndices[1][block_triangle_idx]]; + const VertexData &v3 = vertices[block->mIndices[2][block_triangle_idx]]; + + // Pack the vertices + UVec4 c1(v1.mVertexXY, v2.mVertexXY, v3.mVertexXY, 0); + UVec4 c2(v1.mVertexZY, v2.mVertexZY, v3.mVertexZY, 0); + + // Unpack the x y and z component + UVec4 xc = UVec4::sAnd(c1, UVec4::sReplicate(COMPONENT_MASK)); + UVec4 yc = UVec4::sOr(c1.LogicalShiftRight(), c2.LogicalShiftRight().LogicalShiftLeft()); + UVec4 zc = UVec4::sAnd(c2, UVec4::sReplicate(COMPONENT_MASK)); + + // Convert to float + Vec4 vx = Vec4::sFusedMultiplyAdd(xc.ToFloat(), mScaleX, mOffsetX); + Vec4 vy = Vec4::sFusedMultiplyAdd(yc.ToFloat(), mScaleY, mOffsetY); + Vec4 vz = Vec4::sFusedMultiplyAdd(zc.ToFloat(), mScaleZ, mOffsetZ); + + // Transpose it so we get normal vectors + Mat44 trans = Mat44(vx, vy, vz, Vec4::sZero()).Transposed(); + outV1 = trans.GetAxisX(); + outV2 = trans.GetAxisY(); + outV3 = trans.GetAxisZ(); + } + + /// Get flags for entire triangle block + JPH_INLINE static void sGetFlags(const void *inTriangleStart, uint32 inNumTriangles, uint8 *outTriangleFlags) + { + JPH_ASSERT(inNumTriangles > 0); + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const TriangleBlock *t = header->GetTriangleBlock(); + const TriangleBlock *end = t + ((inNumTriangles + 3) >> 2); + + int triangles_left = inNumTriangles; + do + { + for (int i = 0; i < 4 && triangles_left > 0; ++i, --triangles_left) + *outTriangleFlags++ = t->mFlags[i]; + + ++t; + } + while (t < end); + } + + /// Get flags for a particular triangle + JPH_INLINE static uint8 sGetFlags(const void *inTriangleStart, int inTriangleIndex) + { + const TriangleBlockHeader *header = reinterpret_cast(inTriangleStart); + const TriangleBlock *first_block = header->GetTriangleBlock(); + return first_block[inTriangleIndex >> 2].mFlags[inTriangleIndex & 0b11]; + } + + /// Unpacks triangles and flags, convenience function + JPH_INLINE void Unpack(const void *inTriangleStart, uint32 inNumTriangles, Vec3 *outTriangles, uint8 *outTriangleFlags) const + { + Unpack(inTriangleStart, inNumTriangles, outTriangles); + sGetFlags(inTriangleStart, inNumTriangles, outTriangleFlags); + } + + private: + Vec4 mOffsetX; + Vec4 mOffsetY; + Vec4 mOffsetZ; + Vec4 mScaleX; + Vec4 mScaleY; + Vec4 mScaleZ; + }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/ConfigurationString.h b/WickedEngine/Jolt/ConfigurationString.h new file mode 100644 index 000000000..7c044f574 --- /dev/null +++ b/WickedEngine/Jolt/ConfigurationString.h @@ -0,0 +1,88 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Construct a string that lists the most important configuration settings +inline const char *GetConfigurationString() +{ + return JPH_IF_SINGLE_PRECISION_ELSE("Single", "Double") " precision " +#if defined(JPH_CPU_X86) + "x86 " +#elif defined(JPH_CPU_ARM) + "ARM " +#elif defined(JPH_PLATFORM_WASM) + "WASM " +#endif +#if JPH_CPU_ADDRESS_BITS == 64 + "64-bit " +#elif JPH_CPU_ADDRESS_BITS == 32 + "32-bit " +#endif + "with instructions: " +#ifdef JPH_USE_NEON + "NEON " +#endif +#ifdef JPH_USE_SSE + "SSE2 " +#endif +#ifdef JPH_USE_SSE4_1 + "SSE4.1 " +#endif +#ifdef JPH_USE_SSE4_2 + "SSE4.2 " +#endif +#ifdef JPH_USE_AVX + "AVX " +#endif +#ifdef JPH_USE_AVX2 + "AVX2 " +#endif +#ifdef JPH_USE_AVX512 + "AVX512 " +#endif +#ifdef JPH_USE_F16C + "F16C " +#endif +#ifdef JPH_USE_LZCNT + "LZCNT " +#endif +#ifdef JPH_USE_TZCNT + "TZCNT " +#endif +#ifdef JPH_USE_FMADD + "FMADD " +#endif +#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + "(Cross Platform Deterministic) " +#endif +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + "(FP Exceptions) " +#endif +#ifdef JPH_DEBUG_RENDERER + "(Debug Renderer) " +#endif +#ifdef JPH_PROFILE_ENABLED + "(Profile) " +#endif +#if defined(JPH_OBJECT_LAYER_BITS) && JPH_OBJECT_LAYER_BITS == 32 + "(32-bit ObjectLayer) " +#else + "(16-bit ObjectLayer) " +#endif +#ifdef JPH_ENABLE_ASSERTS + "(Assertions) " +#endif +#ifdef JPH_OBJECT_STREAM + "(ObjectStream) " +#endif +#ifdef JPH_DEBUG + "(Debug) " +#endif + ; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/ARMNeon.h b/WickedEngine/Jolt/Core/ARMNeon.h new file mode 100644 index 000000000..ee4d55278 --- /dev/null +++ b/WickedEngine/Jolt/Core/ARMNeon.h @@ -0,0 +1,88 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_USE_NEON + +#ifdef JPH_COMPILER_MSVC + JPH_NAMESPACE_BEGIN + + // Constructing NEON values + #define JPH_NEON_INT32x4(v1, v2, v3, v4) { int64_t(v1) + (int64_t(v2) << 32), int64_t(v3) + (int64_t(v4) << 32) } + #define JPH_NEON_UINT32x4(v1, v2, v3, v4) { uint64_t(v1) + (uint64_t(v2) << 32), uint64_t(v3) + (uint64_t(v4) << 32) } + #define JPH_NEON_INT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { int64_t(v1) + (int64_t(v2) << 8) + (int64_t(v3) << 16) + (int64_t(v4) << 24) + (int64_t(v5) << 32) + (int64_t(v6) << 40) + (int64_t(v7) << 48) + (int64_t(v8) << 56), int64_t(v9) + (int64_t(v10) << 8) + (int64_t(v11) << 16) + (int64_t(v12) << 24) + (int64_t(v13) << 32) + (int64_t(v14) << 40) + (int64_t(v15) << 48) + (int64_t(v16) << 56) } + + // Generic shuffle vector template + template + JPH_INLINE float32x4_t NeonShuffleFloat32x4(float32x4_t inV1, float32x4_t inV2) + { + float32x4_t ret; + ret = vmovq_n_f32(vgetq_lane_f32(I1 >= 4? inV2 : inV1, I1 & 0b11)); + ret = vsetq_lane_f32(vgetq_lane_f32(I2 >= 4? inV2 : inV1, I2 & 0b11), ret, 1); + ret = vsetq_lane_f32(vgetq_lane_f32(I3 >= 4? inV2 : inV1, I3 & 0b11), ret, 2); + ret = vsetq_lane_f32(vgetq_lane_f32(I4 >= 4? inV2 : inV1, I4 & 0b11), ret, 3); + return ret; + } + + // Specializations + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 2, 2>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vget_low_f32(inV1), vdup_lane_s32(vget_high_f32(inV1), 0)); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 3, 3>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vget_low_f32(inV1), vdup_lane_s32(vget_high_f32(inV1), 1)); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<0, 1, 2, 3>(float32x4_t inV1, float32x4_t inV2) + { + return inV1; + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<1, 0, 3, 2>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vrev64_f32(vget_low_f32(inV1)), vrev64_f32(vget_high_f32(inV1))); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<2, 2, 1, 0>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vdup_lane_s32(vget_high_f32(inV1), 0), vrev64_f32(vget_low_f32(inV1))); + } + + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<2, 3, 0, 1>(float32x4_t inV1, float32x4_t inV2) + { + return vcombine_f32(vget_high_f32(inV1), vget_low_f32(inV1)); + } + + // Used extensively by cross product + template <> + JPH_INLINE float32x4_t NeonShuffleFloat32x4<1, 2, 0, 0>(float32x4_t inV1, float32x4_t inV2) + { + static int8x16_t table = JPH_NEON_INT8x16(0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x00, 0x01, 0x02, 0x03, 0x00, 0x01, 0x02, 0x03); + return vreinterpretq_f32_u8(vqtbl1q_u8(vreinterpretq_u8_f32(inV1), table)); + } + + // Shuffle a vector + #define JPH_NEON_SHUFFLE_F32x4(vec1, vec2, index1, index2, index3, index4) NeonShuffleFloat32x4(vec1, vec2) + + JPH_NAMESPACE_END +#else + // Constructing NEON values + #define JPH_NEON_INT32x4(v1, v2, v3, v4) { v1, v2, v3, v4 } + #define JPH_NEON_UINT32x4(v1, v2, v3, v4) { v1, v2, v3, v4 } + #define JPH_NEON_INT8x16(v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16) { v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12, v13, v14, v15, v16 } + + // Shuffle a vector + #define JPH_NEON_SHUFFLE_F32x4(vec1, vec2, index1, index2, index3, index4) __builtin_shufflevector(vec1, vec2, index1, index2, index3, index4) +#endif + +#endif // JPH_USE_NEON diff --git a/WickedEngine/Jolt/Core/Array.h b/WickedEngine/Jolt/Core/Array.h new file mode 100644 index 000000000..1d167d857 --- /dev/null +++ b/WickedEngine/Jolt/Core/Array.h @@ -0,0 +1,598 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#ifdef JPH_USE_STD_VECTOR + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +template > using Array = std::vector; + +JPH_NAMESPACE_END + +#else + +JPH_NAMESPACE_BEGIN + +/// Simple replacement for std::vector +/// +/// Major differences: +/// - Memory is not initialized to zero (this was causing a lot of page faults when deserializing large MeshShapes / HeightFieldShapes) +/// - Iterators are simple pointers (for now) +/// - No exception safety +/// - No specialization like std::vector has +/// - Not all functions have been implemented +template > +class [[nodiscard]] Array : private Allocator +{ +public: + using value_type = T; + using size_type = size_t; + using pointer = T *; + using const_pointer = const T *; + using reference = T &; + using const_reference = const T &; + + using const_iterator = const T *; + using iterator = T *; + +private: + /// Move elements from one location to another + inline void move(pointer inDestination, pointer inSource, size_type inCount) + { + if constexpr (std::is_trivially_copyable()) + memmove(inDestination, inSource, inCount * sizeof(T)); + else + { + if (inDestination < inSource) + { + for (T *destination_end = inDestination + inCount; inDestination < destination_end; ++inDestination, ++inSource) + { + ::new (inDestination) T(std::move(*inSource)); + inSource->~T(); + } + } + else + { + for (T *destination = inDestination + inCount - 1, *source = inSource + inCount - 1; destination >= inDestination; --destination, --source) + { + ::new (destination) T(std::move(*source)); + source->~T(); + } + } + } + } + + /// Reallocate the data block to inNewCapacity + inline void reallocate(size_type inNewCapacity) + { + JPH_ASSERT(inNewCapacity > 0 && inNewCapacity >= mSize); + + pointer pointer; + if constexpr (AllocatorHasReallocate::sValue) + { + // Reallocate data block + pointer = get_allocator().reallocate(mElements, mCapacity, inNewCapacity); + } + else + { + // Copy data to a new location + pointer = get_allocator().allocate(inNewCapacity); + if (mElements != nullptr) + { + move(pointer, mElements, mSize); + get_allocator().deallocate(mElements, mCapacity); + } + } + mElements = pointer; + mCapacity = inNewCapacity; + } + + /// Destruct elements [inStart, inEnd - 1] + inline void destruct(size_type inStart, size_type inEnd) + { + if constexpr (!is_trivially_destructible()) + if (inStart < inEnd) + for (T *element = mElements + inStart, *element_end = mElements + inEnd; element < element_end; ++element) + element->~T(); + } + +public: + /// Reserve array space + inline void reserve(size_type inNewSize) + { + if (mCapacity < inNewSize) + reallocate(inNewSize); + } + + /// Resize array to new length + inline void resize(size_type inNewSize) + { + destruct(inNewSize, mSize); + reserve(inNewSize); + + if constexpr (!is_trivially_constructible()) + for (T *element = mElements + mSize, *element_end = mElements + inNewSize; element < element_end; ++element) + ::new (element) T; + mSize = inNewSize; + } + + /// Resize array to new length and initialize all elements with inValue + inline void resize(size_type inNewSize, const T &inValue) + { + JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to resize"); + + destruct(inNewSize, mSize); + reserve(inNewSize); + + for (T *element = mElements + mSize, *element_end = mElements + inNewSize; element < element_end; ++element) + ::new (element) T(inValue); + mSize = inNewSize; + } + + /// Destruct all elements and set length to zero + inline void clear() + { + destruct(0, mSize); + mSize = 0; + } + +private: + /// Grow the array by at least inAmount elements + inline void grow(size_type inAmount = 1) + { + size_type min_size = mSize + inAmount; + if (min_size > mCapacity) + { + size_type new_capacity = max(min_size, mCapacity * 2); + reserve(new_capacity); + } + } + + /// Free memory + inline void free() + { + get_allocator().deallocate(mElements, mCapacity); + mElements = nullptr; + mCapacity = 0; + } + + /// Destroy all elements and free memory + inline void destroy() + { + if (mElements != nullptr) + { + clear(); + free(); + } + } + +public: + /// Replace the contents of this array with inBegin .. inEnd + template + inline void assign(Iterator inBegin, Iterator inEnd) + { + clear(); + reserve(size_type(std::distance(inBegin, inEnd))); + + for (Iterator element = inBegin; element != inEnd; ++element) + ::new (&mElements[mSize++]) T(*element); + } + + /// Replace the contents of this array with inList + inline void assign(std::initializer_list inList) + { + clear(); + reserve(size_type(inList.size())); + + for (typename std::initializer_list::iterator i = inList.begin(); i != inList.end(); ++i) + ::new (&mElements[mSize++]) T(*i); + } + + /// Default constructor + Array() = default; + + /// Constructor with allocator + explicit inline Array(const Allocator &inAllocator) : + Allocator(inAllocator) + { + } + + /// Constructor with length + explicit inline Array(size_type inLength, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + resize(inLength); + } + + /// Constructor with length and value + inline Array(size_type inLength, const T &inValue, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + resize(inLength, inValue); + } + + /// Constructor from initializer list + inline Array(std::initializer_list inList, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + assign(inList); + } + + /// Constructor from iterator + inline Array(const_iterator inBegin, const_iterator inEnd, const Allocator &inAllocator = { }) : + Allocator(inAllocator) + { + assign(inBegin, inEnd); + } + + /// Copy constructor + inline Array(const Array &inRHS) : + Allocator(inRHS.get_allocator()) + { + assign(inRHS.begin(), inRHS.end()); + } + + /// Move constructor + inline Array(Array &&inRHS) noexcept : + Allocator(std::move(inRHS.get_allocator())), + mSize(inRHS.mSize), + mCapacity(inRHS.mCapacity), + mElements(inRHS.mElements) + { + inRHS.mSize = 0; + inRHS.mCapacity = 0; + inRHS.mElements = nullptr; + } + + /// Destruct all elements + inline ~Array() + { + destroy(); + } + + /// Get the allocator + inline Allocator & get_allocator() + { + return *this; + } + + inline const Allocator &get_allocator() const + { + return *this; + } + + /// Add element to the back of the array + inline void push_back(const T &inValue) + { + JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to push_back"); + + grow(); + + T *element = mElements + mSize++; + ::new (element) T(inValue); + } + + inline void push_back(T &&inValue) + { + grow(); + + T *element = mElements + mSize++; + ::new (element) T(std::move(inValue)); + } + + /// Construct element at the back of the array + template + inline T & emplace_back(A &&... inValue) + { + grow(); + + T *element = mElements + mSize++; + ::new (element) T(std::forward(inValue)...); + return *element; + } + + /// Remove element from the back of the array + inline void pop_back() + { + JPH_ASSERT(mSize > 0); + mElements[--mSize].~T(); + } + + /// Returns true if there are no elements in the array + inline bool empty() const + { + return mSize == 0; + } + + /// Returns amount of elements in the array + inline size_type size() const + { + return mSize; + } + + /// Returns maximum amount of elements the array can hold + inline size_type capacity() const + { + return mCapacity; + } + + /// Reduce the capacity of the array to match its size + void shrink_to_fit() + { + if (mElements != nullptr) + { + if (mSize == 0) + free(); + else if (mCapacity > mSize) + reallocate(mSize); + } + } + + /// Swap the contents of two arrays + void swap(Array &inRHS) noexcept + { + std::swap(get_allocator(), inRHS.get_allocator()); + std::swap(mSize, inRHS.mSize); + std::swap(mCapacity, inRHS.mCapacity); + std::swap(mElements, inRHS.mElements); + } + + template + void insert(const_iterator inPos, Iterator inBegin, Iterator inEnd) + { + size_type num_elements = size_type(std::distance(inBegin, inEnd)); + if (num_elements > 0) + { + // After grow() inPos may be invalid + size_type first_element = inPos - mElements; + + grow(num_elements); + + T *element_begin = mElements + first_element; + T *element_end = element_begin + num_elements; + move(element_end, element_begin, mSize - first_element); + + for (T *element = element_begin; element < element_end; ++element, ++inBegin) + ::new (element) T(*inBegin); + + mSize += num_elements; + } + } + + void insert(const_iterator inPos, const T &inValue) + { + JPH_ASSERT(&inValue < mElements || &inValue >= mElements + mSize, "Can't pass an element from the array to insert"); + + // After grow() inPos may be invalid + size_type first_element = inPos - mElements; + + grow(); + + T *element = mElements + first_element; + move(element + 1, element, mSize - first_element); + + ::new (element) T(inValue); + mSize++; + } + + /// Remove one element from the array + void erase(const_iterator inIter) + { + size_type p = size_type(inIter - begin()); + JPH_ASSERT(p < mSize); + mElements[p].~T(); + if (p + 1 < mSize) + move(mElements + p, mElements + p + 1, mSize - p - 1); + --mSize; + } + + /// Remove multiple element from the array + void erase(const_iterator inBegin, const_iterator inEnd) + { + size_type p = size_type(inBegin - begin()); + size_type n = size_type(inEnd - inBegin); + JPH_ASSERT(inEnd <= end()); + destruct(p, p + n); + if (p + n < mSize) + move(mElements + p, mElements + p + n, mSize - p - n); + mSize -= n; + } + + /// Iterators + inline const_iterator begin() const + { + return mElements; + } + + inline const_iterator end() const + { + return mElements + mSize; + } + + inline const_iterator cbegin() const + { + return mElements; + } + + inline const_iterator cend() const + { + return mElements + mSize; + } + + inline iterator begin() + { + return mElements; + } + + inline iterator end() + { + return mElements + mSize; + } + + inline const T * data() const + { + return mElements; + } + + inline T * data() + { + return mElements; + } + + /// Access element + inline T & operator [] (size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + inline const T & operator [] (size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + /// Access element + inline T & at(size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + inline const T & at(size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return mElements[inIdx]; + } + + /// First element in the array + inline const T & front() const + { + JPH_ASSERT(mSize > 0); + return mElements[0]; + } + + inline T & front() + { + JPH_ASSERT(mSize > 0); + return mElements[0]; + } + + /// Last element in the array + inline const T & back() const + { + JPH_ASSERT(mSize > 0); + return mElements[mSize - 1]; + } + + inline T & back() + { + JPH_ASSERT(mSize > 0); + return mElements[mSize - 1]; + } + + /// Assignment operator + Array & operator = (const Array &inRHS) + { + if (static_cast(this) != static_cast(&inRHS)) + assign(inRHS.begin(), inRHS.end()); + + return *this; + } + + /// Assignment move operator + Array & operator = (Array &&inRHS) noexcept + { + if (static_cast(this) != static_cast(&inRHS)) + { + destroy(); + + get_allocator() = std::move(inRHS.get_allocator()); + + mSize = inRHS.mSize; + mCapacity = inRHS.mCapacity; + mElements = inRHS.mElements; + + inRHS.mSize = 0; + inRHS.mCapacity = 0; + inRHS.mElements = nullptr; + } + + return *this; + } + + /// Assignment operator + Array & operator = (std::initializer_list inRHS) + { + assign(inRHS); + + return *this; + } + + /// Comparing arrays + bool operator == (const Array &inRHS) const + { + if (mSize != inRHS.mSize) + return false; + for (size_type i = 0; i < mSize; ++i) + if (!(mElements[i] == inRHS.mElements[i])) + return false; + return true; + } + + bool operator != (const Array &inRHS) const + { + if (mSize != inRHS.mSize) + return true; + for (size_type i = 0; i < mSize; ++i) + if (mElements[i] != inRHS.mElements[i]) + return true; + return false; + } + +private: + size_type mSize = 0; + size_type mCapacity = 0; + T * mElements = nullptr; +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for Array + template + struct hash> + { + size_t operator () (const JPH::Array &inRHS) const + { + std::size_t ret = 0; + + // Hash length first + JPH::HashCombine(ret, inRHS.size()); + + // Then hash elements + for (const T &t : inRHS) + JPH::HashCombine(ret, t); + + return ret; + } + }; +} + +JPH_SUPPRESS_WARNING_POP + +#endif // JPH_USE_STD_VECTOR diff --git a/WickedEngine/Jolt/Core/Atomics.h b/WickedEngine/Jolt/Core/Atomics.h new file mode 100644 index 000000000..a53faa5c8 --- /dev/null +++ b/WickedEngine/Jolt/Core/Atomics.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::atomic; +using std::memory_order; +using std::memory_order_relaxed; +using std::memory_order_acquire; +using std::memory_order_release; +using std::memory_order_acq_rel; +using std::memory_order_seq_cst; + +/// Atomically compute the min(ioAtomic, inValue) and store it in ioAtomic, returns true if value was updated +template +bool AtomicMin(atomic &ioAtomic, const T inValue, const memory_order inMemoryOrder = memory_order_seq_cst) +{ + T cur_value = ioAtomic.load(memory_order_relaxed); + while (cur_value > inValue) + if (ioAtomic.compare_exchange_weak(cur_value, inValue, inMemoryOrder)) + return true; + return false; +} + +/// Atomically compute the max(ioAtomic, inValue) and store it in ioAtomic, returns true if value was updated +template +bool AtomicMax(atomic &ioAtomic, const T inValue, const memory_order inMemoryOrder = memory_order_seq_cst) +{ + T cur_value = ioAtomic.load(memory_order_relaxed); + while (cur_value < inValue) + if (ioAtomic.compare_exchange_weak(cur_value, inValue, inMemoryOrder)) + return true; + return false; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/ByteBuffer.h b/WickedEngine/Jolt/Core/ByteBuffer.h new file mode 100644 index 000000000..48d19703f --- /dev/null +++ b/WickedEngine/Jolt/Core/ByteBuffer.h @@ -0,0 +1,74 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Underlying data type for ByteBuffer +using ByteBufferVector = Array>; + +/// Simple byte buffer, aligned to a cache line +class ByteBuffer : public ByteBufferVector +{ +public: + /// Align the size to a multiple of inSize, returns the length after alignment + size_t Align(size_t inSize) + { + // Assert power of 2 + JPH_ASSERT(IsPowerOf2(inSize)); + + // Calculate new size and resize buffer + size_t s = AlignUp(size(), inSize); + resize(s, 0); + + return s; + } + + /// Allocate block of data of inSize elements and return the pointer + template + Type * Allocate(size_t inSize = 1) + { + // Reserve space + size_t s = size(); + resize(s + inSize * sizeof(Type)); + + // Get data pointer + Type *data = reinterpret_cast(&at(s)); + + // Construct elements + for (Type *d = data, *d_end = data + inSize; d < d_end; ++d) + ::new (d) Type; + + // Return pointer + return data; + } + + /// Append inData to the buffer + template + void AppendVector(const Array &inData) + { + size_t size = inData.size() * sizeof(Type); + uint8 *data = Allocate(size); + memcpy(data, &inData[0], size); + } + + /// Get object at inPosition (an offset in bytes) + template + const Type * Get(size_t inPosition) const + { + return reinterpret_cast(&at(inPosition)); + } + + /// Get object at inPosition (an offset in bytes) + template + Type * Get(size_t inPosition) + { + return reinterpret_cast(&at(inPosition)); + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Color.cpp b/WickedEngine/Jolt/Core/Color.cpp new file mode 100644 index 000000000..93d3cabc7 --- /dev/null +++ b/WickedEngine/Jolt/Core/Color.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +// Predefined colors +const Color Color::sBlack(0, 0, 0); +const Color Color::sDarkRed(128, 0, 0); +const Color Color::sRed(255, 0, 0); +const Color Color::sDarkGreen(0, 128, 0); +const Color Color::sGreen(0, 255, 0); +const Color Color::sDarkBlue(0, 0, 128); +const Color Color::sBlue(0, 0, 255); +const Color Color::sYellow(255, 255, 0); +const Color Color::sPurple(255, 0, 255); +const Color Color::sCyan(0, 255, 255); +const Color Color::sOrange(255, 128, 0); +const Color Color::sDarkOrange(128, 64, 0); +const Color Color::sGrey(128, 128, 128); +const Color Color::sLightGrey(192, 192, 192); +const Color Color::sWhite(255, 255, 255); + +// Generated by: http://phrogz.net/css/distinct-colors.html (this algo: https://en.wikipedia.org/wiki/Color_difference#CMC_l:c_.281984.29) +static constexpr Color sColors[] = { Color(255, 0, 0), Color(204, 143, 102), Color(226, 242, 0), Color(41, 166, 124), Color(0, 170, 255), Color(69, 38, 153), Color(153, 38, 130), Color(229, 57, 80), Color(204, 0, 0), Color(255, 170, 0), Color(85, 128, 0), Color(64, 255, 217), Color(0, 75, 140), Color(161, 115, 230), Color(242, 61, 157), Color(178, 101, 89), Color(140, 94, 0), Color(181, 217, 108), Color(64, 242, 255), Color(77, 117, 153), Color(157, 61, 242), Color(140, 0, 56), Color(127, 57, 32), Color(204, 173, 51), Color(64, 255, 64), Color(38, 145, 153), Color(0, 102, 255), Color(242, 0, 226), Color(153, 77, 107), Color(229, 92, 0), Color(140, 126, 70), Color(0, 179, 71), Color(0, 194, 242), Color(27, 0, 204), Color(230, 115, 222), Color(127, 0, 17) }; + +Color Color::sGetDistinctColor(int inIndex) +{ + JPH_ASSERT(inIndex >= 0); + + return sColors[inIndex % (sizeof(sColors) / sizeof(uint32))]; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Color.h b/WickedEngine/Jolt/Core/Color.h new file mode 100644 index 000000000..a2e906bca --- /dev/null +++ b/WickedEngine/Jolt/Core/Color.h @@ -0,0 +1,84 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Color; + +/// Type to use for passing arguments to a function +using ColorArg = Color; + +/// Class that holds an RGBA color with 8-bits per component +class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND Color +{ +public: + /// Constructors + Color() = default; ///< Intentionally not initialized for performance reasons + Color(const Color &inRHS) = default; + Color & operator = (const Color &inRHS) = default; + explicit constexpr Color(uint32 inColor) : mU32(inColor) { } + constexpr Color(uint8 inRed, uint8 inGreen, uint8 inBlue, uint8 inAlpha = 255) : r(inRed), g(inGreen), b(inBlue), a(inAlpha) { } + constexpr Color(ColorArg inRHS, uint8 inAlpha) : r(inRHS.r), g(inRHS.g), b(inRHS.b), a(inAlpha) { } + + /// Comparison + inline bool operator == (ColorArg inRHS) const { return mU32 == inRHS.mU32; } + inline bool operator != (ColorArg inRHS) const { return mU32 != inRHS.mU32; } + + /// Convert to uint32 + uint32 GetUInt32() const { return mU32; } + + /// Element access, 0 = red, 1 = green, 2 = blue, 3 = alpha + inline uint8 operator () (uint inIdx) const { JPH_ASSERT(inIdx < 4); return (&r)[inIdx]; } + inline uint8 & operator () (uint inIdx) { JPH_ASSERT(inIdx < 4); return (&r)[inIdx]; } + + /// Multiply two colors + inline Color operator * (const Color &inRHS) const { return Color(uint8((uint32(r) * inRHS.r) >> 8), uint8((uint32(g) * inRHS.g) >> 8), uint8((uint32(b) * inRHS.b) >> 8), uint8((uint32(a) * inRHS.a) >> 8)); } + + /// Multiply color with intensity in the range [0, 1] + inline Color operator * (float inIntensity) const { return Color(uint8(r * inIntensity), uint8(g * inIntensity), uint8(b * inIntensity), a); } + + /// Convert to Vec4 with range [0, 1] + inline Vec4 ToVec4() const { return Vec4(r, g, b, a) / 255.0f; } + + /// Get grayscale intensity of color + inline uint8 GetIntensity() const { return uint8((uint32(r) * 54 + g * 183 + b * 19) >> 8); } + + /// Get a visually distinct color + static Color sGetDistinctColor(int inIndex); + + /// Predefined colors + static const Color sBlack; + static const Color sDarkRed; + static const Color sRed; + static const Color sDarkGreen; + static const Color sGreen; + static const Color sDarkBlue; + static const Color sBlue; + static const Color sYellow; + static const Color sPurple; + static const Color sCyan; + static const Color sOrange; + static const Color sDarkOrange; + static const Color sGrey; + static const Color sLightGrey; + static const Color sWhite; + + union + { + uint32 mU32; ///< Combined value for red, green, blue and alpha + struct + { + uint8 r; ///< Red channel + uint8 g; ///< Green channel + uint8 b; ///< Blue channel + uint8 a; ///< Alpha channel + }; + }; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Core.h b/WickedEngine/Jolt/Core/Core.h new file mode 100644 index 000000000..f4ce872ab --- /dev/null +++ b/WickedEngine/Jolt/Core/Core.h @@ -0,0 +1,577 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Jolt library version +#define JPH_VERSION_MAJOR 5 +#define JPH_VERSION_MINOR 0 +#define JPH_VERSION_PATCH 1 + +// Determine which features the library was compiled with +#ifdef JPH_DOUBLE_PRECISION + #define JPH_VERSION_FEATURE_BIT_1 1 +#else + #define JPH_VERSION_FEATURE_BIT_1 0 +#endif +#ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + #define JPH_VERSION_FEATURE_BIT_2 1 +#else + #define JPH_VERSION_FEATURE_BIT_2 0 +#endif +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #define JPH_VERSION_FEATURE_BIT_3 1 +#else + #define JPH_VERSION_FEATURE_BIT_3 0 +#endif +#ifdef JPH_PROFILE_ENABLED + #define JPH_VERSION_FEATURE_BIT_4 1 +#else + #define JPH_VERSION_FEATURE_BIT_4 0 +#endif +#ifdef JPH_EXTERNAL_PROFILE + #define JPH_VERSION_FEATURE_BIT_5 1 +#else + #define JPH_VERSION_FEATURE_BIT_5 0 +#endif +#ifdef JPH_DEBUG_RENDERER + #define JPH_VERSION_FEATURE_BIT_6 1 +#else + #define JPH_VERSION_FEATURE_BIT_6 0 +#endif +#ifdef JPH_DISABLE_TEMP_ALLOCATOR + #define JPH_VERSION_FEATURE_BIT_7 1 +#else + #define JPH_VERSION_FEATURE_BIT_7 0 +#endif +#ifdef JPH_DISABLE_CUSTOM_ALLOCATOR + #define JPH_VERSION_FEATURE_BIT_8 1 +#else + #define JPH_VERSION_FEATURE_BIT_8 0 +#endif +#if defined(JPH_OBJECT_LAYER_BITS) && JPH_OBJECT_LAYER_BITS == 32 + #define JPH_VERSION_FEATURE_BIT_9 1 +#else + #define JPH_VERSION_FEATURE_BIT_9 0 +#endif +#ifdef JPH_ENABLE_ASSERTS + #define JPH_VERSION_FEATURE_BIT_10 1 +#else + #define JPH_VERSION_FEATURE_BIT_10 0 +#endif +#ifdef JPH_OBJECT_STREAM + #define JPH_VERSION_FEATURE_BIT_11 1 +#else + #define JPH_VERSION_FEATURE_BIT_11 0 +#endif +#define JPH_VERSION_FEATURES (uint64(JPH_VERSION_FEATURE_BIT_1) | (JPH_VERSION_FEATURE_BIT_2 << 1) | (JPH_VERSION_FEATURE_BIT_3 << 2) | (JPH_VERSION_FEATURE_BIT_4 << 3) | (JPH_VERSION_FEATURE_BIT_5 << 4) | (JPH_VERSION_FEATURE_BIT_6 << 5) | (JPH_VERSION_FEATURE_BIT_7 << 6) | (JPH_VERSION_FEATURE_BIT_8 << 7) | (JPH_VERSION_FEATURE_BIT_9 << 8) | (JPH_VERSION_FEATURE_BIT_10 << 9) | (JPH_VERSION_FEATURE_BIT_11 << 10)) + +// Combine the version and features in a single ID +#define JPH_VERSION_ID ((JPH_VERSION_FEATURES << 24) | (JPH_VERSION_MAJOR << 16) | (JPH_VERSION_MINOR << 8) | JPH_VERSION_PATCH) + +// Determine platform +#if defined(JPH_PLATFORM_BLUE) + // Correct define already defined, this overrides everything else +#elif defined(_WIN32) || defined(_WIN64) + #include + #if WINAPI_FAMILY == WINAPI_FAMILY_APP + #define JPH_PLATFORM_WINDOWS_UWP // Building for Universal Windows Platform + #endif + #define JPH_PLATFORM_WINDOWS +#elif defined(__ANDROID__) // Android is linux too, so that's why we check it first + #define JPH_PLATFORM_ANDROID +#elif defined(__linux__) + #define JPH_PLATFORM_LINUX +#elif defined(__FreeBSD__) + #define JPH_PLATFORM_FREEBSD +#elif defined(__APPLE__) + #include + #if defined(TARGET_OS_IPHONE) && !TARGET_OS_IPHONE + #define JPH_PLATFORM_MACOS + #else + #define JPH_PLATFORM_IOS + #endif +#elif defined(__EMSCRIPTEN__) + #define JPH_PLATFORM_WASM +#endif + +// Platform helper macros +#ifdef JPH_PLATFORM_ANDROID + #define JPH_IF_NOT_ANDROID(x) +#else + #define JPH_IF_NOT_ANDROID(x) x +#endif + +// Determine compiler +#if defined(__clang__) + #define JPH_COMPILER_CLANG +#elif defined(__GNUC__) + #define JPH_COMPILER_GCC +#elif defined(_MSC_VER) + #define JPH_COMPILER_MSVC +#endif + +#if defined(__MINGW64__) || defined (__MINGW32__) + #define JPH_COMPILER_MINGW +#endif + +// Detect CPU architecture +#if defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(_M_IX86) + // X86 CPU architecture + #define JPH_CPU_X86 + #if defined(__x86_64__) || defined(_M_X64) + #define JPH_CPU_ADDRESS_BITS 64 + #else + #define JPH_CPU_ADDRESS_BITS 32 + #endif + #define JPH_USE_SSE + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + + // Detect enabled instruction sets + #if defined(__AVX512F__) && defined(__AVX512VL__) && defined(__AVX512DQ__) && !defined(JPH_USE_AVX512) + #define JPH_USE_AVX512 + #endif + #if (defined(__AVX2__) || defined(JPH_USE_AVX512)) && !defined(JPH_USE_AVX2) + #define JPH_USE_AVX2 + #endif + #if (defined(__AVX__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_AVX) + #define JPH_USE_AVX + #endif + #if (defined(__SSE4_2__) || defined(JPH_USE_AVX)) && !defined(JPH_USE_SSE4_2) + #define JPH_USE_SSE4_2 + #endif + #if (defined(__SSE4_1__) || defined(JPH_USE_SSE4_2)) && !defined(JPH_USE_SSE4_1) + #define JPH_USE_SSE4_1 + #endif + #if (defined(__F16C__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_F16C) + #define JPH_USE_F16C + #endif + #if (defined(__LZCNT__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_LZCNT) + #define JPH_USE_LZCNT + #endif + #if (defined(__BMI__) || defined(JPH_USE_AVX2)) && !defined(JPH_USE_TZCNT) + #define JPH_USE_TZCNT + #endif + #ifndef JPH_CROSS_PLATFORM_DETERMINISTIC // FMA is not compatible with cross platform determinism + #if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC) + #if defined(__FMA__) && !defined(JPH_USE_FMADD) + #define JPH_USE_FMADD + #endif + #elif defined(JPH_COMPILER_MSVC) + #if defined(__AVX2__) && !defined(JPH_USE_FMADD) // AVX2 also enables fused multiply add + #define JPH_USE_FMADD + #endif + #else + #error Undefined compiler + #endif + #endif +#elif defined(__aarch64__) || defined(_M_ARM64) || defined(__arm__) || defined(_M_ARM) + // ARM CPU architecture + #define JPH_CPU_ARM + #if defined(__aarch64__) || defined(_M_ARM64) + #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_USE_NEON + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + #else + #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_VECTOR_ALIGNMENT 8 // 32-bit ARM does not support aligning on the stack on 16 byte boundaries + #define JPH_DVECTOR_ALIGNMENT 8 + #endif +#elif defined(JPH_PLATFORM_WASM) + // WebAssembly CPU architecture + #define JPH_CPU_WASM + #define JPH_CPU_ADDRESS_BITS 32 + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + #ifdef __wasm_simd128__ + #define JPH_USE_SSE + #define JPH_USE_SSE4_1 + #define JPH_USE_SSE4_2 + #endif +#elif defined(__e2k__) + // E2K CPU architecture (MCST Elbrus 2000) + #define JPH_CPU_E2K + #define JPH_CPU_ADDRESS_BITS 64 + #define JPH_VECTOR_ALIGNMENT 16 + #define JPH_DVECTOR_ALIGNMENT 32 + + // Compiler flags on e2k arch determine CPU features + #if defined(__SSE__) && !defined(JPH_USE_SSE) + #define JPH_USE_SSE + #endif +#else + #error Unsupported CPU architecture +#endif + +// If this define is set, Jolt is compiled as a shared library +#ifdef JPH_SHARED_LIBRARY + #ifdef JPH_BUILD_SHARED_LIBRARY + // While building the shared library, we must export these symbols + #ifdef JPH_PLATFORM_WINDOWS + #define JPH_EXPORT __declspec(dllexport) + #else + #define JPH_EXPORT __attribute__ ((visibility ("default"))) + #if defined(JPH_COMPILER_GCC) + // Prevents an issue with GCC attribute parsing (see https://gcc.gnu.org/bugzilla/show_bug.cgi?id=69585) + #define JPH_EXPORT_GCC_BUG_WORKAROUND [[gnu::visibility("default")]] + #endif + #endif + #else + // When linking against Jolt, we must import these symbols + #ifdef JPH_PLATFORM_WINDOWS + #define JPH_EXPORT __declspec(dllimport) + #else + #define JPH_EXPORT __attribute__ ((visibility ("default"))) + #if defined(JPH_COMPILER_GCC) + // Prevents an issue with GCC attribute parsing (see https://gcc.gnu.org/bugzilla/show_bug.cgi?id=69585) + #define JPH_EXPORT_GCC_BUG_WORKAROUND [[gnu::visibility("default")]] + #endif + #endif + #endif +#else + // If the define is not set, we use static linking and symbols don't need to be imported or exported + #define JPH_EXPORT +#endif + +#ifndef JPH_EXPORT_GCC_BUG_WORKAROUND + #define JPH_EXPORT_GCC_BUG_WORKAROUND JPH_EXPORT +#endif + +// Macro used by the RTTI macros to not export a function +#define JPH_NO_EXPORT + +// Pragmas to store / restore the warning state and to disable individual warnings +#ifdef JPH_COMPILER_CLANG +#define JPH_PRAGMA(x) _Pragma(#x) +#define JPH_SUPPRESS_WARNING_PUSH JPH_PRAGMA(clang diagnostic push) +#define JPH_SUPPRESS_WARNING_POP JPH_PRAGMA(clang diagnostic pop) +#define JPH_CLANG_SUPPRESS_WARNING(w) JPH_PRAGMA(clang diagnostic ignored w) +#if __clang_major__ >= 13 + #define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w) JPH_CLANG_SUPPRESS_WARNING(w) +#else + #define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w) +#endif +#if __clang_major__ >= 16 + #define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w) JPH_CLANG_SUPPRESS_WARNING(w) +#else + #define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w) +#endif +#else +#define JPH_CLANG_SUPPRESS_WARNING(w) +#define JPH_CLANG_13_PLUS_SUPPRESS_WARNING(w) +#define JPH_CLANG_16_PLUS_SUPPRESS_WARNING(w) +#endif +#ifdef JPH_COMPILER_GCC +#define JPH_PRAGMA(x) _Pragma(#x) +#define JPH_SUPPRESS_WARNING_PUSH JPH_PRAGMA(GCC diagnostic push) +#define JPH_SUPPRESS_WARNING_POP JPH_PRAGMA(GCC diagnostic pop) +#define JPH_GCC_SUPPRESS_WARNING(w) JPH_PRAGMA(GCC diagnostic ignored w) +#else +#define JPH_GCC_SUPPRESS_WARNING(w) +#endif +#ifdef JPH_COMPILER_MSVC +#define JPH_PRAGMA(x) __pragma(x) +#define JPH_SUPPRESS_WARNING_PUSH JPH_PRAGMA(warning (push)) +#define JPH_SUPPRESS_WARNING_POP JPH_PRAGMA(warning (pop)) +#define JPH_MSVC_SUPPRESS_WARNING(w) JPH_PRAGMA(warning (disable : w)) +#if _MSC_VER >= 1920 && _MSC_VER < 1930 + #define JPH_MSVC2019_SUPPRESS_WARNING(w) JPH_MSVC_SUPPRESS_WARNING(w) +#else + #define JPH_MSVC2019_SUPPRESS_WARNING(w) +#endif +#else +#define JPH_MSVC_SUPPRESS_WARNING(w) +#define JPH_MSVC2019_SUPPRESS_WARNING(w) +#endif + +// Disable common warnings triggered by Jolt when compiling with -Wall +#define JPH_SUPPRESS_WARNINGS \ + JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") \ + JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") \ + JPH_CLANG_SUPPRESS_WARNING("-Wfloat-equal") \ + JPH_CLANG_SUPPRESS_WARNING("-Wsign-conversion") \ + JPH_CLANG_SUPPRESS_WARNING("-Wold-style-cast") \ + JPH_CLANG_SUPPRESS_WARNING("-Wgnu-anonymous-struct") \ + JPH_CLANG_SUPPRESS_WARNING("-Wnested-anon-types") \ + JPH_CLANG_SUPPRESS_WARNING("-Wglobal-constructors") \ + JPH_CLANG_SUPPRESS_WARNING("-Wexit-time-destructors") \ + JPH_CLANG_SUPPRESS_WARNING("-Wnonportable-system-include-path") \ + JPH_CLANG_SUPPRESS_WARNING("-Wlanguage-extension-token") \ + JPH_CLANG_SUPPRESS_WARNING("-Wunused-parameter") \ + JPH_CLANG_SUPPRESS_WARNING("-Wformat-nonliteral") \ + JPH_CLANG_SUPPRESS_WARNING("-Wcovered-switch-default") \ + JPH_CLANG_SUPPRESS_WARNING("-Wcast-align") \ + JPH_CLANG_SUPPRESS_WARNING("-Winvalid-offsetof") \ + JPH_CLANG_SUPPRESS_WARNING("-Wgnu-zero-variadic-macro-arguments") \ + JPH_CLANG_SUPPRESS_WARNING("-Wdocumentation-unknown-command") \ + JPH_CLANG_SUPPRESS_WARNING("-Wctad-maybe-unsupported") \ + JPH_CLANG_13_PLUS_SUPPRESS_WARNING("-Wdeprecated-copy") \ + JPH_CLANG_13_PLUS_SUPPRESS_WARNING("-Wdeprecated-copy-with-dtor") \ + JPH_CLANG_16_PLUS_SUPPRESS_WARNING("-Wunsafe-buffer-usage") \ + JPH_IF_NOT_ANDROID(JPH_CLANG_SUPPRESS_WARNING("-Wimplicit-int-float-conversion")) \ + \ + JPH_GCC_SUPPRESS_WARNING("-Wcomment") \ + JPH_GCC_SUPPRESS_WARNING("-Winvalid-offsetof") \ + JPH_GCC_SUPPRESS_WARNING("-Wclass-memaccess") \ + JPH_GCC_SUPPRESS_WARNING("-Wpedantic") \ + JPH_GCC_SUPPRESS_WARNING("-Wunused-parameter") \ + \ + JPH_MSVC_SUPPRESS_WARNING(4619) /* #pragma warning: there is no warning number 'XXXX' */ \ + JPH_MSVC_SUPPRESS_WARNING(4514) /* 'X' : unreferenced inline function has been removed */ \ + JPH_MSVC_SUPPRESS_WARNING(4710) /* 'X' : function not inlined */ \ + JPH_MSVC_SUPPRESS_WARNING(4711) /* function 'X' selected for automatic inline expansion */ \ + JPH_MSVC_SUPPRESS_WARNING(4820) /* 'X': 'Y' bytes padding added after data member 'Z' */ \ + JPH_MSVC_SUPPRESS_WARNING(4100) /* 'X' : unreferenced formal parameter */ \ + JPH_MSVC_SUPPRESS_WARNING(4626) /* 'X' : assignment operator was implicitly defined as deleted because a base class assignment operator is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(5027) /* 'X' : move assignment operator was implicitly defined as deleted because a base class move assignment operator is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(4365) /* 'argument' : conversion from 'X' to 'Y', signed / unsigned mismatch */ \ + JPH_MSVC_SUPPRESS_WARNING(4324) /* 'X' : structure was padded due to alignment specifier */ \ + JPH_MSVC_SUPPRESS_WARNING(4625) /* 'X' : copy constructor was implicitly defined as deleted because a base class copy constructor is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(5026) /* 'X': move constructor was implicitly defined as deleted because a base class move constructor is inaccessible or deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(4623) /* 'X' : default constructor was implicitly defined as deleted */ \ + JPH_MSVC_SUPPRESS_WARNING(4201) /* nonstandard extension used: nameless struct/union */ \ + JPH_MSVC_SUPPRESS_WARNING(4371) /* 'X': layout of class may have changed from a previous version of the compiler due to better packing of member 'Y' */ \ + JPH_MSVC_SUPPRESS_WARNING(5045) /* Compiler will insert Spectre mitigation for memory load if /Qspectre switch specified */ \ + JPH_MSVC_SUPPRESS_WARNING(4583) /* 'X': destructor is not implicitly called */ \ + JPH_MSVC_SUPPRESS_WARNING(4582) /* 'X': constructor is not implicitly called */ \ + JPH_MSVC_SUPPRESS_WARNING(5219) /* implicit conversion from 'X' to 'Y', possible loss of data */ \ + JPH_MSVC_SUPPRESS_WARNING(4826) /* Conversion from 'X *' to 'JPH::uint64' is sign-extended. This may cause unexpected runtime behavior. (32-bit) */ \ + JPH_MSVC_SUPPRESS_WARNING(5264) /* 'X': 'const' variable is not used */ \ + JPH_MSVC_SUPPRESS_WARNING(4251) /* class 'X' needs to have DLL-interface to be used by clients of class 'Y' */ \ + JPH_MSVC_SUPPRESS_WARNING(4738) /* storing 32-bit float result in memory, possible loss of performance */ \ + JPH_MSVC2019_SUPPRESS_WARNING(5246) /* the initialization of a subobject should be wrapped in braces */ + +// OS-specific includes +#if defined(JPH_PLATFORM_WINDOWS) + #define JPH_BREAKPOINT __debugbreak() +#elif defined(JPH_PLATFORM_BLUE) + // Configuration for a popular game console. + // This file is not distributed because it would violate an NDA. + // Creating one should only be a couple of minutes of work if you have the documentation for the platform + // (you only need to define JPH_BREAKPOINT, JPH_PLATFORM_BLUE_GET_TICKS, JPH_PLATFORM_BLUE_MUTEX*, JPH_PLATFORM_BLUE_RWLOCK* and include the right header). + #include +#elif defined(JPH_PLATFORM_LINUX) || defined(JPH_PLATFORM_ANDROID) || defined(JPH_PLATFORM_MACOS) || defined(JPH_PLATFORM_IOS) || defined(JPH_PLATFORM_FREEBSD) + #if defined(JPH_CPU_X86) + #define JPH_BREAKPOINT __asm volatile ("int $0x3") + #elif defined(JPH_CPU_ARM) + #define JPH_BREAKPOINT __builtin_trap() + #elif defined(JPH_CPU_E2K) + #define JPH_BREAKPOINT __builtin_trap() + #endif +#elif defined(JPH_PLATFORM_WASM) + #define JPH_BREAKPOINT do { } while (false) // Not supported +#else + #error Unknown platform +#endif + +// Begin the JPH namespace +#define JPH_NAMESPACE_BEGIN \ + JPH_SUPPRESS_WARNING_PUSH \ + JPH_SUPPRESS_WARNINGS \ + namespace JPH { + +// End the JPH namespace +#define JPH_NAMESPACE_END \ + } \ + JPH_SUPPRESS_WARNING_POP + +// Suppress warnings generated by the standard template library +#define JPH_SUPPRESS_WARNINGS_STD_BEGIN \ + JPH_SUPPRESS_WARNING_PUSH \ + JPH_MSVC_SUPPRESS_WARNING(4365) \ + JPH_MSVC_SUPPRESS_WARNING(4619) \ + JPH_MSVC_SUPPRESS_WARNING(4710) \ + JPH_MSVC_SUPPRESS_WARNING(4711) \ + JPH_MSVC_SUPPRESS_WARNING(4820) \ + JPH_MSVC_SUPPRESS_WARNING(4514) \ + JPH_MSVC_SUPPRESS_WARNING(5262) \ + JPH_MSVC_SUPPRESS_WARNING(5264) \ + JPH_MSVC_SUPPRESS_WARNING(4738) + +#define JPH_SUPPRESS_WARNINGS_STD_END \ + JPH_SUPPRESS_WARNING_POP + +// Standard C++ includes +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(JPH_USE_SSE) + #include +#elif defined(JPH_USE_NEON) + #ifdef JPH_COMPILER_MSVC + #include + #include + #else + #include + #endif +#endif +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Commonly used STL types +using std::pair; +using std::min; +using std::max; +using std::abs; +using std::sqrt; +using std::ceil; +using std::floor; +using std::trunc; +using std::round; +using std::fmod; +using std::swap; +using std::size; +using std::string; +using std::string_view; +using std::function; +using std::numeric_limits; +using std::isfinite; +using std::isnan; +using std::is_trivial; +using std::is_trivially_constructible; +using std::is_trivially_destructible; +using std::ostream; +using std::istream; + +// Standard types +using uint = unsigned int; +using uint8 = std::uint8_t; +using uint16 = std::uint16_t; +using uint32 = std::uint32_t; +using uint64 = std::uint64_t; + +// Assert sizes of types +static_assert(sizeof(uint) >= 4, "Invalid size of uint"); +static_assert(sizeof(uint8) == 1, "Invalid size of uint8"); +static_assert(sizeof(uint16) == 2, "Invalid size of uint16"); +static_assert(sizeof(uint32) == 4, "Invalid size of uint32"); +static_assert(sizeof(uint64) == 8, "Invalid size of uint64"); +static_assert(sizeof(void *) == (JPH_CPU_ADDRESS_BITS == 64? 8 : 4), "Invalid size of pointer" ); + +// Determine if we want extra debugging code to be active +#if !defined(NDEBUG) && !defined(JPH_NO_DEBUG) + #define JPH_DEBUG +#endif + +// Define inline macro +#if defined(JPH_NO_FORCE_INLINE) + #define JPH_INLINE inline +#elif defined(JPH_COMPILER_CLANG) + #define JPH_INLINE __inline__ __attribute__((always_inline)) +#elif defined(JPH_COMPILER_GCC) + // On gcc 14 using always_inline in debug mode causes error: "inlining failed in call to 'always_inline' 'XXX': function not considered for inlining" + // See: https://github.com/jrouwe/JoltPhysics/issues/1096 + #if __GNUC__ >= 14 && defined(JPH_DEBUG) + #define JPH_INLINE inline + #else + #define JPH_INLINE __inline__ __attribute__((always_inline)) + #endif +#elif defined(JPH_COMPILER_MSVC) + #define JPH_INLINE __forceinline +#else + #error Undefined +#endif + +// Cache line size (used for aligning to cache line) +#ifndef JPH_CACHE_LINE_SIZE + #define JPH_CACHE_LINE_SIZE 64 +#endif + +// Define macro to get current function name +#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC) + #define JPH_FUNCTION_NAME __PRETTY_FUNCTION__ +#elif defined(JPH_COMPILER_MSVC) + #define JPH_FUNCTION_NAME __FUNCTION__ +#else + #error Undefined +#endif + +// Stack allocation +#define JPH_STACK_ALLOC(n) alloca(n) + +// Shorthand for #ifdef JPH_DEBUG / #endif +#ifdef JPH_DEBUG + #define JPH_IF_DEBUG(...) __VA_ARGS__ + #define JPH_IF_NOT_DEBUG(...) +#else + #define JPH_IF_DEBUG(...) + #define JPH_IF_NOT_DEBUG(...) __VA_ARGS__ +#endif + +// Shorthand for #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED / #endif +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #define JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(...) __VA_ARGS__ +#else + #define JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(...) +#endif + +// Helper macros to detect if we're running in single or double precision mode +#ifdef JPH_DOUBLE_PRECISION + #define JPH_IF_SINGLE_PRECISION(...) + #define JPH_IF_SINGLE_PRECISION_ELSE(s, d) d + #define JPH_IF_DOUBLE_PRECISION(...) __VA_ARGS__ +#else + #define JPH_IF_SINGLE_PRECISION(...) __VA_ARGS__ + #define JPH_IF_SINGLE_PRECISION_ELSE(s, d) s + #define JPH_IF_DOUBLE_PRECISION(...) +#endif + +// Helper macro to detect if the debug renderer is active +#ifdef JPH_DEBUG_RENDERER + #define JPH_IF_DEBUG_RENDERER(...) __VA_ARGS__ + #define JPH_IF_NOT_DEBUG_RENDERER(...) +#else + #define JPH_IF_DEBUG_RENDERER(...) + #define JPH_IF_NOT_DEBUG_RENDERER(...) __VA_ARGS__ +#endif + +// Macro to indicate that a parameter / variable is unused +#define JPH_UNUSED(x) (void)x + +// Macro to enable floating point precise mode and to disable fused multiply add instructions +#if defined(JPH_COMPILER_GCC) || defined(JPH_CROSS_PLATFORM_DETERMINISTIC) + // We compile without -ffast-math and -ffp-contract=fast, so we don't need to disable anything + #define JPH_PRECISE_MATH_ON + #define JPH_PRECISE_MATH_OFF +#elif defined(JPH_COMPILER_CLANG) + // We compile without -ffast-math because pragma float_control(precise, on) doesn't seem to actually negate all of the -ffast-math effects and causes the unit tests to fail (even if the pragma is added to all files) + // On clang 14 and later we can turn off float contraction through a pragma (before it was buggy), so if FMA is on we can disable it through this macro + #if (defined(JPH_CPU_ARM) && !defined(JPH_PLATFORM_ANDROID) && __clang_major__ >= 16) || (defined(JPH_CPU_X86) && __clang_major__ >= 14) + #define JPH_PRECISE_MATH_ON \ + _Pragma("float_control(precise, on, push)") \ + _Pragma("clang fp contract(off)") + #define JPH_PRECISE_MATH_OFF \ + _Pragma("float_control(pop)") + #elif __clang_major__ >= 14 && (defined(JPH_USE_FMADD) || defined(FP_FAST_FMA)) + #define JPH_PRECISE_MATH_ON \ + _Pragma("clang fp contract(off)") + #define JPH_PRECISE_MATH_OFF \ + _Pragma("clang fp contract(on)") + #else + #define JPH_PRECISE_MATH_ON + #define JPH_PRECISE_MATH_OFF + #endif +#elif defined(JPH_COMPILER_MSVC) + // Unfortunately there is no way to push the state of fp_contract, so we have to assume it was turned on before JPH_PRECISE_MATH_ON + #define JPH_PRECISE_MATH_ON \ + __pragma(float_control(precise, on, push)) \ + __pragma(fp_contract(off)) + #define JPH_PRECISE_MATH_OFF \ + __pragma(fp_contract(on)) \ + __pragma(float_control(pop)) +#else + #error Undefined +#endif + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/FPControlWord.h b/WickedEngine/Jolt/Core/FPControlWord.h new file mode 100644 index 000000000..a046bffe9 --- /dev/null +++ b/WickedEngine/Jolt/Core/FPControlWord.h @@ -0,0 +1,135 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_CPU_WASM) + +// Not supported + +#elif defined(JPH_USE_SSE) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + mPrevState = _mm_getcsr(); + _mm_setcsr((mPrevState & ~Mask) | Value); + } + + ~FPControlWord() + { + _mm_setcsr((_mm_getcsr() & ~Mask) | (mPrevState & Mask)); + } + +private: + uint mPrevState; +}; + +#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + // Read state before change + _controlfp_s(&mPrevState, 0, 0); + + // Update the state + unsigned int dummy; + _controlfp_s(&dummy, Value, Mask); + } + + ~FPControlWord() + { + // Restore state + unsigned int dummy; + _controlfp_s(&dummy, mPrevState, Mask); + } + +private: + unsigned int mPrevState; +}; + +#elif defined(JPH_CPU_ARM) && defined(JPH_USE_NEON) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + uint64 val; + asm volatile("mrs %0, fpcr" : "=r" (val)); + mPrevState = val; + val &= ~Mask; + val |= Value; + asm volatile("msr fpcr, %0" : /* no output */ : "r" (val)); + } + + ~FPControlWord() + { + uint64 val; + asm volatile("mrs %0, fpcr" : "=r" (val)); + val &= ~Mask; + val |= mPrevState & Mask; + asm volatile("msr fpcr, %0" : /* no output */ : "r" (val)); + } + +private: + uint64 mPrevState; +}; + +#elif defined(JPH_CPU_ARM) + +/// Helper class that needs to be put on the stack to update the state of the floating point control word. +/// This state is kept per thread. +template +class FPControlWord : public NonCopyable +{ +public: + FPControlWord() + { + uint32 val; + asm volatile("vmrs %0, fpscr" : "=r" (val)); + mPrevState = val; + val &= ~Mask; + val |= Value; + asm volatile("vmsr fpscr, %0" : /* no output */ : "r" (val)); + } + + ~FPControlWord() + { + uint32 val; + asm volatile("vmrs %0, fpscr" : "=r" (val)); + val &= ~Mask; + val |= mPrevState & Mask; + asm volatile("vmsr fpscr, %0" : /* no output */ : "r" (val)); + } + +private: + uint32 mPrevState; +}; + +#else + +#error Unsupported CPU architecture + +#endif + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/FPException.h b/WickedEngine/Jolt/Core/FPException.h new file mode 100644 index 000000000..3083f05c0 --- /dev/null +++ b/WickedEngine/Jolt/Core/FPException.h @@ -0,0 +1,74 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + +#if defined(JPH_CPU_WASM) + +// Not supported +class FPExceptionsEnable { }; +class FPExceptionDisableInvalid { }; +class FPExceptionDisableDivByZero { }; + +#elif defined(JPH_USE_SSE) + +/// Enable floating point divide by zero exception and exceptions on invalid numbers +class FPExceptionsEnable : public FPControlWord<0, _MM_MASK_DIV_ZERO | _MM_MASK_INVALID> { }; + +/// Disable invalid floating point value exceptions +class FPExceptionDisableInvalid : public FPControlWord<_MM_MASK_INVALID, _MM_MASK_INVALID> { }; + +/// Disable division by zero floating point exceptions +class FPExceptionDisableDivByZero : public FPControlWord<_MM_MASK_DIV_ZERO, _MM_MASK_DIV_ZERO> { }; + +#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC) + +/// Enable floating point divide by zero exception and exceptions on invalid numbers +class FPExceptionsEnable : public FPControlWord<0, _EM_INVALID | _EM_ZERODIVIDE> { }; + +/// Disable invalid floating point value exceptions +class FPExceptionDisableInvalid : public FPControlWord<_EM_INVALID, _EM_INVALID> { }; + +/// Disable division by zero floating point exceptions +class FPExceptionDisableDivByZero : public FPControlWord<_EM_ZERODIVIDE, _EM_ZERODIVIDE> { }; + +#elif defined(JPH_CPU_ARM) + +/// Invalid operation exception bit +static constexpr uint64 FP_IOE = 1 << 8; + +/// Enable divide by zero exception bit +static constexpr uint64 FP_DZE = 1 << 9; + +/// Enable floating point divide by zero exception and exceptions on invalid numbers +class FPExceptionsEnable : public FPControlWord { }; + +/// Disable invalid floating point value exceptions +class FPExceptionDisableInvalid : public FPControlWord<0, FP_IOE> { }; + +/// Disable division by zero floating point exceptions +class FPExceptionDisableDivByZero : public FPControlWord<0, FP_DZE> { }; + +#else + +#error Unsupported CPU architecture + +#endif + +#else + +/// Dummy implementations +class FPExceptionsEnable { }; +class FPExceptionDisableInvalid { }; +class FPExceptionDisableDivByZero { }; + +#endif + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/FPFlushDenormals.h b/WickedEngine/Jolt/Core/FPFlushDenormals.h new file mode 100644 index 000000000..672a19dbe --- /dev/null +++ b/WickedEngine/Jolt/Core/FPFlushDenormals.h @@ -0,0 +1,41 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_CPU_WASM) + +// Not supported +class FPFlushDenormals { }; + +#elif defined(JPH_USE_SSE) + +/// Helper class that needs to be put on the stack to enable flushing denormals to zero +/// This can make floating point operations much faster when working with very small numbers +class FPFlushDenormals : public FPControlWord<_MM_FLUSH_ZERO_ON, _MM_FLUSH_ZERO_MASK> { }; + +#elif defined(JPH_CPU_ARM) && defined(JPH_COMPILER_MSVC) + +class FPFlushDenormals : public FPControlWord<_DN_FLUSH, _MCW_DN> { }; + +#elif defined(JPH_CPU_ARM) + +/// Flush denormals to zero bit +static constexpr uint64 FP_FZ = 1 << 24; + +/// Helper class that needs to be put on the stack to enable flushing denormals to zero +/// This can make floating point operations much faster when working with very small numbers +class FPFlushDenormals : public FPControlWord { }; + +#else + +#error Unsupported CPU architecture + +#endif + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Factory.cpp b/WickedEngine/Jolt/Core/Factory.cpp new file mode 100644 index 000000000..f5b9dac79 --- /dev/null +++ b/WickedEngine/Jolt/Core/Factory.cpp @@ -0,0 +1,89 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +Factory *Factory::sInstance = nullptr; + +void *Factory::CreateObject(const char *inName) +{ + const RTTI *ci = Find(inName); + return ci != nullptr? ci->CreateObject() : nullptr; +} + +const RTTI *Factory::Find(const char *inName) +{ + ClassNameMap::iterator c = mClassNameMap.find(inName); + return c != mClassNameMap.end()? c->second : nullptr; +} + +const RTTI *Factory::Find(uint32 inHash) +{ + ClassHashMap::iterator c = mClassHashMap.find(inHash); + return c != mClassHashMap.end()? c->second : nullptr; +} + +bool Factory::Register(const RTTI *inRTTI) +{ + // Check if we already know the type + if (Find(inRTTI->GetName()) != nullptr) + return true; + + // Insert this class by name + mClassNameMap.try_emplace(inRTTI->GetName(), inRTTI); + + // Insert this class by hash + if (!mClassHashMap.try_emplace(inRTTI->GetHash(), inRTTI).second) + { + JPH_ASSERT(false, "Hash collision registering type!"); + return false; + } + + // Register base classes + for (int i = 0; i < inRTTI->GetBaseClassCount(); ++i) + if (!Register(inRTTI->GetBaseClass(i))) + return false; + +#ifdef JPH_OBJECT_STREAM + // Register attribute classes + for (int i = 0; i < inRTTI->GetAttributeCount(); ++i) + { + const RTTI *rtti = inRTTI->GetAttribute(i).GetMemberPrimitiveType(); + if (rtti != nullptr && !Register(rtti)) + return false; + } +#endif // JPH_OBJECT_STREAM + + return true; +} + +bool Factory::Register(const RTTI **inRTTIs, uint inNumber) +{ + for (const RTTI **rtti = inRTTIs; rtti < inRTTIs + inNumber; ++rtti) + if (!Register(*rtti)) + return false; + + return true; +} + +void Factory::Clear() +{ + mClassNameMap.clear(); + mClassHashMap.clear(); +} + +Array Factory::GetAllClasses() const +{ + Array all_classes; + all_classes.reserve(mClassNameMap.size()); + for (const ClassNameMap::value_type &c : mClassNameMap) + all_classes.push_back(c.second); + return all_classes; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Factory.h b/WickedEngine/Jolt/Core/Factory.h new file mode 100644 index 000000000..557f38173 --- /dev/null +++ b/WickedEngine/Jolt/Core/Factory.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class is responsible for creating instances of classes based on their name or hash and is mainly used for deserialization of saved data. +class JPH_EXPORT Factory +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Create an object + void * CreateObject(const char *inName); + + /// Find type info for a specific class by name + const RTTI * Find(const char *inName); + + /// Find type info for a specific class by hash + const RTTI * Find(uint32 inHash); + + /// Register an object with the factory. Returns false on failure. + bool Register(const RTTI *inRTTI); + + /// Register a list of objects with the factory. Returns false on failure. + bool Register(const RTTI **inRTTIs, uint inNumber); + + /// Unregisters all types + void Clear(); + + /// Get all registered classes + Array GetAllClasses() const; + + /// Singleton factory instance + static Factory * sInstance; + +private: + using ClassNameMap = UnorderedMap; + + using ClassHashMap = UnorderedMap; + + /// Map of class names to type info + ClassNameMap mClassNameMap; + + // Map of class hash to type info + ClassHashMap mClassHashMap; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/FixedSizeFreeList.h b/WickedEngine/Jolt/Core/FixedSizeFreeList.h new file mode 100644 index 000000000..51f2d1368 --- /dev/null +++ b/WickedEngine/Jolt/Core/FixedSizeFreeList.h @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that allows lock free creation / destruction of objects (unless a new page of objects needs to be allocated) +/// It contains a fixed pool of objects and also allows batching up a lot of objects to be destroyed +/// and doing the actual free in a single atomic operation +template +class FixedSizeFreeList : public NonCopyable +{ +private: + /// Storage type for an Object + struct ObjectStorage + { + /// The object we're storing + Object mObject; + + /// When the object is freed (or in the process of being freed as a batch) this will contain the next free object + /// When an object is in use it will contain the object's index in the free list + atomic mNextFreeObject; + }; + + static_assert(alignof(ObjectStorage) == alignof(Object), "Object not properly aligned"); + + /// Access the object storage given the object index + const ObjectStorage & GetStorage(uint32 inObjectIndex) const { return mPages[inObjectIndex >> mPageShift][inObjectIndex & mObjectMask]; } + ObjectStorage & GetStorage(uint32 inObjectIndex) { return mPages[inObjectIndex >> mPageShift][inObjectIndex & mObjectMask]; } + + /// Size (in objects) of a single page + uint32 mPageSize; + + /// Number of bits to shift an object index to the right to get the page number + uint32 mPageShift; + + /// Mask to and an object index with to get the page number + uint32 mObjectMask; + + /// Total number of pages that are usable + uint32 mNumPages; + + /// Total number of objects that have been allocated + uint32 mNumObjectsAllocated; + + /// Array of pages of objects + ObjectStorage ** mPages = nullptr; + + /// Mutex that is used to allocate a new page if the storage runs out + /// This variable is aligned to the cache line to prevent false sharing with + /// the constants used to index into the list via `Get()`. + alignas(JPH_CACHE_LINE_SIZE) Mutex mPageMutex; + + /// Number of objects that we currently have in the free list / new pages +#ifdef JPH_ENABLE_ASSERTS + atomic mNumFreeObjects; +#endif // JPH_ENABLE_ASSERTS + + /// Simple counter that makes the first free object pointer update with every CAS so that we don't suffer from the ABA problem + atomic mAllocationTag; + + /// Index of first free object, the first 32 bits of an object are used to point to the next free object + atomic mFirstFreeObjectAndTag; + + /// The first free object to use when the free list is empty (may need to allocate a new page) + atomic mFirstFreeObjectInNewPage; + +public: + /// Invalid index + static const uint32 cInvalidObjectIndex = 0xffffffff; + + /// Size of an object + bookkeeping for the freelist + static const int ObjectStorageSize = sizeof(ObjectStorage); + + /// Destructor + inline ~FixedSizeFreeList(); + + /// Initialize the free list, up to inMaxObjects can be allocated + inline void Init(uint inMaxObjects, uint inPageSize); + + /// Lockless construct a new object, inParameters are passed on to the constructor + template + inline uint32 ConstructObject(Parameters &&... inParameters); + + /// Lockless destruct an object and return it to the free pool + inline void DestructObject(uint32 inObjectIndex); + + /// Lockless destruct an object and return it to the free pool + inline void DestructObject(Object *inObject); + + /// A batch of objects that can be destructed + struct Batch + { + uint32 mFirstObjectIndex = cInvalidObjectIndex; + uint32 mLastObjectIndex = cInvalidObjectIndex; + uint32 mNumObjects = 0; + }; + + /// Add a object to an existing batch to be destructed. + /// Adding objects to a batch does not destroy or modify the objects, this will merely link them + /// so that the entire batch can be returned to the free list in a single atomic operation + inline void AddObjectToBatch(Batch &ioBatch, uint32 inObjectIndex); + + /// Lockless destruct batch of objects + inline void DestructObjectBatch(Batch &ioBatch); + + /// Access an object by index. + inline Object & Get(uint32 inObjectIndex) { return GetStorage(inObjectIndex).mObject; } + + /// Access an object by index. + inline const Object & Get(uint32 inObjectIndex) const { return GetStorage(inObjectIndex).mObject; } +}; + +JPH_NAMESPACE_END + +#include "FixedSizeFreeList.inl" diff --git a/WickedEngine/Jolt/Core/FixedSizeFreeList.inl b/WickedEngine/Jolt/Core/FixedSizeFreeList.inl new file mode 100644 index 000000000..dbaae4377 --- /dev/null +++ b/WickedEngine/Jolt/Core/FixedSizeFreeList.inl @@ -0,0 +1,211 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +template +FixedSizeFreeList::~FixedSizeFreeList() +{ + // Check if we got our Init call + if (mPages != nullptr) + { + // Ensure everything is freed before the freelist is destructed + JPH_ASSERT(mNumFreeObjects.load(memory_order_relaxed) == mNumPages * mPageSize); + + // Free memory for pages + uint32 num_pages = mNumObjectsAllocated / mPageSize; + for (uint32 page = 0; page < num_pages; ++page) + AlignedFree(mPages[page]); + Free(mPages); + } +} + +template +void FixedSizeFreeList::Init(uint inMaxObjects, uint inPageSize) +{ + // Check sanity + JPH_ASSERT(inPageSize > 0 && IsPowerOf2(inPageSize)); + JPH_ASSERT(mPages == nullptr); + + // Store configuration parameters + mNumPages = (inMaxObjects + inPageSize - 1) / inPageSize; + mPageSize = inPageSize; + mPageShift = CountTrailingZeros(inPageSize); + mObjectMask = inPageSize - 1; + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects = mNumPages * inPageSize;) + + // Allocate page table + mPages = reinterpret_cast(Allocate(mNumPages * sizeof(ObjectStorage *))); + + // We didn't yet use any objects of any page + mNumObjectsAllocated = 0; + mFirstFreeObjectInNewPage = 0; + + // Start with 1 as the first tag + mAllocationTag = 1; + + // Set first free object (with tag 0) + mFirstFreeObjectAndTag = cInvalidObjectIndex; +} + +template +template +uint32 FixedSizeFreeList::ConstructObject(Parameters &&... inParameters) +{ + for (;;) + { + // Get first object from the linked list + uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire); + uint32 first_free = uint32(first_free_object_and_tag); + if (first_free == cInvalidObjectIndex) + { + // The free list is empty, we take an object from the page that has never been used before + first_free = mFirstFreeObjectInNewPage.fetch_add(1, memory_order_relaxed); + if (first_free >= mNumObjectsAllocated) + { + // Allocate new page + lock_guard lock(mPageMutex); + while (first_free >= mNumObjectsAllocated) + { + uint32 next_page = mNumObjectsAllocated / mPageSize; + if (next_page == mNumPages) + return cInvalidObjectIndex; // Out of space! + mPages[next_page] = reinterpret_cast(AlignedAllocate(mPageSize * sizeof(ObjectStorage), max(alignof(ObjectStorage), JPH_CACHE_LINE_SIZE))); + mNumObjectsAllocated += mPageSize; + } + } + + // Allocation successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_sub(1, memory_order_relaxed);) + ObjectStorage &storage = GetStorage(first_free); + ::new (&storage.mObject) Object(std::forward(inParameters)...); + storage.mNextFreeObject.store(first_free, memory_order_release); + return first_free; + } + else + { + // Load next pointer + uint32 new_first_free = GetStorage(first_free).mNextFreeObject.load(memory_order_acquire); + + // Construct a new first free object tag + uint64 new_first_free_object_and_tag = uint64(new_first_free) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32); + + // Compare and swap + if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release)) + { + // Allocation successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_sub(1, memory_order_relaxed);) + ObjectStorage &storage = GetStorage(first_free); + ::new (&storage.mObject) Object(std::forward(inParameters)...); + storage.mNextFreeObject.store(first_free, memory_order_release); + return first_free; + } + } + } +} + +template +void FixedSizeFreeList::AddObjectToBatch(Batch &ioBatch, uint32 inObjectIndex) +{ + JPH_ASSERT(GetStorage(inObjectIndex).mNextFreeObject.load(memory_order_relaxed) == inObjectIndex, "Trying to add a object to the batch that is already in a free list"); + JPH_ASSERT(ioBatch.mNumObjects != uint32(-1), "Trying to reuse a batch that has already been freed"); + + // Link object in batch to free + if (ioBatch.mFirstObjectIndex == cInvalidObjectIndex) + ioBatch.mFirstObjectIndex = inObjectIndex; + else + GetStorage(ioBatch.mLastObjectIndex).mNextFreeObject.store(inObjectIndex, memory_order_release); + ioBatch.mLastObjectIndex = inObjectIndex; + ioBatch.mNumObjects++; +} + +template +void FixedSizeFreeList::DestructObjectBatch(Batch &ioBatch) +{ + if (ioBatch.mFirstObjectIndex != cInvalidObjectIndex) + { + // Call destructors + if constexpr (!is_trivially_destructible()) + { + uint32 object_idx = ioBatch.mFirstObjectIndex; + do + { + ObjectStorage &storage = GetStorage(object_idx); + storage.mObject.~Object(); + object_idx = storage.mNextFreeObject.load(memory_order_relaxed); + } + while (object_idx != cInvalidObjectIndex); + } + + // Add to objects free list + ObjectStorage &storage = GetStorage(ioBatch.mLastObjectIndex); + for (;;) + { + // Get first object from the list + uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire); + uint32 first_free = uint32(first_free_object_and_tag); + + // Make it the next pointer of the last object in the batch that is to be freed + storage.mNextFreeObject.store(first_free, memory_order_release); + + // Construct a new first free object tag + uint64 new_first_free_object_and_tag = uint64(ioBatch.mFirstObjectIndex) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32); + + // Compare and swap + if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release)) + { + // Free successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_add(ioBatch.mNumObjects, memory_order_relaxed);) + + // Mark the batch as freed +#ifdef JPH_ENABLE_ASSERTS + ioBatch.mNumObjects = uint32(-1); +#endif + return; + } + } + } +} + +template +void FixedSizeFreeList::DestructObject(uint32 inObjectIndex) +{ + JPH_ASSERT(inObjectIndex != cInvalidObjectIndex); + + // Call destructor + ObjectStorage &storage = GetStorage(inObjectIndex); + storage.mObject.~Object(); + + // Add to object free list + for (;;) + { + // Get first object from the list + uint64 first_free_object_and_tag = mFirstFreeObjectAndTag.load(memory_order_acquire); + uint32 first_free = uint32(first_free_object_and_tag); + + // Make it the next pointer of the last object in the batch that is to be freed + storage.mNextFreeObject.store(first_free, memory_order_release); + + // Construct a new first free object tag + uint64 new_first_free_object_and_tag = uint64(inObjectIndex) + (uint64(mAllocationTag.fetch_add(1, memory_order_relaxed)) << 32); + + // Compare and swap + if (mFirstFreeObjectAndTag.compare_exchange_weak(first_free_object_and_tag, new_first_free_object_and_tag, memory_order_release)) + { + // Free successful + JPH_IF_ENABLE_ASSERTS(mNumFreeObjects.fetch_add(1, memory_order_relaxed);) + return; + } + } +} + +template +inline void FixedSizeFreeList::DestructObject(Object *inObject) +{ + uint32 index = reinterpret_cast(inObject)->mNextFreeObject.load(memory_order_relaxed); + JPH_ASSERT(index < mNumObjectsAllocated); + DestructObject(index); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/HashCombine.h b/WickedEngine/Jolt/Core/HashCombine.h new file mode 100644 index 000000000..372070be3 --- /dev/null +++ b/WickedEngine/Jolt/Core/HashCombine.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Implements the FNV-1a hash algorithm +/// @see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function +/// @param inData Data block of bytes +/// @param inSize Number of bytes +/// @param inSeed Seed of the hash (can be used to pass in the hash of a previous operation, otherwise leave default) +/// @return Hash +inline uint64 HashBytes(const void *inData, uint inSize, uint64 inSeed = 0xcbf29ce484222325UL) +{ + uint64 hash = inSeed; + for (const uint8 *data = reinterpret_cast(inData); data < reinterpret_cast(inData) + inSize; ++data) + { + hash = hash ^ uint64(*data); + hash = hash * 0x100000001b3UL; + } + return hash; +} + +/// A 64 bit hash function by Thomas Wang, Jan 1997 +/// See: http://web.archive.org/web/20071223173210/http://www.concentric.net/~Ttwang/tech/inthash.htm +/// @param inValue Value to hash +/// @return Hash +inline uint64 Hash64(uint64 inValue) +{ + uint64 hash = inValue; + hash = (~hash) + (hash << 21); // hash = (hash << 21) - hash - 1; + hash = hash ^ (hash >> 24); + hash = (hash + (hash << 3)) + (hash << 8); // hash * 265 + hash = hash ^ (hash >> 14); + hash = (hash + (hash << 2)) + (hash << 4); // hash * 21 + hash = hash ^ (hash >> 28); + hash = hash + (hash << 31); + return hash; +} + +/// @brief Helper function that hashes a single value into ioSeed +/// Taken from: https://stackoverflow.com/questions/2590677/how-do-i-combine-hash-values-in-c0x +template +inline void HashCombineHelper(size_t &ioSeed, const T &inValue) +{ + std::hash hasher; + ioSeed ^= hasher(inValue) + 0x9e3779b9 + (ioSeed << 6) + (ioSeed >> 2); +} + +/// Hash combiner to use a custom struct in an unordered map or set +/// +/// Usage: +/// +/// struct SomeHashKey +/// { +/// std::string key1; +/// std::string key2; +/// bool key3; +/// }; +/// +/// JPH_MAKE_HASHABLE(SomeHashKey, t.key1, t.key2, t.key3) +template +inline void HashCombine(std::size_t &ioSeed, Values... inValues) +{ + // Hash all values together using a fold expression + (HashCombineHelper(ioSeed, inValues), ...); +} + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +#define JPH_MAKE_HASH_STRUCT(type, name, ...) \ + struct [[nodiscard]] name \ + { \ + std::size_t operator()(const type &t) const \ + { \ + std::size_t ret = 0; \ + ::JPH::HashCombine(ret, __VA_ARGS__); \ + return ret; \ + } \ + }; + +#define JPH_MAKE_HASHABLE(type, ...) \ + JPH_SUPPRESS_WARNING_PUSH \ + JPH_SUPPRESS_WARNINGS \ + namespace std \ + { \ + template<> \ + JPH_MAKE_HASH_STRUCT(type, hash, __VA_ARGS__) \ + } \ + JPH_SUPPRESS_WARNING_POP + +JPH_SUPPRESS_WARNING_POP diff --git a/WickedEngine/Jolt/Core/InsertionSort.h b/WickedEngine/Jolt/Core/InsertionSort.h new file mode 100644 index 000000000..8cd8798be --- /dev/null +++ b/WickedEngine/Jolt/Core/InsertionSort.h @@ -0,0 +1,58 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Implementation of the insertion sort algorithm. +template +inline void InsertionSort(Iterator inBegin, Iterator inEnd, Compare inCompare) +{ + // Empty arrays don't need to be sorted + if (inBegin != inEnd) + { + // Start at the second element + for (Iterator i = inBegin + 1; i != inEnd; ++i) + { + // Move this element to a temporary value + auto x = std::move(*i); + + // Check if the element goes before inBegin (we can't decrement the iterator before inBegin so this needs to be a separate branch) + if (inCompare(x, *inBegin)) + { + // Move all elements to the right to make space for x + Iterator prev; + for (Iterator j = i; j != inBegin; j = prev) + { + prev = j - 1; + *j = *prev; + } + + // Move x to the first place + *inBegin = std::move(x); + } + else + { + // Move elements to the right as long as they are bigger than x + Iterator j = i; + for (Iterator prev = j - 1; inCompare(x, *prev); j = prev, --prev) + *j = std::move(*prev); + + // Move x into place + *j = std::move(x); + } + } + } +} + +/// Implementation of insertion sort algorithm without comparator. +template +inline void InsertionSort(Iterator inBegin, Iterator inEnd) +{ + std::less<> compare; + InsertionSort(inBegin, inEnd, compare); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/IssueReporting.cpp b/WickedEngine/Jolt/Core/IssueReporting.cpp new file mode 100644 index 000000000..e4efe126b --- /dev/null +++ b/WickedEngine/Jolt/Core/IssueReporting.cpp @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +static void DummyTrace([[maybe_unused]] const char *inFMT, ...) +{ + JPH_ASSERT(false); +}; + +TraceFunction Trace = DummyTrace; + +#ifdef JPH_ENABLE_ASSERTS + +static bool DummyAssertFailed(const char *inExpression, const char *inMessage, const char *inFile, uint inLine) +{ + return true; // Trigger breakpoint +}; + +AssertFailedFunction AssertFailed = DummyAssertFailed; + +#endif // JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/IssueReporting.h b/WickedEngine/Jolt/Core/IssueReporting.h new file mode 100644 index 000000000..29aeb911f --- /dev/null +++ b/WickedEngine/Jolt/Core/IssueReporting.h @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Trace function, needs to be overridden by application. This should output a line of text to the log / TTY. +using TraceFunction = void (*)(const char *inFMT, ...); +JPH_EXPORT extern TraceFunction Trace; + +// Always turn on asserts in Debug mode +#if defined(JPH_DEBUG) && !defined(JPH_ENABLE_ASSERTS) + #define JPH_ENABLE_ASSERTS +#endif + +#ifdef JPH_ENABLE_ASSERTS + /// Function called when an assertion fails. This function should return true if a breakpoint needs to be triggered + using AssertFailedFunction = bool(*)(const char *inExpression, const char *inMessage, const char *inFile, uint inLine); + JPH_EXPORT extern AssertFailedFunction AssertFailed; + + // Helper functions to pass message on to failed function + struct AssertLastParam { }; + inline bool AssertFailedParamHelper(const char *inExpression, const char *inFile, uint inLine, AssertLastParam) { return AssertFailed(inExpression, nullptr, inFile, inLine); } + inline bool AssertFailedParamHelper(const char *inExpression, const char *inFile, uint inLine, const char *inMessage, AssertLastParam) { return AssertFailed(inExpression, inMessage, inFile, inLine); } + + /// Main assert macro, usage: JPH_ASSERT(condition, message) or JPH_ASSERT(condition) + #define JPH_ASSERT(inExpression, ...) do { if (!(inExpression) && AssertFailedParamHelper(#inExpression, __FILE__, JPH::uint(__LINE__), ##__VA_ARGS__, JPH::AssertLastParam())) JPH_BREAKPOINT; } while (false) + + #define JPH_IF_ENABLE_ASSERTS(...) __VA_ARGS__ +#else + #define JPH_ASSERT(...) ((void)0) + + #define JPH_IF_ENABLE_ASSERTS(...) +#endif // JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/JobSystem.h b/WickedEngine/Jolt/Core/JobSystem.h new file mode 100644 index 000000000..f12f53809 --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystem.h @@ -0,0 +1,305 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that allows units of work (Jobs) to be scheduled across multiple threads. +/// It allows dependencies between the jobs so that the jobs form a graph. +/// +/// The pattern for using this class is: +/// +/// // Create job system +/// JobSystem *job_system = new JobSystemThreadPool(...); +/// +/// // Create some jobs +/// JobHandle second_job = job_system->CreateJob("SecondJob", Color::sRed, []() { ... }, 1); // Create a job with 1 dependency +/// JobHandle first_job = job_system->CreateJob("FirstJob", Color::sGreen, [second_job]() { ....; second_job.RemoveDependency(); }, 0); // Job can start immediately, will start second job when it's done +/// JobHandle third_job = job_system->CreateJob("ThirdJob", Color::sBlue, []() { ... }, 0); // This job can run immediately as well and can run in parallel to job 1 and 2 +/// +/// // Add the jobs to the barrier so that we can execute them while we're waiting +/// Barrier *barrier = job_system->CreateBarrier(); +/// barrier->AddJob(first_job); +/// barrier->AddJob(second_job); +/// barrier->AddJob(third_job); +/// job_system->WaitForJobs(barrier); +/// +/// // Clean up +/// job_system->DestroyBarrier(barrier); +/// delete job_system; +/// +/// Jobs are guaranteed to be started in the order that their dependency counter becomes zero (in case they're scheduled on a background thread) +/// or in the order they're added to the barrier (when dependency count is zero and when executing on the thread that calls WaitForJobs). +/// +/// If you want to implement your own job system, inherit from JobSystem and implement: +/// +/// * JobSystem::GetMaxConcurrency - This should return the maximum number of jobs that can run in parallel. +/// * JobSystem::CreateJob - This should create a Job object and return it to the caller. +/// * JobSystem::FreeJob - This should free the memory associated with the job object. It is called by the Job destructor when it is Release()-ed for the last time. +/// * JobSystem::QueueJob/QueueJobs - These should store the job pointer in an internal queue to run immediately (dependencies are tracked internally, this function is called when the job can run). +/// The Job objects are reference counted and are guaranteed to stay alive during the QueueJob(s) call. If you store the job in your own data structure you need to call AddRef() to take a reference. +/// After the job has been executed you need to call Release() to release the reference. Make sure you no longer dereference the job pointer after calling Release(). +/// +/// JobSystem::Barrier is used to track the completion of a set of jobs. Jobs will be created by other jobs and added to the barrier while it is being waited on. This means that you cannot +/// create a dependency graph beforehand as the graph changes while jobs are running. Implement the following functions: +/// +/// * Barrier::AddJob/AddJobs - Add a job to the barrier, any call to WaitForJobs will now also wait for this job to complete. +/// If you store the job in a data structure in the Barrier you need to call AddRef() on the job to keep it alive and Release() after you're done with it. +/// * Barrier::OnJobFinished - This function is called when a job has finished executing, you can use this to track completion and remove the job from the list of jobs to wait on. +/// +/// The functions on JobSystem that need to be implemented to support barriers are: +/// +/// * JobSystem::CreateBarrier - Create a new barrier. +/// * JobSystem::DestroyBarrier - Destroy a barrier. +/// * JobSystem::WaitForJobs - This is the main function that is used to wait for all jobs that have been added to a Barrier. WaitForJobs can execute jobs that have +/// been added to the barrier while waiting. It is not wise to execute other jobs that touch physics structures as this can cause race conditions and deadlocks. Please keep in mind that the barrier is +/// only intended to wait on the completion of the Jolt jobs added to it, if you scheduled any jobs in your engine's job system to execute the Jolt jobs as part of QueueJob/QueueJobs, you might still need +/// to wait for these in this function after the barrier is finished waiting. +/// +/// An example implementation is JobSystemThreadPool. If you don't want to write the Barrier class you can also inherit from JobSystemWithBarrier. +class JPH_EXPORT JobSystem : public NonCopyable +{ +protected: + class Job; + +public: + JPH_OVERRIDE_NEW_DELETE + + /// A job handle contains a reference to a job. The job will be deleted as soon as there are no JobHandles. + /// referring to the job and when it is not in the job queue / being processed. + class JobHandle : private Ref + { + public: + /// Constructor + inline JobHandle() = default; + inline JobHandle(const JobHandle &inHandle) = default; + inline JobHandle(JobHandle &&inHandle) noexcept : Ref(std::move(inHandle)) { } + + /// Constructor, only to be used by JobSystem + inline explicit JobHandle(Job *inJob) : Ref(inJob) { } + + /// Assignment + inline JobHandle & operator = (const JobHandle &inHandle) = default; + inline JobHandle & operator = (JobHandle &&inHandle) noexcept = default; + + /// Check if this handle contains a job + inline bool IsValid() const { return GetPtr() != nullptr; } + + /// Check if this job has finished executing + inline bool IsDone() const { return GetPtr() != nullptr && GetPtr()->IsDone(); } + + /// Add to the dependency counter. + inline void AddDependency(int inCount = 1) const { GetPtr()->AddDependency(inCount); } + + /// Remove from the dependency counter. Job will start whenever the dependency counter reaches zero + /// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions. + inline void RemoveDependency(int inCount = 1) const { GetPtr()->RemoveDependencyAndQueue(inCount); } + + /// Remove a dependency from a batch of jobs at once, this can be more efficient than removing them one by one as it requires less locking + static inline void sRemoveDependencies(const JobHandle *inHandles, uint inNumHandles, int inCount = 1); + + /// Helper function to remove dependencies on a static array of job handles + template + static inline void sRemoveDependencies(StaticArray &inHandles, int inCount = 1) + { + sRemoveDependencies(inHandles.data(), inHandles.size(), inCount); + } + + /// Inherit the GetPtr function, only to be used by the JobSystem + using Ref::GetPtr; + }; + + /// A job barrier keeps track of a number of jobs and allows waiting until they are all completed. + class Barrier : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Add a job to this barrier + /// Note that jobs can keep being added to the barrier while waiting for the barrier + virtual void AddJob(const JobHandle &inJob) = 0; + + /// Add multiple jobs to this barrier + /// Note that jobs can keep being added to the barrier while waiting for the barrier + virtual void AddJobs(const JobHandle *inHandles, uint inNumHandles) = 0; + + protected: + /// Job needs to be able to call OnJobFinished + friend class Job; + + /// Destructor, you should call JobSystem::DestroyBarrier instead of destructing this object directly + virtual ~Barrier() = default; + + /// Called by a Job to mark that it is finished + virtual void OnJobFinished(Job *inJob) = 0; + }; + + /// Main function of the job + using JobFunction = function; + + /// Destructor + virtual ~JobSystem() = default; + + /// Get maximum number of concurrently executing jobs + virtual int GetMaxConcurrency() const = 0; + + /// Create a new job, the job is started immediately if inNumDependencies == 0 otherwise it starts when + /// RemoveDependency causes the dependency counter to reach 0. + virtual JobHandle CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) = 0; + + /// Create a new barrier, used to wait on jobs + virtual Barrier * CreateBarrier() = 0; + + /// Destroy a barrier when it is no longer used. The barrier should be empty at this point. + virtual void DestroyBarrier(Barrier *inBarrier) = 0; + + /// Wait for a set of jobs to be finished, note that only 1 thread can be waiting on a barrier at a time + virtual void WaitForJobs(Barrier *inBarrier) = 0; + +protected: + /// A class that contains information for a single unit of work + class Job + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Job([[maybe_unused]] const char *inJobName, [[maybe_unused]] ColorArg inColor, JobSystem *inJobSystem, const JobFunction &inJobFunction, uint32 inNumDependencies) : + #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mJobName(inJobName), + mColor(inColor), + #endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mJobSystem(inJobSystem), + mJobFunction(inJobFunction), + mNumDependencies(inNumDependencies) + { + } + + /// Get the jobs system to which this job belongs + inline JobSystem * GetJobSystem() { return mJobSystem; } + + /// Add or release a reference to this object + inline void AddRef() + { + // Adding a reference can use relaxed memory ordering + mReferenceCount.fetch_add(1, memory_order_relaxed); + } + inline void Release() + { + // Releasing a reference must use release semantics... + if (mReferenceCount.fetch_sub(1, memory_order_release) == 1) + { + // ... so that we can use acquire to ensure that we see any updates from other threads that released a ref before freeing the job + atomic_thread_fence(memory_order_acquire); + mJobSystem->FreeJob(this); + } + } + + /// Add to the dependency counter. + inline void AddDependency(int inCount); + + /// Remove from the dependency counter. Returns true whenever the dependency counter reaches zero + /// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions. + inline bool RemoveDependency(int inCount); + + /// Remove from the dependency counter. Job will be queued whenever the dependency counter reaches zero + /// and if it does it is no longer valid to call the AddDependency/RemoveDependency functions. + inline void RemoveDependencyAndQueue(int inCount); + + /// Set the job barrier that this job belongs to and returns false if this was not possible because the job already finished + inline bool SetBarrier(Barrier *inBarrier) + { + intptr_t barrier = 0; + if (mBarrier.compare_exchange_strong(barrier, reinterpret_cast(inBarrier), memory_order_relaxed)) + return true; + JPH_ASSERT(barrier == cBarrierDoneState, "A job can only belong to 1 barrier"); + return false; + } + + /// Run the job function, returns the number of dependencies that this job still has or cExecutingState or cDoneState + inline uint32 Execute() + { + // Transition job to executing state + uint32 state = 0; // We can only start running with a dependency counter of 0 + if (!mNumDependencies.compare_exchange_strong(state, cExecutingState, memory_order_acquire)) + return state; // state is updated by compare_exchange_strong to the current value + + // Run the job function + { + JPH_PROFILE(mJobName, mColor.GetUInt32()); + mJobFunction(); + } + + // Fetch the barrier pointer and exchange it for the done state, so we're sure that no barrier gets set after we want to call the callback + intptr_t barrier = mBarrier.load(memory_order_relaxed); + for (;;) + { + if (mBarrier.compare_exchange_weak(barrier, cBarrierDoneState, memory_order_relaxed)) + break; + } + JPH_ASSERT(barrier != cBarrierDoneState); + + // Mark job as done + state = cExecutingState; + mNumDependencies.compare_exchange_strong(state, cDoneState, memory_order_relaxed); + JPH_ASSERT(state == cExecutingState); + + // Notify the barrier after we've changed the job to the done state so that any thread reading the state after receiving the callback will see that the job has finished + if (barrier != 0) + reinterpret_cast(barrier)->OnJobFinished(this); + + return cDoneState; + } + + /// Test if the job can be executed + inline bool CanBeExecuted() const { return mNumDependencies.load(memory_order_relaxed) == 0; } + + /// Test if the job finished executing + inline bool IsDone() const { return mNumDependencies.load(memory_order_relaxed) == cDoneState; } + + #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Get the name of the job + const char * GetName() const { return mJobName; } + #endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + + static constexpr uint32 cExecutingState = 0xe0e0e0e0; ///< Value of mNumDependencies when job is executing + static constexpr uint32 cDoneState = 0xd0d0d0d0; ///< Value of mNumDependencies when job is done executing + + static constexpr intptr_t cBarrierDoneState = ~intptr_t(0); ///< Value to use when the barrier has been triggered + +private: + #if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + const char * mJobName; ///< Name of the job + Color mColor; ///< Color of the job in the profiler + #endif // defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + JobSystem * mJobSystem; ///< The job system we belong to + atomic mBarrier = 0; ///< Barrier that this job is associated with (is a Barrier pointer) + JobFunction mJobFunction; ///< Main job function + atomic mReferenceCount = 0; ///< Amount of JobHandles pointing to this job + atomic mNumDependencies; ///< Amount of jobs that need to complete before this job can run + }; + + /// Adds a job to the job queue + virtual void QueueJob(Job *inJob) = 0; + + /// Adds a number of jobs at once to the job queue + virtual void QueueJobs(Job **inJobs, uint inNumJobs) = 0; + + /// Frees a job + virtual void FreeJob(Job *inJob) = 0; +}; + +using JobHandle = JobSystem::JobHandle; + +JPH_NAMESPACE_END + +#include "JobSystem.inl" diff --git a/WickedEngine/Jolt/Core/JobSystem.inl b/WickedEngine/Jolt/Core/JobSystem.inl new file mode 100644 index 000000000..bee4f13b5 --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystem.inl @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +void JobSystem::Job::AddDependency(int inCount) +{ + JPH_IF_ENABLE_ASSERTS(uint32 old_value =) mNumDependencies.fetch_add(inCount, memory_order_relaxed); + JPH_ASSERT(old_value > 0 && old_value != cExecutingState && old_value != cDoneState, "Job is queued, running or done, it is not allowed to add a dependency to a running job"); +} + +bool JobSystem::Job::RemoveDependency(int inCount) +{ + uint32 old_value = mNumDependencies.fetch_sub(inCount, memory_order_release); + JPH_ASSERT(old_value != cExecutingState && old_value != cDoneState, "Job is running or done, it is not allowed to add a dependency to a running job"); + uint32 new_value = old_value - inCount; + JPH_ASSERT(old_value > new_value, "Test wrap around, this is a logic error"); + return new_value == 0; +} + +void JobSystem::Job::RemoveDependencyAndQueue(int inCount) +{ + if (RemoveDependency(inCount)) + mJobSystem->QueueJob(this); +} + +void JobSystem::JobHandle::sRemoveDependencies(const JobHandle *inHandles, uint inNumHandles, int inCount) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumHandles > 0); + + // Get the job system, all jobs should be part of the same job system + JobSystem *job_system = inHandles->GetPtr()->GetJobSystem(); + + // Allocate a buffer to store the jobs that need to be queued + Job **jobs_to_queue = (Job **)JPH_STACK_ALLOC(inNumHandles * sizeof(Job *)); + Job **next_job = jobs_to_queue; + + // Remove the dependencies on all jobs + for (const JobHandle *handle = inHandles, *handle_end = inHandles + inNumHandles; handle < handle_end; ++handle) + { + Job *job = handle->GetPtr(); + JPH_ASSERT(job->GetJobSystem() == job_system); // All jobs should belong to the same job system + if (job->RemoveDependency(inCount)) + *(next_job++) = job; + } + + // If any jobs need to be scheduled, schedule them as a batch + uint num_jobs_to_queue = uint(next_job - jobs_to_queue); + if (num_jobs_to_queue != 0) + job_system->QueueJobs(jobs_to_queue, num_jobs_to_queue); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/JobSystemSingleThreaded.cpp b/WickedEngine/Jolt/Core/JobSystemSingleThreaded.cpp new file mode 100644 index 000000000..5f09d1a04 --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystemSingleThreaded.cpp @@ -0,0 +1,65 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void JobSystemSingleThreaded::Init(uint inMaxJobs) +{ + mJobs.Init(inMaxJobs, inMaxJobs); +} + +JobHandle JobSystemSingleThreaded::CreateJob(const char *inJobName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies) +{ + // Construct an object + uint32 index = mJobs.ConstructObject(inJobName, inColor, this, inJobFunction, inNumDependencies); + JPH_ASSERT(index != AvailableJobs::cInvalidObjectIndex); + Job *job = &mJobs.Get(index); + + // Construct handle to keep a reference, the job is queued below and will immediately complete + JobHandle handle(job); + + // If there are no dependencies, queue the job now + if (inNumDependencies == 0) + QueueJob(job); + + // Return the handle + return handle; +} + +void JobSystemSingleThreaded::FreeJob(Job *inJob) +{ + mJobs.DestructObject(inJob); +} + +void JobSystemSingleThreaded::QueueJob(Job *inJob) +{ + inJob->Execute(); +} + +void JobSystemSingleThreaded::QueueJobs(Job **inJobs, uint inNumJobs) +{ + for (uint i = 0; i < inNumJobs; ++i) + QueueJob(inJobs[i]); +} + +JobSystem::Barrier *JobSystemSingleThreaded::CreateBarrier() +{ + return &mDummyBarrier; +} + +void JobSystemSingleThreaded::DestroyBarrier(Barrier *inBarrier) +{ + // There's nothing to do here, the barrier is just a dummy +} + +void JobSystemSingleThreaded::WaitForJobs(Barrier *inBarrier) +{ + // There's nothing to do here, the barrier is just a dummy, we just execute the jobs immediately +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/JobSystemSingleThreaded.h b/WickedEngine/Jolt/Core/JobSystemSingleThreaded.h new file mode 100644 index 000000000..4db599b31 --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystemSingleThreaded.h @@ -0,0 +1,62 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of a JobSystem without threads, runs jobs as soon as they are added +class JPH_EXPORT JobSystemSingleThreaded final : public JobSystem +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + JobSystemSingleThreaded() = default; + explicit JobSystemSingleThreaded(uint inMaxJobs) { Init(inMaxJobs); } + + /// Initialize the job system + /// @param inMaxJobs Max number of jobs that can be allocated at any time + void Init(uint inMaxJobs); + + // See JobSystem + virtual int GetMaxConcurrency() const override { return 1; } + virtual JobHandle CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) override; + virtual Barrier * CreateBarrier() override; + virtual void DestroyBarrier(Barrier *inBarrier) override; + virtual void WaitForJobs(Barrier *inBarrier) override; + +protected: + // Dummy implementation of Barrier, all jobs are executed immediately + class BarrierImpl : public Barrier + { + public: + JPH_OVERRIDE_NEW_DELETE + + // See Barrier + virtual void AddJob(const JobHandle &inJob) override { /* We don't need to track jobs */ } + virtual void AddJobs(const JobHandle *inHandles, uint inNumHandles) override { /* We don't need to track jobs */ } + + protected: + /// Called by a Job to mark that it is finished + virtual void OnJobFinished(Job *inJob) override { /* We don't need to track jobs */ } + }; + + // See JobSystem + virtual void QueueJob(Job *inJob) override; + virtual void QueueJobs(Job **inJobs, uint inNumJobs) override; + virtual void FreeJob(Job *inJob) override; + + /// Shared barrier since the barrier implementation does nothing + BarrierImpl mDummyBarrier; + + /// Array of jobs (fixed size) + using AvailableJobs = FixedSizeFreeList; + AvailableJobs mJobs; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/JobSystemThreadPool.cpp b/WickedEngine/Jolt/Core/JobSystemThreadPool.cpp new file mode 100644 index 000000000..8a871f5ef --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystemThreadPool.cpp @@ -0,0 +1,360 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +#ifdef JPH_PLATFORM_WINDOWS + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #define WIN32_LEAN_AND_MEAN +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + + JPH_SUPPRESS_WARNING_POP +#endif +#ifdef JPH_PLATFORM_LINUX + #include +#endif + +JPH_NAMESPACE_BEGIN + +void JobSystemThreadPool::Init(uint inMaxJobs, uint inMaxBarriers, int inNumThreads) +{ + JobSystemWithBarrier::Init(inMaxBarriers); + + // Init freelist of jobs + mJobs.Init(inMaxJobs, inMaxJobs); + + // Init queue + for (atomic &j : mQueue) + j = nullptr; + + // Start the worker threads + StartThreads(inNumThreads); +} + +JobSystemThreadPool::JobSystemThreadPool(uint inMaxJobs, uint inMaxBarriers, int inNumThreads) +{ + Init(inMaxJobs, inMaxBarriers, inNumThreads); +} + +void JobSystemThreadPool::StartThreads(int inNumThreads) +{ + // Auto detect number of threads + if (inNumThreads < 0) + inNumThreads = thread::hardware_concurrency() - 1; + + // If no threads are requested we're done + if (inNumThreads == 0) + return; + + // Don't quit the threads + mQuit = false; + + // Allocate heads + mHeads = reinterpret_cast *>(Allocate(sizeof(atomic) * inNumThreads)); + for (int i = 0; i < inNumThreads; ++i) + mHeads[i] = 0; + + // Start running threads + JPH_ASSERT(mThreads.empty()); + mThreads.reserve(inNumThreads); + for (int i = 0; i < inNumThreads; ++i) + mThreads.emplace_back([this, i] { ThreadMain(i); }); +} + +JobSystemThreadPool::~JobSystemThreadPool() +{ + // Stop all worker threads + StopThreads(); +} + +void JobSystemThreadPool::StopThreads() +{ + if (mThreads.empty()) + return; + + // Signal threads that we want to stop and wake them up + mQuit = true; + mSemaphore.Release((uint)mThreads.size()); + + // Wait for all threads to finish + for (thread &t : mThreads) + if (t.joinable()) + t.join(); + + // Delete all threads + mThreads.clear(); + + // Ensure that there are no lingering jobs in the queue + for (uint head = 0; head != mTail; ++head) + { + // Fetch job + Job *job_ptr = mQueue[head & (cQueueLength - 1)].exchange(nullptr); + if (job_ptr != nullptr) + { + // And execute it + job_ptr->Execute(); + job_ptr->Release(); + } + } + + // Destroy heads and reset tail + Free(mHeads); + mHeads = nullptr; + mTail = 0; +} + +JobHandle JobSystemThreadPool::CreateJob(const char *inJobName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies) +{ + JPH_PROFILE_FUNCTION(); + + // Loop until we can get a job from the free list + uint32 index; + for (;;) + { + index = mJobs.ConstructObject(inJobName, inColor, this, inJobFunction, inNumDependencies); + if (index != AvailableJobs::cInvalidObjectIndex) + break; + JPH_ASSERT(false, "No jobs available!"); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + Job *job = &mJobs.Get(index); + + // Construct handle to keep a reference, the job is queued below and may immediately complete + JobHandle handle(job); + + // If there are no dependencies, queue the job now + if (inNumDependencies == 0) + QueueJob(job); + + // Return the handle + return handle; +} + +void JobSystemThreadPool::FreeJob(Job *inJob) +{ + mJobs.DestructObject(inJob); +} + +uint JobSystemThreadPool::GetHead() const +{ + // Find the minimal value across all threads + uint head = mTail; + for (size_t i = 0; i < mThreads.size(); ++i) + head = min(head, mHeads[i].load()); + return head; +} + +void JobSystemThreadPool::QueueJobInternal(Job *inJob) +{ + // Add reference to job because we're adding the job to the queue + inJob->AddRef(); + + // Need to read head first because otherwise the tail can already have passed the head + // We read the head outside of the loop since it involves iterating over all threads and we only need to update + // it if there's not enough space in the queue. + uint head = GetHead(); + + for (;;) + { + // Check if there's space in the queue + uint old_value = mTail; + if (old_value - head >= cQueueLength) + { + // We calculated the head outside of the loop, update head (and we also need to update tail to prevent it from passing head) + head = GetHead(); + old_value = mTail; + + // Second check if there's space in the queue + if (old_value - head >= cQueueLength) + { + // Wake up all threads in order to ensure that they can clear any nullptrs they may not have processed yet + mSemaphore.Release((uint)mThreads.size()); + + // Sleep a little (we have to wait for other threads to update their head pointer in order for us to be able to continue) + std::this_thread::sleep_for(std::chrono::microseconds(100)); + continue; + } + } + + // Write the job pointer if the slot is empty + Job *expected_job = nullptr; + bool success = mQueue[old_value & (cQueueLength - 1)].compare_exchange_strong(expected_job, inJob); + + // Regardless of who wrote the slot, we will update the tail (if the successful thread got scheduled out + // after writing the pointer we still want to be able to continue) + mTail.compare_exchange_strong(old_value, old_value + 1); + + // If we successfully added our job we're done + if (success) + break; + } +} + +void JobSystemThreadPool::QueueJob(Job *inJob) +{ + JPH_PROFILE_FUNCTION(); + + // If we have no worker threads, we can't queue the job either. We assume in this case that the job will be added to a barrier and that the barrier will execute the job when it's Wait() function is called. + if (mThreads.empty()) + return; + + // Queue the job + QueueJobInternal(inJob); + + // Wake up thread + mSemaphore.Release(); +} + +void JobSystemThreadPool::QueueJobs(Job **inJobs, uint inNumJobs) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumJobs > 0); + + // If we have no worker threads, we can't queue the job either. We assume in this case that the job will be added to a barrier and that the barrier will execute the job when it's Wait() function is called. + if (mThreads.empty()) + return; + + // Queue all jobs + for (Job **job = inJobs, **job_end = inJobs + inNumJobs; job < job_end; ++job) + QueueJobInternal(*job); + + // Wake up threads + mSemaphore.Release(min(inNumJobs, (uint)mThreads.size())); +} + +#if defined(JPH_PLATFORM_WINDOWS) + +#if !defined(JPH_COMPILER_MINGW) // MinGW doesn't support __try/__except) + // Sets the current thread name in MSVC debugger + static void RaiseThreadNameException(const char *inName) + { + #pragma pack(push, 8) + + struct THREADNAME_INFO + { + DWORD dwType; // Must be 0x1000. + LPCSTR szName; // Pointer to name (in user addr space). + DWORD dwThreadID; // Thread ID (-1=caller thread). + DWORD dwFlags; // Reserved for future use, must be zero. + }; + + #pragma pack(pop) + + THREADNAME_INFO info; + info.dwType = 0x1000; + info.szName = inName; + info.dwThreadID = (DWORD)-1; + info.dwFlags = 0; + + __try + { + RaiseException(0x406D1388, 0, sizeof(info) / sizeof(ULONG_PTR), (ULONG_PTR *)&info); + } + __except(EXCEPTION_EXECUTE_HANDLER) + { + } + } +#endif // !JPH_COMPILER_MINGW + + static void SetThreadName(const char* inName) + { + JPH_SUPPRESS_WARNING_PUSH + + // Suppress casting warning, it's fine here as GetProcAddress doesn't really return a FARPROC + JPH_CLANG_SUPPRESS_WARNING("-Wcast-function-type") // error : cast from 'FARPROC' (aka 'long long (*)()') to 'SetThreadDescriptionFunc' (aka 'long (*)(void *, const wchar_t *)') converts to incompatible function type + JPH_CLANG_SUPPRESS_WARNING("-Wcast-function-type-strict") // error : cast from 'FARPROC' (aka 'long long (*)()') to 'SetThreadDescriptionFunc' (aka 'long (*)(void *, const wchar_t *)') converts to incompatible function type + JPH_MSVC_SUPPRESS_WARNING(4191) // reinterpret_cast' : unsafe conversion from 'FARPROC' to 'SetThreadDescriptionFunc'. Calling this function through the result pointer may cause your program to fail + + using SetThreadDescriptionFunc = HRESULT(WINAPI*)(HANDLE hThread, PCWSTR lpThreadDescription); + static SetThreadDescriptionFunc SetThreadDescription = reinterpret_cast(GetProcAddress(GetModuleHandleW(L"Kernel32.dll"), "SetThreadDescription")); + + JPH_SUPPRESS_WARNING_POP + + if (SetThreadDescription) + { + wchar_t name_buffer[64] = { 0 }; + if (MultiByteToWideChar(CP_UTF8, 0, inName, -1, name_buffer, sizeof(name_buffer) / sizeof(wchar_t) - 1) == 0) + return; + + SetThreadDescription(GetCurrentThread(), name_buffer); + } +#if !defined(JPH_COMPILER_MINGW) + else if (IsDebuggerPresent()) + RaiseThreadNameException(inName); +#endif // !JPH_COMPILER_MINGW + } +#elif defined(JPH_PLATFORM_LINUX) + static void SetThreadName(const char *inName) + { + JPH_ASSERT(strlen(inName) < 16); // String will be truncated if it is longer + prctl(PR_SET_NAME, inName, 0, 0, 0); + } +#endif // JPH_PLATFORM_LINUX + +void JobSystemThreadPool::ThreadMain(int inThreadIndex) +{ + // Name the thread + char name[64]; + snprintf(name, sizeof(name), "Worker %d", int(inThreadIndex + 1)); + +#if defined(JPH_PLATFORM_WINDOWS) || defined(JPH_PLATFORM_LINUX) + SetThreadName(name); +#endif // JPH_PLATFORM_WINDOWS && !JPH_COMPILER_MINGW + + // Enable floating point exceptions + FPExceptionsEnable enable_exceptions; + JPH_UNUSED(enable_exceptions); + + JPH_PROFILE_THREAD_START(name); + + // Call the thread init function + mThreadInitFunction(inThreadIndex); + + atomic &head = mHeads[inThreadIndex]; + + while (!mQuit) + { + // Wait for jobs + mSemaphore.Acquire(); + + { + JPH_PROFILE("Executing Jobs"); + + // Loop over the queue + while (head != mTail) + { + // Exchange any job pointer we find with a nullptr + atomic &job = mQueue[head & (cQueueLength - 1)]; + if (job.load() != nullptr) + { + Job *job_ptr = job.exchange(nullptr); + if (job_ptr != nullptr) + { + // And execute it + job_ptr->Execute(); + job_ptr->Release(); + } + } + head++; + } + } + } + + // Call the thread exit function + mThreadExitFunction(inThreadIndex); + + JPH_PROFILE_THREAD_END(); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/JobSystemThreadPool.h b/WickedEngine/Jolt/Core/JobSystemThreadPool.h new file mode 100644 index 000000000..2e44cb69e --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystemThreadPool.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::thread; + +/// Implementation of a JobSystem using a thread pool +/// +/// Note that this is considered an example implementation. It is expected that when you integrate +/// the physics engine into your own project that you'll provide your own implementation of the +/// JobSystem built on top of whatever job system your project uses. +class JPH_EXPORT JobSystemThreadPool final : public JobSystemWithBarrier +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Creates a thread pool. + /// @see JobSystemThreadPool::Init + JobSystemThreadPool(uint inMaxJobs, uint inMaxBarriers, int inNumThreads = -1); + JobSystemThreadPool() = default; + virtual ~JobSystemThreadPool() override; + + /// Functions to call when a thread is initialized or exits, must be set before calling Init() + using InitExitFunction = function; + void SetThreadInitFunction(const InitExitFunction &inInitFunction) { mThreadInitFunction = inInitFunction; } + void SetThreadExitFunction(const InitExitFunction &inExitFunction) { mThreadExitFunction = inExitFunction; } + + /// Initialize the thread pool + /// @param inMaxJobs Max number of jobs that can be allocated at any time + /// @param inMaxBarriers Max number of barriers that can be allocated at any time + /// @param inNumThreads Number of threads to start (the number of concurrent jobs is 1 more because the main thread will also run jobs while waiting for a barrier to complete). Use -1 to auto detect the amount of CPU's. + void Init(uint inMaxJobs, uint inMaxBarriers, int inNumThreads = -1); + + // See JobSystem + virtual int GetMaxConcurrency() const override { return int(mThreads.size()) + 1; } + virtual JobHandle CreateJob(const char *inName, ColorArg inColor, const JobFunction &inJobFunction, uint32 inNumDependencies = 0) override; + + /// Change the max concurrency after initialization + void SetNumThreads(int inNumThreads) { StopThreads(); StartThreads(inNumThreads); } + +protected: + // See JobSystem + virtual void QueueJob(Job *inJob) override; + virtual void QueueJobs(Job **inJobs, uint inNumJobs) override; + virtual void FreeJob(Job *inJob) override; + +private: + /// Start/stop the worker threads + void StartThreads(int inNumThreads); + void StopThreads(); + + /// Entry point for a thread + void ThreadMain(int inThreadIndex); + + /// Get the head of the thread that has processed the least amount of jobs + inline uint GetHead() const; + + /// Internal helper function to queue a job + inline void QueueJobInternal(Job *inJob); + + /// Functions to call when initializing or exiting a thread + InitExitFunction mThreadInitFunction = [](int) { }; + InitExitFunction mThreadExitFunction = [](int) { }; + + /// Array of jobs (fixed size) + using AvailableJobs = FixedSizeFreeList; + AvailableJobs mJobs; + + /// Threads running jobs + Array mThreads; + + // The job queue + static constexpr uint32 cQueueLength = 1024; + static_assert(IsPowerOf2(cQueueLength)); // We do bit operations and require queue length to be a power of 2 + atomic mQueue[cQueueLength]; + + // Head and tail of the queue, do this value modulo cQueueLength - 1 to get the element in the mQueue array + atomic * mHeads = nullptr; ///< Per executing thread the head of the current queue + alignas(JPH_CACHE_LINE_SIZE) atomic mTail = 0; ///< Tail (write end) of the queue + + // Semaphore used to signal worker threads that there is new work + Semaphore mSemaphore; + + /// Boolean to indicate that we want to stop the job system + atomic mQuit = false; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/JobSystemWithBarrier.cpp b/WickedEngine/Jolt/Core/JobSystemWithBarrier.cpp new file mode 100644 index 000000000..eaaabdb09 --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystemWithBarrier.cpp @@ -0,0 +1,227 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +JobSystemWithBarrier::BarrierImpl::BarrierImpl() +{ + for (atomic &j : mJobs) + j = nullptr; +} + +JobSystemWithBarrier::BarrierImpl::~BarrierImpl() +{ + JPH_ASSERT(IsEmpty()); +} + +void JobSystemWithBarrier::BarrierImpl::AddJob(const JobHandle &inJob) +{ + JPH_PROFILE_FUNCTION(); + + bool release_semaphore = false; + + // Set the barrier on the job, this returns true if the barrier was successfully set (otherwise the job is already done and we don't need to add it to our list) + Job *job = inJob.GetPtr(); + if (job->SetBarrier(this)) + { + // If the job can be executed we want to release the semaphore an extra time to allow the waiting thread to start executing it + mNumToAcquire++; + if (job->CanBeExecuted()) + { + release_semaphore = true; + mNumToAcquire++; + } + + // Add the job to our job list + job->AddRef(); + uint write_index = mJobWriteIndex++; + while (write_index - mJobReadIndex >= cMaxJobs) + { + JPH_ASSERT(false, "Barrier full, stalling!"); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + mJobs[write_index & (cMaxJobs - 1)] = job; + } + + // Notify waiting thread that a new executable job is available + if (release_semaphore) + mSemaphore.Release(); +} + +void JobSystemWithBarrier::BarrierImpl::AddJobs(const JobHandle *inHandles, uint inNumHandles) +{ + JPH_PROFILE_FUNCTION(); + + bool release_semaphore = false; + + for (const JobHandle *handle = inHandles, *handles_end = inHandles + inNumHandles; handle < handles_end; ++handle) + { + // Set the barrier on the job, this returns true if the barrier was successfully set (otherwise the job is already done and we don't need to add it to our list) + Job *job = handle->GetPtr(); + if (job->SetBarrier(this)) + { + // If the job can be executed we want to release the semaphore an extra time to allow the waiting thread to start executing it + mNumToAcquire++; + if (!release_semaphore && job->CanBeExecuted()) + { + release_semaphore = true; + mNumToAcquire++; + } + + // Add the job to our job list + job->AddRef(); + uint write_index = mJobWriteIndex++; + while (write_index - mJobReadIndex >= cMaxJobs) + { + JPH_ASSERT(false, "Barrier full, stalling!"); + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + mJobs[write_index & (cMaxJobs - 1)] = job; + } + } + + // Notify waiting thread that a new executable job is available + if (release_semaphore) + mSemaphore.Release(); +} + +void JobSystemWithBarrier::BarrierImpl::OnJobFinished(Job *inJob) +{ + JPH_PROFILE_FUNCTION(); + + mSemaphore.Release(); +} + +void JobSystemWithBarrier::BarrierImpl::Wait() +{ + while (mNumToAcquire > 0) + { + { + JPH_PROFILE("Execute Jobs"); + + // Go through all jobs + bool has_executed; + do + { + has_executed = false; + + // Loop through the jobs and erase jobs from the beginning of the list that are done + while (mJobReadIndex < mJobWriteIndex) + { + atomic &job = mJobs[mJobReadIndex & (cMaxJobs - 1)]; + Job *job_ptr = job.load(); + if (job_ptr == nullptr || !job_ptr->IsDone()) + break; + + // Job is finished, release it + job_ptr->Release(); + job = nullptr; + ++mJobReadIndex; + } + + // Loop through the jobs and execute the first executable job + for (uint index = mJobReadIndex; index < mJobWriteIndex; ++index) + { + const atomic &job = mJobs[index & (cMaxJobs - 1)]; + Job *job_ptr = job.load(); + if (job_ptr != nullptr && job_ptr->CanBeExecuted()) + { + // This will only execute the job if it has not already executed + job_ptr->Execute(); + has_executed = true; + break; + } + } + + } while (has_executed); + } + + // Wait for another thread to wake us when either there is more work to do or when all jobs have completed + int num_to_acquire = max(1, mSemaphore.GetValue()); // When there have been multiple releases, we acquire them all at the same time to avoid needlessly spinning on executing jobs + mSemaphore.Acquire(num_to_acquire); + mNumToAcquire -= num_to_acquire; + } + + // All jobs should be done now, release them + while (mJobReadIndex < mJobWriteIndex) + { + atomic &job = mJobs[mJobReadIndex & (cMaxJobs - 1)]; + Job *job_ptr = job.load(); + JPH_ASSERT(job_ptr != nullptr && job_ptr->IsDone()); + job_ptr->Release(); + job = nullptr; + ++mJobReadIndex; + } +} + +void JobSystemWithBarrier::Init(uint inMaxBarriers) +{ + JPH_ASSERT(mBarriers == nullptr); // Already initialized? + + // Init freelist of barriers + mMaxBarriers = inMaxBarriers; + mBarriers = new BarrierImpl [inMaxBarriers]; +} + +JobSystemWithBarrier::JobSystemWithBarrier(uint inMaxBarriers) +{ + Init(inMaxBarriers); +} + +JobSystemWithBarrier::~JobSystemWithBarrier() +{ + // Ensure that none of the barriers are used +#ifdef JPH_ENABLE_ASSERTS + for (const BarrierImpl *b = mBarriers, *b_end = mBarriers + mMaxBarriers; b < b_end; ++b) + JPH_ASSERT(!b->mInUse); +#endif // JPH_ENABLE_ASSERTS + delete [] mBarriers; +} + +JobSystem::Barrier *JobSystemWithBarrier::CreateBarrier() +{ + JPH_PROFILE_FUNCTION(); + + // Find the first unused barrier + for (uint32 index = 0; index < mMaxBarriers; ++index) + { + bool expected = false; + if (mBarriers[index].mInUse.compare_exchange_strong(expected, true)) + return &mBarriers[index]; + } + + return nullptr; +} + +void JobSystemWithBarrier::DestroyBarrier(Barrier *inBarrier) +{ + JPH_PROFILE_FUNCTION(); + + // Check that no jobs are in the barrier + JPH_ASSERT(static_cast(inBarrier)->IsEmpty()); + + // Flag the barrier as unused + bool expected = true; + static_cast(inBarrier)->mInUse.compare_exchange_strong(expected, false); + JPH_ASSERT(expected); +} + +void JobSystemWithBarrier::WaitForJobs(Barrier *inBarrier) +{ + JPH_PROFILE_FUNCTION(); + + // Let our barrier implementation wait for the jobs + static_cast(inBarrier)->Wait(); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/JobSystemWithBarrier.h b/WickedEngine/Jolt/Core/JobSystemWithBarrier.h new file mode 100644 index 000000000..0428824a4 --- /dev/null +++ b/WickedEngine/Jolt/Core/JobSystemWithBarrier.h @@ -0,0 +1,85 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of the Barrier class for a JobSystem +/// +/// This class can be used to make it easier to create a new JobSystem implementation that integrates with your own job system. +/// It will implement all functionality relating to barriers, so the only functions that are left to be implemented are: +/// +/// * JobSystem::GetMaxConcurrency +/// * JobSystem::CreateJob +/// * JobSystem::FreeJob +/// * JobSystem::QueueJob/QueueJobs +/// +/// See instructions in JobSystem for more information on how to implement these. +class JPH_EXPORT JobSystemWithBarrier : public JobSystem +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs barriers + /// @see JobSystemWithBarrier::Init + explicit JobSystemWithBarrier(uint inMaxBarriers); + JobSystemWithBarrier() = default; + virtual ~JobSystemWithBarrier() override; + + /// Initialize the barriers + /// @param inMaxBarriers Max number of barriers that can be allocated at any time + void Init(uint inMaxBarriers); + + // See JobSystem + virtual Barrier * CreateBarrier() override; + virtual void DestroyBarrier(Barrier *inBarrier) override; + virtual void WaitForJobs(Barrier *inBarrier) override; + +private: + class BarrierImpl : public Barrier + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BarrierImpl(); + virtual ~BarrierImpl() override; + + // See Barrier + virtual void AddJob(const JobHandle &inJob) override; + virtual void AddJobs(const JobHandle *inHandles, uint inNumHandles) override; + + /// Check if there are any jobs in the job barrier + inline bool IsEmpty() const { return mJobReadIndex == mJobWriteIndex; } + + /// Wait for all jobs in this job barrier, while waiting, execute jobs that are part of this barrier on the current thread + void Wait(); + + /// Flag to indicate if a barrier has been handed out + atomic mInUse { false }; + + protected: + /// Called by a Job to mark that it is finished + virtual void OnJobFinished(Job *inJob) override; + + /// Jobs queue for the barrier + static constexpr uint cMaxJobs = 2048; + static_assert(IsPowerOf2(cMaxJobs)); // We do bit operations and require max jobs to be a power of 2 + atomic mJobs[cMaxJobs]; ///< List of jobs that are part of this barrier, nullptrs for empty slots + alignas(JPH_CACHE_LINE_SIZE) atomic mJobReadIndex { 0 }; ///< First job that could be valid (modulo cMaxJobs), can be nullptr if other thread is still working on adding the job + alignas(JPH_CACHE_LINE_SIZE) atomic mJobWriteIndex { 0 }; ///< First job that can be written (modulo cMaxJobs) + atomic mNumToAcquire { 0 }; ///< Number of times the semaphore has been released, the barrier should acquire the semaphore this many times (written at the same time as mJobWriteIndex so ok to put in same cache line) + Semaphore mSemaphore; ///< Semaphore used by finishing jobs to signal the barrier that they're done + }; + + /// Array of barriers (we keep them constructed all the time since constructing a semaphore/mutex is not cheap) + uint mMaxBarriers = 0; ///< Max amount of barriers + BarrierImpl * mBarriers = nullptr; ///< List of the actual barriers +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/LinearCurve.cpp b/WickedEngine/Jolt/Core/LinearCurve.cpp new file mode 100644 index 000000000..8c8f7682e --- /dev/null +++ b/WickedEngine/Jolt/Core/LinearCurve.cpp @@ -0,0 +1,51 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(LinearCurve::Point) +{ + JPH_ADD_ATTRIBUTE(Point, mX) + JPH_ADD_ATTRIBUTE(Point, mY) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(LinearCurve) +{ + JPH_ADD_ATTRIBUTE(LinearCurve, mPoints) +} + +float LinearCurve::GetValue(float inX) const +{ + if (mPoints.empty()) + return 0.0f; + + Points::const_iterator i2 = std::lower_bound(mPoints.begin(), mPoints.end(), inX, [](const Point &inPoint, float inValue) { return inPoint.mX < inValue; }); + + if (i2 == mPoints.begin()) + return mPoints.front().mY; + else if (i2 == mPoints.end()) + return mPoints.back().mY; + + Points::const_iterator i1 = i2 - 1; + return i1->mY + (inX - i1->mX) * (i2->mY - i1->mY) / (i2->mX - i1->mX); +} + +void LinearCurve::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPoints); +} + +void LinearCurve::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPoints); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/LinearCurve.h b/WickedEngine/Jolt/Core/LinearCurve.h new file mode 100644 index 000000000..ed77341a7 --- /dev/null +++ b/WickedEngine/Jolt/Core/LinearCurve.h @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamOut; +class StreamIn; + +// A set of points (x, y) that form a linear curve +class JPH_EXPORT LinearCurve +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, LinearCurve) + + /// A point on the curve + class Point + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point) + + float mX = 0.0f; + float mY = 0.0f; + }; + + /// Remove all points + void Clear() { mPoints.clear(); } + + /// Reserve memory for inNumPoints points + void Reserve(uint inNumPoints) { mPoints.reserve(inNumPoints); } + + /// Add a point to the curve. Points must be inserted in ascending X or Sort() needs to be called when all points have been added. + /// @param inX X value + /// @param inY Y value + void AddPoint(float inX, float inY) { mPoints.push_back({ inX, inY }); } + + /// Sort the points on X ascending + void Sort() { QuickSort(mPoints.begin(), mPoints.end(), [](const Point &inLHS, const Point &inRHS) { return inLHS.mX < inRHS.mX; }); } + + /// Get the lowest X value + float GetMinX() const { return mPoints.empty()? 0.0f : mPoints.front().mX; } + + /// Get the highest X value + float GetMaxX() const { return mPoints.empty()? 0.0f : mPoints.back().mX; } + + /// Sample value on the curve + /// @param inX X value to sample at + /// @return Interpolated Y value + float GetValue(float inX) const; + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// The points on the curve, should be sorted ascending by x + using Points = Array; + Points mPoints; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/LockFreeHashMap.h b/WickedEngine/Jolt/Core/LockFreeHashMap.h new file mode 100644 index 000000000..c4cbfa8ec --- /dev/null +++ b/WickedEngine/Jolt/Core/LockFreeHashMap.h @@ -0,0 +1,182 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Allocator for a lock free hash map +class LFHMAllocator : public NonCopyable +{ +public: + /// Destructor + inline ~LFHMAllocator(); + + /// Initialize the allocator + /// @param inObjectStoreSizeBytes Number of bytes to reserve for all key value pairs + inline void Init(uint inObjectStoreSizeBytes); + + /// Clear all allocations + inline void Clear(); + + /// Allocate a new block of data + /// @param inBlockSize Size of block to allocate (will potentially return a smaller block if memory is full). + /// @param ioBegin Should be the start of the first free byte in current memory block on input, will contain the start of the first free byte in allocated block on return. + /// @param ioEnd Should be the byte beyond the current memory block on input, will contain the byte beyond the allocated block on return. + inline void Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd); + + /// Convert a pointer to an offset + template + inline uint32 ToOffset(const T *inData) const; + + /// Convert an offset to a pointer + template + inline T * FromOffset(uint32 inOffset) const; + +private: + uint8 * mObjectStore = nullptr; ///< This contains a contiguous list of objects (possibly of varying size) + uint32 mObjectStoreSizeBytes = 0; ///< The size of mObjectStore in bytes + atomic mWriteOffset { 0 }; ///< Next offset to write to in mObjectStore +}; + +/// Allocator context object for a lock free hash map that allocates a larger memory block at once and hands it out in smaller portions. +/// This avoids contention on the atomic LFHMAllocator::mWriteOffset. +class LFHMAllocatorContext : public NonCopyable +{ +public: + /// Construct a new allocator context + inline LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize); + + /// @brief Allocate data block + /// @param inSize Size of block to allocate. + /// @param inAlignment Alignment of block to allocate. + /// @param outWriteOffset Offset in buffer where block is located + /// @return True if allocation succeeded + inline bool Allocate(uint32 inSize, uint32 inAlignment, uint32 &outWriteOffset); + +private: + LFHMAllocator & mAllocator; + uint32 mBlockSize; + uint32 mBegin = 0; + uint32 mEnd = 0; +}; + +/// Very simple lock free hash map that only allows insertion, retrieval and provides a fixed amount of buckets and fixed storage. +/// Note: This class currently assumes key and value are simple types that need no calls to the destructor. +template +class LockFreeHashMap : public NonCopyable +{ +public: + using MapType = LockFreeHashMap; + + /// Destructor + explicit LockFreeHashMap(LFHMAllocator &inAllocator) : mAllocator(inAllocator) { } + ~LockFreeHashMap(); + + /// Initialization + /// @param inMaxBuckets Max amount of buckets to use in the hashmap. Must be power of 2. + void Init(uint32 inMaxBuckets); + + /// Remove all elements. + /// Note that this cannot happen simultaneously with adding new elements. + void Clear(); + + /// Get the current amount of buckets that the map is using + uint32 GetNumBuckets() const { return mNumBuckets; } + + /// Get the maximum amount of buckets that this map supports + uint32 GetMaxBuckets() const { return mMaxBuckets; } + + /// Update the number of buckets. This must be done after clearing the map and cannot be done concurrently with any other operations on the map. + /// Note that the number of buckets can never become bigger than the specified max buckets during initialization and that it must be a power of 2. + void SetNumBuckets(uint32 inNumBuckets); + + /// A key / value pair that is inserted in the map + class KeyValue + { + public: + const Key & GetKey() const { return mKey; } + Value & GetValue() { return mValue; } + const Value & GetValue() const { return mValue; } + + private: + template friend class LockFreeHashMap; + + Key mKey; ///< Key for this entry + uint32 mNextOffset; ///< Offset in mObjectStore of next KeyValue entry with same hash + Value mValue; ///< Value for this entry + optionally extra bytes + }; + + /// Insert a new element, returns null if map full. + /// Multiple threads can be inserting in the map at the same time. + template + inline KeyValue * Create(LFHMAllocatorContext &ioContext, const Key &inKey, uint64 inKeyHash, int inExtraBytes, Params &&... inConstructorParams); + + /// Find an element, returns null if not found + inline const KeyValue * Find(const Key &inKey, uint64 inKeyHash) const; + + /// Value of an invalid handle + const static uint32 cInvalidHandle = uint32(-1); + + /// Get convert key value pair to uint32 handle + inline uint32 ToHandle(const KeyValue *inKeyValue) const; + + /// Convert uint32 handle back to key and value + inline const KeyValue * FromHandle(uint32 inHandle) const; + +#ifdef JPH_ENABLE_ASSERTS + /// Get the number of key value pairs that this map currently contains. + /// Available only when asserts are enabled because adding elements creates contention on this atomic and negatively affects performance. + inline uint32 GetNumKeyValues() const { return mNumKeyValues; } +#endif // JPH_ENABLE_ASSERTS + + /// Get all key/value pairs + inline void GetAllKeyValues(Array &outAll) const; + + /// Non-const iterator + struct Iterator + { + /// Comparison + bool operator == (const Iterator &inRHS) const { return mMap == inRHS.mMap && mBucket == inRHS.mBucket && mOffset == inRHS.mOffset; } + bool operator != (const Iterator &inRHS) const { return !(*this == inRHS); } + + /// Convert to key value pair + KeyValue & operator * (); + + /// Next item + Iterator & operator ++ (); + + MapType * mMap; + uint32 mBucket; + uint32 mOffset; + }; + + /// Iterate over the map, note that it is not safe to do this in parallel to Clear(). + /// It is safe to do this while adding elements to the map, but newly added elements may or may not be returned by the iterator. + Iterator begin(); + Iterator end(); + +#ifdef JPH_DEBUG + /// Output stats about this map to the log + void TraceStats() const; +#endif + +private: + LFHMAllocator & mAllocator; ///< Allocator used to allocate key value pairs + +#ifdef JPH_ENABLE_ASSERTS + atomic mNumKeyValues = 0; ///< Number of key value pairs in the store +#endif // JPH_ENABLE_ASSERTS + + atomic * mBuckets = nullptr; ///< This contains the offset in mObjectStore of the first object with a particular hash + uint32 mNumBuckets = 0; ///< Current number of buckets + uint32 mMaxBuckets = 0; ///< Maximum number of buckets +}; + +JPH_NAMESPACE_END + +#include "LockFreeHashMap.inl" diff --git a/WickedEngine/Jolt/Core/LockFreeHashMap.inl b/WickedEngine/Jolt/Core/LockFreeHashMap.inl new file mode 100644 index 000000000..aa4b875e9 --- /dev/null +++ b/WickedEngine/Jolt/Core/LockFreeHashMap.inl @@ -0,0 +1,351 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/////////////////////////////////////////////////////////////////////////////////// +// LFHMAllocator +/////////////////////////////////////////////////////////////////////////////////// + +inline LFHMAllocator::~LFHMAllocator() +{ + AlignedFree(mObjectStore); +} + +inline void LFHMAllocator::Init(uint inObjectStoreSizeBytes) +{ + JPH_ASSERT(mObjectStore == nullptr); + + mObjectStoreSizeBytes = inObjectStoreSizeBytes; + mObjectStore = reinterpret_cast(JPH::AlignedAllocate(inObjectStoreSizeBytes, 16)); +} + +inline void LFHMAllocator::Clear() +{ + mWriteOffset = 0; +} + +inline void LFHMAllocator::Allocate(uint32 inBlockSize, uint32 &ioBegin, uint32 &ioEnd) +{ + // If we're already beyond the end of our buffer then don't do an atomic add. + // It's possible that many keys are inserted after the allocator is full, making it possible + // for mWriteOffset (uint32) to wrap around to zero. When this happens, there will be a memory corruption. + // This way, we will be able to progress the write offset beyond the size of the buffer + // worst case by max * inBlockSize. + if (mWriteOffset.load(memory_order_relaxed) >= mObjectStoreSizeBytes) + return; + + // Atomically fetch a block from the pool + uint32 begin = mWriteOffset.fetch_add(inBlockSize, memory_order_relaxed); + uint32 end = min(begin + inBlockSize, mObjectStoreSizeBytes); + + if (ioEnd == begin) + { + // Block is allocated straight after our previous block + begin = ioBegin; + } + else + { + // Block is a new block + begin = min(begin, mObjectStoreSizeBytes); + } + + // Store the begin and end of the resulting block + ioBegin = begin; + ioEnd = end; +} + +template +inline uint32 LFHMAllocator::ToOffset(const T *inData) const +{ + const uint8 *data = reinterpret_cast(inData); + JPH_ASSERT(data >= mObjectStore && data < mObjectStore + mObjectStoreSizeBytes); + return uint32(data - mObjectStore); +} + +template +inline T *LFHMAllocator::FromOffset(uint32 inOffset) const +{ + JPH_ASSERT(inOffset < mObjectStoreSizeBytes); + return reinterpret_cast(mObjectStore + inOffset); +} + +/////////////////////////////////////////////////////////////////////////////////// +// LFHMAllocatorContext +/////////////////////////////////////////////////////////////////////////////////// + +inline LFHMAllocatorContext::LFHMAllocatorContext(LFHMAllocator &inAllocator, uint32 inBlockSize) : + mAllocator(inAllocator), + mBlockSize(inBlockSize) +{ +} + +inline bool LFHMAllocatorContext::Allocate(uint32 inSize, uint32 inAlignment, uint32 &outWriteOffset) +{ + // Calculate needed bytes for alignment + JPH_ASSERT(IsPowerOf2(inAlignment)); + uint32 alignment_mask = inAlignment - 1; + uint32 alignment = (inAlignment - (mBegin & alignment_mask)) & alignment_mask; + + // Check if we have space + if (mEnd - mBegin < inSize + alignment) + { + // Allocate a new block + mAllocator.Allocate(mBlockSize, mBegin, mEnd); + + // Update alignment + alignment = (inAlignment - (mBegin & alignment_mask)) & alignment_mask; + + // Check if we have space again + if (mEnd - mBegin < inSize + alignment) + return false; + } + + // Make the allocation + mBegin += alignment; + outWriteOffset = mBegin; + mBegin += inSize; + return true; +} + +/////////////////////////////////////////////////////////////////////////////////// +// LockFreeHashMap +/////////////////////////////////////////////////////////////////////////////////// + +template +void LockFreeHashMap::Init(uint32 inMaxBuckets) +{ + JPH_ASSERT(inMaxBuckets >= 4 && IsPowerOf2(inMaxBuckets)); + JPH_ASSERT(mBuckets == nullptr); + + mNumBuckets = inMaxBuckets; + mMaxBuckets = inMaxBuckets; + + mBuckets = reinterpret_cast *>(AlignedAllocate(inMaxBuckets * sizeof(atomic), 16)); + + Clear(); +} + +template +LockFreeHashMap::~LockFreeHashMap() +{ + AlignedFree(mBuckets); +} + +template +void LockFreeHashMap::Clear() +{ +#ifdef JPH_ENABLE_ASSERTS + // Reset number of key value pairs + mNumKeyValues = 0; +#endif // JPH_ENABLE_ASSERTS + + // Reset buckets 4 at a time + static_assert(sizeof(atomic) == sizeof(uint32)); + UVec4 invalid_handle = UVec4::sReplicate(cInvalidHandle); + uint32 *start = reinterpret_cast(mBuckets); + const uint32 *end = start + mNumBuckets; + JPH_ASSERT(IsAligned(start, 16)); + while (start < end) + { + invalid_handle.StoreInt4Aligned(start); + start += 4; + } +} + +template +void LockFreeHashMap::SetNumBuckets(uint32 inNumBuckets) +{ + JPH_ASSERT(mNumKeyValues == 0); + JPH_ASSERT(inNumBuckets <= mMaxBuckets); + JPH_ASSERT(inNumBuckets >= 4 && IsPowerOf2(inNumBuckets)); + + mNumBuckets = inNumBuckets; +} + +template +template +inline typename LockFreeHashMap::KeyValue *LockFreeHashMap::Create(LFHMAllocatorContext &ioContext, const Key &inKey, uint64 inKeyHash, int inExtraBytes, Params &&... inConstructorParams) +{ + // This is not a multi map, test the key hasn't been inserted yet + JPH_ASSERT(Find(inKey, inKeyHash) == nullptr); + + // Calculate total size + uint size = sizeof(KeyValue) + inExtraBytes; + + // Get the write offset for this key value pair + uint32 write_offset; + if (!ioContext.Allocate(size, alignof(KeyValue), write_offset)) + return nullptr; + +#ifdef JPH_ENABLE_ASSERTS + // Increment amount of entries in map + mNumKeyValues.fetch_add(1, memory_order_relaxed); +#endif // JPH_ENABLE_ASSERTS + + // Construct the key/value pair + KeyValue *kv = mAllocator.template FromOffset(write_offset); + JPH_ASSERT(intptr_t(kv) % alignof(KeyValue) == 0); +#ifdef JPH_DEBUG + memset(kv, 0xcd, size); +#endif + kv->mKey = inKey; + new (&kv->mValue) Value(std::forward(inConstructorParams)...); + + // Get the offset to the first object from the bucket with corresponding hash + atomic &offset = mBuckets[inKeyHash & (mNumBuckets - 1)]; + + // Add this entry as the first element in the linked list + uint32 old_offset = offset.load(memory_order_relaxed); + for (;;) + { + kv->mNextOffset = old_offset; + if (offset.compare_exchange_weak(old_offset, write_offset, memory_order_release)) + break; + } + + return kv; +} + +template +inline const typename LockFreeHashMap::KeyValue *LockFreeHashMap::Find(const Key &inKey, uint64 inKeyHash) const +{ + // Get the offset to the keyvalue object from the bucket with corresponding hash + uint32 offset = mBuckets[inKeyHash & (mNumBuckets - 1)].load(memory_order_acquire); + while (offset != cInvalidHandle) + { + // Loop through linked list of values until the right one is found + const KeyValue *kv = mAllocator.template FromOffset(offset); + if (kv->mKey == inKey) + return kv; + offset = kv->mNextOffset; + } + + // Not found + return nullptr; +} + +template +inline uint32 LockFreeHashMap::ToHandle(const KeyValue *inKeyValue) const +{ + return mAllocator.ToOffset(inKeyValue); +} + +template +inline const typename LockFreeHashMap::KeyValue *LockFreeHashMap::FromHandle(uint32 inHandle) const +{ + return mAllocator.template FromOffset(inHandle); +} + +template +inline void LockFreeHashMap::GetAllKeyValues(Array &outAll) const +{ + for (const atomic *bucket = mBuckets; bucket < mBuckets + mNumBuckets; ++bucket) + { + uint32 offset = *bucket; + while (offset != cInvalidHandle) + { + const KeyValue *kv = mAllocator.template FromOffset(offset); + outAll.push_back(kv); + offset = kv->mNextOffset; + } + } +} + +template +typename LockFreeHashMap::Iterator LockFreeHashMap::begin() +{ + // Start with the first bucket + Iterator it { this, 0, mBuckets[0] }; + + // If it doesn't contain a valid entry, use the ++ operator to find the first valid entry + if (it.mOffset == cInvalidHandle) + ++it; + + return it; +} + +template +typename LockFreeHashMap::Iterator LockFreeHashMap::end() +{ + return { this, mNumBuckets, cInvalidHandle }; +} + +template +typename LockFreeHashMap::KeyValue &LockFreeHashMap::Iterator::operator* () +{ + JPH_ASSERT(mOffset != cInvalidHandle); + + return *mMap->mAllocator.template FromOffset(mOffset); +} + +template +typename LockFreeHashMap::Iterator &LockFreeHashMap::Iterator::operator++ () +{ + JPH_ASSERT(mBucket < mMap->mNumBuckets); + + // Find the next key value in this bucket + if (mOffset != cInvalidHandle) + { + const KeyValue *kv = mMap->mAllocator.template FromOffset(mOffset); + mOffset = kv->mNextOffset; + if (mOffset != cInvalidHandle) + return *this; + } + + // Loop over next buckets + for (;;) + { + // Next bucket + ++mBucket; + if (mBucket >= mMap->mNumBuckets) + return *this; + + // Fetch the first entry in the bucket + mOffset = mMap->mBuckets[mBucket]; + if (mOffset != cInvalidHandle) + return *this; + } +} + +#ifdef JPH_DEBUG + +template +void LockFreeHashMap::TraceStats() const +{ + const int cMaxPerBucket = 256; + + int max_objects_per_bucket = 0; + int num_objects = 0; + int histogram[cMaxPerBucket]; + for (int i = 0; i < cMaxPerBucket; ++i) + histogram[i] = 0; + + for (atomic *bucket = mBuckets, *bucket_end = mBuckets + mNumBuckets; bucket < bucket_end; ++bucket) + { + int objects_in_bucket = 0; + uint32 offset = *bucket; + while (offset != cInvalidHandle) + { + const KeyValue *kv = mAllocator.template FromOffset(offset); + offset = kv->mNextOffset; + ++objects_in_bucket; + ++num_objects; + } + max_objects_per_bucket = max(objects_in_bucket, max_objects_per_bucket); + histogram[min(objects_in_bucket, cMaxPerBucket - 1)]++; + } + + Trace("max_objects_per_bucket = %d, num_buckets = %u, num_objects = %d", max_objects_per_bucket, mNumBuckets, num_objects); + + for (int i = 0; i < cMaxPerBucket; ++i) + if (histogram[i] != 0) + Trace("%d: %d", i, histogram[i]); +} + +#endif + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Memory.cpp b/WickedEngine/Jolt/Core/Memory.cpp new file mode 100644 index 000000000..d500fc05a --- /dev/null +++ b/WickedEngine/Jolt/Core/Memory.cpp @@ -0,0 +1,85 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DISABLE_CUSTOM_ALLOCATOR + #define JPH_ALLOC_FN(x) x + #define JPH_ALLOC_SCOPE +#else + #define JPH_ALLOC_FN(x) x##Impl + #define JPH_ALLOC_SCOPE static +#endif + +JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(Allocate)(size_t inSize) +{ + JPH_ASSERT(inSize > 0); + return malloc(inSize); +} + +JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(Reallocate)(void *inBlock, size_t inSize) +{ + JPH_ASSERT(inSize > 0); + return realloc(inBlock, inSize); +} + +JPH_ALLOC_SCOPE void JPH_ALLOC_FN(Free)(void *inBlock) +{ + free(inBlock); +} + +JPH_ALLOC_SCOPE void *JPH_ALLOC_FN(AlignedAllocate)(size_t inSize, size_t inAlignment) +{ + JPH_ASSERT(inSize > 0 && inAlignment > 0); + +#if defined(JPH_PLATFORM_WINDOWS) + // Microsoft doesn't implement posix_memalign + return _aligned_malloc(inSize, inAlignment); +#else + void *block = nullptr; + JPH_SUPPRESS_WARNING_PUSH + JPH_GCC_SUPPRESS_WARNING("-Wunused-result") + JPH_CLANG_SUPPRESS_WARNING("-Wunused-result") + posix_memalign(&block, inAlignment, inSize); + JPH_SUPPRESS_WARNING_POP + return block; +#endif +} + +JPH_ALLOC_SCOPE void JPH_ALLOC_FN(AlignedFree)(void *inBlock) +{ +#if defined(JPH_PLATFORM_WINDOWS) + _aligned_free(inBlock); +#else + free(inBlock); +#endif +} + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +AllocateFunction Allocate = nullptr; +ReallocateFunction Reallocate = nullptr; +FreeFunction Free = nullptr; +AlignedAllocateFunction AlignedAllocate = nullptr; +AlignedFreeFunction AlignedFree = nullptr; + +void RegisterDefaultAllocator() +{ + Allocate = AllocateImpl; + Reallocate = ReallocateImpl; + Free = FreeImpl; + AlignedAllocate = AlignedAllocateImpl; + AlignedFree = AlignedFreeImpl; +} + +#endif // JPH_DISABLE_CUSTOM_ALLOCATOR + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Memory.h b/WickedEngine/Jolt/Core/Memory.h new file mode 100644 index 000000000..cae9f09c1 --- /dev/null +++ b/WickedEngine/Jolt/Core/Memory.h @@ -0,0 +1,58 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +// Normal memory allocation, must be at least 8 byte aligned on 32 bit platform and 16 byte aligned on 64 bit platform +using AllocateFunction = void *(*)(size_t inSize); +using ReallocateFunction = void *(*)(void *inBlock, size_t inSize); +using FreeFunction = void (*)(void *inBlock); + +// Aligned memory allocation +using AlignedAllocateFunction = void *(*)(size_t inSize, size_t inAlignment); +using AlignedFreeFunction = void (*)(void *inBlock); + +// User defined allocation / free functions +JPH_EXPORT extern AllocateFunction Allocate; +JPH_EXPORT extern ReallocateFunction Reallocate; +JPH_EXPORT extern FreeFunction Free; +JPH_EXPORT extern AlignedAllocateFunction AlignedAllocate; +JPH_EXPORT extern AlignedFreeFunction AlignedFree; + +/// Register platform default allocation / free functions +JPH_EXPORT void RegisterDefaultAllocator(); + +/// Macro to override the new and delete functions +#define JPH_OVERRIDE_NEW_DELETE \ + JPH_INLINE void *operator new (size_t inCount) { return JPH::Allocate(inCount); } \ + JPH_INLINE void operator delete (void *inPointer) noexcept { JPH::Free(inPointer); } \ + JPH_INLINE void *operator new[] (size_t inCount) { return JPH::Allocate(inCount); } \ + JPH_INLINE void operator delete[] (void *inPointer) noexcept { JPH::Free(inPointer); } \ + JPH_INLINE void *operator new (size_t inCount, std::align_val_t inAlignment) { return JPH::AlignedAllocate(inCount, static_cast(inAlignment)); } \ + JPH_INLINE void operator delete (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } \ + JPH_INLINE void *operator new[] (size_t inCount, std::align_val_t inAlignment) { return JPH::AlignedAllocate(inCount, static_cast(inAlignment)); } \ + JPH_INLINE void operator delete[] (void *inPointer, [[maybe_unused]] std::align_val_t inAlignment) noexcept { JPH::AlignedFree(inPointer); } + +#else + +// Directly define the allocation functions +JPH_EXPORT void *Allocate(size_t inSize); +JPH_EXPORT void *Reallocate(void *inBlock, size_t inSize); +JPH_EXPORT void Free(void *inBlock); +JPH_EXPORT void *AlignedAllocate(size_t inSize, size_t inAlignment); +JPH_EXPORT void AlignedFree(void *inBlock); + +// Don't implement allocator registering +inline void RegisterDefaultAllocator() { } + +// Don't override new/delete +#define JPH_OVERRIDE_NEW_DELETE + +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Mutex.h b/WickedEngine/Jolt/Core/Mutex.h new file mode 100644 index 000000000..6969aa45d --- /dev/null +++ b/WickedEngine/Jolt/Core/Mutex.h @@ -0,0 +1,223 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::mutex; +using std::shared_mutex; +using std::thread; +using std::lock_guard; +using std::shared_lock; +using std::unique_lock; + +#ifdef JPH_PLATFORM_BLUE + +// On Platform Blue the mutex class is not very fast so we implement it using the official APIs +class MutexBase : public NonCopyable +{ +public: + MutexBase() + { + JPH_PLATFORM_BLUE_MUTEX_INIT(mMutex); + } + + ~MutexBase() + { + JPH_PLATFORM_BLUE_MUTEX_DESTROY(mMutex); + } + + inline bool try_lock() + { + return JPH_PLATFORM_BLUE_MUTEX_TRYLOCK(mMutex); + } + + inline void lock() + { + JPH_PLATFORM_BLUE_MUTEX_LOCK(mMutex); + } + + inline void unlock() + { + JPH_PLATFORM_BLUE_MUTEX_UNLOCK(mMutex); + } + +private: + JPH_PLATFORM_BLUE_MUTEX mMutex; +}; + +// On Platform Blue the shared_mutex class is not very fast so we implement it using the official APIs +class SharedMutexBase : public NonCopyable +{ +public: + SharedMutexBase() + { + JPH_PLATFORM_BLUE_RWLOCK_INIT(mRWLock); + } + + ~SharedMutexBase() + { + JPH_PLATFORM_BLUE_RWLOCK_DESTROY(mRWLock); + } + + inline bool try_lock() + { + return JPH_PLATFORM_BLUE_RWLOCK_TRYWLOCK(mRWLock); + } + + inline bool try_lock_shared() + { + return JPH_PLATFORM_BLUE_RWLOCK_TRYRLOCK(mRWLock); + } + + inline void lock() + { + JPH_PLATFORM_BLUE_RWLOCK_WLOCK(mRWLock); + } + + inline void unlock() + { + JPH_PLATFORM_BLUE_RWLOCK_WUNLOCK(mRWLock); + } + + inline void lock_shared() + { + JPH_PLATFORM_BLUE_RWLOCK_RLOCK(mRWLock); + } + + inline void unlock_shared() + { + JPH_PLATFORM_BLUE_RWLOCK_RUNLOCK(mRWLock); + } + +private: + JPH_PLATFORM_BLUE_RWLOCK mRWLock; +}; + +#else + +// On other platforms just use the STL implementation +using MutexBase = mutex; +using SharedMutexBase = shared_mutex; + +#endif // JPH_PLATFORM_BLUE + +#if defined(JPH_ENABLE_ASSERTS) || defined(JPH_PROFILE_ENABLED) || defined(JPH_EXTERNAL_PROFILE) + +/// Very simple wrapper around MutexBase which tracks lock contention in the profiler +/// and asserts that locks/unlocks take place on the same thread +class Mutex : public MutexBase +{ +public: + inline bool try_lock() + { + JPH_ASSERT(mLockedThreadID != std::this_thread::get_id()); + if (MutexBase::try_lock()) + { + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + return true; + } + return false; + } + + inline void lock() + { + if (!try_lock()) + { + JPH_PROFILE("Lock", 0xff00ffff); + MutexBase::lock(); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + } + } + + inline void unlock() + { + JPH_ASSERT(mLockedThreadID == std::this_thread::get_id()); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = thread::id();) + MutexBase::unlock(); + } + +#ifdef JPH_ENABLE_ASSERTS + inline bool is_locked() + { + return mLockedThreadID != thread::id(); + } +#endif // JPH_ENABLE_ASSERTS + +private: + JPH_IF_ENABLE_ASSERTS(thread::id mLockedThreadID;) +}; + +/// Very simple wrapper around SharedMutexBase which tracks lock contention in the profiler +/// and asserts that locks/unlocks take place on the same thread +class SharedMutex : public SharedMutexBase +{ +public: + inline bool try_lock() + { + JPH_ASSERT(mLockedThreadID != std::this_thread::get_id()); + if (SharedMutexBase::try_lock()) + { + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + return true; + } + return false; + } + + inline void lock() + { + if (!try_lock()) + { + JPH_PROFILE("WLock", 0xff00ffff); + SharedMutexBase::lock(); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = std::this_thread::get_id();) + } + } + + inline void unlock() + { + JPH_ASSERT(mLockedThreadID == std::this_thread::get_id()); + JPH_IF_ENABLE_ASSERTS(mLockedThreadID = thread::id();) + SharedMutexBase::unlock(); + } + +#ifdef JPH_ENABLE_ASSERTS + inline bool is_locked() + { + return mLockedThreadID != thread::id(); + } +#endif // JPH_ENABLE_ASSERTS + + inline void lock_shared() + { + if (!try_lock_shared()) + { + JPH_PROFILE("RLock", 0xff00ffff); + SharedMutexBase::lock_shared(); + } + } + +private: + JPH_IF_ENABLE_ASSERTS(thread::id mLockedThreadID;) +}; + +#else + +using Mutex = MutexBase; +using SharedMutex = SharedMutexBase; + +#endif + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/MutexArray.h b/WickedEngine/Jolt/Core/MutexArray.h new file mode 100644 index 000000000..f8fdd8279 --- /dev/null +++ b/WickedEngine/Jolt/Core/MutexArray.h @@ -0,0 +1,98 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A mutex array protects a number of resources with a limited amount of mutexes. +/// It uses hashing to find the mutex of a particular object. +/// The idea is that if the amount of threads is much smaller than the amount of mutexes +/// that there is a relatively small chance that two different objects map to the same mutex. +template +class MutexArray : public NonCopyable +{ +public: + /// Constructor, constructs an empty mutex array that you need to initialize with Init() + MutexArray() = default; + + /// Constructor, constructs an array with inNumMutexes entries + explicit MutexArray(uint inNumMutexes) { Init(inNumMutexes); } + + /// Destructor + ~MutexArray() { delete [] mMutexStorage; } + + /// Initialization + /// @param inNumMutexes The amount of mutexes to allocate + void Init(uint inNumMutexes) + { + JPH_ASSERT(mMutexStorage == nullptr); + JPH_ASSERT(inNumMutexes > 0 && IsPowerOf2(inNumMutexes)); + + mMutexStorage = new MutexStorage[inNumMutexes]; + mNumMutexes = inNumMutexes; + } + + /// Get the number of mutexes that were allocated + inline uint GetNumMutexes() const + { + return mNumMutexes; + } + + /// Convert an object index to a mutex index + inline uint32 GetMutexIndex(uint32 inObjectIndex) const + { + std::hash hasher; + return hasher(inObjectIndex) & (mNumMutexes - 1); + } + + /// Get the mutex belonging to a certain object by index + inline MutexType & GetMutexByObjectIndex(uint32 inObjectIndex) + { + return mMutexStorage[GetMutexIndex(inObjectIndex)].mMutex; + } + + /// Get a mutex by index in the array + inline MutexType & GetMutexByIndex(uint32 inMutexIndex) + { + return mMutexStorage[inMutexIndex].mMutex; + } + + /// Lock all mutexes + void LockAll() + { + JPH_PROFILE_FUNCTION(); + + MutexStorage *end = mMutexStorage + mNumMutexes; + for (MutexStorage *m = mMutexStorage; m < end; ++m) + m->mMutex.lock(); + } + + /// Unlock all mutexes + void UnlockAll() + { + JPH_PROFILE_FUNCTION(); + + MutexStorage *end = mMutexStorage + mNumMutexes; + for (MutexStorage *m = mMutexStorage; m < end; ++m) + m->mMutex.unlock(); + } + +private: + /// Align the mutex to a cache line to ensure there is no false sharing (this is platform dependent, we do this to be safe) + struct alignas(JPH_CACHE_LINE_SIZE) MutexStorage + { + JPH_OVERRIDE_NEW_DELETE + + MutexType mMutex; + }; + + MutexStorage * mMutexStorage = nullptr; + uint mNumMutexes = 0; +}; + +JPH_NAMESPACE_END + diff --git a/WickedEngine/Jolt/Core/NonCopyable.h b/WickedEngine/Jolt/Core/NonCopyable.h new file mode 100644 index 000000000..18b431e92 --- /dev/null +++ b/WickedEngine/Jolt/Core/NonCopyable.h @@ -0,0 +1,18 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that makes another class non-copyable. Usage: Inherit from NonCopyable. +class JPH_EXPORT NonCopyable +{ +public: + NonCopyable() = default; + NonCopyable(const NonCopyable &) = delete; + void operator = (const NonCopyable &) = delete; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Profiler.cpp b/WickedEngine/Jolt/Core/Profiler.cpp new file mode 100644 index 000000000..839939966 --- /dev/null +++ b/WickedEngine/Jolt/Core/Profiler.cpp @@ -0,0 +1,346 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#ifdef JPH_PROFILE_ENABLED + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Profiler +////////////////////////////////////////////////////////////////////////////////////////// + +Profiler *Profiler::sInstance = nullptr; + +#ifdef JPH_SHARED_LIBRARY + static thread_local ProfileThread *sInstance = nullptr; + + ProfileThread *ProfileThread::sGetInstance() + { + return sInstance; + } + + void ProfileThread::sSetInstance(ProfileThread *inInstance) + { + sInstance = inInstance; + } +#else + thread_local ProfileThread *ProfileThread::sInstance = nullptr; +#endif + +bool ProfileMeasurement::sOutOfSamplesReported = false; + +void Profiler::UpdateReferenceTime() +{ + mReferenceTick = GetProcessorTickCount(); + mReferenceTime = std::chrono::high_resolution_clock::now(); +} + +uint64 Profiler::GetProcessorTicksPerSecond() const +{ + uint64 ticks = GetProcessorTickCount(); + std::chrono::high_resolution_clock::time_point time = std::chrono::high_resolution_clock::now(); + + return (ticks - mReferenceTick) * 1000000000ULL / std::chrono::duration_cast(time - mReferenceTime).count(); +} + +void Profiler::NextFrame() +{ + std::lock_guard lock(mLock); + + if (mDump) + { + DumpInternal(); + mDump = false; + } + + for (ProfileThread *t : mThreads) + t->mCurrentSample = 0; + + UpdateReferenceTime(); +} + +void Profiler::Dump(const string_view &inTag) +{ + mDump = true; + mDumpTag = inTag; +} + +void Profiler::AddThread(ProfileThread *inThread) +{ + std::lock_guard lock(mLock); + + mThreads.push_back(inThread); +} + +void Profiler::RemoveThread(ProfileThread *inThread) +{ + std::lock_guard lock(mLock); + + Array::iterator i = std::find(mThreads.begin(), mThreads.end(), inThread); + JPH_ASSERT(i != mThreads.end()); + mThreads.erase(i); +} + +void Profiler::sAggregate(int inDepth, uint32 inColor, ProfileSample *&ioSample, const ProfileSample *inEnd, Aggregators &ioAggregators, KeyToAggregator &ioKeyToAggregator) +{ + // Store depth + ioSample->mDepth = uint8(min(255, inDepth)); + + // Update color + if (ioSample->mColor == 0) + ioSample->mColor = inColor; + else + inColor = ioSample->mColor; + + // Start accumulating totals + uint64 cycles_this_with_children = ioSample->mEndCycle - ioSample->mStartCycle; + + // Loop over following samples until we find a sample that starts on or after our end + ProfileSample *sample; + for (sample = ioSample + 1; sample < inEnd && sample->mStartCycle < ioSample->mEndCycle; ++sample) + { + JPH_ASSERT(sample[-1].mStartCycle <= sample->mStartCycle); + JPH_ASSERT(sample->mStartCycle >= ioSample->mStartCycle); + JPH_ASSERT(sample->mEndCycle <= ioSample->mEndCycle); + + // Recurse and skip over the children of this child + sAggregate(inDepth + 1, inColor, sample, inEnd, ioAggregators, ioKeyToAggregator); + } + + // Find the aggregator for this name / filename pair + Aggregator *aggregator; + KeyToAggregator::iterator aggregator_idx = ioKeyToAggregator.find(ioSample->mName); + if (aggregator_idx == ioKeyToAggregator.end()) + { + // Not found, add to map and insert in array + ioKeyToAggregator.try_emplace(ioSample->mName, ioAggregators.size()); + ioAggregators.emplace_back(ioSample->mName); + aggregator = &ioAggregators.back(); + } + else + { + // Found + aggregator = &ioAggregators[aggregator_idx->second]; + } + + // Add the measurement to the aggregator + aggregator->AccumulateMeasurement(cycles_this_with_children); + + // Update ioSample to the last child of ioSample + JPH_ASSERT(sample[-1].mStartCycle <= ioSample->mEndCycle); + JPH_ASSERT(sample >= inEnd || sample->mStartCycle >= ioSample->mEndCycle); + ioSample = sample - 1; +} + +void Profiler::DumpInternal() +{ + // Freeze data from threads + // Note that this is not completely thread safe: As a profile sample is added mCurrentSample is incremented + // but the data is not written until the sample finishes. So if we dump the profile information while + // some other thread is running, we may get some garbage information from the previous frame + Threads threads; + for (ProfileThread *t : mThreads) + threads.push_back({ t->mThreadName, t->mSamples, t->mSamples + t->mCurrentSample }); + + // Shift all samples so that the first sample is at zero + uint64 min_cycle = 0xffffffffffffffffUL; + for (const ThreadSamples &t : threads) + if (t.mSamplesBegin < t.mSamplesEnd) + min_cycle = min(min_cycle, t.mSamplesBegin[0].mStartCycle); + for (const ThreadSamples &t : threads) + for (ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s) + { + s->mStartCycle -= min_cycle; + s->mEndCycle -= min_cycle; + } + + // Determine tag of this profile + String tag; + if (mDumpTag.empty()) + { + // Next sequence number + static int number = 0; + ++number; + tag = ConvertToString(number); + } + else + { + // Take provided tag + tag = mDumpTag; + mDumpTag.clear(); + } + + // Aggregate data across threads + Aggregators aggregators; + KeyToAggregator key_to_aggregators; + for (const ThreadSamples &t : threads) + for (ProfileSample *s = t.mSamplesBegin, *end = t.mSamplesEnd; s < end; ++s) + sAggregate(0, Color::sGetDistinctColor(0).GetUInt32(), s, end, aggregators, key_to_aggregators); + + // Dump as chart + DumpChart(tag.c_str(), threads, key_to_aggregators, aggregators); +} + +static String sHTMLEncode(const char *inString) +{ + String str(inString); + StringReplace(str, "<", "<"); + StringReplace(str, ">", ">"); + return str; +} + +void Profiler::DumpChart(const char *inTag, const Threads &inThreads, const KeyToAggregator &inKeyToAggregators, const Aggregators &inAggregators) +{ + // Open file + std::ofstream f; + f.open(StringFormat("profile_chart_%s.html", inTag).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + // Write header + f << R"( + + + Profile Chart + + + + + + + +
    + +)"; +} + +JPH_NAMESPACE_END + +#endif // JPH_PROFILE_ENABLED diff --git a/WickedEngine/Jolt/Core/Profiler.h b/WickedEngine/Jolt/Core/Profiler.h new file mode 100644 index 000000000..bbe641821 --- /dev/null +++ b/WickedEngine/Jolt/Core/Profiler.h @@ -0,0 +1,284 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#include +#include +#include + +#if defined(JPH_EXTERNAL_PROFILE) + +JPH_NAMESPACE_BEGIN + +/// Create this class on the stack to start sampling timing information of a particular scope. +/// +/// Left unimplemented intentionally. Needs to be implemented by the user of the library. +/// On construction a measurement should start, on destruction it should be stopped. +class alignas(16) ExternalProfileMeasurement : public NonCopyable +{ +public: + /// Constructor + ExternalProfileMeasurement(const char *inName, uint32 inColor = 0); + ~ExternalProfileMeasurement(); + +private: + uint8 mUserData[64]; +}; + +JPH_NAMESPACE_END + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to do the actual profiling +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +// Dummy implementations +#define JPH_PROFILE_THREAD_START(name) +#define JPH_PROFILE_THREAD_END() +#define JPH_PROFILE_NEXTFRAME() +#define JPH_PROFILE_DUMP(...) + +// Scope profiling measurement +#define JPH_PROFILE_TAG2(line) profile##line +#define JPH_PROFILE_TAG(line) JPH_PROFILE_TAG2(line) + +/// Macro to collect profiling information. +/// +/// Usage: +/// +/// { +/// JPH_PROFILE("Operation"); +/// do operation; +/// } +/// +#define JPH_PROFILE(...) ExternalProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__) + +// Scope profiling for function +#define JPH_PROFILE_FUNCTION() JPH_PROFILE(JPH_FUNCTION_NAME) + +JPH_SUPPRESS_WARNING_POP + +#elif defined(JPH_PROFILE_ENABLED) + +JPH_NAMESPACE_BEGIN + +class ProfileSample; +class ProfileThread; + +/// Singleton class for managing profiling information +class JPH_EXPORT Profiler : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Profiler() { UpdateReferenceTime(); } + + /// Increments the frame counter to provide statistics per frame + void NextFrame(); + + /// Dump profiling statistics at the start of the next frame + /// @param inTag If not empty, this overrides the auto incrementing number in the filename of the dump file + void Dump(const string_view &inTag = string_view()); + + /// Add a thread to be instrumented + void AddThread(ProfileThread *inThread); + + /// Remove a thread from being instrumented + void RemoveThread(ProfileThread *inThread); + + /// Singleton instance + static Profiler * sInstance; + +private: + /// Helper class to freeze ProfileSamples per thread while processing them + struct ThreadSamples + { + String mThreadName; + ProfileSample * mSamplesBegin; + ProfileSample * mSamplesEnd; + }; + + /// Helper class to aggregate ProfileSamples + class Aggregator + { + public: + /// Constructor + Aggregator(const char *inName) : mName(inName) { } + + /// Accumulate results for a measurement + void AccumulateMeasurement(uint64 inCyclesInCallWithChildren) + { + mCallCounter++; + mTotalCyclesInCallWithChildren += inCyclesInCallWithChildren; + mMinCyclesInCallWithChildren = min(inCyclesInCallWithChildren, mMinCyclesInCallWithChildren); + mMaxCyclesInCallWithChildren = max(inCyclesInCallWithChildren, mMaxCyclesInCallWithChildren); + } + + /// Sort descending by total cycles + bool operator < (const Aggregator &inRHS) const + { + return mTotalCyclesInCallWithChildren > inRHS.mTotalCyclesInCallWithChildren; + } + + /// Identification + const char * mName; ///< User defined name of this item + + /// Statistics + uint32 mCallCounter = 0; ///< Number of times AccumulateMeasurement was called + uint64 mTotalCyclesInCallWithChildren = 0; ///< Total amount of cycles spent in this scope + uint64 mMinCyclesInCallWithChildren = 0xffffffffffffffffUL; ///< Minimum amount of cycles spent per call + uint64 mMaxCyclesInCallWithChildren = 0; ///< Maximum amount of cycles spent per call + }; + + using Threads = Array; + using Aggregators = Array; + using KeyToAggregator = UnorderedMap; + + /// Helper function to aggregate profile sample data + static void sAggregate(int inDepth, uint32 inColor, ProfileSample *&ioSample, const ProfileSample *inEnd, Aggregators &ioAggregators, KeyToAggregator &ioKeyToAggregator); + + /// We measure the amount of ticks per second, this function resets the reference time point + void UpdateReferenceTime(); + + /// Get the amount of ticks per second, note that this number will never be fully accurate as the amount of ticks per second may vary with CPU load, so this number is only to be used to give an indication of time for profiling purposes + uint64 GetProcessorTicksPerSecond() const; + + /// Dump profiling statistics + void DumpInternal(); + void DumpChart(const char *inTag, const Threads &inThreads, const KeyToAggregator &inKeyToAggregators, const Aggregators &inAggregators); + + std::mutex mLock; ///< Lock that protects mThreads + uint64 mReferenceTick; ///< Tick count at the start of the frame + std::chrono::high_resolution_clock::time_point mReferenceTime; ///< Time at the start of the frame + Array mThreads; ///< List of all active threads + bool mDump = false; ///< When true, the samples are dumped next frame + String mDumpTag; ///< When not empty, this overrides the auto incrementing number of the dump filename +}; + +// Class that contains the information of a single scoped measurement +class alignas(16) JPH_EXPORT_GCC_BUG_WORKAROUND ProfileSample : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + const char * mName; ///< User defined name of this item + uint32 mColor; ///< Color to use for this sample + uint8 mDepth; ///< Calculated depth + uint8 mUnused[3]; + uint64 mStartCycle; ///< Cycle counter at start of measurement + uint64 mEndCycle; ///< Cycle counter at end of measurement +}; + +/// Collects all samples of a single thread +class ProfileThread : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + inline ProfileThread(const string_view &inThreadName); + inline ~ProfileThread(); + + static const uint cMaxSamples = 65536; + + String mThreadName; ///< Name of the thread that we're collecting information for + ProfileSample mSamples[cMaxSamples]; ///< Buffer of samples + uint mCurrentSample = 0; ///< Next position to write a sample to + +#ifdef JPH_SHARED_LIBRARY + JPH_EXPORT static void sSetInstance(ProfileThread *inInstance); + JPH_EXPORT static ProfileThread *sGetInstance(); +#else + static inline void sSetInstance(ProfileThread *inInstance) { sInstance = inInstance; } + static inline ProfileThread *sGetInstance() { return sInstance; } + +private: + static thread_local ProfileThread *sInstance; +#endif +}; + +/// Create this class on the stack to start sampling timing information of a particular scope +class JPH_EXPORT ProfileMeasurement : public NonCopyable +{ +public: + /// Constructor + inline ProfileMeasurement(const char *inName, uint32 inColor = 0); + inline ~ProfileMeasurement(); + +private: + ProfileSample * mSample; + ProfileSample mTemp; + + static bool sOutOfSamplesReported; +}; + +JPH_NAMESPACE_END + +#include "Profiler.inl" + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to do the actual profiling +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +/// Start instrumenting program +#define JPH_PROFILE_START(name) do { Profiler::sInstance = new Profiler; JPH_PROFILE_THREAD_START(name); } while (false) + +/// End instrumenting program +#define JPH_PROFILE_END() do { JPH_PROFILE_THREAD_END(); delete Profiler::sInstance; Profiler::sInstance = nullptr; } while (false) + +/// Start instrumenting a thread +#define JPH_PROFILE_THREAD_START(name) do { if (Profiler::sInstance) ProfileThread::sSetInstance(new ProfileThread(name)); } while (false) + +/// End instrumenting a thread +#define JPH_PROFILE_THREAD_END() do { delete ProfileThread::sGetInstance(); ProfileThread::sSetInstance(nullptr); } while (false) + +/// Scope profiling measurement +#define JPH_PROFILE_TAG2(line) profile##line +#define JPH_PROFILE_TAG(line) JPH_PROFILE_TAG2(line) +#define JPH_PROFILE(...) ProfileMeasurement JPH_PROFILE_TAG(__LINE__)(__VA_ARGS__) + +/// Scope profiling for function +#define JPH_PROFILE_FUNCTION() JPH_PROFILE(JPH_FUNCTION_NAME) + +/// Update frame counter +#define JPH_PROFILE_NEXTFRAME() Profiler::sInstance->NextFrame() + +/// Dump profiling info +#define JPH_PROFILE_DUMP(...) Profiler::sInstance->Dump(__VA_ARGS__) + +JPH_SUPPRESS_WARNING_POP + +#else + +////////////////////////////////////////////////////////////////////////////////////////// +// Dummy profiling instructions +////////////////////////////////////////////////////////////////////////////////////////// + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +#define JPH_PROFILE_START(name) +#define JPH_PROFILE_END() +#define JPH_PROFILE_THREAD_START(name) +#define JPH_PROFILE_THREAD_END() +#define JPH_PROFILE(...) +#define JPH_PROFILE_FUNCTION() +#define JPH_PROFILE_NEXTFRAME() +#define JPH_PROFILE_DUMP(...) + +JPH_SUPPRESS_WARNING_POP + +#endif diff --git a/WickedEngine/Jolt/Core/Profiler.inl b/WickedEngine/Jolt/Core/Profiler.inl new file mode 100644 index 000000000..cc429ba51 --- /dev/null +++ b/WickedEngine/Jolt/Core/Profiler.inl @@ -0,0 +1,89 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// ProfileThread +////////////////////////////////////////////////////////////////////////////////////////// + +ProfileThread::ProfileThread(const string_view &inThreadName) : + mThreadName(inThreadName) +{ + Profiler::sInstance->AddThread(this); +} + +ProfileThread::~ProfileThread() +{ + Profiler::sInstance->RemoveThread(this); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// ProfileMeasurement +////////////////////////////////////////////////////////////////////////////////////////// + +ProfileMeasurement::ProfileMeasurement(const char *inName, uint32 inColor) +{ + ProfileThread *current_thread = ProfileThread::sGetInstance(); + if (current_thread == nullptr) + { + // Thread not instrumented + mSample = nullptr; + } + else if (current_thread->mCurrentSample < ProfileThread::cMaxSamples) + { + // Get pointer to write data to + mSample = ¤t_thread->mSamples[current_thread->mCurrentSample++]; + + // Start constructing sample (will end up on stack) + mTemp.mName = inName; + mTemp.mColor = inColor; + + // Collect start sample last + mTemp.mStartCycle = GetProcessorTickCount(); + } + else + { + // Out of samples + if (!sOutOfSamplesReported) + { + Trace("ProfileMeasurement: Too many samples, some data will be lost!"); + sOutOfSamplesReported = true; + } + mSample = nullptr; + } +} + +ProfileMeasurement::~ProfileMeasurement() +{ + if (mSample != nullptr) + { + // Finalize sample + mTemp.mEndCycle = GetProcessorTickCount(); + + // Write it to the memory buffer bypassing the cache + static_assert(sizeof(ProfileSample) == 32, "Assume 32 bytes"); + static_assert(alignof(ProfileSample) == 16, "Assume 16 byte alignment"); + #if defined(JPH_USE_SSE) + const __m128i *src = reinterpret_cast(&mTemp); + __m128i *dst = reinterpret_cast<__m128i *>(mSample); + __m128i val = _mm_loadu_si128(src); + _mm_stream_si128(dst, val); + val = _mm_loadu_si128(src + 1); + _mm_stream_si128(dst + 1, val); + #elif defined(JPH_USE_NEON) + const int *src = reinterpret_cast(&mTemp); + int *dst = reinterpret_cast(mSample); + int32x4_t val = vld1q_s32(src); + vst1q_s32(dst, val); + val = vld1q_s32(src + 4); + vst1q_s32(dst + 4, val); + #else + memcpy(mSample, &mTemp, sizeof(ProfileSample)); + #endif + mSample = nullptr; + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/QuickSort.h b/WickedEngine/Jolt/Core/QuickSort.h new file mode 100644 index 000000000..0b4a3ff9a --- /dev/null +++ b/WickedEngine/Jolt/Core/QuickSort.h @@ -0,0 +1,137 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper function for QuickSort, will move the pivot element to inMiddle. +template +inline void QuickSortMedianOfThree(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare) +{ + // This should be guaranteed because we switch over to insertion sort when there's 32 or less elements + JPH_ASSERT(inFirst != inMiddle && inMiddle != inLast); + + if (inCompare(*inMiddle, *inFirst)) + swap(*inFirst, *inMiddle); + + if (inCompare(*inLast, *inFirst)) + swap(*inFirst, *inLast); + + if (inCompare(*inLast, *inMiddle)) + swap(*inMiddle, *inLast); +} + +/// Helper function for QuickSort using the Ninther method, will move the pivot element to inMiddle. +template +inline void QuickSortNinther(Iterator inFirst, Iterator inMiddle, Iterator inLast, Compare inCompare) +{ + // Divide the range in 8 equal parts (this means there are 9 points) + auto diff = (inLast - inFirst) >> 3; + auto two_diff = diff << 1; + + // Median of first 3 points + Iterator mid1 = inFirst + diff; + QuickSortMedianOfThree(inFirst, mid1, inFirst + two_diff, inCompare); + + // Median of second 3 points + QuickSortMedianOfThree(inMiddle - diff, inMiddle, inMiddle + diff, inCompare); + + // Median of third 3 points + Iterator mid3 = inLast - diff; + QuickSortMedianOfThree(inLast - two_diff, mid3, inLast, inCompare); + + // Determine the median of the 3 medians + QuickSortMedianOfThree(mid1, inMiddle, mid3, inCompare); +} + +/// Implementation of the quick sort algorithm. The STL version implementation is not consistent across platforms. +template +inline void QuickSort(Iterator inBegin, Iterator inEnd, Compare inCompare) +{ + // Implementation based on https://en.wikipedia.org/wiki/Quicksort using Hoare's partition scheme + + // Loop so that we only need to do 1 recursive call instead of 2. + for (;;) + { + // If there's less than 2 elements we're done + auto num_elements = inEnd - inBegin; + if (num_elements < 2) + return; + + // Fall back to insertion sort if there are too few elements + if (num_elements <= 32) + { + InsertionSort(inBegin, inEnd, inCompare); + return; + } + + // Determine pivot + Iterator pivot_iterator = inBegin + ((num_elements - 1) >> 1); + QuickSortNinther(inBegin, pivot_iterator, inEnd - 1, inCompare); + auto pivot = *pivot_iterator; + + // Left and right iterators + Iterator i = inBegin; + Iterator j = inEnd; + + for (;;) + { + // Find the first element that is bigger than the pivot + while (inCompare(*i, pivot)) + i++; + + // Find the last element that is smaller than the pivot + do + --j; + while (inCompare(pivot, *j)); + + // If the two iterators crossed, we're done + if (i >= j) + break; + + // Swap the elements + swap(*i, *j); + + // Note that the first while loop in this function should + // have been do i++ while (...) but since we cannot decrement + // the iterator from inBegin we left that out, so we need to do + // it here. + ++i; + } + + // Include the middle element on the left side + j++; + + // Check which partition is smaller + if (j - inBegin < inEnd - j) + { + // Left side is smaller, recurse to left first + QuickSort(inBegin, j, inCompare); + + // Loop again with the right side to avoid a call + inBegin = j; + } + else + { + // Right side is smaller, recurse to right first + QuickSort(j, inEnd, inCompare); + + // Loop again with the left side to avoid a call + inEnd = j; + } + } +} + +/// Implementation of quick sort algorithm without comparator. +template +inline void QuickSort(Iterator inBegin, Iterator inEnd) +{ + std::less<> compare; + QuickSort(inBegin, inEnd, compare); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/RTTI.cpp b/WickedEngine/Jolt/Core/RTTI.cpp new file mode 100644 index 000000000..91235745a --- /dev/null +++ b/WickedEngine/Jolt/Core/RTTI.cpp @@ -0,0 +1,149 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// RTTI +////////////////////////////////////////////////////////////////////////////////////////// + +RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject) : + mName(inName), + mSize(inSize), + mCreate(inCreateObject), + mDestruct(inDestructObject) +{ + JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed"); +} + +RTTI::RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI) : + mName(inName), + mSize(inSize), + mCreate(inCreateObject), + mDestruct(inDestructObject) +{ + JPH_ASSERT(inDestructObject != nullptr, "Object cannot be destructed"); + + inCreateRTTI(*this); +} + +int RTTI::GetBaseClassCount() const +{ + return (int)mBaseClasses.size(); +} + +const RTTI *RTTI::GetBaseClass(int inIdx) const +{ + return mBaseClasses[inIdx].mRTTI; +} + +uint32 RTTI::GetHash() const +{ + // Perform diffusion step to get from 64 to 32 bits (see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function) + uint64 hash = HashString(mName); + return (uint32)(hash ^ (hash >> 32)); +} + +void *RTTI::CreateObject() const +{ + return IsAbstract()? nullptr : mCreate(); +} + +void RTTI::DestructObject(void *inObject) const +{ + mDestruct(inObject); +} + +void RTTI::AddBaseClass(const RTTI *inRTTI, int inOffset) +{ + JPH_ASSERT(inOffset >= 0 && inOffset < mSize, "Base class not contained in derived class"); + + // Add base class + BaseClass base; + base.mRTTI = inRTTI; + base.mOffset = inOffset; + mBaseClasses.push_back(base); + +#ifdef JPH_OBJECT_STREAM + // Add attributes of base class + for (const SerializableAttribute &a : inRTTI->mAttributes) + mAttributes.push_back(SerializableAttribute(a, inOffset)); +#endif // JPH_OBJECT_STREAM +} + +bool RTTI::operator == (const RTTI &inRHS) const +{ + // Compare addresses + if (this == &inRHS) + return true; + + // Check that the names differ (if that is the case we probably have two instances + // of the same attribute info across the program, probably the second is in a DLL) + JPH_ASSERT(strcmp(mName, inRHS.mName) != 0); + return false; +} + +bool RTTI::IsKindOf(const RTTI *inRTTI) const +{ + // Check if this is the same type + if (this == inRTTI) + return true; + + // Check all base classes + for (const BaseClass &b : mBaseClasses) + if (b.mRTTI->IsKindOf(inRTTI)) + return true; + + return false; +} + +const void *RTTI::CastTo(const void *inObject, const RTTI *inRTTI) const +{ + JPH_ASSERT(inObject != nullptr); + + // Check if this is the same type + if (this == inRTTI) + return inObject; + + // Check all base classes + for (const BaseClass &b : mBaseClasses) + { + // Cast the pointer to the base class + const void *casted = (const void *)(((const uint8 *)inObject) + b.mOffset); + + // Test base class + const void *rv = b.mRTTI->CastTo(casted, inRTTI); + if (rv != nullptr) + return rv; + } + + // Not possible to cast + return nullptr; +} + +#ifdef JPH_OBJECT_STREAM + +void RTTI::AddAttribute(const SerializableAttribute &inAttribute) +{ + mAttributes.push_back(inAttribute); +} + +int RTTI::GetAttributeCount() const +{ + return (int)mAttributes.size(); +} + +const SerializableAttribute &RTTI::GetAttribute(int inIdx) const +{ + return mAttributes[inIdx]; +} + +#endif // JPH_OBJECT_STREAM + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/RTTI.h b/WickedEngine/Jolt/Core/RTTI.h new file mode 100644 index 000000000..0de966049 --- /dev/null +++ b/WickedEngine/Jolt/Core/RTTI.h @@ -0,0 +1,436 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// RTTI +////////////////////////////////////////////////////////////////////////////////////////// + +/// Light weight runtime type information system. This way we don't need to turn +/// on the default RTTI system of the compiler (introducing a possible overhead for every +/// class) +/// +/// Notes: +/// - An extra virtual member function is added. This adds 8 bytes to the size of +/// an instance of the class (unless you are already using virtual functions). +/// +/// To use RTTI on a specific class use: +/// +/// Header file: +/// +/// class Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL_BASE(Foo) +/// } +/// +/// class Bar : public Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL(Bar) +/// }; +/// +/// Implementation file: +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(Foo) +/// { +/// } +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL(Bar) +/// { +/// JPH_ADD_BASE_CLASS(Bar, Foo) // Multiple inheritance is allowed, just do JPH_ADD_BASE_CLASS for every base class +/// } +/// +/// For abstract classes use: +/// +/// Header file: +/// +/// class Foo +/// { +/// JPH_DECLARE_RTTI_ABSTRACT_BASE(Foo) +/// +/// public: +/// virtual void AbstractFunction() = 0; +/// } +/// +/// class Bar : public Foo +/// { +/// JPH_DECLARE_RTTI_VIRTUAL(Bar) +/// +/// public: +/// virtual void AbstractFunction() { } // Function is now implemented so this class is no longer abstract +/// }; +/// +/// Implementation file: +/// +/// JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(Foo) +/// { +/// } +/// +/// JPH_IMPLEMENT_RTTI_VIRTUAL(Bar) +/// { +/// JPH_ADD_BASE_CLASS(Bar, Foo) +/// } +/// +/// Example of usage in a program: +/// +/// Foo *foo_ptr = new Foo; +/// Foo *bar_ptr = new Bar; +/// +/// IsType(foo_ptr, RTTI(Bar)) returns false +/// IsType(bar_ptr, RTTI(Bar)) returns true +/// +/// IsKindOf(foo_ptr, RTTI(Bar)) returns false +/// IsKindOf(bar_ptr, RTTI(Foo)) returns true +/// IsKindOf(bar_ptr, RTTI(Bar)) returns true +/// +/// StaticCast(foo_ptr) asserts and returns foo_ptr casted to pBar +/// StaticCast(bar_ptr) returns bar_ptr casted to pBar +/// +/// DynamicCast(foo_ptr) returns nullptr +/// DynamicCast(bar_ptr) returns bar_ptr casted to pBar +/// +/// Other feature of DynamicCast: +/// +/// class A { int data[5]; }; +/// class B { int data[7]; }; +/// class C : public A, public B { int data[9]; }; +/// +/// C *c = new C; +/// A *a = c; +/// +/// Note that: +/// +/// B *b = (B *)a; +/// +/// generates an invalid pointer, +/// +/// B *b = StaticCast(a); +/// +/// doesn't compile, and +/// +/// B *b = DynamicCast(a); +/// +/// does the correct cast +class JPH_EXPORT RTTI +{ +public: + /// Function to create an object + using pCreateObjectFunction = void *(*)(); + + /// Function to destroy an object + using pDestructObjectFunction = void (*)(void *inObject); + + /// Function to initialize the runtime type info structure + using pCreateRTTIFunction = void (*)(RTTI &inRTTI); + + /// Constructor + RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject); + RTTI(const char *inName, int inSize, pCreateObjectFunction inCreateObject, pDestructObjectFunction inDestructObject, pCreateRTTIFunction inCreateRTTI); + + // Properties + inline const char * GetName() const { return mName; } + void SetName(const char *inName) { mName = inName; } + inline int GetSize() const { return mSize; } + bool IsAbstract() const { return mCreate == nullptr || mDestruct == nullptr; } + int GetBaseClassCount() const; + const RTTI * GetBaseClass(int inIdx) const; + uint32 GetHash() const; + + /// Create an object of this type (returns nullptr if the object is abstract) + void * CreateObject() const; + + /// Destruct object of this type (does nothing if the object is abstract) + void DestructObject(void *inObject) const; + + /// Add base class + void AddBaseClass(const RTTI *inRTTI, int inOffset); + + /// Equality operators + bool operator == (const RTTI &inRHS) const; + bool operator != (const RTTI &inRHS) const { return !(*this == inRHS); } + + /// Test if this class is derived from class of type inRTTI + bool IsKindOf(const RTTI *inRTTI) const; + + /// Cast inObject of this type to object of type inRTTI, returns nullptr if the cast is unsuccessful + const void * CastTo(const void *inObject, const RTTI *inRTTI) const; + +#ifdef JPH_OBJECT_STREAM + /// Attribute access + void AddAttribute(const SerializableAttribute &inAttribute); + int GetAttributeCount() const; + const SerializableAttribute & GetAttribute(int inIdx) const; +#endif // JPH_OBJECT_STREAM + +protected: + /// Base class information + struct BaseClass + { + const RTTI * mRTTI; + int mOffset; + }; + + const char * mName; ///< Class name + int mSize; ///< Class size + StaticArray mBaseClasses; ///< Names of base classes + pCreateObjectFunction mCreate; ///< Pointer to a function that will create a new instance of this class + pDestructObjectFunction mDestruct; ///< Pointer to a function that will destruct an object of this class +#ifdef JPH_OBJECT_STREAM + StaticArray mAttributes; ///< All attributes of this class +#endif // JPH_OBJECT_STREAM +}; + +////////////////////////////////////////////////////////////////////////////////////////// +// Add run time type info to types that don't have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_RTTI_NON_VIRTUAL +#define JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name) \ +public: \ + JPH_OVERRIDE_NEW_DELETE \ + friend linkage RTTI * GetRTTIOfType(class_name *); \ + friend inline const RTTI * GetRTTI([[maybe_unused]] const class_name *inObject) { return GetRTTIOfType(static_cast(nullptr)); }\ + static void sCreateRTTI(RTTI &inRTTI); \ + +// JPH_IMPLEMENT_RTTI_NON_VIRTUAL +#define JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but when you cannot insert the declaration in the class +// itself, for example for templates and third party classes +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_RTTI_OUTSIDE_CLASS +#define JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name) \ + linkage RTTI * GetRTTIOfType(class_name *); \ + inline const RTTI * GetRTTI(const class_name *inObject) { return GetRTTIOfType((class_name *)nullptr); }\ + void CreateRTTI##class_name(RTTI &inRTTI); \ + +// JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS +#define JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti((const char *)#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &CreateRTTI##class_name); \ + return &rtti; \ + } \ + void CreateRTTI##class_name(RTTI &inRTTI) + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but for classes that have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_DECLARE_RTTI_HELPER(linkage, class_name, modifier) \ +public: \ + JPH_OVERRIDE_NEW_DELETE \ + friend linkage RTTI * GetRTTIOfType(class_name *); \ + friend inline const RTTI * GetRTTI(const class_name *inObject) { return inObject->GetRTTI(); } \ + virtual const RTTI * GetRTTI() const modifier; \ + virtual const void * CastTo(const RTTI *inRTTI) const modifier; \ + static void sCreateRTTI(RTTI &inRTTI); \ + +// JPH_DECLARE_RTTI_VIRTUAL - for derived classes with RTTI +#define JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, override) + +// JPH_IMPLEMENT_RTTI_VIRTUAL +#define JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), []() -> void * { return new class_name; }, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + const RTTI * class_name::GetRTTI() const \ + { \ + return JPH_RTTI(class_name); \ + } \ + const void * class_name::CastTo(const RTTI *inRTTI) const \ + { \ + return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI); \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +// JPH_DECLARE_RTTI_VIRTUAL_BASE - for concrete base class that has RTTI +#define JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, ) + +// JPH_IMPLEMENT_RTTI_VIRTUAL_BASE +#define JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) + +// JPH_DECLARE_RTTI_ABSTRACT - for derived abstract class that have RTTI +#define JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, override) + +// JPH_IMPLEMENT_RTTI_ABSTRACT +#define JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) \ + RTTI * GetRTTIOfType(class_name *) \ + { \ + static RTTI rtti(#class_name, sizeof(class_name), nullptr, [](void *inObject) { delete (class_name *)inObject; }, &class_name::sCreateRTTI); \ + return &rtti; \ + } \ + const RTTI * class_name::GetRTTI() const \ + { \ + return JPH_RTTI(class_name); \ + } \ + const void * class_name::CastTo(const RTTI *inRTTI) const \ + { \ + return JPH_RTTI(class_name)->CastTo((const void *)this, inRTTI); \ + } \ + void class_name::sCreateRTTI(RTTI &inRTTI) \ + +// JPH_DECLARE_RTTI_ABSTRACT_BASE - for abstract base class that has RTTI +#define JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name) \ + JPH_DECLARE_RTTI_HELPER(linkage, class_name, ) + +// JPH_IMPLEMENT_RTTI_ABSTRACT_BASE +#define JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) + +////////////////////////////////////////////////////////////////////////////////////////// +// Declare an RTTI class for registering with the factory +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_DECLARE_RTTI_FOR_FACTORY(linkage, class_name) \ + linkage RTTI * GetRTTIOfType(class class_name *); + +#define JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(linkage, name_space, class_name) \ + namespace name_space { \ + class class_name; \ + linkage RTTI * GetRTTIOfType(class class_name *); \ + } + +////////////////////////////////////////////////////////////////////////////////////////// +// Find the RTTI of a class +////////////////////////////////////////////////////////////////////////////////////////// + +#define JPH_RTTI(class_name) GetRTTIOfType(static_cast(nullptr)) + +////////////////////////////////////////////////////////////////////////////////////////// +// Macro to rename a class, useful for embedded classes: +// +// class A { class B { }; } +// +// Now use JPH_RENAME_CLASS(B, A::B) to avoid conflicts with other classes named B +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_RENAME_CLASS +#define JPH_RENAME_CLASS(class_name, new_name) \ + inRTTI.SetName(#new_name); + +////////////////////////////////////////////////////////////////////////////////////////// +// Macro to add base classes +////////////////////////////////////////////////////////////////////////////////////////// + +/// Define very dirty macro to get the offset of a baseclass into a class +#define JPH_BASE_CLASS_OFFSET(inClass, inBaseClass) ((int(uint64((inBaseClass *)((inClass *)0x10000))))-0x10000) + +// JPH_ADD_BASE_CLASS +#define JPH_ADD_BASE_CLASS(class_name, base_class_name) \ + inRTTI.AddBaseClass(JPH_RTTI(base_class_name), JPH_BASE_CLASS_OFFSET(class_name, base_class_name)); + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros and templates to identify a class +////////////////////////////////////////////////////////////////////////////////////////// + +/// Check if inObject is of DstType +template +inline bool IsType(const Type *inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +template +inline bool IsType(const RefConst &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +template +inline bool IsType(const Ref &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || *inObject->GetRTTI() == *inRTTI; +} + +/// Check if inObject is or is derived from DstType +template +inline bool IsKindOf(const Type *inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +template +inline bool IsKindOf(const RefConst &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +template +inline bool IsKindOf(const Ref &inObject, const RTTI *inRTTI) +{ + return inObject == nullptr || inObject->GetRTTI()->IsKindOf(inRTTI); +} + +/// Cast inObject to DstType, asserts on failure +template || std::is_base_of_v, bool> = true> +inline const DstType *StaticCast(const SrcType *inObject) +{ + return static_cast(inObject); +} + +template || std::is_base_of_v, bool> = true> +inline DstType *StaticCast(SrcType *inObject) +{ + return static_cast(inObject); +} + +template || std::is_base_of_v, bool> = true> +inline const DstType *StaticCast(const RefConst &inObject) +{ + return static_cast(inObject.GetPtr()); +} + +template || std::is_base_of_v, bool> = true> +inline DstType *StaticCast(const Ref &inObject) +{ + return static_cast(inObject.GetPtr()); +} + +/// Cast inObject to DstType, returns nullptr on failure +template +inline const DstType *DynamicCast(const SrcType *inObject) +{ + return inObject != nullptr? reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType))) : nullptr; +} + +template +inline DstType *DynamicCast(SrcType *inObject) +{ + return inObject != nullptr? const_cast(reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr; +} + +template +inline const DstType *DynamicCast(const RefConst &inObject) +{ + return inObject != nullptr? reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType))) : nullptr; +} + +template +inline DstType *DynamicCast(const Ref &inObject) +{ + return inObject != nullptr? const_cast(reinterpret_cast(inObject->CastTo(JPH_RTTI(DstType)))) : nullptr; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Reference.h b/WickedEngine/Jolt/Core/Reference.h new file mode 100644 index 000000000..d6fdf1da5 --- /dev/null +++ b/WickedEngine/Jolt/Core/Reference.h @@ -0,0 +1,226 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +// Forward declares +template class Ref; +template class RefConst; + +/// Simple class to facilitate reference counting / releasing +/// Derive your class from RefTarget and you can reference it by using Ref or RefConst +/// +/// Reference counting classes keep an integer which indicates how many references +/// to the object are active. Reference counting objects are derived from RefTarget +/// and staT & their life with a reference count of zero. They can then be assigned +/// to equivalents of pointers (Ref) which will increase the reference count immediately. +/// If the destructor of Ref is called or another object is assigned to the reference +/// counting pointer it will decrease the reference count of the object again. If this +/// reference count becomes zero, the object is destroyed. +/// +/// This provides a very powerful mechanism to prevent memory leaks, but also gives +/// some responsibility to the programmer. The most notable point is that you cannot +/// have one object reference another and have the other reference the first one +/// back, because this way the reference count of both objects will never become +/// lower than 1, resulting in a memory leak. By carefully designing your classes +/// (and particularly identifying who owns who in the class hierarchy) you can avoid +/// these problems. +template +class RefTarget +{ +public: + /// Constructor + inline RefTarget() = default; + inline RefTarget(const RefTarget &) { /* Do not copy refcount */ } + inline ~RefTarget() { JPH_IF_ENABLE_ASSERTS(uint32 value = mRefCount.load(memory_order_relaxed);) JPH_ASSERT(value == 0 || value == cEmbedded); } ///< assert no one is referencing us + + /// Mark this class as embedded, this means the type can be used in a compound or constructed on the stack. + /// The Release function will never destruct the object, it is assumed the destructor will be called by whoever allocated + /// the object and at that point in time it is checked that no references are left to the structure. + inline void SetEmbedded() const { JPH_IF_ENABLE_ASSERTS(uint32 old = ) mRefCount.fetch_add(cEmbedded, memory_order_relaxed); JPH_ASSERT(old < cEmbedded); } + + /// Assignment operator + inline RefTarget & operator = (const RefTarget &) { /* Don't copy refcount */ return *this; } + + /// Get current refcount of this object + uint32 GetRefCount() const { return mRefCount.load(memory_order_relaxed); } + + /// Add or release a reference to this object + inline void AddRef() const + { + // Adding a reference can use relaxed memory ordering + mRefCount.fetch_add(1, memory_order_relaxed); + } + + inline void Release() const + { + // Releasing a reference must use release semantics... + if (mRefCount.fetch_sub(1, memory_order_release) == 1) + { + // ... so that we can use acquire to ensure that we see any updates from other threads that released a ref before deleting the object + atomic_thread_fence(memory_order_acquire); + delete static_cast(this); + } + } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + static int sInternalGetRefCountOffset() { return offsetof(T, mRefCount); } + +protected: + static constexpr uint32 cEmbedded = 0x0ebedded; ///< A large value that gets added to the refcount to mark the object as embedded + + mutable atomic mRefCount = 0; ///< Current reference count +}; + +/// Pure virtual version of RefTarget +class JPH_EXPORT RefTargetVirtual +{ +public: + /// Virtual destructor + virtual ~RefTargetVirtual() = default; + + /// Virtual add reference + virtual void AddRef() = 0; + + /// Virtual release reference + virtual void Release() = 0; +}; + +/// Class for automatic referencing, this is the equivalent of a pointer to type T +/// if you assign a value to this class it will increment the reference count by one +/// of this object, and if you assign something else it will decrease the reference +/// count of the first object again. If it reaches a reference count of zero it will +/// be deleted +template +class Ref +{ +public: + /// Constructor + inline Ref() : mPtr(nullptr) { } + inline Ref(T *inRHS) : mPtr(inRHS) { AddRef(); } + inline Ref(const Ref &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline Ref(Ref &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline ~Ref() { Release(); } + + /// Assignment operators + inline Ref & operator = (T *inRHS) { if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; } + inline Ref & operator = (const Ref &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline Ref & operator = (Ref &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + + /// Casting operators + inline operator T *() const { return mPtr; } + + /// Access like a normal pointer + inline T * operator -> () const { return mPtr; } + inline T & operator * () const { return *mPtr; } + + /// Comparison + inline bool operator == (const T * inRHS) const { return mPtr == inRHS; } + inline bool operator == (const Ref &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const T * inRHS) const { return mPtr != inRHS; } + inline bool operator != (const Ref &inRHS) const { return mPtr != inRHS.mPtr; } + + /// Get pointer + inline T * GetPtr() const { return mPtr; } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + void ** InternalGetPointer() { return reinterpret_cast(&mPtr); } + +private: + template friend class RefConst; + + /// Use "variable = nullptr;" to release an object, do not call these functions + inline void AddRef() { if (mPtr != nullptr) mPtr->AddRef(); } + inline void Release() { if (mPtr != nullptr) mPtr->Release(); } + + T * mPtr; ///< Pointer to object that we are reference counting +}; + +/// Class for automatic referencing, this is the equivalent of a CONST pointer to type T +/// if you assign a value to this class it will increment the reference count by one +/// of this object, and if you assign something else it will decrease the reference +/// count of the first object again. If it reaches a reference count of zero it will +/// be deleted +template +class RefConst +{ +public: + /// Constructor + inline RefConst() : mPtr(nullptr) { } + inline RefConst(const T * inRHS) : mPtr(inRHS) { AddRef(); } + inline RefConst(const RefConst &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline RefConst(RefConst &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline RefConst(const Ref &inRHS) : mPtr(inRHS.mPtr) { AddRef(); } + inline RefConst(Ref &&inRHS) noexcept : mPtr(inRHS.mPtr) { inRHS.mPtr = nullptr; } + inline ~RefConst() { Release(); } + + /// Assignment operators + inline RefConst & operator = (const T * inRHS) { if (mPtr != inRHS) { Release(); mPtr = inRHS; AddRef(); } return *this; } + inline RefConst & operator = (const RefConst &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline RefConst & operator = (RefConst &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + inline RefConst & operator = (const Ref &inRHS) { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; AddRef(); } return *this; } + inline RefConst & operator = (Ref &&inRHS) noexcept { if (mPtr != inRHS.mPtr) { Release(); mPtr = inRHS.mPtr; inRHS.mPtr = nullptr; } return *this; } + + /// Casting operators + inline operator const T * () const { return mPtr; } + + /// Access like a normal pointer + inline const T * operator -> () const { return mPtr; } + inline const T & operator * () const { return *mPtr; } + + /// Comparison + inline bool operator == (const T * inRHS) const { return mPtr == inRHS; } + inline bool operator == (const RefConst &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator == (const Ref &inRHS) const { return mPtr == inRHS.mPtr; } + inline bool operator != (const T * inRHS) const { return mPtr != inRHS; } + inline bool operator != (const RefConst &inRHS) const { return mPtr != inRHS.mPtr; } + inline bool operator != (const Ref &inRHS) const { return mPtr != inRHS.mPtr; } + + /// Get pointer + inline const T * GetPtr() const { return mPtr; } + + /// INTERNAL HELPER FUNCTION USED BY SERIALIZATION + void ** InternalGetPointer() { return const_cast(reinterpret_cast(&mPtr)); } + +private: + /// Use "variable = nullptr;" to release an object, do not call these functions + inline void AddRef() { if (mPtr != nullptr) mPtr->AddRef(); } + inline void Release() { if (mPtr != nullptr) mPtr->Release(); } + + const T * mPtr; ///< Pointer to object that we are reference counting +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for Ref + template + struct hash> + { + size_t operator () (const JPH::Ref &inRHS) const + { + return hash { }(inRHS.GetPtr()); + } + }; + + /// Declare std::hash for RefConst + template + struct hash> + { + size_t operator () (const JPH::RefConst &inRHS) const + { + return hash { }(inRHS.GetPtr()); + } + }; +} + +JPH_SUPPRESS_WARNING_POP diff --git a/WickedEngine/Jolt/Core/Result.h b/WickedEngine/Jolt/Core/Result.h new file mode 100644 index 000000000..da3b98cb3 --- /dev/null +++ b/WickedEngine/Jolt/Core/Result.h @@ -0,0 +1,177 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +// GCC doesn't properly detect that mState is used to ensure that mResult is initialized +JPH_GCC_SUPPRESS_WARNING("-Wmaybe-uninitialized") + +/// Helper class that either contains a valid result or an error +template +class Result +{ +public: + /// Default constructor + Result() { } + + /// Copy constructor + Result(const Result &inRHS) : + mState(inRHS.mState) + { + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (inRHS.mResult); + break; + + case EState::Error: + ::new (&mError) String(inRHS.mError); + break; + + case EState::Invalid: + break; + } + } + + /// Move constructor + Result(Result &&inRHS) noexcept : + mState(inRHS.mState) + { + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (std::move(inRHS.mResult)); + break; + + case EState::Error: + ::new (&mError) String(std::move(inRHS.mError)); + break; + + case EState::Invalid: + break; + } + + // Don't reset the state of inRHS, the destructors still need to be called after a move operation + } + + /// Destructor + ~Result() { Clear(); } + + /// Copy assignment + Result & operator = (const Result &inRHS) + { + Clear(); + + mState = inRHS.mState; + + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (inRHS.mResult); + break; + + case EState::Error: + ::new (&mError) String(inRHS.mError); + break; + + case EState::Invalid: + break; + } + + return *this; + } + + /// Move assignment + Result & operator = (Result &&inRHS) noexcept + { + Clear(); + + mState = inRHS.mState; + + switch (inRHS.mState) + { + case EState::Valid: + ::new (&mResult) Type (std::move(inRHS.mResult)); + break; + + case EState::Error: + ::new (&mError) String(std::move(inRHS.mError)); + break; + + case EState::Invalid: + break; + } + + // Don't reset the state of inRHS, the destructors still need to be called after a move operation + + return *this; + } + + /// Clear result or error + void Clear() + { + switch (mState) + { + case EState::Valid: + mResult.~Type(); + break; + + case EState::Error: + mError.~String(); + break; + + case EState::Invalid: + break; + } + + mState = EState::Invalid; + } + + /// Checks if the result is still uninitialized + bool IsEmpty() const { return mState == EState::Invalid; } + + /// Checks if the result is valid + bool IsValid() const { return mState == EState::Valid; } + + /// Get the result value + const Type & Get() const { JPH_ASSERT(IsValid()); return mResult; } + + /// Set the result value + void Set(const Type &inResult) { Clear(); ::new (&mResult) Type(inResult); mState = EState::Valid; } + + /// Set the result value (move value) + void Set(Type &&inResult) { Clear(); ::new (&mResult) Type(std::move(inResult)); mState = EState::Valid; } + + /// Check if we had an error + bool HasError() const { return mState == EState::Error; } + + /// Get the error value + const String & GetError() const { JPH_ASSERT(HasError()); return mError; } + + /// Set an error value + void SetError(const char *inError) { Clear(); ::new (&mError) String(inError); mState = EState::Error; } + void SetError(const string_view &inError) { Clear(); ::new (&mError) String(inError); mState = EState::Error; } + void SetError(String &&inError) { Clear(); ::new (&mError) String(std::move(inError)); mState = EState::Error; } + +private: + union + { + Type mResult; ///< The actual result object + String mError; ///< The error description if the result failed + }; + + /// State of the result + enum class EState : uint8 + { + Invalid, + Valid, + Error + }; + + EState mState = EState::Invalid; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/STLAlignedAllocator.h b/WickedEngine/Jolt/Core/STLAlignedAllocator.h new file mode 100644 index 000000000..60e3ad487 --- /dev/null +++ b/WickedEngine/Jolt/Core/STLAlignedAllocator.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// STL allocator that takes care that memory is aligned to N bytes +template +class STLAlignedAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is stateless + using is_always_equal = std::true_type; + + /// Allocator supports moving + using propagate_on_container_move_assignment = std::true_type; + + /// Constructor + inline STLAlignedAllocator() = default; + + /// Constructor from other allocator + template + inline explicit STLAlignedAllocator(const STLAlignedAllocator &) { } + + /// Allocate memory + inline pointer allocate(size_type inN) + { + return (pointer)AlignedAllocate(inN * sizeof(value_type), N); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type) + { + AlignedFree(inPointer); + } + + /// Allocators are stateless so assumed to be equal + inline bool operator == (const STLAlignedAllocator &) const + { + return true; + } + + inline bool operator != (const STLAlignedAllocator &) const + { + return false; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLAlignedAllocator; + }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/STLAllocator.h b/WickedEngine/Jolt/Core/STLAllocator.h new file mode 100644 index 000000000..0e3ee459a --- /dev/null +++ b/WickedEngine/Jolt/Core/STLAllocator.h @@ -0,0 +1,127 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Default implementation of AllocatorHasReallocate which tells if an allocator has a reallocate function +template struct AllocatorHasReallocate { static constexpr bool sValue = false; }; + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + +/// STL allocator that forwards to our allocation functions +template +class STLAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is stateless + using is_always_equal = std::true_type; + + /// Allocator supports moving + using propagate_on_container_move_assignment = std::true_type; + + /// Constructor + inline STLAllocator() = default; + + /// Constructor from other allocator + template + inline STLAllocator(const STLAllocator &) { } + + /// If this allocator needs to fall back to aligned allocations because the type requires it + static constexpr bool needs_aligned_allocate = alignof(T) > (JPH_CPU_ADDRESS_BITS == 32? 8 : 16); + + /// Allocate memory + inline pointer allocate(size_type inN) + { + if constexpr (needs_aligned_allocate) + return pointer(AlignedAllocate(inN * sizeof(value_type), alignof(T))); + else + return pointer(Allocate(inN * sizeof(value_type))); + } + + /// Should we expose a reallocate function? + static constexpr bool has_reallocate = std::is_trivially_copyable() && !needs_aligned_allocate; + + /// Reallocate memory + template > + inline pointer reallocate(pointer inOldPointer, [[maybe_unused]] size_type inOldSize, size_type inNewSize) + { + JPH_ASSERT(inNewSize > 0); // Reallocating to zero size is implementation dependent, so we don't allow it + return pointer(Reallocate(inOldPointer, inNewSize * sizeof(value_type))); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type) + { + if constexpr (needs_aligned_allocate) + AlignedFree(inPointer); + else + Free(inPointer); + } + + /// Allocators are stateless so assumed to be equal + inline bool operator == (const STLAllocator &) const + { + return true; + } + + inline bool operator != (const STLAllocator &) const + { + return false; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLAllocator; + }; +}; + +/// The STLAllocator implements the reallocate function if the alignment of the class is smaller or equal to the default alignment for the platform +template struct AllocatorHasReallocate> { static constexpr bool sValue = STLAllocator::has_reallocate; }; + +#else + +template using STLAllocator = std::allocator; + +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + +// Declare STL containers that use our allocator +using String = std::basic_string, STLAllocator>; +using IStringStream = std::basic_istringstream, STLAllocator>; + +JPH_NAMESPACE_END + +#if (!defined(JPH_PLATFORM_WINDOWS) || defined(JPH_COMPILER_MINGW)) && !defined(JPH_DISABLE_CUSTOM_ALLOCATOR) + +namespace std +{ + /// Declare std::hash for String, for some reason on Linux based platforms template deduction takes the wrong variant + template <> + struct hash + { + inline size_t operator () (const JPH::String &inRHS) const + { + return hash { } (inRHS); + } + }; +} + +#endif // (!JPH_PLATFORM_WINDOWS || JPH_COMPILER_MINGW) && !JPH_DISABLE_CUSTOM_ALLOCATOR diff --git a/WickedEngine/Jolt/Core/STLTempAllocator.h b/WickedEngine/Jolt/Core/STLTempAllocator.h new file mode 100644 index 000000000..cf7c39d45 --- /dev/null +++ b/WickedEngine/Jolt/Core/STLTempAllocator.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// STL allocator that wraps around TempAllocator +template +class STLTempAllocator +{ +public: + using value_type = T; + + /// Pointer to type + using pointer = T *; + using const_pointer = const T *; + + /// Reference to type. + /// Can be removed in C++20. + using reference = T &; + using const_reference = const T &; + + using size_type = size_t; + using difference_type = ptrdiff_t; + + /// The allocator is not stateless (depends on the temp allocator) + using is_always_equal = std::false_type; + + /// Constructor + inline STLTempAllocator(TempAllocator &inAllocator) : mAllocator(inAllocator) { } + + /// Constructor from other allocator + template + inline explicit STLTempAllocator(const STLTempAllocator &inRHS) : mAllocator(inRHS.GetAllocator()) { } + + /// Allocate memory + inline pointer allocate(size_type inN) + { + return pointer(mAllocator.Allocate(uint(inN * sizeof(value_type)))); + } + + /// Free memory + inline void deallocate(pointer inPointer, size_type inN) + { + mAllocator.Free(inPointer, uint(inN * sizeof(value_type))); + } + + /// Allocators are not-stateless, assume if allocator address matches that the allocators are the same + inline bool operator == (const STLTempAllocator &inRHS) const + { + return &mAllocator == &inRHS.mAllocator; + } + + inline bool operator != (const STLTempAllocator &inRHS) const + { + return &mAllocator != &inRHS.mAllocator; + } + + /// Converting to allocator for other type + template + struct rebind + { + using other = STLTempAllocator; + }; + + /// Get our temp allocator + TempAllocator & GetAllocator() const + { + return mAllocator; + } + +private: + TempAllocator & mAllocator; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/ScopeExit.h b/WickedEngine/Jolt/Core/ScopeExit.h new file mode 100644 index 000000000..613838421 --- /dev/null +++ b/WickedEngine/Jolt/Core/ScopeExit.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that calls a function when it goes out of scope +template +class ScopeExit : public NonCopyable +{ +public: + /// Constructor specifies the exit function + JPH_INLINE explicit ScopeExit(F &&inFunction) : mFunction(std::move(inFunction)) { } + + /// Destructor calls the exit function + JPH_INLINE ~ScopeExit() { if (!mInvoked) mFunction(); } + + /// Call the exit function now instead of when going out of scope + JPH_INLINE void Invoke() + { + if (!mInvoked) + { + mFunction(); + mInvoked = true; + } + } + + /// No longer call the exit function when going out of scope + JPH_INLINE void Release() + { + mInvoked = true; + } + +private: + F mFunction; + bool mInvoked = false; +}; + +#define JPH_SCOPE_EXIT_TAG2(line) scope_exit##line +#define JPH_SCOPE_EXIT_TAG(line) JPH_SCOPE_EXIT_TAG2(line) + +/// Usage: JPH_SCOPE_EXIT([]{ code to call on scope exit }); +#define JPH_SCOPE_EXIT(...) ScopeExit JPH_SCOPE_EXIT_TAG(__LINE__)(__VA_ARGS__) + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Semaphore.cpp b/WickedEngine/Jolt/Core/Semaphore.cpp new file mode 100644 index 000000000..294aa755f --- /dev/null +++ b/WickedEngine/Jolt/Core/Semaphore.cpp @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_PLATFORM_WINDOWS + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #define WIN32_LEAN_AND_MEAN +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + + JPH_SUPPRESS_WARNING_POP +#endif + +JPH_NAMESPACE_BEGIN + +Semaphore::Semaphore() +{ +#ifdef JPH_PLATFORM_WINDOWS + mSemaphore = CreateSemaphore(nullptr, 0, INT_MAX, nullptr); +#endif +} + +Semaphore::~Semaphore() +{ +#ifdef JPH_PLATFORM_WINDOWS + CloseHandle(mSemaphore); +#endif +} + +void Semaphore::Release(uint inNumber) +{ + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_PLATFORM_WINDOWS + int old_value = mCount.fetch_add(inNumber); + if (old_value < 0) + { + int new_value = old_value + (int)inNumber; + int num_to_release = min(new_value, 0) - old_value; + ::ReleaseSemaphore(mSemaphore, num_to_release, nullptr); + } +#else + std::lock_guard lock(mLock); + mCount += (int)inNumber; + if (inNumber > 1) + mWaitVariable.notify_all(); + else + mWaitVariable.notify_one(); +#endif +} + +void Semaphore::Acquire(uint inNumber) +{ + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_PLATFORM_WINDOWS + int old_value = mCount.fetch_sub(inNumber); + int new_value = old_value - (int)inNumber; + if (new_value < 0) + { + int num_to_acquire = min(old_value, 0) - new_value; + for (int i = 0; i < num_to_acquire; ++i) + WaitForSingleObject(mSemaphore, INFINITE); + } +#else + std::unique_lock lock(mLock); + mCount -= (int)inNumber; + mWaitVariable.wait(lock, [this]() { return mCount >= 0; }); +#endif +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/Semaphore.h b/WickedEngine/Jolt/Core/Semaphore.h new file mode 100644 index 000000000..498b1b8d4 --- /dev/null +++ b/WickedEngine/Jolt/Core/Semaphore.h @@ -0,0 +1,51 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +// Things we're using from STL +using std::atomic; +using std::mutex; +using std::condition_variable; + +/// Implements a semaphore +/// When we switch to C++20 we can use counting_semaphore to unify this +class JPH_EXPORT Semaphore +{ +public: + /// Constructor + Semaphore(); + ~Semaphore(); + + /// Release the semaphore, signaling the thread waiting on the barrier that there may be work + void Release(uint inNumber = 1); + + /// Acquire the semaphore inNumber times + void Acquire(uint inNumber = 1); + + /// Get the current value of the semaphore + inline int GetValue() const { return mCount; } + +private: +#ifdef JPH_PLATFORM_WINDOWS + // On windows we use a semaphore object since it is more efficient than a lock and a condition variable + alignas(JPH_CACHE_LINE_SIZE) atomic mCount { 0 }; ///< We increment mCount for every release, to acquire we decrement the count. If the count is negative we know that we are waiting on the actual semaphore. + void * mSemaphore; ///< The semaphore is an expensive construct so we only acquire/release it if we know that we need to wait/have waiting threads +#else + // Other platforms: Emulate a semaphore using a mutex, condition variable and count + mutex mLock; + condition_variable mWaitVariable; + int mCount = 0; +#endif +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/StaticArray.h b/WickedEngine/Jolt/Core/StaticArray.h new file mode 100644 index 000000000..21a685c16 --- /dev/null +++ b/WickedEngine/Jolt/Core/StaticArray.h @@ -0,0 +1,325 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple variable length array backed by a fixed size buffer +template +class [[nodiscard]] StaticArray +{ +public: + using value_type = T; + + using size_type = uint; + + static constexpr uint Capacity = N; + + /// Default constructor + StaticArray() = default; + + /// Constructor from initializer list + explicit StaticArray(std::initializer_list inList) + { + JPH_ASSERT(inList.size() <= N); + for (typename std::initializer_list::iterator i = inList.begin(); i != inList.end(); ++i) + ::new (reinterpret_cast(&mElements[mSize++])) T(*i); + } + + /// Copy constructor + StaticArray(const StaticArray &inRHS) + { + while (mSize < inRHS.mSize) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + /// Destruct all elements + ~StaticArray() + { + if constexpr (!is_trivially_destructible()) + for (T *e = reinterpret_cast(mElements), *end = e + mSize; e < end; ++e) + e->~T(); + } + + /// Destruct all elements and set length to zero + void clear() + { + if constexpr (!is_trivially_destructible()) + for (T *e = reinterpret_cast(mElements), *end = e + mSize; e < end; ++e) + e->~T(); + mSize = 0; + } + + /// Add element to the back of the array + void push_back(const T &inElement) + { + JPH_ASSERT(mSize < N); + ::new (&mElements[mSize++]) T(inElement); + } + + /// Construct element at the back of the array + template + void emplace_back(A &&... inElement) + { + JPH_ASSERT(mSize < N); + ::new (&mElements[mSize++]) T(std::forward(inElement)...); + } + + /// Remove element from the back of the array + void pop_back() + { + JPH_ASSERT(mSize > 0); + reinterpret_cast(mElements[--mSize]).~T(); + } + + /// Returns true if there are no elements in the array + bool empty() const + { + return mSize == 0; + } + + /// Returns amount of elements in the array + size_type size() const + { + return mSize; + } + + /// Returns maximum amount of elements the array can hold + size_type capacity() const + { + return N; + } + + /// Resize array to new length + void resize(size_type inNewSize) + { + JPH_ASSERT(inNewSize <= N); + if constexpr (!is_trivially_constructible()) + for (T *element = reinterpret_cast(mElements) + mSize, *element_end = reinterpret_cast(mElements) + inNewSize; element < element_end; ++element) + ::new (element) T; + if constexpr (!is_trivially_destructible()) + for (T *element = reinterpret_cast(mElements) + inNewSize, *element_end = reinterpret_cast(mElements) + mSize; element < element_end; ++element) + element->~T(); + mSize = inNewSize; + } + + using const_iterator = const T *; + + /// Iterators + const_iterator begin() const + { + return reinterpret_cast(mElements); + } + + const_iterator end() const + { + return reinterpret_cast(mElements + mSize); + } + + using iterator = T *; + + iterator begin() + { + return reinterpret_cast(mElements); + } + + iterator end() + { + return reinterpret_cast(mElements + mSize); + } + + const T * data() const + { + return reinterpret_cast(mElements); + } + + T * data() + { + return reinterpret_cast(mElements); + } + + /// Access element + T & operator [] (size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + const T & operator [] (size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + /// Access element + T & at(size_type inIdx) + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + const T & at(size_type inIdx) const + { + JPH_ASSERT(inIdx < mSize); + return reinterpret_cast(mElements[inIdx]); + } + + /// First element in the array + const T & front() const + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[0]); + } + + T & front() + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[0]); + } + + /// Last element in the array + const T & back() const + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[mSize - 1]); + } + + T & back() + { + JPH_ASSERT(mSize > 0); + return reinterpret_cast(mElements[mSize - 1]); + } + + /// Remove one element from the array + void erase(const_iterator inIter) + { + size_type p = size_type(inIter - begin()); + JPH_ASSERT(p < mSize); + reinterpret_cast(mElements[p]).~T(); + if (p + 1 < mSize) + memmove(mElements + p, mElements + p + 1, (mSize - p - 1) * sizeof(T)); + --mSize; + } + + /// Remove multiple element from the array + void erase(const_iterator inBegin, const_iterator inEnd) + { + size_type p = size_type(inBegin - begin()); + size_type n = size_type(inEnd - inBegin); + JPH_ASSERT(inEnd <= end()); + for (size_type i = 0; i < n; ++i) + reinterpret_cast(mElements[p + i]).~T(); + if (p + n < mSize) + memmove(mElements + p, mElements + p + n, (mSize - p - n) * sizeof(T)); + mSize -= n; + } + + /// Assignment operator + StaticArray & operator = (const StaticArray &inRHS) + { + size_type rhs_size = inRHS.size(); + + if (static_cast(this) != static_cast(&inRHS)) + { + clear(); + + while (mSize < rhs_size) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + return *this; + } + + /// Assignment operator with static array of different max length + template + StaticArray & operator = (const StaticArray &inRHS) + { + size_type rhs_size = inRHS.size(); + JPH_ASSERT(rhs_size <= N); + + if (static_cast(this) != static_cast(&inRHS)) + { + clear(); + + while (mSize < rhs_size) + { + ::new (&mElements[mSize]) T(inRHS[mSize]); + ++mSize; + } + } + + return *this; + } + + /// Comparing arrays + bool operator == (const StaticArray &inRHS) const + { + if (mSize != inRHS.mSize) + return false; + for (size_type i = 0; i < mSize; ++i) + if (!(reinterpret_cast(mElements[i]) == reinterpret_cast(inRHS.mElements[i]))) + return false; + return true; + } + + bool operator != (const StaticArray &inRHS) const + { + if (mSize != inRHS.mSize) + return true; + for (size_type i = 0; i < mSize; ++i) + if (reinterpret_cast(mElements[i]) != reinterpret_cast(inRHS.mElements[i])) + return true; + return false; + } + +protected: + struct alignas(T) Storage + { + uint8 mData[sizeof(T)]; + }; + + static_assert(sizeof(T) == sizeof(Storage), "Mismatch in size"); + static_assert(alignof(T) == alignof(Storage), "Mismatch in alignment"); + + size_type mSize = 0; + Storage mElements[N]; +}; + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat") + +namespace std +{ + /// Declare std::hash for StaticArray + template + struct hash> + { + size_t operator () (const JPH::StaticArray &inRHS) const + { + std::size_t ret = 0; + + // Hash length first + JPH::HashCombine(ret, inRHS.size()); + + // Then hash elements + for (const T &t : inRHS) + JPH::HashCombine(ret, t); + + return ret; + } + }; +} + +JPH_SUPPRESS_WARNING_POP diff --git a/WickedEngine/Jolt/Core/StreamIn.h b/WickedEngine/Jolt/Core/StreamIn.h new file mode 100644 index 000000000..d5f9f3673 --- /dev/null +++ b/WickedEngine/Jolt/Core/StreamIn.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple binary input stream +class JPH_EXPORT StreamIn : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~StreamIn() = default; + + /// Read a string of bytes from the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) = 0; + + /// Returns true when an attempt has been made to read past the end of the file + virtual bool IsEOF() const = 0; + + /// Returns true if there was an IO failure + virtual bool IsFailed() const = 0; + + /// Read a primitive (e.g. float, int, etc.) from the binary stream + template , bool> = true> + void Read(T &outT) + { + ReadBytes(&outT, sizeof(outT)); + } + + /// Read a vector of primitives from the binary stream + template , bool> = true> + void Read(Array &outT) + { + typename Array::size_type len = outT.size(); // Initialize to previous array size, this is used for validation in the StateRecorder class + Read(len); + if (!IsEOF() && !IsFailed()) + { + outT.resize(len); + if constexpr (std::is_same_v || std::is_same_v || std::is_same_v) + { + // These types have unused components that we don't want to read + for (typename Array::size_type i = 0; i < len; ++i) + Read(outT[i]); + } + else + { + // Read all elements at once + ReadBytes(outT.data(), len * sizeof(T)); + } + } + else + outT.clear(); + } + + /// Read a string from the binary stream (reads the number of characters and then the characters) + template + void Read(std::basic_string &outString) + { + typename std::basic_string::size_type len = 0; + Read(len); + if (!IsEOF() && !IsFailed()) + { + outString.resize(len); + ReadBytes(outString.data(), len * sizeof(Type)); + } + else + outString.clear(); + } + + /// Read a vector of primitives from the binary stream using a custom function to read the elements + template + void Read(Array &outT, const F &inReadElement) + { + typename Array::size_type len = outT.size(); // Initialize to previous array size, this is used for validation in the StateRecorder class + Read(len); + if (!IsEOF() && !IsFailed()) + { + outT.resize(len); + for (typename Array::size_type i = 0; i < len; ++i) + inReadElement(*this, outT[i]); + } + else + outT.clear(); + } + + /// Read a Vec3 (don't read W) + void Read(Vec3 &outVec) + { + ReadBytes(&outVec, 3 * sizeof(float)); + outVec = Vec3::sFixW(outVec.mValue); + } + + /// Read a DVec3 (don't read W) + void Read(DVec3 &outVec) + { + ReadBytes(&outVec, 3 * sizeof(double)); + outVec = DVec3::sFixW(outVec.mValue); + } + + /// Read a DMat44 (don't read W component of translation) + void Read(DMat44 &outVec) + { + Vec4 x, y, z; + Read(x); + Read(y); + Read(z); + + DVec3 t; + Read(t); + + outVec = DMat44(x, y, z, t); + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/StreamOut.h b/WickedEngine/Jolt/Core/StreamOut.h new file mode 100644 index 000000000..d21bf3213 --- /dev/null +++ b/WickedEngine/Jolt/Core/StreamOut.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Simple binary output stream +class JPH_EXPORT StreamOut : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~StreamOut() = default; + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) = 0; + + /// Returns true if there was an IO failure + virtual bool IsFailed() const = 0; + + /// Write a primitive (e.g. float, int, etc.) to the binary stream + template , bool> = true> + void Write(const T &inT) + { + WriteBytes(&inT, sizeof(inT)); + } + + /// Write a vector of primitives to the binary stream + template , bool> = true> + void Write(const Array &inT) + { + typename Array::size_type len = inT.size(); + Write(len); + if (!IsFailed()) + { + if constexpr (std::is_same_v || std::is_same_v || std::is_same_v) + { + // These types have unused components that we don't want to write + for (typename Array::size_type i = 0; i < len; ++i) + Write(inT[i]); + } + else + { + // Write all elements at once + WriteBytes(inT.data(), len * sizeof(T)); + } + } + } + + /// Write a string to the binary stream (writes the number of characters and then the characters) + template + void Write(const std::basic_string &inString) + { + typename std::basic_string::size_type len = inString.size(); + Write(len); + if (!IsFailed()) + WriteBytes(inString.data(), len * sizeof(Type)); + } + + /// Write a vector of primitives to the binary stream using a custom write function + template + void Write(const Array &inT, const F &inWriteElement) + { + typename Array::size_type len = inT.size(); + Write(len); + if (!IsFailed()) + for (typename Array::size_type i = 0; i < len; ++i) + inWriteElement(inT[i], *this); + } + + /// Write a Vec3 (don't write W) + void Write(const Vec3 &inVec) + { + WriteBytes(&inVec, 3 * sizeof(float)); + } + + /// Write a DVec3 (don't write W) + void Write(const DVec3 &inVec) + { + WriteBytes(&inVec, 3 * sizeof(double)); + } + + /// Write a DMat44 (don't write W component of translation) + void Write(const DMat44 &inVec) + { + Write(inVec.GetColumn4(0)); + Write(inVec.GetColumn4(1)); + Write(inVec.GetColumn4(2)); + + Write(inVec.GetTranslation()); + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/StreamUtils.h b/WickedEngine/Jolt/Core/StreamUtils.h new file mode 100644 index 000000000..e374c5210 --- /dev/null +++ b/WickedEngine/Jolt/Core/StreamUtils.h @@ -0,0 +1,167 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +namespace StreamUtils { + +template +using ObjectToIDMap = UnorderedMap; + +template +using IDToObjectMap = Array>; + +// Restore a single object by reading the hash of the type, constructing it and then calling the restore function +template +Result> RestoreObject(StreamIn &inStream, void (Type::*inRestoreBinaryStateFunction)(StreamIn &)) +{ + Result> result; + + // Read the hash of the type + uint32 hash; + inStream.Read(hash); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read type hash"); + return result; + } + + // Get the RTTI for the type + const RTTI *rtti = Factory::sInstance->Find(hash); + if (rtti == nullptr) + { + result.SetError("Failed to create instance of type"); + return result; + } + + // Construct and read the data of the type + Ref object = reinterpret_cast(rtti->CreateObject()); + (object->*inRestoreBinaryStateFunction)(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to restore object"); + return result; + } + + result.Set(object); + return result; +} + +/// Save an object reference to a stream. Uses a map to map objects to IDs which is also used to prevent writing duplicates. +template +void SaveObjectReference(StreamOut &inStream, const Type *inObject, ObjectToIDMap *ioObjectToIDMap) +{ + if (ioObjectToIDMap == nullptr || inObject == nullptr) + { + // Write null ID + inStream.Write(~uint32(0)); + } + else + { + typename ObjectToIDMap::const_iterator id = ioObjectToIDMap->find(inObject); + if (id != ioObjectToIDMap->end()) + { + // Existing object, write ID + inStream.Write(id->second); + } + else + { + // New object, write the ID + uint32 new_id = uint32(ioObjectToIDMap->size()); + (*ioObjectToIDMap)[inObject] = new_id; + inStream.Write(new_id); + + // Write the object + inObject->SaveBinaryState(inStream); + } + } +} + +/// Restore an object reference from stream. +template +Result> RestoreObjectReference(StreamIn &inStream, IDToObjectMap &ioIDToObjectMap) +{ + Result> result; + + // Read id + uint32 id = ~uint32(0); + inStream.Read(id); + + // Check null + if (id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if it already exists + if (id >= ioIDToObjectMap.size()) + { + // New object, restore it + result = Type::sRestoreFromBinaryState(inStream); + if (result.HasError()) + return result; + JPH_ASSERT(id == ioIDToObjectMap.size()); + ioIDToObjectMap.push_back(result.Get()); + } + else + { + // Existing object filter + result.Set(ioIDToObjectMap[id].GetPtr()); + } + + return result; +} + +// Save an array of objects to a stream. +template +void SaveObjectArray(StreamOut &inStream, const ArrayType &inArray, ObjectToIDMap *ioObjectToIDMap) +{ + inStream.Write(size_t(inArray.size())); + for (const ValueType *value: inArray) + SaveObjectReference(inStream, value, ioObjectToIDMap); +} + +// Restore an array of objects from a stream. +template +Result RestoreObjectArray(StreamIn &inStream, IDToObjectMap &ioIDToObjectMap) +{ + Result result; + + size_t len; + inStream.Read(len); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read stream"); + return result; + } + + ArrayType values; + values.reserve(len); + for (size_t i = 0; i < len; ++i) + { + Result value = RestoreObjectReference(inStream, ioIDToObjectMap); + if (value.HasError()) + { + result.SetError(value.GetError()); + return result; + } + values.push_back(std::move(value.Get())); + } + + result.Set(values); + return result; +} + +} // StreamUtils + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/StreamWrapper.h b/WickedEngine/Jolt/Core/StreamWrapper.h new file mode 100644 index 000000000..66a36b057 --- /dev/null +++ b/WickedEngine/Jolt/Core/StreamWrapper.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +/// Wrapper around std::ostream +class StreamOutWrapper : public StreamOut +{ +public: + /// Constructor + StreamOutWrapper(ostream &ioWrapped) : mWrapped(ioWrapped) { } + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) override { mWrapped.write((const char *)inData, inNumBytes); } + + /// Returns true if there was an IO failure + virtual bool IsFailed() const override { return mWrapped.fail(); } + +private: + ostream & mWrapped; +}; + +/// Wrapper around std::istream +class StreamInWrapper : public StreamIn +{ +public: + /// Constructor + StreamInWrapper(istream &ioWrapped) : mWrapped(ioWrapped) { } + + /// Write a string of bytes to the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) override { mWrapped.read((char *)outData, inNumBytes); } + + /// Returns true when an attempt has been made to read past the end of the file + virtual bool IsEOF() const override { return mWrapped.eof(); } + + /// Returns true if there was an IO failure + virtual bool IsFailed() const override { return mWrapped.fail(); } + +private: + istream & mWrapped; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/StringTools.cpp b/WickedEngine/Jolt/Core/StringTools.cpp new file mode 100644 index 000000000..6eae982cc --- /dev/null +++ b/WickedEngine/Jolt/Core/StringTools.cpp @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +String StringFormat(const char *inFMT, ...) +{ + char buffer[1024]; + + // Format the string + va_list list; + va_start(list, inFMT); + vsnprintf(buffer, sizeof(buffer), inFMT, list); + va_end(list); + + return String(buffer); +} + +void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace) +{ + size_t index = 0; + for (;;) + { + index = ioString.find(inSearch, index); + if (index == String::npos) + break; + + ioString.replace(index, inSearch.size(), inReplace); + + index += inReplace.size(); + } +} + +void StringToVector(const string_view &inString, Array &outVector, const string_view &inDelimiter, bool inClearVector) +{ + JPH_ASSERT(inDelimiter.size() > 0); + + // Ensure vector empty + if (inClearVector) + outVector.clear(); + + // No string? no elements + if (inString.empty()) + return; + + // Start with initial string + String s(inString); + + // Add to vector while we have a delimiter + size_t i; + while (!s.empty() && (i = s.find(inDelimiter)) != String::npos) + { + outVector.push_back(s.substr(0, i)); + s.erase(0, i + inDelimiter.length()); + } + + // Add final element + outVector.push_back(s); +} + +void VectorToString(const Array &inVector, String &outString, const string_view &inDelimiter) +{ + // Ensure string empty + outString.clear(); + + for (const String &s : inVector) + { + // Add delimiter if not first element + if (!outString.empty()) + outString.append(inDelimiter); + + // Add element + outString.append(s); + } +} + +String ToLower(const string_view &inString) +{ + String out; + out.reserve(inString.length()); + for (char c : inString) + out.push_back((char)tolower(c)); + return out; +} + +const char *NibbleToBinary(uint32 inNibble) +{ + static const char *nibbles[] = { "0000", "0001", "0010", "0011", "0100", "0101", "0110", "0111", "1000", "1001", "1010", "1011", "1100", "1101", "1110", "1111" }; + return nibbles[inNibble & 0xf]; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/StringTools.h b/WickedEngine/Jolt/Core/StringTools.h new file mode 100644 index 000000000..d3def9356 --- /dev/null +++ b/WickedEngine/Jolt/Core/StringTools.h @@ -0,0 +1,51 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Create a formatted text string for debugging purposes. +/// Note that this function has an internal buffer of 1024 characters, so long strings will be trimmed. +JPH_EXPORT String StringFormat(const char *inFMT, ...); + +/// Convert type to string +template +String ConvertToString(const T &inValue) +{ + using OStringStream = std::basic_ostringstream, STLAllocator>; + OStringStream oss; + oss << inValue; + return oss.str(); +} + +/// Calculate the FNV-1a hash of inString. +/// @see https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function +constexpr uint64 HashString(const char *inString) +{ + uint64 hash = 14695981039346656037UL; + for (const char *c = inString; *c != 0; ++c) + { + hash ^= *c; + hash = hash * 1099511628211UL; + } + return hash; +} + +/// Replace substring with other string +JPH_EXPORT void StringReplace(String &ioString, const string_view &inSearch, const string_view &inReplace); + +/// Convert a delimited string to an array of strings +JPH_EXPORT void StringToVector(const string_view &inString, Array &outVector, const string_view &inDelimiter = ",", bool inClearVector = true); + +/// Convert an array strings to a delimited string +JPH_EXPORT void VectorToString(const Array &inVector, String &outString, const string_view &inDelimiter = ","); + +/// Convert a string to lower case +JPH_EXPORT String ToLower(const string_view &inString); + +/// Converts the lower 4 bits of inNibble to a string that represents the number in binary format +JPH_EXPORT const char *NibbleToBinary(uint32 inNibble); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/TempAllocator.h b/WickedEngine/Jolt/Core/TempAllocator.h new file mode 100644 index 000000000..af0cf3137 --- /dev/null +++ b/WickedEngine/Jolt/Core/TempAllocator.h @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Allocator for temporary allocations. +/// This allocator works as a stack: The blocks must always be freed in the reverse order as they are allocated. +/// Note that allocations and frees can take place from different threads, but the order is guaranteed though +/// job dependencies, so it is not needed to use any form of locking. +class JPH_EXPORT TempAllocator : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + virtual ~TempAllocator() = default; + + /// Allocates inSize bytes of memory, returned memory address must be JPH_RVECTOR_ALIGNMENT byte aligned + virtual void * Allocate(uint inSize) = 0; + + /// Frees inSize bytes of memory located at inAddress + virtual void Free(void *inAddress, uint inSize) = 0; +}; + +/// Default implementation of the temp allocator that allocates a large block through malloc upfront +class JPH_EXPORT TempAllocatorImpl final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the allocator with a maximum allocatable size of inSize + explicit TempAllocatorImpl(uint inSize) : + mBase(static_cast(AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT))), + mSize(inSize) + { + } + + /// Destructor, frees the block + virtual ~TempAllocatorImpl() override + { + JPH_ASSERT(mTop == 0); + AlignedFree(mBase); + } + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + if (inSize == 0) + { + return nullptr; + } + else + { + uint new_top = mTop + AlignUp(inSize, JPH_RVECTOR_ALIGNMENT); + if (new_top > mSize) + { + Trace("TempAllocator: Out of memory"); + std::abort(); + } + void *address = mBase + mTop; + mTop = new_top; + return address; + } + } + + // See: TempAllocator + virtual void Free(void *inAddress, uint inSize) override + { + if (inAddress == nullptr) + { + JPH_ASSERT(inSize == 0); + } + else + { + mTop -= AlignUp(inSize, JPH_RVECTOR_ALIGNMENT); + if (mBase + mTop != inAddress) + { + Trace("TempAllocator: Freeing in the wrong order"); + std::abort(); + } + } + } + + // Check if no allocations have been made + bool IsEmpty() const + { + return mTop == 0; + } + +private: + uint8 * mBase; ///< Base address of the memory block + uint mSize; ///< Size of the memory block + uint mTop = 0; ///< Current top of the stack +}; + +/// Implementation of the TempAllocator that just falls back to malloc/free +/// Note: This can be quite slow when running in the debugger as large memory blocks need to be initialized with 0xcd +class JPH_EXPORT TempAllocatorMalloc final : public TempAllocator +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // See: TempAllocator + virtual void * Allocate(uint inSize) override + { + return inSize > 0? AlignedAllocate(inSize, JPH_RVECTOR_ALIGNMENT) : nullptr; + } + + // See: TempAllocator + virtual void Free(void *inAddress, [[maybe_unused]] uint inSize) override + { + if (inAddress != nullptr) + AlignedFree(inAddress); + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/TickCounter.cpp b/WickedEngine/Jolt/Core/TickCounter.cpp new file mode 100644 index 000000000..ae0a0d008 --- /dev/null +++ b/WickedEngine/Jolt/Core/TickCounter.cpp @@ -0,0 +1,34 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#if defined(JPH_PLATFORM_WINDOWS) + JPH_SUPPRESS_WARNING_PUSH + JPH_MSVC_SUPPRESS_WARNING(5039) // winbase.h(13179): warning C5039: 'TpSetCallbackCleanupGroup': pointer or reference to potentially throwing function passed to 'extern "C"' function under -EHc. Undefined behavior may occur if this function throws an exception. + #define WIN32_LEAN_AND_MEAN +#ifndef JPH_COMPILER_MINGW + #include +#else + #include +#endif + JPH_SUPPRESS_WARNING_POP +#endif + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM)) + +uint64 GetProcessorTickCount() +{ + LARGE_INTEGER count; + QueryPerformanceCounter(&count); + return uint64(count.QuadPart); +} + +#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM) + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/TickCounter.h b/WickedEngine/Jolt/Core/TickCounter.h new file mode 100644 index 000000000..2b5410e3d --- /dev/null +++ b/WickedEngine/Jolt/Core/TickCounter.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Include for __rdtsc +#if defined(JPH_PLATFORM_WINDOWS) + #include +#elif defined(JPH_CPU_X86) && defined(JPH_COMPILER_GCC) + #include +#elif defined(JPH_CPU_E2K) + #include +#endif + +JPH_NAMESPACE_BEGIN + +#if defined(JPH_PLATFORM_WINDOWS_UWP) || (defined(JPH_PLATFORM_WINDOWS) && defined(JPH_CPU_ARM)) + +/// Functionality to get the processors cycle counter +uint64 GetProcessorTickCount(); // Not inline to avoid having to include Windows.h + +#else + +/// Functionality to get the processors cycle counter +JPH_INLINE uint64 GetProcessorTickCount() +{ +#if defined(JPH_PLATFORM_BLUE) + return JPH_PLATFORM_BLUE_GET_TICKS(); +#elif defined(JPH_CPU_X86) + return __rdtsc(); +#elif defined(JPH_CPU_E2K) + return __rdtsc(); +#elif defined(JPH_CPU_ARM) && defined(JPH_USE_NEON) + uint64 val; + asm volatile("mrs %0, cntvct_el0" : "=r" (val)); + return val; +#elif defined(JPH_CPU_ARM) + return 0; // Not supported +#elif defined(JPH_CPU_WASM) + return 0; // Not supported +#else + #error Undefined +#endif +} + +#endif // JPH_PLATFORM_WINDOWS_UWP || (JPH_PLATFORM_WINDOWS && JPH_CPU_ARM) + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/UnorderedMap.h b/WickedEngine/Jolt/Core/UnorderedMap.h new file mode 100644 index 000000000..f4ae2ce96 --- /dev/null +++ b/WickedEngine/Jolt/Core/UnorderedMap.h @@ -0,0 +1,15 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +template , class KeyEqual = std::equal_to> using UnorderedMap = std::unordered_map>>; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Core/UnorderedSet.h b/WickedEngine/Jolt/Core/UnorderedSet.h new file mode 100644 index 000000000..bcb31a005 --- /dev/null +++ b/WickedEngine/Jolt/Core/UnorderedSet.h @@ -0,0 +1,15 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +template , class KeyEqual = std::equal_to> using UnorderedSet = std::unordered_set>; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/AABox.h b/WickedEngine/Jolt/Geometry/AABox.h new file mode 100644 index 000000000..f4f02668d --- /dev/null +++ b/WickedEngine/Jolt/Geometry/AABox.h @@ -0,0 +1,304 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Axis aligned box +class [[nodiscard]] AABox +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + AABox() : mMin(Vec3::sReplicate(FLT_MAX)), mMax(Vec3::sReplicate(-FLT_MAX)) { } + AABox(Vec3Arg inMin, Vec3Arg inMax) : mMin(inMin), mMax(inMax) { } + AABox(DVec3Arg inMin, DVec3Arg inMax) : mMin(inMin.ToVec3RoundDown()), mMax(inMax.ToVec3RoundUp()) { } + AABox(Vec3Arg inCenter, float inRadius) : mMin(inCenter - Vec3::sReplicate(inRadius)), mMax(inCenter + Vec3::sReplicate(inRadius)) { } + + /// Create box from 2 points + static AABox sFromTwoPoints(Vec3Arg inP1, Vec3Arg inP2) { return AABox(Vec3::sMin(inP1, inP2), Vec3::sMax(inP1, inP2)); } + + /// Get bounding box of size 2 * FLT_MAX + static AABox sBiggest() + { + return AABox(Vec3::sReplicate(-FLT_MAX), Vec3::sReplicate(FLT_MAX)); + } + + /// Comparison operators + bool operator == (const AABox &inRHS) const { return mMin == inRHS.mMin && mMax == inRHS.mMax; } + bool operator != (const AABox &inRHS) const { return mMin != inRHS.mMin || mMax != inRHS.mMax; } + + /// Reset the bounding box to an empty bounding box + void SetEmpty() + { + mMin = Vec3::sReplicate(FLT_MAX); + mMax = Vec3::sReplicate(-FLT_MAX); + } + + /// Check if the bounding box is valid (max >= min) + bool IsValid() const + { + return mMin.GetX() <= mMax.GetX() && mMin.GetY() <= mMax.GetY() && mMin.GetZ() <= mMax.GetZ(); + } + + /// Encapsulate point in bounding box + void Encapsulate(Vec3Arg inPos) + { + mMin = Vec3::sMin(mMin, inPos); + mMax = Vec3::sMax(mMax, inPos); + } + + /// Encapsulate bounding box in bounding box + void Encapsulate(const AABox &inRHS) + { + mMin = Vec3::sMin(mMin, inRHS.mMin); + mMax = Vec3::sMax(mMax, inRHS.mMax); + } + + /// Encapsulate triangle in bounding box + void Encapsulate(const Triangle &inRHS) + { + Vec3 v = Vec3::sLoadFloat3Unsafe(inRHS.mV[0]); + Encapsulate(v); + v = Vec3::sLoadFloat3Unsafe(inRHS.mV[1]); + Encapsulate(v); + v = Vec3::sLoadFloat3Unsafe(inRHS.mV[2]); + Encapsulate(v); + } + + /// Encapsulate triangle in bounding box + void Encapsulate(const VertexList &inVertices, const IndexedTriangle &inTriangle) + { + for (uint32 idx : inTriangle.mIdx) + Encapsulate(Vec3(inVertices[idx])); + } + + /// Intersect this bounding box with inOther, returns the intersection + AABox Intersect(const AABox &inOther) const + { + return AABox(Vec3::sMax(mMin, inOther.mMin), Vec3::sMin(mMax, inOther.mMax)); + } + + /// Make sure that each edge of the bounding box has a minimal length + void EnsureMinimalEdgeLength(float inMinEdgeLength) + { + Vec3 min_length = Vec3::sReplicate(inMinEdgeLength); + mMax = Vec3::sSelect(mMax, mMin + min_length, Vec3::sLess(mMax - mMin, min_length)); + } + + /// Widen the box on both sides by inVector + void ExpandBy(Vec3Arg inVector) + { + mMin -= inVector; + mMax += inVector; + } + + /// Get center of bounding box + Vec3 GetCenter() const + { + return 0.5f * (mMin + mMax); + } + + /// Get extent of bounding box (half of the size) + Vec3 GetExtent() const + { + return 0.5f * (mMax - mMin); + } + + /// Get size of bounding box + Vec3 GetSize() const + { + return mMax - mMin; + } + + /// Get surface area of bounding box + float GetSurfaceArea() const + { + Vec3 extent = mMax - mMin; + return 2.0f * (extent.GetX() * extent.GetY() + extent.GetX() * extent.GetZ() + extent.GetY() * extent.GetZ()); + } + + /// Get volume of bounding box + float GetVolume() const + { + Vec3 extent = mMax - mMin; + return extent.GetX() * extent.GetY() * extent.GetZ(); + } + + /// Check if this box contains another box + bool Contains(const AABox &inOther) const + { + return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther.mMin), Vec3::sGreaterOrEqual(mMax, inOther.mMax)).TestAllXYZTrue(); + } + + /// Check if this box contains a point + bool Contains(Vec3Arg inOther) const + { + return UVec4::sAnd(Vec3::sLessOrEqual(mMin, inOther), Vec3::sGreaterOrEqual(mMax, inOther)).TestAllXYZTrue(); + } + + /// Check if this box contains a point + bool Contains(DVec3Arg inOther) const + { + return Contains(Vec3(inOther)); + } + + /// Check if this box overlaps with another box + bool Overlaps(const AABox &inOther) const + { + return !UVec4::sOr(Vec3::sGreater(mMin, inOther.mMax), Vec3::sLess(mMax, inOther.mMin)).TestAnyXYZTrue(); + } + + /// Check if this box overlaps with a plane + bool Overlaps(const Plane &inPlane) const + { + Vec3 normal = inPlane.GetNormal(); + float dist_normal = inPlane.SignedDistance(GetSupport(normal)); + float dist_min_normal = inPlane.SignedDistance(GetSupport(-normal)); + return dist_normal * dist_min_normal <= 0.0f; // If both support points are on the same side of the plane we don't overlap + } + + /// Translate bounding box + void Translate(Vec3Arg inTranslation) + { + mMin += inTranslation; + mMax += inTranslation; + } + + /// Translate bounding box + void Translate(DVec3Arg inTranslation) + { + mMin = (DVec3(mMin) + inTranslation).ToVec3RoundDown(); + mMax = (DVec3(mMax) + inTranslation).ToVec3RoundUp(); + } + + /// Transform bounding box + AABox Transformed(Mat44Arg inMatrix) const + { + // Start with the translation of the matrix + Vec3 new_min, new_max; + new_min = new_max = inMatrix.GetTranslation(); + + // Now find the extreme points by considering the product of the min and max with each column of inMatrix + for (int c = 0; c < 3; ++c) + { + Vec3 col = inMatrix.GetColumn3(c); + + Vec3 a = col * mMin[c]; + Vec3 b = col * mMax[c]; + + new_min += Vec3::sMin(a, b); + new_max += Vec3::sMax(a, b); + } + + // Return the new bounding box + return AABox(new_min, new_max); + } + + /// Transform bounding box + AABox Transformed(DMat44Arg inMatrix) const + { + AABox transformed = Transformed(inMatrix.GetRotation()); + transformed.Translate(inMatrix.GetTranslation()); + return transformed; + } + + /// Scale this bounding box, can handle non-uniform and negative scaling + AABox Scaled(Vec3Arg inScale) const + { + return AABox::sFromTwoPoints(mMin * inScale, mMax * inScale); + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return Vec3::sSelect(mMax, mMin, Vec3::sLess(inDirection, Vec3::sZero())); + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + outVertices.resize(4); + + int axis = inDirection.Abs().GetHighestComponentIndex(); + if (inDirection[axis] < 0.0f) + { + switch (axis) + { + case 0: + outVertices[0] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + break; + + case 1: + outVertices[0] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + break; + + case 2: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + break; + } + } + else + { + switch (axis) + { + case 0: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[2] = Vec3(mMin.GetX(), mMax.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + break; + + case 1: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMin.GetY(), mMax.GetZ()); + outVertices[3] = Vec3(mMin.GetX(), mMin.GetY(), mMax.GetZ()); + break; + + case 2: + outVertices[0] = Vec3(mMin.GetX(), mMin.GetY(), mMin.GetZ()); + outVertices[1] = Vec3(mMin.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[2] = Vec3(mMax.GetX(), mMax.GetY(), mMin.GetZ()); + outVertices[3] = Vec3(mMax.GetX(), mMin.GetY(), mMin.GetZ()); + break; + } + } + } + + /// Get the closest point on or in this box to inPoint + Vec3 GetClosestPoint(Vec3Arg inPoint) const + { + return Vec3::sMin(Vec3::sMax(inPoint, mMin), mMax); + } + + /// Get the squared distance between inPoint and this box (will be 0 if in Point is inside the box) + inline float GetSqDistanceTo(Vec3Arg inPoint) const + { + return (GetClosestPoint(inPoint) - inPoint).LengthSq(); + } + + /// Bounding box min and max + Vec3 mMin; + Vec3 mMax; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/AABox4.h b/WickedEngine/Jolt/Geometry/AABox4.h new file mode 100644 index 000000000..4465d4dab --- /dev/null +++ b/WickedEngine/Jolt/Geometry/AABox4.h @@ -0,0 +1,224 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions that process 4 axis aligned boxes at the same time using SIMD +/// Test if 4 bounding boxes overlap with 1 bounding box, splat 1 box +JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ) +{ + // Splat values of box 1 + Vec4 box1_minx = inBox1.mMin.SplatX(); + Vec4 box1_miny = inBox1.mMin.SplatY(); + Vec4 box1_minz = inBox1.mMin.SplatZ(); + Vec4 box1_maxx = inBox1.mMax.SplatX(); + Vec4 box1_maxy = inBox1.mMax.SplatY(); + Vec4 box1_maxz = inBox1.mMax.SplatZ(); + + // Test separation over each axis + UVec4 nooverlapx = UVec4::sOr(Vec4::sGreater(box1_minx, inBox2MaxX), Vec4::sGreater(inBox2MinX, box1_maxx)); + UVec4 nooverlapy = UVec4::sOr(Vec4::sGreater(box1_miny, inBox2MaxY), Vec4::sGreater(inBox2MinY, box1_maxy)); + UVec4 nooverlapz = UVec4::sOr(Vec4::sGreater(box1_minz, inBox2MaxZ), Vec4::sGreater(inBox2MinZ, box1_maxz)); + + // Return overlap + return UVec4::sNot(UVec4::sOr(UVec4::sOr(nooverlapx, nooverlapy), nooverlapz)); +} + +/// Scale 4 axis aligned boxes +JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ) +{ + Vec4 scale_x = inScale.SplatX(); + Vec4 scaled_min_x = scale_x * inBoxMinX; + Vec4 scaled_max_x = scale_x * inBoxMaxX; + outBoundsMinX = Vec4::sMin(scaled_min_x, scaled_max_x); // Negative scale can flip min and max + outBoundsMaxX = Vec4::sMax(scaled_min_x, scaled_max_x); + + Vec4 scale_y = inScale.SplatY(); + Vec4 scaled_min_y = scale_y * inBoxMinY; + Vec4 scaled_max_y = scale_y * inBoxMaxY; + outBoundsMinY = Vec4::sMin(scaled_min_y, scaled_max_y); + outBoundsMaxY = Vec4::sMax(scaled_min_y, scaled_max_y); + + Vec4 scale_z = inScale.SplatZ(); + Vec4 scaled_min_z = scale_z * inBoxMinZ; + Vec4 scaled_max_z = scale_z * inBoxMaxZ; + outBoundsMinZ = Vec4::sMin(scaled_min_z, scaled_max_z); + outBoundsMaxZ = Vec4::sMax(scaled_min_z, scaled_max_z); +} + +/// Enlarge 4 bounding boxes with extent (add to both sides) +JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ) +{ + Vec4 extent_x = inExtent.SplatX(); + ioBoundsMinX -= extent_x; + ioBoundsMaxX += extent_x; + + Vec4 extent_y = inExtent.SplatY(); + ioBoundsMinY -= extent_y; + ioBoundsMaxY += extent_y; + + Vec4 extent_z = inExtent.SplatZ(); + ioBoundsMinZ -= extent_z; + ioBoundsMaxZ += extent_z; +} + +/// Test if 4 bounding boxes overlap with a point +JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Splat point to 4 component vectors + Vec4 point_x = Vec4(inPoint).SplatX(); + Vec4 point_y = Vec4(inPoint).SplatY(); + Vec4 point_z = Vec4(inPoint).SplatZ(); + + // Test if point overlaps with box + UVec4 overlapx = UVec4::sAnd(Vec4::sGreaterOrEqual(point_x, inBoxMinX), Vec4::sLessOrEqual(point_x, inBoxMaxX)); + UVec4 overlapy = UVec4::sAnd(Vec4::sGreaterOrEqual(point_y, inBoxMinY), Vec4::sLessOrEqual(point_y, inBoxMaxY)); + UVec4 overlapz = UVec4::sAnd(Vec4::sGreaterOrEqual(point_z, inBoxMinZ), Vec4::sLessOrEqual(point_z, inBoxMaxZ)); + + // Test if all are overlapping + return UVec4::sAnd(UVec4::sAnd(overlapx, overlapy), overlapz); +} + +/// Test if 4 bounding boxes overlap with an oriented box +JPH_INLINE UVec4 AABox4VsBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f) +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box) + + // Compute translation vector t (the translation of B in the space of A) + Vec4 t[3] { + inOrientation.GetTranslation().SplatX() - 0.5f * (inBoxMinX + inBoxMaxX), + inOrientation.GetTranslation().SplatY() - 0.5f * (inBoxMinY + inBoxMaxY), + inOrientation.GetTranslation().SplatZ() - 0.5f * (inBoxMinZ + inBoxMaxZ) }; + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { inOrientation.GetAxisX().Abs() + epsilon, inOrientation.GetAxisY().Abs() + epsilon, inOrientation.GetAxisZ().Abs() + epsilon }; + + // Half extents for a + Vec4 a_half_extents[3] { + 0.5f * (inBoxMaxX - inBoxMinX), + 0.5f * (inBoxMaxY - inBoxMinY), + 0.5f * (inBoxMaxZ - inBoxMinZ) }; + + // Half extents of b + Vec4 b_half_extents_x = inHalfExtents.SplatX(); + Vec4 b_half_extents_y = inHalfExtents.SplatY(); + Vec4 b_half_extents_z = inHalfExtents.SplatZ(); + + // Each component corresponds to 1 overlapping OBB vs ABB + UVec4 overlaps = UVec4(0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff); + + // Test axes L = A0, L = A1, L = A2 + Vec4 ra, rb; + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[i]; + rb = b_half_extents_x * abs_r[0][i] + b_half_extents_y * abs_r[1][i] + b_half_extents_z * abs_r[2][i]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual(t[i].Abs(), ra + rb)); + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[0] * abs_r[i][0] + a_half_extents[1] * abs_r[i][1] + a_half_extents[2] * abs_r[i][2]; + rb = Vec4::sReplicate(inHalfExtents[i]); + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(0, i) + t[1] * inOrientation(1, i) + t[2] * inOrientation(2, i)).Abs(), ra + rb)); + } + + // Test axis L = A0 x B0 + ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1]; + rb = b_half_extents_y * abs_r[2][0] + b_half_extents_z * abs_r[1][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 0) - t[1] * inOrientation(2, 0)).Abs(), ra + rb)); + + // Test axis L = A0 x B1 + ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1]; + rb = b_half_extents_x * abs_r[2][0] + b_half_extents_z * abs_r[0][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 1) - t[1] * inOrientation(2, 1)).Abs(), ra + rb)); + + // Test axis L = A0 x B2 + ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1]; + rb = b_half_extents_x * abs_r[1][0] + b_half_extents_y * abs_r[0][0]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 2) - t[1] * inOrientation(2, 2)).Abs(), ra + rb)); + + // Test axis L = A1 x B0 + ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0]; + rb = b_half_extents_y * abs_r[2][1] + b_half_extents_z * abs_r[1][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 0) - t[2] * inOrientation(0, 0)).Abs(), ra + rb)); + + // Test axis L = A1 x B1 + ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0]; + rb = b_half_extents_x * abs_r[2][1] + b_half_extents_z * abs_r[0][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 1) - t[2] * inOrientation(0, 1)).Abs(), ra + rb)); + + // Test axis L = A1 x B2 + ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0]; + rb = b_half_extents_x * abs_r[1][1] + b_half_extents_y * abs_r[0][1]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 2) - t[2] * inOrientation(0, 2)).Abs(), ra + rb)); + + // Test axis L = A2 x B0 + ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0]; + rb = b_half_extents_y * abs_r[2][2] + b_half_extents_z * abs_r[1][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 0) - t[0] * inOrientation(1, 0)).Abs(), ra + rb)); + + // Test axis L = A2 x B1 + ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0]; + rb = b_half_extents_x * abs_r[2][2] + b_half_extents_z * abs_r[0][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 1) - t[0] * inOrientation(1, 1)).Abs(), ra + rb)); + + // Test axis L = A2 x B2 + ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0]; + rb = b_half_extents_x * abs_r[1][2] + b_half_extents_y * abs_r[0][2]; + overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 2) - t[0] * inOrientation(1, 2)).Abs(), ra + rb)); + + // Return if the OBB vs AABBs are intersecting + return overlaps; +} + +/// Convenience function that tests 4 AABoxes vs OrientedBox +JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f) +{ + return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon); +} + +/// Get the squared distance between 4 AABoxes and a point +JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec4Arg inPointX, Vec4Arg inPointY, Vec4Arg inPointZ, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Get closest point on box + Vec4 closest_x = Vec4::sMin(Vec4::sMax(inPointX, inBoxMinX), inBoxMaxX); + Vec4 closest_y = Vec4::sMin(Vec4::sMax(inPointY, inBoxMinY), inBoxMaxY); + Vec4 closest_z = Vec4::sMin(Vec4::sMax(inPointZ, inBoxMinZ), inBoxMaxZ); + + // Return the squared distance between the box and point + return Square(closest_x - inPointX) + Square(closest_y - inPointY) + Square(closest_z - inPointZ); +} + +/// Get the squared distance between 4 AABoxes and a point +JPH_INLINE Vec4 AABox4DistanceSqToPoint(Vec3 inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + return AABox4DistanceSqToPoint(inPoint.SplatX(), inPoint.SplatY(), inPoint.SplatZ(), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); +} + +/// Test 4 AABoxes vs a sphere +JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + // Test the distance from the center of the sphere to the box is smaller than the radius + Vec4 distance_sq = AABox4DistanceSqToPoint(inCenterX, inCenterY, inCenterZ, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); + return Vec4::sLessOrEqual(distance_sq, inRadiusSq); +} + +/// Test 4 AABoxes vs a sphere +JPH_INLINE UVec4 AABox4VsSphere(Vec3Arg inCenter, float inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ) +{ + return AABox4VsSphere(inCenter.SplatX(), inCenter.SplatY(), inCenter.SplatZ(), Vec4::sReplicate(inRadiusSq), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/ClipPoly.h b/WickedEngine/Jolt/Geometry/ClipPoly.h new file mode 100644 index 000000000..e0ef4f22b --- /dev/null +++ b/WickedEngine/Jolt/Geometry/ClipPoly.h @@ -0,0 +1,200 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Clip inPolygonToClip against the positive halfspace of plane defined by inPlaneOrigin and inPlaneNormal. +/// inPlaneNormal does not need to be normalized. +template +void ClipPolyVsPlane(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inPlaneOrigin, Vec3Arg inPlaneNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + JPH_ASSERT(outClippedPolygon.empty()); + + // Determine state of last point + Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1]; + float prev_num = (inPlaneOrigin - e1).Dot(inPlaneNormal); + bool prev_inside = prev_num < 0.0f; + + // Loop through all vertices + for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j) + { + // Check if second point is inside + Vec3Arg e2 = inPolygonToClip[j]; + float num = (inPlaneOrigin - e2).Dot(inPlaneNormal); + bool cur_inside = num < 0.0f; + + // In -> Out or Out -> In: Add point on clipping plane + if (cur_inside != prev_inside) + { + // Solve: (X - inPlaneOrigin) . inPlaneNormal = 0 and X = e1 + t * (e2 - e1) for X + Vec3 e12 = e2 - e1; + float denom = e12.Dot(inPlaneNormal); + if (denom != 0.0f) + outClippedPolygon.push_back(e1 + (prev_num / denom) * e12); + else + cur_inside = prev_inside; // Edge is parallel to plane, treat point as if it were on the same side as the last point + } + + // Point inside, add it + if (cur_inside) + outClippedPolygon.push_back(e2); + + // Update previous state + prev_num = num; + prev_inside = cur_inside; + e1 = e2; + } +} + +/// Clip polygon versus polygon. +/// Both polygons are assumed to be in counter clockwise order. +/// @param inClippingPolygonNormal is used to create planes of all edges in inClippingPolygon against which inPolygonToClip is clipped, inClippingPolygonNormal does not need to be normalized +/// @param inClippingPolygon is the polygon which inClippedPolygon is clipped against +/// @param inPolygonToClip is the polygon that is clipped +/// @param outClippedPolygon will contain clipped polygon when function returns +template +void ClipPolyVsPoly(const VERTEX_ARRAY &inPolygonToClip, const VERTEX_ARRAY &inClippingPolygon, Vec3Arg inClippingPolygonNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + JPH_ASSERT(inClippingPolygon.size() >= 3); + + VERTEX_ARRAY tmp_vertices[2]; + int tmp_vertices_idx = 0; + + for (typename VERTEX_ARRAY::size_type i = 0; i < inClippingPolygon.size(); ++i) + { + // Get edge to clip against + Vec3 clip_e1 = inClippingPolygon[i]; + Vec3 clip_e2 = inClippingPolygon[(i + 1) % inClippingPolygon.size()]; + Vec3 clip_normal = inClippingPolygonNormal.Cross(clip_e2 - clip_e1); // Pointing inward to the clipping polygon + + // Get source and target polygon + const VERTEX_ARRAY &src_polygon = (i == 0)? inPolygonToClip : tmp_vertices[tmp_vertices_idx]; + tmp_vertices_idx ^= 1; + VERTEX_ARRAY &tgt_polygon = (i == inClippingPolygon.size() - 1)? outClippedPolygon : tmp_vertices[tmp_vertices_idx]; + tgt_polygon.clear(); + + // Clip against the edge + ClipPolyVsPlane(src_polygon, clip_e1, clip_normal, tgt_polygon); + + // Break out if no polygon left + if (tgt_polygon.size() < 3) + { + outClippedPolygon.clear(); + break; + } + } +} + +/// Clip inPolygonToClip against an edge, the edge is projected on inPolygonToClip using inClippingEdgeNormal. +/// The positive half space (the side on the edge in the direction of inClippingEdgeNormal) is cut away. +template +void ClipPolyVsEdge(const VERTEX_ARRAY &inPolygonToClip, Vec3Arg inEdgeVertex1, Vec3Arg inEdgeVertex2, Vec3Arg inClippingEdgeNormal, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 3); + JPH_ASSERT(outClippedPolygon.empty()); + + // Get normal that is perpendicular to the edge and the clipping edge normal + Vec3 edge = inEdgeVertex2 - inEdgeVertex1; + Vec3 edge_normal = inClippingEdgeNormal.Cross(edge); + + // Project vertices of edge on inPolygonToClip + Vec3 polygon_normal = (inPolygonToClip[2] - inPolygonToClip[0]).Cross(inPolygonToClip[1] - inPolygonToClip[0]); + float polygon_normal_len_sq = polygon_normal.LengthSq(); + Vec3 v1 = inEdgeVertex1 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex1) * polygon_normal / polygon_normal_len_sq; + Vec3 v2 = inEdgeVertex2 + polygon_normal.Dot(inPolygonToClip[0] - inEdgeVertex2) * polygon_normal / polygon_normal_len_sq; + Vec3 v12 = v2 - v1; + float v12_len_sq = v12.LengthSq(); + + // Determine state of last point + Vec3 e1 = inPolygonToClip[inPolygonToClip.size() - 1]; + float prev_num = (inEdgeVertex1 - e1).Dot(edge_normal); + bool prev_inside = prev_num < 0.0f; + + // Loop through all vertices + for (typename VERTEX_ARRAY::size_type j = 0; j < inPolygonToClip.size(); ++j) + { + // Check if second point is inside + Vec3 e2 = inPolygonToClip[j]; + float num = (inEdgeVertex1 - e2).Dot(edge_normal); + bool cur_inside = num < 0.0f; + + // In -> Out or Out -> In: Add point on clipping plane + if (cur_inside != prev_inside) + { + // Solve: (X - inPlaneOrigin) . inPlaneNormal = 0 and X = e1 + t * (e2 - e1) for X + Vec3 e12 = e2 - e1; + float denom = e12.Dot(edge_normal); + Vec3 clipped_point = e1 + (prev_num / denom) * e12; + + // Project point on line segment v1, v2 so see if it falls outside if the edge + float projection = (clipped_point - v1).Dot(v12); + if (projection < 0.0f) + outClippedPolygon.push_back(v1); + else if (projection > v12_len_sq) + outClippedPolygon.push_back(v2); + else + outClippedPolygon.push_back(clipped_point); + } + + // Update previous state + prev_num = num; + prev_inside = cur_inside; + e1 = e2; + } +} + +/// Clip polygon vs axis aligned box, inPolygonToClip is assume to be in counter clockwise order. +/// Output will be stored in outClippedPolygon. Everything inside inAABox will be kept. +template +void ClipPolyVsAABox(const VERTEX_ARRAY &inPolygonToClip, const AABox &inAABox, VERTEX_ARRAY &outClippedPolygon) +{ + JPH_ASSERT(inPolygonToClip.size() >= 2); + + VERTEX_ARRAY tmp_vertices[2]; + int tmp_vertices_idx = 0; + + for (int coord = 0; coord < 3; ++coord) + for (int side = 0; side < 2; ++side) + { + // Get plane to clip against + Vec3 origin = Vec3::sZero(), normal = Vec3::sZero(); + if (side == 0) + { + normal.SetComponent(coord, 1.0f); + origin.SetComponent(coord, inAABox.mMin[coord]); + } + else + { + normal.SetComponent(coord, -1.0f); + origin.SetComponent(coord, inAABox.mMax[coord]); + } + + // Get source and target polygon + const VERTEX_ARRAY &src_polygon = tmp_vertices_idx == 0? inPolygonToClip : tmp_vertices[tmp_vertices_idx & 1]; + tmp_vertices_idx++; + VERTEX_ARRAY &tgt_polygon = tmp_vertices_idx == 6? outClippedPolygon : tmp_vertices[tmp_vertices_idx & 1]; + tgt_polygon.clear(); + + // Clip against the edge + ClipPolyVsPlane(src_polygon, origin, normal, tgt_polygon); + + // Break out if no polygon left + if (tgt_polygon.size() < 3) + { + outClippedPolygon.clear(); + return; + } + + // Flip normal + normal = -normal; + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/ClosestPoint.h b/WickedEngine/Jolt/Geometry/ClosestPoint.h new file mode 100644 index 000000000..a437763f5 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/ClosestPoint.h @@ -0,0 +1,498 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +// Turn off fused multiply add instruction because it makes the equations of the form a * b - c * d inaccurate below +JPH_PRECISE_MATH_ON + +/// Helper utils to find the closest point to a line segment, triangle or tetrahedron +namespace ClosestPoint +{ + /// Compute barycentric coordinates of closest point to origin for infinite line defined by (inA, inB) + /// Point can then be computed as inA * outU + inB * outV + /// Returns false if the points inA, inB do not form a line (are at the same point) + inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV) + { + Vec3 ab = inB - inA; + float denominator = ab.LengthSq(); + if (denominator < Square(FLT_EPSILON)) + { + // Degenerate line segment, fallback to points + if (inA.LengthSq() < inB.LengthSq()) + { + // A closest + outU = 1.0f; + outV = 0.0f; + } + else + { + // B closest + outU = 0.0f; + outV = 1.0f; + } + return false; + } + else + { + outV = -inA.Dot(ab) / denominator; + outU = 1.0f - outV; + } + return true; + } + + /// Compute barycentric coordinates of closest point to origin for plane defined by (inA, inB, inC) + /// Point can then be computed as inA * outU + inB * outV + inC * outW + /// Returns false if the points inA, inB, inC do not form a plane (are on the same line or at the same point) + inline bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, float &outU, float &outV, float &outW) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Barycentric Coordinates) + // With p = 0 + // Adjusted to always include the shortest edge of the triangle in the calculation to improve numerical accuracy + + // First calculate the three edges + Vec3 v0 = inB - inA; + Vec3 v1 = inC - inA; + Vec3 v2 = inC - inB; + + // Make sure that the shortest edge is included in the calculation to keep the products a * b - c * d as small as possible to preserve accuracy + float d00 = v0.LengthSq(); + float d11 = v1.LengthSq(); + float d22 = v2.LengthSq(); + if (d00 <= d22) + { + // Use v0 and v1 to calculate barycentric coordinates + float d01 = v0.Dot(v1); + + // Denominator must be positive: + // |v0|^2 * |v1|^2 - (v0 . v1)^2 = |v0|^2 * |v1|^2 * (1 - cos(angle)^2) >= 0 + float denominator = d00 * d11 - d01 * d01; + if (denominator < 1.0e-12f) + { + // Degenerate triangle, return coordinates along longest edge + if (d00 > d11) + { + GetBaryCentricCoordinates(inA, inB, outU, outV); + outW = 0.0f; + } + else + { + GetBaryCentricCoordinates(inA, inC, outU, outW); + outV = 0.0f; + } + return false; + } + else + { + float a0 = inA.Dot(v0); + float a1 = inA.Dot(v1); + outV = (d01 * a1 - d11 * a0) / denominator; + outW = (d01 * a0 - d00 * a1) / denominator; + outU = 1.0f - outV - outW; + } + } + else + { + // Use v1 and v2 to calculate barycentric coordinates + float d12 = v1.Dot(v2); + + float denominator = d11 * d22 - d12 * d12; + if (denominator < 1.0e-12f) + { + // Degenerate triangle, return coordinates along longest edge + if (d11 > d22) + { + GetBaryCentricCoordinates(inA, inC, outU, outW); + outV = 0.0f; + } + else + { + GetBaryCentricCoordinates(inB, inC, outV, outW); + outU = 0.0f; + } + return false; + } + else + { + float c1 = inC.Dot(v1); + float c2 = inC.Dot(v2); + outU = (d22 * c1 - d12 * c2) / denominator; + outV = (d11 * c2 - d12 * c1) / denominator; + outW = 1.0f - outU - outV; + } + } + return true; + } + + /// Get the closest point to the origin of line (inA, inB) + /// outSet describes which features are closest: 1 = a, 2 = b, 3 = line segment ab + inline Vec3 GetClosestPointOnLine(Vec3Arg inA, Vec3Arg inB, uint32 &outSet) + { + float u, v; + GetBaryCentricCoordinates(inA, inB, u, v); + if (v <= 0.0f) + { + // inA is closest point + outSet = 0b0001; + return inA; + } + else if (u <= 0.0f) + { + // inB is closest point + outSet = 0b0010; + return inB; + } + else + { + // Closest point lies on line inA inB + outSet = 0b0011; + return u * inA + v * inB; + } + } + + /// Get the closest point to the origin of triangle (inA, inB, inC) + /// outSet describes which features are closest: 1 = a, 2 = b, 4 = c, 5 = line segment ac, 7 = triangle interior etc. + /// If MustIncludeC is true, the function assumes that C is part of the closest feature (vertex, edge, face) and does less work, if the assumption is not true then a closest point to the other features is returned. + template + inline Vec3 GetClosestPointOnTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, uint32 &outSet) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Triangle to Point) + // With p = 0 + + // The most accurate normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter and if bc is shorter than ac then we swap a with c to a is always on the shortest edge + UVec4 swap_ac; + { + Vec3 ac = inC - inA; + Vec3 bc = inC - inB; + swap_ac = Vec4::sLess(bc.DotV4(bc), ac.DotV4(ac)); + } + Vec3 a = Vec3::sSelect(inA, inC, swap_ac); + Vec3 c = Vec3::sSelect(inC, inA, swap_ac); + + // Calculate normal + Vec3 ab = inB - a; + Vec3 ac = c - a; + Vec3 n = ab.Cross(ac); + float n_len_sq = n.LengthSq(); + + // Check degenerate + if (n_len_sq < 1.0e-10f) // Square(FLT_EPSILON) was too small and caused numerical problems, see test case TestCollideParallelTriangleVsCapsule + { + // Degenerate, fallback to vertices and edges + + // Start with vertex C being the closest + uint32 closest_set = 0b0100; + Vec3 closest_point = inC; + float best_dist_sq = inC.LengthSq(); + + // If the closest point must include C then A or B cannot be closest + // Note that we test vertices first because we want to prefer a closest vertex over a closest edge (this results in an outSet with fewer bits set) + if constexpr (!MustIncludeC) + { + // Try vertex A + float a_len_sq = inA.LengthSq(); + if (a_len_sq < best_dist_sq) + { + closest_set = 0b0001; + closest_point = inA; + best_dist_sq = a_len_sq; + } + + // Try vertex B + float b_len_sq = inB.LengthSq(); + if (b_len_sq < best_dist_sq) + { + closest_set = 0b0010; + closest_point = inB; + best_dist_sq = b_len_sq; + } + } + + // Edge AC + float ac_len_sq = ac.LengthSq(); + if (ac_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-a.Dot(ac) / ac_len_sq, 0.0f, 1.0f); + Vec3 q = a + v * ac; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0101; + closest_point = q; + best_dist_sq = dist_sq; + } + } + + // Edge BC + Vec3 bc = inC - inB; + float bc_len_sq = bc.LengthSq(); + if (bc_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-inB.Dot(bc) / bc_len_sq, 0.0f, 1.0f); + Vec3 q = inB + v * bc; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0110; + closest_point = q; + best_dist_sq = dist_sq; + } + } + + // If the closest point must include C then AB cannot be closest + if constexpr (!MustIncludeC) + { + // Edge AB + ab = inB - inA; + float ab_len_sq = ab.LengthSq(); + if (ab_len_sq > Square(FLT_EPSILON)) + { + float v = Clamp(-inA.Dot(ab) / ab_len_sq, 0.0f, 1.0f); + Vec3 q = inA + v * ab; + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_set = 0b0011; + closest_point = q; + best_dist_sq = dist_sq; + } + } + } + + outSet = closest_set; + return closest_point; + } + + // Check if P in vertex region outside A + Vec3 ap = -a; + float d1 = ab.Dot(ap); + float d2 = ac.Dot(ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + outSet = swap_ac.GetX()? 0b0100 : 0b0001; + return a; // barycentric coordinates (1,0,0) + } + + // Check if P in vertex region outside B + Vec3 bp = -inB; + float d3 = ab.Dot(bp); + float d4 = ac.Dot(bp); + if (d3 >= 0.0f && d4 <= d3) + { + outSet = 0b0010; + return inB; // barycentric coordinates (0,1,0) + } + + // Check if P in edge region of AB, if so return projection of P onto AB + if (d1 * d4 <= d3 * d2 && d1 >= 0.0f && d3 <= 0.0f) + { + float v = d1 / (d1 - d3); + outSet = swap_ac.GetX()? 0b0110 : 0b0011; + return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + Vec3 cp = -c; + float d5 = ab.Dot(cp); + float d6 = ac.Dot(cp); + if (d6 >= 0.0f && d5 <= d6) + { + outSet = swap_ac.GetX()? 0b0001 : 0b0100; + return c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + if (d5 * d2 <= d1 * d6 && d2 >= 0.0f && d6 <= 0.0f) + { + float w = d2 / (d2 - d6); + outSet = 0b0101; + 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 + float d4_d3 = d4 - d3; + float d5_d6 = d5 - d6; + if (d3 * d6 <= d5 * d4 && d4_d3 >= 0.0f && d5_d6 >= 0.0f) + { + float w = d4_d3 / (d4_d3 + d5_d6); + outSet = swap_ac.GetX()? 0b0011 : 0b0110; + return inB + w * (c - inB); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. + // Here we deviate from Christer Ericson's article to improve accuracy. + // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal| + // Closest point to origin is then: distance . normal / |normal| + // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates + // and then calculating the closest point based on those coordinates. + outSet = 0b0111; + return n * (a + inB + c).Dot(n) / (3.0f * n_len_sq); + } + + /// Check if the origin is outside the plane of triangle (inA, inB, inC). inD specifies the front side of the plane. + inline bool OriginOutsideOfPlane(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point) + // With p = 0 + + // Test if point p and d lie on opposite sides of plane through abc + Vec3 n = (inB - inA).Cross(inC - inA); + float signp = inA.Dot(n); // [AP AB AC] + float signd = (inD - inA).Dot(n); // [AD AB AC] + + // Points on opposite sides if expression signs are the same + // Note that we left out the minus sign in signp so we need to check > 0 instead of < 0 as in Christer's book + // We compare against a small negative value to allow for a little bit of slop in the calculations + return signp * signd > -FLT_EPSILON; + } + + /// Returns for each of the planes of the tetrahedron if the origin is inside it + /// Roughly equivalent to: + /// [OriginOutsideOfPlane(inA, inB, inC, inD), + /// OriginOutsideOfPlane(inA, inC, inD, inB), + /// OriginOutsideOfPlane(inA, inD, inB, inC), + /// OriginOutsideOfPlane(inB, inD, inC, inA)] + inline UVec4 OriginOutsideOfTetrahedronPlanes(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD) + { + Vec3 ab = inB - inA; + Vec3 ac = inC - inA; + Vec3 ad = inD - inA; + Vec3 bd = inD - inB; + Vec3 bc = inC - inB; + + Vec3 ab_cross_ac = ab.Cross(ac); + Vec3 ac_cross_ad = ac.Cross(ad); + Vec3 ad_cross_ab = ad.Cross(ab); + Vec3 bd_cross_bc = bd.Cross(bc); + + // For each plane get the side on which the origin is + float signp0 = inA.Dot(ab_cross_ac); // ABC + float signp1 = inA.Dot(ac_cross_ad); // ACD + float signp2 = inA.Dot(ad_cross_ab); // ADB + float signp3 = inB.Dot(bd_cross_bc); // BDC + Vec4 signp(signp0, signp1, signp2, signp3); + + // For each plane get the side that is outside (determined by the 4th point) + float signd0 = ad.Dot(ab_cross_ac); // D + float signd1 = ab.Dot(ac_cross_ad); // B + float signd2 = ac.Dot(ad_cross_ab); // C + float signd3 = -ab.Dot(bd_cross_bc); // A + Vec4 signd(signd0, signd1, signd2, signd3); + + // The winding of all triangles has been chosen so that signd should have the + // same sign for all components. If this is not the case the tetrahedron + // is degenerate and we return that the origin is in front of all sides + int sign_bits = signd.GetSignBits(); + switch (sign_bits) + { + case 0: + // All positive + return Vec4::sGreaterOrEqual(signp, Vec4::sReplicate(-FLT_EPSILON)); + + case 0xf: + // All negative + return Vec4::sLessOrEqual(signp, Vec4::sReplicate(FLT_EPSILON)); + + default: + // Mixed signs, degenerate tetrahedron + return UVec4::sReplicate(0xffffffff); + } + } + + /// Get the closest point between tetrahedron (inA, inB, inC, inD) to the origin + /// outSet specifies which feature was closest, 1 = a, 2 = b, 4 = c, 8 = d. Edges have 2 bits set, triangles 3 and if the point is in the interior 4 bits are set. + /// If MustIncludeD is true, the function assumes that D is part of the closest feature (vertex, edge, face, tetrahedron) and does less work, if the assumption is not true then a closest point to the other features is returned. + template + inline Vec3 GetClosestPointOnTetrahedron(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inD, uint32 &outSet) + { + // Taken from: Real-Time Collision Detection - Christer Ericson (Section: Closest Point on Tetrahedron to Point) + // With p = 0 + + // Start out assuming point inside all halfspaces, so closest to itself + uint32 closest_set = 0b1111; + Vec3 closest_point = Vec3::sZero(); + float best_dist_sq = FLT_MAX; + + // Determine for each of the faces of the tetrahedron if the origin is in front of the plane + UVec4 origin_out_of_planes = OriginOutsideOfTetrahedronPlanes(inA, inB, inC, inD); + + // If point outside face abc then compute closest point on abc + if (origin_out_of_planes.GetX()) // OriginOutsideOfPlane(inA, inB, inC, inD) + { + if constexpr (MustIncludeD) + { + // If the closest point must include D then ABC cannot be closest but the closest point + // cannot be an interior point either so we return A as closest point + closest_set = 0b0001; + closest_point = inA; + } + else + { + // Test the face normally + closest_point = GetClosestPointOnTriangle(inA, inB, inC, closest_set); + } + best_dist_sq = closest_point.LengthSq(); + } + + // Repeat test for face acd + if (origin_out_of_planes.GetY()) // OriginOutsideOfPlane(inA, inC, inD, inB) + { + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inA, inC, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_dist_sq = dist_sq; + closest_point = q; + closest_set = (set & 0b0001) + ((set & 0b0110) << 1); + } + } + + // Repeat test for face adb + if (origin_out_of_planes.GetZ()) // OriginOutsideOfPlane(inA, inD, inB, inC) + { + // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward + // and it improves consistency for GJK which will always add a new vertex D and keep the closest + // feature from the previous iteration in ABC + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inA, inB, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_dist_sq = dist_sq; + closest_point = q; + closest_set = (set & 0b0011) + ((set & 0b0100) << 1); + } + } + + // Repeat test for face bdc + if (origin_out_of_planes.GetW()) // OriginOutsideOfPlane(inB, inD, inC, inA) + { + // Keep original vertex order, it doesn't matter if the triangle is facing inward or outward + // and it improves consistency for GJK which will always add a new vertex D and keep the closest + // feature from the previous iteration in ABC + uint32 set; + Vec3 q = GetClosestPointOnTriangle(inB, inC, inD, set); + float dist_sq = q.LengthSq(); + if (dist_sq < best_dist_sq) + { + closest_point = q; + closest_set = set << 1; + } + } + + outSet = closest_set; + return closest_point; + } +}; + +JPH_PRECISE_MATH_OFF + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/ConvexHullBuilder.cpp b/WickedEngine/Jolt/Geometry/ConvexHullBuilder.cpp new file mode 100644 index 000000000..68578e4fa --- /dev/null +++ b/WickedEngine/Jolt/Geometry/ConvexHullBuilder.cpp @@ -0,0 +1,1464 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#ifdef JPH_CONVEX_BUILDER_DEBUG + #include +#endif + +JPH_NAMESPACE_BEGIN + +ConvexHullBuilder::Face::~Face() +{ + // Free all edges + Edge *e = mFirstEdge; + if (e != nullptr) + { + do + { + Edge *next = e->mNextEdge; + delete e; + e = next; + } while (e != mFirstEdge); + } +} + +void ConvexHullBuilder::Face::CalculateNormalAndCentroid(const Vec3 *inPositions) +{ + // Get point that we use to construct a triangle fan + Edge *e = mFirstEdge; + Vec3 y0 = inPositions[e->mStartIdx]; + + // Get the 2nd point + e = e->mNextEdge; + Vec3 y1 = inPositions[e->mStartIdx]; + + // Start accumulating the centroid + mCentroid = y0 + y1; + int n = 2; + + // Start accumulating the normal + mNormal = Vec3::sZero(); + + // Loop over remaining edges accumulating normals in a triangle fan fashion + for (e = e->mNextEdge; e != mFirstEdge; e = e->mNextEdge) + { + // Get the 3rd point + Vec3 y2 = inPositions[e->mStartIdx]; + + // Calculate edges (counter clockwise) + Vec3 e0 = y1 - y0; + Vec3 e1 = y2 - y1; + Vec3 e2 = y0 - y2; + + // The best normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the others must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter: e1 or e2 + UVec4 e1_shorter_than_e2 = Vec4::sLess(e1.DotV4(e1), e2.DotV4(e2)); + + // We calculate both normals and then select the one that had the shortest edge for our normal (this avoids branching) + Vec3 normal_e01 = e0.Cross(e1); + Vec3 normal_e02 = e2.Cross(e0); + mNormal += Vec3::sSelect(normal_e02, normal_e01, e1_shorter_than_e2); + + // Accumulate centroid + mCentroid += y2; + n++; + + // Update y1 for next triangle + y1 = y2; + } + + // Finalize centroid + mCentroid /= float(n); +} + +void ConvexHullBuilder::Face::Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) +{ + JPH_ASSERT(mFirstEdge == nullptr); + JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2); + + // Create 3 edges + Edge *e0 = new Edge(this, inIdx0); + Edge *e1 = new Edge(this, inIdx1); + Edge *e2 = new Edge(this, inIdx2); + + // Link edges + e0->mNextEdge = e1; + e1->mNextEdge = e2; + e2->mNextEdge = e0; + mFirstEdge = e0; + + CalculateNormalAndCentroid(inPositions); +} + +ConvexHullBuilder::ConvexHullBuilder(const Positions &inPositions) : + mPositions(inPositions) +{ +#ifdef JPH_CONVEX_BUILDER_DEBUG + mIteration = 0; + + // Center the drawing of the first hull around the origin and calculate the delta offset between states + mOffset = RVec3::sZero(); + if (mPositions.empty()) + { + // No hull will be generated + mDelta = Vec3::sZero(); + } + else + { + Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX); + for (Vec3 v : mPositions) + { + minv = Vec3::sMin(minv, v); + maxv = Vec3::sMax(maxv, v); + mOffset -= v; + } + mOffset /= Real(mPositions.size()); + mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0); + mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there + } +#endif +} + +void ConvexHullBuilder::FreeFaces() +{ + for (Face *f : mFaces) + delete f; + mFaces.clear(); +} + +void ConvexHullBuilder::GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const +{ + outFace = nullptr; + outDistSq = 0.0f; + + for (Face *f : inFaces) + if (!f->mRemoved) + { + // Determine distance to face + float dot = f->mNormal.Dot(inPoint - f->mCentroid); + if (dot > 0.0f) + { + float dist_sq = dot * dot / f->mNormal.LengthSq(); + if (dist_sq > outDistSq) + { + outFace = f; + outDistSq = dist_sq; + } + } + } +} + +float ConvexHullBuilder::GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const +{ + bool all_inside = true; + float edge_dist_sq = FLT_MAX; + + // Test if it is inside the edges of the polygon + Edge *edge = inFace->mFirstEdge; + Vec3 p1 = mPositions[edge->GetPreviousEdge()->mStartIdx]; + do + { + Vec3 p2 = mPositions[edge->mStartIdx]; + if ((p2 - p1).Cross(inPoint - p1).Dot(inFace->mNormal) < 0.0f) + { + // It is outside + all_inside = false; + + // Measure distance to this edge + uint32 s; + edge_dist_sq = min(edge_dist_sq, ClosestPoint::GetClosestPointOnLine(p1 - inPoint, p2 - inPoint, s).LengthSq()); + } + p1 = p2; + edge = edge->mNextEdge; + } while (edge != inFace->mFirstEdge); + + return all_inside? 0.0f : edge_dist_sq; +} + +bool ConvexHullBuilder::AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq) +{ + Vec3 point = mPositions[inPositionIdx]; + + // Find the face for which the point is furthest away + Face *best_face; + float best_dist_sq; + GetFaceForPoint(point, inFaces, best_face, best_dist_sq); + + if (best_face != nullptr) + { + // Check if this point is within the tolerance margin to the plane + if (best_dist_sq <= inToleranceSq) + { + // Check distance to edges + float dist_to_edge_sq = GetDistanceToEdgeSq(point, best_face); + if (dist_to_edge_sq > inToleranceSq) + { + // Point is outside of the face and too far away to discard + mCoplanarList.push_back({ inPositionIdx, dist_to_edge_sq }); + } + } + else + { + // This point is in front of the face, add it to the conflict list + if (best_dist_sq > best_face->mFurthestPointDistanceSq) + { + // This point is further away than any others, update the distance and add point as last point + best_face->mFurthestPointDistanceSq = best_dist_sq; + best_face->mConflictList.push_back(inPositionIdx); + } + else + { + // Not the furthest point, add it as the before last point + best_face->mConflictList.insert(best_face->mConflictList.begin() + best_face->mConflictList.size() - 1, inPositionIdx); + } + + return true; + } + } + + return false; +} + +float ConvexHullBuilder::DetermineCoplanarDistance() const +{ + // Formula as per: Implementing Quickhull - Dirk Gregorius. + Vec3 vmax = Vec3::sZero(); + for (Vec3 v : mPositions) + vmax = Vec3::sMax(vmax, v.Abs()); + return 3.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY() + vmax.GetZ()); +} + +int ConvexHullBuilder::GetNumVerticesUsed() const +{ + UnorderedSet used_verts; + for (Face *f : mFaces) + { + Edge *e = f->mFirstEdge; + do + { + used_verts.insert(e->mStartIdx); + e = e->mNextEdge; + } while (e != f->mFirstEdge); + } + return (int)used_verts.size(); +} + +bool ConvexHullBuilder::ContainsFace(const Array &inIndices) const +{ + for (Face *f : mFaces) + { + Edge *e = f->mFirstEdge; + Array::const_iterator index = std::find(inIndices.begin(), inIndices.end(), e->mStartIdx); + if (index != inIndices.end()) + { + size_t matches = 0; + + do + { + // Check if index matches + if (*index != e->mStartIdx) + break; + + // Increment number of matches + matches++; + + // Next index in list of inIndices + index++; + if (index == inIndices.end()) + index = inIndices.begin(); + + // Next edge + e = e->mNextEdge; + } while (e != f->mFirstEdge); + + if (matches == inIndices.size()) + return true; + } + } + + return false; +} + +ConvexHullBuilder::EResult ConvexHullBuilder::Initialize(int inMaxVertices, float inTolerance, const char *&outError) +{ + // Free the faces possibly left over from an earlier hull + FreeFaces(); + + // Test that we have at least 3 points + if (mPositions.size() < 3) + { + outError = "Need at least 3 points to make a hull"; + return EResult::TooFewPoints; + } + + // Determine a suitable tolerance for detecting that points are coplanar + float coplanar_tolerance_sq = Square(DetermineCoplanarDistance()); + + // Increase desired tolerance if accuracy doesn't allow it + float tolerance_sq = max(coplanar_tolerance_sq, Square(inTolerance)); + + // Find point furthest from the origin + int idx1 = -1; + float max_dist_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + { + float dist_sq = mPositions[i].LengthSq(); + if (dist_sq > max_dist_sq) + { + max_dist_sq = dist_sq; + idx1 = i; + } + } + JPH_ASSERT(idx1 >= 0); + + // Find point that is furthest away from this point + int idx2 = -1; + max_dist_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1) + { + float dist_sq = (mPositions[i] - mPositions[idx1]).LengthSq(); + if (dist_sq > max_dist_sq) + { + max_dist_sq = dist_sq; + idx2 = i; + } + } + JPH_ASSERT(idx2 >= 0); + + // Find point that forms the biggest triangle + int idx3 = -1; + float best_triangle_area_sq = -1.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1 && i != idx2) + { + float triangle_area_sq = (mPositions[idx1] - mPositions[i]).Cross(mPositions[idx2] - mPositions[i]).LengthSq(); + if (triangle_area_sq > best_triangle_area_sq) + { + best_triangle_area_sq = triangle_area_sq; + idx3 = i; + } + } + JPH_ASSERT(idx3 >= 0); + if (best_triangle_area_sq < cMinTriangleAreaSq) + { + outError = "Could not find a suitable initial triangle because its area was too small"; + return EResult::Degenerate; + } + + // Check if we have only 3 vertices + if (mPositions.size() == 3) + { + // Create two triangles (back to back) + Face *t1 = CreateTriangle(idx1, idx2, idx3); + Face *t2 = CreateTriangle(idx1, idx3, idx2); + + // Link faces edges + sLinkFace(t1->mFirstEdge, t2->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t2->mFirstEdge); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state + DrawState(); +#endif + + return EResult::Success; + } + + // Find point that forms the biggest tetrahedron + Vec3 initial_plane_normal = (mPositions[idx2] - mPositions[idx1]).Cross(mPositions[idx3] - mPositions[idx1]).Normalized(); + Vec3 initial_plane_centroid = (mPositions[idx1] + mPositions[idx2] + mPositions[idx3]) / 3.0f; + int idx4 = -1; + float max_dist = 0.0f; + for (int i = 0; i < (int)mPositions.size(); ++i) + if (i != idx1 && i != idx2 && i != idx3) + { + float dist = (mPositions[i] - initial_plane_centroid).Dot(initial_plane_normal); + if (abs(dist) > abs(max_dist)) + { + max_dist = dist; + idx4 = i; + } + } + + // Check if the hull is coplanar + if (Square(max_dist) <= 25.0f * coplanar_tolerance_sq) + { + // First project all points in 2D space + Vec3 base1 = initial_plane_normal.GetNormalizedPerpendicular(); + Vec3 base2 = initial_plane_normal.Cross(base1); + Array positions_2d; + positions_2d.reserve(mPositions.size()); + for (Vec3 v : mPositions) + positions_2d.emplace_back(base1.Dot(v), base2.Dot(v), 0.0f); + + // Build hull + Array edges_2d; + ConvexHullBuilder2D builder_2d(positions_2d); + ConvexHullBuilder2D::EResult result = builder_2d.Initialize(idx1, idx2, idx3, inMaxVertices, inTolerance, edges_2d); + + // Create faces (back to back) + Face *f1 = CreateFace(); + Face *f2 = CreateFace(); + + // Create edges for face 1 + Array edges_f1; + edges_f1.reserve(edges_2d.size()); + for (int start_idx : edges_2d) + { + Edge *edge = new Edge(f1, start_idx); + if (edges_f1.empty()) + f1->mFirstEdge = edge; + else + edges_f1.back()->mNextEdge = edge; + edges_f1.push_back(edge); + } + edges_f1.back()->mNextEdge = f1->mFirstEdge; + + // Create edges for face 2 + Array edges_f2; + edges_f2.reserve(edges_2d.size()); + for (int i = (int)edges_2d.size() - 1; i >= 0; --i) + { + Edge *edge = new Edge(f2, edges_2d[i]); + if (edges_f2.empty()) + f2->mFirstEdge = edge; + else + edges_f2.back()->mNextEdge = edge; + edges_f2.push_back(edge); + } + edges_f2.back()->mNextEdge = f2->mFirstEdge; + + // Link edges + for (size_t i = 0; i < edges_2d.size(); ++i) + sLinkFace(edges_f1[i], edges_f2[(2 * edges_2d.size() - 2 - i) % edges_2d.size()]); + + // Calculate the plane for both faces + f1->CalculateNormalAndCentroid(mPositions.data()); + f2->mNormal = -f1->mNormal; + f2->mCentroid = f1->mCentroid; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state + DrawState(); +#endif + + return result == ConvexHullBuilder2D::EResult::MaxVerticesReached? EResult::MaxVerticesReached : EResult::Success; + } + + // Ensure the planes are facing outwards + if (max_dist < 0.0f) + swap(idx2, idx3); + + // Create tetrahedron + Face *t1 = CreateTriangle(idx1, idx2, idx4); + Face *t2 = CreateTriangle(idx2, idx3, idx4); + Face *t3 = CreateTriangle(idx3, idx1, idx4); + Face *t4 = CreateTriangle(idx1, idx3, idx2); + + // Link face edges + sLinkFace(t1->mFirstEdge, t4->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge, t2->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t1->mFirstEdge->mNextEdge->mNextEdge, t3->mFirstEdge->mNextEdge); + sLinkFace(t2->mFirstEdge, t4->mFirstEdge->mNextEdge); + sLinkFace(t2->mFirstEdge->mNextEdge, t3->mFirstEdge->mNextEdge->mNextEdge); + sLinkFace(t3->mFirstEdge, t4->mFirstEdge); + + // Build the initial conflict lists + Faces faces { t1, t2, t3, t4 }; + for (int idx = 0; idx < (int)mPositions.size(); ++idx) + if (idx != idx1 && idx != idx2 && idx != idx3 && idx != idx4) + AssignPointToFace(idx, faces, tolerance_sq); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw current state including conflict list + DrawState(true); + + // Increment iteration counter + ++mIteration; +#endif + + // Overestimate of the actual amount of vertices we use, for limiting the amount of vertices in the hull + int num_vertices_used = 4; + + // Loop through the remainder of the points and add them + for (;;) + { + // Find the face with the furthest point on it + Face *face_with_furthest_point = nullptr; + float furthest_dist_sq = 0.0f; + for (Face *f : mFaces) + if (f->mFurthestPointDistanceSq > furthest_dist_sq) + { + furthest_dist_sq = f->mFurthestPointDistanceSq; + face_with_furthest_point = f; + } + + int furthest_point_idx; + if (face_with_furthest_point != nullptr) + { + // Take the furthest point + furthest_point_idx = face_with_furthest_point->mConflictList.back(); + face_with_furthest_point->mConflictList.pop_back(); + } + else if (!mCoplanarList.empty()) + { + // Try to assign points to faces (this also recalculates the distance to the hull for the coplanar vertices) + CoplanarList coplanar; + mCoplanarList.swap(coplanar); + bool added = false; + for (const Coplanar &c : coplanar) + added |= AssignPointToFace(c.mPositionIdx, mFaces, tolerance_sq); + + // If we were able to assign a point, loop again to pick it up + if (added) + continue; + + // If the coplanar list is empty, there are no points left and we're done + if (mCoplanarList.empty()) + break; + + do + { + // Find the vertex that is furthest from the hull + CoplanarList::size_type best_idx = 0; + float best_dist_sq = mCoplanarList.front().mDistanceSq; + for (CoplanarList::size_type idx = 1; idx < mCoplanarList.size(); ++idx) + { + const Coplanar &c = mCoplanarList[idx]; + if (c.mDistanceSq > best_dist_sq) + { + best_idx = idx; + best_dist_sq = c.mDistanceSq; + } + } + + // Swap it to the end + swap(mCoplanarList[best_idx], mCoplanarList.back()); + + // Remove it + furthest_point_idx = mCoplanarList.back().mPositionIdx; + mCoplanarList.pop_back(); + + // Find the face for which the point is furthest away + GetFaceForPoint(mPositions[furthest_point_idx], mFaces, face_with_furthest_point, best_dist_sq); + } while (!mCoplanarList.empty() && face_with_furthest_point == nullptr); + + if (face_with_furthest_point == nullptr) + break; + } + else + { + // If there are no more vertices, we're done + break; + } + + // Check if we have a limit on the max vertices that we should produce + if (num_vertices_used >= inMaxVertices) + { + // Count the actual amount of used vertices (we did not take the removal of any vertices into account) + num_vertices_used = GetNumVerticesUsed(); + + // Check if there are too many + if (num_vertices_used >= inMaxVertices) + return EResult::MaxVerticesReached; + } + + // We're about to add another vertex + ++num_vertices_used; + + // Add the point to the hull + Faces new_faces; + AddPoint(face_with_furthest_point, furthest_point_idx, coplanar_tolerance_sq, new_faces); + + // Redistribute points on conflict lists belonging to removed faces + for (const Face *face : mFaces) + if (face->mRemoved) + for (int idx : face->mConflictList) + AssignPointToFace(idx, new_faces, tolerance_sq); + + // Permanently delete faces that we removed in AddPoint() + GarbageCollectFaces(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw state at the end of this step including conflict list + DrawState(true); + + // Increment iteration counter + ++mIteration; +#endif + } + + // Check if we are left with a hull. It is possible that hull building fails if the points are nearly coplanar. + if (mFaces.size() < 2) + { + outError = "Too few faces in hull"; + return EResult::TooFewFaces; + } + + return EResult::Success; +} + +void ConvexHullBuilder::AddPoint(Face *inFacingFace, int inIdx, float inCoplanarToleranceSq, Faces &outNewFaces) +{ + // Get position + Vec3 pos = mPositions[inIdx]; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw point to be added + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + pos), Color::sYellow, 0.1f); + DebugRenderer::sInstance->DrawText3D(cDrawScale * (mOffset + pos), ConvertToString(inIdx), Color::sWhite); +#endif + +#ifdef JPH_ENABLE_ASSERTS + // Check if structure is intact + ValidateFaces(); +#endif + + // Find edge of convex hull of faces that are not facing the new vertex + FullEdges edges; + FindEdge(inFacingFace, pos, edges); + JPH_ASSERT(edges.size() >= 3); + + // Create new faces + outNewFaces.reserve(edges.size()); + for (const FullEdge &e : edges) + { + JPH_ASSERT(e.mStartIdx != e.mEndIdx); + Face *f = CreateTriangle(e.mStartIdx, e.mEndIdx, inIdx); + outNewFaces.push_back(f); + } + + // Link edges + for (Faces::size_type i = 0; i < outNewFaces.size(); ++i) + { + sLinkFace(outNewFaces[i]->mFirstEdge, edges[i].mNeighbourEdge); + sLinkFace(outNewFaces[i]->mFirstEdge->mNextEdge, outNewFaces[(i + 1) % outNewFaces.size()]->mFirstEdge->mNextEdge->mNextEdge); + } + + // Loop on faces that were modified until nothing needs to be checked anymore + Faces affected_faces = outNewFaces; + while (!affected_faces.empty()) + { + // Take the next face + Face *face = affected_faces.back(); + affected_faces.pop_back(); + + if (!face->mRemoved) + { + // Merge with neighbour if this is a degenerate face + MergeDegenerateFace(face, affected_faces); + + // Merge with coplanar neighbours (or when the neighbour forms a concave edge) + if (!face->mRemoved) + MergeCoplanarOrConcaveFaces(face, inCoplanarToleranceSq, affected_faces); + } + } + +#ifdef JPH_ENABLE_ASSERTS + // Check if structure is intact + ValidateFaces(); +#endif +} + +void ConvexHullBuilder::GarbageCollectFaces() +{ + for (int i = (int)mFaces.size() - 1; i >= 0; --i) + { + Face *f = mFaces[i]; + if (f->mRemoved) + { + FreeFace(f); + mFaces.erase(mFaces.begin() + i); + } + } +} + +ConvexHullBuilder::Face *ConvexHullBuilder::CreateFace() +{ + // Call provider to create face + Face *f = new Face(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Remember iteration counter + f->mIteration = mIteration; +#endif + + // Add to list + mFaces.push_back(f); + return f; +} + +ConvexHullBuilder::Face *ConvexHullBuilder::CreateTriangle(int inIdx1, int inIdx2, int inIdx3) +{ + Face *f = CreateFace(); + f->Initialize(inIdx1, inIdx2, inIdx3, mPositions.data()); + return f; +} + +void ConvexHullBuilder::FreeFace(Face *inFace) +{ + JPH_ASSERT(inFace->mRemoved); + +#ifdef JPH_ENABLE_ASSERTS + // Make sure that this face is not connected + Edge *e = inFace->mFirstEdge; + if (e != nullptr) + do + { + JPH_ASSERT(e->mNeighbourEdge == nullptr); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +#endif + + // Free the face + delete inFace; +} + +void ConvexHullBuilder::sLinkFace(Edge *inEdge1, Edge *inEdge2) +{ + // Check not connected yet + JPH_ASSERT(inEdge1->mNeighbourEdge == nullptr); + JPH_ASSERT(inEdge2->mNeighbourEdge == nullptr); + JPH_ASSERT(inEdge1->mFace != inEdge2->mFace); + + // Check vertices match + JPH_ASSERT(inEdge1->mStartIdx == inEdge2->mNextEdge->mStartIdx); + JPH_ASSERT(inEdge2->mStartIdx == inEdge1->mNextEdge->mStartIdx); + + // Link up + inEdge1->mNeighbourEdge = inEdge2; + inEdge2->mNeighbourEdge = inEdge1; +} + +void ConvexHullBuilder::sUnlinkFace(Face *inFace) +{ + // Unlink from neighbours + Edge *e = inFace->mFirstEdge; + do + { + if (e->mNeighbourEdge != nullptr) + { + // Validate that neighbour points to us + JPH_ASSERT(e->mNeighbourEdge->mNeighbourEdge == e); + + // Unlink + e->mNeighbourEdge->mNeighbourEdge = nullptr; + e->mNeighbourEdge = nullptr; + } + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const +{ + // Assert that we were given an empty array + JPH_ASSERT(outEdges.empty()); + + // Should start with a facing face + JPH_ASSERT(inFacingFace->IsFacing(inVertex)); + + // Flag as removed + inFacingFace->mRemoved = true; + + // Instead of recursing, we build our own stack with the information we need + struct StackEntry + { + Edge * mFirstEdge; + Edge * mCurrentEdge; + }; + constexpr int cMaxEdgeLength = 128; + StackEntry stack[cMaxEdgeLength]; + int cur_stack_pos = 0; + + static_assert(alignof(Edge) >= 2, "Need lowest bit to indicate to tell if we completed the loop"); + + // Start with the face / edge provided + stack[0].mFirstEdge = inFacingFace->mFirstEdge; + stack[0].mCurrentEdge = reinterpret_cast(reinterpret_cast(inFacingFace->mFirstEdge) | 1); // Set lowest bit of pointer to make it different from the first edge + + for (;;) + { + StackEntry &cur_entry = stack[cur_stack_pos]; + + // Next edge + Edge *raw_e = cur_entry.mCurrentEdge; + Edge *e = reinterpret_cast(reinterpret_cast(raw_e) & ~uintptr_t(1)); // Remove the lowest bit which was used to indicate that this is the first edge we're testing + cur_entry.mCurrentEdge = e->mNextEdge; + + // If we're back at the first edge we've completed the face and we're done + if (raw_e == cur_entry.mFirstEdge) + { + // This face needs to be removed, unlink it now, caller will free + sUnlinkFace(e->mFace); + + // Pop from stack + if (--cur_stack_pos < 0) + break; + } + else + { + // Visit neighbour face + Edge *ne = e->mNeighbourEdge; + if (ne != nullptr) + { + Face *n = ne->mFace; + if (!n->mRemoved) + { + // Check if vertex is on the front side of this face + if (n->IsFacing(inVertex)) + { + // Vertex on front, this face needs to be removed + n->mRemoved = true; + + // Add element to the stack of elements to visit + cur_stack_pos++; + JPH_ASSERT(cur_stack_pos < cMaxEdgeLength); + StackEntry &new_entry = stack[cur_stack_pos]; + new_entry.mFirstEdge = ne; + new_entry.mCurrentEdge = ne->mNextEdge; // We don't need to test this edge again since we came from it + } + else + { + // Vertex behind, keep edge + FullEdge full; + full.mNeighbourEdge = ne; + full.mStartIdx = e->mStartIdx; + full.mEndIdx = ne->mStartIdx; + outEdges.push_back(full); + } + } + } + } + } + + // Assert that we have a fully connected loop +#ifdef JPH_ENABLE_ASSERTS + for (int i = 0; i < (int)outEdges.size(); ++i) + JPH_ASSERT(outEdges[i].mEndIdx == outEdges[(i + 1) % outEdges.size()].mStartIdx); +#endif + +#ifdef JPH_CONVEX_BUILDER_DEBUG + // Draw edge of facing faces + for (int i = 0; i < (int)outEdges.size(); ++i) + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]), cDrawScale * (mOffset + mPositions[outEdges[i].mEndIdx]), Color::sWhite, 0.01f); + DrawState(); +#endif +} + +void ConvexHullBuilder::MergeFaces(Edge *inEdge) +{ + // Get the face + Face *face = inEdge->mFace; + + // Find the previous and next edge + Edge *next_edge = inEdge->mNextEdge; + Edge *prev_edge = inEdge->GetPreviousEdge(); + + // Get the other face + Edge *other_edge = inEdge->mNeighbourEdge; + Face *other_face = other_edge->mFace; + + // Check if attempting to merge with self + JPH_ASSERT(face != other_face); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(face, Color::sGreen); + DrawWireFace(other_face, Color::sRed); + DrawState(); +#endif + + // Loop over the edges of the other face and make them belong to inFace + Edge *edge = other_edge->mNextEdge; + prev_edge->mNextEdge = edge; + for (;;) + { + edge->mFace = face; + if (edge->mNextEdge == other_edge) + { + // Terminate when we are back at other_edge + edge->mNextEdge = next_edge; + break; + } + edge = edge->mNextEdge; + } + + // If the first edge happens to be inEdge we need to fix it because this edge is no longer part of the face. + // Note that we replace it with the first edge of the merged face so that if the MergeFace function is called + // from a loop that loops around the face that it will still terminate after visiting all edges once. + if (face->mFirstEdge == inEdge) + face->mFirstEdge = prev_edge->mNextEdge; + + // Free the edges + delete inEdge; + delete other_edge; + + // Mark the other face as removed + other_face->mFirstEdge = nullptr; + other_face->mRemoved = true; + + // Recalculate plane + face->CalculateNormalAndCentroid(mPositions.data()); + + // Merge conflict lists + if (face->mFurthestPointDistanceSq > other_face->mFurthestPointDistanceSq) + { + // This face has a point that's further away, make sure it remains the last one as we add the other points to this faces list + face->mConflictList.insert(face->mConflictList.end() - 1, other_face->mConflictList.begin(), other_face->mConflictList.end()); + } + else + { + // The other face has a point that's furthest away, add that list at the end. + face->mConflictList.insert(face->mConflictList.end(), other_face->mConflictList.begin(), other_face->mConflictList.end()); + face->mFurthestPointDistanceSq = other_face->mFurthestPointDistanceSq; + } + other_face->mConflictList.clear(); + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(face, Color::sWhite); + DrawState(); +#endif +} + +void ConvexHullBuilder::MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces) +{ + // Check area of face + if (inFace->mNormal.LengthSq() < cMinTriangleAreaSq) + { + // Find longest edge, since this face is a sliver this should keep the face convex + float max_length_sq = 0.0f; + Edge *longest_edge = nullptr; + Edge *e = inFace->mFirstEdge; + Vec3 p1 = mPositions[e->mStartIdx]; + do + { + Edge *next = e->mNextEdge; + Vec3 p2 = mPositions[next->mStartIdx]; + float length_sq = (p2 - p1).LengthSq(); + if (length_sq >= max_length_sq) + { + max_length_sq = length_sq; + longest_edge = e; + } + p1 = p2; + e = next; + } while (e != inFace->mFirstEdge); + + // Merge with face on longest edge + MergeFaces(longest_edge); + + // Remove any invalid edges + RemoveInvalidEdges(inFace, ioAffectedFaces); + } +} + +void ConvexHullBuilder::MergeCoplanarOrConcaveFaces(Face *inFace, float inCoplanarToleranceSq, Faces &ioAffectedFaces) +{ + bool merged = false; + + Edge *edge = inFace->mFirstEdge; + do + { + // Store next edge since this edge can be removed + Edge *next_edge = edge->mNextEdge; + + // Test if centroid of one face is above plane of the other face by inCoplanarToleranceSq. + // If so we need to merge other face into inFace. + const Face *other_face = edge->mNeighbourEdge->mFace; + Vec3 delta_centroid = other_face->mCentroid - inFace->mCentroid; + float dist_other_face_centroid = inFace->mNormal.Dot(delta_centroid); + float signed_dist_other_face_centroid_sq = abs(dist_other_face_centroid) * dist_other_face_centroid; + float dist_face_centroid = -other_face->mNormal.Dot(delta_centroid); + float signed_dist_face_centroid_sq = abs(dist_face_centroid) * dist_face_centroid; + float face_normal_len_sq = inFace->mNormal.LengthSq(); + float other_face_normal_len_sq = other_face->mNormal.LengthSq(); + if ((signed_dist_other_face_centroid_sq > -inCoplanarToleranceSq * face_normal_len_sq + || signed_dist_face_centroid_sq > -inCoplanarToleranceSq * other_face_normal_len_sq) + && inFace->mNormal.Dot(other_face->mNormal) > 0.0f) // Never merge faces that are back to back + { + MergeFaces(edge); + merged = true; + } + + edge = next_edge; + } while (edge != inFace->mFirstEdge); + + if (merged) + RemoveInvalidEdges(inFace, ioAffectedFaces); +} + +void ConvexHullBuilder::sMarkAffected(Face *inFace, Faces &ioAffectedFaces) +{ + if (std::find(ioAffectedFaces.begin(), ioAffectedFaces.end(), inFace) == ioAffectedFaces.end()) + ioAffectedFaces.push_back(inFace); +} + +void ConvexHullBuilder::RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces) +{ + // This marks that the plane needs to be recalculated (we delay this until the end of the + // function since we don't use the plane and we want to avoid calculating it multiple times) + bool recalculate_plane = false; + + // We keep going through this loop until no more edges were removed + bool removed; + do + { + removed = false; + + // Loop over all edges in this face + Edge *edge = inFace->mFirstEdge; + Face *neighbour_face = edge->mNeighbourEdge->mFace; + do + { + Edge *next_edge = edge->mNextEdge; + Face *next_neighbour_face = next_edge->mNeighbourEdge->mFace; + + if (neighbour_face == inFace) + { + // We only remove 1 edge at a time, check if this edge's next edge is our neighbour. + // If this check fails, we will continue to scan along the edge until we find an edge where this is the case. + if (edge->mNeighbourEdge == next_edge) + { + // This edge leads back to the starting point, this means the edge is interior and needs to be removed +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sBlue); + DrawState(); +#endif + + // Remove edge + Edge *prev_edge = edge->GetPreviousEdge(); + prev_edge->mNextEdge = next_edge->mNextEdge; + if (inFace->mFirstEdge == edge || inFace->mFirstEdge == next_edge) + inFace->mFirstEdge = prev_edge; + delete edge; + delete next_edge; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sGreen); + DrawState(); +#endif + + // Check if inFace now has only 2 edges left + if (RemoveTwoEdgeFace(inFace, ioAffectedFaces)) + return; // Bail if face no longer exists + + // Restart the loop + recalculate_plane = true; + removed = true; + break; + } + } + else if (neighbour_face == next_neighbour_face) + { + // There are two edges that connect to the same face, we will remove the second one +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sYellow); + DrawWireFace(neighbour_face, Color::sRed); + DrawState(); +#endif + + // First merge the neighbours edges + Edge *neighbour_edge = next_edge->mNeighbourEdge; + Edge *next_neighbour_edge = neighbour_edge->mNextEdge; + if (neighbour_face->mFirstEdge == next_neighbour_edge) + neighbour_face->mFirstEdge = neighbour_edge; + neighbour_edge->mNextEdge = next_neighbour_edge->mNextEdge; + neighbour_edge->mNeighbourEdge = edge; + delete next_neighbour_edge; + + // Then merge my own edges + if (inFace->mFirstEdge == next_edge) + inFace->mFirstEdge = edge; + edge->mNextEdge = next_edge->mNextEdge; + edge->mNeighbourEdge = neighbour_edge; + delete next_edge; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sYellow); + DrawWireFace(neighbour_face, Color::sGreen); + DrawState(); +#endif + + // Check if neighbour has only 2 edges left + if (!RemoveTwoEdgeFace(neighbour_face, ioAffectedFaces)) + { + // No, we need to recalculate its plane + neighbour_face->CalculateNormalAndCentroid(mPositions.data()); + + // Mark neighbour face as affected + sMarkAffected(neighbour_face, ioAffectedFaces); + } + + // Check if inFace now has only 2 edges left + if (RemoveTwoEdgeFace(inFace, ioAffectedFaces)) + return; // Bail if face no longer exists + + // Restart loop + recalculate_plane = true; + removed = true; + break; + } + + // This edge is ok, go to the next edge + edge = next_edge; + neighbour_face = next_neighbour_face; + + } while (edge != inFace->mFirstEdge); + } while (removed); + + // Recalculate plane? + if (recalculate_plane) + inFace->CalculateNormalAndCentroid(mPositions.data()); +} + +bool ConvexHullBuilder::RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const +{ + // Check if this face contains only 2 edges + Edge *edge = inFace->mFirstEdge; + Edge *next_edge = edge->mNextEdge; + JPH_ASSERT(edge != next_edge); // 1 edge faces should not exist + if (next_edge->mNextEdge == edge) + { +#ifdef JPH_CONVEX_BUILDER_DEBUG + DrawWireFace(inFace, Color::sRed); + DrawState(); +#endif + + // Schedule both neighbours for re-checking + Edge *neighbour_edge = edge->mNeighbourEdge; + Face *neighbour_face = neighbour_edge->mFace; + Edge *next_neighbour_edge = next_edge->mNeighbourEdge; + Face *next_neighbour_face = next_neighbour_edge->mFace; + sMarkAffected(neighbour_face, ioAffectedFaces); + sMarkAffected(next_neighbour_face, ioAffectedFaces); + + // Link my neighbours to each other + neighbour_edge->mNeighbourEdge = next_neighbour_edge; + next_neighbour_edge->mNeighbourEdge = neighbour_edge; + + // Unlink my edges + edge->mNeighbourEdge = nullptr; + next_edge->mNeighbourEdge = nullptr; + + // Mark this face as removed + inFace->mRemoved = true; + + return true; + } + + return false; +} + +#ifdef JPH_ENABLE_ASSERTS + +void ConvexHullBuilder::DumpFace(const Face *inFace) const +{ + Trace("f:0x%p", inFace); + + const Edge *e = inFace->mFirstEdge; + do + { + Trace("\te:0x%p { i:%d e:0x%p f:0x%p }", e, e->mStartIdx, e->mNeighbourEdge, e->mNeighbourEdge->mFace); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::DumpFaces() const +{ + Trace("Dump Faces:"); + + for (const Face *f : mFaces) + if (!f->mRemoved) + DumpFace(f); +} + +void ConvexHullBuilder::ValidateFace(const Face *inFace) const +{ + if (inFace->mRemoved) + { + const Edge *e = inFace->mFirstEdge; + if (e != nullptr) + do + { + JPH_ASSERT(e->mNeighbourEdge == nullptr); + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); + } + else + { + int edge_count = 0; + + const Edge *e = inFace->mFirstEdge; + do + { + // Count edge + ++edge_count; + + // Validate that adjacent faces are all different + if (mFaces.size() > 2) + for (const Edge *other_edge = e->mNextEdge; other_edge != inFace->mFirstEdge; other_edge = other_edge->mNextEdge) + JPH_ASSERT(e->mNeighbourEdge->mFace != other_edge->mNeighbourEdge->mFace); + + // Assert that the face is correct + JPH_ASSERT(e->mFace == inFace); + + // Assert that we have a neighbour + const Edge *nb_edge = e->mNeighbourEdge; + JPH_ASSERT(nb_edge != nullptr); + if (nb_edge != nullptr) + { + // Assert that our neighbours edge points to us + JPH_ASSERT(nb_edge->mNeighbourEdge == e); + + // Assert that it belongs to a different face + JPH_ASSERT(nb_edge->mFace != inFace); + + // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex + JPH_ASSERT(nb_edge->mNextEdge->mStartIdx == e->mStartIdx); + + // Assert that my next edge points to the same vertex as my neighbours vertex + JPH_ASSERT(e->mNextEdge->mStartIdx == nb_edge->mStartIdx); + } + e = e->mNextEdge; + } while (e != inFace->mFirstEdge); + + // Assert that we have 3 or more edges + JPH_ASSERT(edge_count >= 3); + } +} + +void ConvexHullBuilder::ValidateFaces() const +{ + for (const Face *f : mFaces) + ValidateFace(f); +} + +#endif // JPH_ENABLE_ASSERTS + +void ConvexHullBuilder::GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const +{ + // Fourth point is the average of all face centroids + Vec3 v4 = Vec3::sZero(); + for (const Face *f : mFaces) + v4 += f->mCentroid; + v4 /= float(mFaces.size()); + + // Calculate mass and center of mass of this convex hull by summing all tetrahedrons + outVolume = 0.0f; + outCenterOfMass = Vec3::sZero(); + for (const Face *f : mFaces) + { + // Get the first vertex that we'll use to create a triangle fan + Edge *e = f->mFirstEdge; + Vec3 v1 = mPositions[e->mStartIdx]; + + // Get the second vertex + e = e->mNextEdge; + Vec3 v2 = mPositions[e->mStartIdx]; + + for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge) + { + // Fetch the last point of the triangle + Vec3 v3 = mPositions[e->mStartIdx]; + + // Calculate center of mass and mass of this tetrahedron, + // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume + float volume_tetrahedron = (v1 - v4).Dot((v2 - v4).Cross(v3 - v4)); // Needs to be divided by 6, postpone this until the end of the loop + Vec3 center_of_mass_tetrahedron = v1 + v2 + v3 + v4; // Needs to be divided by 4, postpone this until the end of the loop + + // Accumulate results + outVolume += volume_tetrahedron; + outCenterOfMass += volume_tetrahedron * center_of_mass_tetrahedron; + + // Update v2 for next triangle + v2 = v3; + } + } + + // Calculate center of mass, fall back to average point in case there is no volume (everything is on a plane in this case) + if (outVolume > FLT_EPSILON) + outCenterOfMass /= 4.0f * outVolume; + else + outCenterOfMass = v4; + + outVolume /= 6.0f; +} + +void ConvexHullBuilder::DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const +{ + outCoplanarDistance = DetermineCoplanarDistance(); + + // This measures the distance from a polygon to the furthest point outside of the hull + float max_error = 0.0f; + Face *max_error_face = nullptr; + int max_error_point = -1; + + for (int i = 0; i < (int)mPositions.size(); ++i) + { + Vec3 v = mPositions[i]; + + // This measures the closest edge from all faces to point v + // Note that we take the min of all faces since there may be multiple near coplanar faces so if we were to test this per face + // we may find that a point is outside of a polygon and mark it as an error, while it is actually inside a nearly coplanar + // polygon. + float min_edge_dist_sq = FLT_MAX; + Face *min_edge_dist_face = nullptr; + + for (Face *f : mFaces) + { + // Check if point is on or in front of plane + float normal_len = f->mNormal.Length(); + JPH_ASSERT(normal_len > 0.0f); + float plane_dist = f->mNormal.Dot(v - f->mCentroid) / normal_len; + if (plane_dist > -outCoplanarDistance) + { + // Check distance to the edges of this face + float edge_dist_sq = GetDistanceToEdgeSq(v, f); + if (edge_dist_sq < min_edge_dist_sq) + { + min_edge_dist_sq = edge_dist_sq; + min_edge_dist_face = f; + } + + // If the point is inside the polygon and the point is in front of the plane, measure the distance + if (edge_dist_sq == 0.0f && plane_dist > max_error) + { + max_error = plane_dist; + max_error_face = f; + max_error_point = i; + } + } + } + + // If the minimum distance to an edge is further than our current max error, we use that as max error + float min_edge_dist = sqrt(min_edge_dist_sq); + if (min_edge_dist_face != nullptr && min_edge_dist > max_error) + { + max_error = min_edge_dist; + max_error_face = min_edge_dist_face; + max_error_point = i; + } + } + + outFaceWithMaxError = max_error_face; + outMaxError = max_error; + outMaxErrorPositionIdx = max_error_point; +} + +#ifdef JPH_CONVEX_BUILDER_DEBUG + +void ConvexHullBuilder::DrawState(bool inDrawConflictList) const +{ + // Draw origin + DebugRenderer::sInstance->DrawMarker(cDrawScale * mOffset, Color::sRed, 0.2f); + + int face_idx = 0; + + // Draw faces + for (const Face *f : mFaces) + if (!f->mRemoved) + { + Color iteration_color = Color::sGetDistinctColor(f->mIteration); + Color face_color = Color::sGetDistinctColor(face_idx++); + + // First point + const Edge *e = f->mFirstEdge; + RVec3 p1 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + // Second point + e = e->mNextEdge; + RVec3 p2 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + // First line + DebugRenderer::sInstance->DrawLine(p1, p2, Color::sGrey); + + do + { + // Third point + e = e->mNextEdge; + RVec3 p3 = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + + DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, iteration_color); + + DebugRenderer::sInstance->DrawLine(p2, p3, Color::sGrey); + + p2 = p3; + } + while (e != f->mFirstEdge); + + // Draw normal + RVec3 centroid = cDrawScale * (mOffset + f->mCentroid); + DebugRenderer::sInstance->DrawArrow(centroid, centroid + f->mNormal.NormalizedOr(Vec3::sZero()), face_color, 0.01f); + + // Draw conflict list + if (inDrawConflictList) + for (int idx : f->mConflictList) + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), face_color, 0.05f); + } + + // Offset to the right + mOffset += mDelta; +} + +void ConvexHullBuilder::DrawWireFace(const Face *inFace, ColorArg inColor) const +{ + const Edge *e = inFace->mFirstEdge; + RVec3 prev = cDrawScale * (mOffset + mPositions[e->mStartIdx]); + do + { + const Edge *next = e->mNextEdge; + RVec3 cur = cDrawScale * (mOffset + mPositions[next->mStartIdx]); + DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f); + DebugRenderer::sInstance->DrawText3D(prev, ConvertToString(e->mStartIdx), inColor); + e = next; + prev = cur; + } while (e != inFace->mFirstEdge); +} + +void ConvexHullBuilder::DrawEdge(const Edge *inEdge, ColorArg inColor) const +{ + RVec3 p1 = cDrawScale * (mOffset + mPositions[inEdge->mStartIdx]); + RVec3 p2 = cDrawScale * (mOffset + mPositions[inEdge->mNextEdge->mStartIdx]); + DebugRenderer::sInstance->DrawArrow(p1, p2, inColor, 0.01f); +} + +#endif // JPH_CONVEX_BUILDER_DEBUG + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE + +void ConvexHullBuilder::DumpShape() const +{ + static atomic sShapeNo = 1; + int shape_no = sShapeNo++; + + std::ofstream f; + f.open(StringFormat("dumped_shape%d.cpp", shape_no).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + f << "{\n"; + for (Vec3 v : mPositions) + f << StringFormat("\tVec3(%.9gf, %.9gf, %.9gf),\n", (double)v.GetX(), (double)v.GetY(), (double)v.GetZ()); + f << "},\n"; +} + +#endif // JPH_CONVEX_BUILDER_DUMP_SHAPE + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/ConvexHullBuilder.h b/WickedEngine/Jolt/Geometry/ConvexHullBuilder.h new file mode 100644 index 000000000..7a6aea8ca --- /dev/null +++ b/WickedEngine/Jolt/Geometry/ConvexHullBuilder.h @@ -0,0 +1,276 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +//#define JPH_CONVEX_BUILDER_DEBUG +//#define JPH_CONVEX_BUILDER_DUMP_SHAPE + +#ifdef JPH_CONVEX_BUILDER_DEBUG + #include +#endif + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder that tries to create hulls as accurately as possible. Used for offline processing. +class JPH_EXPORT ConvexHullBuilder : public NonCopyable +{ +public: + // Forward declare + class Face; + + /// Class that holds the information of an edge + class Edge : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Edge(Face *inFace, int inStartIdx) : mFace(inFace), mStartIdx(inStartIdx) { } + + /// Get the previous edge + inline Edge * GetPreviousEdge() + { + Edge *prev_edge = this; + while (prev_edge->mNextEdge != this) + prev_edge = prev_edge->mNextEdge; + return prev_edge; + } + + Face * mFace; ///< Face that this edge belongs to + Edge * mNextEdge = nullptr; ///< Next edge of this face + Edge * mNeighbourEdge = nullptr; ///< Edge that this edge is connected to + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + }; + + using ConflictList = Array; + + /// Class that holds the information of one face + class Face : public NonCopyable + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + ~Face(); + + /// Initialize a face with three indices + void Initialize(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions); + + /// Calculates the centroid and normal for this face + void CalculateNormalAndCentroid(const Vec3 *inPositions); + + /// Check if face inFace is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(inPosition - mCentroid) > 0.0f; + } + + Vec3 mNormal; ///< Normal of this face, length is 2 times area of face + Vec3 mCentroid; ///< Center of the face + ConflictList mConflictList; ///< Positions associated with this edge (that are closest to this edge). The last position in the list is the point that is furthest away from the face. + Edge * mFirstEdge = nullptr; ///< First edge of this face + float mFurthestPointDistanceSq = 0.0f; ///< Squared distance of furthest point from the conflict list to the face + bool mRemoved = false; ///< Flag that indicates that face has been removed (face will be freed later) +#ifdef JPH_CONVEX_BUILDER_DEBUG + int mIteration; ///< Iteration that this face was created +#endif + }; + + // Typedefs + using Positions = Array; + using Faces = Array; + + /// Constructor + explicit ConvexHullBuilder(const Positions &inPositions); + + /// Destructor + ~ConvexHullBuilder() { FreeFaces(); } + + /// Result enum that indicates how the hull got created + enum class EResult + { + Success, ///< Hull building finished successfully + MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached + TooFewPoints, ///< Too few points to create a hull + TooFewFaces, ///< Too few faces in the created hull (signifies precision errors during building) + Degenerate, ///< Degenerate hull detected + }; + + /// Takes all positions as provided by the constructor and use them to build a hull + /// Any points that are closer to the hull than inTolerance will be discarded + /// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit. + /// @param inTolerance Max distance that a point is allowed to be outside of the hull + /// @param outError Error message when building fails + /// @return Status code that reports if the hull was created or not + EResult Initialize(int inMaxVertices, float inTolerance, const char *&outError); + + /// Returns the amount of vertices that are currently used by the hull + int GetNumVerticesUsed() const; + + /// Returns true if the hull contains a polygon with inIndices (counter clockwise indices in mPositions) + bool ContainsFace(const Array &inIndices) const; + + /// Calculate the center of mass and the volume of the current convex hull + void GetCenterOfMassAndVolume(Vec3 &outCenterOfMass, float &outVolume) const; + + /// Determines the point that is furthest outside of the hull and reports how far it is outside of the hull (which indicates a failure during hull building) + /// @param outFaceWithMaxError The face that caused the error + /// @param outMaxError The maximum distance of a point to the hull + /// @param outMaxErrorPositionIdx The index of the point that had this distance + /// @param outCoplanarDistance Points that are less than this distance from the hull are considered on the hull. This should be used as a lowerbound for the allowed error. + void DetermineMaxError(Face *&outFaceWithMaxError, float &outMaxError, int &outMaxErrorPositionIdx, float &outCoplanarDistance) const; + + /// Access to the created faces. Memory is owned by the convex hull builder. + const Faces & GetFaces() const { return mFaces; } + +private: + /// Minimal square area of a triangle (used for merging and checking if a triangle is degenerate) + static constexpr float cMinTriangleAreaSq = 1.0e-12f; + +#ifdef JPH_CONVEX_BUILDER_DEBUG + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + + /// Class that holds an edge including start and end index + class FullEdge + { + public: + Edge * mNeighbourEdge; ///< Edge that this edge is connected to + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + int mEndIdx; ///< Vertex index in mPosition that indicates the end vertex of this edge + }; + + // Private typedefs + using FullEdges = Array; + + // Determine a suitable tolerance for detecting that points are coplanar + float DetermineCoplanarDistance() const; + + /// Find the face for which inPoint is furthest to the front + /// @param inPoint Point to test + /// @param inFaces List of faces to test + /// @param outFace Returns the best face + /// @param outDistSq Returns the squared distance how much inPoint is in front of the plane of the face + void GetFaceForPoint(Vec3Arg inPoint, const Faces &inFaces, Face *&outFace, float &outDistSq) const; + + /// @brief Calculates the distance between inPoint and inFace + /// @param inFace Face to test + /// @param inPoint Point to test + /// @return If the projection of the point on the plane is interior to the face 0, otherwise the squared distance to the closest edge + float GetDistanceToEdgeSq(Vec3Arg inPoint, const Face *inFace) const; + + /// Assigns a position to one of the supplied faces based on which face is closest. + /// @param inPositionIdx Index of the position to add + /// @param inFaces List of faces to consider + /// @param inToleranceSq Tolerance of the hull, if the point is closer to the face than this, we ignore it + /// @return True if point was assigned, false if it was discarded or added to the coplanar list + bool AssignPointToFace(int inPositionIdx, const Faces &inFaces, float inToleranceSq); + + /// Add a new point to the convex hull + void AddPoint(Face *inFacingFace, int inIdx, float inToleranceSq, Faces &outNewFaces); + + /// Remove all faces that have been marked 'removed' from mFaces list + void GarbageCollectFaces(); + + /// Create a new face + Face * CreateFace(); + + /// Create a new triangle + Face * CreateTriangle(int inIdx1, int inIdx2, int inIdx3); + + /// Delete a face (checking that it is not connected to any other faces) + void FreeFace(Face *inFace); + + /// Release all faces and edges + void FreeFaces(); + + /// Link face edge to other face edge + static void sLinkFace(Edge *inEdge1, Edge *inEdge2); + + /// Unlink this face from all of its neighbours + static void sUnlinkFace(Face *inFace); + + /// Given one face that faces inVertex, find the edges of the faces that are not facing inVertex. + /// Will flag all those faces for removal. + void FindEdge(Face *inFacingFace, Vec3Arg inVertex, FullEdges &outEdges) const; + + /// Merges the two faces that share inEdge into the face inEdge->mFace + void MergeFaces(Edge *inEdge); + + /// Merges inFace with a neighbour if it is degenerate (a sliver) + void MergeDegenerateFace(Face *inFace, Faces &ioAffectedFaces); + + /// Merges any coplanar as well as neighbours that form a non-convex edge into inFace. + /// Faces are considered coplanar if the distance^2 of the other face's centroid is smaller than inToleranceSq. + void MergeCoplanarOrConcaveFaces(Face *inFace, float inToleranceSq, Faces &ioAffectedFaces); + + /// Mark face as affected if it is not already in the list + static void sMarkAffected(Face *inFace, Faces &ioAffectedFaces); + + /// Removes all invalid edges. + /// 1. Merges inFace with faces that share two edges with it since this means inFace or the other face cannot be convex or the edge is colinear. + /// 2. Removes edges that are interior to inFace (that have inFace on both sides) + /// Any faces that need to be checked for validity will be added to ioAffectedFaces. + void RemoveInvalidEdges(Face *inFace, Faces &ioAffectedFaces); + + /// Removes inFace if it consists of only 2 edges, linking its neighbouring faces together + /// Any faces that need to be checked for validity will be added to ioAffectedFaces. + /// @return True if face was removed. + bool RemoveTwoEdgeFace(Face *inFace, Faces &ioAffectedFaces) const; + +#ifdef JPH_ENABLE_ASSERTS + /// Dumps the text representation of a face to the TTY + void DumpFace(const Face *inFace) const; + + /// Dumps the text representation of all faces to the TTY + void DumpFaces() const; + + /// Check consistency of 1 face + void ValidateFace(const Face *inFace) const; + + /// Check consistency of all faces + void ValidateFaces() const; +#endif + +#ifdef JPH_CONVEX_BUILDER_DEBUG + /// Draw state of algorithm + void DrawState(bool inDrawConflictList = false) const; + + /// Draw a face for debugging purposes + void DrawWireFace(const Face *inFace, ColorArg inColor) const; + + /// Draw an edge for debugging purposes + void DrawEdge(const Edge *inEdge, ColorArg inColor) const; +#endif + +#ifdef JPH_CONVEX_BUILDER_DUMP_SHAPE + void DumpShape() const; +#endif + + const Positions & mPositions; ///< List of positions (some of them are part of the hull) + Faces mFaces; ///< List of faces that are part of the hull (if !mRemoved) + + struct Coplanar + { + int mPositionIdx; ///< Index in mPositions + float mDistanceSq; ///< Distance to the edge of closest face (should be > 0) + }; + using CoplanarList = Array; + + CoplanarList mCoplanarList; ///< List of positions that are coplanar to a face but outside of the face, these are added to the hull at the end + +#ifdef JPH_CONVEX_BUILDER_DEBUG + int mIteration; ///< Number of iterations we've had so far (for debug purposes) + mutable RVec3 mOffset; ///< Offset to use for state drawing + Vec3 mDelta; ///< Delta offset between next states +#endif +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.cpp b/WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.cpp new file mode 100644 index 000000000..1dedb4a92 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.cpp @@ -0,0 +1,335 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + #include +#endif + +JPH_NAMESPACE_BEGIN + +void ConvexHullBuilder2D::Edge::CalculateNormalAndCenter(const Vec3 *inPositions) +{ + Vec3 p1 = inPositions[mStartIdx]; + Vec3 p2 = inPositions[mNextEdge->mStartIdx]; + + // Center of edge + mCenter = 0.5f * (p1 + p2); + + // Create outward pointing normal. + // We have two choices for the normal (which satisfies normal . edge = 0): + // normal1 = (-edge.y, edge.x, 0) + // normal2 = (edge.y, -edge.x, 0) + // We want (normal x edge).z > 0 so that the normal points out of the polygon. Only normal2 satisfies this condition. + Vec3 edge = p2 - p1; + mNormal = Vec3(edge.GetY(), -edge.GetX(), 0); +} + +ConvexHullBuilder2D::ConvexHullBuilder2D(const Positions &inPositions) : + mPositions(inPositions) +{ +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + // Center the drawing of the first hull around the origin and calculate the delta offset between states + mOffset = RVec3::sZero(); + if (mPositions.empty()) + { + // No hull will be generated + mDelta = Vec3::sZero(); + } + else + { + Vec3 maxv = Vec3::sReplicate(-FLT_MAX), minv = Vec3::sReplicate(FLT_MAX); + for (Vec3 v : mPositions) + { + minv = Vec3::sMin(minv, v); + maxv = Vec3::sMax(maxv, v); + mOffset -= v; + } + mOffset /= Real(mPositions.size()); + mDelta = Vec3((maxv - minv).GetX() + 0.5f, 0, 0); + mOffset += mDelta; // Don't start at origin, we're already drawing the final hull there + } +#endif +} + +ConvexHullBuilder2D::~ConvexHullBuilder2D() +{ + FreeEdges(); +} + +void ConvexHullBuilder2D::FreeEdges() +{ + if (mFirstEdge == nullptr) + return; + + Edge *edge = mFirstEdge; + do + { + Edge *next = edge->mNextEdge; + delete edge; + edge = next; + } while (edge != mFirstEdge); + + mFirstEdge = nullptr; + mNumEdges = 0; +} + +#ifdef JPH_ENABLE_ASSERTS + +void ConvexHullBuilder2D::ValidateEdges() const +{ + if (mFirstEdge == nullptr) + { + JPH_ASSERT(mNumEdges == 0); + return; + } + + int count = 0; + + Edge *edge = mFirstEdge; + do + { + // Validate connectivity + JPH_ASSERT(edge->mNextEdge->mPrevEdge == edge); + JPH_ASSERT(edge->mPrevEdge->mNextEdge == edge); + + ++count; + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + // Validate that count matches + JPH_ASSERT(count == mNumEdges); +} + +#endif // JPH_ENABLE_ASSERTS + +void ConvexHullBuilder2D::AssignPointToEdge(int inPositionIdx, const Array &inEdges) const +{ + Vec3 point = mPositions[inPositionIdx]; + + Edge *best_edge = nullptr; + float best_dist_sq = 0.0f; + + // Test against all edges + for (Edge *edge : inEdges) + { + // Determine distance to edge + float dot = edge->mNormal.Dot(point - edge->mCenter); + if (dot > 0.0f) + { + float dist_sq = dot * dot / edge->mNormal.LengthSq(); + if (dist_sq > best_dist_sq) + { + best_edge = edge; + best_dist_sq = dist_sq; + } + } + } + + // If this point is in front of the edge, add it to the conflict list + if (best_edge != nullptr) + { + if (best_dist_sq > best_edge->mFurthestPointDistanceSq) + { + // This point is further away than any others, update the distance and add point as last point + best_edge->mFurthestPointDistanceSq = best_dist_sq; + best_edge->mConflictList.push_back(inPositionIdx); + } + else + { + // Not the furthest point, add it as the before last point + best_edge->mConflictList.insert(best_edge->mConflictList.begin() + best_edge->mConflictList.size() - 1, inPositionIdx); + } + } +} + +ConvexHullBuilder2D::EResult ConvexHullBuilder2D::Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges) +{ + // Clear any leftovers + FreeEdges(); + outEdges.clear(); + + // Reset flag + EResult result = EResult::Success; + + // Determine a suitable tolerance for detecting that points are colinear + // Formula as per: Implementing Quickhull - Dirk Gregorius. + Vec3 vmax = Vec3::sZero(); + for (Vec3 v : mPositions) + vmax = Vec3::sMax(vmax, v.Abs()); + float colinear_tolerance_sq = Square(2.0f * FLT_EPSILON * (vmax.GetX() + vmax.GetY())); + + // Increase desired tolerance if accuracy doesn't allow it + float tolerance_sq = max(colinear_tolerance_sq, Square(inTolerance)); + + // Start with the initial indices in counter clockwise order + float z = (mPositions[inIdx2] - mPositions[inIdx1]).Cross(mPositions[inIdx3] - mPositions[inIdx1]).GetZ(); + if (z < 0.0f) + swap(inIdx1, inIdx2); + + // Create and link edges + Edge *e1 = new Edge(inIdx1); + Edge *e2 = new Edge(inIdx2); + Edge *e3 = new Edge(inIdx3); + e1->mNextEdge = e2; + e1->mPrevEdge = e3; + e2->mNextEdge = e3; + e2->mPrevEdge = e1; + e3->mNextEdge = e1; + e3->mPrevEdge = e2; + mFirstEdge = e1; + mNumEdges = 3; + + // Build the initial conflict lists + Array edges { e1, e2, e3 }; + for (Edge *edge : edges) + edge->CalculateNormalAndCenter(mPositions.data()); + for (int idx = 0; idx < (int)mPositions.size(); ++idx) + if (idx != inIdx1 && idx != inIdx2 && idx != inIdx3) + AssignPointToEdge(idx, edges); + + JPH_IF_ENABLE_ASSERTS(ValidateEdges();) +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + DrawState(); +#endif + + // Add the remaining points to the hull + for (;;) + { + // Check if we've reached the max amount of vertices that are allowed + if (mNumEdges >= inMaxVertices) + { + result = EResult::MaxVerticesReached; + break; + } + + // Find the edge with the furthest point on it + Edge *edge_with_furthest_point = nullptr; + float furthest_dist_sq = 0.0f; + Edge *edge = mFirstEdge; + do + { + if (edge->mFurthestPointDistanceSq > furthest_dist_sq) + { + furthest_dist_sq = edge->mFurthestPointDistanceSq; + edge_with_furthest_point = edge; + } + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + // If there is none closer than our tolerance value, we're done + if (edge_with_furthest_point == nullptr || furthest_dist_sq < tolerance_sq) + break; + + // Take the furthest point + int furthest_point_idx = edge_with_furthest_point->mConflictList.back(); + edge_with_furthest_point->mConflictList.pop_back(); + Vec3 furthest_point = mPositions[furthest_point_idx]; + + // Find the horizon of edges that need to be removed + Edge *first_edge = edge_with_furthest_point; + do + { + Edge *prev = first_edge->mPrevEdge; + if (!prev->IsFacing(furthest_point)) + break; + first_edge = prev; + } while (first_edge != edge_with_furthest_point); + + Edge *last_edge = edge_with_furthest_point; + do + { + Edge *next = last_edge->mNextEdge; + if (!next->IsFacing(furthest_point)) + break; + last_edge = next; + } while (last_edge != edge_with_furthest_point); + + // Create new edges + e1 = new Edge(first_edge->mStartIdx); + e2 = new Edge(furthest_point_idx); + e1->mNextEdge = e2; + e1->mPrevEdge = first_edge->mPrevEdge; + e2->mPrevEdge = e1; + e2->mNextEdge = last_edge->mNextEdge; + e1->mPrevEdge->mNextEdge = e1; + e2->mNextEdge->mPrevEdge = e2; + mFirstEdge = e1; // We could delete mFirstEdge so just update it to the newly created edge + mNumEdges += 2; + + // Calculate normals + Array new_edges { e1, e2 }; + for (Edge *new_edge : new_edges) + new_edge->CalculateNormalAndCenter(mPositions.data()); + + // Delete the old edges + for (;;) + { + Edge *next = first_edge->mNextEdge; + + // Redistribute points in conflict list + for (int idx : first_edge->mConflictList) + AssignPointToEdge(idx, new_edges); + + // Delete the old edge + delete first_edge; + --mNumEdges; + + if (first_edge == last_edge) + break; + first_edge = next; + } + + JPH_IF_ENABLE_ASSERTS(ValidateEdges();) + #ifdef JPH_CONVEX_BUILDER_2D_DEBUG + DrawState(); + #endif + } + + // Convert the edge list to a list of indices + outEdges.reserve(mNumEdges); + Edge *edge = mFirstEdge; + do + { + outEdges.push_back(edge->mStartIdx); + edge = edge->mNextEdge; + } while (edge != mFirstEdge); + + return result; +} + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + +void ConvexHullBuilder2D::DrawState() +{ + int color_idx = 0; + + const Edge *edge = mFirstEdge; + do + { + const Edge *next = edge->mNextEdge; + + // Get unique color per edge + Color color = Color::sGetDistinctColor(color_idx++); + + // Draw edge and normal + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + mPositions[edge->mStartIdx]), cDrawScale * (mOffset + mPositions[next->mStartIdx]), color, 0.1f); + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + edge->mCenter), cDrawScale * (mOffset + edge->mCenter) + edge->mNormal.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f); + + // Draw points that belong to this edge in the same color + for (int idx : edge->mConflictList) + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + mPositions[idx]), color, 0.05f); + + edge = next; + } while (edge != mFirstEdge); + + mOffset += mDelta; +} + +#endif + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.h b/WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.h new file mode 100644 index 000000000..ff06a3403 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/ConvexHullBuilder2D.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +//#define JPH_CONVEX_BUILDER_2D_DEBUG + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder that tries to create 2D hulls as accurately as possible. Used for offline processing. +class JPH_EXPORT ConvexHullBuilder2D : public NonCopyable +{ +public: + using Positions = Array; + using Edges = Array; + + /// Constructor + /// @param inPositions Positions used to make the hull. Uses X and Y component of Vec3 only! + explicit ConvexHullBuilder2D(const Positions &inPositions); + + /// Destructor + ~ConvexHullBuilder2D(); + + /// Result enum that indicates how the hull got created + enum class EResult + { + Success, ///< Hull building finished successfully + MaxVerticesReached, ///< Hull building finished successfully, but the desired accuracy was not reached because the max vertices limit was reached + }; + + /// Takes all positions as provided by the constructor and use them to build a hull + /// Any points that are closer to the hull than inTolerance will be discarded + /// @param inIdx1 , inIdx2 , inIdx3 The indices to use as initial hull (in any order) + /// @param inMaxVertices Max vertices to allow in the hull. Specify INT_MAX if there is no limit. + /// @param inTolerance Max distance that a point is allowed to be outside of the hull + /// @param outEdges On success this will contain the list of indices that form the hull (counter clockwise) + /// @return Status code that reports if the hull was created or not + EResult Initialize(int inIdx1, int inIdx2, int inIdx3, int inMaxVertices, float inTolerance, Edges &outEdges); + +private: +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + + class Edge; + + /// Frees all edges + void FreeEdges(); + + /// Assigns a position to one of the supplied edges based on which edge is closest. + /// @param inPositionIdx Index of the position to add + /// @param inEdges List of edges to consider + void AssignPointToEdge(int inPositionIdx, const Array &inEdges) const; + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + /// Draw state of algorithm + void DrawState(); +#endif + +#ifdef JPH_ENABLE_ASSERTS + /// Validate that the edge structure is intact + void ValidateEdges() const; +#endif + + using ConflictList = Array; + + /// Linked list of edges + class Edge + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Edge(int inStartIdx) : mStartIdx(inStartIdx) { } + + /// Calculate the center of the edge and the edge normal + void CalculateNormalAndCenter(const Vec3 *inPositions); + + /// Check if this edge is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const { return mNormal.Dot(inPosition - mCenter) > 0.0f; } + + Vec3 mNormal; ///< Normal of the edge (not normalized) + Vec3 mCenter; ///< Center of the edge + ConflictList mConflictList; ///< Positions associated with this edge (that are closest to this edge). Last entry is the one furthest away from the edge, remainder is unsorted. + Edge * mPrevEdge = nullptr; ///< Previous edge in circular list + Edge * mNextEdge = nullptr; ///< Next edge in circular list + int mStartIdx; ///< Position index of start of this edge + float mFurthestPointDistanceSq = 0.0f; ///< Squared distance of furthest point from the conflict list to the edge + }; + + const Positions & mPositions; ///< List of positions (some of them are part of the hull) + Edge * mFirstEdge = nullptr; ///< First edge of the hull + int mNumEdges = 0; ///< Number of edges in hull + +#ifdef JPH_CONVEX_BUILDER_2D_DEBUG + RVec3 mOffset; ///< Offset to use for state drawing + Vec3 mDelta; ///< Delta offset between next states +#endif +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/ConvexSupport.h b/WickedEngine/Jolt/Geometry/ConvexSupport.h new file mode 100644 index 000000000..3ba2c935d --- /dev/null +++ b/WickedEngine/Jolt/Geometry/ConvexSupport.h @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get the support point for a convex object +/// Structure that transforms a convex object (supports only uniform scaling) +template +struct TransformedConvexObject +{ + /// Create transformed convex object. + TransformedConvexObject(Mat44Arg inTransform, const ConvexObject &inObject) : + mTransform(inTransform), + mObject(inObject) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return mTransform * mObject.GetSupport(mTransform.Multiply3x3Transposed(inDirection)); + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace(Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + mObject.GetSupportingFace(mTransform.Multiply3x3Transposed(inDirection), outVertices); + + for (Vec3 &v : outVertices) + v = mTransform * v; + } + + Mat44 mTransform; + const ConvexObject & mObject; +}; + +/// Structure that adds a convex radius +template +struct AddConvexRadius +{ + AddConvexRadius(const ConvexObject &inObject, float inRadius) : + mObject(inObject), + mRadius(inRadius) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + float length = inDirection.Length(); + return length > 0.0f ? mObject.GetSupport(inDirection) + (mRadius / length) * inDirection : mObject.GetSupport(inDirection); + } + + const ConvexObject & mObject; + float mRadius; +}; + +/// Structure that performs a Minkowski difference A - B +template +struct MinkowskiDifference +{ + MinkowskiDifference(const ConvexObjectA &inObjectA, const ConvexObjectB &inObjectB) : + mObjectA(inObjectA), + mObjectB(inObjectB) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + return mObjectA.GetSupport(inDirection) - mObjectB.GetSupport(-inDirection); + } + + const ConvexObjectA & mObjectA; + const ConvexObjectB & mObjectB; +}; + +/// Class that wraps a point so that it can be used with convex collision detection +struct PointConvexSupport +{ + /// Calculate the support vector for this convex shape. + Vec3 GetSupport([[maybe_unused]] Vec3Arg inDirection) const + { + return mPoint; + } + + Vec3 mPoint; +}; + +/// Class that wraps a triangle so that it can used with convex collision detection +struct TriangleConvexSupport +{ + /// Constructor + TriangleConvexSupport(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : + mV1(inV1), + mV2(inV2), + mV3(inV3) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + // Project vertices on inDirection + float d1 = mV1.Dot(inDirection); + float d2 = mV2.Dot(inDirection); + float d3 = mV3.Dot(inDirection); + + // Return vertex with biggest projection + if (d1 > d2) + { + if (d1 > d3) + return mV1; + else + return mV3; + } + else + { + if (d2 > d3) + return mV2; + else + return mV3; + } + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY &outVertices) const + { + outVertices.push_back(mV1); + outVertices.push_back(mV2); + outVertices.push_back(mV3); + } + + /// The three vertices of the triangle + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; +}; + +/// Class that wraps a polygon so that it can used with convex collision detection +template +struct PolygonConvexSupport +{ + /// Constructor + explicit PolygonConvexSupport(const VERTEX_ARRAY &inVertices) : + mVertices(inVertices) + { + } + + /// Calculate the support vector for this convex shape. + Vec3 GetSupport(Vec3Arg inDirection) const + { + Vec3 support_point = mVertices[0]; + float best_dot = mVertices[0].Dot(inDirection); + + for (typename VERTEX_ARRAY::const_iterator v = mVertices.begin() + 1; v < mVertices.end(); ++v) + { + float dot = v->Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + support_point = *v; + } + } + + return support_point; + } + + /// Get the vertices of the face that faces inDirection the most + template + void GetSupportingFace([[maybe_unused]] Vec3Arg inDirection, VERTEX_ARRAY_ARG &outVertices) const + { + for (Vec3 v : mVertices) + outVertices.push_back(v); + } + + /// The vertices of the polygon + const VERTEX_ARRAY & mVertices; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/EPAConvexHullBuilder.h b/WickedEngine/Jolt/Geometry/EPAConvexHullBuilder.h new file mode 100644 index 000000000..45571cc97 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/EPAConvexHullBuilder.h @@ -0,0 +1,844 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Define to validate the integrity of the hull structure +//#define JPH_EPA_CONVEX_BUILDER_VALIDATE + +// Define to draw the building of the hull for debugging purposes +//#define JPH_EPA_CONVEX_BUILDER_DRAW + +#include + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + #include + #include +#endif + +JPH_NAMESPACE_BEGIN + +/// A convex hull builder specifically made for the EPA penetration depth calculation. It trades accuracy for speed and will simply abort of the hull forms defects due to numerical precision problems. +class EPAConvexHullBuilder : public NonCopyable +{ +private: +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + /// Factor to scale convex hull when debug drawing the construction process + static constexpr Real cDrawScale = 10; +#endif + +public: + // Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2 + // In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2 + // Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2. + static constexpr int cMaxTriangles = 256; ///< Max triangles in hull + static constexpr int cMaxPoints = cMaxTriangles / 2; ///< Max number of points in hull + + // Constants + static constexpr int cMaxEdgeLength = 128; ///< Max number of edges in FindEdge + static constexpr float cMinTriangleArea = 1.0e-10f; ///< Minimum area of a triangle before, if smaller than this it will not be added to the priority queue + static constexpr float cBarycentricEpsilon = 1.0e-3f; ///< Epsilon value used to determine if a point is in the interior of a triangle + + // Forward declare + class Triangle; + + /// Class that holds the information of an edge + class Edge + { + public: + /// Information about neighbouring triangle + Triangle * mNeighbourTriangle; ///< Triangle that neighbours this triangle + int mNeighbourEdge; ///< Index in mEdge that specifies edge that this Edge is connected to + + int mStartIdx; ///< Vertex index in mPositions that indicates the start vertex of this edge + }; + + using Edges = StaticArray; + using NewTriangles = StaticArray; + + /// Class that holds the information of one triangle + class Triangle : public NonCopyable + { + public: + /// Constructor + inline Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions); + + /// Check if triangle is facing inPosition + inline bool IsFacing(Vec3Arg inPosition) const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(inPosition - mCentroid) > 0.0f; + } + + /// Check if triangle is facing the origin + inline bool IsFacingOrigin() const + { + JPH_ASSERT(!mRemoved); + return mNormal.Dot(mCentroid) < 0.0f; + } + + /// Get the next edge of edge inIndex + inline const Edge & GetNextEdge(int inIndex) const + { + return mEdge[(inIndex + 1) % 3]; + } + + Edge mEdge[3]; ///< 3 edges of this triangle + Vec3 mNormal; ///< Normal of this triangle, length is 2 times area of triangle + Vec3 mCentroid; ///< Center of the triangle + float mClosestLenSq = FLT_MAX; ///< Closest distance^2 from origin to triangle + float mLambda[2]; ///< Barycentric coordinates of closest point to origin on triangle + bool mLambdaRelativeTo0; ///< How to calculate the closest point, true: y0 + l0 * (y1 - y0) + l1 * (y2 - y0), false: y1 + l0 * (y0 - y1) + l1 * (y2 - y1) + bool mClosestPointInterior = false; ///< Flag that indicates that the closest point from this triangle to the origin is an interior point + bool mRemoved = false; ///< Flag that indicates that triangle has been removed + bool mInQueue = false; ///< Flag that indicates that this triangle was placed in the sorted heap (stays true after it is popped because the triangle is freed by the main EPA algorithm loop) +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + int mIteration; ///< Iteration that this triangle was created +#endif + }; + + /// Factory that creates triangles in a fixed size buffer + class TriangleFactory : public NonCopyable + { + private: + /// Struct that stores both a triangle or a next pointer in case the triangle is unused + union alignas(Triangle) Block + { + uint8 mTriangle[sizeof(Triangle)]; + Block * mNextFree; + }; + + /// Storage for triangle data + Block mTriangles[cMaxTriangles]; ///< Storage for triangles + Block * mNextFree = nullptr; ///< List of free triangles + int mHighWatermark = 0; ///< High water mark for used triangles (if mNextFree == nullptr we can take one from here) + + public: + /// Return all triangles to the free pool + void Clear() + { + mNextFree = nullptr; + mHighWatermark = 0; + } + + /// Allocate a new triangle with 3 indexes + Triangle * CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) + { + Triangle *t; + if (mNextFree != nullptr) + { + // Entry available from the free list + t = reinterpret_cast(&mNextFree->mTriangle); + mNextFree = mNextFree->mNextFree; + } + else + { + // Allocate from never used before triangle store + if (mHighWatermark >= cMaxTriangles) + return nullptr; // Buffer full + t = reinterpret_cast(&mTriangles[mHighWatermark].mTriangle); + ++mHighWatermark; + } + + // Call constructor + new (t) Triangle(inIdx0, inIdx1, inIdx2, inPositions); + + return t; + } + + /// Free a triangle + void FreeTriangle(Triangle *inT) + { + // Destruct triangle + inT->~Triangle(); +#ifdef JPH_DEBUG + memset(inT, 0xcd, sizeof(Triangle)); +#endif + + // Add triangle to the free list + Block *tu = reinterpret_cast(inT); + tu->mNextFree = mNextFree; + mNextFree = tu; + } + }; + + // Typedefs + using PointsBase = StaticArray; + using Triangles = StaticArray; + + /// Specialized points list that allows direct access to the size + class Points : public PointsBase + { + public: + size_type & GetSizeRef() + { + return mSize; + } + }; + + /// Specialized triangles list that keeps them sorted on closest distance to origin + class TriangleQueue : public Triangles + { + public: + /// Function to sort triangles on closest distance to origin + static bool sTriangleSorter(const Triangle *inT1, const Triangle *inT2) + { + return inT1->mClosestLenSq > inT2->mClosestLenSq; + } + + /// Add triangle to the list + void push_back(Triangle *inT) + { + // Add to base + Triangles::push_back(inT); + + // Mark in queue + inT->mInQueue = true; + + // Resort heap + std::push_heap(begin(), end(), sTriangleSorter); + } + + /// Peek the next closest triangle without removing it + Triangle * PeekClosest() + { + return front(); + } + + /// Get next closest triangle + Triangle * PopClosest() + { + // Move closest to end + std::pop_heap(begin(), end(), sTriangleSorter); + + // Remove last triangle + Triangle *t = back(); + pop_back(); + return t; + } + }; + + /// Constructor + explicit EPAConvexHullBuilder(const Points &inPositions) : + mPositions(inPositions) + { +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + mIteration = 0; + mOffset = RVec3::sZero(); +#endif + } + + /// Initialize the hull with 3 points + void Initialize(int inIdx1, int inIdx2, int inIdx3) + { + // Release triangles + mFactory.Clear(); + + // Create triangles (back to back) + Triangle *t1 = CreateTriangle(inIdx1, inIdx2, inIdx3); + Triangle *t2 = CreateTriangle(inIdx1, inIdx3, inIdx2); + + // Link triangles edges + sLinkTriangle(t1, 0, t2, 2); + sLinkTriangle(t1, 1, t2, 1); + sLinkTriangle(t1, 2, t2, 0); + + // Always add both triangles to the priority queue + mTriangleQueue.push_back(t1); + mTriangleQueue.push_back(t2); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw current state + DrawState(); + + // Increment iteration counter + ++mIteration; +#endif + } + + /// Check if there's another triangle to process from the queue + bool HasNextTriangle() const + { + return !mTriangleQueue.empty(); + } + + /// Access to the next closest triangle to the origin (won't remove it from the queue). + Triangle * PeekClosestTriangleInQueue() + { + return mTriangleQueue.PeekClosest(); + } + + /// Access to the next closest triangle to the origin and remove it from the queue. + Triangle * PopClosestTriangleFromQueue() + { + return mTriangleQueue.PopClosest(); + } + + /// Find the triangle on which inPosition is the furthest to the front + /// Note this function works as long as all points added have been added with AddPoint(..., FLT_MAX). + Triangle * FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq) + { + Triangle *best = nullptr; + float best_dist_sq = 0.0f; + + for (Triangle *t : mTriangleQueue) + if (!t->mRemoved) + { + float dot = t->mNormal.Dot(inPosition - t->mCentroid); + if (dot > 0.0f) + { + float dist_sq = dot * dot / t->mNormal.LengthSq(); + if (dist_sq > best_dist_sq) + { + best = t; + best_dist_sq = dist_sq; + } + } + } + + outBestDistSq = best_dist_sq; + return best; + } + + /// Add a new point to the convex hull + bool AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles) + { + // Get position + Vec3 pos = mPositions[inIdx]; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw new support point + DrawMarker(pos, Color::sYellow, 1.0f); +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + // Check if structure is intact + ValidateTriangles(); +#endif + + // Find edge of convex hull of triangles that are not facing the new vertex w + Edges edges; + if (!FindEdge(inFacingTriangle, pos, edges)) + return false; + + // Create new triangles + int num_edges = edges.size(); + for (int i = 0; i < num_edges; ++i) + { + // Create new triangle + Triangle *nt = CreateTriangle(edges[i].mStartIdx, edges[(i + 1) % num_edges].mStartIdx, inIdx); + if (nt == nullptr) + return false; + outTriangles.push_back(nt); + + // Check if we need to put this triangle in the priority queue + if ((nt->mClosestPointInterior && nt->mClosestLenSq < inClosestDistSq) // For the main algorithm + || nt->mClosestLenSq < 0.0f) // For when the origin is not inside the hull yet + mTriangleQueue.push_back(nt); + } + + // Link edges + for (int i = 0; i < num_edges; ++i) + { + sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge); + sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2); + } + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + // Check if structure is intact + ValidateTriangles(); +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw state of the hull + DrawState(); + + // Increment iteration counter + ++mIteration; +#endif + + return true; + } + + /// Free a triangle + void FreeTriangle(Triangle *inT) + { +#ifdef JPH_ENABLE_ASSERTS + // Make sure that this triangle is not connected + JPH_ASSERT(inT->mRemoved); + for (const Edge &e : inT->mEdge) + JPH_ASSERT(e.mNeighbourTriangle == nullptr); +#endif + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + // Remove from list of all triangles + Triangles::iterator i = std::find(mTriangles.begin(), mTriangles.end(), inT); + JPH_ASSERT(i != mTriangles.end()); + mTriangles.erase(i); +#endif + + mFactory.FreeTriangle(inT); + } + +private: + /// Create a new triangle + Triangle * CreateTriangle(int inIdx1, int inIdx2, int inIdx3) + { + // Call provider to create triangle + Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data()); + if (t == nullptr) + return nullptr; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Remember iteration counter + t->mIteration = mIteration; +#endif + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + // Add to list of triangles for debugging purposes + mTriangles.push_back(t); +#endif + + return t; + } + + /// Link triangle edge to other triangle edge + static void sLinkTriangle(Triangle *inT1, int inEdge1, Triangle *inT2, int inEdge2) + { + JPH_ASSERT(inEdge1 >= 0 && inEdge1 < 3); + JPH_ASSERT(inEdge2 >= 0 && inEdge2 < 3); + Edge &e1 = inT1->mEdge[inEdge1]; + Edge &e2 = inT2->mEdge[inEdge2]; + + // Check not connected yet + JPH_ASSERT(e1.mNeighbourTriangle == nullptr); + JPH_ASSERT(e2.mNeighbourTriangle == nullptr); + + // Check vertices match + JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx); + JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx); + + // Link up + e1.mNeighbourTriangle = inT2; + e1.mNeighbourEdge = inEdge2; + e2.mNeighbourTriangle = inT1; + e2.mNeighbourEdge = inEdge1; + } + + /// Unlink this triangle + void UnlinkTriangle(Triangle *inT) + { + // Unlink from neighbours + for (int i = 0; i < 3; ++i) + { + Edge &edge = inT->mEdge[i]; + if (edge.mNeighbourTriangle != nullptr) + { + Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge]; + + // Validate that neighbour points to us + JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT); + JPH_ASSERT(neighbour_edge.mNeighbourEdge == i); + + // Unlink + neighbour_edge.mNeighbourTriangle = nullptr; + edge.mNeighbourTriangle = nullptr; + } + } + + // If this triangle is not in the priority queue, we can delete it now + if (!inT->mInQueue) + FreeTriangle(inT); + } + + /// Given one triangle that faces inVertex, find the edges of the triangles that are not facing inVertex. + /// Will flag all those triangles for removal. + bool FindEdge(Triangle *inFacingTriangle, Vec3Arg inVertex, Edges &outEdges) + { + // Assert that we were given an empty array + JPH_ASSERT(outEdges.empty()); + + // Should start with a facing triangle + JPH_ASSERT(inFacingTriangle->IsFacing(inVertex)); + + // Flag as removed + inFacingTriangle->mRemoved = true; + + // Instead of recursing, we build our own stack with the information we need + struct StackEntry + { + Triangle * mTriangle; + int mEdge; + int mIter; + }; + StackEntry stack[cMaxEdgeLength]; + int cur_stack_pos = 0; + + // Start with the triangle / edge provided + stack[0].mTriangle = inFacingTriangle; + stack[0].mEdge = 0; + stack[0].mIter = -1; // Start with edge 0 (is incremented below before use) + + // Next index that we expect to find, if we don't then there are 'islands' + int next_expected_start_idx = -1; + + for (;;) + { + StackEntry &cur_entry = stack[cur_stack_pos]; + + // Next iteration + if (++cur_entry.mIter >= 3) + { + // This triangle needs to be removed, unlink it now + UnlinkTriangle(cur_entry.mTriangle); + + // Pop from stack + if (--cur_stack_pos < 0) + break; + } + else + { + // Visit neighbour + Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3]; + Triangle *n = e.mNeighbourTriangle; + if (n != nullptr && !n->mRemoved) + { + // Check if vertex is on the front side of this triangle + if (n->IsFacing(inVertex)) + { + // Vertex on front, this triangle needs to be removed + n->mRemoved = true; + + // Add element to the stack of elements to visit + cur_stack_pos++; + JPH_ASSERT(cur_stack_pos < cMaxEdgeLength); + StackEntry &new_entry = stack[cur_stack_pos]; + new_entry.mTriangle = n; + new_entry.mEdge = e.mNeighbourEdge; + new_entry.mIter = 0; // Is incremented before use, we don't need to test this edge again since we came from it + } + else + { + // Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means + // the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar + // triangles as before and some behind the point. At this point we just abort adding the point because + // we've reached numerical precision. + // Note that we do not need to test if the first and last edge connect, since when there are islands + // there should be at least 2 disconnects. + if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1) + return false; + + // Next expected index is the start index of our neighbour's edge + next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx; + + // Vertex behind, keep edge + outEdges.push_back(e); + } + } + } + } + + // Assert that we have a fully connected loop + JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw edge of facing triangles + for (int i = 0; i < (int)outEdges.size(); ++i) + { + RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]); + DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f); + DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite); + } + + // Draw the state with the facing triangles removed + DrawState(); +#endif + + // When we start with two triangles facing away from each other and adding a point that is on the plane, + // sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list. + // In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration) + return outEdges.size() >= 3; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE + /// Check consistency of 1 triangle + void ValidateTriangle(const Triangle *inT) const + { + if (inT->mRemoved) + { + // Validate that removed triangles are not connected to anything + for (const Edge &my_edge : inT->mEdge) + JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr); + } + else + { + for (int i = 0; i < 3; ++i) + { + const Edge &my_edge = inT->mEdge[i]; + + // Assert that we have a neighbour + const Triangle *nb = my_edge.mNeighbourTriangle; + JPH_ASSERT(nb != nullptr); + + if (nb != nullptr) + { + // Assert that our neighbours edge points to us + const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge]; + JPH_ASSERT(nb_edge.mNeighbourTriangle == inT); + JPH_ASSERT(nb_edge.mNeighbourEdge == i); + + // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex + const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge); + JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx); + + // Assert that my next edge points to the same vertex as my neighbours vertex + const Edge &my_next_edge = inT->GetNextEdge(i); + JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx); + } + } + } + } + + /// Check consistency of all triangles + void ValidateTriangles() const + { + for (const Triangle *t : mTriangles) + ValidateTriangle(t); + } +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW +public: + /// Draw state of algorithm + void DrawState() + { + // Draw origin + DebugRenderer::sInstance->DrawCoordinateSystem(RMat44::sTranslation(cDrawScale * mOffset), 1.0f); + + // Draw triangles + for (const Triangle *t : mTriangles) + if (!t->mRemoved) + { + // Calculate the triangle vertices + RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]); + RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]); + RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]); + + // Draw triangle + DebugRenderer::sInstance->DrawTriangle(p1, p2, p3, Color::sGetDistinctColor(t->mIteration)); + DebugRenderer::sInstance->DrawWireTriangle(p1, p2, p3, Color::sGrey); + + // Draw normal + RVec3 centroid = cDrawScale * (mOffset + t->mCentroid); + float len = t->mNormal.Length(); + if (len > 0.0f) + DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f); + } + + // Determine max position + float min_x = FLT_MAX; + float max_x = -FLT_MAX; + for (Vec3 p : mPositions) + { + min_x = min(min_x, p.GetX()); + max_x = max(max_x, p.GetX()); + } + + // Offset to the right + mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f); + } + + /// Draw a label to indicate the next stage in the algorithm + void DrawLabel(const string_view &inText) + { + DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale); + + mOffset += Vec3(5.0f, 0.0f, 0.0f); + } + + /// Draw geometry for debugging purposes + void DrawGeometry(const DebugRenderer::GeometryRef &inGeometry, ColorArg inColor) + { + RMat44 origin = RMat44::sScale(Vec3::sReplicate(cDrawScale)) * RMat44::sTranslation(mOffset); + DebugRenderer::sInstance->DrawGeometry(origin, inGeometry->mBounds.Transformed(origin), inGeometry->mBounds.GetExtent().LengthSq(), inColor, inGeometry); + + mOffset += Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0); + } + + /// Draw a triangle for debugging purposes + void DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor) + { + RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]); + for (const Edge &edge : inTriangle.mEdge) + { + RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]); + DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f); + prev = cur; + } + } + + /// Draw a marker for debugging purposes + void DrawMarker(Vec3Arg inPosition, ColorArg inColor, float inSize) + { + DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + inPosition), inColor, inSize); + } + + /// Draw an arrow for debugging purposes + void DrawArrow(Vec3Arg inFrom, Vec3Arg inTo, ColorArg inColor, float inArrowSize) + { + DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + inFrom), cDrawScale * (mOffset + inTo), inColor, inArrowSize); + } +#endif + +private: + TriangleFactory mFactory; ///< Factory to create new triangles and remove old ones + const Points & mPositions; ///< List of positions (some of them are part of the hull) + TriangleQueue mTriangleQueue; ///< List of triangles that are part of the hull that still need to be checked (if !mRemoved) + +#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW) + Triangles mTriangles; ///< The list of all triangles in this hull (for debug purposes) +#endif + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + int mIteration; ///< Number of iterations we've had so far (for debug purposes) + RVec3 mOffset; ///< Offset to use for state drawing +#endif +}; + +// The determinant that is calculated in the Triangle constructor is really sensitive +// to numerical round off, disable the fmadd instructions to maintain precision. +JPH_PRECISE_MATH_ON + +EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions) +{ + // Fill in indexes + JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2); + mEdge[0].mStartIdx = inIdx0; + mEdge[1].mStartIdx = inIdx1; + mEdge[2].mStartIdx = inIdx2; + + // Clear links + mEdge[0].mNeighbourTriangle = nullptr; + mEdge[1].mNeighbourTriangle = nullptr; + mEdge[2].mNeighbourTriangle = nullptr; + + // Get vertex positions + Vec3 y0 = inPositions[inIdx0]; + Vec3 y1 = inPositions[inIdx1]; + Vec3 y2 = inPositions[inIdx2]; + + // Calculate centroid + mCentroid = (y0 + y1 + y2) / 3.0f; + + // Calculate edges + Vec3 y10 = y1 - y0; + Vec3 y20 = y2 - y0; + Vec3 y21 = y2 - y1; + + // The most accurate normal is calculated by using the two shortest edges + // See: https://box2d.org/posts/2014/01/troublesome-triangle/ + // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length). + // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal. + // We first check which of the edges is shorter. + float y20_dot_y20 = y20.Dot(y20); + float y21_dot_y21 = y21.Dot(y21); + if (y20_dot_y20 < y21_dot_y21) + { + // We select the edges y10 and y20 + mNormal = y10.Cross(y20); + + // Check if triangle is degenerate + float normal_len_sq = mNormal.LengthSq(); + if (normal_len_sq > cMinTriangleArea) + { + // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal| + // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates and then calculating the closest + // point based on those coordinates. Note that we preserve the sign of the distance to check on which side the origin is. + float c_dot_n = mCentroid.Dot(mNormal); + mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq; + + // Calculate closest point to origin using barycentric coordinates: + // + // v = y0 + l0 * (y1 - y0) + l1 * (y2 - y0) + // v . (y1 - y0) = 0 + // v . (y2 - y0) = 0 + // + // Written in matrix form: + // + // | y10.y10 y20.y10 | | l0 | = | -y0.y10 | + // | y10.y20 y20.y20 | | l1 | | -y0.y20 | + // + // (y10 = y1 - y0 etc.) + // + // Cramers rule to invert matrix: + float y10_dot_y10 = y10.LengthSq(); + float y10_dot_y20 = y10.Dot(y20); + float determinant = y10_dot_y10 * y20_dot_y20 - y10_dot_y20 * y10_dot_y20; + if (determinant > 0.0f) // If determinant == 0 then the system is linearly dependent and the triangle is degenerate, since y10.10 * y20.y20 > y10.y20^2 it should also be > 0 + { + float y0_dot_y10 = y0.Dot(y10); + float y0_dot_y20 = y0.Dot(y20); + float l0 = (y10_dot_y20 * y0_dot_y20 - y20_dot_y20 * y0_dot_y10) / determinant; + float l1 = (y10_dot_y20 * y0_dot_y10 - y10_dot_y10 * y0_dot_y20) / determinant; + mLambda[0] = l0; + mLambda[1] = l1; + mLambdaRelativeTo0 = true; + + // Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because + // our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle + // to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue. + if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon) + mClosestPointInterior = true; + } + } + } + else + { + // We select the edges y10 and y21 + mNormal = y10.Cross(y21); + + // Check if triangle is degenerate + float normal_len_sq = mNormal.LengthSq(); + if (normal_len_sq > cMinTriangleArea) + { + // Again calculate distance between triangle and origin + float c_dot_n = mCentroid.Dot(mNormal); + mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq; + + // Calculate closest point to origin using barycentric coordinates but this time using y1 as the reference vertex + // + // v = y1 + l0 * (y0 - y1) + l1 * (y2 - y1) + // v . (y0 - y1) = 0 + // v . (y2 - y1) = 0 + // + // Written in matrix form: + // + // | y10.y10 -y21.y10 | | l0 | = | y1.y10 | + // | -y10.y21 y21.y21 | | l1 | | -y1.y21 | + // + // Cramers rule to invert matrix: + float y10_dot_y10 = y10.LengthSq(); + float y10_dot_y21 = y10.Dot(y21); + float determinant = y10_dot_y10 * y21_dot_y21 - y10_dot_y21 * y10_dot_y21; + if (determinant > 0.0f) + { + float y1_dot_y10 = y1.Dot(y10); + float y1_dot_y21 = y1.Dot(y21); + float l0 = (y21_dot_y21 * y1_dot_y10 - y10_dot_y21 * y1_dot_y21) / determinant; + float l1 = (y10_dot_y21 * y1_dot_y10 - y10_dot_y10 * y1_dot_y21) / determinant; + mLambda[0] = l0; + mLambda[1] = l1; + mLambdaRelativeTo0 = false; + + // Again check if the closest point is inside the triangle + if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon) + mClosestPointInterior = true; + } + } + } +} + +JPH_PRECISE_MATH_OFF + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/EPAPenetrationDepth.h b/WickedEngine/Jolt/Geometry/EPAPenetrationDepth.h new file mode 100644 index 000000000..1a9a4501c --- /dev/null +++ b/WickedEngine/Jolt/Geometry/EPAPenetrationDepth.h @@ -0,0 +1,555 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +//#define JPH_EPA_PENETRATION_DEPTH_DEBUG + +JPH_NAMESPACE_BEGIN + +/// Implementation of Expanding Polytope Algorithm as described in: +/// +/// Proximity Queries and Penetration Depth Computation on 3D Game Objects - Gino van den Bergen +/// +/// The implementation of this algorithm does not completely follow the article, instead of splitting +/// triangles at each edge as in fig. 7 in the article, we build a convex hull (removing any triangles that +/// are facing the new point, thereby avoiding the problem of getting really oblong triangles as mentioned in +/// the article). +/// +/// The algorithm roughly works like: +/// +/// - Start with a simplex of the Minkowski sum (difference) of two objects that was calculated by GJK +/// - This simplex should contain the origin (or else GJK would have reported: no collision) +/// - In cases where the simplex consists of 1 - 3 points, find some extra support points (of the Minkowski sum) to get to at least 4 points +/// - Convert this into a convex hull with non-zero volume (which includes the origin) +/// - A: Calculate the closest point to the origin for all triangles of the hull and take the closest one +/// - Calculate a new support point (of the Minkowski sum) in this direction and add this point to the convex hull +/// - This will remove all faces that are facing the new point and will create new triangles to fill up the hole +/// - Loop to A until no closer point found +/// - The closest point indicates the position / direction of least penetration +class EPAPenetrationDepth +{ +private: + // Typedefs + static constexpr int cMaxPoints = EPAConvexHullBuilder::cMaxPoints; + static constexpr int cMaxPointsToIncludeOriginInHull = 32; + static_assert(cMaxPointsToIncludeOriginInHull < cMaxPoints); + + using Triangle = EPAConvexHullBuilder::Triangle; + using Points = EPAConvexHullBuilder::Points; + + /// The GJK algorithm, used to start the EPA algorithm + GJKClosestPoint mGJK; + +#ifdef JPH_ENABLE_ASSERTS + /// Tolerance as passed to the GJK algorithm, used for asserting. + float mGJKTolerance = 0.0f; +#endif // JPH_ENABLE_ASSERTS + + /// A list of support points for the EPA algorithm + class SupportPoints + { + public: + /// List of support points + Points mY; + Vec3 mP[cMaxPoints]; + Vec3 mQ[cMaxPoints]; + + /// Calculate and add new support point to the list of points + template + Vec3 Add(const A &inA, const B &inB, Vec3Arg inDirection, int &outIndex) + { + // Get support point of the minkowski sum A - B + Vec3 p = inA.GetSupport(inDirection); + Vec3 q = inB.GetSupport(-inDirection); + Vec3 w = p - q; + + // Store new point + outIndex = mY.size(); + mY.push_back(w); + mP[outIndex] = p; + mQ[outIndex] = q; + + return w; + } + }; + +public: + /// Return code for GetPenetrationDepthStepGJK + enum class EStatus + { + NotColliding, ///< Returned if the objects don't collide, in this case outPointA/outPointB are invalid + Colliding, ///< Returned if the objects penetrate + Indeterminate ///< Returned if the objects penetrate further than the convex radius. In this case you need to call GetPenetrationDepthStepEPA to get the actual penetration depth. + }; + + /// Calculates penetration depth between two objects, first step of two (the GJK step) + /// + /// @param inAExcludingConvexRadius Object A without convex radius. + /// @param inBExcludingConvexRadius Object B without convex radius. + /// @param inConvexRadiusA Convex radius for A. + /// @param inConvexRadiusB Convex radius for B. + /// @param ioV Pass in previously returned value or (1, 0, 0). On return this value is changed to direction to move B out of collision along the shortest path (magnitude is meaningless). + /// @param inTolerance Minimal distance before A and B are considered colliding. + /// @param outPointA Position on A that has the least amount of penetration. + /// @param outPointB Position on B that has the least amount of penetration. + /// Use |outPointB - outPointA| to get the distance of penetration. + template + EStatus GetPenetrationDepthStepGJK(const AE &inAExcludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, float inConvexRadiusB, float inTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + JPH_PROFILE_FUNCTION(); + + JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inTolerance;) + + // Don't supply a zero ioV, we only want to get points on the hull of the Minkowsky sum and not internal points + JPH_ASSERT(!ioV.IsNearZero()); + + // Get closest points + float combined_radius = inConvexRadiusA + inConvexRadiusB; + float combined_radius_sq = combined_radius * combined_radius; + float closest_points_dist_sq = mGJK.GetClosestPoints(inAExcludingConvexRadius, inBExcludingConvexRadius, inTolerance, combined_radius_sq, ioV, outPointA, outPointB); + if (closest_points_dist_sq > combined_radius_sq) + { + // No collision + return EStatus::NotColliding; + } + if (closest_points_dist_sq > 0.0f) + { + // Collision within convex radius, adjust points for convex radius + float v_len = sqrt(closest_points_dist_sq); // GetClosestPoints function returns |ioV|^2 when return value < FLT_MAX + outPointA += ioV * (inConvexRadiusA / v_len); + outPointB -= ioV * (inConvexRadiusB / v_len); + return EStatus::Colliding; + } + + return EStatus::Indeterminate; + } + + /// Calculates penetration depth between two objects, second step (the EPA step) + /// + /// @param inAIncludingConvexRadius Object A with convex radius + /// @param inBIncludingConvexRadius Object B with convex radius + /// @param inTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON. + /// @param outV Direction to move B out of collision along the shortest path (magnitude is meaningless) + /// @param outPointA Position on A that has the least amount of penetration + /// @param outPointB Position on B that has the least amount of penetration + /// Use |outPointB - outPointA| to get the distance of penetration + /// + /// @return False if the objects don't collide, in this case outPointA/outPointB are invalid. + /// True if the objects penetrate + template + bool GetPenetrationDepthStepEPA(const AI &inAIncludingConvexRadius, const BI &inBIncludingConvexRadius, float inTolerance, Vec3 &outV, Vec3 &outPointA, Vec3 &outPointB) + { + JPH_PROFILE_FUNCTION(); + + // Check that the tolerance makes sense (smaller value than this will just result in needless iterations) + JPH_ASSERT(inTolerance >= FLT_EPSILON); + + // Fetch the simplex from GJK algorithm + SupportPoints support_points; + mGJK.GetClosestPointsSimplex(support_points.mY.data(), support_points.mP, support_points.mQ, support_points.mY.GetSizeRef()); + + // Fill up the amount of support points to 4 + switch (support_points.mY.size()) + { + case 1: + { + // 1 vertex, which must be at the origin, which is useless for our purpose + JPH_ASSERT(support_points.mY[0].IsNearZero(Square(mGJKTolerance))); + support_points.mY.pop_back(); + + // Add support points in 4 directions to form a tetrahedron around the origin + int p1, p2, p3, p4; + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, 1, 0), p1); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(-1, -1, -1), p2); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(1, -1, -1), p3); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, Vec3(0, -1, 1), p4); + JPH_ASSERT(p1 == 0); + JPH_ASSERT(p2 == 1); + JPH_ASSERT(p3 == 2); + JPH_ASSERT(p4 == 3); + break; + } + + case 2: + { + // Two vertices, create 3 extra by taking perpendicular axis and rotating it around in 120 degree increments + Vec3 axis = (support_points.mY[1] - support_points.mY[0]).Normalized(); + Mat44 rotation = Mat44::sRotation(axis, DegreesToRadians(120.0f)); + Vec3 dir1 = axis.GetNormalizedPerpendicular(); + Vec3 dir2 = rotation * dir1; + Vec3 dir3 = rotation * dir2; + int p1, p2, p3; + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir1, p1); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir2, p2); + (void)support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, dir3, p3); + JPH_ASSERT(p1 == 2); + JPH_ASSERT(p2 == 3); + JPH_ASSERT(p3 == 4); + break; + } + + case 3: + case 4: + // We already have enough points + break; + } + + // Create hull out of the initial points + JPH_ASSERT(support_points.mY.size() >= 3); + EPAConvexHullBuilder hull(support_points.mY); +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Build initial hull"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Init: num_points = %u", (uint)support_points.mY.size()); +#endif + hull.Initialize(0, 1, 2); + for (typename Points::size_type i = 3; i < support_points.mY.size(); ++i) + { + float dist_sq; + Triangle *t = hull.FindFacingTriangle(support_points.mY[i], dist_sq); + if (t != nullptr) + { + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!hull.AddPoint(t, i, FLT_MAX, new_triangles)) + { + // We can't recover from a failure to add a point to the hull because the old triangles have been unlinked already. + // Assume no collision. This can happen if the shapes touch in 1 point (or plane) in which case the hull is degenerate. + return false; + } + } + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Complete hull"); + + // Generate the hull of the Minkowski difference for visualization + MinkowskiDifference diff(inAIncludingConvexRadius, inBIncludingConvexRadius); + DebugRenderer::GeometryRef geometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); }); + hull.DrawGeometry(geometry, Color::sYellow); + + hull.DrawLabel("Ensure origin in hull"); +#endif + + // Loop until we are sure that the origin is inside the hull + for (;;) + { + // Get the next closest triangle + Triangle *t = hull.PeekClosestTriangleInQueue(); + + // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap) + if (t->mRemoved) + { + hull.PopClosestTriangleFromQueue(); + + // If we run out of triangles, we couldn't include the origin in the hull so there must be very little penetration and we report no collision. + if (!hull.HasNextTriangle()) + return false; + + hull.FreeTriangle(t); + continue; + } + + // If the closest to the triangle is zero or positive, the origin is in the hull and we can proceed to the main algorithm + if (t->mClosestLenSq >= 0.0f) + break; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Next iteration"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("EncapsulateOrigin: verts = (%d, %d, %d), closest_dist_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)", + t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx, + t->mClosestLenSq, + t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(), + t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ()); +#endif + + // Remove the triangle from the queue before we start adding new ones (which may result in a new closest triangle at the front of the queue) + hull.PopClosestTriangleFromQueue(); + + // Add a support point to get the origin inside the hull + int new_index; + Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index); + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw the point that we're adding + hull.DrawMarker(w, Color::sRed, 1.0f); + hull.DrawWireTriangle(*t, Color::sRed); + hull.DrawState(); +#endif + + // Add the point to the hull, if we fail we terminate and report no collision + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!t->IsFacing(w) || !hull.AddPoint(t, new_index, FLT_MAX, new_triangles)) + return false; + + // The triangle is facing the support point "w" and can now be safely removed + JPH_ASSERT(t->mRemoved); + hull.FreeTriangle(t); + + // If we run out of triangles or points, we couldn't include the origin in the hull so there must be very little penetration and we report no collision. + if (!hull.HasNextTriangle() || support_points.mY.size() >= cMaxPointsToIncludeOriginInHull) + return false; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Main algorithm"); +#endif + + // Current closest distance to origin + float closest_dist_sq = FLT_MAX; + + // Remember last good triangle + Triangle *last = nullptr; + + // If we want to flip the penetration depth + bool flip_v_sign = false; + + // Loop until closest point found + do + { + // Get closest triangle to the origin + Triangle *t = hull.PopClosestTriangleFromQueue(); + + // Don't process removed triangles, just free them (because they're in a heap we don't remove them earlier since we would have to rebuild the sorted heap) + if (t->mRemoved) + { + hull.FreeTriangle(t); + continue; + } + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Next iteration"); +#endif +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("FindClosest: verts = (%d, %d, %d), closest_len_sq = %g, centroid = (%g, %g, %g), normal = (%g, %g, %g)", + t->mEdge[0].mStartIdx, t->mEdge[1].mStartIdx, t->mEdge[2].mStartIdx, + t->mClosestLenSq, + t->mCentroid.GetX(), t->mCentroid.GetY(), t->mCentroid.GetZ(), + t->mNormal.GetX(), t->mNormal.GetY(), t->mNormal.GetZ()); +#endif + // Check if next triangle is further away than closest point, we've found the closest point + if (t->mClosestLenSq >= closest_dist_sq) + break; + + // Replace last good with this triangle + if (last != nullptr) + hull.FreeTriangle(last); + last = t; + + // Add support point in direction of normal of the plane + // Note that the article uses the closest point between the origin and plane, but this always has the exact same direction as the normal (if the origin is behind the plane) + // and this way we do less calculations and lose less precision + int new_index; + Vec3 w = support_points.Add(inAIncludingConvexRadius, inBIncludingConvexRadius, t->mNormal, new_index); + + // Project w onto the triangle normal + float dot = t->mNormal.Dot(w); + + // Check if we just found a separating axis. This can happen if the shape shrunk by convex radius and then expanded by + // convex radius is bigger then the original shape due to inaccuracies in the shrinking process. + if (dot < 0.0f) + return false; + + // Get the distance squared (along normal) to the support point + float dist_sq = Square(dot) / t->mNormal.LengthSq(); + +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("FindClosest: w = (%g, %g, %g), dot = %g, dist_sq = %g", + w.GetX(), w.GetY(), w.GetZ(), + dot, dist_sq); +#endif +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + // Draw the point that we're adding + hull.DrawMarker(w, Color::sPurple, 1.0f); + hull.DrawWireTriangle(*t, Color::sPurple); + hull.DrawState(); +#endif + + // If the error became small enough, we've converged + if (dist_sq - t->mClosestLenSq < t->mClosestLenSq * inTolerance) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Converged"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // Keep track of the minimum distance + closest_dist_sq = min(closest_dist_sq, dist_sq); + + // If the triangle thinks this point is not front facing, we've reached numerical precision and we're done + if (!t->IsFacing(w)) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Not facing triangle"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // Add point to hull + EPAConvexHullBuilder::NewTriangles new_triangles; + if (!hull.AddPoint(t, new_index, closest_dist_sq, new_triangles)) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Could not add point"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + break; + } + + // If the hull is starting to form defects then we're reaching numerical precision and we have to stop + bool has_defect = false; + for (const Triangle *nt : new_triangles) + if (nt->IsFacingOrigin()) + { + has_defect = true; + break; + } + if (has_defect) + { +#ifdef JPH_EPA_PENETRATION_DEPTH_DEBUG + Trace("Has defect"); +#endif // JPH_EPA_PENETRATION_DEPTH_DEBUG + // When the hull has defects it is possible that the origin has been classified on the wrong side of the triangle + // so we do an additional check to see if the penetration in the -triangle normal direction is smaller than + // the penetration in the triangle normal direction. If so we must flip the sign of the penetration depth. + Vec3 w2 = inAIncludingConvexRadius.GetSupport(-t->mNormal) - inBIncludingConvexRadius.GetSupport(t->mNormal); + float dot2 = -t->mNormal.Dot(w2); + if (dot2 < dot) + flip_v_sign = true; + break; + } + } + while (hull.HasNextTriangle() && support_points.mY.size() < cMaxPoints); + + // Determine closest points, if last == null it means the hull was a plane so there's no penetration + if (last == nullptr) + return false; + +#ifdef JPH_EPA_CONVEX_BUILDER_DRAW + hull.DrawLabel("Closest found"); + hull.DrawWireTriangle(*last, Color::sWhite); + hull.DrawArrow(last->mCentroid, last->mCentroid + last->mNormal.NormalizedOr(Vec3::sZero()), Color::sWhite, 0.1f); + hull.DrawState(); +#endif + + // Calculate penetration by getting the vector from the origin to the closest point on the triangle: + // distance = (centroid - origin) . normal / |normal|, closest = origin + distance * normal / |normal| + outV = (last->mCentroid.Dot(last->mNormal) / last->mNormal.LengthSq()) * last->mNormal; + + // If penetration is near zero, treat this as a non collision since we cannot find a good normal + if (outV.IsNearZero()) + return false; + + // Check if we have to flip the sign of the penetration depth + if (flip_v_sign) + outV = -outV; + + // Use the barycentric coordinates for the closest point to the origin to find the contact points on A and B + Vec3 p0 = support_points.mP[last->mEdge[0].mStartIdx]; + Vec3 p1 = support_points.mP[last->mEdge[1].mStartIdx]; + Vec3 p2 = support_points.mP[last->mEdge[2].mStartIdx]; + + Vec3 q0 = support_points.mQ[last->mEdge[0].mStartIdx]; + Vec3 q1 = support_points.mQ[last->mEdge[1].mStartIdx]; + Vec3 q2 = support_points.mQ[last->mEdge[2].mStartIdx]; + + if (last->mLambdaRelativeTo0) + { + // y0 was the reference vertex + outPointA = p0 + last->mLambda[0] * (p1 - p0) + last->mLambda[1] * (p2 - p0); + outPointB = q0 + last->mLambda[0] * (q1 - q0) + last->mLambda[1] * (q2 - q0); + } + else + { + // y1 was the reference vertex + outPointA = p1 + last->mLambda[0] * (p0 - p1) + last->mLambda[1] * (p2 - p1); + outPointB = q1 + last->mLambda[0] * (q0 - q1) + last->mLambda[1] * (q2 - q1); + } + + return true; + } + + /// This function combines the GJK and EPA steps and is provided as a convenience function. + /// Note: less performant since you're providing all support functions in one go + /// Note 2: You need to initialize ioV, see documentation at GetPenetrationDepthStepGJK! + template + bool GetPenetrationDepth(const AE &inAExcludingConvexRadius, const AI &inAIncludingConvexRadius, float inConvexRadiusA, const BE &inBExcludingConvexRadius, const BI &inBIncludingConvexRadius, float inConvexRadiusB, float inCollisionToleranceSq, float inPenetrationTolerance, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + // Check result of collision detection + switch (GetPenetrationDepthStepGJK(inAExcludingConvexRadius, inConvexRadiusA, inBExcludingConvexRadius, inConvexRadiusB, inCollisionToleranceSq, ioV, outPointA, outPointB)) + { + case EPAPenetrationDepth::EStatus::Colliding: + return true; + + case EPAPenetrationDepth::EStatus::NotColliding: + return false; + + case EPAPenetrationDepth::EStatus::Indeterminate: + return GetPenetrationDepthStepEPA(inAIncludingConvexRadius, inBIncludingConvexRadius, inPenetrationTolerance, ioV, outPointA, outPointB); + } + + JPH_ASSERT(false); + return false; + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inCollisionTolerance The minimal distance between A and B before they are considered colliding + /// @param inPenetrationTolerance A factor that determines the accuracy of the result. If the change of the squared distance is less than inTolerance * current_penetration_depth^2 the algorithm will terminate. Should be bigger or equal to FLT_EPSILON. + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A. + /// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B. + /// @param inReturnDeepestPoint If the shapes are initially intersecting this determines if the EPA algorithm will run to find the deepest point + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// @param outPointA is the contact point on A + /// @param outPointB is the contact point on B + /// @param outContactNormal is either the contact normal when the objects are touching or the penetration axis when the objects are penetrating at the start of the sweep (pointing from A to B, length will not be 1) + /// + /// @return true if the a hit was found, in which case ioLambda, outPointA, outPointB and outSurfaceNormal are updated. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inCollisionTolerance, float inPenetrationTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, bool inReturnDeepestPoint, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outContactNormal) + { + JPH_IF_ENABLE_ASSERTS(mGJKTolerance = inCollisionTolerance;) + + // First determine if there's a collision at all + if (!mGJK.CastShape(inStart, inDirection, inCollisionTolerance, inA, inB, inConvexRadiusA, inConvexRadiusB, ioLambda, outPointA, outPointB, outContactNormal)) + return false; + + // When our contact normal is too small, we don't have an accurate result + bool contact_normal_invalid = outContactNormal.IsNearZero(Square(inCollisionTolerance)); + + if (inReturnDeepestPoint + && ioLambda == 0.0f // Only when lambda = 0 we can have the bodies overlap + && (inConvexRadiusA + inConvexRadiusB == 0.0f // When no convex radius was provided we can never trust contact points at lambda = 0 + || contact_normal_invalid)) + { + // If we're initially intersecting, we need to run the EPA algorithm in order to find the deepest contact point + AddConvexRadius add_convex_a(inA, inConvexRadiusA); + AddConvexRadius add_convex_b(inB, inConvexRadiusB); + TransformedConvexObject> transformed_a(inStart, add_convex_a); + if (!GetPenetrationDepthStepEPA(transformed_a, add_convex_b, inPenetrationTolerance, outContactNormal, outPointA, outPointB)) + return false; + } + else if (contact_normal_invalid) + { + // If we weren't able to calculate a contact normal, use the cast direction instead + outContactNormal = inDirection; + } + + return true; + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/Ellipse.h b/WickedEngine/Jolt/Geometry/Ellipse.h new file mode 100644 index 000000000..bfa508faa --- /dev/null +++ b/WickedEngine/Jolt/Geometry/Ellipse.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Ellipse centered around the origin +/// @see https://en.wikipedia.org/wiki/Ellipse +class Ellipse +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct ellipse with radius A along the X-axis and B along the Y-axis + Ellipse(float inA, float inB) : mA(inA), mB(inB) { JPH_ASSERT(inA > 0.0f); JPH_ASSERT(inB > 0.0f); } + + /// Check if inPoint is inside the ellipse + bool IsInside(const Float2 &inPoint) const + { + return Square(inPoint.x / mA) + Square(inPoint.y / mB) <= 1.0f; + } + + /// Get the closest point on the ellipse to inPoint + /// Assumes inPoint is outside the ellipse + /// @see Rotation Joint Limits in Quaternion Space by Gino van den Bergen, section 10.1 in Game Engine Gems 3. + Float2 GetClosestPoint(const Float2 &inPoint) const + { + float a_sq = Square(mA); + float b_sq = Square(mB); + + // Equation of ellipse: f(x, y) = (x/a)^2 + (y/b)^2 - 1 = 0 [1] + // Normal on surface: (df/dx, df/dy) = (2 x / a^2, 2 y / b^2) + // Closest point (x', y') on ellipse to point (x, y): (x', y') + t (x / a^2, y / b^2) = (x, y) + // <=> (x', y') = (a^2 x / (t + a^2), b^2 y / (t + b^2)) + // Requiring point to be on ellipse (substituting into [1]): g(t) = (a x / (t + a^2))^2 + (b y / (t + b^2))^2 - 1 = 0 + + // Newton raphson iteration, starting at t = 0 + float t = 0.0f; + for (;;) + { + // Calculate g(t) + float t_plus_a_sq = t + a_sq; + float t_plus_b_sq = t + b_sq; + float gt = Square(mA * inPoint.x / t_plus_a_sq) + Square(mB * inPoint.y / t_plus_b_sq) - 1.0f; + + // Check if g(t) it is close enough to zero + if (abs(gt) < 1.0e-6f) + return Float2(a_sq * inPoint.x / t_plus_a_sq, b_sq * inPoint.y / t_plus_b_sq); + + // Get derivative dg/dt = g'(t) = -2 (b^2 y^2 / (t + b^2)^3 + a^2 x^2 / (t + a^2)^3) + float gt_accent = -2.0f * + (a_sq * Square(inPoint.x) / Cubed(t_plus_a_sq) + + b_sq * Square(inPoint.y) / Cubed(t_plus_b_sq)); + + // Calculate t for next iteration: tn+1 = tn - g(t) / g'(t) + float tn = t - gt / gt_accent; + t = tn; + } + } + + /// Get normal at point inPoint (non-normalized vector) + Float2 GetNormal(const Float2 &inPoint) const + { + // Calculated by [d/dx f(x, y), d/dy f(x, y)], where f(x, y) is the ellipse equation from above + return Float2(inPoint.x / Square(mA), inPoint.y / Square(mB)); + } + +private: + float mA; ///< Radius along X-axis + float mB; ///< Radius along Y-axis +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/GJKClosestPoint.h b/WickedEngine/Jolt/Geometry/GJKClosestPoint.h new file mode 100644 index 000000000..d4bfbf40c --- /dev/null +++ b/WickedEngine/Jolt/Geometry/GJKClosestPoint.h @@ -0,0 +1,952 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +//#define JPH_GJK_DEBUG +#ifdef JPH_GJK_DEBUG + #include + #include +#endif + +JPH_NAMESPACE_BEGIN + +/// Convex vs convex collision detection +/// Based on: A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen +class GJKClosestPoint : public NonCopyable +{ +private: + /// Get new closest point to origin given simplex mY of mNumPoints points + /// + /// @param inPrevVLenSq Length of |outV|^2 from the previous iteration, used as a maximum value when selecting a new closest point. + /// @param outV Closest point + /// @param outVLenSq |outV|^2 + /// @param outSet Set of points that form the new simplex closest to the origin (bit 1 = mY[0], bit 2 = mY[1], ...) + /// + /// If LastPointPartOfClosestFeature is true then the last point added will be assumed to be part of the closest feature and the function will do less work. + /// + /// @return True if new closest point was found. + /// False if the function failed, in this case the output variables are not modified + template + bool GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const + { +#ifdef JPH_GJK_DEBUG + for (int i = 0; i < mNumPoints; ++i) + Trace("y[%d] = [%s], |y[%d]| = %g", i, ConvertToString(mY[i]).c_str(), i, (double)mY[i].Length()); +#endif + + uint32 set; + Vec3 v; + + switch (mNumPoints) + { + case 1: + // Single point + set = 0b0001; + v = mY[0]; + break; + + case 2: + // Line segment + v = ClosestPoint::GetClosestPointOnLine(mY[0], mY[1], set); + break; + + case 3: + // Triangle + v = ClosestPoint::GetClosestPointOnTriangle(mY[0], mY[1], mY[2], set); + break; + + case 4: + // Tetrahedron + v = ClosestPoint::GetClosestPointOnTetrahedron(mY[0], mY[1], mY[2], mY[3], set); + break; + + default: + JPH_ASSERT(false); + return false; + } + +#ifdef JPH_GJK_DEBUG + Trace("GetClosest: set = 0b%s, v = [%s], |v| = %g", NibbleToBinary(set), ConvertToString(v).c_str(), (double)v.Length()); +#endif + + float v_len_sq = v.LengthSq(); + if (v_len_sq < inPrevVLenSq) // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false + { + // Return closest point + outV = v; + outVLenSq = v_len_sq; + outSet = set; + return true; + } + + // No better match found +#ifdef JPH_GJK_DEBUG + Trace("New closer point is further away, failed to converge"); +#endif + return false; + } + + // Get max(|Y_0|^2 .. |Y_n|^2) + float GetMaxYLengthSq() const + { + float y_len_sq = mY[0].LengthSq(); + for (int i = 1; i < mNumPoints; ++i) + y_len_sq = max(y_len_sq, mY[i].LengthSq()); + return y_len_sq; + } + + // Remove points that are not in the set, only updates mY + void UpdatePointSetY(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mY[num_points] = mY[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // GCC 11.3 thinks the assignments to mP, mQ and mY below may use uninitialized variables + JPH_SUPPRESS_WARNING_PUSH + JPH_GCC_SUPPRESS_WARNING("-Wmaybe-uninitialized") + + // Remove points that are not in the set, only updates mP + void UpdatePointSetP(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mP[num_points] = mP[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, only updates mP and mQ + void UpdatePointSetPQ(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mP[num_points] = mP[i]; + mQ[num_points] = mQ[i]; + ++num_points; + } + mNumPoints = num_points; + } + + // Remove points that are not in the set, updates mY, mP and mQ + void UpdatePointSetYPQ(uint32 inSet) + { + int num_points = 0; + for (int i = 0; i < mNumPoints; ++i) + if ((inSet & (1 << i)) != 0) + { + mY[num_points] = mY[i]; + mP[num_points] = mP[i]; + mQ[num_points] = mQ[i]; + ++num_points; + } + mNumPoints = num_points; + } + + JPH_SUPPRESS_WARNING_POP + + // Calculate closest points on A and B + void CalculatePointAAndB(Vec3 &outPointA, Vec3 &outPointB) const + { + switch (mNumPoints) + { + case 1: + outPointA = mP[0]; + outPointB = mQ[0]; + break; + + case 2: + { + float u, v; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], u, v); + outPointA = u * mP[0] + v * mP[1]; + outPointB = u * mQ[0] + v * mQ[1]; + } + break; + + case 3: + { + float u, v, w; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], u, v, w); + outPointA = u * mP[0] + v * mP[1] + w * mP[2]; + outPointB = u * mQ[0] + v * mQ[1] + w * mQ[2]; + } + break; + + case 4: + #ifdef JPH_DEBUG + memset(&outPointA, 0xcd, sizeof(outPointA)); + memset(&outPointB, 0xcd, sizeof(outPointB)); + #endif + break; + } + } + +public: + /// Test if inA and inB intersect + /// + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inTolerance Minimal distance between objects when the objects are considered to be colliding + /// @param ioV is used as initial separating axis (provide a zero vector if you don't know yet) + /// + /// @return True if they intersect (in which case ioV = (0, 0, 0)). + /// False if they don't intersect in which case ioV is a separating axis in the direction from A to B (magnitude is meaningless) + template + bool Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + +#ifdef JPH_GJK_DEBUG + for (int i = 0; i < 4; ++i) + mY[i] = Vec3::sZero(); +#endif + + // Previous length^2 of v + float prev_v_len_sq = FLT_MAX; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints); +#endif + + // Get support points for shape A and B in the direction of v + Vec3 p = inA.GetSupport(ioV); + Vec3 q = inB.GetSupport(-ioV); + + // Get support point of the minkowski sum A - B of v + Vec3 w = p - q; + + // If the support point sA-B(v) is in the opposite direction as v, then we have found a separating axis and there is no intersection + if (ioV.Dot(w) < 0.0f) + { + // Separating axis found +#ifdef JPH_GJK_DEBUG + Trace("Separating axis"); +#endif + return false; + } + + // Store the point for later use + mY[mNumPoints] = w; + ++mNumPoints; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + // Determine the new closest point + float v_len_sq; // Length^2 of v + uint32 set; // Set of points that form the new simplex + if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set)) + return false; + + // If there are 4 points, the origin is inside the tetrahedron and we're done + if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // If v is very close to zero, we consider this a collision + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance zero"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // If v is very small compared to the length of y, we also consider this a collision + if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq()) + { +#ifdef JPH_GJK_DEBUG + Trace("Machine precision reached"); +#endif + ioV = Vec3::sZero(); + return true; + } + + // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin + // Note: This must be done before terminating as converged since the separating axis is -v + ioV = -ioV; + + // If the squared length of v is not changing enough, we've converged and there is no collision + JPH_ASSERT(prev_v_len_sq >= v_len_sq); + if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq) + { + // v is a separating axis +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + return false; + } + prev_v_len_sq = v_len_sq; + + // Update the points of the simplex + UpdatePointSetY(set); + } + } + + /// Get closest points between inA and inB + /// + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inTolerance The minimal distance between A and B before the objects are considered colliding and processing is terminated. + /// @param inMaxDistSq The maximum squared distance between A and B before the objects are considered infinitely far away and processing is terminated. + /// @param ioV Initial guess for the separating axis. Start with any non-zero vector if you don't know. + /// If return value is 0, ioV = (0, 0, 0). + /// If the return value is bigger than 0 but smaller than FLT_MAX, ioV will be the separating axis in the direction from A to B and its length the squared distance between A and B. + /// If the return value is FLT_MAX, ioV will be the separating axis in the direction from A to B and the magnitude of the vector is meaningless. + /// @param outPointA , outPointB + /// If the return value is 0 the points are invalid. + /// If the return value is bigger than 0 but smaller than FLT_MAX these will contain the closest point on A and B. + /// If the return value is FLT_MAX the points are invalid. + /// + /// @return The squared distance between A and B or FLT_MAX when they are further away than inMaxDistSq. + template + float GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + +#ifdef JPH_GJK_DEBUG + // Generate the hull of the Minkowski difference for visualization + MinkowskiDifference diff(inA, inB); + mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); }); + + for (int i = 0; i < 4; ++i) + { + mY[i] = Vec3::sZero(); + mP[i] = Vec3::sZero(); + mQ[i] = Vec3::sZero(); + } +#endif + + // Length^2 of v + float v_len_sq = ioV.LengthSq(); + + // Previous length^2 of v + float prev_v_len_sq = FLT_MAX; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints); +#endif + + // Get support points for shape A and B in the direction of v + Vec3 p = inA.GetSupport(ioV); + Vec3 q = inB.GetSupport(-ioV); + + // Get support point of the minkowski sum A - B of v + Vec3 w = p - q; + + float dot = ioV.Dot(w); + +#ifdef JPH_GJK_DEBUG + // Draw -ioV to show the closest point to the origin from the previous simplex + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f); + + // Draw ioV to show where we're probing next + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + ioV, Color::sCyan, 0.05f); + + // Draw w, the support point + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + w, Color::sGreen, 0.05f); + DebugRenderer::sInstance->DrawMarker(mOffset + w, Color::sGreen, 1.0f); + + // Draw the simplex and the Minkowski difference around it + DrawState(); +#endif + + // Test if we have a separation of more than inMaxDistSq, in which case we terminate early + if (dot < 0.0f && dot * dot > v_len_sq * inMaxDistSq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance bigger than max"); +#endif +#ifdef JPH_DEBUG + memset(&outPointA, 0xcd, sizeof(outPointA)); + memset(&outPointB, 0xcd, sizeof(outPointB)); +#endif + return FLT_MAX; + } + + // Store the point for later use + mY[mNumPoints] = w; + mP[mNumPoints] = p; + mQ[mNumPoints] = q; + ++mNumPoints; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + uint32 set; + if (!GetClosest(prev_v_len_sq, ioV, v_len_sq, set)) + { + --mNumPoints; // Undo add last point + break; + } + + // If there are 4 points, the origin is inside the tetrahedron and we're done + if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // Update the points of the simplex + UpdatePointSetYPQ(set); + + // If v is very close to zero, we consider this a collision + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Distance zero"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // If v is very small compared to the length of y, we also consider this a collision +#ifdef JPH_GJK_DEBUG + Trace("Check v small compared to y: %g <= %g", (double)v_len_sq, (double)(FLT_EPSILON * GetMaxYLengthSq())); +#endif + if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq()) + { +#ifdef JPH_GJK_DEBUG + Trace("Machine precision reached"); +#endif + ioV = Vec3::sZero(); + v_len_sq = 0.0f; + break; + } + + // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin + // Note: This must be done before terminating as converged since the separating axis is -v + ioV = -ioV; + + // If the squared length of v is not changing enough, we've converged and there is no collision +#ifdef JPH_GJK_DEBUG + Trace("Check v not changing enough: %g <= %g", (double)(prev_v_len_sq - v_len_sq), (double)(FLT_EPSILON * prev_v_len_sq)); +#endif + JPH_ASSERT(prev_v_len_sq >= v_len_sq); + if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq) + { + // v is a separating axis +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + prev_v_len_sq = v_len_sq; + } + + // Get the closest points + CalculatePointAAndB(outPointA, outPointB); + +#ifdef JPH_GJK_DEBUG + Trace("Return: v = [%s], |v| = %g", ConvertToString(ioV).c_str(), (double)ioV.Length()); + + // Draw -ioV to show the closest point to the origin from the previous simplex + DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f); + + // Draw the closest points + DebugRenderer::sInstance->DrawMarker(mOffset + outPointA, Color::sGreen, 1.0f); + DebugRenderer::sInstance->DrawMarker(mOffset + outPointB, Color::sPurple, 1.0f); + + // Draw the simplex and the Minkowski difference around it + DrawState(); +#endif + + JPH_ASSERT(ioV.LengthSq() == v_len_sq); + return v_len_sq; + } + + /// Get the resulting simplex after the GetClosestPoints algorithm finishes. + /// If it returned a squared distance of 0, the origin will be contained in the simplex. + void GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const + { + uint size = sizeof(Vec3) * mNumPoints; + memcpy(outY, mY, size); + memcpy(outP, mP, size); + memcpy(outQ, mQ, size); + outNumPoints = mNumPoints; + } + + /// Test if a ray inRayOrigin + lambda * inRayDirection for lambda e [0, ioLambda> intersects inA + /// + /// Code based upon: Ray Casting against General Convex Objects with Application to Continuous Collision Detection - Gino van den Bergen + /// + /// @param inRayOrigin Origin of the ray + /// @param inRayDirection Direction of the ray (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between the ray and A before it is considered colliding + /// @param inA A convex object that has the GetSupport(Vec3) function + /// @param ioLambda The max fraction along the ray, on output updated with the actual collision fraction. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda. + template + bool CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda) + { + float tolerance_sq = Square(inTolerance); + + // Reset state + mNumPoints = 0; + + float lambda = 0.0f; + Vec3 x = inRayOrigin; + Vec3 v = x - inA.GetSupport(Vec3::sZero()); + float v_len_sq = FLT_MAX; + bool allow_restart = false; + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints); +#endif + + // Get new support point + Vec3 p = inA.GetSupport(v); + Vec3 w = x - p; + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + float v_dot_w = v.Dot(w); +#ifdef JPH_GJK_DEBUG + Trace("v . w = %g", (double)v_dot_w); +#endif + if (v_dot_w > 0.0f) + { + // If ray and normal are in the same direction, we've passed A and there's no collision + float v_dot_r = v.Dot(inRayDirection); +#ifdef JPH_GJK_DEBUG + Trace("v . r = %g", (double)v_dot_r); +#endif + if (v_dot_r >= 0.0f) + return false; + + // Update the lower bound for lambda + float delta = v_dot_w / v_dot_r; + float old_lambda = lambda; + lambda -= delta; +#ifdef JPH_GJK_DEBUG + Trace("lambda = %g, delta = %g", (double)lambda, (double)delta); +#endif + + // If lambda didn't change, we cannot converge any further and we assume a hit + if (old_lambda == lambda) + break; + + // If lambda is bigger or equal than max, we don't have a hit + if (lambda >= ioLambda) + return false; + + // Update x to new closest point on the ray + x = inRayOrigin + lambda * inRayDirection; + + // We've shifted x, so reset v_len_sq so that it is not used as early out for GetClosest + v_len_sq = FLT_MAX; + + // We allow rebuilding the simplex once after x changes because the simplex was built + // for another x and numerical round off builds up as you keep adding points to an + // existing simplex + allow_restart = true; + } + + // Add p to set P: P = P U {p} + mP[mNumPoints] = p; + ++mNumPoints; + + // Calculate Y = {x} - P + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - mP[i]; + + // Determine the new closest point from Y to origin + uint32 set; // Set of points that form the new simplex + if (!GetClosest(v_len_sq, v, v_len_sq, set)) + { +#ifdef JPH_GJK_DEBUG + Trace("Failed to converge"); +#endif + + // Only allow 1 restart, if we still can't get a closest point + // we're so close that we return this as a hit + if (!allow_restart) + break; + + // If we fail to converge, we start again with the last point as simplex +#ifdef JPH_GJK_DEBUG + Trace("Restarting"); +#endif + allow_restart = false; + mP[0] = p; + mNumPoints = 1; + v = x - p; + v_len_sq = FLT_MAX; + continue; + } + else if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + + // We're inside the tetrahedron, we have a hit (verify that length of v is 0) + JPH_ASSERT(v_len_sq == 0.0f); + break; + } + + // Update the points P to form the new simplex + // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration + UpdatePointSetP(set); + + // Check if x is close enough to inA + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + } + + // Store hit fraction + ioLambda = lambda; + return true; + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between A and B before they are considered colliding + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda) + { + // Transform the shape to be cast to the starting position + TransformedConvexObject transformed_a(inStart, inA); + + // Calculate the minkowski difference inB - inA + // inA is moving, so we need to add the back side of inB to the front side of inA + MinkowskiDifference difference(inB, transformed_a); + + // Do a raycast against the Minkowski difference + return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda); + } + + /// Test if a cast shape inA moving from inStart to lambda * inStart.GetTranslation() + inDirection where lambda e [0, ioLambda> intersects inB + /// + /// @param inStart Start position and orientation of the convex object + /// @param inDirection Direction of the sweep (ioLambda * inDirection determines length) + /// @param inTolerance The minimal distance between A and B before they are considered colliding + /// @param inA The convex object A, must support the GetSupport(Vec3) function. + /// @param inB The convex object B, must support the GetSupport(Vec3) function. + /// @param inConvexRadiusA The convex radius of A, this will be added on all sides to pad A. + /// @param inConvexRadiusB The convex radius of B, this will be added on all sides to pad B. + /// @param ioLambda The max fraction along the sweep, on output updated with the actual collision fraction. + /// @param outPointA is the contact point on A (if outSeparatingAxis is near zero, this may not be not the deepest point) + /// @param outPointB is the contact point on B (if outSeparatingAxis is near zero, this may not be not the deepest point) + /// @param outSeparatingAxis On return this will contain a vector that points from A to B along the smallest distance of separation. + /// The length of this vector indicates the separation of A and B without their convex radius. + /// If it is near zero, the direction may not be accurate as the bodies may overlap when lambda = 0. + /// + /// @return true if a hit was found, ioLambda is the solution for lambda and outPoint and outSeparatingAxis are valid. + template + bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis) + { + float tolerance_sq = Square(inTolerance); + + // Calculate how close A and B (without their convex radius) need to be to each other in order for us to consider this a collision + float sum_convex_radius = inConvexRadiusA + inConvexRadiusB; + + // Transform the shape to be cast to the starting position + TransformedConvexObject transformed_a(inStart, inA); + + // Reset state + mNumPoints = 0; + + float lambda = 0.0f; + Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero + Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero + float v_len_sq = FLT_MAX; + bool allow_restart = false; + + // Keeps track of separating axis of the previous iteration. + // Initialized at zero as we don't know if our first v is actually a separating axis. + Vec3 prev_v = Vec3::sZero(); + + for (;;) + { +#ifdef JPH_GJK_DEBUG + Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints); +#endif + + // Calculate the minkowski difference inB - inA + // inA is moving, so we need to add the back side of inB to the front side of inA + // Keep the support points on A and B separate so that in the end we can calculate a contact point + Vec3 p = transformed_a.GetSupport(-v); + Vec3 q = inB.GetSupport(v); + Vec3 w = x - (q - p); + +#ifdef JPH_GJK_DEBUG + Trace("w = [%s]", ConvertToString(w).c_str()); +#endif + + // Difference from article to this code: + // We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm. + // However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii. + // From p we have to subtract: inConvexRadiusA * v / |v| + // To q we have to add: inConvexRadiusB * v / |v| + // This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v| + // So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v| + float v_dot_w = v.Dot(w) - sum_convex_radius * v.Length(); +#ifdef JPH_GJK_DEBUG + Trace("v . w = %g", (double)v_dot_w); +#endif + if (v_dot_w > 0.0f) + { + // If ray and normal are in the same direction, we've passed A and there's no collision + float v_dot_r = v.Dot(inDirection); +#ifdef JPH_GJK_DEBUG + Trace("v . r = %g", (double)v_dot_r); +#endif + if (v_dot_r >= 0.0f) + return false; + + // Update the lower bound for lambda + float delta = v_dot_w / v_dot_r; + float old_lambda = lambda; + lambda -= delta; +#ifdef JPH_GJK_DEBUG + Trace("lambda = %g, delta = %g", (double)lambda, (double)delta); +#endif + + // If lambda didn't change, we cannot converge any further and we assume a hit + if (old_lambda == lambda) + break; + + // If lambda is bigger or equal than max, we don't have a hit + if (lambda >= ioLambda) + return false; + + // Update x to new closest point on the ray + x = lambda * inDirection; + + // We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false + v_len_sq = FLT_MAX; + + // Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating + // as soon as A and B are inConvexRadiusA + inConvexRadiusB apart + tolerance_sq = Square(inTolerance + sum_convex_radius); + + // We allow rebuilding the simplex once after x changes because the simplex was built + // for another x and numerical round off builds up as you keep adding points to an + // existing simplex + allow_restart = true; + } + + // Add p to set P, q to set Q: P = P U {p}, Q = Q U {q} + mP[mNumPoints] = p; + mQ[mNumPoints] = q; + ++mNumPoints; + + // Calculate Y = {x} - (Q - P) + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - (mQ[i] - mP[i]); + + // Determine the new closest point from Y to origin + uint32 set; // Set of points that form the new simplex + if (!GetClosest(v_len_sq, v, v_len_sq, set)) + { +#ifdef JPH_GJK_DEBUG + Trace("Failed to converge"); +#endif + + // Only allow 1 restart, if we still can't get a closest point + // we're so close that we return this as a hit + if (!allow_restart) + break; + + // If we fail to converge, we start again with the last point as simplex +#ifdef JPH_GJK_DEBUG + Trace("Restarting"); +#endif + allow_restart = false; + mP[0] = p; + mQ[0] = q; + mNumPoints = 1; + v = x - q; + v_len_sq = FLT_MAX; + continue; + } + else if (set == 0xf) + { +#ifdef JPH_GJK_DEBUG + Trace("Full simplex"); +#endif + + // We're inside the tetrahedron, we have a hit (verify that length of v is 0) + JPH_ASSERT(v_len_sq == 0.0f); + break; + } + + // Update the points P and Q to form the new simplex + // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration + UpdatePointSetPQ(set); + + // Check if A and B are touching according to our tolerance + if (v_len_sq <= tolerance_sq) + { +#ifdef JPH_GJK_DEBUG + Trace("Converged"); +#endif + break; + } + + // Store our v to return as separating axis + prev_v = v; + } + + // Calculate Y = {x} - (Q - P) again so we can calculate the contact points + for (int i = 0; i < mNumPoints; ++i) + mY[i] = x - (mQ[i] - mP[i]); + + // Calculate the offset we need to apply to A and B to correct for the convex radius + Vec3 normalized_v = v.NormalizedOr(Vec3::sZero()); + Vec3 convex_radius_a = inConvexRadiusA * normalized_v; + Vec3 convex_radius_b = inConvexRadiusB * normalized_v; + + // Get the contact point + // Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms. + switch (mNumPoints) + { + case 1: + outPointB = mQ[0] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : mP[0] - convex_radius_a; + break; + + case 2: + { + float bu, bv; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], bu, bv); + outPointB = bu * mQ[0] + bv * mQ[1] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] - convex_radius_a; + } + break; + + case 3: + case 4: // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration. + { + float bu, bv, bw; + ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], bu, bv, bw); + outPointB = bu * mQ[0] + bv * mQ[1] + bw * mQ[2] + convex_radius_b; + outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] + bw * mP[2] - convex_radius_a; + } + break; + } + + // Store separating axis, in case we have a convex radius we can just return v, + // otherwise v will be very small and we resort to returning previous v as an approximation. + outSeparatingAxis = sum_convex_radius > 0.0f? -v : -prev_v; + + // Store hit fraction + ioLambda = lambda; + return true; + } + +private: +#ifdef JPH_GJK_DEBUG + /// Draw state of algorithm + void DrawState() + { + RMat44 origin = RMat44::sTranslation(mOffset); + + // Draw origin + DebugRenderer::sInstance->DrawCoordinateSystem(origin, 1.0f); + + // Draw the hull + DebugRenderer::sInstance->DrawGeometry(origin, mGeometry->mBounds.Transformed(origin), mGeometry->mBounds.GetExtent().LengthSq(), Color::sYellow, mGeometry); + + // Draw Y + for (int i = 0; i < mNumPoints; ++i) + { + // Draw support point + RVec3 y_i = origin * mY[i]; + DebugRenderer::sInstance->DrawMarker(y_i, Color::sRed, 1.0f); + for (int j = i + 1; j < mNumPoints; ++j) + { + // Draw edge + RVec3 y_j = origin * mY[j]; + DebugRenderer::sInstance->DrawLine(y_i, y_j, Color::sRed); + for (int k = j + 1; k < mNumPoints; ++k) + { + // Make sure triangle faces the origin + RVec3 y_k = origin * mY[k]; + RVec3 center = (y_i + y_j + y_k) / Real(3); + RVec3 normal = (y_j - y_i).Cross(y_k - y_i); + if (normal.Dot(center) < Real(0)) + DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey); + else + DebugRenderer::sInstance->DrawTriangle(y_i, y_k, y_j, Color::sLightGrey); + } + } + } + + // Offset to the right + mOffset += Vec3(mGeometry->mBounds.GetSize().GetX() + 2.0f, 0, 0); + } +#endif // JPH_GJK_DEBUG + + Vec3 mY[4]; ///< Support points on A - B + Vec3 mP[4]; ///< Support point on A + Vec3 mQ[4]; ///< Support point on B + int mNumPoints = 0; ///< Number of points in mY, mP and mQ that are valid + +#ifdef JPH_GJK_DEBUG + DebugRenderer::GeometryRef mGeometry; ///< A visualization of the minkowski difference for state drawing + RVec3 mOffset = RVec3::sZero(); ///< Offset to use for state drawing +#endif +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/IndexedTriangle.h b/WickedEngine/Jolt/Geometry/IndexedTriangle.h new file mode 100644 index 000000000..0275fccad --- /dev/null +++ b/WickedEngine/Jolt/Geometry/IndexedTriangle.h @@ -0,0 +1,115 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Triangle with 32-bit indices +class IndexedTriangleNoMaterial +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + IndexedTriangleNoMaterial() = default; + constexpr IndexedTriangleNoMaterial(uint32 inI1, uint32 inI2, uint32 inI3) : mIdx { inI1, inI2, inI3 } { } + + /// Check if two triangles are identical + bool operator == (const IndexedTriangleNoMaterial &inRHS) const + { + return mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2]; + } + + /// Check if two triangles are equivalent (using the same vertices) + bool IsEquivalent(const IndexedTriangleNoMaterial &inRHS) const + { + return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[2]) + || (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[0]) + || (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[1]); + } + + /// Check if two triangles are opposite (using the same vertices but in opposing order) + bool IsOpposite(const IndexedTriangleNoMaterial &inRHS) const + { + return (mIdx[0] == inRHS.mIdx[0] && mIdx[1] == inRHS.mIdx[2] && mIdx[2] == inRHS.mIdx[1]) + || (mIdx[0] == inRHS.mIdx[1] && mIdx[1] == inRHS.mIdx[0] && mIdx[2] == inRHS.mIdx[2]) + || (mIdx[0] == inRHS.mIdx[2] && mIdx[1] == inRHS.mIdx[1] && mIdx[2] == inRHS.mIdx[0]); + } + + /// Check if triangle is degenerate + bool IsDegenerate(const VertexList &inVertices) const + { + Vec3 v0(inVertices[mIdx[0]]); + Vec3 v1(inVertices[mIdx[1]]); + Vec3 v2(inVertices[mIdx[2]]); + + return (v1 - v0).Cross(v2 - v0).IsNearZero(); + } + + /// Rotate the vertices so that the second vertex becomes first etc. This does not change the represented triangle. + void Rotate() + { + uint32 tmp = mIdx[0]; + mIdx[0] = mIdx[1]; + mIdx[1] = mIdx[2]; + mIdx[2] = tmp; + } + + /// Get center of triangle + Vec3 GetCentroid(const VertexList &inVertices) const + { + return (Vec3(inVertices[mIdx[0]]) + Vec3(inVertices[mIdx[1]]) + Vec3(inVertices[mIdx[2]])) / 3.0f; + } + + uint32 mIdx[3]; +}; + +/// Triangle with 32-bit indices and material index +class IndexedTriangle : public IndexedTriangleNoMaterial +{ +public: + using IndexedTriangleNoMaterial::IndexedTriangleNoMaterial; + + /// Constructor + constexpr IndexedTriangle(uint32 inI1, uint32 inI2, uint32 inI3, uint32 inMaterialIndex) : IndexedTriangleNoMaterial(inI1, inI2, inI3), mMaterialIndex(inMaterialIndex) { } + + /// Check if two triangles are identical + bool operator == (const IndexedTriangle &inRHS) const + { + return mMaterialIndex == inRHS.mMaterialIndex && IndexedTriangleNoMaterial::operator==(inRHS); + } + + /// Rotate the vertices so that the lowest vertex becomes the first. This does not change the represented triangle. + IndexedTriangle GetLowestIndexFirst() const + { + if (mIdx[0] < mIdx[1]) + { + if (mIdx[0] < mIdx[2]) + return IndexedTriangle(mIdx[0], mIdx[1], mIdx[2], mMaterialIndex); // 0 is smallest + else + return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex); // 2 is smallest + } + else + { + if (mIdx[1] < mIdx[2]) + return IndexedTriangle(mIdx[1], mIdx[2], mIdx[0], mMaterialIndex); // 1 is smallest + else + return IndexedTriangle(mIdx[2], mIdx[0], mIdx[1], mMaterialIndex); // 2 is smallest + } + } + + uint32 mMaterialIndex = 0; +}; + +using IndexedTriangleNoMaterialList = Array; +using IndexedTriangleList = Array; + +JPH_NAMESPACE_END + +// Create a std::hash for IndexedTriangleNoMaterial and IndexedTriangle +JPH_MAKE_HASHABLE(JPH::IndexedTriangleNoMaterial, t.mIdx[0], t.mIdx[1], t.mIdx[2]) +JPH_MAKE_HASHABLE(JPH::IndexedTriangle, t.mIdx[0], t.mIdx[1], t.mIdx[2], t.mMaterialIndex) diff --git a/WickedEngine/Jolt/Geometry/Indexify.cpp b/WickedEngine/Jolt/Geometry/Indexify.cpp new file mode 100644 index 000000000..ac0ff90a5 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/Indexify.cpp @@ -0,0 +1,218 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +static JPH_INLINE const Float3 &sIndexifyGetFloat3(const TriangleList &inTriangles, uint32 inVertexIndex) +{ + return inTriangles[inVertexIndex / 3].mV[inVertexIndex % 3]; +} + +static JPH_INLINE Vec3 sIndexifyGetVec3(const TriangleList &inTriangles, uint32 inVertexIndex) +{ + return Vec3::sLoadFloat3Unsafe(sIndexifyGetFloat3(inTriangles, inVertexIndex)); +} + +static void sIndexifyVerticesBruteForce(const TriangleList &inTriangles, const uint32 *inVertexIndices, const uint32 *inVertexIndicesEnd, Array &ioWeldedVertices, float inVertexWeldDistance) +{ + float weld_dist_sq = Square(inVertexWeldDistance); + + // Compare every vertex + for (const uint32 *v1_idx = inVertexIndices; v1_idx < inVertexIndicesEnd; ++v1_idx) + { + Vec3 v1 = sIndexifyGetVec3(inTriangles, *v1_idx); + + // with every other vertex... + for (const uint32 *v2_idx = v1_idx + 1; v2_idx < inVertexIndicesEnd; ++v2_idx) + { + Vec3 v2 = sIndexifyGetVec3(inTriangles, *v2_idx); + + // If they're weldable + if ((v2 - v1).LengthSq() <= weld_dist_sq) + { + // Find the lowest indices both indices link to + uint32 idx1 = *v1_idx; + for (;;) + { + uint32 new_idx1 = ioWeldedVertices[idx1]; + if (new_idx1 >= idx1) + break; + idx1 = new_idx1; + } + uint32 idx2 = *v2_idx; + for (;;) + { + uint32 new_idx2 = ioWeldedVertices[idx2]; + if (new_idx2 >= idx2) + break; + idx2 = new_idx2; + } + + // Order the vertices + uint32 lowest = min(idx1, idx2); + uint32 highest = max(idx1, idx2); + + // Link highest to lowest + ioWeldedVertices[highest] = lowest; + + // Also update the vertices we started from to avoid creating long chains + ioWeldedVertices[*v1_idx] = lowest; + ioWeldedVertices[*v2_idx] = lowest; + break; + } + } + } +} + +static void sIndexifyVerticesRecursively(const TriangleList &inTriangles, uint32 *ioVertexIndices, uint inNumVertices, uint32 *ioScratch, Array &ioWeldedVertices, float inVertexWeldDistance, uint inMaxRecursion) +{ + // Check if we have few enough vertices to do a brute force search + // Or if we've recursed too deep (this means we chipped off a few vertices each iteration because all points are very close) + if (inNumVertices <= 8 || inMaxRecursion == 0) + { + sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance); + return; + } + + // Calculate bounds + AABox bounds; + for (const uint32 *v = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; v < v_end; ++v) + bounds.Encapsulate(sIndexifyGetVec3(inTriangles, *v)); + + // Determine split plane + int split_axis = bounds.GetExtent().GetHighestComponentIndex(); + float split_value = bounds.GetCenter()[split_axis]; + + // Partition vertices + uint32 *v_read = ioVertexIndices, *v_write = ioVertexIndices, *v_end = ioVertexIndices + inNumVertices; + uint32 *scratch = ioScratch; + while (v_read < v_end) + { + // Calculate distance to plane + float distance_to_split_plane = sIndexifyGetFloat3(inTriangles, *v_read)[split_axis] - split_value; + if (distance_to_split_plane < -inVertexWeldDistance) + { + // Vertex is on the right side + *v_write = *v_read; + ++v_read; + ++v_write; + } + else if (distance_to_split_plane > inVertexWeldDistance) + { + // Vertex is on the wrong side, swap with the last vertex + --v_end; + swap(*v_read, *v_end); + } + else + { + // Vertex is too close to the split plane, it goes on both sides + *scratch++ = *v_read++; + } + } + + // Check if we made any progress + uint num_vertices_on_both_sides = (uint)(scratch - ioScratch); + if (num_vertices_on_both_sides == inNumVertices) + { + sIndexifyVerticesBruteForce(inTriangles, ioVertexIndices, ioVertexIndices + inNumVertices, ioWeldedVertices, inVertexWeldDistance); + return; + } + + // Calculate how we classified the vertices + uint num_vertices_left = (uint)(v_write - ioVertexIndices); + uint num_vertices_right = (uint)(ioVertexIndices + inNumVertices - v_end); + JPH_ASSERT(num_vertices_left + num_vertices_right + num_vertices_on_both_sides == inNumVertices); + memcpy(v_write, ioScratch, num_vertices_on_both_sides * sizeof(uint32)); + + // Recurse + uint max_recursion = inMaxRecursion - 1; + sIndexifyVerticesRecursively(inTriangles, ioVertexIndices, num_vertices_left + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion); + sIndexifyVerticesRecursively(inTriangles, ioVertexIndices + num_vertices_left, num_vertices_right + num_vertices_on_both_sides, ioScratch, ioWeldedVertices, inVertexWeldDistance, max_recursion); +} + +void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance) +{ + uint num_triangles = (uint)inTriangles.size(); + uint num_vertices = num_triangles * 3; + + // Create a list of all vertex indices + Array vertex_indices; + vertex_indices.resize(num_vertices); + for (uint i = 0; i < num_vertices; ++i) + vertex_indices[i] = i; + + // Link each vertex to itself + Array welded_vertices; + welded_vertices.resize(num_vertices); + for (uint i = 0; i < num_vertices; ++i) + welded_vertices[i] = i; + + // A scope to free memory used by the scratch array + { + // Some scratch memory, used for the vertices that fall in both partitions + Array scratch; + scratch.resize(num_vertices); + + // Recursively split the vertices + sIndexifyVerticesRecursively(inTriangles, vertex_indices.data(), num_vertices, scratch.data(), welded_vertices, inVertexWeldDistance, 32); + } + + // Do a pass to complete the welding, linking each vertex to the vertex it is welded to + // (and since we're going from 0 to N we can be sure that the vertex we're linking to is already linked to the lowest vertex) + uint num_resulting_vertices = 0; + for (uint i = 0; i < num_vertices; ++i) + { + JPH_ASSERT(welded_vertices[welded_vertices[i]] <= welded_vertices[i]); + welded_vertices[i] = welded_vertices[welded_vertices[i]]; + if (welded_vertices[i] == i) + ++num_resulting_vertices; + } + + // Collect the vertices + outVertices.clear(); + outVertices.reserve(num_resulting_vertices); + for (uint i = 0; i < num_vertices; ++i) + if (welded_vertices[i] == i) + { + // New vertex + welded_vertices[i] = (uint32)outVertices.size(); + outVertices.push_back(sIndexifyGetFloat3(inTriangles, i)); + } + else + { + // Reused vertex, remap index + welded_vertices[i] = welded_vertices[welded_vertices[i]]; + } + + // Create indexed triangles + outTriangles.clear(); + outTriangles.reserve(num_triangles); + for (uint t = 0; t < num_triangles; ++t) + { + IndexedTriangle it; + it.mMaterialIndex = inTriangles[t].mMaterialIndex; + for (int v = 0; v < 3; ++v) + it.mIdx[v] = welded_vertices[t * 3 + v]; + if (!it.IsDegenerate(outVertices)) + outTriangles.push_back(it); + } +} + +void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles) +{ + outTriangles.resize(inTriangles.size()); + for (size_t t = 0; t < inTriangles.size(); ++t) + { + outTriangles[t].mMaterialIndex = inTriangles[t].mMaterialIndex; + for (int v = 0; v < 3; ++v) + outTriangles[t].mV[v] = inVertices[inTriangles[t].mIdx[v]]; + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/Indexify.h b/WickedEngine/Jolt/Geometry/Indexify.h new file mode 100644 index 000000000..01fb80530 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/Indexify.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Take a list of triangles and get the unique set of vertices and use them to create indexed triangles. +/// Vertices that are less than inVertexWeldDistance apart will be combined to a single vertex. +JPH_EXPORT void Indexify(const TriangleList &inTriangles, VertexList &outVertices, IndexedTriangleList &outTriangles, float inVertexWeldDistance = 1.0e-4f); + +/// Take a list of indexed triangles and unpack them +JPH_EXPORT void Deindexify(const VertexList &inVertices, const IndexedTriangleList &inTriangles, TriangleList &outTriangles); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/MortonCode.h b/WickedEngine/Jolt/Geometry/MortonCode.h new file mode 100644 index 000000000..e750d7e6d --- /dev/null +++ b/WickedEngine/Jolt/Geometry/MortonCode.h @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class MortonCode +{ +public: + /// First converts a floating point value in the range [0, 1] to a 10 bit fixed point integer. + /// Then expands a 10-bit integer into 30 bits by inserting 2 zeros after each bit. + static uint32 sExpandBits(float inV) + { + JPH_ASSERT(inV >= 0.0f && inV <= 1.0f); + uint32 v = uint32(inV * 1023.0f + 0.5f); + JPH_ASSERT(v < 1024); + v = (v * 0x00010001u) & 0xFF0000FFu; + v = (v * 0x00000101u) & 0x0F00F00Fu; + v = (v * 0x00000011u) & 0xC30C30C3u; + v = (v * 0x00000005u) & 0x49249249u; + return v; + } + + /// Calculate the morton code for inVector, given that all vectors lie in inVectorBounds + static uint32 sGetMortonCode(Vec3Arg inVector, const AABox &inVectorBounds) + { + // Convert to 10 bit fixed point + Vec3 scaled = (inVector - inVectorBounds.mMin) / inVectorBounds.GetSize(); + uint x = sExpandBits(scaled.GetX()); + uint y = sExpandBits(scaled.GetY()); + uint z = sExpandBits(scaled.GetZ()); + return (x << 2) + (y << 1) + z; + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/OrientedBox.cpp b/WickedEngine/Jolt/Geometry/OrientedBox.cpp new file mode 100644 index 000000000..31c38c877 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/OrientedBox.cpp @@ -0,0 +1,178 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +bool OrientedBox::Overlaps(const AABox &inBox, float inEpsilon) const +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box) + + // Convert AABox to center / extent representation + Vec3 a_center = inBox.GetCenter(); + Vec3 a_half_extents = inBox.GetExtent(); + + // Compute rotation matrix expressing b in a's coordinate frame + Mat44 rot(mOrientation.GetColumn4(0), mOrientation.GetColumn4(1), mOrientation.GetColumn4(2), mOrientation.GetColumn4(3) - Vec4(a_center, 0)); + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon }; + + // Test axes L = A0, L = A1, L = A2 + float ra, rb; + for (int i = 0; i < 3; i++) + { + ra = a_half_extents[i]; + rb = mHalfExtents[0] * abs_r[0][i] + mHalfExtents[1] * abs_r[1][i] + mHalfExtents[2] * abs_r[2][i]; + if (abs(rot(i, 3)) > ra + rb) return false; + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = a_half_extents.Dot(abs_r[i]); + rb = mHalfExtents[i]; + if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false; + } + + // Test axis L = A0 x B0 + ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1]; + rb = mHalfExtents[1] * abs_r[2][0] + mHalfExtents[2] * abs_r[1][0]; + if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false; + + // Test axis L = A0 x B1 + ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1]; + rb = mHalfExtents[0] * abs_r[2][0] + mHalfExtents[2] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false; + + // Test axis L = A0 x B2 + ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1]; + rb = mHalfExtents[0] * abs_r[1][0] + mHalfExtents[1] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false; + + // Test axis L = A1 x B0 + ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0]; + rb = mHalfExtents[1] * abs_r[2][1] + mHalfExtents[2] * abs_r[1][1]; + if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false; + + // Test axis L = A1 x B1 + ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0]; + rb = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[2] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false; + + // Test axis L = A1 x B2 + ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0]; + rb = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false; + + // Test axis L = A2 x B0 + ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0]; + rb = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[1][2]; + if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false; + + // Test axis L = A2 x B1 + ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0]; + rb = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false; + + // Test axis L = A2 x B2 + ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0]; + rb = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[1] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false; + + // Since no separating axis is found, the OBB and AAB must be intersecting + return true; +} + +bool OrientedBox::Overlaps(const OrientedBox &inBox, float inEpsilon) const +{ + // Taken from: Real Time Collision Detection - Christer Ericson + // Chapter 4.4.1, page 103-105. + // Note that A is this, B is inBox + + // Compute rotation matrix expressing b in a's coordinate frame + Mat44 rot = mOrientation.InversedRotationTranslation() * inBox.mOrientation; + + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null (see text for details) + Vec3 epsilon = Vec3::sReplicate(inEpsilon); + Vec3 abs_r[3] { rot.GetAxisX().Abs() + epsilon, rot.GetAxisY().Abs() + epsilon, rot.GetAxisZ().Abs() + epsilon }; + + // Test axes L = A0, L = A1, L = A2 + float ra, rb; + for (int i = 0; i < 3; i++) + { + ra = mHalfExtents[i]; + rb = inBox.mHalfExtents[0] * abs_r[0][i] + inBox.mHalfExtents[1] * abs_r[1][i] + inBox.mHalfExtents[2] * abs_r[2][i]; + if (abs(rot(i, 3)) > ra + rb) return false; + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) + { + ra = mHalfExtents.Dot(abs_r[i]); + rb = inBox.mHalfExtents[i]; + if (abs(rot.GetTranslation().Dot(rot.GetColumn3(i))) > ra + rb) return false; + } + + // Test axis L = A0 x B0 + ra = mHalfExtents[1] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][1]; + rb = inBox.mHalfExtents[1] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[1][0]; + if (abs(rot(2, 3) * rot(1, 0) - rot(1, 3) * rot(2, 0)) > ra + rb) return false; + + // Test axis L = A0 x B1 + ra = mHalfExtents[1] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][1]; + rb = inBox.mHalfExtents[0] * abs_r[2][0] + inBox.mHalfExtents[2] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 1) - rot(1, 3) * rot(2, 1)) > ra + rb) return false; + + // Test axis L = A0 x B2 + ra = mHalfExtents[1] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][1]; + rb = inBox.mHalfExtents[0] * abs_r[1][0] + inBox.mHalfExtents[1] * abs_r[0][0]; + if (abs(rot(2, 3) * rot(1, 2) - rot(1, 3) * rot(2, 2)) > ra + rb) return false; + + // Test axis L = A1 x B0 + ra = mHalfExtents[0] * abs_r[0][2] + mHalfExtents[2] * abs_r[0][0]; + rb = inBox.mHalfExtents[1] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[1][1]; + if (abs(rot(0, 3) * rot(2, 0) - rot(2, 3) * rot(0, 0)) > ra + rb) return false; + + // Test axis L = A1 x B1 + ra = mHalfExtents[0] * abs_r[1][2] + mHalfExtents[2] * abs_r[1][0]; + rb = inBox.mHalfExtents[0] * abs_r[2][1] + inBox.mHalfExtents[2] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 1) - rot(2, 3) * rot(0, 1)) > ra + rb) return false; + + // Test axis L = A1 x B2 + ra = mHalfExtents[0] * abs_r[2][2] + mHalfExtents[2] * abs_r[2][0]; + rb = inBox.mHalfExtents[0] * abs_r[1][1] + inBox.mHalfExtents[1] * abs_r[0][1]; + if (abs(rot(0, 3) * rot(2, 2) - rot(2, 3) * rot(0, 2)) > ra + rb) return false; + + // Test axis L = A2 x B0 + ra = mHalfExtents[0] * abs_r[0][1] + mHalfExtents[1] * abs_r[0][0]; + rb = inBox.mHalfExtents[1] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[1][2]; + if (abs(rot(1, 3) * rot(0, 0) - rot(0, 3) * rot(1, 0)) > ra + rb) return false; + + // Test axis L = A2 x B1 + ra = mHalfExtents[0] * abs_r[1][1] + mHalfExtents[1] * abs_r[1][0]; + rb = inBox.mHalfExtents[0] * abs_r[2][2] + inBox.mHalfExtents[2] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 1) - rot(0, 3) * rot(1, 1)) > ra + rb) return false; + + // Test axis L = A2 x B2 + ra = mHalfExtents[0] * abs_r[2][1] + mHalfExtents[1] * abs_r[2][0]; + rb = inBox.mHalfExtents[0] * abs_r[1][2] + inBox.mHalfExtents[1] * abs_r[0][2]; + if (abs(rot(1, 3) * rot(0, 2) - rot(0, 3) * rot(1, 2)) > ra + rb) return false; + + // Since no separating axis is found, the OBBs must be intersecting + return true; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/OrientedBox.h b/WickedEngine/Jolt/Geometry/OrientedBox.h new file mode 100644 index 000000000..c5c2a0e16 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/OrientedBox.h @@ -0,0 +1,39 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class AABox; + +/// Oriented box +class [[nodiscard]] JPH_EXPORT_GCC_BUG_WORKAROUND OrientedBox +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + OrientedBox() = default; + OrientedBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents) : mOrientation(inOrientation), mHalfExtents(inHalfExtents) { } + + /// Construct from axis aligned box and transform. Only works for rotation/translation matrix (no scaling / shearing). + OrientedBox(Mat44Arg inOrientation, const AABox &inBox) : OrientedBox(inOrientation.PreTranslated(inBox.GetCenter()), inBox.GetExtent()) { } + + /// Test if oriented box overlaps with axis aligned box each other + bool Overlaps(const AABox &inBox, float inEpsilon = 1.0e-6f) const; + + /// Test if two oriented boxes overlap each other + bool Overlaps(const OrientedBox &inBox, float inEpsilon = 1.0e-6f) const; + + Mat44 mOrientation; ///< Transform that positions and rotates the local space axis aligned box into world space + Vec3 mHalfExtents; ///< Half extents (half the size of the edge) of the local space axis aligned box +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/Plane.h b/WickedEngine/Jolt/Geometry/Plane.h new file mode 100644 index 000000000..c969dbd6f --- /dev/null +++ b/WickedEngine/Jolt/Geometry/Plane.h @@ -0,0 +1,86 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// An infinite plane described by the formula X . Normal + Constant = 0. +class [[nodiscard]] Plane +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Plane() = default; + explicit Plane(Vec4Arg inNormalAndConstant) : mNormalAndConstant(inNormalAndConstant) { } + Plane(Vec3Arg inNormal, float inConstant) : mNormalAndConstant(inNormal, inConstant) { } + + /// Create from point and normal + static Plane sFromPointAndNormal(Vec3Arg inPoint, Vec3Arg inNormal) { return Plane(Vec4(inNormal, -inNormal.Dot(inPoint))); } + + /// Create from point and normal, double precision version that more accurately calculates the plane constant + static Plane sFromPointAndNormal(DVec3Arg inPoint, Vec3Arg inNormal) { return Plane(Vec4(inNormal, -float(DVec3(inNormal).Dot(inPoint)))); } + + /// Create from 3 counter clockwise points + static Plane sFromPointsCCW(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) { return sFromPointAndNormal(inV1, (inV2 - inV1).Cross(inV3 - inV1).Normalized()); } + + // Properties + Vec3 GetNormal() const { return Vec3(mNormalAndConstant); } + void SetNormal(Vec3Arg inNormal) { mNormalAndConstant = Vec4(inNormal, mNormalAndConstant.GetW()); } + float GetConstant() const { return mNormalAndConstant.GetW(); } + void SetConstant(float inConstant) { mNormalAndConstant.SetW(inConstant); } + + /// Offset the plane (positive value means move it in the direction of the plane normal) + Plane Offset(float inDistance) const { return Plane(mNormalAndConstant - Vec4(Vec3::sZero(), inDistance)); } + + /// Transform the plane by a matrix + inline Plane GetTransformed(Mat44Arg inTransform) const + { + Vec3 transformed_normal = inTransform.Multiply3x3(GetNormal()); + return Plane(transformed_normal, GetConstant() - inTransform.GetTranslation().Dot(transformed_normal)); + } + + /// Distance point to plane + float SignedDistance(Vec3Arg inPoint) const { return inPoint.Dot(GetNormal()) + GetConstant(); } + + /// Returns intersection point between 3 planes + static bool sIntersectPlanes(const Plane &inP1, const Plane &inP2, const Plane &inP3, Vec3 &outPoint) + { + // We solve the equation: + // |ax, ay, az, aw| | x | | 0 | + // |bx, by, bz, bw| * | y | = | 0 | + // |cx, cy, cz, cw| | z | | 0 | + // | 0, 0, 0, 1| | 1 | | 1 | + // Where normal of plane 1 = (ax, ay, az), plane constant of 1 = aw, normal of plane 2 = (bx, by, bz) etc. + // This involves inverting the matrix and multiplying it with [0, 0, 0, 1] + + // Fetch the normals and plane constants for the three planes + Vec4 a = inP1.mNormalAndConstant; + Vec4 b = inP2.mNormalAndConstant; + Vec4 c = inP3.mNormalAndConstant; + + // Result is a vector that we have to divide by: + float denominator = Vec3(a).Dot(Vec3(b).Cross(Vec3(c))); + if (denominator == 0.0f) + return false; + + // The numerator is: + // [aw*(bz*cy-by*cz)+ay*(bw*cz-bz*cw)+az*(by*cw-bw*cy)] + // [aw*(bx*cz-bz*cx)+ax*(bz*cw-bw*cz)+az*(bw*cx-bx*cw)] + // [aw*(by*cx-bx*cy)+ax*(bw*cy-by*cw)+ay*(bx*cw-bw*cx)] + Vec4 numerator = + a.SplatW() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()) + + a.Swizzle() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()) + + a.Swizzle() * (b.Swizzle() * c.Swizzle() - b.Swizzle() * c.Swizzle()); + + outPoint = Vec3(numerator) / denominator; + return true; + } + +private: + Vec4 mNormalAndConstant; ///< XYZ = normal, W = constant, plane: x . normal + constant = 0 +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/RayAABox.h b/WickedEngine/Jolt/Geometry/RayAABox.h new file mode 100644 index 000000000..4506fadbb --- /dev/null +++ b/WickedEngine/Jolt/Geometry/RayAABox.h @@ -0,0 +1,241 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Helper structure holding the reciprocal of a ray for Ray vs AABox testing +class RayInvDirection +{ +public: + /// Constructors + inline RayInvDirection() = default; + inline explicit RayInvDirection(Vec3Arg inDirection) { Set(inDirection); } + + /// Set reciprocal from ray direction + inline void Set(Vec3Arg inDirection) + { + // if (abs(inDirection) <= Epsilon) the ray is nearly parallel to the slab. + mIsParallel = Vec3::sLessOrEqual(inDirection.Abs(), Vec3::sReplicate(1.0e-20f)); + + // Calculate 1 / direction while avoiding division by zero + mInvDirection = Vec3::sSelect(inDirection, Vec3::sReplicate(1.0f), mIsParallel).Reciprocal(); + } + + Vec3 mInvDirection; ///< 1 / ray direction + UVec4 mIsParallel; ///< for each component if it is parallel to the coordinate axis +}; + +/// Intersect AABB with ray, returns minimal distance along ray or FLT_MAX if no hit +/// Note: Can return negative value if ray starts in box +JPH_INLINE float RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY()); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ()); + return Vec3::sSelect(t_min, flt_max, no_intersection).GetX(); +} + +/// Intersect 4 AABBs with ray, returns minimal distance along ray or FLT_MAX if no hit +/// Note: Can return negative value if ray starts in box +JPH_INLINE Vec4 RayAABox4(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) +{ + // Constants + Vec4 flt_min = Vec4::sReplicate(-FLT_MAX); + Vec4 flt_max = Vec4::sReplicate(FLT_MAX); + + // Origin + Vec4 originx = inOrigin.SplatX(); + Vec4 originy = inOrigin.SplatY(); + Vec4 originz = inOrigin.SplatZ(); + + // Parallel + UVec4 parallelx = inInvDirection.mIsParallel.SplatX(); + UVec4 parallely = inInvDirection.mIsParallel.SplatY(); + UVec4 parallelz = inInvDirection.mIsParallel.SplatZ(); + + // Inverse direction + Vec4 invdirx = inInvDirection.mInvDirection.SplatX(); + Vec4 invdiry = inInvDirection.mInvDirection.SplatY(); + Vec4 invdirz = inInvDirection.mInvDirection.SplatZ(); + + // Test against all three axii simultaneously. + Vec4 t1x = (inBoundsMinX - originx) * invdirx; + Vec4 t1y = (inBoundsMinY - originy) * invdiry; + Vec4 t1z = (inBoundsMinZ - originz) * invdirz; + Vec4 t2x = (inBoundsMaxX - originx) * invdirx; + Vec4 t2y = (inBoundsMaxY - originy) * invdiry; + Vec4 t2z = (inBoundsMaxZ - originz) * invdirz; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec4 t_minx = Vec4::sSelect(Vec4::sMin(t1x, t2x), flt_min, parallelx); + Vec4 t_miny = Vec4::sSelect(Vec4::sMin(t1y, t2y), flt_min, parallely); + Vec4 t_minz = Vec4::sSelect(Vec4::sMin(t1z, t2z), flt_min, parallelz); + Vec4 t_maxx = Vec4::sSelect(Vec4::sMax(t1x, t2x), flt_max, parallelx); + Vec4 t_maxy = Vec4::sSelect(Vec4::sMax(t1y, t2y), flt_max, parallely); + Vec4 t_maxz = Vec4::sSelect(Vec4::sMax(t1z, t2z), flt_max, parallelz); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + Vec4 t_min = Vec4::sMax(Vec4::sMax(t_minx, t_miny), t_minz); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + Vec4 t_max = Vec4::sMin(Vec4::sMin(t_maxx, t_maxy), t_maxz); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec4::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec4::sLess(t_max, Vec4::sZero())); + + // if bounds are invalid return FLOAT_MAX; + UVec4 bounds_invalid = UVec4::sOr(UVec4::sOr(Vec4::sGreater(inBoundsMinX, inBoundsMaxX), Vec4::sGreater(inBoundsMinY, inBoundsMaxY)), Vec4::sGreater(inBoundsMinZ, inBoundsMaxZ)); + no_intersection = UVec4::sOr(no_intersection, bounds_invalid); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlapx = UVec4::sAnd(parallelx, UVec4::sOr(Vec4::sLess(originx, inBoundsMinX), Vec4::sGreater(originx, inBoundsMaxX))); + UVec4 no_parallel_overlapy = UVec4::sAnd(parallely, UVec4::sOr(Vec4::sLess(originy, inBoundsMinY), Vec4::sGreater(originy, inBoundsMaxY))); + UVec4 no_parallel_overlapz = UVec4::sAnd(parallelz, UVec4::sOr(Vec4::sLess(originz, inBoundsMinZ), Vec4::sGreater(originz, inBoundsMaxZ))); + no_intersection = UVec4::sOr(no_intersection, UVec4::sOr(UVec4::sOr(no_parallel_overlapx, no_parallel_overlapy), no_parallel_overlapz)); + return Vec4::sSelect(t_min, flt_max, no_intersection); +} + +/// Intersect AABB with ray, returns minimal and maximal distance along ray or FLT_MAX, -FLT_MAX if no hit +/// Note: Can return negative value for outMin if ray starts in box +JPH_INLINE void RayAABox(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float &outMin, float &outMax) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return FLT_MAX; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatY()); + no_intersection = UVec4::sOr(no_intersection, no_intersection.SplatZ()); + outMin = Vec3::sSelect(t_min, flt_max, no_intersection).GetX(); + outMax = Vec3::sSelect(t_max, flt_min, no_intersection).GetX(); +} + +/// Intersect AABB with ray, returns true if there is a hit closer than inClosest +JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax, float inClosest) +{ + // Constants + Vec3 flt_min = Vec3::sReplicate(-FLT_MAX); + Vec3 flt_max = Vec3::sReplicate(FLT_MAX); + + // Test against all three axii simultaneously. + Vec3 t1 = (inBoundsMin - inOrigin) * inInvDirection.mInvDirection; + Vec3 t2 = (inBoundsMax - inOrigin) * inInvDirection.mInvDirection; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec3 t_min = Vec3::sSelect(Vec3::sMin(t1, t2), flt_min, inInvDirection.mIsParallel); + Vec3 t_max = Vec3::sSelect(Vec3::sMax(t1, t2), flt_max, inInvDirection.mIsParallel); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + t_min = Vec3::sMax(t_min, t_min.Swizzle()); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + t_max = Vec3::sMin(t_max, t_max.Swizzle()); + + // if (t_min > t_max) return false; + UVec4 no_intersection = Vec3::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return false; + no_intersection = UVec4::sOr(no_intersection, Vec3::sLess(t_max, Vec3::sZero())); + + // if (t_min > inClosest) return false; + no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater(t_min, Vec3::sReplicate(inClosest))); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return false; else return true; + UVec4 no_parallel_overlap = UVec4::sOr(Vec3::sLess(inOrigin, inBoundsMin), Vec3::sGreater(inOrigin, inBoundsMax)); + no_intersection = UVec4::sOr(no_intersection, UVec4::sAnd(inInvDirection.mIsParallel, no_parallel_overlap)); + + return !no_intersection.TestAnyXYZTrue(); +} + +/// Intersect AABB with ray without hit fraction, based on separating axis test +/// @see http://www.codercorner.com/RayAABB.cpp +JPH_INLINE bool RayAABoxHits(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inBoundsMin, Vec3Arg inBoundsMax) +{ + Vec3 extents = inBoundsMax - inBoundsMin; + + Vec3 diff = 2.0f * inOrigin - inBoundsMin - inBoundsMax; + Vec3 abs_diff = diff.Abs(); + + UVec4 no_intersection = UVec4::sAnd(Vec3::sGreater(abs_diff, extents), Vec3::sGreaterOrEqual(diff * inDirection, Vec3::sZero())); + + Vec3 abs_dir = inDirection.Abs(); + Vec3 abs_dir_yzz = abs_dir.Swizzle(); + Vec3 abs_dir_xyx = abs_dir.Swizzle(); + + Vec3 extents_yzz = extents.Swizzle(); + Vec3 extents_xyx = extents.Swizzle(); + + Vec3 diff_yzx = diff.Swizzle(); + + Vec3 dir_yzx = inDirection.Swizzle(); + + no_intersection = UVec4::sOr(no_intersection, Vec3::sGreater((inDirection * diff_yzx - dir_yzx * diff).Abs(), extents_xyx * abs_dir_yzz + extents_yzz * abs_dir_xyx)); + + return !no_intersection.TestAnyXYZTrue(); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/RayAABox8.h b/WickedEngine/Jolt/Geometry/RayAABox8.h new file mode 100644 index 000000000..260e3636c --- /dev/null +++ b/WickedEngine/Jolt/Geometry/RayAABox8.h @@ -0,0 +1,76 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Intersect 8 AABBs with ray, returns minimal distance along ray or FLT_MAX if no hit +/// Note: Can return negative value if ray starts in box +JPH_INLINE Vec8 RayAABox8(Vec3Arg inOrigin, const RayInvDirection &inInvDirection, Vec8Arg inBoundsMinX, Vec8Arg inBoundsMinY, Vec8Arg inBoundsMinZ, Vec8Arg inBoundsMaxX, Vec8Arg inBoundsMaxY, Vec8Arg inBoundsMaxZ) +{ + // Constants + Vec8 flt_min = Vec8::sReplicate(-FLT_MAX); + Vec8 flt_max = Vec8::sReplicate(FLT_MAX); + + // Origin + Vec8 originx = Vec8::sSplatX(Vec4(inOrigin)); + Vec8 originy = Vec8::sSplatY(Vec4(inOrigin)); + Vec8 originz = Vec8::sSplatZ(Vec4(inOrigin)); + + // Parallel + UVec8 parallelx = UVec8::sSplatX(inInvDirection.mIsParallel); + UVec8 parallely = UVec8::sSplatY(inInvDirection.mIsParallel); + UVec8 parallelz = UVec8::sSplatZ(inInvDirection.mIsParallel); + + // Inverse direction + Vec8 invdirx = Vec8::sSplatX(Vec4(inInvDirection.mInvDirection)); + Vec8 invdiry = Vec8::sSplatY(Vec4(inInvDirection.mInvDirection)); + Vec8 invdirz = Vec8::sSplatZ(Vec4(inInvDirection.mInvDirection)); + + // Test against all three axii simultaneously. + Vec8 t1x = (inBoundsMinX - originx) * invdirx; + Vec8 t1y = (inBoundsMinY - originy) * invdiry; + Vec8 t1z = (inBoundsMinZ - originz) * invdirz; + Vec8 t2x = (inBoundsMaxX - originx) * invdirx; + Vec8 t2y = (inBoundsMaxY - originy) * invdiry; + Vec8 t2z = (inBoundsMaxZ - originz) * invdirz; + + // Compute the max of min(t1,t2) and the min of max(t1,t2) ensuring we don't + // use the results from any directions parallel to the slab. + Vec8 t_minx = Vec8::sSelect(Vec8::sMin(t1x, t2x), flt_min, parallelx); + Vec8 t_miny = Vec8::sSelect(Vec8::sMin(t1y, t2y), flt_min, parallely); + Vec8 t_minz = Vec8::sSelect(Vec8::sMin(t1z, t2z), flt_min, parallelz); + Vec8 t_maxx = Vec8::sSelect(Vec8::sMax(t1x, t2x), flt_max, parallelx); + Vec8 t_maxy = Vec8::sSelect(Vec8::sMax(t1y, t2y), flt_max, parallely); + Vec8 t_maxz = Vec8::sSelect(Vec8::sMax(t1z, t2z), flt_max, parallelz); + + // t_min.xyz = maximum(t_min.x, t_min.y, t_min.z); + Vec8 t_min = Vec8::sMax(Vec8::sMax(t_minx, t_miny), t_minz); + + // t_max.xyz = minimum(t_max.x, t_max.y, t_max.z); + Vec8 t_max = Vec8::sMin(Vec8::sMin(t_maxx, t_maxy), t_maxz); + + // if (t_min > t_max) return FLT_MAX; + UVec8 no_intersection = Vec8::sGreater(t_min, t_max); + + // if (t_max < 0.0f) return FLT_MAX; + no_intersection = UVec8::sOr(no_intersection, Vec8::sLess(t_max, Vec8::sZero())); + + // if bounds are invalid return FLOAT_MAX; + UVec8 bounds_invalid = UVec8::sOr(UVec8::sOr(Vec8::sGreater(inBoundsMinX, inBoundsMaxX), Vec8::sGreater(inBoundsMinY, inBoundsMaxY)), Vec8::sGreater(inBoundsMinZ, inBoundsMaxZ)); + no_intersection = UVec8::sOr(no_intersection, bounds_invalid); + + // if (inInvDirection.mIsParallel && !(Min <= inOrigin && inOrigin <= Max)) return FLT_MAX; else return t_min; + UVec8 no_parallel_overlapx = UVec8::sAnd(parallelx, UVec8::sOr(Vec8::sLess(originx, inBoundsMinX), Vec8::sGreater(originx, inBoundsMaxX))); + UVec8 no_parallel_overlapy = UVec8::sAnd(parallely, UVec8::sOr(Vec8::sLess(originy, inBoundsMinY), Vec8::sGreater(originy, inBoundsMaxY))); + UVec8 no_parallel_overlapz = UVec8::sAnd(parallelz, UVec8::sOr(Vec8::sLess(originz, inBoundsMinZ), Vec8::sGreater(originz, inBoundsMaxZ))); + no_intersection = UVec8::sOr(no_intersection, UVec8::sOr(UVec8::sOr(no_parallel_overlapx, no_parallel_overlapy), no_parallel_overlapz)); + return Vec8::sSelect(t_min, flt_max, no_intersection); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/RayCapsule.h b/WickedEngine/Jolt/Geometry/RayCapsule.h new file mode 100644 index 000000000..4862931b6 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/RayCapsule.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection +/// against a capsule centered around the origin with its axis along the Y axis and half height specified. +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the capsule, the returned fraction will be 0. +/// @param inCapsuleHalfHeight Distance from the origin to the center of the top sphere (or that of the bottom) +/// @param inCapsuleRadius Radius of the top/bottom sphere +JPH_INLINE float RayCapsule(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCapsuleHalfHeight, float inCapsuleRadius) +{ + // Test infinite cylinder + float cylinder = RayCylinder(inRayOrigin, inRayDirection, inCapsuleRadius); + if (cylinder == FLT_MAX) + return FLT_MAX; + + // If this hit is in the finite cylinder we have our fraction + if (abs(inRayOrigin.GetY() + cylinder * inRayDirection.GetY()) <= inCapsuleHalfHeight) + return cylinder; + + // Test upper and lower sphere + Vec3 sphere_center(0, inCapsuleHalfHeight, 0); + float upper = RaySphere(inRayOrigin, inRayDirection, sphere_center, inCapsuleRadius); + float lower = RaySphere(inRayOrigin, inRayDirection, -sphere_center, inCapsuleRadius); + return min(upper, lower); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/RayCylinder.h b/WickedEngine/Jolt/Geometry/RayCylinder.h new file mode 100644 index 000000000..cabed0680 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/RayCylinder.h @@ -0,0 +1,101 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection +/// against an infinite cylinder centered along the Y axis +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Direction of the ray. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0. +/// @param inCylinderRadius Radius of the infinite cylinder +JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderRadius) +{ + // Remove Y component of ray to see of ray intersects with infinite cylinder + UVec4 mask_y = UVec4(0, 0xffffffff, 0, 0); + Vec3 origin_xz = Vec3::sSelect(inRayOrigin, Vec3::sZero(), mask_y); + float origin_xz_len_sq = origin_xz.LengthSq(); + float r_sq = Square(inCylinderRadius); + if (origin_xz_len_sq > r_sq) + { + // Ray starts outside of the infinite cylinder + // Solve: |RayOrigin_xz + fraction * RayDirection_xz|^2 = r^2 to find fraction + Vec3 direction_xz = Vec3::sSelect(inRayDirection, Vec3::sZero(), mask_y); + float a = direction_xz.LengthSq(); + float b = 2.0f * origin_xz.Dot(direction_xz); + float c = origin_xz_len_sq - r_sq; + float fraction1, fraction2; + if (FindRoot(a, b, c, fraction1, fraction2) == 0) + return FLT_MAX; // No intersection with infinite cylinder + + // Get fraction corresponding to the ray entering the circle + float fraction = min(fraction1, fraction2); + if (fraction >= 0.0f) + return fraction; + } + else + { + // Ray starts inside the infinite cylinder + return 0.0f; + } + + // No collision + return FLT_MAX; +} + +/// Test a ray against a cylinder centered around the origin with its axis along the Y axis and half height specified. +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inRayOrigin Origin of the ray. If the ray starts inside the cylinder, the returned fraction will be 0. +/// @param inCylinderRadius Radius of the cylinder +/// @param inCylinderHalfHeight Distance from the origin to the top (or bottom) of the cylinder +JPH_INLINE float RayCylinder(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inCylinderHalfHeight, float inCylinderRadius) +{ + // Test infinite cylinder + float fraction = RayCylinder(inRayOrigin, inRayDirection, inCylinderRadius); + if (fraction == FLT_MAX) + return FLT_MAX; + + // If this hit is in the finite cylinder we have our fraction + if (abs(inRayOrigin.GetY() + fraction * inRayDirection.GetY()) <= inCylinderHalfHeight) + return fraction; + + // Check if ray could hit the top or bottom plane of the cylinder + float direction_y = inRayDirection.GetY(); + if (direction_y != 0.0f) + { + // Solving line equation: x = ray_origin + fraction * ray_direction + // and plane equation: plane_normal . x + plane_constant = 0 + // fraction = (-plane_constant - plane_normal . ray_origin) / (plane_normal . ray_direction) + // when the ray_direction.y < 0: + // plane_constant = -cylinder_half_height, plane_normal = (0, 1, 0) + // else + // plane_constant = -cylinder_half_height, plane_normal = (0, -1, 0) + float origin_y = inRayOrigin.GetY(); + float plane_fraction; + if (direction_y < 0.0f) + plane_fraction = (inCylinderHalfHeight - origin_y) / direction_y; + else + plane_fraction = -(inCylinderHalfHeight + origin_y) / direction_y; + + // Check if the hit is in front of the ray + if (plane_fraction >= 0.0f) + { + // Test if this hit is inside the cylinder + Vec3 point = inRayOrigin + plane_fraction * inRayDirection; + float dist_sq = Square(point.GetX()) + Square(point.GetZ()); + if (dist_sq <= Square(inCylinderRadius)) + return plane_fraction; + } + } + + // No collision + return FLT_MAX; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/RaySphere.h b/WickedEngine/Jolt/Geometry/RaySphere.h new file mode 100644 index 000000000..d5cfb1905 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/RaySphere.h @@ -0,0 +1,96 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere, +/// @return FLT_MAX if there is no intersection, otherwise the fraction along the ray. +/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inSphereCenter Position of the center of the sphere +/// @param inSphereRadius Radius of the sphere +JPH_INLINE float RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius) +{ + // Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction + Vec3 center_origin = inRayOrigin - inSphereCenter; + float a = inRayDirection.LengthSq(); + float b = 2.0f * inRayDirection.Dot(center_origin); + float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius; + float fraction1, fraction2; + if (FindRoot(a, b, c, fraction1, fraction2) == 0) + return c <= 0.0f? 0.0f : FLT_MAX; // Return if origin is inside the sphere + + // Sort so that the smallest is first + if (fraction1 > fraction2) + swap(fraction1, fraction2); + + // Test solution with lowest fraction, this will be the ray entering the sphere + if (fraction1 >= 0.0f) + return fraction1; // Sphere is before the ray start + + // Test solution with highest fraction, this will be the ray leaving the sphere + if (fraction2 >= 0.0f) + return 0.0f; // We start inside the sphere + + // No solution + return FLT_MAX; +} + +/// Tests a ray starting at inRayOrigin and extending infinitely in inRayDirection against a sphere. +/// Outputs entry and exit points (outMinFraction and outMaxFraction) along the ray (which could be negative if the hit point is before the start of the ray). +/// @param inRayOrigin Ray origin. If the ray starts inside the sphere, the returned fraction will be 0. +/// @param inRayDirection Ray direction. Does not need to be normalized. +/// @param inSphereCenter Position of the center of the sphere. +/// @param inSphereRadius Radius of the sphere. +/// @param outMinFraction Returned lowest intersection fraction +/// @param outMaxFraction Returned highest intersection fraction +/// @return The amount of intersections with the sphere. +/// If 1 intersection is returned outMinFraction will be equal to outMaxFraction +JPH_INLINE int RaySphere(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, Vec3Arg inSphereCenter, float inSphereRadius, float &outMinFraction, float &outMaxFraction) +{ + // Solve: |RayOrigin + fraction * RayDirection - SphereCenter|^2 = SphereRadius^2 for fraction + Vec3 center_origin = inRayOrigin - inSphereCenter; + float a = inRayDirection.LengthSq(); + float b = 2.0f * inRayDirection.Dot(center_origin); + float c = center_origin.LengthSq() - inSphereRadius * inSphereRadius; + float fraction1, fraction2; + switch (FindRoot(a, b, c, fraction1, fraction2)) + { + case 0: + if (c <= 0.0f) + { + // Origin inside sphere + outMinFraction = outMaxFraction = 0.0f; + return 1; + } + else + { + // Origin outside of the sphere + return 0; + } + break; + + case 1: + // Ray is touching the sphere + outMinFraction = outMaxFraction = fraction1; + return 1; + + default: + // Ray enters and exits the sphere + + // Sort so that the smallest is first + if (fraction1 > fraction2) + swap(fraction1, fraction2); + + outMinFraction = fraction1; + outMaxFraction = fraction2; + return 2; + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/RayTriangle.h b/WickedEngine/Jolt/Geometry/RayTriangle.h new file mode 100644 index 000000000..dabd0275c --- /dev/null +++ b/WickedEngine/Jolt/Geometry/RayTriangle.h @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Intersect ray with triangle, returns closest point or FLT_MAX if no hit (branch less version) +/// Adapted from: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm +JPH_INLINE float RayTriangle(Vec3Arg inOrigin, Vec3Arg inDirection, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) +{ + // Epsilon + Vec3 epsilon = Vec3::sReplicate(1.0e-12f); + + // Zero & one + Vec3 zero = Vec3::sZero(); + Vec3 one = Vec3::sReplicate(1.0f); + + // Find vectors for two edges sharing inV0 + Vec3 e1 = inV1 - inV0; + Vec3 e2 = inV2 - inV0; + + // Begin calculating determinant - also used to calculate u parameter + Vec3 p = inDirection.Cross(e2); + + // if determinant is near zero, ray lies in plane of triangle + Vec3 det = Vec3::sReplicate(e1.Dot(p)); + + // Check if determinant is near zero + UVec4 det_near_zero = Vec3::sLess(det.Abs(), epsilon); + + // When the determinant is near zero, set it to one to avoid dividing by zero + det = Vec3::sSelect(det, Vec3::sReplicate(1.0f), det_near_zero); + + // Calculate distance from inV0 to ray origin + Vec3 s = inOrigin - inV0; + + // Calculate u parameter + Vec3 u = Vec3::sReplicate(s.Dot(p)) / det; + + // Prepare to test v parameter + Vec3 q = s.Cross(e1); + + // Calculate v parameter + Vec3 v = Vec3::sReplicate(inDirection.Dot(q)) / det; + + // Get intersection point + Vec3 t = Vec3::sReplicate(e2.Dot(q)) / det; + + // Check if there is an intersection + UVec4 no_intersection = + UVec4::sOr + ( + UVec4::sOr + ( + UVec4::sOr + ( + det_near_zero, + Vec3::sLess(u, zero) + ), + UVec4::sOr + ( + Vec3::sLess(v, zero), + Vec3::sGreater(u + v, one) + ) + ), + Vec3::sLess(t, zero) + ); + + // Select intersection point or FLT_MAX based on if there is an intersection or not + return Vec3::sSelect(t, Vec3::sReplicate(FLT_MAX), no_intersection).GetX(); +} + +/// Intersect ray with 4 triangles in SOA format, returns 4 vector of closest points or FLT_MAX if no hit (uses bit tricks to do less divisions) +JPH_INLINE Vec4 RayTriangle4(Vec3Arg inOrigin, Vec3Arg inDirection, Vec4Arg inV0X, Vec4Arg inV0Y, Vec4Arg inV0Z, Vec4Arg inV1X, Vec4Arg inV1Y, Vec4Arg inV1Z, Vec4Arg inV2X, Vec4Arg inV2Y, Vec4Arg inV2Z) +{ + // Epsilon + Vec4 epsilon = Vec4::sReplicate(1.0e-12f); + + // Zero + Vec4 zero = Vec4::sZero(); + + // Find vectors for two edges sharing inV0 + Vec4 e1x = inV1X - inV0X; + Vec4 e1y = inV1Y - inV0Y; + Vec4 e1z = inV1Z - inV0Z; + Vec4 e2x = inV2X - inV0X; + Vec4 e2y = inV2Y - inV0Y; + Vec4 e2z = inV2Z - inV0Z; + + // Get direction vector components + Vec4 dx = inDirection.SplatX(); + Vec4 dy = inDirection.SplatY(); + Vec4 dz = inDirection.SplatZ(); + + // Begin calculating determinant - also used to calculate u parameter + Vec4 px = dy * e2z - dz * e2y; + Vec4 py = dz * e2x - dx * e2z; + Vec4 pz = dx * e2y - dy * e2x; + + // if determinant is near zero, ray lies in plane of triangle + Vec4 det = e1x * px + e1y * py + e1z * pz; + + // Get sign bit for determinant and make positive + Vec4 det_sign = Vec4::sAnd(det, UVec4::sReplicate(0x80000000).ReinterpretAsFloat()); + det = Vec4::sXor(det, det_sign); + + // Check which determinants are near zero + UVec4 det_near_zero = Vec4::sLess(det, epsilon); + + // Set components of the determinant to 1 that are near zero to avoid dividing by zero + det = Vec4::sSelect(det, Vec4::sReplicate(1.0f), det_near_zero); + + // Calculate distance from inV0 to ray origin + Vec4 sx = inOrigin.SplatX() - inV0X; + Vec4 sy = inOrigin.SplatY() - inV0Y; + Vec4 sz = inOrigin.SplatZ() - inV0Z; + + // Calculate u parameter and flip sign if determinant was negative + Vec4 u = Vec4::sXor(sx * px + sy * py + sz * pz, det_sign); + + // Prepare to test v parameter + Vec4 qx = sy * e1z - sz * e1y; + Vec4 qy = sz * e1x - sx * e1z; + Vec4 qz = sx * e1y - sy * e1x; + + // Calculate v parameter and flip sign if determinant was negative + Vec4 v = Vec4::sXor(dx * qx + dy * qy + dz * qz, det_sign); + + // Get intersection point and flip sign if determinant was negative + Vec4 t = Vec4::sXor(e2x * qx + e2y * qy + e2z * qz, det_sign); + + // Check if there is an intersection + UVec4 no_intersection = + UVec4::sOr + ( + UVec4::sOr + ( + UVec4::sOr + ( + det_near_zero, + Vec4::sLess(u, zero) + ), + UVec4::sOr + ( + Vec4::sLess(v, zero), + Vec4::sGreater(u + v, det) + ) + ), + Vec4::sLess(t, zero) + ); + + // Select intersection point or FLT_MAX based on if there is an intersection or not + return Vec4::sSelect(t / det, Vec4::sReplicate(FLT_MAX), no_intersection); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/RayTriangle8.h b/WickedEngine/Jolt/Geometry/RayTriangle8.h new file mode 100644 index 000000000..b97bca3f1 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/RayTriangle8.h @@ -0,0 +1,91 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Intersect ray with 8 triangles in SOA format, returns 8 vector of closest points or FLT_MAX if no hit +JPH_INLINE Vec8 RayTriangle8(Vec3Arg inOrigin, Vec3Arg inDirection, Vec8Arg inV0X, Vec8Arg inV0Y, Vec8Arg inV0Z, Vec8Arg inV1X, Vec8Arg inV1Y, Vec8Arg inV1Z, Vec8Arg inV2X, Vec8Arg inV2Y, Vec8Arg inV2Z) +{ + // Epsilon + Vec8 epsilon = Vec8::sReplicate(1.0e-12f); + + // Zero & one + Vec8 zero = Vec8::sZero(); + Vec8 one = Vec8::sReplicate(1.0f); + + // Find vectors for two edges sharing inV0 + Vec8 e1x = inV1X - inV0X; + Vec8 e1y = inV1Y - inV0Y; + Vec8 e1z = inV1Z - inV0Z; + Vec8 e2x = inV2X - inV0X; + Vec8 e2y = inV2Y - inV0Y; + Vec8 e2z = inV2Z - inV0Z; + + // Get direction vector components + Vec8 dx = Vec8::sSplatX(Vec4(inDirection)); + Vec8 dy = Vec8::sSplatY(Vec4(inDirection)); + Vec8 dz = Vec8::sSplatZ(Vec4(inDirection)); + + // Begin calculating determinant - also used to calculate u parameter + Vec8 px = dy * e2z - dz * e2y; + Vec8 py = dz * e2x - dx * e2z; + Vec8 pz = dx * e2y - dy * e2x; + + // if determinant is near zero, ray lies in plane of triangle + Vec8 det = e1x * px + e1y * py + e1z * pz; + + // Check which determinants are near zero + UVec8 det_near_zero = Vec8::sLess(det.Abs(), epsilon); + + // Set components of the determinant to 1 that are near zero to avoid dividing by zero + det = Vec8::sSelect(det, Vec8::sReplicate(1.0f), det_near_zero); + + // Calculate distance from inV0 to ray origin + Vec8 sx = Vec8::sSplatX(Vec4(inOrigin)) - inV0X; + Vec8 sy = Vec8::sSplatY(Vec4(inOrigin)) - inV0Y; + Vec8 sz = Vec8::sSplatZ(Vec4(inOrigin)) - inV0Z; + + // Calculate u parameter and flip sign if determinant was negative + Vec8 u = (sx * px + sy * py + sz * pz) / det; + + // Prepare to test v parameter + Vec8 qx = sy * e1z - sz * e1y; + Vec8 qy = sz * e1x - sx * e1z; + Vec8 qz = sx * e1y - sy * e1x; + + // Calculate v parameter and flip sign if determinant was negative + Vec8 v = (dx * qx + dy * qy + dz * qz) / det; + + // Get intersection point and flip sign if determinant was negative + Vec8 t = (e2x * qx + e2y * qy + e2z * qz) / det; + + // Check if there is an intersection + UVec8 no_intersection = + UVec8::sOr + ( + UVec8::sOr + ( + UVec8::sOr + ( + det_near_zero, + Vec8::sLess(u, zero) + ), + UVec8::sOr + ( + Vec8::sLess(v, zero), + Vec8::sGreater(u + v, one) + ) + ), + Vec8::sLess(t, zero) + ); + + // Select intersection point or FLT_MAX based on if there is an intersection or not + return Vec8::sSelect(t, Vec8::sReplicate(FLT_MAX), no_intersection); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/Sphere.h b/WickedEngine/Jolt/Geometry/Sphere.h new file mode 100644 index 000000000..c9aa0b3e2 --- /dev/null +++ b/WickedEngine/Jolt/Geometry/Sphere.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] Sphere +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + inline Sphere() = default; + inline Sphere(const Float3 &inCenter, float inRadius) : mCenter(inCenter), mRadius(inRadius) { } + inline Sphere(Vec3Arg inCenter, float inRadius) : mRadius(inRadius) { inCenter.StoreFloat3(&mCenter); } + + /// Calculate the support vector for this convex shape. + inline Vec3 GetSupport(Vec3Arg inDirection) const + { + float length = inDirection.Length(); + return length > 0.0f ? Vec3::sLoadFloat3Unsafe(mCenter) + (mRadius/ length) * inDirection : Vec3::sLoadFloat3Unsafe(mCenter); + } + + // Properties + inline Vec3 GetCenter() const { return Vec3::sLoadFloat3Unsafe(mCenter); } + inline float GetRadius() const { return mRadius; } + + /// Test if two spheres overlap + inline bool Overlaps(const Sphere &inB) const + { + return (Vec3::sLoadFloat3Unsafe(mCenter) - Vec3::sLoadFloat3Unsafe(inB.mCenter)).LengthSq() <= Square(mRadius + inB.mRadius); + } + + /// Check if this sphere overlaps with a box + inline bool Overlaps(const AABox &inOther) const + { + return inOther.GetSqDistanceTo(GetCenter()) <= Square(mRadius); + } + + /// Create the minimal sphere that encapsulates this sphere and inPoint + inline void EncapsulatePoint(Vec3Arg inPoint) + { + // Calculate distance between point and center + Vec3 center = GetCenter(); + Vec3 d_vec = inPoint - center; + float d_sq = d_vec.LengthSq(); + if (d_sq > Square(mRadius)) + { + // It is further away than radius, we need to widen the sphere + // The diameter of the new sphere is radius + d, so the new radius is half of that + float d = sqrt(d_sq); + float radius = 0.5f * (mRadius + d); + + // The center needs to shift by new radius - old radius in the direction of d + center += (radius - mRadius) / d * d_vec; + + // Store new sphere + center.StoreFloat3(&mCenter); + mRadius = radius; + } + } + +private: + Float3 mCenter; + float mRadius; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Geometry/Triangle.h b/WickedEngine/Jolt/Geometry/Triangle.h new file mode 100644 index 000000000..a88d08c1f --- /dev/null +++ b/WickedEngine/Jolt/Geometry/Triangle.h @@ -0,0 +1,34 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// A simple triangle and its material +class Triangle +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Triangle() = default; + Triangle(const Float3 &inV1, const Float3 &inV2, const Float3 &inV3) : mV { inV1, inV2, inV3 } { } + Triangle(const Float3 &inV1, const Float3 &inV2, const Float3 &inV3, uint32 inMaterialIndex) : Triangle(inV1, inV2, inV3) { mMaterialIndex = inMaterialIndex; } + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) { inV1.StoreFloat3(&mV[0]); inV2.StoreFloat3(&mV[1]); inV3.StoreFloat3(&mV[2]); } + + /// Get center of triangle + Vec3 GetCentroid() const + { + return (Vec3::sLoadFloat3Unsafe(mV[0]) + Vec3::sLoadFloat3Unsafe(mV[1]) + Vec3::sLoadFloat3Unsafe(mV[2])) * (1.0f / 3.0f); + } + + /// Vertices + Float3 mV[3]; + uint32 mMaterialIndex = 0; ///< Follows mV[3] so that we can read mV as 4 vectors +}; + +using TriangleList = Array; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Jolt.cmake b/WickedEngine/Jolt/Jolt.cmake new file mode 100644 index 000000000..b0336a191 --- /dev/null +++ b/WickedEngine/Jolt/Jolt.cmake @@ -0,0 +1,660 @@ +# Requires C++ 17 +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Root +set(JOLT_PHYSICS_ROOT ${PHYSICS_REPO_ROOT}/Jolt) + +# Source files +set(JOLT_PHYSICS_SRC_FILES + ${JOLT_PHYSICS_ROOT}/AABBTree/AABBTreeBuilder.cpp + ${JOLT_PHYSICS_ROOT}/AABBTree/AABBTreeBuilder.h + ${JOLT_PHYSICS_ROOT}/AABBTree/AABBTreeToBuffer.h + ${JOLT_PHYSICS_ROOT}/AABBTree/NodeCodec/NodeCodecQuadTreeHalfFloat.h + ${JOLT_PHYSICS_ROOT}/AABBTree/TriangleCodec/TriangleCodecIndexed8BitPackSOA4Flags.h + ${JOLT_PHYSICS_ROOT}/Core/ARMNeon.h + ${JOLT_PHYSICS_ROOT}/Core/Array.h + ${JOLT_PHYSICS_ROOT}/Core/Atomics.h + ${JOLT_PHYSICS_ROOT}/Core/ByteBuffer.h + ${JOLT_PHYSICS_ROOT}/Core/Color.cpp + ${JOLT_PHYSICS_ROOT}/Core/Color.h + ${JOLT_PHYSICS_ROOT}/Core/Core.h + ${JOLT_PHYSICS_ROOT}/Core/Factory.cpp + ${JOLT_PHYSICS_ROOT}/Core/Factory.h + ${JOLT_PHYSICS_ROOT}/Core/FixedSizeFreeList.h + ${JOLT_PHYSICS_ROOT}/Core/FixedSizeFreeList.inl + ${JOLT_PHYSICS_ROOT}/Core/FPControlWord.h + ${JOLT_PHYSICS_ROOT}/Core/FPException.h + ${JOLT_PHYSICS_ROOT}/Core/FPFlushDenormals.h + ${JOLT_PHYSICS_ROOT}/Core/HashCombine.h + ${JOLT_PHYSICS_ROOT}/Core/InsertionSort.h + ${JOLT_PHYSICS_ROOT}/Core/IssueReporting.cpp + ${JOLT_PHYSICS_ROOT}/Core/IssueReporting.h + ${JOLT_PHYSICS_ROOT}/Core/JobSystem.h + ${JOLT_PHYSICS_ROOT}/Core/JobSystem.inl + ${JOLT_PHYSICS_ROOT}/Core/JobSystemSingleThreaded.cpp + ${JOLT_PHYSICS_ROOT}/Core/JobSystemSingleThreaded.h + ${JOLT_PHYSICS_ROOT}/Core/JobSystemThreadPool.cpp + ${JOLT_PHYSICS_ROOT}/Core/JobSystemThreadPool.h + ${JOLT_PHYSICS_ROOT}/Core/JobSystemWithBarrier.cpp + ${JOLT_PHYSICS_ROOT}/Core/JobSystemWithBarrier.h + ${JOLT_PHYSICS_ROOT}/Core/LinearCurve.cpp + ${JOLT_PHYSICS_ROOT}/Core/LinearCurve.h + ${JOLT_PHYSICS_ROOT}/Core/LockFreeHashMap.h + ${JOLT_PHYSICS_ROOT}/Core/LockFreeHashMap.inl + ${JOLT_PHYSICS_ROOT}/Core/Memory.cpp + ${JOLT_PHYSICS_ROOT}/Core/Memory.h + ${JOLT_PHYSICS_ROOT}/Core/Mutex.h + ${JOLT_PHYSICS_ROOT}/Core/MutexArray.h + ${JOLT_PHYSICS_ROOT}/Core/NonCopyable.h + ${JOLT_PHYSICS_ROOT}/Core/Profiler.cpp + ${JOLT_PHYSICS_ROOT}/Core/Profiler.h + ${JOLT_PHYSICS_ROOT}/Core/Profiler.inl + ${JOLT_PHYSICS_ROOT}/Core/QuickSort.h + ${JOLT_PHYSICS_ROOT}/Core/Reference.h + ${JOLT_PHYSICS_ROOT}/Core/Result.h + ${JOLT_PHYSICS_ROOT}/Core/RTTI.cpp + ${JOLT_PHYSICS_ROOT}/Core/RTTI.h + ${JOLT_PHYSICS_ROOT}/Core/ScopeExit.h + ${JOLT_PHYSICS_ROOT}/Core/Semaphore.cpp + ${JOLT_PHYSICS_ROOT}/Core/Semaphore.h + ${JOLT_PHYSICS_ROOT}/Core/StaticArray.h + ${JOLT_PHYSICS_ROOT}/Core/StreamIn.h + ${JOLT_PHYSICS_ROOT}/Core/StreamOut.h + ${JOLT_PHYSICS_ROOT}/Core/StreamUtils.h + ${JOLT_PHYSICS_ROOT}/Core/StreamWrapper.h + ${JOLT_PHYSICS_ROOT}/Core/StringTools.cpp + ${JOLT_PHYSICS_ROOT}/Core/StringTools.h + ${JOLT_PHYSICS_ROOT}/Core/STLAlignedAllocator.h + ${JOLT_PHYSICS_ROOT}/Core/STLAllocator.h + ${JOLT_PHYSICS_ROOT}/Core/STLTempAllocator.h + ${JOLT_PHYSICS_ROOT}/Core/TempAllocator.h + ${JOLT_PHYSICS_ROOT}/Core/TickCounter.cpp + ${JOLT_PHYSICS_ROOT}/Core/TickCounter.h + ${JOLT_PHYSICS_ROOT}/Core/UnorderedMap.h + ${JOLT_PHYSICS_ROOT}/Core/UnorderedSet.h + ${JOLT_PHYSICS_ROOT}/Geometry/AABox.h + ${JOLT_PHYSICS_ROOT}/Geometry/AABox4.h + ${JOLT_PHYSICS_ROOT}/Geometry/ClipPoly.h + ${JOLT_PHYSICS_ROOT}/Geometry/ClosestPoint.h + ${JOLT_PHYSICS_ROOT}/Geometry/ConvexHullBuilder.cpp + ${JOLT_PHYSICS_ROOT}/Geometry/ConvexHullBuilder.h + ${JOLT_PHYSICS_ROOT}/Geometry/ConvexHullBuilder2D.cpp + ${JOLT_PHYSICS_ROOT}/Geometry/ConvexHullBuilder2D.h + ${JOLT_PHYSICS_ROOT}/Geometry/ConvexSupport.h + ${JOLT_PHYSICS_ROOT}/Geometry/Ellipse.h + ${JOLT_PHYSICS_ROOT}/Geometry/EPAConvexHullBuilder.h + ${JOLT_PHYSICS_ROOT}/Geometry/EPAPenetrationDepth.h + ${JOLT_PHYSICS_ROOT}/Geometry/GJKClosestPoint.h + ${JOLT_PHYSICS_ROOT}/Geometry/IndexedTriangle.h + ${JOLT_PHYSICS_ROOT}/Geometry/Indexify.cpp + ${JOLT_PHYSICS_ROOT}/Geometry/Indexify.h + ${JOLT_PHYSICS_ROOT}/Geometry/MortonCode.h + ${JOLT_PHYSICS_ROOT}/Geometry/OrientedBox.cpp + ${JOLT_PHYSICS_ROOT}/Geometry/OrientedBox.h + ${JOLT_PHYSICS_ROOT}/Geometry/Plane.h + ${JOLT_PHYSICS_ROOT}/Geometry/RayAABox.h + ${JOLT_PHYSICS_ROOT}/Geometry/RayAABox8.h + ${JOLT_PHYSICS_ROOT}/Geometry/RayCapsule.h + ${JOLT_PHYSICS_ROOT}/Geometry/RayCylinder.h + ${JOLT_PHYSICS_ROOT}/Geometry/RaySphere.h + ${JOLT_PHYSICS_ROOT}/Geometry/RayTriangle.h + ${JOLT_PHYSICS_ROOT}/Geometry/RayTriangle8.h + ${JOLT_PHYSICS_ROOT}/Geometry/Sphere.h + ${JOLT_PHYSICS_ROOT}/Geometry/Triangle.h + ${JOLT_PHYSICS_ROOT}/Jolt.cmake + ${JOLT_PHYSICS_ROOT}/Jolt.h + ${JOLT_PHYSICS_ROOT}/Math/DMat44.h + ${JOLT_PHYSICS_ROOT}/Math/DMat44.inl + ${JOLT_PHYSICS_ROOT}/Math/Double3.h + ${JOLT_PHYSICS_ROOT}/Math/DVec3.h + ${JOLT_PHYSICS_ROOT}/Math/DVec3.inl + ${JOLT_PHYSICS_ROOT}/Math/DynMatrix.h + ${JOLT_PHYSICS_ROOT}/Math/EigenValueSymmetric.h + ${JOLT_PHYSICS_ROOT}/Math/FindRoot.h + ${JOLT_PHYSICS_ROOT}/Math/Float2.h + ${JOLT_PHYSICS_ROOT}/Math/Float3.h + ${JOLT_PHYSICS_ROOT}/Math/Float4.h + ${JOLT_PHYSICS_ROOT}/Math/GaussianElimination.h + ${JOLT_PHYSICS_ROOT}/Math/HalfFloat.h + ${JOLT_PHYSICS_ROOT}/Math/Mat44.h + ${JOLT_PHYSICS_ROOT}/Math/Mat44.inl + ${JOLT_PHYSICS_ROOT}/Math/Math.h + ${JOLT_PHYSICS_ROOT}/Math/MathTypes.h + ${JOLT_PHYSICS_ROOT}/Math/Matrix.h + ${JOLT_PHYSICS_ROOT}/Math/Quat.h + ${JOLT_PHYSICS_ROOT}/Math/Quat.inl + ${JOLT_PHYSICS_ROOT}/Math/Real.h + ${JOLT_PHYSICS_ROOT}/Math/Swizzle.h + ${JOLT_PHYSICS_ROOT}/Math/Trigonometry.h + ${JOLT_PHYSICS_ROOT}/Math/UVec4.h + ${JOLT_PHYSICS_ROOT}/Math/UVec4.inl + ${JOLT_PHYSICS_ROOT}/Math/UVec8.h + ${JOLT_PHYSICS_ROOT}/Math/UVec8.inl + ${JOLT_PHYSICS_ROOT}/Math/Vec3.cpp + ${JOLT_PHYSICS_ROOT}/Math/Vec3.h + ${JOLT_PHYSICS_ROOT}/Math/Vec3.inl + ${JOLT_PHYSICS_ROOT}/Math/Vec4.h + ${JOLT_PHYSICS_ROOT}/Math/Vec4.inl + ${JOLT_PHYSICS_ROOT}/Math/Vec8.h + ${JOLT_PHYSICS_ROOT}/Math/Vec8.inl + ${JOLT_PHYSICS_ROOT}/Math/Vector.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/SerializableObject.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/SerializableObject.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/AllowedDOFs.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/Body.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Body/Body.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/Body.inl + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyAccess.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyAccess.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyActivationListener.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyCreationSettings.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyCreationSettings.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyFilter.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyID.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyInterface.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyInterface.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyLock.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyLockInterface.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyLockMulti.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyManager.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyManager.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyPair.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/BodyType.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/MassProperties.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Body/MassProperties.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/MotionProperties.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Body/MotionProperties.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/MotionProperties.inl + ${JOLT_PHYSICS_ROOT}/Physics/Body/MotionQuality.h + ${JOLT_PHYSICS_ROOT}/Physics/Body/MotionType.h + ${JOLT_PHYSICS_ROOT}/Physics/Character/Character.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Character/Character.h + ${JOLT_PHYSICS_ROOT}/Physics/Character/CharacterBase.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Character/CharacterBase.h + ${JOLT_PHYSICS_ROOT}/Physics/Character/CharacterVirtual.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Character/CharacterVirtual.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/AABoxCast.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ActiveEdgeMode.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ActiveEdges.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BackFaceMode.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhase.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhase.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseLayer.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/BroadPhaseQuery.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/QuadTree.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/BroadPhase/QuadTree.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CastConvexVsTriangles.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CastConvexVsTriangles.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CastSphereVsTriangles.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CastSphereVsTriangles.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CastResult.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollectFacesMode.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideConvexVsTriangles.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideConvexVsTriangles.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollidePointResult.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollideSphereVsTriangles.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionCollector.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionCollectorImpl.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionDispatch.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionDispatch.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionGroup.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/CollisionGroup.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ContactListener.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/EstimateCollisionResponse.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/EstimateCollisionResponse.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/GroupFilter.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/GroupFilter.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/GroupFilterTable.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/GroupFilterTable.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/InternalEdgeRemovingCollector.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ManifoldBetweenTwoFaces.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ManifoldBetweenTwoFaces.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseQuery.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseQuery.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseStats.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/NarrowPhaseStats.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ObjectLayer.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ObjectLayerPairFilterMask.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ObjectLayerPairFilterTable.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/PhysicsMaterial.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/PhysicsMaterial.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/PhysicsMaterialSimple.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/PhysicsMaterialSimple.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/RayCast.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/BoxShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/BoxShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/CapsuleShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/CapsuleShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/CompoundShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/CompoundShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/CompoundShapeVisitors.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/ConvexHullShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/ConvexHullShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/ConvexShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/ConvexShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/CylinderShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/CylinderShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/DecoratedShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/DecoratedShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/GetTrianglesContext.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/HeightFieldShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/HeightFieldShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/MeshShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/MeshShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/MutableCompoundShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/MutableCompoundShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/OffsetCenterOfMassShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/RotatedTranslatedShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/RotatedTranslatedShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/ScaledShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/ScaledShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/ScaleHelpers.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/Shape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/Shape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/SphereShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/SphereShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/StaticCompoundShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/StaticCompoundShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/SubShapeID.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/SubShapeIDPair.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/TaperedCapsuleShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/TaperedCapsuleShape.gliffy + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/TaperedCapsuleShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/TriangleShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/Shape/TriangleShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ShapeCast.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/ShapeFilter.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/SortReverseAndStore.h + ${JOLT_PHYSICS_ROOT}/Physics/Collision/TransformedShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Collision/TransformedShape.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/CalculateSolverSteps.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConeConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConeConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/Constraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/Constraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintManager.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintManager.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/AngleConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/AxisConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/GearConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/PointConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/SpringPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ContactConstraintManager.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/ContactConstraintManager.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/DistanceConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/DistanceConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/FixedConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/FixedConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/GearConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/GearConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/HingeConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/HingeConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/MotorSettings.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/MotorSettings.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PathConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PathConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PathConstraintPath.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PathConstraintPath.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PathConstraintPathHermite.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PathConstraintPathHermite.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PointConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PointConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PulleyConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/PulleyConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/RackAndPinionConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/RackAndPinionConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SixDOFConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SixDOFConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SliderConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SliderConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SpringSettings.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SpringSettings.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SwingTwistConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/SwingTwistConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/TwoBodyConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Constraints/TwoBodyConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/DeterminismLog.cpp + ${JOLT_PHYSICS_ROOT}/Physics/DeterminismLog.h + ${JOLT_PHYSICS_ROOT}/Physics/EActivation.h + ${JOLT_PHYSICS_ROOT}/Physics/EPhysicsUpdateError.h + ${JOLT_PHYSICS_ROOT}/Physics/IslandBuilder.cpp + ${JOLT_PHYSICS_ROOT}/Physics/IslandBuilder.h + ${JOLT_PHYSICS_ROOT}/Physics/LargeIslandSplitter.cpp + ${JOLT_PHYSICS_ROOT}/Physics/LargeIslandSplitter.h + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsLock.cpp + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsLock.h + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsScene.cpp + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsScene.h + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsSettings.h + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsStepListener.h + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsSystem.cpp + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsSystem.h + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsUpdateContext.cpp + ${JOLT_PHYSICS_ROOT}/Physics/PhysicsUpdateContext.h + ${JOLT_PHYSICS_ROOT}/Physics/Ragdoll/Ragdoll.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Ragdoll/Ragdoll.h + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyContactListener.h + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyCreationSettings.cpp + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyCreationSettings.h + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyManifold.h + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyMotionProperties.h + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyMotionProperties.cpp + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyShape.cpp + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyShape.h + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodySharedSettings.cpp + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodySharedSettings.h + ${JOLT_PHYSICS_ROOT}/Physics/SoftBody/SoftBodyVertex.h + ${JOLT_PHYSICS_ROOT}/Physics/StateRecorder.h + ${JOLT_PHYSICS_ROOT}/Physics/StateRecorderImpl.cpp + ${JOLT_PHYSICS_ROOT}/Physics/StateRecorderImpl.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/MotorcycleController.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/MotorcycleController.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/TrackedVehicleController.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/TrackedVehicleController.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleAntiRollBar.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleAntiRollBar.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleCollisionTester.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleCollisionTester.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleConstraint.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleConstraint.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleController.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleController.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleDifferential.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleDifferential.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleEngine.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleEngine.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleTrack.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleTrack.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleTransmission.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/VehicleTransmission.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/Wheel.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/Wheel.h + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/WheeledVehicleController.cpp + ${JOLT_PHYSICS_ROOT}/Physics/Vehicle/WheeledVehicleController.h + ${JOLT_PHYSICS_ROOT}/RegisterTypes.cpp + ${JOLT_PHYSICS_ROOT}/RegisterTypes.h + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRenderer.cpp + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRenderer.h + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRendererPlayback.cpp + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRendererPlayback.h + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRendererRecorder.cpp + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRendererRecorder.h + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRendererSimple.cpp + ${JOLT_PHYSICS_ROOT}/Renderer/DebugRendererSimple.h + ${JOLT_PHYSICS_ROOT}/Skeleton/SkeletalAnimation.cpp + ${JOLT_PHYSICS_ROOT}/Skeleton/SkeletalAnimation.h + ${JOLT_PHYSICS_ROOT}/Skeleton/Skeleton.cpp + ${JOLT_PHYSICS_ROOT}/Skeleton/Skeleton.h + ${JOLT_PHYSICS_ROOT}/Skeleton/SkeletonMapper.cpp + ${JOLT_PHYSICS_ROOT}/Skeleton/SkeletonMapper.h + ${JOLT_PHYSICS_ROOT}/Skeleton/SkeletonPose.cpp + ${JOLT_PHYSICS_ROOT}/Skeleton/SkeletonPose.h + ${JOLT_PHYSICS_ROOT}/TriangleGrouper/TriangleGrouper.h + ${JOLT_PHYSICS_ROOT}/TriangleGrouper/TriangleGrouperClosestCentroid.cpp + ${JOLT_PHYSICS_ROOT}/TriangleGrouper/TriangleGrouperClosestCentroid.h + ${JOLT_PHYSICS_ROOT}/TriangleGrouper/TriangleGrouperMorton.cpp + ${JOLT_PHYSICS_ROOT}/TriangleGrouper/TriangleGrouperMorton.h + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitter.cpp + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitter.h + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterBinning.cpp + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterBinning.h + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterFixedLeafSize.h + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterLongestAxis.cpp + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterLongestAxis.h + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterMean.cpp + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterMean.h + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterMorton.cpp + ${JOLT_PHYSICS_ROOT}/TriangleSplitter/TriangleSplitterMorton.h +) + +if (ENABLE_OBJECT_STREAM) + set(JOLT_PHYSICS_SRC_FILES + ${JOLT_PHYSICS_SRC_FILES} + ${JOLT_PHYSICS_ROOT}/ObjectStream/GetPrimitiveTypeOfType.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStream.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStream.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamBinaryIn.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamBinaryIn.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamBinaryOut.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamBinaryOut.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamIn.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamIn.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamOut.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamOut.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamTextIn.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamTextIn.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamTextOut.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamTextOut.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/ObjectStreamTypes.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/SerializableAttribute.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/SerializableAttributeEnum.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/SerializableAttributeTyped.h + ${JOLT_PHYSICS_ROOT}/ObjectStream/TypeDeclarations.cpp + ${JOLT_PHYSICS_ROOT}/ObjectStream/TypeDeclarations.h + ) +endif() + +if ("${CMAKE_SYSTEM_NAME}" STREQUAL "Windows") + # Add natvis file + set(JOLT_PHYSICS_SRC_FILES ${JOLT_PHYSICS_SRC_FILES} ${JOLT_PHYSICS_ROOT}/Jolt.natvis) +endif() + +# Group source files +source_group(TREE ${JOLT_PHYSICS_ROOT} FILES ${JOLT_PHYSICS_SRC_FILES}) + +# Create Jolt lib +add_library(Jolt ${JOLT_PHYSICS_SRC_FILES}) + +if (BUILD_SHARED_LIBS) + # Set default visibility to hidden + set(CMAKE_CXX_VISIBILITY_PRESET hidden) + + if (GENERATE_DEBUG_SYMBOLS) + if (MSVC) + # MSVC specific option to enable PDB generation + set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DEBUG:FASTLINK") + else() + # Clang/GCC option to enable debug symbol generation + set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} -g") + endif() + endif() + + # Set linker flags for other build types to be the same as release + set(CMAKE_SHARED_LINKER_FLAGS_RELEASEASAN "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}") + set(CMAKE_SHARED_LINKER_FLAGS_RELEASEUBSAN "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}") + set(CMAKE_SHARED_LINKER_FLAGS_RELEASECOVERAGE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}") + set(CMAKE_SHARED_LINKER_FLAGS_DISTRIBUTION "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}") + + # Public define to instruct user code to import Jolt symbols (rather than use static linking) + target_compile_definitions(Jolt PUBLIC JPH_SHARED_LIBRARY) + + # Private define to instruct the library to export symbols for shared linking + target_compile_definitions(Jolt PRIVATE JPH_BUILD_SHARED_LIBRARY) +endif() + +# Use repository as include directory when building, install directory when installing +target_include_directories(Jolt PUBLIC + $ + $) +target_precompile_headers(Jolt PRIVATE ${JOLT_PHYSICS_ROOT}/Jolt.h) + +# Set the debug/non-debug build flags +target_compile_definitions(Jolt PUBLIC "$<$:_DEBUG>") +target_compile_definitions(Jolt PUBLIC "$<$:NDEBUG>") + +# ASAN should use the default allocators +target_compile_definitions(Jolt PUBLIC "$<$:JPH_DISABLE_TEMP_ALLOCATOR;JPH_DISABLE_CUSTOM_ALLOCATOR>") + +# Setting floating point exceptions +if (FLOATING_POINT_EXCEPTIONS_ENABLED AND "${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") + target_compile_definitions(Jolt PUBLIC "$<$:JPH_FLOATING_POINT_EXCEPTIONS_ENABLED>") +endif() + +# Setting the disable custom allocator flag +if (DISABLE_CUSTOM_ALLOCATOR) + target_compile_definitions(Jolt PUBLIC JPH_DISABLE_CUSTOM_ALLOCATOR) +endif() + +# Setting enable asserts flag +if (USE_ASSERTS) + target_compile_definitions(Jolt PUBLIC JPH_ENABLE_ASSERTS) +endif() + +# Setting double precision flag +if (DOUBLE_PRECISION) + target_compile_definitions(Jolt PUBLIC JPH_DOUBLE_PRECISION) +endif() + +# Setting to attempt cross platform determinism +if (CROSS_PLATFORM_DETERMINISTIC) + target_compile_definitions(Jolt PUBLIC JPH_CROSS_PLATFORM_DETERMINISTIC) +endif() + +# Setting to determine number of bits in ObjectLayer +if (OBJECT_LAYER_BITS) + target_compile_definitions(Jolt PUBLIC JPH_OBJECT_LAYER_BITS=${OBJECT_LAYER_BITS}) +endif() + +if (USE_STD_VECTOR) + target_compile_definitions(Jolt PUBLIC JPH_USE_STD_VECTOR) +endif() + +# Setting to periodically trace broadphase stats to help determine if the broadphase layer configuration is optimal +if (TRACK_BROADPHASE_STATS) + target_compile_definitions(Jolt PUBLIC JPH_TRACK_BROADPHASE_STATS) +endif() + +# Setting to periodically trace narrowphase stats to help determine which collision queries could be optimized +if (TRACK_NARROWPHASE_STATS) + target_compile_definitions(Jolt PUBLIC JPH_TRACK_NARROWPHASE_STATS) +endif() + +# Enable the debug renderer +if (DEBUG_RENDERER_IN_DISTRIBUTION) + target_compile_definitions(Jolt PUBLIC "JPH_DEBUG_RENDERER") +elseif (DEBUG_RENDERER_IN_DEBUG_AND_RELEASE) + target_compile_definitions(Jolt PUBLIC "$<$:JPH_DEBUG_RENDERER>") +endif() + +# Enable the profiler +if (PROFILER_IN_DISTRIBUTION) + target_compile_definitions(Jolt PUBLIC "JPH_PROFILE_ENABLED") +elseif (PROFILER_IN_DEBUG_AND_RELEASE) + target_compile_definitions(Jolt PUBLIC "$<$:JPH_PROFILE_ENABLED>") +endif() + +# Compile the ObjectStream class and RTTI attribute information +if (ENABLE_OBJECT_STREAM) + target_compile_definitions(Jolt PUBLIC JPH_OBJECT_STREAM) +endif() + +# Emit the instruction set definitions to ensure that child projects use the same settings even if they override the used instruction sets (a mismatch causes link errors) +function(EMIT_X86_INSTRUCTION_SET_DEFINITIONS) + if (USE_AVX512) + target_compile_definitions(Jolt PUBLIC JPH_USE_AVX512) + endif() + if (USE_AVX2) + target_compile_definitions(Jolt PUBLIC JPH_USE_AVX2) + endif() + if (USE_AVX) + target_compile_definitions(Jolt PUBLIC JPH_USE_AVX) + endif() + if (USE_SSE4_1) + target_compile_definitions(Jolt PUBLIC JPH_USE_SSE4_1) + endif() + if (USE_SSE4_2) + target_compile_definitions(Jolt PUBLIC JPH_USE_SSE4_2) + endif() + if (USE_LZCNT) + target_compile_definitions(Jolt PUBLIC JPH_USE_LZCNT) + endif() + if (USE_TZCNT) + target_compile_definitions(Jolt PUBLIC JPH_USE_TZCNT) + endif() + if (USE_F16C) + target_compile_definitions(Jolt PUBLIC JPH_USE_F16C) + endif() + if (USE_FMADD AND NOT CROSS_PLATFORM_DETERMINISTIC) + target_compile_definitions(Jolt PUBLIC JPH_USE_FMADD) + endif() +endfunction() + +# Add the compiler commandline flags to select the right instruction sets +if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") + if ("${CMAKE_VS_PLATFORM_NAME}" STREQUAL "x86" OR "${CMAKE_VS_PLATFORM_NAME}" STREQUAL "x64") + if (USE_AVX512) + target_compile_options(Jolt PUBLIC /arch:AVX512) + elseif (USE_AVX2) + target_compile_options(Jolt PUBLIC /arch:AVX2) + elseif (USE_AVX) + target_compile_options(Jolt PUBLIC /arch:AVX) + endif() + EMIT_X86_INSTRUCTION_SET_DEFINITIONS() + endif() +else() + if (XCODE) + # XCode builds for multiple architectures, we can't set global flags + elseif (CROSS_COMPILE_ARM OR CMAKE_OSX_ARCHITECTURES MATCHES "arm64" OR "${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "aarch64") + # ARM64 uses no special commandline flags + elseif ("${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "x86_64" OR "${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "AMD64" OR "${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "x86" OR "${CMAKE_SYSTEM_PROCESSOR}" STREQUAL "i386") + # x86 and x86_64 + # On 32-bit builds we need to default to using SSE instructions, the x87 FPU instructions have higher intermediate precision + # which will cause problems in the collision detection code (the effect is similar to leaving FMA on, search for + # JPH_PRECISE_MATH_ON for the locations where this is a problem). + + if (USE_AVX512) + target_compile_options(Jolt PUBLIC -mavx512f -mavx512vl -mavx512dq -mavx2 -mbmi -mpopcnt -mlzcnt -mf16c) + elseif (USE_AVX2) + target_compile_options(Jolt PUBLIC -mavx2 -mbmi -mpopcnt -mlzcnt -mf16c) + elseif (USE_AVX) + target_compile_options(Jolt PUBLIC -mavx -mpopcnt) + elseif (USE_SSE4_2) + target_compile_options(Jolt PUBLIC -msse4.2 -mpopcnt) + elseif (USE_SSE4_1) + target_compile_options(Jolt PUBLIC -msse4.1) + else() + target_compile_options(Jolt PUBLIC -msse2) + endif() + if (USE_LZCNT) + target_compile_options(Jolt PUBLIC -mlzcnt) + endif() + if (USE_TZCNT) + target_compile_options(Jolt PUBLIC -mbmi) + endif() + if (USE_F16C) + target_compile_options(Jolt PUBLIC -mf16c) + endif() + if (USE_FMADD AND NOT CROSS_PLATFORM_DETERMINISTIC) + target_compile_options(Jolt PUBLIC -mfma) + endif() + + if (NOT MSVC) + target_compile_options(Jolt PUBLIC -mfpmath=sse) + endif() + + EMIT_X86_INSTRUCTION_SET_DEFINITIONS() + endif() +endif() diff --git a/WickedEngine/Jolt/Jolt.h b/WickedEngine/Jolt/Jolt.h new file mode 100644 index 000000000..acc400ce9 --- /dev/null +++ b/WickedEngine/Jolt/Jolt.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +// Project includes +#include +#include +#include +#include +#include +#include +#include +#include +#include diff --git a/WickedEngine/Jolt/Jolt.natvis b/WickedEngine/Jolt/Jolt.natvis new file mode 100644 index 000000000..b4e9ddbd0 --- /dev/null +++ b/WickedEngine/Jolt/Jolt.natvis @@ -0,0 +1,97 @@ + + + + r={(int)r}, g={(int)g}, b={(int)b}, a={(int)a} + + + {x}, {y} + + + {x}, {y}, {z} + + + {x}, {y}, {z}, {w} + + + {mF32[0]}, {mF32[1]}, {mF32[2]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]} + + + {mF64[0]}, {mF64[1]}, {mF64[2]}, L^2={mF64[0]*mF64[0]+mF64[1]*mF64[1]+mF64[2]*mF64[2]} + + + {mF32[0]}, {mF32[1]}, {mF32[2]}, {mF32[3]}, L^2={mF32[0]*mF32[0]+mF32[1]*mF32[1]+mF32[2]*mF32[2]+mF32[3]*mF32[3]} + + + {mU32[0]}, {mU32[1]}, {mU32[2]}, {mU32[3]} + + + {mValue} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol[3].mF32[0]} + + + {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol[3].mF32[1]} + + + {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol[3].mF32[2]} + + + {mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, {mCol[3].mF32[3]} + + + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} | {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} | {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]} + + + {mCol[0].mF32[0]}, {mCol[1].mF32[0]}, {mCol[2].mF32[0]}, {mCol3.mF64[0]} + + + {mCol[0].mF32[1]}, {mCol[1].mF32[1]}, {mCol[2].mF32[1]}, {mCol3.mF64[1]} + + + {mCol[0].mF32[2]}, {mCol[1].mF32[2]}, {mCol[2].mF32[2]}, {mCol3.mF64[2]} + + + {mCol[0].mF32[3]}, {mCol[1].mF32[3]}, {mCol[2].mF32[3]}, 1} + + + + + min=({mMin}), max=({mMax}) + + + {mID} + + + {mDebugName}: p=({mPosition.mF32[0],g}, {mPosition.mF32[1],g}, {mPosition.mF32[2],g}), r=({mRotation.mValue.mF32[0],g}, {mRotation.mValue.mF32[1],g}, {mRotation.mValue.mF32[2],g}, {mRotation.mValue.mF32[3],g}), v=({mLinearVelocity.mF32[0],g}, {mLinearVelocity.mF32[1],g}, {mLinearVelocity.mF32[2],g}), w=({mAngularVelocity.mF32[0],g}, {mAngularVelocity.mF32[1],g}, {mAngularVelocity.mF32[2],g}) + + + bodies={mBodies._Mypair._Myval2._Mylast - mBodies._Mypair._Myval2._Myfirst}, active={mActiveBodies._Mypair._Myval2._Mylast - mActiveBodies._Mypair._Myval2._Myfirst} + + + size={mSize} + + mSize + + mSize + (value_type *)mElements + + + + + size={mSize} + + mSize + mCapacity + + mSize + mElements + + + + diff --git a/WickedEngine/Jolt/Math/DMat44.h b/WickedEngine/Jolt/Math/DMat44.h new file mode 100644 index 000000000..65c968716 --- /dev/null +++ b/WickedEngine/Jolt/Math/DMat44.h @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Holds a 4x4 matrix of floats with the last column consisting of doubles +class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DMat44 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying column type + using Type = Vec4::Type; + using DType = DVec3::Type; + using DTypeArg = DVec3::TypeArg; + + // Argument type + using ArgType = DMat44Arg; + + /// Constructor + DMat44() = default; ///< Intentionally not initialized for performance reasons + JPH_INLINE DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4); + DMat44(const DMat44 &inM2) = default; + DMat44 & operator = (const DMat44 &inM2) = default; + JPH_INLINE explicit DMat44(Mat44Arg inM); + JPH_INLINE DMat44(Mat44Arg inRot, DVec3Arg inT); + JPH_INLINE DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4); + + /// Zero matrix + static JPH_INLINE DMat44 sZero(); + + /// Identity matrix + static JPH_INLINE DMat44 sIdentity(); + + /// Rotate from quaternion + static JPH_INLINE DMat44 sRotation(QuatArg inQuat) { return DMat44(Mat44::sRotation(inQuat), DVec3::sZero()); } + + /// Get matrix that translates + static JPH_INLINE DMat44 sTranslation(DVec3Arg inV) { return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), inV); } + + /// Get matrix that rotates and translates + static JPH_INLINE DMat44 sRotationTranslation(QuatArg inR, DVec3Arg inT) { return DMat44(Mat44::sRotation(inR), inT); } + + /// Get inverse matrix of sRotationTranslation + static JPH_INLINE DMat44 sInverseRotationTranslation(QuatArg inR, DVec3Arg inT); + + /// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal) + static JPH_INLINE DMat44 sScale(Vec3Arg inV) { return DMat44(Mat44::sScale(inV), DVec3::sZero()); } + + /// Convert to Mat44 rounding to nearest + JPH_INLINE Mat44 ToMat44() const { return Mat44(mCol[0], mCol[1], mCol[2], Vec3(mCol3)); } + + /// Comparison + JPH_INLINE bool operator == (DMat44Arg inM2) const; + JPH_INLINE bool operator != (DMat44Arg inM2) const { return !(*this == inM2); } + + /// Test if two matrices are close + JPH_INLINE bool IsClose(DMat44Arg inM2, float inMaxDistSq = 1.0e-12f) const; + + /// Multiply matrix by matrix + JPH_INLINE DMat44 operator * (Mat44Arg inM) const; + + /// Multiply matrix by matrix + JPH_INLINE DMat44 operator * (DMat44Arg inM) const; + + /// Multiply vector by matrix + JPH_INLINE DVec3 operator * (Vec3Arg inV) const; + + /// Multiply vector by matrix + JPH_INLINE DVec3 operator * (DVec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const { return GetRotation().Multiply3x3(inV); } + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE DVec3 Multiply3x3(DVec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$) + JPH_INLINE Vec3 Multiply3x3Transposed(Vec3Arg inV) const { return GetRotation().Multiply3x3Transposed(inV); } + + /// Scale a matrix: result = this * Mat44::sScale(inScale) + JPH_INLINE DMat44 PreScaled(Vec3Arg inScale) const; + + /// Scale a matrix: result = Mat44::sScale(inScale) * this + JPH_INLINE DMat44 PostScaled(Vec3Arg inScale) const; + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE DMat44 PreTranslated(Vec3Arg inTranslation) const; + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE DMat44 PreTranslated(DVec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE DMat44 PostTranslated(Vec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE DMat44 PostTranslated(DVec3Arg inTranslation) const; + + /// Access to the columns + JPH_INLINE Vec3 GetAxisX() const { return Vec3(mCol[0]); } + JPH_INLINE void SetAxisX(Vec3Arg inV) { mCol[0] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisY() const { return Vec3(mCol[1]); } + JPH_INLINE void SetAxisY(Vec3Arg inV) { mCol[1] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisZ() const { return Vec3(mCol[2]); } + JPH_INLINE void SetAxisZ(Vec3Arg inV) { mCol[2] = Vec4(inV, 0.0f); } + JPH_INLINE DVec3 GetTranslation() const { return mCol3; } + JPH_INLINE void SetTranslation(DVec3Arg inV) { mCol3 = inV; } + JPH_INLINE Vec3 GetColumn3(uint inCol) const { JPH_ASSERT(inCol < 3); return Vec3(mCol[inCol]); } + JPH_INLINE void SetColumn3(uint inCol, Vec3Arg inV) { JPH_ASSERT(inCol < 3); mCol[inCol] = Vec4(inV, 0.0f); } + JPH_INLINE Vec4 GetColumn4(uint inCol) const { JPH_ASSERT(inCol < 3); return mCol[inCol]; } + JPH_INLINE void SetColumn4(uint inCol, Vec4Arg inV) { JPH_ASSERT(inCol < 3); mCol[inCol] = inV; } + + /// Transpose 3x3 subpart of matrix + JPH_INLINE Mat44 Transposed3x3() const { return GetRotation().Transposed3x3(); } + + /// Inverse 4x4 matrix + JPH_INLINE DMat44 Inversed() const; + + /// Inverse 4x4 matrix when it only contains rotation and translation + JPH_INLINE DMat44 InversedRotationTranslation() const; + + /// Get rotation part only (note: retains the first 3 values from the bottom row) + JPH_INLINE Mat44 GetRotation() const { return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); } + + /// Updates the rotation part of this matrix (the first 3 columns) + JPH_INLINE void SetRotation(Mat44Arg inRotation); + + /// Convert to quaternion + JPH_INLINE Quat GetQuaternion() const { return GetRotation().GetQuaternion(); } + + /// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved) + JPH_INLINE Mat44 GetDirectionPreservingMatrix() const { return GetRotation().Inversed3x3().Transposed3x3(); } + + /// Works identical to Mat44::Decompose + JPH_INLINE DMat44 Decompose(Vec3 &outScale) const { return DMat44(GetRotation().Decompose(outScale), mCol3); } + + /// To String + friend ostream & operator << (ostream &inStream, DMat44Arg inM) + { + inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol3; + return inStream; + } + +private: + Vec4 mCol[3]; ///< Rotation columns + DVec3 mCol3; ///< Translation column, 4th element is assumed to be 1 +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "DMat44.inl" diff --git a/WickedEngine/Jolt/Math/DMat44.inl b/WickedEngine/Jolt/Math/DMat44.inl new file mode 100644 index 000000000..462cf7911 --- /dev/null +++ b/WickedEngine/Jolt/Math/DMat44.inl @@ -0,0 +1,310 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +DMat44::DMat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, DVec3Arg inC4) : + mCol { inC1, inC2, inC3 }, + mCol3(inC4) +{ +} + +DMat44::DMat44(Type inC1, Type inC2, Type inC3, DTypeArg inC4) : + mCol { inC1, inC2, inC3 }, + mCol3(inC4) +{ +} + +DMat44::DMat44(Mat44Arg inM) : + mCol { inM.GetColumn4(0), inM.GetColumn4(1), inM.GetColumn4(2) }, + mCol3(inM.GetTranslation()) +{ +} + +DMat44::DMat44(Mat44Arg inRot, DVec3Arg inT) : + mCol { inRot.GetColumn4(0), inRot.GetColumn4(1), inRot.GetColumn4(2) }, + mCol3(inT) +{ +} + +DMat44 DMat44::sZero() +{ + return DMat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), DVec3::sZero()); +} + +DMat44 DMat44::sIdentity() +{ + return DMat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), DVec3::sZero()); +} + +DMat44 DMat44::sInverseRotationTranslation(QuatArg inR, DVec3Arg inT) +{ + Mat44 m = Mat44::sRotation(inR.Conjugated()); + DMat44 dm(m, DVec3::sZero()); + dm.SetTranslation(-dm.Multiply3x3(inT)); + return dm; +} + +bool DMat44::operator == (DMat44Arg inM2) const +{ + return mCol[0] == inM2.mCol[0] + && mCol[1] == inM2.mCol[1] + && mCol[2] == inM2.mCol[2] + && mCol3 == inM2.mCol3; +} + +bool DMat44::IsClose(DMat44Arg inM2, float inMaxDistSq) const +{ + for (int i = 0; i < 3; ++i) + if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq)) + return false; + return mCol3.IsClose(inM2.mCol3, double(inMaxDistSq)); +} + +DVec3 DMat44::operator * (Vec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + return DVec3::sFixW(_mm256_add_pd(mCol3.mValue, _mm256_cvtps_pd(t))); +#elif defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + __m128d low = _mm_add_pd(mCol3.mValue.mLow, _mm_cvtps_pd(t)); + __m128d high = _mm_add_pd(mCol3.mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(t, t, _MM_SHUFFLE(2, 2, 2, 2)))); + return DVec3({ low, high }); +#elif defined(JPH_USE_NEON) + float32x4_t t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + float64x2_t low = vaddq_f64(mCol3.mValue.val[0], vcvt_f64_f32(vget_low_f32(t))); + float64x2_t high = vaddq_f64(mCol3.mValue.val[1], vcvt_high_f64_f32(t)); + return DVec3::sFixW({ low, high }); +#else + return DVec3( + mCol3.mF64[0] + double(mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2]), + mCol3.mF64[1] + double(mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2]), + mCol3.mF64[2] + double(mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2])); +#endif +} + +DVec3 DMat44::operator * (DVec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m256d t = _mm256_add_pd(mCol3.mValue, _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2]))); + return DVec3::sFixW(t); +#elif defined(JPH_USE_SSE) + __m128d xxxx = _mm_set1_pd(inV.mF64[0]); + __m128d yyyy = _mm_set1_pd(inV.mF64[1]); + __m128d zzzz = _mm_set1_pd(inV.mF64[2]); + __m128 col0 = mCol[0].mValue; + __m128 col1 = mCol[1].mValue; + __m128 col2 = mCol[2].mValue; + __m128d t_low = _mm_add_pd(mCol3.mValue.mLow, _mm_mul_pd(_mm_cvtps_pd(col0), xxxx)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz)); + __m128d t_high = _mm_add_pd(mCol3.mValue.mHigh, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz)); + return DVec3({ t_low, t_high }); +#elif defined(JPH_USE_NEON) + float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0); + float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1); + float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0); + float32x4_t col0 = mCol[0].mValue; + float32x4_t col1 = mCol[1].mValue; + float32x4_t col2 = mCol[2].mValue; + float64x2_t t_low = vaddq_f64(mCol3.mValue.val[0], vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz)); + float64x2_t t_high = vaddq_f64(mCol3.mValue.val[1], vmulq_f64(vcvt_high_f64_f32(col0), xxxx)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz)); + return DVec3::sFixW({ t_low, t_high }); +#else + return DVec3( + mCol3.mF64[0] + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], + mCol3.mF64[1] + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], + mCol3.mF64[2] + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]); +#endif +} + +DVec3 DMat44::Multiply3x3(DVec3Arg inV) const +{ +#if defined(JPH_USE_AVX) + __m256d t = _mm256_mul_pd(_mm256_cvtps_pd(mCol[0].mValue), _mm256_set1_pd(inV.mF64[0])); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[1].mValue), _mm256_set1_pd(inV.mF64[1]))); + t = _mm256_add_pd(t, _mm256_mul_pd(_mm256_cvtps_pd(mCol[2].mValue), _mm256_set1_pd(inV.mF64[2]))); + return DVec3::sFixW(t); +#elif defined(JPH_USE_SSE) + __m128d xxxx = _mm_set1_pd(inV.mF64[0]); + __m128d yyyy = _mm_set1_pd(inV.mF64[1]); + __m128d zzzz = _mm_set1_pd(inV.mF64[2]); + __m128 col0 = mCol[0].mValue; + __m128 col1 = mCol[1].mValue; + __m128 col2 = mCol[2].mValue; + __m128d t_low = _mm_mul_pd(_mm_cvtps_pd(col0), xxxx); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col1), yyyy)); + t_low = _mm_add_pd(t_low, _mm_mul_pd(_mm_cvtps_pd(col2), zzzz)); + __m128d t_high = _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col0, col0, _MM_SHUFFLE(2, 2, 2, 2))), xxxx); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col1, col1, _MM_SHUFFLE(2, 2, 2, 2))), yyyy)); + t_high = _mm_add_pd(t_high, _mm_mul_pd(_mm_cvtps_pd(_mm_shuffle_ps(col2, col2, _MM_SHUFFLE(2, 2, 2, 2))), zzzz)); + return DVec3({ t_low, t_high }); +#elif defined(JPH_USE_NEON) + float64x2_t xxxx = vdupq_laneq_f64(inV.mValue.val[0], 0); + float64x2_t yyyy = vdupq_laneq_f64(inV.mValue.val[0], 1); + float64x2_t zzzz = vdupq_laneq_f64(inV.mValue.val[1], 0); + float32x4_t col0 = mCol[0].mValue; + float32x4_t col1 = mCol[1].mValue; + float32x4_t col2 = mCol[2].mValue; + float64x2_t t_low = vmulq_f64(vcvt_f64_f32(vget_low_f32(col0)), xxxx); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col1)), yyyy)); + t_low = vaddq_f64(t_low, vmulq_f64(vcvt_f64_f32(vget_low_f32(col2)), zzzz)); + float64x2_t t_high = vmulq_f64(vcvt_high_f64_f32(col0), xxxx); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col1), yyyy)); + t_high = vaddq_f64(t_high, vmulq_f64(vcvt_high_f64_f32(col2), zzzz)); + return DVec3::sFixW({ t_low, t_high }); +#else + return DVec3( + double(mCol[0].mF32[0]) * inV.mF64[0] + double(mCol[1].mF32[0]) * inV.mF64[1] + double(mCol[2].mF32[0]) * inV.mF64[2], + double(mCol[0].mF32[1]) * inV.mF64[0] + double(mCol[1].mF32[1]) * inV.mF64[1] + double(mCol[2].mF32[1]) * inV.mF64[2], + double(mCol[0].mF32[2]) * inV.mF64[0] + double(mCol[1].mF32[2]) * inV.mF64[1] + double(mCol[2].mF32[2]) * inV.mF64[2]); +#endif +} + +DMat44 DMat44::operator * (Mat44Arg inM) const +{ + DMat44 result; + + // Rotation part +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.GetColumn4(i).mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.GetColumn4(i).mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + { + Vec4 coli = inM.GetColumn4(i); + result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2]; + } +#endif + + // Translation part + result.mCol3 = *this * inM.GetTranslation(); + + return result; +} + +DMat44 DMat44::operator * (DMat44Arg inM) const +{ + DMat44 result; + + // Rotation part +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.GetColumn4(i).mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + { + Vec4 coli = inM.mCol[i]; + result.mCol[i] = mCol[0] * coli.mF32[0] + mCol[1] * coli.mF32[1] + mCol[2] * coli.mF32[2]; + } +#endif + + // Translation part + result.mCol3 = *this * inM.GetTranslation(); + + return result; +} + +void DMat44::SetRotation(Mat44Arg inRotation) +{ + mCol[0] = inRotation.GetColumn4(0); + mCol[1] = inRotation.GetColumn4(1); + mCol[2] = inRotation.GetColumn4(2); +} + +DMat44 DMat44::PreScaled(Vec3Arg inScale) const +{ + return DMat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol3); +} + +DMat44 DMat44::PostScaled(Vec3Arg inScale) const +{ + Vec4 scale(inScale, 1); + return DMat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], DVec3(scale) * mCol3); +} + +DMat44 DMat44::PreTranslated(Vec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation)); +} + +DMat44 DMat44::PreTranslated(DVec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + Multiply3x3(inTranslation)); +} + +DMat44 DMat44::PostTranslated(Vec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation); +} + +DMat44 DMat44::PostTranslated(DVec3Arg inTranslation) const +{ + return DMat44(mCol[0], mCol[1], mCol[2], GetTranslation() + inTranslation); +} + +DMat44 DMat44::Inversed() const +{ + DMat44 m(GetRotation().Inversed3x3()); + m.mCol3 = -m.Multiply3x3(mCol3); + return m; +} + +DMat44 DMat44::InversedRotationTranslation() const +{ + DMat44 m(GetRotation().Transposed3x3()); + m.mCol3 = -m.Multiply3x3(mCol3); + return m; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/DVec3.h b/WickedEngine/Jolt/Math/DVec3.h new file mode 100644 index 000000000..58e0a06f3 --- /dev/null +++ b/WickedEngine/Jolt/Math/DVec3.h @@ -0,0 +1,288 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// 3 component vector of doubles (stored as 4 vectors). +/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined +class [[nodiscard]] alignas(JPH_DVECTOR_ALIGNMENT) DVec3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_AVX) + using Type = __m256d; + using TypeArg = __m256d; +#elif defined(JPH_USE_SSE) + using Type = struct { __m128d mLow, mHigh; }; + using TypeArg = const Type &; +#elif defined(JPH_USE_NEON) + using Type = float64x2x2_t; + using TypeArg = const Type &; +#else + using Type = struct { double mData[4]; }; + using TypeArg = const Type &; +#endif + + // Argument type + using ArgType = DVec3Arg; + + /// Constructor + DVec3() = default; ///< Intentionally not initialized for performance reasons + DVec3(const DVec3 &inRHS) = default; + DVec3 & operator = (const DVec3 &inRHS) = default; + JPH_INLINE explicit DVec3(Vec3Arg inRHS); + JPH_INLINE explicit DVec3(Vec4Arg inRHS); + JPH_INLINE DVec3(TypeArg inRHS) : mValue(inRHS) { CheckW(); } + + /// Create a vector from 3 components + JPH_INLINE DVec3(double inX, double inY, double inZ); + + /// Load 3 doubles from memory + explicit JPH_INLINE DVec3(const Double3 &inV); + + /// Vector with all zeros + static JPH_INLINE DVec3 sZero(); + + /// Vectors with the principal axis + static JPH_INLINE DVec3 sAxisX() { return DVec3(1, 0, 0); } + static JPH_INLINE DVec3 sAxisY() { return DVec3(0, 1, 0); } + static JPH_INLINE DVec3 sAxisZ() { return DVec3(0, 0, 1); } + + /// Replicate inV across all components + static JPH_INLINE DVec3 sReplicate(double inV); + + /// Vector with all NaN's + static JPH_INLINE DVec3 sNaN(); + + /// Load 3 doubles from memory (reads 64 bits extra which it doesn't use) + static JPH_INLINE DVec3 sLoadDouble3Unsafe(const Double3 &inV); + + /// Store 3 doubles to memory + JPH_INLINE void StoreDouble3(Double3 *outV) const; + + /// Convert to float vector 3 rounding to nearest + JPH_INLINE explicit operator Vec3() const; + + /// Prepare to convert to float vector 3 rounding towards zero (returns DVec3 that can be converted to a Vec3 to get the rounding) + JPH_INLINE DVec3 PrepareRoundToZero() const; + + /// Prepare to convert to float vector 3 rounding towards positive/negative inf (returns DVec3 that can be converted to a Vec3 to get the rounding) + JPH_INLINE DVec3 PrepareRoundToInf() const; + + /// Convert to float vector 3 rounding down + JPH_INLINE Vec3 ToVec3RoundDown() const; + + /// Convert to float vector 3 rounding up + JPH_INLINE Vec3 ToVec3RoundUp() const; + + /// Return the minimum value of each of the components + static JPH_INLINE DVec3 sMin(DVec3Arg inV1, DVec3Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE DVec3 sMax(DVec3Arg inV1, DVec3Arg inV2); + + /// Clamp a vector between min and max (component wise) + static JPH_INLINE DVec3 sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax); + + /// Equals (component wise) + static JPH_INLINE DVec3 sEquals(DVec3Arg inV1, DVec3Arg inV2); + + /// Less than (component wise) + static JPH_INLINE DVec3 sLess(DVec3Arg inV1, DVec3Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE DVec3 sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE DVec3 sGreater(DVec3Arg inV1, DVec3Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE DVec3 sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE DVec3 sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd); + + /// Component wise select, returns inV1 when highest bit of inControl = 0 and inV2 when highest bit of inControl = 1 + static JPH_INLINE DVec3 sSelect(DVec3Arg inV1, DVec3Arg inV2, DVec3Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE DVec3 sOr(DVec3Arg inV1, DVec3Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE DVec3 sXor(DVec3Arg inV1, DVec3Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE DVec3 sAnd(DVec3Arg inV1, DVec3Arg inV2); + + /// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Get individual components +#if defined(JPH_USE_AVX) + JPH_INLINE double GetX() const { return _mm_cvtsd_f64(_mm256_castpd256_pd128(mValue)); } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return mF64[2]; } +#elif defined(JPH_USE_SSE) + JPH_INLINE double GetX() const { return _mm_cvtsd_f64(mValue.mLow); } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return _mm_cvtsd_f64(mValue.mHigh); } +#elif defined(JPH_USE_NEON) + JPH_INLINE double GetX() const { return vgetq_lane_f64(mValue.val[0], 0); } + JPH_INLINE double GetY() const { return vgetq_lane_f64(mValue.val[0], 1); } + JPH_INLINE double GetZ() const { return vgetq_lane_f64(mValue.val[1], 0); } +#else + JPH_INLINE double GetX() const { return mF64[0]; } + JPH_INLINE double GetY() const { return mF64[1]; } + JPH_INLINE double GetZ() const { return mF64[2]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(double inX) { mF64[0] = inX; } + JPH_INLINE void SetY(double inY) { mF64[1] = inY; } + JPH_INLINE void SetZ(double inZ) { mF64[2] = mF64[3] = inZ; } // Assure Z and W are the same + + /// Set all components + JPH_INLINE void Set(double inX, double inY, double inZ) { *this = DVec3(inX, inY, inZ); } + + /// Get double component by index + JPH_INLINE double operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 3); return mF64[inCoordinate]; } + + /// Set double component by index + JPH_INLINE void SetComponent(uint inCoordinate, double inValue) { JPH_ASSERT(inCoordinate < 3); mF64[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same + + /// Comparison + JPH_INLINE bool operator == (DVec3Arg inV2) const; + JPH_INLINE bool operator != (DVec3Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(DVec3Arg inV2, double inMaxDistSq = 1.0e-24) const; + + /// Test if vector is near zero + JPH_INLINE bool IsNearZero(double inMaxDistSq = 1.0e-24) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(double inTolerance = 1.0e-12) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two double vectors (component wise) + JPH_INLINE DVec3 operator * (DVec3Arg inV2) const; + + /// Multiply vector with double + JPH_INLINE DVec3 operator * (double inV2) const; + + /// Multiply vector with double + friend JPH_INLINE DVec3 operator * (double inV1, DVec3Arg inV2); + + /// Divide vector by double + JPH_INLINE DVec3 operator / (double inV2) const; + + /// Multiply vector with double + JPH_INLINE DVec3 & operator *= (double inV2); + + /// Multiply vector with vector + JPH_INLINE DVec3 & operator *= (DVec3Arg inV2); + + /// Divide vector by double + JPH_INLINE DVec3 & operator /= (double inV2); + + /// Add two vectors (component wise) + JPH_INLINE DVec3 operator + (Vec3Arg inV2) const; + + /// Add two double vectors (component wise) + JPH_INLINE DVec3 operator + (DVec3Arg inV2) const; + + /// Add two vectors (component wise) + JPH_INLINE DVec3 & operator += (Vec3Arg inV2); + + /// Add two double vectors (component wise) + JPH_INLINE DVec3 & operator += (DVec3Arg inV2); + + /// Negate + JPH_INLINE DVec3 operator - () const; + + /// Subtract two vectors (component wise) + JPH_INLINE DVec3 operator - (Vec3Arg inV2) const; + + /// Subtract two double vectors (component wise) + JPH_INLINE DVec3 operator - (DVec3Arg inV2) const; + + /// Add two vectors (component wise) + JPH_INLINE DVec3 & operator -= (Vec3Arg inV2); + + /// Add two double vectors (component wise) + JPH_INLINE DVec3 & operator -= (DVec3Arg inV2); + + /// Divide (component wise) + JPH_INLINE DVec3 operator / (DVec3Arg inV2) const; + + /// Return the absolute value of each of the components + JPH_INLINE DVec3 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE DVec3 Reciprocal() const; + + /// Cross product + JPH_INLINE DVec3 Cross(DVec3Arg inV2) const; + + /// Dot product + JPH_INLINE double Dot(DVec3Arg inV2) const; + + /// Squared length of vector + JPH_INLINE double LengthSq() const; + + /// Length of vector + JPH_INLINE double Length() const; + + /// Normalize vector + JPH_INLINE DVec3 Normalized() const; + + /// Component wise square root + JPH_INLINE DVec3 Sqrt() const; + + /// Get vector that contains the sign of each element (returns 1 if positive, -1 if negative) + JPH_INLINE DVec3 GetSign() const; + + /// To String + friend ostream & operator << (ostream &inStream, DVec3Arg inV) + { + inStream << inV.mF64[0] << ", " << inV.mF64[1] << ", " << inV.mF64[2]; + return inStream; + } + + /// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0 + JPH_INLINE void CheckW() const; + + /// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero + static JPH_INLINE Type sFixW(TypeArg inValue); + + /// Representations of true and false for boolean operations + inline static const double cTrue = BitCast(~uint64(0)); + inline static const double cFalse = 0.0; + + union + { + Type mValue; + double mF64[4]; + }; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "DVec3.inl" diff --git a/WickedEngine/Jolt/Math/DVec3.inl b/WickedEngine/Jolt/Math/DVec3.inl new file mode 100644 index 000000000..5b7603e69 --- /dev/null +++ b/WickedEngine/Jolt/Math/DVec3.inl @@ -0,0 +1,930 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// Create a std::hash for DVec3 +JPH_MAKE_HASHABLE(JPH::DVec3, t.GetX(), t.GetY(), t.GetZ()) + +JPH_NAMESPACE_BEGIN + +DVec3::DVec3(Vec3Arg inRHS) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_cvtps_pd(inRHS.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_cvtps_pd(inRHS.mValue); + mValue.mHigh = _mm_cvtps_pd(_mm_shuffle_ps(inRHS.mValue, inRHS.mValue, _MM_SHUFFLE(2, 2, 2, 2))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vcvt_f64_f32(vget_low_f32(inRHS.mValue)); + mValue.val[1] = vcvt_high_f64_f32(inRHS.mValue); +#else + mF64[0] = (double)inRHS.GetX(); + mF64[1] = (double)inRHS.GetY(); + mF64[2] = (double)inRHS.GetZ(); + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +DVec3::DVec3(Vec4Arg inRHS) : + DVec3(Vec3(inRHS)) +{ +} + +DVec3::DVec3(double inX, double inY, double inZ) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_set_pd(inZ, inZ, inY, inX); // Assure Z and W are the same +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_set_pd(inY, inX); + mValue.mHigh = _mm_set1_pd(inZ); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vcombine_f64(vcreate_f64(*reinterpret_cast(&inX)), vcreate_f64(*reinterpret_cast(&inY))); + mValue.val[1] = vdupq_n_f64(inZ); +#else + mF64[0] = inX; + mF64[1] = inY; + mF64[2] = inZ; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +DVec3::DVec3(const Double3 &inV) +{ +#if defined(JPH_USE_AVX) + Type x = _mm256_castpd128_pd256(_mm_load_sd(&inV.x)); + Type y = _mm256_castpd128_pd256(_mm_load_sd(&inV.y)); + Type z = _mm256_broadcast_sd(&inV.z); + Type xy = _mm256_unpacklo_pd(x, y); + mValue = _mm256_blend_pd(xy, z, 0b1100); // Assure Z and W are the same +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_loadu_pd(&inV.x); + mValue.mHigh = _mm_set1_pd(inV.z); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vld1q_f64(&inV.x); + mValue.val[1] = vdupq_n_f64(inV.z); +#else + mF64[0] = inV.x; + mF64[1] = inV.y; + mF64[2] = inV.z; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif +} + +void DVec3::CheckW() const +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + // Avoid asserts when both components are NaN + JPH_ASSERT(reinterpret_cast(mF64)[2] == reinterpret_cast(mF64)[3]); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +/// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero +DVec3::Type DVec3::sFixW(TypeArg inValue) +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #if defined(JPH_USE_AVX) + return _mm256_shuffle_pd(inValue, inValue, 2); + #elif defined(JPH_USE_SSE) + Type value; + value.mLow = inValue.mLow; + value.mHigh = _mm_shuffle_pd(inValue.mHigh, inValue.mHigh, 0); + return value; + #elif defined(JPH_USE_NEON) + Type value; + value.val[0] = inValue.val[0]; + value.val[1] = vdupq_laneq_f64(inValue.val[1], 0); + return value; + #else + Type value; + value.mData[0] = inValue.mData[0]; + value.mData[1] = inValue.mData[1]; + value.mData[2] = inValue.mData[2]; + value.mData[3] = inValue.mData[2]; + return value; + #endif +#else + return inValue; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +DVec3 DVec3::sZero() +{ +#if defined(JPH_USE_AVX) + return _mm256_setzero_pd(); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ zero, zero }); +#elif defined(JPH_USE_NEON) + float64x2_t zero = vdupq_n_f64(0.0); + return DVec3({ zero, zero }); +#else + return DVec3(0, 0, 0); +#endif +} + +DVec3 DVec3::sReplicate(double inV) +{ +#if defined(JPH_USE_AVX) + return _mm256_set1_pd(inV); +#elif defined(JPH_USE_SSE) + __m128d value = _mm_set1_pd(inV); + return DVec3({ value, value }); +#elif defined(JPH_USE_NEON) + float64x2_t value = vdupq_n_f64(inV); + return DVec3({ value, value }); +#else + return DVec3(inV, inV, inV); +#endif +} + +DVec3 DVec3::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +DVec3 DVec3::sLoadDouble3Unsafe(const Double3 &inV) +{ +#if defined(JPH_USE_AVX) + Type v = _mm256_loadu_pd(&inV.x); +#elif defined(JPH_USE_SSE) + Type v; + v.mLow = _mm_loadu_pd(&inV.x); + v.mHigh = _mm_set1_pd(inV.z); +#elif defined(JPH_USE_NEON) + Type v = vld1q_f64_x2(&inV.x); +#else + Type v = { inV.x, inV.y, inV.z }; +#endif + return sFixW(v); +} + +void DVec3::StoreDouble3(Double3 *outV) const +{ + outV->x = mF64[0]; + outV->y = mF64[1]; + outV->z = mF64[2]; +} + +DVec3::operator Vec3() const +{ +#if defined(JPH_USE_AVX) + return _mm256_cvtpd_ps(mValue); +#elif defined(JPH_USE_SSE) + __m128 low = _mm_cvtpd_ps(mValue.mLow); + __m128 high = _mm_cvtpd_ps(mValue.mHigh); + return _mm_shuffle_ps(low, high, _MM_SHUFFLE(1, 0, 1, 0)); +#elif defined(JPH_USE_NEON) + return vcvt_high_f32_f64(vcvtx_f32_f64(mValue.val[0]), mValue.val[1]); +#else + return Vec3((float)GetX(), (float)GetY(), (float)GetZ()); +#endif +} + +DVec3 DVec3::sMin(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_min_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_min_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_min_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vminq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vminq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(min(inV1.mF64[0], inV2.mF64[0]), + min(inV1.mF64[1], inV2.mF64[1]), + min(inV1.mF64[2], inV2.mF64[2])); +#endif +} + +DVec3 DVec3::sMax(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_max_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_max_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_max_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmaxq_f64(inV1.mValue.val[0], inV2.mValue.val[0]), vmaxq_f64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(max(inV1.mF64[0], inV2.mF64[0]), + max(inV1.mF64[1], inV2.mF64[1]), + max(inV1.mF64[2], inV2.mF64[2])); +#endif +} + +DVec3 DVec3::sClamp(DVec3Arg inV, DVec3Arg inMin, DVec3Arg inMax) +{ + return sMax(sMin(inV, inMax), inMin); +} + +DVec3 DVec3::sEquals(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_EQ_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpeq_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpeq_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_u64_f64(vceqq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_u64_f64(vceqq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] == inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] == inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] == inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sLess(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LT_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmplt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmplt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_u64_f64(vcltq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_u64_f64(vcltq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] < inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] < inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] < inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sLessOrEqual(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_LE_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmple_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmple_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_u64_f64(vcleq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_u64_f64(vcleq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] <= inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] <= inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] <= inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sGreater(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GT_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpgt_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpgt_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_u64_f64(vcgtq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_u64_f64(vcgtq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] > inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] > inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] > inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sGreaterOrEqual(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_cmp_pd(inV1.mValue, inV2.mValue, _CMP_GE_OQ); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_cmpge_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_cmpge_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vreinterpretq_u64_f64(vcgeq_f64(inV1.mValue.val[0], inV2.mValue.val[0])), vreinterpretq_u64_f64(vcgeq_f64(inV1.mValue.val[1], inV2.mValue.val[1])) }); +#else + return DVec3(inV1.mF64[0] >= inV2.mF64[0]? cTrue : cFalse, + inV1.mF64[1] >= inV2.mF64[1]? cTrue : cFalse, + inV1.mF64[2] >= inV2.mF64[2]? cTrue : cFalse); +#endif +} + +DVec3 DVec3::sFusedMultiplyAdd(DVec3Arg inMul1, DVec3Arg inMul2, DVec3Arg inAdd) +{ +#if defined(JPH_USE_AVX) + #ifdef JPH_USE_FMADD + return _mm256_fmadd_pd(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm256_add_pd(_mm256_mul_pd(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return DVec3({ vmlaq_f64(inAdd.mValue.val[0], inMul1.mValue.val[0], inMul2.mValue.val[0]), vmlaq_f64(inAdd.mValue.val[1], inMul1.mValue.val[1], inMul2.mValue.val[1]) }); +#else + return inMul1 * inMul2 + inAdd; +#endif +} + +DVec3 DVec3::sSelect(DVec3Arg inV1, DVec3Arg inV2, DVec3Arg inControl) +{ +#if defined(JPH_USE_AVX) + return _mm256_blendv_pd(inV1.mValue, inV2.mValue, inControl.mValue); +#elif defined(JPH_USE_SSE4_1) + Type v = { _mm_blendv_pd(inV1.mValue.mLow, inV2.mValue.mLow, inControl.mValue.mLow), _mm_blendv_pd(inV1.mValue.mHigh, inV2.mValue.mHigh, inControl.mValue.mHigh) }; + return sFixW(v); +#elif defined(JPH_USE_NEON) + Type v = { vbslq_f64(vshrq_n_s64(inControl.mValue.val[0], 63), inV2.mValue.val[0], inV1.mValue.val[0]), vbslq_f64(vshrq_n_s64(inControl.mValue.val[1], 63), inV2.mValue.val[1], inV1.mValue.val[1]) }; + return sFixW(v); +#else + DVec3 result; + for (int i = 0; i < 3; i++) + result.mF64[i] = BitCast(inControl.mF64[i])? inV2.mF64[i] : inV1.mF64[i]; +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + result.mF64[3] = result.mF64[2]; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + return result; +#endif +} + +DVec3 DVec3::sOr(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_or_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_or_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_or_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vorrq_s64(inV1.mValue.val[0], inV2.mValue.val[0]), vorrq_s64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) | BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) | BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) | BitCast(inV2.mF64[2]))); +#endif +} + +DVec3 DVec3::sXor(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_xor_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_xor_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_xor_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ veorq_s64(inV1.mValue.val[0], inV2.mValue.val[0]), veorq_s64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) ^ BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) ^ BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) ^ BitCast(inV2.mF64[2]))); +#endif +} + +DVec3 DVec3::sAnd(DVec3Arg inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_and_pd(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_and_pd(inV1.mValue.mLow, inV2.mValue.mLow), _mm_and_pd(inV1.mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vandq_s64(inV1.mValue.val[0], inV2.mValue.val[0]), vandq_s64(inV1.mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(BitCast(BitCast(inV1.mF64[0]) & BitCast(inV2.mF64[0])), + BitCast(BitCast(inV1.mF64[1]) & BitCast(inV2.mF64[1])), + BitCast(BitCast(inV1.mF64[2]) & BitCast(inV2.mF64[2]))); +#endif +} + +int DVec3::GetTrues() const +{ +#if defined(JPH_USE_AVX) + return _mm256_movemask_pd(mValue) & 0x7; +#elif defined(JPH_USE_SSE) + return (_mm_movemask_pd(mValue.mLow) + (_mm_movemask_pd(mValue.mHigh) << 2)) & 0x7; +#else + return int((BitCast(mF64[0]) >> 63) | ((BitCast(mF64[1]) >> 63) << 1) | ((BitCast(mF64[2]) >> 63) << 2)); +#endif +} + +bool DVec3::TestAnyTrue() const +{ + return GetTrues() != 0; +} + +bool DVec3::TestAllTrue() const +{ + return GetTrues() == 0x7; +} + +bool DVec3::operator == (DVec3Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +bool DVec3::IsClose(DVec3Arg inV2, double inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool DVec3::IsNearZero(double inMaxDistSq) const +{ + return LengthSq() <= inMaxDistSq; +} + +DVec3 DVec3::operator * (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_mul_pd(mValue.mLow, inV2.mValue.mLow), _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_f64(mValue.val[0], inV2.mValue.val[0]), vmulq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] * inV2.mF64[0], mF64[1] * inV2.mF64[1], mF64[2] * inV2.mF64[2]); +#endif +} + +DVec3 DVec3::operator * (double inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + return DVec3({ _mm_mul_pd(mValue.mLow, v), _mm_mul_pd(mValue.mHigh, v) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_n_f64(mValue.val[0], inV2), vmulq_n_f64(mValue.val[1], inV2) }); +#else + return DVec3(mF64[0] * inV2, mF64[1] * inV2, mF64[2] * inV2); +#endif +} + +DVec3 operator * (double inV1, DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + return _mm256_mul_pd(_mm256_set1_pd(inV1), inV2.mValue); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV1); + return DVec3({ _mm_mul_pd(v, inV2.mValue.mLow), _mm_mul_pd(v, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vmulq_n_f64(inV2.mValue.val[0], inV1), vmulq_n_f64(inV2.mValue.val[1], inV1) }); +#else + return DVec3(inV1 * inV2.mF64[0], inV1 * inV2.mF64[1], inV1 * inV2.mF64[2]); +#endif +} + +DVec3 DVec3::operator / (double inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_div_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + return DVec3({ _mm_div_pd(mValue.mLow, v), _mm_div_pd(mValue.mHigh, v) }); +#elif defined(JPH_USE_NEON) + float64x2_t v = vdupq_n_f64(inV2); + return DVec3({ vdivq_f64(mValue.val[0], v), vdivq_f64(mValue.val[1], v) }); +#else + return DVec3(mF64[0] / inV2, mF64[1] / inV2, mF64[2] / inV2); +#endif +} + +DVec3 &DVec3::operator *= (double inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_mul_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + mValue.mLow = _mm_mul_pd(mValue.mLow, v); + mValue.mHigh = _mm_mul_pd(mValue.mHigh, v); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vmulq_n_f64(mValue.val[0], inV2); + mValue.val[1] = vmulq_n_f64(mValue.val[1], inV2); +#else + for (int i = 0; i < 3; ++i) + mF64[i] *= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator *= (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_mul_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_mul_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vmulq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vmulq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] *= inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator /= (double inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_div_pd(mValue, _mm256_set1_pd(inV2)); +#elif defined(JPH_USE_SSE) + __m128d v = _mm_set1_pd(inV2); + mValue.mLow = _mm_div_pd(mValue.mLow, v); + mValue.mHigh = _mm_div_pd(mValue.mHigh, v); +#elif defined(JPH_USE_NEON) + float64x2_t v = vdupq_n_f64(inV2); + mValue.val[0] = vdivq_f64(mValue.val[0], v); + mValue.val[1] = vdivq_f64(mValue.val[1], v); +#else + for (int i = 0; i < 3; ++i) + mF64[i] /= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator + (Vec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) }); +#else + return DVec3(mF64[0] + inV2.mF32[0], mF64[1] + inV2.mF32[1], mF64[2] + inV2.mF32[2]); +#endif +} + +DVec3 DVec3::operator + (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_add_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_add_pd(mValue.mLow, inV2.mValue.mLow), _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vaddq_f64(mValue.val[0], inV2.mValue.val[0]), vaddq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] + inV2.mF64[0], mF64[1] + inV2.mF64[1], mF64[2] + inV2.mF64[2]); +#endif +} + +DVec3 &DVec3::operator += (Vec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_add_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_add_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)); + mValue.mHigh = _mm_add_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vaddq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))); + mValue.val[1] = vaddq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)); +#else + for (int i = 0; i < 3; ++i) + mF64[i] += inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator += (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_add_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_add_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_add_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vaddq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vaddq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] += inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator - () const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(_mm256_setzero_pd(), mValue); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ _mm_sub_pd(zero, mValue.mLow), _mm_sub_pd(zero, mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + float64x2_t zero = vdupq_n_f64(0); + return DVec3({ vsubq_f64(zero, mValue.val[0]), vsubq_f64(zero, mValue.val[1]) }); + #else + return DVec3({ vnegq_f64(mValue.val[0]), vnegq_f64(mValue.val[1]) }); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return DVec3(0.0 - mF64[0], 0.0 - mF64[1], 0.0 - mF64[2]); + #else + return DVec3(-mF64[0], -mF64[1], -mF64[2]); + #endif +#endif +} + +DVec3 DVec3::operator - (Vec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)), _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))), vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)) }); +#else + return DVec3(mF64[0] - inV2.mF32[0], mF64[1] - inV2.mF32[1], mF64[2] - inV2.mF32[2]); +#endif +} + +DVec3 DVec3::operator - (DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + return _mm256_sub_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sub_pd(mValue.mLow, inV2.mValue.mLow), _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsubq_f64(mValue.val[0], inV2.mValue.val[0]), vsubq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] - inV2.mF64[0], mF64[1] - inV2.mF64[1], mF64[2] - inV2.mF64[2]); +#endif +} + +DVec3 &DVec3::operator -= (Vec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_sub_pd(mValue, _mm256_cvtps_pd(inV2.mValue)); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_sub_pd(mValue.mLow, _mm_cvtps_pd(inV2.mValue)); + mValue.mHigh = _mm_sub_pd(mValue.mHigh, _mm_cvtps_pd(_mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vsubq_f64(mValue.val[0], vcvt_f64_f32(vget_low_f32(inV2.mValue))); + mValue.val[1] = vsubq_f64(mValue.val[1], vcvt_high_f64_f32(inV2.mValue)); +#else + for (int i = 0; i < 3; ++i) + mF64[i] -= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 &DVec3::operator -= (DVec3Arg inV2) +{ +#if defined(JPH_USE_AVX) + mValue = _mm256_sub_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + mValue.mLow = _mm_sub_pd(mValue.mLow, inV2.mValue.mLow); + mValue.mHigh = _mm_sub_pd(mValue.mHigh, inV2.mValue.mHigh); +#elif defined(JPH_USE_NEON) + mValue.val[0] = vsubq_f64(mValue.val[0], inV2.mValue.val[0]); + mValue.val[1] = vsubq_f64(mValue.val[1], inV2.mValue.val[1]); +#else + for (int i = 0; i < 3; ++i) + mF64[i] -= inV2.mF64[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF64[3] = mF64[2]; + #endif +#endif + return *this; +} + +DVec3 DVec3::operator / (DVec3Arg inV2) const +{ + inV2.CheckW(); +#if defined(JPH_USE_AVX) + return _mm256_div_pd(mValue, inV2.mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_div_pd(mValue.mLow, inV2.mValue.mLow), _mm_div_pd(mValue.mHigh, inV2.mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vdivq_f64(mValue.val[0], inV2.mValue.val[0]), vdivq_f64(mValue.val[1], inV2.mValue.val[1]) }); +#else + return DVec3(mF64[0] / inV2.mF64[0], mF64[1] / inV2.mF64[1], mF64[2] / inV2.mF64[2]); +#endif +} + +DVec3 DVec3::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm256_range_pd(mValue, mValue, 0b1000); +#elif defined(JPH_USE_AVX) + return _mm256_max_pd(_mm256_sub_pd(_mm256_setzero_pd(), mValue), mValue); +#elif defined(JPH_USE_SSE) + __m128d zero = _mm_setzero_pd(); + return DVec3({ _mm_max_pd(_mm_sub_pd(zero, mValue.mLow), mValue.mLow), _mm_max_pd(_mm_sub_pd(zero, mValue.mHigh), mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vabsq_f64(mValue.val[0]), vabsq_f64(mValue.val[1]) }); +#else + return DVec3(abs(mF64[0]), abs(mF64[1]), abs(mF64[2])); +#endif +} + +DVec3 DVec3::Reciprocal() const +{ + return sReplicate(1.0) / mValue; +} + +DVec3 DVec3::Cross(DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX2) + __m256d t1 = _mm256_permute4x64_pd(inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t1 = _mm256_mul_pd(t1, mValue); + __m256d t2 = _mm256_permute4x64_pd(mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t2 = _mm256_mul_pd(t2, inV2.mValue); + __m256d t3 = _mm256_sub_pd(t1, t2); + return _mm256_permute4x64_pd(t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same +#else + return DVec3(mF64[1] * inV2.mF64[2] - mF64[2] * inV2.mF64[1], + mF64[2] * inV2.mF64[0] - mF64[0] * inV2.mF64[2], + mF64[0] * inV2.mF64[1] - mF64[1] * inV2.mF64[0]); +#endif +} + +double DVec3::Dot(DVec3Arg inV2) const +{ +#if defined(JPH_USE_AVX) + __m256d mul = _mm256_mul_pd(mValue, inV2.mValue); + __m128d xy = _mm256_castpd256_pd128(mul); + __m128d yx = _mm_shuffle_pd(xy, xy, 1); + __m128d sum = _mm_add_pd(xy, yx); + __m128d zw = _mm256_extractf128_pd(mul, 1); + sum = _mm_add_pd(sum, zw); + return _mm_cvtsd_f64(sum); +#elif defined(JPH_USE_SSE) + __m128d xy = _mm_mul_pd(mValue.mLow, inV2.mValue.mLow); + __m128d yx = _mm_shuffle_pd(xy, xy, 1); + __m128d sum = _mm_add_pd(xy, yx); + __m128d z = _mm_mul_sd(mValue.mHigh, inV2.mValue.mHigh); + sum = _mm_add_pd(sum, z); + return _mm_cvtsd_f64(sum); +#elif defined(JPH_USE_NEON) + float64x2_t mul_low = vmulq_f64(mValue.val[0], inV2.mValue.val[0]); + float64x2_t mul_high = vmulq_f64(mValue.val[1], inV2.mValue.val[1]); + return vaddvq_f64(mul_low) + vgetq_lane_f64(mul_high, 0); +#else + double dot = 0.0; + for (int i = 0; i < 3; i++) + dot += mF64[i] * inV2.mF64[i]; + return dot; +#endif +} + +double DVec3::LengthSq() const +{ + return Dot(*this); +} + +DVec3 DVec3::Sqrt() const +{ +#if defined(JPH_USE_AVX) + return _mm256_sqrt_pd(mValue); +#elif defined(JPH_USE_SSE) + return DVec3({ _mm_sqrt_pd(mValue.mLow), _mm_sqrt_pd(mValue.mHigh) }); +#elif defined(JPH_USE_NEON) + return DVec3({ vsqrtq_f64(mValue.val[0]), vsqrtq_f64(mValue.val[1]) }); +#else + return DVec3(sqrt(mF64[0]), sqrt(mF64[1]), sqrt(mF64[2])); +#endif +} + +double DVec3::Length() const +{ + return sqrt(Dot(*this)); +} + +DVec3 DVec3::Normalized() const +{ + return *this / Length(); +} + +bool DVec3::IsNormalized(double inTolerance) const +{ + return abs(LengthSq() - 1.0) <= inTolerance; +} + +bool DVec3::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return (_mm256_fpclass_pd_mask(mValue, 0b10000001) & 0x7) != 0; +#elif defined(JPH_USE_AVX) + return (_mm256_movemask_pd(_mm256_cmp_pd(mValue, mValue, _CMP_UNORD_Q)) & 0x7) != 0; +#elif defined(JPH_USE_SSE) + return ((_mm_movemask_pd(_mm_cmpunord_pd(mValue.mLow, mValue.mLow)) + (_mm_movemask_pd(_mm_cmpunord_pd(mValue.mHigh, mValue.mHigh)) << 2)) & 0x7) != 0; +#else + return isnan(mF64[0]) || isnan(mF64[1]) || isnan(mF64[2]); +#endif +} + +DVec3 DVec3::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm256_fixupimm_pd(mValue, mValue, _mm256_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_AVX) + __m256d minus_one = _mm256_set1_pd(-1.0); + __m256d one = _mm256_set1_pd(1.0); + return _mm256_or_pd(_mm256_and_pd(mValue, minus_one), one); +#elif defined(JPH_USE_SSE) + __m128d minus_one = _mm_set1_pd(-1.0); + __m128d one = _mm_set1_pd(1.0); + return DVec3({ _mm_or_pd(_mm_and_pd(mValue.mLow, minus_one), one), _mm_or_pd(_mm_and_pd(mValue.mHigh, minus_one), one) }); +#elif defined(JPH_USE_NEON) + float64x2_t minus_one = vdupq_n_f64(-1.0f); + float64x2_t one = vdupq_n_f64(1.0f); + return DVec3({ vorrq_s64(vandq_s64(mValue.val[0], minus_one), one), vorrq_s64(vandq_s64(mValue.val[1], minus_one), one) }); +#else + return DVec3(std::signbit(mF64[0])? -1.0 : 1.0, + std::signbit(mF64[1])? -1.0 : 1.0, + std::signbit(mF64[2])? -1.0 : 1.0); +#endif +} + +DVec3 DVec3::PrepareRoundToZero() const +{ + // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float + constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1; + +#if defined(JPH_USE_AVX) + return _mm256_and_pd(mValue, _mm256_castsi256_pd(_mm256_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss)))); +#elif defined(JPH_USE_SSE) + __m128d mask = _mm_castsi128_pd(_mm_set1_epi64x(int64_t(~cDoubleToFloatMantissaLoss))); + return DVec3({ _mm_and_pd(mValue.mLow, mask), _mm_and_pd(mValue.mHigh, mask) }); +#elif defined(JPH_USE_NEON) + float64x2_t mask = vreinterpretq_f64_u64(vdupq_n_u64(~cDoubleToFloatMantissaLoss)); + return DVec3({ vandq_s64(mValue.val[0], mask), vandq_s64(mValue.val[1], mask) }); +#else + double x = BitCast(BitCast(mF64[0]) & ~cDoubleToFloatMantissaLoss); + double y = BitCast(BitCast(mF64[1]) & ~cDoubleToFloatMantissaLoss); + double z = BitCast(BitCast(mF64[2]) & ~cDoubleToFloatMantissaLoss); + + return DVec3(x, y, z); +#endif +} + +DVec3 DVec3::PrepareRoundToInf() const +{ + // Float has 23 bit mantissa, double 52 bit mantissa => we lose 29 bits when converting from double to float + constexpr uint64 cDoubleToFloatMantissaLoss = (1U << 29) - 1; + +#if defined(JPH_USE_AVX512) + __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss); + __mmask8 is_zero = _mm256_testn_epi64_mask(_mm256_castpd_si256(mValue), mantissa_loss); + __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + return _mm256_mask_blend_pd(is_zero, value_or_mantissa_loss, mValue); +#elif defined(JPH_USE_AVX) + __m256i mantissa_loss = _mm256_set1_epi64x(cDoubleToFloatMantissaLoss); + __m256d value_and_mantissa_loss = _mm256_and_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + __m256d is_zero = _mm256_cmp_pd(value_and_mantissa_loss, _mm256_setzero_pd(), _CMP_EQ_OQ); + __m256d value_or_mantissa_loss = _mm256_or_pd(mValue, _mm256_castsi256_pd(mantissa_loss)); + return _mm256_blendv_pd(value_or_mantissa_loss, mValue, is_zero); +#elif defined(JPH_USE_SSE4_1) + __m128i mantissa_loss = _mm_set1_epi64x(cDoubleToFloatMantissaLoss); + __m128d zero = _mm_setzero_pd(); + __m128d value_and_mantissa_loss_low = _mm_and_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss)); + __m128d is_zero_low = _mm_cmpeq_pd(value_and_mantissa_loss_low, zero); + __m128d value_or_mantissa_loss_low = _mm_or_pd(mValue.mLow, _mm_castsi128_pd(mantissa_loss)); + __m128d value_and_mantissa_loss_high = _mm_and_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss)); + __m128d is_zero_high = _mm_cmpeq_pd(value_and_mantissa_loss_high, zero); + __m128d value_or_mantissa_loss_high = _mm_or_pd(mValue.mHigh, _mm_castsi128_pd(mantissa_loss)); + return DVec3({ _mm_blendv_pd(value_or_mantissa_loss_low, mValue.mLow, is_zero_low), _mm_blendv_pd(value_or_mantissa_loss_high, mValue.mHigh, is_zero_high) }); +#elif defined(JPH_USE_NEON) + float64x2_t mantissa_loss = vreinterpretq_f64_u64(vdupq_n_u64(cDoubleToFloatMantissaLoss)); + float64x2_t zero = vdupq_n_f64(0.0); + float64x2_t value_and_mantissa_loss_low = vandq_s64(mValue.val[0], mantissa_loss); + float64x2_t is_zero_low = vceqq_f64(value_and_mantissa_loss_low, zero); + float64x2_t value_or_mantissa_loss_low = vorrq_s64(mValue.val[0], mantissa_loss); + float64x2_t value_and_mantissa_loss_high = vandq_s64(mValue.val[1], mantissa_loss); + float64x2_t value_low = vbslq_f64(is_zero_low, mValue.val[0], value_or_mantissa_loss_low); + float64x2_t is_zero_high = vceqq_f64(value_and_mantissa_loss_high, zero); + float64x2_t value_or_mantissa_loss_high = vorrq_s64(mValue.val[1], mantissa_loss); + float64x2_t value_high = vbslq_f64(is_zero_high, mValue.val[1], value_or_mantissa_loss_high); + return DVec3({ value_low, value_high }); +#else + uint64 ux = BitCast(mF64[0]); + uint64 uy = BitCast(mF64[1]); + uint64 uz = BitCast(mF64[2]); + + double x = BitCast((ux & cDoubleToFloatMantissaLoss) == 0? ux : (ux | cDoubleToFloatMantissaLoss)); + double y = BitCast((uy & cDoubleToFloatMantissaLoss) == 0? uy : (uy | cDoubleToFloatMantissaLoss)); + double z = BitCast((uz & cDoubleToFloatMantissaLoss) == 0? uz : (uz | cDoubleToFloatMantissaLoss)); + + return DVec3(x, y, z); +#endif +} + +Vec3 DVec3::ToVec3RoundDown() const +{ + DVec3 to_zero = PrepareRoundToZero(); + DVec3 to_inf = PrepareRoundToInf(); + return Vec3(DVec3::sSelect(to_zero, to_inf, DVec3::sLess(*this, DVec3::sZero()))); +} + +Vec3 DVec3::ToVec3RoundUp() const +{ + DVec3 to_zero = PrepareRoundToZero(); + DVec3 to_inf = PrepareRoundToInf(); + return Vec3(DVec3::sSelect(to_inf, to_zero, DVec3::sLess(*this, DVec3::sZero()))); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Double3.h b/WickedEngine/Jolt/Math/Double3.h new file mode 100644 index 000000000..90d3c1642 --- /dev/null +++ b/WickedEngine/Jolt/Math/Double3.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that holds 3 doubles. Used as a storage class. Convert to DVec3 for calculations. +class [[nodiscard]] Double3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Double3() = default; ///< Intentionally not initialized for performance reasons + Double3(const Double3 &inRHS) = default; + Double3 & operator = (const Double3 &inRHS) = default; + Double3(double inX, double inY, double inZ) : x(inX), y(inY), z(inZ) { } + + double operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 3); + return *(&x + inCoordinate); + } + + bool operator == (const Double3 &inRHS) const + { + return x == inRHS.x && y == inRHS.y && z == inRHS.z; + } + + bool operator != (const Double3 &inRHS) const + { + return x != inRHS.x || y != inRHS.y || z != inRHS.z; + } + + double x; + double y; + double z; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +// Create a std::hash for Double3 +JPH_MAKE_HASHABLE(JPH::Double3, t.x, t.y, t.z) diff --git a/WickedEngine/Jolt/Math/DynMatrix.h b/WickedEngine/Jolt/Math/DynMatrix.h new file mode 100644 index 000000000..76db294c7 --- /dev/null +++ b/WickedEngine/Jolt/Math/DynMatrix.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Dynamic resizable matrix class +class [[nodiscard]] DynMatrix +{ +public: + /// Constructor + DynMatrix(const DynMatrix &) = default; + DynMatrix(uint inRows, uint inCols) : mRows(inRows), mCols(inCols) { mElements.resize(inRows * inCols); } + + /// Access an element + float operator () (uint inRow, uint inCol) const { JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; } + float & operator () (uint inRow, uint inCol) { JPH_ASSERT(inRow < mRows && inCol < mCols); return mElements[inRow * mCols + inCol]; } + + /// Get dimensions + uint GetCols() const { return mCols; } + uint GetRows() const { return mRows; } + +private: + uint mRows; + uint mCols; + Array mElements; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/EigenValueSymmetric.h b/WickedEngine/Jolt/Math/EigenValueSymmetric.h new file mode 100644 index 000000000..43436ee96 --- /dev/null +++ b/WickedEngine/Jolt/Math/EigenValueSymmetric.h @@ -0,0 +1,175 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Function to determine the eigen vectors and values of a N x N real symmetric matrix +/// by Jacobi transformations. This method is most suitable for N < 10. +/// +/// Taken and adapted from Numerical Recipies paragraph 11.1 +/// +/// An eigen vector is a vector v for which \f$A \: v = \lambda \: v\f$ +/// +/// Where: +/// A: A square matrix. +/// \f$\lambda\f$: a non-zero constant value. +/// +/// @see https://en.wikipedia.org/wiki/Eigenvalues_and_eigenvectors +/// +/// Matrix is a matrix type, which has dimensions N x N. +/// @param inMatrix is the matrix of which to return the eigenvalues and vectors +/// @param outEigVec will contain a matrix whose columns contain the normalized eigenvectors (must be identity before call) +/// @param outEigVal will contain the eigenvalues +template +bool EigenValueSymmetric(const Matrix &inMatrix, Matrix &outEigVec, Vector &outEigVal) +{ + // This algorithm works with very small numbers and can trigger invalid float exceptions when not flushing denormals + FPFlushDenormals flush_denormals; + (void)flush_denormals; + + // Maximum number of sweeps to make + const int cMaxSweeps = 50; + + // Get problem dimension + const uint n = inMatrix.GetRows(); + + // Make sure the dimensions are right + JPH_ASSERT(inMatrix.GetRows() == n); + JPH_ASSERT(inMatrix.GetCols() == n); + JPH_ASSERT(outEigVec.GetRows() == n); + JPH_ASSERT(outEigVec.GetCols() == n); + JPH_ASSERT(outEigVal.GetRows() == n); + JPH_ASSERT(outEigVec.IsIdentity()); + + // Get the matrix in a so we can mess with it + Matrix a = inMatrix; + + Vector b, z; + + for (uint ip = 0; ip < n; ++ip) + { + // Initialize b to diagonal of a + b[ip] = a(ip, ip); + + // Initialize output to diagonal of a + outEigVal[ip] = a(ip, ip); + + // Reset z + z[ip] = 0.0f; + } + + for (int sweep = 0; sweep < cMaxSweeps; ++sweep) + { + // Get the sum of the off-diagonal elements of a + float sm = 0.0f; + for (uint ip = 0; ip < n - 1; ++ip) + for (uint iq = ip + 1; iq < n; ++iq) + sm += abs(a(ip, iq)); + + // Normal return, convergence to machine underflow + if (sm == 0.0f) + { + // Sanity checks + #ifdef JPH_ENABLE_ASSERTS + for (uint c = 0; c < n; ++c) + { + // Check if the eigenvector is normalized + JPH_ASSERT(outEigVec.GetColumn(c).IsNormalized()); + + // Check if inMatrix * eigen_vector = eigen_value * eigen_vector + Vector mat_eigvec = inMatrix * outEigVec.GetColumn(c); + Vector eigval_eigvec = outEigVal[c] * outEigVec.GetColumn(c); + JPH_ASSERT(mat_eigvec.IsClose(eigval_eigvec, max(mat_eigvec.LengthSq(), eigval_eigvec.LengthSq()) * 1.0e-6f)); + } + #endif + + // Success + return true; + } + + // On the first three sweeps use a fraction of the sum of the off diagonal elements as threshold + float tresh = sweep < 4? 0.2f * sm / Square(n) : 0.0f; + + for (uint ip = 0; ip < n - 1; ++ip) + for (uint iq = ip + 1; iq < n; ++iq) + { + float g = 100.0f * abs(a(ip, iq)); + + // After four sweeps, skip the rotation if the off-diagonal element is small + if (sweep > 4 + && abs(outEigVal[ip]) + g == abs(outEigVal[ip]) + && abs(outEigVal[iq]) + g == abs(outEigVal[iq])) + { + a(ip, iq) = 0.0f; + } + else if (abs(a(ip, iq)) > tresh) + { + float h = outEigVal[iq] - outEigVal[ip]; + + float t; + if (abs(h) + g == abs(h)) + { + t = a(ip, iq) / h; + } + else + { + float theta = 0.5f * h / a(ip, iq); // Warning: Can become inf if a(ip, iq) too small + t = 1.0f / (abs(theta) + sqrt(1.0f + theta * theta)); // Warning: Squaring large value can make it inf + if (theta < 0.0f) t = -t; + } + + float c = 1.0f / sqrt(1.0f + t * t); + float s = t * c; + float tau = s / (1.0f + c); + h = t * a(ip, iq); + + a(ip, iq) = 0.0f; + + // !Modification from Numerical Recipes! + // h can become infinite due to numerical overflow, this only happens when a(ip, iq) is very small + // so we can safely set a(ip, iq) to zero and skip the rotation, see lines marked with 'Warning' above. + if (!isnan(h)) + { + z[ip] -= h; + z[iq] += h; + + outEigVal[ip] -= h; + outEigVal[iq] += h; + + #define JPH_EVS_ROTATE(a, i, j, k, l) \ + g = a(i, j), \ + h = a(k, l), \ + a(i, j) = g - s * (h + g * tau), \ + a(k, l) = h + s * (g - h * tau) + + uint j; + for (j = 0; j < ip; ++j) JPH_EVS_ROTATE(a, j, ip, j, iq); + for (j = ip + 1; j < iq; ++j) JPH_EVS_ROTATE(a, ip, j, j, iq); + for (j = iq + 1; j < n; ++j) JPH_EVS_ROTATE(a, ip, j, iq, j); + for (j = 0; j < n; ++j) JPH_EVS_ROTATE(outEigVec, j, ip, j, iq); + + #undef JPH_EVS_ROTATE + } + } + } + + // Update eigenvalues with the sum of ta_pq and reinitialize z + for (uint ip = 0; ip < n; ++ip) + { + b[ip] += z[ip]; + outEigVal[ip] = b[ip]; + z[ip] = 0.0f; + } + } + + // Failure + JPH_ASSERT(false, "Too many iterations"); + return false; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/FindRoot.h b/WickedEngine/Jolt/Math/FindRoot.h new file mode 100644 index 000000000..21fef9f61 --- /dev/null +++ b/WickedEngine/Jolt/Math/FindRoot.h @@ -0,0 +1,42 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Find the roots of \f$inA \: x^2 + inB \: x + inC = 0\f$. +/// @return The number of roots, actual roots in outX1 and outX2. +/// If number of roots returned is 1 then outX1 == outX2. +template +inline int FindRoot(const T inA, const T inB, const T inC, T &outX1, T &outX2) +{ + // Check if this is a linear equation + if (inA == T(0)) + { + // Check if this is a constant equation + if (inB == T(0)) + return 0; + + // Linear equation with 1 solution + outX1 = outX2 = -inC / inB; + return 1; + } + + // See Numerical Recipes in C, Chapter 5.6 Quadratic and Cubic Equations + T det = Square(inB) - T(4) * inA * inC; + if (det < T(0)) + return 0; + T q = (inB + Sign(inB) * sqrt(det)) / T(-2); + outX1 = q / inA; + if (q == T(0)) + { + outX2 = outX1; + return 1; + } + outX2 = inC / q; + return 2; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Float2.h b/WickedEngine/Jolt/Math/Float2.h new file mode 100644 index 000000000..9c05d4139 --- /dev/null +++ b/WickedEngine/Jolt/Math/Float2.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that holds 2 floats, used as a storage class mainly. +class [[nodiscard]] Float2 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float2() = default; ///< Intentionally not initialized for performance reasons + Float2(const Float2 &inRHS) = default; + Float2 & operator = (const Float2 &inRHS) = default; + Float2(float inX, float inY) : x(inX), y(inY) { } + + bool operator == (const Float2 &inRHS) const { return x == inRHS.x && y == inRHS.y; } + bool operator != (const Float2 &inRHS) const { return x != inRHS.x || y != inRHS.y; } + + /// To String + friend ostream & operator << (ostream &inStream, const Float2 &inV) + { + inStream << inV.x << ", " << inV.y; + return inStream; + } + + float x; + float y; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Float3.h b/WickedEngine/Jolt/Math/Float3.h new file mode 100644 index 000000000..e288201b9 --- /dev/null +++ b/WickedEngine/Jolt/Math/Float3.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that holds 3 floats. Used as a storage class. Convert to Vec3 for calculations. +class [[nodiscard]] Float3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float3() = default; ///< Intentionally not initialized for performance reasons + Float3(const Float3 &inRHS) = default; + Float3 & operator = (const Float3 &inRHS) = default; + constexpr Float3(float inX, float inY, float inZ) : x(inX), y(inY), z(inZ) { } + + float operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 3); + return *(&x + inCoordinate); + } + + bool operator == (const Float3 &inRHS) const + { + return x == inRHS.x && y == inRHS.y && z == inRHS.z; + } + + bool operator != (const Float3 &inRHS) const + { + return x != inRHS.x || y != inRHS.y || z != inRHS.z; + } + + float x; + float y; + float z; +}; + +using VertexList = Array; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +// Create a std::hash for Float3 +JPH_MAKE_HASHABLE(JPH::Float3, t.x, t.y, t.z) diff --git a/WickedEngine/Jolt/Math/Float4.h b/WickedEngine/Jolt/Math/Float4.h new file mode 100644 index 000000000..30845f638 --- /dev/null +++ b/WickedEngine/Jolt/Math/Float4.h @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Class that holds 4 float values. Convert to Vec4 to perform calculations. +class [[nodiscard]] Float4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + Float4() = default; ///< Intentionally not initialized for performance reasons + Float4(const Float4 &inRHS) = default; + Float4(float inX, float inY, float inZ, float inW) : x(inX), y(inY), z(inZ), w(inW) { } + + float operator [] (int inCoordinate) const + { + JPH_ASSERT(inCoordinate < 4); + return *(&x + inCoordinate); + } + + float x; + float y; + float z; + float w; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/GaussianElimination.h b/WickedEngine/Jolt/Math/GaussianElimination.h new file mode 100644 index 000000000..a2bfd38cb --- /dev/null +++ b/WickedEngine/Jolt/Math/GaussianElimination.h @@ -0,0 +1,102 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// This function performs Gauss-Jordan elimination to solve a matrix equation. +/// A must be an NxN matrix and B must be an NxM matrix forming the equation A * x = B +/// on output B will contain x and A will be destroyed. +/// +/// This code can be used for example to compute the inverse of a matrix. +/// Set A to the matrix to invert, set B to identity and let GaussianElimination solve +/// the equation, on return B will be the inverse of A. And A is destroyed. +/// +/// Taken and adapted from Numerical Recipies in C paragraph 2.1 +template +bool GaussianElimination(MatrixA &ioA, MatrixB &ioB, float inTolerance = 1.0e-16f) +{ + // Get problem dimensions + const uint n = ioA.GetCols(); + const uint m = ioB.GetCols(); + + // Check matrix requirement + JPH_ASSERT(ioA.GetRows() == n); + JPH_ASSERT(ioB.GetRows() == n); + + // Create array for bookkeeping on pivoting + int *ipiv = (int *)JPH_STACK_ALLOC(n * sizeof(int)); + memset(ipiv, 0, n * sizeof(int)); + + for (uint i = 0; i < n; ++i) + { + // Initialize pivot element as the diagonal + uint pivot_row = i, pivot_col = i; + + // Determine pivot element + float largest_element = 0.0f; + for (uint j = 0; j < n; ++j) + if (ipiv[j] != 1) + for (uint k = 0; k < n; ++k) + { + if (ipiv[k] == 0) + { + float element = abs(ioA(j, k)); + if (element >= largest_element) + { + largest_element = element; + pivot_row = j; + pivot_col = k; + } + } + else if (ipiv[k] > 1) + { + return false; + } + } + + // Mark this column as used + ++ipiv[pivot_col]; + + // Exchange rows when needed so that the pivot element is at ioA(pivot_col, pivot_col) instead of at ioA(pivot_row, pivot_col) + if (pivot_row != pivot_col) + { + for (uint j = 0; j < n; ++j) + swap(ioA(pivot_row, j), ioA(pivot_col, j)); + for (uint j = 0; j < m; ++j) + swap(ioB(pivot_row, j), ioB(pivot_col, j)); + } + + // Get diagonal element that we are about to set to 1 + float diagonal_element = ioA(pivot_col, pivot_col); + if (abs(diagonal_element) < inTolerance) + return false; + + // Divide the whole row by the pivot element, making ioA(pivot_col, pivot_col) = 1 + for (uint j = 0; j < n; ++j) + ioA(pivot_col, j) /= diagonal_element; + for (uint j = 0; j < m; ++j) + ioB(pivot_col, j) /= diagonal_element; + ioA(pivot_col, pivot_col) = 1.0f; + + // Next reduce the rows, except for the pivot one, + // after this step the pivot_col column is zero except for the pivot element which is 1 + for (uint j = 0; j < n; ++j) + if (j != pivot_col) + { + float element = ioA(j, pivot_col); + for (uint k = 0; k < n; ++k) + ioA(j, k) -= ioA(pivot_col, k) * element; + for (uint k = 0; k < m; ++k) + ioB(j, k) -= ioB(pivot_col, k) * element; + ioA(j, pivot_col) = 0.0f; + } + } + + // Success + return true; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/HalfFloat.h b/WickedEngine/Jolt/Math/HalfFloat.h new file mode 100644 index 000000000..d28071e30 --- /dev/null +++ b/WickedEngine/Jolt/Math/HalfFloat.h @@ -0,0 +1,204 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +using HalfFloat = uint16; + +// Define half float constant values +static constexpr HalfFloat HALF_FLT_MAX = 0x7bff; +static constexpr HalfFloat HALF_FLT_MAX_NEGATIVE = 0xfbff; +static constexpr HalfFloat HALF_FLT_INF = 0x7c00; +static constexpr HalfFloat HALF_FLT_INF_NEGATIVE = 0xfc00; +static constexpr HalfFloat HALF_FLT_NANQ = 0x7e00; +static constexpr HalfFloat HALF_FLT_NANQ_NEGATIVE = 0xfe00; + +namespace HalfFloatConversion { + +// Layout of a float +static constexpr int FLOAT_SIGN_POS = 31; +static constexpr int FLOAT_EXPONENT_POS = 23; +static constexpr int FLOAT_EXPONENT_BITS = 8; +static constexpr int FLOAT_EXPONENT_MASK = (1 << FLOAT_EXPONENT_BITS) - 1; +static constexpr int FLOAT_EXPONENT_BIAS = 127; +static constexpr int FLOAT_MANTISSA_BITS = 23; +static constexpr int FLOAT_MANTISSA_MASK = (1 << FLOAT_MANTISSA_BITS) - 1; +static constexpr int FLOAT_EXPONENT_AND_MANTISSA_MASK = FLOAT_MANTISSA_MASK + (FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS); + +// Layout of half float +static constexpr int HALF_FLT_SIGN_POS = 15; +static constexpr int HALF_FLT_EXPONENT_POS = 10; +static constexpr int HALF_FLT_EXPONENT_BITS = 5; +static constexpr int HALF_FLT_EXPONENT_MASK = (1 << HALF_FLT_EXPONENT_BITS) - 1; +static constexpr int HALF_FLT_EXPONENT_BIAS = 15; +static constexpr int HALF_FLT_MANTISSA_BITS = 10; +static constexpr int HALF_FLT_MANTISSA_MASK = (1 << HALF_FLT_MANTISSA_BITS) - 1; +static constexpr int HALF_FLT_EXPONENT_AND_MANTISSA_MASK = HALF_FLT_MANTISSA_MASK + (HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS); + +/// Define half-float rounding modes +enum ERoundingMode +{ + ROUND_TO_NEG_INF, ///< Round to negative infinity + ROUND_TO_POS_INF, ///< Round to positive infinity + ROUND_TO_NEAREST, ///< Round to nearest value +}; + +/// Convert a float (32-bits) to a half float (16-bits), fallback version when no intrinsics available +template +inline HalfFloat FromFloatFallback(float inV) +{ + // Reinterpret the float as an uint32 + uint32 value = BitCast(inV); + + // Extract exponent + uint32 exponent = (value >> FLOAT_EXPONENT_POS) & FLOAT_EXPONENT_MASK; + + // Extract mantissa + uint32 mantissa = value & FLOAT_MANTISSA_MASK; + + // Extract the sign and move it into the right spot for the half float (so we can just or it in at the end) + HalfFloat hf_sign = HalfFloat(value >> (FLOAT_SIGN_POS - HALF_FLT_SIGN_POS)) & (1 << HALF_FLT_SIGN_POS); + + // Check NaN or INF + if (exponent == FLOAT_EXPONENT_MASK) // NaN or INF + return hf_sign | (mantissa == 0? HALF_FLT_INF : HALF_FLT_NANQ); + + // Rebias the exponent for half floats + int rebiased_exponent = int(exponent) - FLOAT_EXPONENT_BIAS + HALF_FLT_EXPONENT_BIAS; + + // Check overflow to infinity + if (rebiased_exponent >= HALF_FLT_EXPONENT_MASK) + { + bool round_up = RoundingMode == ROUND_TO_NEAREST || (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF); + return hf_sign | (round_up? HALF_FLT_INF : HALF_FLT_MAX); + } + + // Check underflow to zero + if (rebiased_exponent < -HALF_FLT_MANTISSA_BITS) + { + bool round_up = RoundingMode != ROUND_TO_NEAREST && (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && (value & FLOAT_EXPONENT_AND_MANTISSA_MASK) != 0; + return hf_sign | (round_up? 1 : 0); + } + + HalfFloat hf_exponent; + int shift; + if (rebiased_exponent <= 0) + { + // Underflow to denormalized number + hf_exponent = 0; + mantissa |= 1 << FLOAT_MANTISSA_BITS; // Add the implicit 1 bit to the mantissa + shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS + 1 - rebiased_exponent; + } + else + { + // Normal half float + hf_exponent = HalfFloat(rebiased_exponent << HALF_FLT_EXPONENT_POS); + shift = FLOAT_MANTISSA_BITS - HALF_FLT_MANTISSA_BITS; + } + + // Compose the half float + HalfFloat hf_mantissa = HalfFloat(mantissa >> shift); + HalfFloat hf = hf_sign | hf_exponent | hf_mantissa; + + // Calculate the remaining bits that we're discarding + uint remainder = mantissa & ((1 << shift) - 1); + + if constexpr (RoundingMode == ROUND_TO_NEAREST) + { + // Round to nearest + uint round_threshold = 1 << (shift - 1); + if (remainder > round_threshold // Above threshold, we must always round + || (remainder == round_threshold && (hf_mantissa & 1))) // When equal, round to nearest even + hf++; // May overflow to infinity + } + else + { + // Round up or down (truncate) depending on the rounding mode + bool round_up = (hf_sign == 0) == (RoundingMode == ROUND_TO_POS_INF) && remainder != 0; + if (round_up) + hf++; // May overflow to infinity + } + + return hf; +} + +/// Convert a float (32-bits) to a half float (16-bits) +template +JPH_INLINE HalfFloat FromFloat(float inV) +{ +#ifdef JPH_USE_F16C + union + { + __m128i u128; + HalfFloat u16[8]; + } hf; + __m128 val = _mm_load_ss(&inV); + switch (RoundingMode) + { + case ROUND_TO_NEG_INF: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEG_INF); + break; + case ROUND_TO_POS_INF: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_POS_INF); + break; + case ROUND_TO_NEAREST: + hf.u128 = _mm_cvtps_ph(val, _MM_FROUND_TO_NEAREST_INT); + break; + } + return hf.u16[0]; +#else + return FromFloatFallback(inV); +#endif +} + +/// Convert 4 half floats (lower 64 bits) to floats, fallback version when no intrinsics available +inline Vec4 ToFloatFallback(UVec4Arg inValue) +{ + // Unpack half floats to 4 uint32's + UVec4 value = inValue.Expand4Uint16Lo(); + + // Normal half float path, extract the exponent and mantissa, shift them into place and update the exponent bias + UVec4 exponent_mantissa = UVec4::sAnd(value, UVec4::sReplicate(HALF_FLT_EXPONENT_AND_MANTISSA_MASK)).LogicalShiftLeft() + UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS) << FLOAT_EXPONENT_POS); + + // Denormalized half float path, renormalize the float + UVec4 exponent_mantissa_denormalized = ((exponent_mantissa + UVec4::sReplicate(1 << FLOAT_EXPONENT_POS)).ReinterpretAsFloat() - UVec4::sReplicate((FLOAT_EXPONENT_BIAS - HALF_FLT_EXPONENT_BIAS + 1) << FLOAT_EXPONENT_POS).ReinterpretAsFloat()).ReinterpretAsInt(); + + // NaN / INF path, set all exponent bits + UVec4 exponent_mantissa_nan_inf = UVec4::sOr(exponent_mantissa, UVec4::sReplicate(FLOAT_EXPONENT_MASK << FLOAT_EXPONENT_POS)); + + // Get the exponent to determine which of the paths we should take + UVec4 exponent_mask = UVec4::sReplicate(HALF_FLT_EXPONENT_MASK << HALF_FLT_EXPONENT_POS); + UVec4 exponent = UVec4::sAnd(value, exponent_mask); + UVec4 is_denormalized = UVec4::sEquals(exponent, UVec4::sZero()); + UVec4 is_nan_inf = UVec4::sEquals(exponent, exponent_mask); + + // Select the correct result + UVec4 result_exponent_mantissa = UVec4::sSelect(UVec4::sSelect(exponent_mantissa, exponent_mantissa_nan_inf, is_nan_inf), exponent_mantissa_denormalized, is_denormalized); + + // Extract the sign bit and shift it to the left + UVec4 sign = UVec4::sAnd(value, UVec4::sReplicate(1 << HALF_FLT_SIGN_POS)).LogicalShiftLeft(); + + // Construct the float + return UVec4::sOr(sign, result_exponent_mantissa).ReinterpretAsFloat(); +} + +/// Convert 4 half floats (lower 64 bits) to floats +JPH_INLINE Vec4 ToFloat(UVec4Arg inValue) +{ +#if defined(JPH_USE_F16C) + return _mm_cvtph_ps(inValue.mValue); +#elif defined(JPH_USE_NEON) + return vcvt_f32_f16(vreinterpret_f16_f32(vget_low_f32(inValue.mValue))); +#else + return ToFloatFallback(inValue); +#endif +} + +} // HalfFloatConversion + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Mat44.h b/WickedEngine/Jolt/Math/Mat44.h new file mode 100644 index 000000000..1a1e254c5 --- /dev/null +++ b/WickedEngine/Jolt/Math/Mat44.h @@ -0,0 +1,243 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix. +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Mat44 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying column type + using Type = Vec4::Type; + + // Argument type + using ArgType = Mat44Arg; + + /// Constructor + Mat44() = default; ///< Intentionally not initialized for performance reasons + JPH_INLINE Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4); + JPH_INLINE Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4); + Mat44(const Mat44 &inM2) = default; + Mat44 & operator = (const Mat44 &inM2) = default; + JPH_INLINE Mat44(Type inC1, Type inC2, Type inC3, Type inC4); + + /// Zero matrix + static JPH_INLINE Mat44 sZero(); + + /// Identity matrix + static JPH_INLINE Mat44 sIdentity(); + + /// Matrix filled with NaN's + static JPH_INLINE Mat44 sNaN(); + + /// Load 16 floats from memory + static JPH_INLINE Mat44 sLoadFloat4x4(const Float4 *inV); + + /// Load 16 floats from memory, 16 bytes aligned + static JPH_INLINE Mat44 sLoadFloat4x4Aligned(const Float4 *inV); + + /// Rotate around X, Y or Z axis (angle in radians) + static JPH_INLINE Mat44 sRotationX(float inX); + static JPH_INLINE Mat44 sRotationY(float inY); + static JPH_INLINE Mat44 sRotationZ(float inZ); + + /// Rotate around arbitrary axis + static JPH_INLINE Mat44 sRotation(Vec3Arg inAxis, float inAngle); + + /// Rotate from quaternion + static JPH_INLINE Mat44 sRotation(QuatArg inQuat); + + /// Get matrix that translates + static JPH_INLINE Mat44 sTranslation(Vec3Arg inV); + + /// Get matrix that rotates and translates + static JPH_INLINE Mat44 sRotationTranslation(QuatArg inR, Vec3Arg inT); + + /// Get inverse matrix of sRotationTranslation + static JPH_INLINE Mat44 sInverseRotationTranslation(QuatArg inR, Vec3Arg inT); + + /// Get matrix that scales uniformly + static JPH_INLINE Mat44 sScale(float inScale); + + /// Get matrix that scales (produces a matrix with (inV, 1) on its diagonal) + static JPH_INLINE Mat44 sScale(Vec3Arg inV); + + /// Get outer product of inV and inV2 (equivalent to \f$inV1 \otimes inV2\f$) + static JPH_INLINE Mat44 sOuterProduct(Vec3Arg inV1, Vec3Arg inV2); + + /// Get matrix that represents a cross product \f$A \times B = \text{sCrossProduct}(A) \: B\f$ + static JPH_INLINE Mat44 sCrossProduct(Vec3Arg inV); + + /// Returns matrix ML so that \f$ML(q) \: p = q \: p\f$ (where p and q are quaternions) + static JPH_INLINE Mat44 sQuatLeftMultiply(QuatArg inQ); + + /// Returns matrix MR so that \f$MR(q) \: p = p \: q\f$ (where p and q are quaternions) + static JPH_INLINE Mat44 sQuatRightMultiply(QuatArg inQ); + + /// Returns a look at matrix that transforms from world space to view space + /// @param inPos Position of the camera + /// @param inTarget Target of the camera + /// @param inUp Up vector + static JPH_INLINE Mat44 sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp); + + /// Returns a right-handed perspective projection matrix + static JPH_INLINE Mat44 sPerspective(float inFovY, float inAspect, float inNear, float inFar); + + /// Get float component by element index + JPH_INLINE float operator () (uint inRow, uint inColumn) const { JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; } + JPH_INLINE float & operator () (uint inRow, uint inColumn) { JPH_ASSERT(inRow < 4); JPH_ASSERT(inColumn < 4); return mCol[inColumn].mF32[inRow]; } + + /// Comparison + JPH_INLINE bool operator == (Mat44Arg inM2) const; + JPH_INLINE bool operator != (Mat44Arg inM2) const { return !(*this == inM2); } + + /// Test if two matrices are close + JPH_INLINE bool IsClose(Mat44Arg inM2, float inMaxDistSq = 1.0e-12f) const; + + /// Multiply matrix by matrix + JPH_INLINE Mat44 operator * (Mat44Arg inM) const; + + /// Multiply vector by matrix + JPH_INLINE Vec3 operator * (Vec3Arg inV) const; + JPH_INLINE Vec4 operator * (Vec4Arg inV) const; + + /// Multiply vector by only 3x3 part of the matrix + JPH_INLINE Vec3 Multiply3x3(Vec3Arg inV) const; + + /// Multiply vector by only 3x3 part of the transpose of the matrix (\f$result = this^T \: inV\f$) + JPH_INLINE Vec3 Multiply3x3Transposed(Vec3Arg inV) const; + + /// Multiply 3x3 matrix by 3x3 matrix + JPH_INLINE Mat44 Multiply3x3(Mat44Arg inM) const; + + /// Multiply transpose of 3x3 matrix by 3x3 matrix (\f$result = this^T \: inM\f$) + JPH_INLINE Mat44 Multiply3x3LeftTransposed(Mat44Arg inM) const; + + /// Multiply 3x3 matrix by the transpose of a 3x3 matrix (\f$result = this \: inM^T\f$) + JPH_INLINE Mat44 Multiply3x3RightTransposed(Mat44Arg inM) const; + + /// Multiply matrix with float + JPH_INLINE Mat44 operator * (float inV) const; + friend JPH_INLINE Mat44 operator * (float inV, Mat44Arg inM) { return inM * inV; } + + /// Multiply matrix with float + JPH_INLINE Mat44 & operator *= (float inV); + + /// Per element addition of matrix + JPH_INLINE Mat44 operator + (Mat44Arg inM) const; + + /// Negate + JPH_INLINE Mat44 operator - () const; + + /// Per element subtraction of matrix + JPH_INLINE Mat44 operator - (Mat44Arg inM) const; + + /// Per element addition of matrix + JPH_INLINE Mat44 & operator += (Mat44Arg inM); + + /// Access to the columns + JPH_INLINE Vec3 GetAxisX() const { return Vec3(mCol[0]); } + JPH_INLINE void SetAxisX(Vec3Arg inV) { mCol[0] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisY() const { return Vec3(mCol[1]); } + JPH_INLINE void SetAxisY(Vec3Arg inV) { mCol[1] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetAxisZ() const { return Vec3(mCol[2]); } + JPH_INLINE void SetAxisZ(Vec3Arg inV) { mCol[2] = Vec4(inV, 0.0f); } + JPH_INLINE Vec3 GetTranslation() const { return Vec3(mCol[3]); } + JPH_INLINE void SetTranslation(Vec3Arg inV) { mCol[3] = Vec4(inV, 1.0f); } + JPH_INLINE Vec3 GetDiagonal3() const { return Vec3(mCol[0][0], mCol[1][1], mCol[2][2]); } + JPH_INLINE void SetDiagonal3(Vec3Arg inV) { mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); } + JPH_INLINE Vec4 GetDiagonal4() const { return Vec4(mCol[0][0], mCol[1][1], mCol[2][2], mCol[3][3]); } + JPH_INLINE void SetDiagonal4(Vec4Arg inV) { mCol[0][0] = inV.GetX(); mCol[1][1] = inV.GetY(); mCol[2][2] = inV.GetZ(); mCol[3][3] = inV.GetW(); } + JPH_INLINE Vec3 GetColumn3(uint inCol) const { JPH_ASSERT(inCol < 4); return Vec3(mCol[inCol]); } + JPH_INLINE void SetColumn3(uint inCol, Vec3Arg inV) { JPH_ASSERT(inCol < 4); mCol[inCol] = Vec4(inV, inCol == 3? 1.0f : 0.0f); } + JPH_INLINE Vec4 GetColumn4(uint inCol) const { JPH_ASSERT(inCol < 4); return mCol[inCol]; } + JPH_INLINE void SetColumn4(uint inCol, Vec4Arg inV) { JPH_ASSERT(inCol < 4); mCol[inCol] = inV; } + + /// Store matrix to memory + JPH_INLINE void StoreFloat4x4(Float4 *outV) const; + + /// Transpose matrix + JPH_INLINE Mat44 Transposed() const; + + /// Transpose 3x3 subpart of matrix + JPH_INLINE Mat44 Transposed3x3() const; + + /// Inverse 4x4 matrix + JPH_INLINE Mat44 Inversed() const; + + /// Inverse 4x4 matrix when it only contains rotation and translation + JPH_INLINE Mat44 InversedRotationTranslation() const; + + /// Get the determinant of a 3x3 matrix + JPH_INLINE float GetDeterminant3x3() const; + + /// Get the adjoint of a 3x3 matrix + JPH_INLINE Mat44 Adjointed3x3() const; + + /// Inverse 3x3 matrix + JPH_INLINE Mat44 Inversed3x3() const; + + /// *this = inM.Inversed3x3(), returns false if the matrix is singular in which case *this is unchanged + JPH_INLINE bool SetInversed3x3(Mat44Arg inM); + + /// Get rotation part only (note: retains the first 3 values from the bottom row) + JPH_INLINE Mat44 GetRotation() const; + + /// Get rotation part only (note: also clears the bottom row) + JPH_INLINE Mat44 GetRotationSafe() const; + + /// Updates the rotation part of this matrix (the first 3 columns) + JPH_INLINE void SetRotation(Mat44Arg inRotation); + + /// Convert to quaternion + JPH_INLINE Quat GetQuaternion() const; + + /// Get matrix that transforms a direction with the same transform as this matrix (length is not preserved) + JPH_INLINE Mat44 GetDirectionPreservingMatrix() const { return GetRotation().Inversed3x3().Transposed3x3(); } + + /// Pre multiply by translation matrix: result = this * Mat44::sTranslation(inTranslation) + JPH_INLINE Mat44 PreTranslated(Vec3Arg inTranslation) const; + + /// Post multiply by translation matrix: result = Mat44::sTranslation(inTranslation) * this (i.e. add inTranslation to the 4-th column) + JPH_INLINE Mat44 PostTranslated(Vec3Arg inTranslation) const; + + /// Scale a matrix: result = this * Mat44::sScale(inScale) + JPH_INLINE Mat44 PreScaled(Vec3Arg inScale) const; + + /// Scale a matrix: result = Mat44::sScale(inScale) * this + JPH_INLINE Mat44 PostScaled(Vec3Arg inScale) const; + + /// Decompose a matrix into a rotation & translation part and into a scale part so that: + /// this = return_value * Mat44::sScale(outScale). + /// This equation only holds when the matrix is orthogonal, if it is not the returned matrix + /// will be made orthogonal using the modified Gram-Schmidt algorithm (see: https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process) + JPH_INLINE Mat44 Decompose(Vec3 &outScale) const; + +#ifndef JPH_DOUBLE_PRECISION + /// In single precision mode just return the matrix itself + JPH_INLINE Mat44 ToMat44() const { return *this; } +#endif // !JPH_DOUBLE_PRECISION + + /// To String + friend ostream & operator << (ostream &inStream, Mat44Arg inM) + { + inStream << inM.mCol[0] << ", " << inM.mCol[1] << ", " << inM.mCol[2] << ", " << inM.mCol[3]; + return inStream; + } + +private: + Vec4 mCol[4]; ///< Column +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Mat44.inl" diff --git a/WickedEngine/Jolt/Math/Mat44.inl b/WickedEngine/Jolt/Math/Mat44.inl new file mode 100644 index 000000000..76577b715 --- /dev/null +++ b/WickedEngine/Jolt/Math/Mat44.inl @@ -0,0 +1,952 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#define JPH_EL(r, c) mCol[c].mF32[r] + +Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec4Arg inC4) : + mCol { inC1, inC2, inC3, inC4 } +{ +} + +Mat44::Mat44(Vec4Arg inC1, Vec4Arg inC2, Vec4Arg inC3, Vec3Arg inC4) : + mCol { inC1, inC2, inC3, Vec4(inC4, 1.0f) } +{ +} + +Mat44::Mat44(Type inC1, Type inC2, Type inC3, Type inC4) : + mCol { inC1, inC2, inC3, inC4 } +{ +} + +Mat44 Mat44::sZero() +{ + return Mat44(Vec4::sZero(), Vec4::sZero(), Vec4::sZero(), Vec4::sZero()); +} + +Mat44 Mat44::sIdentity() +{ + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sNaN() +{ + return Mat44(Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN(), Vec4::sNaN()); +} + +Mat44 Mat44::sLoadFloat4x4(const Float4 *inV) +{ + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = Vec4::sLoadFloat4(inV + c); + return result; +} + +Mat44 Mat44::sLoadFloat4x4Aligned(const Float4 *inV) +{ + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = Vec4::sLoadFloat4Aligned(inV + c); + return result; +} + +Mat44 Mat44::sRotationX(float inX) +{ + Vec4 sv, cv; + Vec4::sReplicate(inX).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, c, s, 0), Vec4(0, -s, c, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotationY(float inY) +{ + Vec4 sv, cv; + Vec4::sReplicate(inY).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(c, 0, -s, 0), Vec4(0, 1, 0, 0), Vec4(s, 0, c, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotationZ(float inZ) +{ + Vec4 sv, cv; + Vec4::sReplicate(inZ).SinCos(sv, cv); + float s = sv.GetX(), c = cv.GetX(); + return Mat44(Vec4(c, s, 0, 0), Vec4(-s, c, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sRotation(QuatArg inQuat) +{ + JPH_ASSERT(inQuat.IsNormalized()); + + // See: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation section 'Quaternion-derived rotation matrix' +#ifdef JPH_USE_SSE4_1 + __m128 xyzw = inQuat.mValue.mValue; + __m128 two_xyzw = _mm_add_ps(xyzw, xyzw); + __m128 yzxw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 0, 2, 1)); + __m128 two_yzxw = _mm_add_ps(yzxw, yzxw); + __m128 zxyw = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 1, 0, 2)); + __m128 two_zxyw = _mm_add_ps(zxyw, zxyw); + __m128 wwww = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 3, 3, 3)); + __m128 diagonal = _mm_sub_ps(_mm_sub_ps(_mm_set1_ps(1.0f), _mm_mul_ps(two_yzxw, yzxw)), _mm_mul_ps(two_zxyw, zxyw)); // (1 - 2 y^2 - 2 z^2, 1 - 2 x^2 - 2 z^2, 1 - 2 x^2 - 2 y^2, 1 - 4 w^2) + __m128 plus = _mm_add_ps(_mm_mul_ps(two_xyzw, zxyw), _mm_mul_ps(two_yzxw, wwww)); // 2 * (xz + yw, xy + zw, yz + xw, ww) + __m128 minus = _mm_sub_ps(_mm_mul_ps(two_yzxw, xyzw), _mm_mul_ps(two_zxyw, wwww)); // 2 * (xy - zw, yz - xw, xz - yw, 0) + + // Workaround for compiler changing _mm_sub_ps(_mm_mul_ps(...), ...) into a fused multiply sub instruction, resulting in w not being 0 + // There doesn't appear to be a reliable way to turn this off in Clang + minus = _mm_insert_ps(minus, minus, 0b1000); + + __m128 col0 = _mm_blend_ps(_mm_blend_ps(plus, diagonal, 0b0001), minus, 0b1100); // (1 - 2 y^2 - 2 z^2, 2 xy + 2 zw, 2 xz - 2 yw, 0) + __m128 col1 = _mm_blend_ps(_mm_blend_ps(diagonal, minus, 0b1001), plus, 0b0100); // (2 xy - 2 zw, 1 - 2 x^2 - 2 z^2, 2 yz + 2 xw, 0) + __m128 col2 = _mm_blend_ps(_mm_blend_ps(minus, plus, 0b0001), diagonal, 0b0100); // (2 xz + 2 yw, 2 yz - 2 xw, 1 - 2 x^2 - 2 y^2, 0) + __m128 col3 = _mm_set_ps(1, 0, 0, 0); + + return Mat44(col0, col1, col2, col3); +#else + float x = inQuat.GetX(); + float y = inQuat.GetY(); + float z = inQuat.GetZ(); + float w = inQuat.GetW(); + + float tx = x + x; // Note: Using x + x instead of 2.0f * x to force this function to return the same value as the SSE4.1 version across platforms. + float ty = y + y; + float tz = z + z; + + float xx = tx * x; + float yy = ty * y; + float zz = tz * z; + float xy = tx * y; + float xz = tx * z; + float xw = tx * w; + float yz = ty * z; + float yw = ty * w; + float zw = tz * w; + + return Mat44(Vec4((1.0f - yy) - zz, xy + zw, xz - yw, 0.0f), // Note: Added extra brackets to force this function to return the same value as the SSE4.1 version across platforms. + Vec4(xy - zw, (1.0f - zz) - xx, yz + xw, 0.0f), + Vec4(xz + yw, yz - xw, (1.0f - xx) - yy, 0.0f), + Vec4(0.0f, 0.0f, 0.0f, 1.0f)); +#endif +} + +Mat44 Mat44::sRotation(Vec3Arg inAxis, float inAngle) +{ + return sRotation(Quat::sRotation(inAxis, inAngle)); +} + +Mat44 Mat44::sTranslation(Vec3Arg inV) +{ + return Mat44(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(inV, 1)); +} + +Mat44 Mat44::sRotationTranslation(QuatArg inR, Vec3Arg inT) +{ + Mat44 m = sRotation(inR); + m.SetTranslation(inT); + return m; +} + +Mat44 Mat44::sInverseRotationTranslation(QuatArg inR, Vec3Arg inT) +{ + Mat44 m = sRotation(inR.Conjugated()); + m.SetTranslation(-m.Multiply3x3(inT)); + return m; +} + +Mat44 Mat44::sScale(float inScale) +{ + return Mat44(Vec4(inScale, 0, 0, 0), Vec4(0, inScale, 0, 0), Vec4(0, 0, inScale, 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sScale(Vec3Arg inV) +{ + return Mat44(Vec4(inV.GetX(), 0, 0, 0), Vec4(0, inV.GetY(), 0, 0), Vec4(0, 0, inV.GetZ(), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sOuterProduct(Vec3Arg inV1, Vec3Arg inV2) +{ + Vec4 v1(inV1, 0); + return Mat44(v1 * inV2.SplatX(), v1 * inV2.SplatY(), v1 * inV2.SplatZ(), Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::sCrossProduct(Vec3Arg inV) +{ +#ifdef JPH_USE_SSE4_1 + // Zero out the W component + __m128 zero = _mm_setzero_ps(); + __m128 v = _mm_blend_ps(inV.mValue, zero, 0b1000); + + // Negate + __m128 min_v = _mm_sub_ps(zero, v); + + return Mat44( + _mm_shuffle_ps(v, min_v, _MM_SHUFFLE(3, 1, 2, 3)), // [0, z, -y, 0] + _mm_shuffle_ps(min_v, v, _MM_SHUFFLE(3, 0, 3, 2)), // [-z, 0, x, 0] + _mm_blend_ps(_mm_shuffle_ps(v, v, _MM_SHUFFLE(3, 3, 3, 1)), _mm_shuffle_ps(min_v, min_v, _MM_SHUFFLE(3, 3, 0, 3)), 0b0010), // [y, -x, 0, 0] + Vec4(0, 0, 0, 1)); +#else + float x = inV.GetX(); + float y = inV.GetY(); + float z = inV.GetZ(); + + return Mat44( + Vec4(0, z, -y, 0), + Vec4(-z, 0, x, 0), + Vec4(y, -x, 0, 0), + Vec4(0, 0, 0, 1)); +#endif +} + +Mat44 Mat44::sLookAt(Vec3Arg inPos, Vec3Arg inTarget, Vec3Arg inUp) +{ + Vec3 direction = (inTarget - inPos).NormalizedOr(-Vec3::sAxisZ()); + Vec3 right = direction.Cross(inUp).NormalizedOr(Vec3::sAxisX()); + Vec3 up = right.Cross(direction); + + return Mat44(Vec4(right, 0), Vec4(up, 0), Vec4(-direction, 0), Vec4(inPos, 1)).InversedRotationTranslation(); +} + +Mat44 Mat44::sPerspective(float inFovY, float inAspect, float inNear, float inFar) +{ + float height = 1.0f / Tan(0.5f * inFovY); + float width = height / inAspect; + float range = inFar / (inNear - inFar); + + return Mat44(Vec4(width, 0.0f, 0.0f, 0.0f), Vec4(0.0f, height, 0.0f, 0.0f), Vec4(0.0f, 0.0f, range, -1.0f), Vec4(0.0f, 0.0f, range * inNear, 0.0f)); +} + +bool Mat44::operator == (Mat44Arg inM2) const +{ + return UVec4::sAnd( + UVec4::sAnd(Vec4::sEquals(mCol[0], inM2.mCol[0]), Vec4::sEquals(mCol[1], inM2.mCol[1])), + UVec4::sAnd(Vec4::sEquals(mCol[2], inM2.mCol[2]), Vec4::sEquals(mCol[3], inM2.mCol[3])) + ).TestAllTrue(); +} + +bool Mat44::IsClose(Mat44Arg inM2, float inMaxDistSq) const +{ + for (int i = 0; i < 4; ++i) + if (!mCol[i].IsClose(inM2.mCol[i], inMaxDistSq)) + return false; + return true; +} + +Mat44 Mat44::operator * (Mat44Arg inM) const +{ + Mat44 result; +#if defined(JPH_USE_SSE) + for (int i = 0; i < 4; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(3, 3, 3, 3)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 4; ++i) + { + Type c = inM.mCol[i].mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(c, 3)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2] + mCol[3] * inM.mCol[i].mF32[3]; +#endif + return result; +} + +Vec3 Mat44::operator * (Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, mCol[3].mValue); + return Vec3::sFixW(t); +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + t = vaddq_f32(t, mCol[3].mValue); // Don't combine this with the first mul into a fused multiply add, causes precision issues + return Vec3::sFixW(t); +#else + return Vec3( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2]); +#endif +} + +Vec4 Mat44::operator * (Vec4Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[3].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(3, 3, 3, 3)))); + return t; +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + t = vmlaq_f32(t, mCol[3].mValue, vdupq_laneq_f32(inV.mValue, 3)); + return t; +#else + return Vec4( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2] + mCol[3].mF32[0] * inV.mF32[3], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2] + mCol[3].mF32[1] * inV.mF32[3], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2] + mCol[3].mF32[2] * inV.mF32[3], + mCol[0].mF32[3] * inV.mF32[0] + mCol[1].mF32[3] * inV.mF32[1] + mCol[2].mF32[3] * inV.mF32[2] + mCol[3].mF32[3] * inV.mF32[3]); +#endif +} + +Vec3 Mat44::Multiply3x3(Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE) + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(inV.mValue, inV.mValue, _MM_SHUFFLE(2, 2, 2, 2)))); + return Vec3::sFixW(t); +#elif defined(JPH_USE_NEON) + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(inV.mValue, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(inV.mValue, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(inV.mValue, 2)); + return Vec3::sFixW(t); +#else + return Vec3( + mCol[0].mF32[0] * inV.mF32[0] + mCol[1].mF32[0] * inV.mF32[1] + mCol[2].mF32[0] * inV.mF32[2], + mCol[0].mF32[1] * inV.mF32[0] + mCol[1].mF32[1] * inV.mF32[1] + mCol[2].mF32[1] * inV.mF32[2], + mCol[0].mF32[2] * inV.mF32[0] + mCol[1].mF32[2] * inV.mF32[1] + mCol[2].mF32[2] * inV.mF32[2]); +#endif +} + +Vec3 Mat44::Multiply3x3Transposed(Vec3Arg inV) const +{ +#if defined(JPH_USE_SSE4_1) + __m128 x = _mm_dp_ps(mCol[0].mValue, inV.mValue, 0x7f); + __m128 y = _mm_dp_ps(mCol[1].mValue, inV.mValue, 0x7f); + __m128 xy = _mm_blend_ps(x, y, 0b0010); + __m128 z = _mm_dp_ps(mCol[2].mValue, inV.mValue, 0x7f); + __m128 xyzz = _mm_blend_ps(xy, z, 0b1100); + return xyzz; +#else + return Transposed3x3().Multiply3x3(inV); +#endif +} + +Mat44 Mat44::Multiply3x3(Mat44Arg inM) const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + Mat44 result; +#if defined(JPH_USE_SSE) + for (int i = 0; i < 3; ++i) + { + __m128 c = inM.mCol[i].mValue; + __m128 t = _mm_mul_ps(mCol[0].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(0, 0, 0, 0))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[1].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(1, 1, 1, 1)))); + t = _mm_add_ps(t, _mm_mul_ps(mCol[2].mValue, _mm_shuffle_ps(c, c, _MM_SHUFFLE(2, 2, 2, 2)))); + result.mCol[i].mValue = t; + } +#elif defined(JPH_USE_NEON) + for (int i = 0; i < 3; ++i) + { + Type c = inM.mCol[i].mValue; + Type t = vmulq_f32(mCol[0].mValue, vdupq_laneq_f32(c, 0)); + t = vmlaq_f32(t, mCol[1].mValue, vdupq_laneq_f32(c, 1)); + t = vmlaq_f32(t, mCol[2].mValue, vdupq_laneq_f32(c, 2)); + result.mCol[i].mValue = t; + } +#else + for (int i = 0; i < 3; ++i) + result.mCol[i] = mCol[0] * inM.mCol[i].mF32[0] + mCol[1] * inM.mCol[i].mF32[1] + mCol[2] * inM.mCol[i].mF32[2]; +#endif + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Multiply3x3LeftTransposed(Mat44Arg inM) const +{ + // Transpose left hand side + Mat44 trans = Transposed3x3(); + + // Do 3x3 matrix multiply + Mat44 result; + result.mCol[0] = trans.mCol[0] * inM.mCol[0].SplatX() + trans.mCol[1] * inM.mCol[0].SplatY() + trans.mCol[2] * inM.mCol[0].SplatZ(); + result.mCol[1] = trans.mCol[0] * inM.mCol[1].SplatX() + trans.mCol[1] * inM.mCol[1].SplatY() + trans.mCol[2] * inM.mCol[1].SplatZ(); + result.mCol[2] = trans.mCol[0] * inM.mCol[2].SplatX() + trans.mCol[1] * inM.mCol[2].SplatY() + trans.mCol[2] * inM.mCol[2].SplatZ(); + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Multiply3x3RightTransposed(Mat44Arg inM) const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + Mat44 result; + result.mCol[0] = mCol[0] * inM.mCol[0].SplatX() + mCol[1] * inM.mCol[1].SplatX() + mCol[2] * inM.mCol[2].SplatX(); + result.mCol[1] = mCol[0] * inM.mCol[0].SplatY() + mCol[1] * inM.mCol[1].SplatY() + mCol[2] * inM.mCol[2].SplatY(); + result.mCol[2] = mCol[0] * inM.mCol[0].SplatZ() + mCol[1] * inM.mCol[1].SplatZ() + mCol[2] * inM.mCol[2].SplatZ(); + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::operator * (float inV) const +{ + Vec4 multiplier = Vec4::sReplicate(inV); + + Mat44 result; + for (int c = 0; c < 4; ++c) + result.mCol[c] = mCol[c] * multiplier; + return result; +} + +Mat44 &Mat44::operator *= (float inV) +{ + for (int c = 0; c < 4; ++c) + mCol[c] *= inV; + + return *this; +} + +Mat44 Mat44::operator + (Mat44Arg inM) const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[i] + inM.mCol[i]; + return result; +} + +Mat44 Mat44::operator - () const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = -mCol[i]; + return result; +} + +Mat44 Mat44::operator - (Mat44Arg inM) const +{ + Mat44 result; + for (int i = 0; i < 4; ++i) + result.mCol[i] = mCol[i] - inM.mCol[i]; + return result; +} + +Mat44 &Mat44::operator += (Mat44Arg inM) +{ + for (int c = 0; c < 4; ++c) + mCol[c] += inM.mCol[c]; + + return *this; +} + +void Mat44::StoreFloat4x4(Float4 *outV) const +{ + for (int c = 0; c < 4; ++c) + mCol[c].StoreFloat4(outV + c); +} + +Mat44 Mat44::Transposed() const +{ +#if defined(JPH_USE_SSE) + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + + Mat44 result; + result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1)); + result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[3].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(3, 1, 3, 1)); + return result; +#elif defined(JPH_USE_NEON) + float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue); + float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, mCol[3].mValue); + float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]); + float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]); + + Mat44 result; + result.mCol[0].mValue = tmp3.val[0]; + result.mCol[1].mValue = tmp3.val[1]; + result.mCol[2].mValue = tmp4.val[0]; + result.mCol[3].mValue = tmp4.val[1]; + return result; +#else + Mat44 result; + for (int c = 0; c < 4; ++c) + for (int r = 0; r < 4; ++r) + result.mCol[r].mF32[c] = mCol[c].mF32[r]; + return result; +#endif +} + +Mat44 Mat44::Transposed3x3() const +{ +#if defined(JPH_USE_SSE) + __m128 zero = _mm_setzero_ps(); + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp3 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 tmp2 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 tmp4 = _mm_shuffle_ps(mCol[2].mValue, zero, _MM_SHUFFLE(3, 2, 3, 2)); + + Mat44 result; + result.mCol[0].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(2, 0, 2, 0)); + result.mCol[1].mValue = _mm_shuffle_ps(tmp1, tmp2, _MM_SHUFFLE(3, 1, 3, 1)); + result.mCol[2].mValue = _mm_shuffle_ps(tmp3, tmp4, _MM_SHUFFLE(2, 0, 2, 0)); +#elif defined(JPH_USE_NEON) + float32x4x2_t tmp1 = vzipq_f32(mCol[0].mValue, mCol[2].mValue); + float32x4x2_t tmp2 = vzipq_f32(mCol[1].mValue, vdupq_n_f32(0)); + float32x4x2_t tmp3 = vzipq_f32(tmp1.val[0], tmp2.val[0]); + float32x4x2_t tmp4 = vzipq_f32(tmp1.val[1], tmp2.val[1]); + + Mat44 result; + result.mCol[0].mValue = tmp3.val[0]; + result.mCol[1].mValue = tmp3.val[1]; + result.mCol[2].mValue = tmp4.val[0]; +#else + Mat44 result; + for (int c = 0; c < 3; ++c) + { + for (int r = 0; r < 3; ++r) + result.mCol[c].mF32[r] = mCol[r].mF32[c]; + result.mCol[c].mF32[3] = 0; + } +#endif + result.mCol[3] = Vec4(0, 0, 0, 1); + return result; +} + +Mat44 Mat44::Inversed() const +{ +#if defined(JPH_USE_SSE) + // Algorithm from: http://download.intel.com/design/PentiumIII/sml/24504301.pdf + // Streaming SIMD Extensions - Inverse of 4x4 Matrix + // Adapted to load data using _mm_shuffle_ps instead of loading from memory + // Replaced _mm_rcp_ps with _mm_div_ps for better accuracy + + __m128 tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 row1 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(1, 0, 1, 0)); + __m128 row0 = _mm_shuffle_ps(tmp1, row1, _MM_SHUFFLE(2, 0, 2, 0)); + row1 = _mm_shuffle_ps(row1, tmp1, _MM_SHUFFLE(3, 1, 3, 1)); + tmp1 = _mm_shuffle_ps(mCol[0].mValue, mCol[1].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 row3 = _mm_shuffle_ps(mCol[2].mValue, mCol[3].mValue, _MM_SHUFFLE(3, 2, 3, 2)); + __m128 row2 = _mm_shuffle_ps(tmp1, row3, _MM_SHUFFLE(2, 0, 2, 0)); + row3 = _mm_shuffle_ps(row3, tmp1, _MM_SHUFFLE(3, 1, 3, 1)); + + tmp1 = _mm_mul_ps(row2, row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + __m128 minor0 = _mm_mul_ps(row1, tmp1); + __m128 minor1 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0); + minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1); + minor1 = _mm_shuffle_ps(minor1, minor1, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(row1, row2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0); + __m128 minor3 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1)); + minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3); + minor3 = _mm_shuffle_ps(minor3, minor3, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, _MM_SHUFFLE(1, 0, 3, 2)), row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + row2 = _mm_shuffle_ps(row2, row2, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0); + __m128 minor2 = _mm_mul_ps(row0, tmp1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1)); + minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2); + minor2 = _mm_shuffle_ps(minor2, minor2, _MM_SHUFFLE(1, 0, 3, 2)); + + tmp1 = _mm_mul_ps(row0, row1); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2); + minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2); + minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1)); + + tmp1 = _mm_mul_ps(row0, row3); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1)); + minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1); + minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1)); + + tmp1 = _mm_mul_ps(row0, row2); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(2, 3, 0, 1)); + minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1); + minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1)); + tmp1 = _mm_shuffle_ps(tmp1, tmp1, _MM_SHUFFLE(1, 0, 3, 2)); + minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1)); + minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3); + + __m128 det = _mm_mul_ps(row0, minor0); + det = _mm_add_ps(_mm_shuffle_ps(det, det, _MM_SHUFFLE(2, 3, 0, 1)), det); // Original code did (x + z) + (y + w), changed to (x + y) + (z + w) to match the ARM code below and make the result cross platform deterministic + det = _mm_add_ss(_mm_shuffle_ps(det, det, _MM_SHUFFLE(1, 0, 3, 2)), det); + det = _mm_div_ss(_mm_set_ss(1.0f), det); + det = _mm_shuffle_ps(det, det, _MM_SHUFFLE(0, 0, 0, 0)); + + Mat44 result; + result.mCol[0].mValue = _mm_mul_ps(det, minor0); + result.mCol[1].mValue = _mm_mul_ps(det, minor1); + result.mCol[2].mValue = _mm_mul_ps(det, minor2); + result.mCol[3].mValue = _mm_mul_ps(det, minor3); + return result; +#elif defined(JPH_USE_NEON) + // Adapted from the SSE version, there's surprising few articles about efficient ways of calculating an inverse for ARM on the internet + Type tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 0, 1, 4, 5); + Type row1 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 0, 1, 4, 5); + Type row0 = JPH_NEON_SHUFFLE_F32x4(tmp1, row1, 0, 2, 4, 6); + row1 = JPH_NEON_SHUFFLE_F32x4(row1, tmp1, 1, 3, 5, 7); + tmp1 = JPH_NEON_SHUFFLE_F32x4(mCol[0].mValue, mCol[1].mValue, 2, 3, 6, 7); + Type row3 = JPH_NEON_SHUFFLE_F32x4(mCol[2].mValue, mCol[3].mValue, 2, 3, 6, 7); + Type row2 = JPH_NEON_SHUFFLE_F32x4(tmp1, row3, 0, 2, 4, 6); + row3 = JPH_NEON_SHUFFLE_F32x4(row3, tmp1, 1, 3, 5, 7); + + tmp1 = vmulq_f32(row2, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + Type minor0 = vmulq_f32(row1, tmp1); + Type minor1 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(vmulq_f32(row1, tmp1), minor0); + minor1 = vsubq_f32(vmulq_f32(row0, tmp1), minor1); + minor1 = JPH_NEON_SHUFFLE_F32x4(minor1, minor1, 2, 3, 0, 1); + + tmp1 = vmulq_f32(row1, row2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor0 = vaddq_f32(vmulq_f32(row3, tmp1), minor0); + Type minor3 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(minor0, vmulq_f32(row3, tmp1)); + minor3 = vsubq_f32(vmulq_f32(row0, tmp1), minor3); + minor3 = JPH_NEON_SHUFFLE_F32x4(minor3, minor3, 2, 3, 0, 1); + + tmp1 = JPH_NEON_SHUFFLE_F32x4(row1, row1, 2, 3, 0, 1); + tmp1 = vmulq_f32(tmp1, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + row2 = JPH_NEON_SHUFFLE_F32x4(row2, row2, 2, 3, 0, 1); + minor0 = vaddq_f32(vmulq_f32(row2, tmp1), minor0); + Type minor2 = vmulq_f32(row0, tmp1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor0 = vsubq_f32(minor0, vmulq_f32(row2, tmp1)); + minor2 = vsubq_f32(vmulq_f32(row0, tmp1), minor2); + minor2 = JPH_NEON_SHUFFLE_F32x4(minor2, minor2, 2, 3, 0, 1); + + tmp1 = vmulq_f32(row0, row1); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor2 = vaddq_f32(vmulq_f32(row3, tmp1), minor2); + minor3 = vsubq_f32(vmulq_f32(row2, tmp1), minor3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor2 = vsubq_f32(vmulq_f32(row3, tmp1), minor2); + minor3 = vsubq_f32(minor3, vmulq_f32(row2, tmp1)); + + tmp1 = vmulq_f32(row0, row3); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor1 = vsubq_f32(minor1, vmulq_f32(row2, tmp1)); + minor2 = vaddq_f32(vmulq_f32(row1, tmp1), minor2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor1 = vaddq_f32(vmulq_f32(row2, tmp1), minor1); + minor2 = vsubq_f32(minor2, vmulq_f32(row1, tmp1)); + + tmp1 = vmulq_f32(row0, row2); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 1, 0, 3, 2); + minor1 = vaddq_f32(vmulq_f32(row3, tmp1), minor1); + minor3 = vsubq_f32(minor3, vmulq_f32(row1, tmp1)); + tmp1 = JPH_NEON_SHUFFLE_F32x4(tmp1, tmp1, 2, 3, 0, 1); + minor1 = vsubq_f32(minor1, vmulq_f32(row3, tmp1)); + minor3 = vaddq_f32(vmulq_f32(row1, tmp1), minor3); + + Type det = vmulq_f32(row0, minor0); + det = vdupq_n_f32(vaddvq_f32(det)); + det = vdivq_f32(vdupq_n_f32(1.0f), det); + + Mat44 result; + result.mCol[0].mValue = vmulq_f32(det, minor0); + result.mCol[1].mValue = vmulq_f32(det, minor1); + result.mCol[2].mValue = vmulq_f32(det, minor2); + result.mCol[3].mValue = vmulq_f32(det, minor3); + return result; +#else + float m00 = JPH_EL(0, 0), m10 = JPH_EL(1, 0), m20 = JPH_EL(2, 0), m30 = JPH_EL(3, 0); + float m01 = JPH_EL(0, 1), m11 = JPH_EL(1, 1), m21 = JPH_EL(2, 1), m31 = JPH_EL(3, 1); + float m02 = JPH_EL(0, 2), m12 = JPH_EL(1, 2), m22 = JPH_EL(2, 2), m32 = JPH_EL(3, 2); + float m03 = JPH_EL(0, 3), m13 = JPH_EL(1, 3), m23 = JPH_EL(2, 3), m33 = JPH_EL(3, 3); + + float m10211120 = m10 * m21 - m11 * m20; + float m10221220 = m10 * m22 - m12 * m20; + float m10231320 = m10 * m23 - m13 * m20; + float m10311130 = m10 * m31 - m11 * m30; + float m10321230 = m10 * m32 - m12 * m30; + float m10331330 = m10 * m33 - m13 * m30; + float m11221221 = m11 * m22 - m12 * m21; + float m11231321 = m11 * m23 - m13 * m21; + float m11321231 = m11 * m32 - m12 * m31; + float m11331331 = m11 * m33 - m13 * m31; + float m12231322 = m12 * m23 - m13 * m22; + float m12331332 = m12 * m33 - m13 * m32; + float m20312130 = m20 * m31 - m21 * m30; + float m20322230 = m20 * m32 - m22 * m30; + float m20332330 = m20 * m33 - m23 * m30; + float m21322231 = m21 * m32 - m22 * m31; + float m21332331 = m21 * m33 - m23 * m31; + float m22332332 = m22 * m33 - m23 * m32; + + Vec4 col0(m11 * m22332332 - m12 * m21332331 + m13 * m21322231, -m10 * m22332332 + m12 * m20332330 - m13 * m20322230, m10 * m21332331 - m11 * m20332330 + m13 * m20312130, -m10 * m21322231 + m11 * m20322230 - m12 * m20312130); + Vec4 col1(-m01 * m22332332 + m02 * m21332331 - m03 * m21322231, m00 * m22332332 - m02 * m20332330 + m03 * m20322230, -m00 * m21332331 + m01 * m20332330 - m03 * m20312130, m00 * m21322231 - m01 * m20322230 + m02 * m20312130); + Vec4 col2(m01 * m12331332 - m02 * m11331331 + m03 * m11321231, -m00 * m12331332 + m02 * m10331330 - m03 * m10321230, m00 * m11331331 - m01 * m10331330 + m03 * m10311130, -m00 * m11321231 + m01 * m10321230 - m02 * m10311130); + Vec4 col3(-m01 * m12231322 + m02 * m11231321 - m03 * m11221221, m00 * m12231322 - m02 * m10231320 + m03 * m10221220, -m00 * m11231321 + m01 * m10231320 - m03 * m10211120, m00 * m11221221 - m01 * m10221220 + m02 * m10211120); + + float det = m00 * col0.mF32[0] + m01 * col0.mF32[1] + m02 * col0.mF32[2] + m03 * col0.mF32[3]; + + return Mat44(col0 / det, col1 / det, col2 / det, col3 / det); +#endif +} + +Mat44 Mat44::InversedRotationTranslation() const +{ + Mat44 m = Transposed3x3(); + m.SetTranslation(-m.Multiply3x3(GetTranslation())); + return m; +} + +float Mat44::GetDeterminant3x3() const +{ + return GetAxisX().Dot(GetAxisY().Cross(GetAxisZ())); +} + +Mat44 Mat44::Adjointed3x3() const +{ + return Mat44( + Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0) + - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0), + Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0) + - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0), + Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) + - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0), + Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::Inversed3x3() const +{ + float det = GetDeterminant3x3(); + + return Mat44( + (Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0) + - Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0)) / det, + (Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(2, 1), JPH_EL(2, 2), JPH_EL(2, 0), 0) + - Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(2, 2), JPH_EL(2, 0), JPH_EL(2, 1), 0)) / det, + (Vec4(JPH_EL(0, 1), JPH_EL(0, 2), JPH_EL(0, 0), 0) * Vec4(JPH_EL(1, 2), JPH_EL(1, 0), JPH_EL(1, 1), 0) + - Vec4(JPH_EL(0, 2), JPH_EL(0, 0), JPH_EL(0, 1), 0) * Vec4(JPH_EL(1, 1), JPH_EL(1, 2), JPH_EL(1, 0), 0)) / det, + Vec4(0, 0, 0, 1)); +} + +bool Mat44::SetInversed3x3(Mat44Arg inM) +{ + float det = inM.GetDeterminant3x3(); + + // If the determinant is zero the matrix is singular and we return false + if (det == 0.0f) + return false; + + // Finish calculating the inverse + *this = inM.Adjointed3x3(); + mCol[0] /= det; + mCol[1] /= det; + mCol[2] /= det; + return true; +} + +Quat Mat44::GetQuaternion() const +{ + float tr = mCol[0].mF32[0] + mCol[1].mF32[1] + mCol[2].mF32[2]; + + if (tr >= 0.0f) + { + float s = sqrt(tr + 1.0f); + float is = 0.5f / s; + return Quat( + (mCol[1].mF32[2] - mCol[2].mF32[1]) * is, + (mCol[2].mF32[0] - mCol[0].mF32[2]) * is, + (mCol[0].mF32[1] - mCol[1].mF32[0]) * is, + 0.5f * s); + } + else + { + int i = 0; + if (mCol[1].mF32[1] > mCol[0].mF32[0]) i = 1; + if (mCol[2].mF32[2] > mCol[i].mF32[i]) i = 2; + + if (i == 0) + { + float s = sqrt(mCol[0].mF32[0] - (mCol[1].mF32[1] + mCol[2].mF32[2]) + 1); + float is = 0.5f / s; + return Quat( + 0.5f * s, + (mCol[1].mF32[0] + mCol[0].mF32[1]) * is, + (mCol[0].mF32[2] + mCol[2].mF32[0]) * is, + (mCol[1].mF32[2] - mCol[2].mF32[1]) * is); + } + else if (i == 1) + { + float s = sqrt(mCol[1].mF32[1] - (mCol[2].mF32[2] + mCol[0].mF32[0]) + 1); + float is = 0.5f / s; + return Quat( + (mCol[1].mF32[0] + mCol[0].mF32[1]) * is, + 0.5f * s, + (mCol[2].mF32[1] + mCol[1].mF32[2]) * is, + (mCol[2].mF32[0] - mCol[0].mF32[2]) * is); + } + else + { + JPH_ASSERT(i == 2); + + float s = sqrt(mCol[2].mF32[2] - (mCol[0].mF32[0] + mCol[1].mF32[1]) + 1); + float is = 0.5f / s; + return Quat( + (mCol[0].mF32[2] + mCol[2].mF32[0]) * is, + (mCol[2].mF32[1] + mCol[1].mF32[2]) * is, + 0.5f * s, + (mCol[0].mF32[1] - mCol[1].mF32[0]) * is); + } + } +} + +Mat44 Mat44::sQuatLeftMultiply(QuatArg inQ) +{ + return Mat44( + Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle(), + Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle(), + Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle(), + inQ.mValue); +} + +Mat44 Mat44::sQuatRightMultiply(QuatArg inQ) +{ + return Mat44( + Vec4(1, -1, 1, -1) * inQ.mValue.Swizzle(), + Vec4(1, 1, -1, -1) * inQ.mValue.Swizzle(), + Vec4(-1, 1, 1, -1) * inQ.mValue.Swizzle(), + inQ.mValue); +} + +Mat44 Mat44::GetRotation() const +{ + JPH_ASSERT(mCol[0][3] == 0.0f); + JPH_ASSERT(mCol[1][3] == 0.0f); + JPH_ASSERT(mCol[2][3] == 0.0f); + + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(0, 0, 0, 1)); +} + +Mat44 Mat44::GetRotationSafe() const +{ +#if defined(JPH_USE_AVX512) + return Mat44(_mm_maskz_mov_ps(0b0111, mCol[0].mValue), + _mm_maskz_mov_ps(0b0111, mCol[1].mValue), + _mm_maskz_mov_ps(0b0111, mCol[2].mValue), + Vec4(0, 0, 0, 1)); +#elif defined(JPH_USE_SSE4_1) + __m128 zero = _mm_setzero_ps(); + return Mat44(_mm_blend_ps(mCol[0].mValue, zero, 8), + _mm_blend_ps(mCol[1].mValue, zero, 8), + _mm_blend_ps(mCol[2].mValue, zero, 8), + Vec4(0, 0, 0, 1)); +#elif defined(JPH_USE_NEON) + return Mat44(vsetq_lane_f32(0, mCol[0].mValue, 3), + vsetq_lane_f32(0, mCol[1].mValue, 3), + vsetq_lane_f32(0, mCol[2].mValue, 3), + Vec4(0, 0, 0, 1)); +#else + return Mat44(Vec4(mCol[0].mF32[0], mCol[0].mF32[1], mCol[0].mF32[2], 0), + Vec4(mCol[1].mF32[0], mCol[1].mF32[1], mCol[1].mF32[2], 0), + Vec4(mCol[2].mF32[0], mCol[2].mF32[1], mCol[2].mF32[2], 0), + Vec4(0, 0, 0, 1)); +#endif +} + +void Mat44::SetRotation(Mat44Arg inRotation) +{ + mCol[0] = inRotation.mCol[0]; + mCol[1] = inRotation.mCol[1]; + mCol[2] = inRotation.mCol[2]; +} + +Mat44 Mat44::PreTranslated(Vec3Arg inTranslation) const +{ + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + Multiply3x3(inTranslation), 1)); +} + +Mat44 Mat44::PostTranslated(Vec3Arg inTranslation) const +{ + return Mat44(mCol[0], mCol[1], mCol[2], Vec4(GetTranslation() + inTranslation, 1)); +} + +Mat44 Mat44::PreScaled(Vec3Arg inScale) const +{ + return Mat44(inScale.GetX() * mCol[0], inScale.GetY() * mCol[1], inScale.GetZ() * mCol[2], mCol[3]); +} + +Mat44 Mat44::PostScaled(Vec3Arg inScale) const +{ + Vec4 scale(inScale, 1); + return Mat44(scale * mCol[0], scale * mCol[1], scale * mCol[2], scale * mCol[3]); +} + +Mat44 Mat44::Decompose(Vec3 &outScale) const +{ + // Start the modified Gram-Schmidt algorithm + // X axis will just be normalized + Vec3 x = GetAxisX(); + + // Make Y axis perpendicular to X + Vec3 y = GetAxisY(); + float x_dot_x = x.LengthSq(); + y -= (x.Dot(y) / x_dot_x) * x; + + // Make Z axis perpendicular to X + Vec3 z = GetAxisZ(); + z -= (x.Dot(z) / x_dot_x) * x; + + // Make Z axis perpendicular to Y + float y_dot_y = y.LengthSq(); + z -= (y.Dot(z) / y_dot_y) * y; + + // Determine the scale + float z_dot_z = z.LengthSq(); + outScale = Vec3(x_dot_x, y_dot_y, z_dot_z).Sqrt(); + + // If the resulting x, y and z vectors don't form a right handed matrix, flip the z axis. + if (x.Cross(y).Dot(z) < 0.0f) + outScale.SetZ(-outScale.GetZ()); + + // Determine the rotation and translation + return Mat44(Vec4(x / outScale.GetX(), 0), Vec4(y / outScale.GetY(), 0), Vec4(z / outScale.GetZ(), 0), GetColumn4(3)); +} + +#undef JPH_EL + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Math.h b/WickedEngine/Jolt/Math/Math.h new file mode 100644 index 000000000..ffb72f7c0 --- /dev/null +++ b/WickedEngine/Jolt/Math/Math.h @@ -0,0 +1,203 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// The constant \f$\pi\f$ +static constexpr float JPH_PI = 3.14159265358979323846f; + +/// Convert a value from degrees to radians +JPH_INLINE constexpr float DegreesToRadians(float inV) +{ + return inV * (JPH_PI / 180.0f); +} + +/// Convert a value from radians to degrees +JPH_INLINE constexpr float RadiansToDegrees(float inV) +{ + return inV * (180.0f / JPH_PI); +} + +/// Convert angle in radians to the range \f$[-\pi, \pi]\f$ +inline float CenterAngleAroundZero(float inV) +{ + if (inV < -JPH_PI) + { + do + inV += 2.0f * JPH_PI; + while (inV < -JPH_PI); + } + else if (inV > JPH_PI) + { + do + inV -= 2.0f * JPH_PI; + while (inV > JPH_PI); + } + JPH_ASSERT(inV >= -JPH_PI && inV <= JPH_PI); + return inV; +} + +/// Clamp a value between two values +template +JPH_INLINE constexpr T Clamp(T inV, T inMin, T inMax) +{ + return min(max(inV, inMin), inMax); +} + +/// Square a value +template +JPH_INLINE constexpr T Square(T inV) +{ + return inV * inV; +} + +/// Returns \f$inV^3\f$. +template +JPH_INLINE constexpr T Cubed(T inV) +{ + return inV * inV * inV; +} + +/// Get the sign of a value +template +JPH_INLINE constexpr T Sign(T inV) +{ + return inV < 0? T(-1) : T(1); +} + +/// Check if inV is a power of 2 +template +constexpr bool IsPowerOf2(T inV) +{ + return (inV & (inV - 1)) == 0; +} + +/// Align inV up to the next inAlignment bytes +template +inline T AlignUp(T inV, uint64 inAlignment) +{ + JPH_ASSERT(IsPowerOf2(inAlignment)); + return T((uint64(inV) + inAlignment - 1) & ~(inAlignment - 1)); +} + +/// Check if inV is inAlignment aligned +template +inline bool IsAligned(T inV, uint64 inAlignment) +{ + JPH_ASSERT(IsPowerOf2(inAlignment)); + return (uint64(inV) & (inAlignment - 1)) == 0; +} + +/// Compute number of trailing zero bits (how many low bits are zero) +inline uint CountTrailingZeros(uint32 inValue) +{ +#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM) + #if defined(JPH_USE_TZCNT) + return _tzcnt_u32(inValue); + #elif defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanForward(&result, inValue); + return result; + #else + if (inValue == 0) + return 32; + return __builtin_ctz(inValue); + #endif +#elif defined(JPH_CPU_ARM) + #if defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanForward(&result, inValue); + return result; + #else + return __builtin_clz(__builtin_bitreverse32(inValue)); + #endif +#elif defined(JPH_CPU_E2K) + return inValue ? __builtin_ctz(inValue) : 32; +#else + #error Undefined +#endif +} + +/// Compute the number of leading zero bits (how many high bits are zero) +inline uint CountLeadingZeros(uint32 inValue) +{ +#if defined(JPH_CPU_X86) || defined(JPH_CPU_WASM) + #if defined(JPH_USE_LZCNT) + return _lzcnt_u32(inValue); + #elif defined(JPH_COMPILER_MSVC) + if (inValue == 0) + return 32; + unsigned long result; + _BitScanReverse(&result, inValue); + return 31 - result; + #else + if (inValue == 0) + return 32; + return __builtin_clz(inValue); + #endif +#elif defined(JPH_CPU_ARM) + #if defined(JPH_COMPILER_MSVC) + return _CountLeadingZeros(inValue); + #else + return __builtin_clz(inValue); + #endif +#elif defined(JPH_CPU_E2K) + return inValue ? __builtin_clz(inValue) : 32; +#else + #error Undefined +#endif +} + +/// Count the number of 1 bits in a value +inline uint CountBits(uint32 inValue) +{ +#if defined(JPH_COMPILER_CLANG) || defined(JPH_COMPILER_GCC) + return __builtin_popcount(inValue); +#elif defined(JPH_COMPILER_MSVC) + #if defined(JPH_USE_SSE4_2) + return _mm_popcnt_u32(inValue); + #elif defined(JPH_USE_NEON) && (_MSC_VER >= 1930) // _CountOneBits not available on MSVC2019 + return _CountOneBits(inValue); + #else + inValue = inValue - ((inValue >> 1) & 0x55555555); + inValue = (inValue & 0x33333333) + ((inValue >> 2) & 0x33333333); + inValue = (inValue + (inValue >> 4)) & 0x0F0F0F0F; + return (inValue * 0x01010101) >> 24; + #endif +#else + #error Undefined +#endif +} + +/// Get the next higher power of 2 of a value, or the value itself if the value is already a power of 2 +inline uint32 GetNextPowerOf2(uint32 inValue) +{ + return inValue <= 1? uint32(1) : uint32(1) << (32 - CountLeadingZeros(inValue - 1)); +} + +// Simple implementation of C++20 std::bit_cast (unfortunately not constexpr) +template +JPH_INLINE To BitCast(const From &inValue) +{ + static_assert(std::is_trivially_constructible_v); + static_assert(sizeof(From) == sizeof(To)); + + union FromTo + { + To mTo; + From mFrom; + }; + + FromTo convert; + convert.mFrom = inValue; + return convert.mTo; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/MathTypes.h b/WickedEngine/Jolt/Math/MathTypes.h new file mode 100644 index 000000000..8d019ae42 --- /dev/null +++ b/WickedEngine/Jolt/Math/MathTypes.h @@ -0,0 +1,34 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Vec3; +class DVec3; +class Vec4; +class UVec4; +class Vec8; +class UVec8; +class Quat; +class Mat44; +class DMat44; + +// Types to use for passing arguments to functions +using Vec3Arg = const Vec3; +#ifdef JPH_USE_AVX + using DVec3Arg = const DVec3; +#else + using DVec3Arg = const DVec3 &; +#endif +using Vec4Arg = const Vec4; +using UVec4Arg = const UVec4; +using Vec8Arg = const Vec8; +using UVec8Arg = const UVec8; +using QuatArg = const Quat; +using Mat44Arg = const Mat44 &; +using DMat44Arg = const DMat44 &; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Matrix.h b/WickedEngine/Jolt/Math/Matrix.h new file mode 100644 index 000000000..031665bbe --- /dev/null +++ b/WickedEngine/Jolt/Math/Matrix.h @@ -0,0 +1,259 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Templatized matrix class +template +class [[nodiscard]] Matrix +{ +public: + /// Constructor + inline Matrix() = default; + inline Matrix(const Matrix &inM2) { *this = inM2; } + + /// Dimensions + inline uint GetRows() const { return Rows; } + inline uint GetCols() const { return Cols; } + + /// Zero matrix + inline void SetZero() + { + for (uint c = 0; c < Cols; ++c) + mCol[c].SetZero(); + } + + inline static Matrix sZero() { Matrix m; m.SetZero(); return m; } + + /// Check if this matrix consists of all zeros + inline bool IsZero() const + { + for (uint c = 0; c < Cols; ++c) + if (!mCol[c].IsZero()) + return false; + + return true; + } + + /// Identity matrix + inline void SetIdentity() + { + // Clear matrix + SetZero(); + + // Set diagonal to 1 + for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc) + mCol[rc].mF32[rc] = 1.0f; + } + + inline static Matrix sIdentity() { Matrix m; m.SetIdentity(); return m; } + + /// Check if this matrix is identity + bool IsIdentity() const { return *this == sIdentity(); } + + /// Diagonal matrix + inline void SetDiagonal(const Vector &inV) + { + // Clear matrix + SetZero(); + + // Set diagonal + for (uint rc = 0, min_rc = min(Rows, Cols); rc < min_rc; ++rc) + mCol[rc].mF32[rc] = inV[rc]; + } + + inline static Matrix sDiagonal(const Vector &inV) + { + Matrix m; + m.SetDiagonal(inV); + return m; + } + + /// Copy a (part) of another matrix into this matrix + template + void CopyPart(const OtherMatrix &inM, uint inSourceRow, uint inSourceCol, uint inNumRows, uint inNumCols, uint inDestRow, uint inDestCol) + { + for (uint c = 0; c < inNumCols; ++c) + for (uint r = 0; r < inNumRows; ++r) + mCol[inDestCol + c].mF32[inDestRow + r] = inM(inSourceRow + r, inSourceCol + c); + } + + /// Get float component by element index + inline float operator () (uint inRow, uint inColumn) const + { + JPH_ASSERT(inRow < Rows); + JPH_ASSERT(inColumn < Cols); + return mCol[inColumn].mF32[inRow]; + } + + inline float & operator () (uint inRow, uint inColumn) + { + JPH_ASSERT(inRow < Rows); + JPH_ASSERT(inColumn < Cols); + return mCol[inColumn].mF32[inRow]; + } + + /// Comparison + inline bool operator == (const Matrix &inM2) const + { + for (uint c = 0; c < Cols; ++c) + if (mCol[c] != inM2.mCol[c]) + return false; + return true; + } + + inline bool operator != (const Matrix &inM2) const + { + for (uint c = 0; c < Cols; ++c) + if (mCol[c] != inM2.mCol[c]) + return true; + return false; + } + + /// Assignment + inline Matrix & operator = (const Matrix &inM2) + { + for (uint c = 0; c < Cols; ++c) + mCol[c] = inM2.mCol[c]; + return *this; + } + + /// Multiply matrix by matrix + template + inline Matrix operator * (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < OtherCols; ++c) + for (uint r = 0; r < Rows; ++r) + { + float dot = 0.0f; + for (uint i = 0; i < Cols; ++i) + dot += mCol[i].mF32[r] * inM.mCol[c].mF32[i]; + m.mCol[c].mF32[r] = dot; + } + return m; + } + + /// Multiply vector by matrix + inline Vector operator * (const Vector &inV) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + { + float dot = 0.0f; + for (uint c = 0; c < Cols; ++c) + dot += mCol[c].mF32[r] * inV.mF32[c]; + v.mF32[r] = dot; + } + return v; + } + + /// Multiply matrix with float + inline Matrix operator * (float inV) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] * inV; + return m; + } + + inline friend Matrix operator * (float inV, const Matrix &inM) + { + return inM * inV; + } + + /// Per element addition of matrix + inline Matrix operator + (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] + inM.mCol[c]; + return m; + } + + /// Per element subtraction of matrix + inline Matrix operator - (const Matrix &inM) const + { + Matrix m; + for (uint c = 0; c < Cols; ++c) + m.mCol[c] = mCol[c] - inM.mCol[c]; + return m; + } + + /// Transpose matrix + inline Matrix Transposed() const + { + Matrix m; + for (uint r = 0; r < Rows; ++r) + for (uint c = 0; c < Cols; ++c) + m.mCol[r].mF32[c] = mCol[c].mF32[r]; + return m; + } + + /// Inverse matrix + bool SetInversed(const Matrix &inM) + { + if constexpr (Rows != Cols) JPH_ASSERT(false); + Matrix copy(inM); + SetIdentity(); + return GaussianElimination(copy, *this); + } + + inline Matrix Inversed() const + { + Matrix m; + m.SetInversed(*this); + return m; + } + + /// To String + friend ostream & operator << (ostream &inStream, const Matrix &inM) + { + for (uint i = 0; i < Cols - 1; ++i) + inStream << inM.mCol[i] << ", "; + inStream << inM.mCol[Cols - 1]; + return inStream; + } + + /// Column access + const Vector & GetColumn(int inIdx) const { return mCol[inIdx]; } + Vector & GetColumn(int inIdx) { return mCol[inIdx]; } + + Vector mCol[Cols]; ///< Column +}; + +// The template specialization doesn't sit well with Doxygen +#ifndef JPH_PLATFORM_DOXYGEN + +/// Specialization of SetInversed for 2x2 matrix +template <> +inline bool Matrix<2, 2>::SetInversed(const Matrix<2, 2> &inM) +{ + // Fetch elements + float a = inM.mCol[0].mF32[0]; + float b = inM.mCol[1].mF32[0]; + float c = inM.mCol[0].mF32[1]; + float d = inM.mCol[1].mF32[1]; + + // Calculate determinant + float det = a * d - b * c; + if (det == 0.0f) + return false; + + // Construct inverse + mCol[0].mF32[0] = d / det; + mCol[1].mF32[0] = -b / det; + mCol[0].mF32[1] = -c / det; + mCol[1].mF32[1] = a / det; + return true; +} + +#endif // !JPH_PLATFORM_DOXYGEN + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Quat.h b/WickedEngine/Jolt/Math/Quat.h new file mode 100644 index 000000000..10564cab1 --- /dev/null +++ b/WickedEngine/Jolt/Math/Quat.h @@ -0,0 +1,255 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Quaternion class, quaternions are 4 dimensional vectors which can describe rotations in 3 dimensional +/// space if their length is 1. +/// +/// They are written as: +/// +/// \f$q = w + x \: i + y \: j + z \: k\f$ +/// +/// or in vector notation: +/// +/// \f$q = [w, v] = [w, x, y, z]\f$ +/// +/// Where: +/// +/// w = the real part +/// v = the imaginary part, (x, y, z) +/// +/// Note that we store the quaternion in a Vec4 as [x, y, z, w] because that makes +/// it easy to extract the rotation axis of the quaternion: +/// +/// q = [cos(angle / 2), sin(angle / 2) * rotation_axis] +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Quat +{ +public: + JPH_OVERRIDE_NEW_DELETE + + ///@name Constructors + ///@{ + inline Quat() = default; ///< Intentionally not initialized for performance reasons + Quat(const Quat &inRHS) = default; + Quat & operator = (const Quat &inRHS) = default; + inline Quat(float inX, float inY, float inZ, float inW) : mValue(inX, inY, inZ, inW) { } + inline explicit Quat(Vec4Arg inV) : mValue(inV) { } + ///@} + + ///@name Tests + ///@{ + + /// Check if two quaternions are exactly equal + inline bool operator == (QuatArg inRHS) const { return mValue == inRHS.mValue; } + + /// Check if two quaternions are different + inline bool operator != (QuatArg inRHS) const { return mValue != inRHS.mValue; } + + /// If this quaternion is close to inRHS. Note that q and -q represent the same rotation, this is not checked here. + inline bool IsClose(QuatArg inRHS, float inMaxDistSq = 1.0e-12f) const { return mValue.IsClose(inRHS.mValue, inMaxDistSq); } + + /// If the length of this quaternion is 1 +/- inTolerance + inline bool IsNormalized(float inTolerance = 1.0e-5f) const { return mValue.IsNormalized(inTolerance); } + + /// If any component of this quaternion is a NaN (not a number) + inline bool IsNaN() const { return mValue.IsNaN(); } + + ///@} + ///@name Get components + ///@{ + + /// Get X component (imaginary part i) + JPH_INLINE float GetX() const { return mValue.GetX(); } + + /// Get Y component (imaginary part j) + JPH_INLINE float GetY() const { return mValue.GetY(); } + + /// Get Z component (imaginary part k) + JPH_INLINE float GetZ() const { return mValue.GetZ(); } + + /// Get W component (real part) + JPH_INLINE float GetW() const { return mValue.GetW(); } + + /// Get the imaginary part of the quaternion + JPH_INLINE Vec3 GetXYZ() const { return Vec3(mValue); } + + /// Get the quaternion as a Vec4 + JPH_INLINE Vec4 GetXYZW() const { return mValue; } + + /// Set individual components + JPH_INLINE void SetX(float inX) { mValue.SetX(inX); } + JPH_INLINE void SetY(float inY) { mValue.SetY(inY); } + JPH_INLINE void SetZ(float inZ) { mValue.SetZ(inZ); } + JPH_INLINE void SetW(float inW) { mValue.SetW(inW); } + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ, float inW) { mValue.Set(inX, inY, inZ, inW); } + + ///@} + ///@name Default quaternions + ///@{ + + /// @return [0, 0, 0, 0] + JPH_INLINE static Quat sZero() { return Quat(Vec4::sZero()); } + + /// @return [1, 0, 0, 0] (or in storage format Quat(0, 0, 0, 1)) + JPH_INLINE static Quat sIdentity() { return Quat(0, 0, 0, 1); } + + ///@} + + /// Rotation from axis and angle + JPH_INLINE static Quat sRotation(Vec3Arg inAxis, float inAngle); + + /// Get axis and angle that represents this quaternion, outAngle will always be in the range \f$[0, \pi]\f$ + JPH_INLINE void GetAxisAngle(Vec3 &outAxis, float &outAngle) const; + + /// Create quaternion that rotates a vector from the direction of inFrom to the direction of inTo along the shortest path + /// @see https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm + JPH_INLINE static Quat sFromTo(Vec3Arg inFrom, Vec3Arg inTo); + + /// Random unit quaternion + template + inline static Quat sRandom(Random &inRandom); + + /// Conversion from Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians. + inline static Quat sEulerAngles(Vec3Arg inAngles); + + /// Conversion to Euler angles. Rotation order is X then Y then Z (RotZ * RotY * RotX). Angles in radians. + inline Vec3 GetEulerAngles() const; + + ///@name Length / normalization operations + ///@{ + + /// Squared length of quaternion. + /// @return Squared length of quaternion (\f$|v|^2\f$) + JPH_INLINE float LengthSq() const { return mValue.LengthSq(); } + + /// Length of quaternion. + /// @return Length of quaternion (\f$|v|\f$) + JPH_INLINE float Length() const { return mValue.Length(); } + + /// Normalize the quaternion (make it length 1) + JPH_INLINE Quat Normalized() const { return Quat(mValue.Normalized()); } + + ///@} + ///@name Additions / multiplications + ///@{ + + JPH_INLINE void operator += (QuatArg inRHS) { mValue += inRHS.mValue; } + JPH_INLINE void operator -= (QuatArg inRHS) { mValue -= inRHS.mValue; } + JPH_INLINE void operator *= (float inValue) { mValue *= inValue; } + JPH_INLINE void operator /= (float inValue) { mValue /= inValue; } + JPH_INLINE Quat operator - () const { return Quat(-mValue); } + JPH_INLINE Quat operator + (QuatArg inRHS) const { return Quat(mValue + inRHS.mValue); } + JPH_INLINE Quat operator - (QuatArg inRHS) const { return Quat(mValue - inRHS.mValue); } + JPH_INLINE Quat operator * (QuatArg inRHS) const; + JPH_INLINE Quat operator * (float inValue) const { return Quat(mValue * inValue); } + inline friend Quat operator * (float inValue, QuatArg inRHS) { return Quat(inRHS.mValue * inValue); } + JPH_INLINE Quat operator / (float inValue) const { return Quat(mValue / inValue); } + + ///@} + + /// Rotate a vector by this quaternion + JPH_INLINE Vec3 operator * (Vec3Arg inValue) const; + + /// Rotate a vector by the inverse of this quaternion + JPH_INLINE Vec3 InverseRotate(Vec3Arg inValue) const; + + /// Rotate a the vector (1, 0, 0) with this quaternion + JPH_INLINE Vec3 RotateAxisX() const; + + /// Rotate a the vector (0, 1, 0) with this quaternion + JPH_INLINE Vec3 RotateAxisY() const; + + /// Rotate a the vector (0, 0, 1) with this quaternion + JPH_INLINE Vec3 RotateAxisZ() const; + + /// Dot product + JPH_INLINE float Dot(QuatArg inRHS) const { return mValue.Dot(inRHS.mValue); } + + /// The conjugate [w, -x, -y, -z] is the same as the inverse for unit quaternions + JPH_INLINE Quat Conjugated() const { return Quat(Vec4::sXor(mValue, UVec4(0x80000000, 0x80000000, 0x80000000, 0).ReinterpretAsFloat())); } + + /// Get inverse quaternion + JPH_INLINE Quat Inversed() const { return Conjugated() / Length(); } + + /// Ensures that the W component is positive by negating the entire quaternion if it is not. This is useful when you want to store a quaternion as a 3 vector by discarding W and reconstructing it as sqrt(1 - x^2 - y^2 - z^2). + JPH_INLINE Quat EnsureWPositive() const { return Quat(Vec4::sXor(mValue, Vec4::sAnd(mValue.SplatW(), UVec4::sReplicate(0x80000000).ReinterpretAsFloat()))); } + + /// Get a quaternion that is perpendicular to this quaternion + JPH_INLINE Quat GetPerpendicular() const { return Quat(Vec4(1, -1, 1, -1) * mValue.Swizzle()); } + + /// Get rotation angle around inAxis (uses Swing Twist Decomposition to get the twist quaternion and uses q(axis, angle) = [cos(angle / 2), axis * sin(angle / 2)]) + JPH_INLINE float GetRotationAngle(Vec3Arg inAxis) const { return GetW() == 0.0f? JPH_PI : 2.0f * ATan(GetXYZ().Dot(inAxis) / GetW()); } + + /// Swing Twist Decomposition: any quaternion can be split up as: + /// + /// \f[q = q_{swing} \: q_{twist}\f] + /// + /// where \f$q_{twist}\f$ rotates only around axis v. + /// + /// \f$q_{twist}\f$ is: + /// + /// \f[q_{twist} = \frac{[q_w, q_{ijk} \cdot v \: v]}{\left|[q_w, q_{ijk} \cdot v \: v]\right|}\f] + /// + /// where q_w is the real part of the quaternion and q_i the imaginary part (a 3 vector). + /// + /// The swing can then be calculated as: + /// + /// \f[q_{swing} = q \: q_{twist}^* \f] + /// + /// Where \f$q_{twist}^*\f$ = complex conjugate of \f$q_{twist}\f$ + JPH_INLINE Quat GetTwist(Vec3Arg inAxis) const; + + /// Decomposes quaternion into swing and twist component: + /// + /// \f$q = q_{swing} \: q_{twist}\f$ + /// + /// where \f$q_{swing} \: \hat{x} = q_{twist} \: \hat{y} = q_{twist} \: \hat{z} = 0\f$ + /// + /// In other words: + /// + /// - \f$q_{twist}\f$ only rotates around the X-axis. + /// - \f$q_{swing}\f$ only rotates around the Y and Z-axis. + /// + /// @see Gino van den Bergen - Rotational Joint Limits in Quaternion Space - GDC 2016 + JPH_INLINE void GetSwingTwist(Quat &outSwing, Quat &outTwist) const; + + /// Linear interpolation between two quaternions (for small steps). + /// @param inFraction is in the range [0, 1] + /// @param inDestination The destination quaternion + /// @return (1 - inFraction) * this + fraction * inDestination + JPH_INLINE Quat LERP(QuatArg inDestination, float inFraction) const; + + /// Spherical linear interpolation between two quaternions. + /// @param inFraction is in the range [0, 1] + /// @param inDestination The destination quaternion + /// @return When fraction is zero this quaternion is returned, when fraction is 1 inDestination is returned. + /// When fraction is between 0 and 1 an interpolation along the shortest path is returned. + JPH_INLINE Quat SLERP(QuatArg inDestination, float inFraction) const; + + /// Load 3 floats from memory (X, Y and Z component and then calculates W) reads 32 bits extra which it doesn't use + static JPH_INLINE Quat sLoadFloat3Unsafe(const Float3 &inV); + + /// Store 3 as floats to memory (X, Y and Z component) + JPH_INLINE void StoreFloat3(Float3 *outV) const; + + /// To String + friend ostream & operator << (ostream &inStream, QuatArg inQ) { inStream << inQ.mValue; return inStream; } + + /// 4 vector that stores [x, y, z, w] parts of the quaternion + Vec4 mValue; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Quat.inl" diff --git a/WickedEngine/Jolt/Math/Quat.inl b/WickedEngine/Jolt/Math/Quat.inl new file mode 100644 index 000000000..72b78341b --- /dev/null +++ b/WickedEngine/Jolt/Math/Quat.inl @@ -0,0 +1,328 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +Quat Quat::operator * (QuatArg inRHS) const +{ +#if defined(JPH_USE_SSE4_1) + // Taken from: http://momchil-velikov.blogspot.nl/2013/10/fast-sse-quternion-multiplication.html + __m128 abcd = mValue.mValue; + __m128 xyzw = inRHS.mValue.mValue; + + __m128 t0 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(3, 3, 3, 3)); + __m128 t1 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 3, 0, 1)); + + __m128 t3 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(0, 0, 0, 0)); + __m128 t4 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(1, 0, 3, 2)); + + __m128 t5 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(1, 1, 1, 1)); + __m128 t6 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(2, 0, 3, 1)); + + // [d,d,d,d] * [z,w,x,y] = [dz,dw,dx,dy] + __m128 m0 = _mm_mul_ps(t0, t1); + + // [a,a,a,a] * [y,x,w,z] = [ay,ax,aw,az] + __m128 m1 = _mm_mul_ps(t3, t4); + + // [b,b,b,b] * [z,x,w,y] = [bz,bx,bw,by] + __m128 m2 = _mm_mul_ps(t5, t6); + + // [c,c,c,c] * [w,z,x,y] = [cw,cz,cx,cy] + __m128 t7 = _mm_shuffle_ps(abcd, abcd, _MM_SHUFFLE(2, 2, 2, 2)); + __m128 t8 = _mm_shuffle_ps(xyzw, xyzw, _MM_SHUFFLE(3, 2, 0, 1)); + __m128 m3 = _mm_mul_ps(t7, t8); + + // [dz,dw,dx,dy] + -[ay,ax,aw,az] = [dz+ay,dw-ax,dx+aw,dy-az] + __m128 e = _mm_addsub_ps(m0, m1); + + // [dx+aw,dz+ay,dy-az,dw-ax] + e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(1, 3, 0, 2)); + + // [dx+aw,dz+ay,dy-az,dw-ax] + -[bz,bx,bw,by] = [dx+aw+bz,dz+ay-bx,dy-az+bw,dw-ax-by] + e = _mm_addsub_ps(e, m2); + + // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + e = _mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 0, 1, 3)); + + // [dz+ay-bx,dw-ax-by,dy-az+bw,dx+aw+bz] + -[cw,cz,cx,cy] = [dz+ay-bx+cw,dw-ax-by-cz,dy-az+bw+cx,dx+aw+bz-cy] + e = _mm_addsub_ps(e, m3); + + // [dw-ax-by-cz,dz+ay-bx+cw,dy-az+bw+cx,dx+aw+bz-cy] + return Quat(Vec4(_mm_shuffle_ps(e, e, _MM_SHUFFLE(2, 3, 1, 0)))); +#else + float lx = mValue.GetX(); + float ly = mValue.GetY(); + float lz = mValue.GetZ(); + float lw = mValue.GetW(); + + float rx = inRHS.mValue.GetX(); + float ry = inRHS.mValue.GetY(); + float rz = inRHS.mValue.GetZ(); + float rw = inRHS.mValue.GetW(); + + float x = lw * rx + lx * rw + ly * rz - lz * ry; + float y = lw * ry - lx * rz + ly * rw + lz * rx; + float z = lw * rz + lx * ry - ly * rx + lz * rw; + float w = lw * rw - lx * rx - ly * ry - lz * rz; + + return Quat(x, y, z, w); +#endif +} + +Quat Quat::sRotation(Vec3Arg inAxis, float inAngle) +{ + // returns [inAxis * sin(0.5f * inAngle), cos(0.5f * inAngle)] + JPH_ASSERT(inAxis.IsNormalized()); + Vec4 s, c; + Vec4::sReplicate(0.5f * inAngle).SinCos(s, c); + return Quat(Vec4::sSelect(Vec4(inAxis) * s, c, UVec4(0, 0, 0, 0xffffffffU))); +} + +void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const +{ + JPH_ASSERT(IsNormalized()); + Quat w_pos = EnsureWPositive(); + float abs_w = w_pos.GetW(); + if (abs_w >= 1.0f) + { + outAxis = Vec3::sZero(); + outAngle = 0.0f; + } + else + { + outAngle = 2.0f * ACos(abs_w); + outAxis = w_pos.GetXYZ().NormalizedOr(Vec3::sZero()); + } +} + +Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo) +{ + /* + Uses (inFrom = v1, inTo = v2): + + angle = arcos(v1 . v2 / |v1||v2|) + axis = normalize(v1 x v2) + + Quaternion is then: + + s = sin(angle / 2) + x = axis.x * s + y = axis.y * s + z = axis.z * s + w = cos(angle / 2) + + Using identities: + + sin(2 * a) = 2 * sin(a) * cos(a) + cos(2 * a) = cos(a)^2 - sin(a)^2 + sin(a)^2 + cos(a)^2 = 1 + + This reduces to: + + x = (v1 x v2).x + y = (v1 x v2).y + z = (v1 x v2).z + w = |v1||v2| + v1 . v2 + + which then needs to be normalized because the whole equation was multiplied by 2 cos(angle / 2) + */ + + float len_v1_v2 = sqrt(inFrom.LengthSq() * inTo.LengthSq()); + float w = len_v1_v2 + inFrom.Dot(inTo); + + if (w == 0.0f) + { + if (len_v1_v2 == 0.0f) + { + // If either of the vectors has zero length, there is no rotation and we return identity + return Quat::sIdentity(); + } + else + { + // If vectors are perpendicular, take one of the many 180 degree rotations that exist + return Quat(Vec4(inFrom.GetNormalizedPerpendicular(), 0)); + } + } + + Vec3 v = inFrom.Cross(inTo); + return Quat(Vec4(v, w)).Normalized(); +} + +template +Quat Quat::sRandom(Random &inRandom) +{ + std::uniform_real_distribution zero_to_one(0.0f, 1.0f); + float x0 = zero_to_one(inRandom); + float r1 = sqrt(1.0f - x0), r2 = sqrt(x0); + std::uniform_real_distribution zero_to_two_pi(0.0f, 2.0f * JPH_PI); + Vec4 s, c; + Vec4(zero_to_two_pi(inRandom), zero_to_two_pi(inRandom), 0, 0).SinCos(s, c); + return Quat(s.GetX() * r1, c.GetX() * r1, s.GetY() * r2, c.GetY() * r2); +} + +Quat Quat::sEulerAngles(Vec3Arg inAngles) +{ + Vec4 half(0.5f * inAngles); + Vec4 s, c; + half.SinCos(s, c); + + float cx = c.GetX(); + float sx = s.GetX(); + float cy = c.GetY(); + float sy = s.GetY(); + float cz = c.GetZ(); + float sz = s.GetZ(); + + return Quat( + cz * sx * cy - sz * cx * sy, + cz * cx * sy + sz * sx * cy, + sz * cx * cy - cz * sx * sy, + cz * cx * cy + sz * sx * sy); +} + +Vec3 Quat::GetEulerAngles() const +{ + float y_sq = GetY() * GetY(); + + // X + float t0 = 2.0f * (GetW() * GetX() + GetY() * GetZ()); + float t1 = 1.0f - 2.0f * (GetX() * GetX() + y_sq); + + // Y + float t2 = 2.0f * (GetW() * GetY() - GetZ() * GetX()); + t2 = t2 > 1.0f? 1.0f : t2; + t2 = t2 < -1.0f? -1.0f : t2; + + // Z + float t3 = 2.0f * (GetW() * GetZ() + GetX() * GetY()); + float t4 = 1.0f - 2.0f * (y_sq + GetZ() * GetZ()); + + return Vec3(ATan2(t0, t1), ASin(t2), ATan2(t3, t4)); +} + +Quat Quat::GetTwist(Vec3Arg inAxis) const +{ + Quat twist(Vec4(GetXYZ().Dot(inAxis) * inAxis, GetW())); + float twist_len = twist.LengthSq(); + if (twist_len != 0.0f) + return twist / sqrt(twist_len); + else + return Quat::sIdentity(); +} + +void Quat::GetSwingTwist(Quat &outSwing, Quat &outTwist) const +{ + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float s = sqrt(Square(w) + Square(x)); + if (s != 0.0f) + { + outTwist = Quat(x / s, 0, 0, w / s); + outSwing = Quat(0, (w * y - x * z) / s, (w * z + x * y) / s, s); + } + else + { + // If both x and w are zero, this must be a 180 degree rotation around either y or z + outTwist = Quat::sIdentity(); + outSwing = *this; + } +} + +Quat Quat::LERP(QuatArg inDestination, float inFraction) const +{ + float scale0 = 1.0f - inFraction; + return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(inFraction) * inDestination.mValue); +} + +Quat Quat::SLERP(QuatArg inDestination, float inFraction) const +{ + // Difference at which to LERP instead of SLERP + const float delta = 0.0001f; + + // Calc cosine + float sign_scale1 = 1.0f; + float cos_omega = Dot(inDestination); + + // Adjust signs (if necessary) + if (cos_omega < 0.0f) + { + cos_omega = -cos_omega; + sign_scale1 = -1.0f; + } + + // Calculate coefficients + float scale0, scale1; + if (1.0f - cos_omega > delta) + { + // Standard case (slerp) + float omega = ACos(cos_omega); + float sin_omega = Sin(omega); + scale0 = Sin((1.0f - inFraction) * omega) / sin_omega; + scale1 = sign_scale1 * Sin(inFraction * omega) / sin_omega; + } + else + { + // Quaternions are very close so we can do a linear interpolation + scale0 = 1.0f - inFraction; + scale1 = sign_scale1 * inFraction; + } + + // Interpolate between the two quaternions + return Quat(Vec4::sReplicate(scale0) * mValue + Vec4::sReplicate(scale1) * inDestination.mValue).Normalized(); +} + +Vec3 Quat::operator * (Vec3Arg inValue) const +{ + // Rotating a vector by a quaternion is done by: p' = q * p * q^-1 (q^-1 = conjugated(q) for a unit quaternion) + JPH_ASSERT(IsNormalized()); + return Vec3((*this * Quat(Vec4(inValue, 0)) * Conjugated()).mValue); +} + +Vec3 Quat::InverseRotate(Vec3Arg inValue) const +{ + JPH_ASSERT(IsNormalized()); + return Vec3((Conjugated() * Quat(Vec4(inValue, 0)) * *this).mValue); +} + +Vec3 Quat::RotateAxisX() const +{ + // This is *this * Vec3::sAxisX() written out: + JPH_ASSERT(IsNormalized()); + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float tx = 2.0f * x, tw = 2.0f * w; + return Vec3(tx * x + tw * w - 1.0f, tx * y + z * tw, tx * z - y * tw); +} + +Vec3 Quat::RotateAxisY() const +{ + // This is *this * Vec3::sAxisY() written out: + JPH_ASSERT(IsNormalized()); + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float ty = 2.0f * y, tw = 2.0f * w; + return Vec3(x * ty - z * tw, tw * w + ty * y - 1.0f, x * tw + ty * z); +} + +Vec3 Quat::RotateAxisZ() const +{ + // This is *this * Vec3::sAxisZ() written out: + JPH_ASSERT(IsNormalized()); + float x = GetX(), y = GetY(), z = GetZ(), w = GetW(); + float tz = 2.0f * z, tw = 2.0f * w; + return Vec3(x * tz + y * tw, y * tz - x * tw, tw * w + tz * z - 1.0f); +} + +void Quat::StoreFloat3(Float3 *outV) const +{ + JPH_ASSERT(IsNormalized()); + EnsureWPositive().GetXYZ().StoreFloat3(outV); +} + +Quat Quat::sLoadFloat3Unsafe(const Float3 &inV) +{ + Vec3 v = Vec3::sLoadFloat3Unsafe(inV); + float w = sqrt(max(1.0f - v.LengthSq(), 0.0f)); // It is possible that the length of v is a fraction above 1, and we don't want to introduce NaN's in that case so we clamp to 0 + return Quat(Vec4(v, w)); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Real.h b/WickedEngine/Jolt/Math/Real.h new file mode 100644 index 000000000..7773abf7e --- /dev/null +++ b/WickedEngine/Jolt/Math/Real.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DOUBLE_PRECISION + +// Define real to double +using Real = double; +using Real3 = Double3; +using RVec3 = DVec3; +using RVec3Arg = DVec3Arg; +using RMat44 = DMat44; +using RMat44Arg = DMat44Arg; + +#define JPH_RVECTOR_ALIGNMENT JPH_DVECTOR_ALIGNMENT + +#else + +// Define real to float +using Real = float; +using Real3 = Float3; +using RVec3 = Vec3; +using RVec3Arg = Vec3Arg; +using RMat44 = Mat44; +using RMat44Arg = Mat44Arg; + +#define JPH_RVECTOR_ALIGNMENT JPH_VECTOR_ALIGNMENT + +#endif // JPH_DOUBLE_PRECISION + +// Put the 'real' operator in a namespace so that users can opt in to use it: +// using namespace JPH::literals; +namespace literals { + constexpr Real operator ""_r (long double inValue) { return Real(inValue); } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Swizzle.h b/WickedEngine/Jolt/Math/Swizzle.h new file mode 100644 index 000000000..ad8dfbc14 --- /dev/null +++ b/WickedEngine/Jolt/Math/Swizzle.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum indicating which component to use when swizzling +enum +{ + SWIZZLE_X = 0, ///< Use the X component + SWIZZLE_Y = 1, ///< Use the Y component + SWIZZLE_Z = 2, ///< Use the Z component + SWIZZLE_W = 3, ///< Use the W component + SWIZZLE_UNUSED = 2, ///< We always use the Z component when we don't specifically want to initialize a value, this is consistent with what is done in Vec3(x, y, z), Vec3(Float3 &) and Vec3::sLoadFloat3Unsafe +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Trigonometry.h b/WickedEngine/Jolt/Math/Trigonometry.h new file mode 100644 index 000000000..3503dd17b --- /dev/null +++ b/WickedEngine/Jolt/Math/Trigonometry.h @@ -0,0 +1,79 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +// Note that this file exists because std::sin etc. are not platform independent and will lead to non-deterministic simulation + +/// Sine of x (input in radians) +JPH_INLINE float Sin(float inX) +{ + Vec4 s, c; + Vec4::sReplicate(inX).SinCos(s, c); + return s.GetX(); +} + +/// Cosine of x (input in radians) +JPH_INLINE float Cos(float inX) +{ + Vec4 s, c; + Vec4::sReplicate(inX).SinCos(s, c); + return c.GetX(); +} + +/// Tangent of x (input in radians) +JPH_INLINE float Tan(float inX) +{ + return Vec4::sReplicate(inX).Tan().GetX(); +} + +/// Arc sine of x (returns value in the range [-PI / 2, PI / 2]) +/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin +JPH_INLINE float ASin(float inX) +{ + return Vec4::sReplicate(inX).ASin().GetX(); +} + +/// Arc cosine of x (returns value in the range [0, PI]) +/// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos +JPH_INLINE float ACos(float inX) +{ + return Vec4::sReplicate(inX).ACos().GetX(); +} + +/// An approximation of ACos, max error is 4.2e-3 over the entire range [-1, 1], is approximately 2.5x faster than ACos +JPH_INLINE float ACosApproximate(float inX) +{ + // See: https://www.johndcook.com/blog/2022/09/06/inverse-cosine-near-1/ + // See also: https://seblagarde.wordpress.com/2014/12/01/inverse-trigonometric-functions-gpu-optimization-for-amd-gcn-architecture/ + // Taylor of cos(x) = 1 - x^2 / 2 + ... + // Substitute x = sqrt(2 y) we get: cos(sqrt(2 y)) = 1 - y + // Substitute z = 1 - y we get: cos(sqrt(2 (1 - z))) = z <=> acos(z) = sqrt(2 (1 - z)) + // To avoid the discontinuity at 1, instead of using the Taylor expansion of acos(x) we use acos(x) / sqrt(2 (1 - x)) = 1 + (1 - x) / 12 + ... + // Since the approximation was made at 1, it has quite a large error at 0 meaning that if we want to extend to the + // range [-1, 1] by mirroring the range [0, 1], the value at 0+ is not the same as 0-. + // So we observe that the form of the Taylor expansion is f(x) = sqrt(1 - x) * (a + b x) and we fit the function so that f(0) = pi / 2 + // this gives us a = pi / 2. f(1) = 0 regardless of b. We search for a constant b that minimizes the error in the range [0, 1]. + float abs_x = min(abs(inX), 1.0f); // Ensure that we don't get a value larger than 1 + float val = sqrt(1.0f - abs_x) * (JPH_PI / 2 - 0.175394f * abs_x); + + // Our approximation is valid in the range [0, 1], extend it to the range [-1, 1] + return inX < 0? JPH_PI - val : val; +} + +/// Arc tangent of x (returns value in the range [-PI / 2, PI / 2]) +JPH_INLINE float ATan(float inX) +{ + return Vec4::sReplicate(inX).ATan().GetX(); +} + +/// Arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI]) +JPH_INLINE float ATan2(float inY, float inX) +{ + return Vec4::sATan2(Vec4::sReplicate(inY), Vec4::sReplicate(inX)).GetX(); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/UVec4.h b/WickedEngine/Jolt/Math/UVec4.h new file mode 100644 index 000000000..4855e2296 --- /dev/null +++ b/WickedEngine/Jolt/Math/UVec4.h @@ -0,0 +1,220 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) UVec4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128i; +#elif defined(JPH_USE_NEON) + using Type = uint32x4_t; +#else + using Type = struct { uint32 mData[4]; }; +#endif + + /// Constructor + UVec4() = default; ///< Intentionally not initialized for performance reasons + UVec4(const UVec4 &inRHS) = default; + UVec4 & operator = (const UVec4 &inRHS) = default; + JPH_INLINE UVec4(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 4 integer components + JPH_INLINE UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW); + + /// Comparison + JPH_INLINE bool operator == (UVec4Arg inV2) const; + JPH_INLINE bool operator != (UVec4Arg inV2) const { return !(*this == inV2); } + + /// Swizzle the elements in inV + template + JPH_INLINE UVec4 Swizzle() const; + + /// Vector with all zeros + static JPH_INLINE UVec4 sZero(); + + /// Replicate int inV across all components + static JPH_INLINE UVec4 sReplicate(uint32 inV); + + /// Load 1 int from memory and place it in the X component, zeros Y, Z and W + static JPH_INLINE UVec4 sLoadInt(const uint32 *inV); + + /// Load 4 ints from memory + static JPH_INLINE UVec4 sLoadInt4(const uint32 *inV); + + /// Load 4 ints from memory, aligned to 16 bytes + static JPH_INLINE UVec4 sLoadInt4Aligned(const uint32 *inV); + + /// Gather 4 ints from memory at inBase + inOffsets[i] * Scale + template + static JPH_INLINE UVec4 sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets); + + /// Return the minimum value of each of the components + static JPH_INLINE UVec4 sMin(UVec4Arg inV1, UVec4Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE UVec4 sMax(UVec4Arg inV1, UVec4Arg inV2); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(UVec4Arg inV1, UVec4Arg inV2); + + /// Component wise select, returns inV1 when highest bit of inControl = 0 and inV2 when highest bit of inControl = 1 + static JPH_INLINE UVec4 sSelect(UVec4Arg inV1, UVec4Arg inV2, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE UVec4 sOr(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE UVec4 sXor(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE UVec4 sAnd(UVec4Arg inV1, UVec4Arg inV2); + + /// Logical not (component wise) + static JPH_INLINE UVec4 sNot(UVec4Arg inV1); + + /// Sorts the elements in inIndex so that the values that correspond to trues in inValue are the first elements. + /// The remaining elements will be set to inValue.w. + /// I.e. if inValue = (true, false, true, false) and inIndex = (1, 2, 3, 4) the function returns (1, 3, 4, 4). + static JPH_INLINE UVec4 sSort4True(UVec4Arg inValue, UVec4Arg inIndex); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE uint32 GetX() const { return uint32(_mm_cvtsi128_si32(mValue)); } + JPH_INLINE uint32 GetY() const { return mU32[1]; } + JPH_INLINE uint32 GetZ() const { return mU32[2]; } + JPH_INLINE uint32 GetW() const { return mU32[3]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE uint32 GetX() const { return vgetq_lane_u32(mValue, 0); } + JPH_INLINE uint32 GetY() const { return vgetq_lane_u32(mValue, 1); } + JPH_INLINE uint32 GetZ() const { return vgetq_lane_u32(mValue, 2); } + JPH_INLINE uint32 GetW() const { return vgetq_lane_u32(mValue, 3); } +#else + JPH_INLINE uint32 GetX() const { return mU32[0]; } + JPH_INLINE uint32 GetY() const { return mU32[1]; } + JPH_INLINE uint32 GetZ() const { return mU32[2]; } + JPH_INLINE uint32 GetW() const { return mU32[3]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(uint32 inX) { mU32[0] = inX; } + JPH_INLINE void SetY(uint32 inY) { mU32[1] = inY; } + JPH_INLINE void SetZ(uint32 inZ) { mU32[2] = inZ; } + JPH_INLINE void SetW(uint32 inW) { mU32[3] = inW; } + + /// Get component by index + JPH_INLINE uint32 operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; } + JPH_INLINE uint32 & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 4); return mU32[inCoordinate]; } + + /// Multiplies each of the 4 integer components with an integer (discards any overflow) + JPH_INLINE UVec4 operator * (UVec4Arg inV2) const; + + /// Adds an integer value to all integer components (discards any overflow) + JPH_INLINE UVec4 operator + (UVec4Arg inV2); + + /// Add two integer vectors (component wise) + JPH_INLINE UVec4 & operator += (UVec4Arg inV2); + + /// Replicate the X component to all components + JPH_INLINE UVec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE UVec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE UVec4 SplatZ() const; + + /// Replicate the W component to all components + JPH_INLINE UVec4 SplatW() const; + + /// Convert each component from an int to a float + JPH_INLINE Vec4 ToFloat() const; + + /// Reinterpret UVec4 as a Vec4 (doesn't change the bits) + JPH_INLINE Vec4 ReinterpretAsFloat() const; + + /// Store 4 ints to memory + JPH_INLINE void StoreInt4(uint32 *outV) const; + + /// Store 4 ints to memory, aligned to 16 bytes + JPH_INLINE void StoreInt4Aligned(uint32 *outV) const; + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if any of X, Y or Z components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyXYZTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Test if X, Y and Z components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllXYZTrue() const; + + /// Count the number of components that are true (true is when highest bit of component is set) + JPH_INLINE int CountTrues() const; + + /// Store if X is true in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 (true is when highest bit of component is set) + JPH_INLINE int GetTrues() const; + + /// Shift all components by Count bits to the left (filling with zeros from the left) + template + JPH_INLINE UVec4 LogicalShiftLeft() const; + + /// Shift all components by Count bits to the right (filling with zeros from the right) + template + JPH_INLINE UVec4 LogicalShiftRight() const; + + /// Shift all components by Count bits to the right (shifting in the value of the highest bit) + template + JPH_INLINE UVec4 ArithmeticShiftRight() const; + + /// Takes the lower 4 16 bits and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Uint16Lo() const; + + /// Takes the upper 4 16 bits and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Uint16Hi() const; + + /// Takes byte 0 .. 3 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte0() const; + + /// Takes byte 4 .. 7 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte4() const; + + /// Takes byte 8 .. 11 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte8() const; + + /// Takes byte 12 .. 15 and expands them to X, Y, Z and W + JPH_INLINE UVec4 Expand4Byte12() const; + + /// Shift vector components by 4 - Count floats to the left, so if Count = 1 the resulting vector is (W, 0, 0, 0), when Count = 3 the resulting vector is (Y, Z, W, 0) + JPH_INLINE UVec4 ShiftComponents4Minus(int inCount) const; + + /// To String + friend ostream & operator << (ostream &inStream, UVec4Arg inV) + { + inStream << inV.mU32[0] << ", " << inV.mU32[1] << ", " << inV.mU32[2] << ", " << inV.mU32[3]; + return inStream; + } + + union + { + Type mValue; + uint32 mU32[4]; + }; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "UVec4.inl" diff --git a/WickedEngine/Jolt/Math/UVec4.inl b/WickedEngine/Jolt/Math/UVec4.inl new file mode 100644 index 000000000..e01d66b11 --- /dev/null +++ b/WickedEngine/Jolt/Math/UVec4.inl @@ -0,0 +1,573 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +UVec4::UVec4(uint32 inX, uint32 inY, uint32 inZ, uint32 inW) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_epi32(int(inW), int(inZ), int(inY), int(inX)); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_u32(static_cast(inX) | (static_cast(inY) << 32)); + uint32x2_t zw = vcreate_u32(static_cast(inZ) | (static_cast(inW) << 32)); + mValue = vcombine_u32(xy, zw); +#else + mU32[0] = inX; + mU32[1] = inY; + mU32[2] = inZ; + mU32[3] = inW; +#endif +} + +bool UVec4::operator == (UVec4Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +template +UVec4 UVec4::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX)); +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW); +#else + return UVec4(mU32[SwizzleX], mU32[SwizzleY], mU32[SwizzleZ], mU32[SwizzleW]); +#endif +} + +UVec4 UVec4::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_si128(); +#elif defined(JPH_USE_NEON) + return vdupq_n_u32(0); +#else + return UVec4(0, 0, 0, 0); +#endif +} + +UVec4 UVec4::sReplicate(uint32 inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_epi32(int(inV)); +#elif defined(JPH_USE_NEON) + return vdupq_n_u32(inV); +#else + return UVec4(inV, inV, inV, inV); +#endif +} + +UVec4 UVec4::sLoadInt(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_load_ss(reinterpret_cast(inV))); +#elif defined(JPH_USE_NEON) + return vsetq_lane_u32(*inV, vdupq_n_u32(0), 0); +#else + return UVec4(*inV, 0, 0, 0); +#endif +} + +UVec4 UVec4::sLoadInt4(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u32(inV); +#else + return UVec4(inV[0], inV[1], inV[2], inV[3]); +#endif +} + +UVec4 UVec4::sLoadInt4Aligned(const uint32 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_load_si128(reinterpret_cast(inV)); +#elif defined(JPH_USE_NEON) + return vld1q_u32(inV); // ARM doesn't make distinction between aligned or not +#else + return UVec4(inV[0], inV[1], inV[2], inV[3]); +#endif +} + +template +UVec4 UVec4::sGatherInt4(const uint32 *inBase, UVec4Arg inOffsets) +{ +#ifdef JPH_USE_AVX2 + return _mm_i32gather_epi32(reinterpret_cast(inBase), inOffsets.mValue, Scale); +#else + return Vec4::sGatherFloat4(reinterpret_cast(inBase), inOffsets).ReinterpretAsInt(); +#endif +} + +UVec4 UVec4::sMin(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_min_epu32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_u32(inV1.mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = min(inV1.mU32[i], inV2.mU32[i]); + return result; +#endif +} + +UVec4 UVec4::sMax(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_max_epu32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_u32(inV1.mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = max(inV1.mU32[i], inV2.mU32[i]); + return result; +#endif +} + +UVec4 UVec4::sEquals(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_cmpeq_epi32(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vceqq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] == inV2.mU32[0]? 0xffffffffu : 0, + inV1.mU32[1] == inV2.mU32[1]? 0xffffffffu : 0, + inV1.mU32[2] == inV2.mU32[2]? 0xffffffffu : 0, + inV1.mU32[3] == inV2.mU32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 UVec4::sSelect(UVec4Arg inV1, UVec4Arg inV2, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(inV1.mValue), _mm_castsi128_ps(inV2.mValue), _mm_castsi128_ps(inControl.mValue))); +#elif defined(JPH_USE_NEON) + return vbslq_u32(vshrq_n_s32(inControl.mValue, 31), inV2.mValue, inV1.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = inControl.mU32[i] ? inV2.mU32[i] : inV1.mU32[i]; + return result; +#endif +} + +UVec4 UVec4::sOr(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vorrq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] | inV2.mU32[0], + inV1.mU32[1] | inV2.mU32[1], + inV1.mU32[2] | inV2.mU32[2], + inV1.mU32[3] | inV2.mU32[3]); +#endif +} + +UVec4 UVec4::sXor(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return veorq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] ^ inV2.mU32[0], + inV1.mU32[1] ^ inV2.mU32[1], + inV1.mU32[2] ^ inV2.mU32[2], + inV1.mU32[3] ^ inV2.mU32[3]); +#endif +} + +UVec4 UVec4::sAnd(UVec4Arg inV1, UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_si128(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vandq_u32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mU32[0] & inV2.mU32[0], + inV1.mU32[1] & inV2.mU32[1], + inV1.mU32[2] & inV2.mU32[2], + inV1.mU32[3] & inV2.mU32[3]); +#endif +} + + +UVec4 UVec4::sNot(UVec4Arg inV1) +{ +#if defined(JPH_USE_AVX512) + return _mm_ternarylogic_epi32(inV1.mValue, inV1.mValue, inV1.mValue, 0b01010101); +#elif defined(JPH_USE_SSE) + return sXor(inV1, sReplicate(0xffffffff)); +#elif defined(JPH_USE_NEON) + return vmvnq_u32(inV1.mValue); +#else + return UVec4(~inV1.mU32[0], ~inV1.mU32[1], ~inV1.mU32[2], ~inV1.mU32[3]); +#endif +} + +UVec4 UVec4::sSort4True(UVec4Arg inValue, UVec4Arg inIndex) +{ + // If inValue.z is false then shift W to Z + UVec4 v = UVec4::sSelect(inIndex.Swizzle(), inIndex, inValue.SplatZ()); + + // If inValue.y is false then shift Z and further to Y and further + v = UVec4::sSelect(v.Swizzle(), v, inValue.SplatY()); + + // If inValue.x is false then shift X and further to Y and further + v = UVec4::sSelect(v.Swizzle(), v, inValue.SplatX()); + + return v; +} + +UVec4 UVec4::operator * (UVec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_mullo_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_u32(mValue, inV2.mValue); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = mU32[i] * inV2.mU32[i]; + return result; +#endif +} + +UVec4 UVec4::operator + (UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_add_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_u32(mValue, inV2.mValue); +#else + return UVec4(mU32[0] + inV2.mU32[0], + mU32[1] + inV2.mU32[1], + mU32[2] + inV2.mU32[2], + mU32[3] + inV2.mU32[3]); +#endif +} + +UVec4 &UVec4::operator += (UVec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_epi32(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_u32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mU32[i] += inV2.mU32[i]; +#endif + return *this; +} + +UVec4 UVec4::SplatX() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 0); +#else + return UVec4(mU32[0], mU32[0], mU32[0], mU32[0]); +#endif +} + +UVec4 UVec4::SplatY() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 1); +#else + return UVec4(mU32[1], mU32[1], mU32[1], mU32[1]); +#endif +} + +UVec4 UVec4::SplatZ() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 2); +#else + return UVec4(mU32[2], mU32[2], mU32[2], mU32[2]); +#endif +} + +UVec4 UVec4::SplatW() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_epi32(mValue, _MM_SHUFFLE(3, 3, 3, 3)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_u32(mValue, 3); +#else + return UVec4(mU32[3], mU32[3], mU32[3], mU32[3]); +#endif +} + +Vec4 UVec4::ToFloat() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvtepi32_ps(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_f32_s32(mValue); +#else + return Vec4((float)mU32[0], (float)mU32[1], (float)mU32[2], (float)mU32[3]); +#endif +} + +Vec4 UVec4::ReinterpretAsFloat() const +{ +#if defined(JPH_USE_SSE) + return Vec4(_mm_castsi128_ps(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_f32_s32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +void UVec4::StoreInt4(uint32 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_storeu_si128(reinterpret_cast<__m128i *>(outV), mValue); +#elif defined(JPH_USE_NEON) + vst1q_u32(outV, mValue); +#else + for (int i = 0; i < 4; ++i) + outV[i] = mU32[i]; +#endif +} + +void UVec4::StoreInt4Aligned(uint32 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_store_si128(reinterpret_cast<__m128i *>(outV), mValue); +#elif defined(JPH_USE_NEON) + vst1q_u32(outV, mValue); // ARM doesn't make distinction between aligned or not +#else + for (int i = 0; i < 4; ++i) + outV[i] = mU32[i]; +#endif +} + +int UVec4::CountTrues() const +{ +#if defined(JPH_USE_SSE) + return CountBits(_mm_movemask_ps(_mm_castsi128_ps(mValue))); +#elif defined(JPH_USE_NEON) + return vaddvq_u32(vshrq_n_u32(mValue, 31)); +#else + return (mU32[0] >> 31) + (mU32[1] >> 31) + (mU32[2] >> 31) + (mU32[3] >> 31); +#endif +} + +int UVec4::GetTrues() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_ps(_mm_castsi128_ps(mValue)); +#elif defined(JPH_USE_NEON) + int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3); + return vaddvq_u32(vshlq_u32(vshrq_n_u32(mValue, 31), shift)); +#else + return (mU32[0] >> 31) | ((mU32[1] >> 31) << 1) | ((mU32[2] >> 31) << 2) | ((mU32[3] >> 31) << 3); +#endif +} + +bool UVec4::TestAnyTrue() const +{ + return GetTrues() != 0; +} + +bool UVec4::TestAnyXYZTrue() const +{ + return (GetTrues() & 0b111) != 0; +} + +bool UVec4::TestAllTrue() const +{ + return GetTrues() == 0b1111; +} + +bool UVec4::TestAllXYZTrue() const +{ + return (GetTrues() & 0b111) == 0b111; +} + +template +UVec4 UVec4::LogicalShiftLeft() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_slli_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vshlq_n_u32(mValue, Count); +#else + return UVec4(mU32[0] << Count, mU32[1] << Count, mU32[2] << Count, mU32[3] << Count); +#endif +} + +template +UVec4 UVec4::LogicalShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_srli_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vshrq_n_u32(mValue, Count); +#else + return UVec4(mU32[0] >> Count, mU32[1] >> Count, mU32[2] >> Count, mU32[3] >> Count); +#endif +} + +template +UVec4 UVec4::ArithmeticShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#if defined(JPH_USE_SSE) + return _mm_srai_epi32(mValue, Count); +#elif defined(JPH_USE_NEON) + return vshrq_n_s32(mValue, Count); +#else + return UVec4(uint32(int32_t(mU32[0]) >> Count), + uint32(int32_t(mU32[1]) >> Count), + uint32(int32_t(mU32[2]) >> Count), + uint32(int32_t(mU32[3]) >> Count)); +#endif +} + +UVec4 UVec4::Expand4Uint16Lo() const +{ +#if defined(JPH_USE_SSE) + return _mm_unpacklo_epi16(mValue, _mm_castps_si128(_mm_setzero_ps())); +#elif defined(JPH_USE_NEON) + int16x4_t value = vget_low_s16(mValue); + int16x4_t zero = vdup_n_s16(0); + return vcombine_s16(vzip1_s16(value, zero), vzip2_s16(value, zero)); +#else + return UVec4(mU32[0] & 0xffff, + (mU32[0] >> 16) & 0xffff, + mU32[1] & 0xffff, + (mU32[1] >> 16) & 0xffff); +#endif +} + +UVec4 UVec4::Expand4Uint16Hi() const +{ +#if defined(JPH_USE_SSE) + return _mm_unpackhi_epi16(mValue, _mm_castps_si128(_mm_setzero_ps())); +#elif defined(JPH_USE_NEON) + int16x4_t value = vget_high_s16(mValue); + int16x4_t zero = vdup_n_s16(0); + return vcombine_s16(vzip1_s16(value, zero), vzip2_s16(value, zero)); +#else + return UVec4(mU32[2] & 0xffff, + (mU32[2] >> 16) & 0xffff, + mU32[3] & 0xffff, + (mU32[3] >> 16) & 0xffff); +#endif +} + +UVec4 UVec4::Expand4Byte0() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff03), int(0xffffff02), int(0xffffff01), int(0xffffff00))); +#elif defined(JPH_USE_NEON) + int8x16_t idx = JPH_NEON_INT8x16(0x00, 0x7f, 0x7f, 0x7f, 0x01, 0x7f, 0x7f, 0x7f, 0x02, 0x7f, 0x7f, 0x7f, 0x03, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[0] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte4() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff07), int(0xffffff06), int(0xffffff05), int(0xffffff04))); +#elif defined(JPH_USE_NEON) + int8x16_t idx = JPH_NEON_INT8x16(0x04, 0x7f, 0x7f, 0x7f, 0x05, 0x7f, 0x7f, 0x7f, 0x06, 0x7f, 0x7f, 0x7f, 0x07, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[1] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte8() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0b), int(0xffffff0a), int(0xffffff09), int(0xffffff08))); +#elif defined(JPH_USE_NEON) + int8x16_t idx = JPH_NEON_INT8x16(0x08, 0x7f, 0x7f, 0x7f, 0x09, 0x7f, 0x7f, 0x7f, 0x0a, 0x7f, 0x7f, 0x7f, 0x0b, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[2] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::Expand4Byte12() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, _mm_set_epi32(int(0xffffff0f), int(0xffffff0e), int(0xffffff0d), int(0xffffff0c))); +#elif defined(JPH_USE_NEON) + int8x16_t idx = JPH_NEON_INT8x16(0x0c, 0x7f, 0x7f, 0x7f, 0x0d, 0x7f, 0x7f, 0x7f, 0x0e, 0x7f, 0x7f, 0x7f, 0x0f, 0x7f, 0x7f, 0x7f); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result; + for (int i = 0; i < 4; i++) + result.mU32[i] = (mU32[3] >> (i * 8)) & 0xff; + return result; +#endif +} + +UVec4 UVec4::ShiftComponents4Minus(int inCount) const +{ +#if defined(JPH_USE_SSE4_1) || defined(JPH_USE_NEON) + alignas(UVec4) static constexpr uint32 sFourMinusXShuffle[5][4] = + { + { 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }, + { 0x0f0e0d0c, 0xffffffff, 0xffffffff, 0xffffffff }, + { 0x0b0a0908, 0x0f0e0d0c, 0xffffffff, 0xffffffff }, + { 0x07060504, 0x0b0a0908, 0x0f0e0d0c, 0xffffffff }, + { 0x03020100, 0x07060504, 0x0b0a0908, 0x0f0e0d0c } + }; +#endif + +#if defined(JPH_USE_SSE4_1) + return _mm_shuffle_epi8(mValue, *reinterpret_cast(sFourMinusXShuffle[inCount])); +#elif defined(JPH_USE_NEON) + uint8x16_t idx = vreinterpretq_u8_u32(*reinterpret_cast(sFourMinusXShuffle[inCount])); + return vreinterpretq_u32_s8(vqtbl1q_s8(vreinterpretq_s8_u32(mValue), idx)); +#else + UVec4 result = UVec4::sZero(); + for (int i = 0; i < inCount; i++) + result.mU32[i] = mU32[i + 4 - inCount]; + return result; +#endif +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/UVec8.h b/WickedEngine/Jolt/Math/UVec8.h new file mode 100644 index 000000000..bab31cbeb --- /dev/null +++ b/WickedEngine/Jolt/Math/UVec8.h @@ -0,0 +1,100 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] UVec8 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + UVec8() = default; ///< Intentionally not initialized for performance reasons + UVec8(const UVec8 &inRHS) = default; + JPH_INLINE UVec8(__m256i inRHS) : mValue(inRHS) { } + + /// Set 256 bit vector from 2 128 bit vectors + JPH_INLINE UVec8(UVec4Arg inLo, UVec4Arg inHi); + + /// Comparison + JPH_INLINE bool operator == (UVec8Arg inV2) const; + JPH_INLINE bool operator != (UVec8Arg inV2) const { return !(*this == inV2); } + + /// Replicate int across all components + static JPH_INLINE UVec8 sReplicate(uint32 inV); + + /// Replicate the X component of inV to all components + static JPH_INLINE UVec8 sSplatX(UVec4Arg inV); + + /// Replicate the Y component of inV to all components + static JPH_INLINE UVec8 sSplatY(UVec4Arg inV); + + /// Replicate the Z component of inV to all components + static JPH_INLINE UVec8 sSplatZ(UVec4Arg inV); + + /// Equals (component wise) + static JPH_INLINE UVec8 sEquals(UVec8Arg inV1, UVec8Arg inV2); + + /// Component wise select, returns inV1 when highest bit of inControl = 0 and inV2 when highest bit of inControl = 1 + static JPH_INLINE UVec8 sSelect(UVec8Arg inV1, UVec8Arg inV2, UVec8Arg inControl); + + /// Logical or + static JPH_INLINE UVec8 sOr(UVec8Arg inV1, UVec8Arg inV2); + + /// Logical xor + static JPH_INLINE UVec8 sXor(UVec8Arg inV1, UVec8Arg inV2); + + /// Logical and + static JPH_INLINE UVec8 sAnd(UVec8Arg inV1, UVec8Arg inV2); + + /// Get float component by index + JPH_INLINE uint32 operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 8); return mU32[inCoordinate]; } + JPH_INLINE uint32 & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 8); return mU32[inCoordinate]; } + + /// 256 bit variant of Vec::Swizzle (no cross 128 bit lane swizzle) + template + JPH_INLINE UVec8 Swizzle() const; + + /// Test if any of the components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAnyTrue() const; + + /// Test if all components are true (true is when highest bit of component is set) + JPH_INLINE bool TestAllTrue() const; + + /// Fetch the lower 128 bit from a 256 bit variable + JPH_INLINE UVec4 LowerVec4() const; + + /// Fetch the higher 128 bit from a 256 bit variable + JPH_INLINE UVec4 UpperVec4() const; + + /// Converts int to float + JPH_INLINE Vec8 ToFloat() const; + + /// Shift all components by Count bits to the left (filling with zeros from the left) + template + JPH_INLINE UVec8 LogicalShiftLeft() const; + + /// Shift all components by Count bits to the right (filling with zeros from the right) + template + JPH_INLINE UVec8 LogicalShiftRight() const; + + /// Shift all components by Count bits to the right (shifting in the value of the highest bit) + template + JPH_INLINE UVec8 ArithmeticShiftRight() const; + + union + { + __m256i mValue; + uint32 mU32[8]; + }; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "UVec8.inl" diff --git a/WickedEngine/Jolt/Math/UVec8.inl b/WickedEngine/Jolt/Math/UVec8.inl new file mode 100644 index 000000000..0d7d3117c --- /dev/null +++ b/WickedEngine/Jolt/Math/UVec8.inl @@ -0,0 +1,138 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +JPH_NAMESPACE_BEGIN + +UVec8::UVec8(UVec4Arg inLo, UVec4Arg inHi) : + mValue(_mm256_insertf128_si256(_mm256_castsi128_si256(inLo.mValue), inHi.mValue, 1)) +{ +} + +bool UVec8::operator == (UVec8Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +UVec8 UVec8::sReplicate(uint32 inV) +{ + return _mm256_set1_epi32(int(inV)); +} + +UVec8 UVec8::sSplatX(UVec4Arg inV) +{ + return _mm256_set1_epi32(inV.GetX()); +} + +UVec8 UVec8::sSplatY(UVec4Arg inV) +{ + return _mm256_set1_epi32(inV.GetY()); +} + +UVec8 UVec8::sSplatZ(UVec4Arg inV) +{ + return _mm256_set1_epi32(inV.GetZ()); +} + +UVec8 UVec8::sEquals(UVec8Arg inV1, UVec8Arg inV2) +{ +#ifdef JPH_USE_AVX2 + return _mm256_cmpeq_epi32(inV1.mValue, inV2.mValue); +#else + return UVec8(UVec4::sEquals(inV1.LowerVec4(), inV2.LowerVec4()), UVec4::sEquals(inV1.UpperVec4(), inV2.UpperVec4())); +#endif +} + +UVec8 UVec8::sSelect(UVec8Arg inV1, UVec8Arg inV2, UVec8Arg inControl) +{ + return _mm256_castps_si256(_mm256_blendv_ps(_mm256_castsi256_ps(inV1.mValue), _mm256_castsi256_ps(inV2.mValue), _mm256_castsi256_ps(inControl.mValue))); +} + +UVec8 UVec8::sOr(UVec8Arg inV1, UVec8Arg inV2) +{ + return _mm256_castps_si256(_mm256_or_ps(_mm256_castsi256_ps(inV1.mValue), _mm256_castsi256_ps(inV2.mValue))); +} + +UVec8 UVec8::sXor(UVec8Arg inV1, UVec8Arg inV2) +{ + return _mm256_castps_si256(_mm256_xor_ps(_mm256_castsi256_ps(inV1.mValue), _mm256_castsi256_ps(inV2.mValue))); +} + +UVec8 UVec8::sAnd(UVec8Arg inV1, UVec8Arg inV2) +{ + return _mm256_castps_si256(_mm256_and_ps(_mm256_castsi256_ps(inV1.mValue), _mm256_castsi256_ps(inV2.mValue))); +} + +template +UVec8 UVec8::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + + return _mm256_castps_si256(_mm256_shuffle_ps(_mm256_castsi256_ps(mValue), _mm256_castsi256_ps(mValue), _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX))); +} + +bool UVec8::TestAnyTrue() const +{ + return _mm256_movemask_ps(_mm256_castsi256_ps(mValue)) != 0; +} + +bool UVec8::TestAllTrue() const +{ + return _mm256_movemask_ps(_mm256_castsi256_ps(mValue)) == 0xff; +} + +UVec4 UVec8::LowerVec4() const +{ + return _mm256_castsi256_si128(mValue); +} + +UVec4 UVec8::UpperVec4() const +{ + return _mm_castps_si128(_mm256_extractf128_ps(_mm256_castsi256_ps(mValue), 1)); +} + +Vec8 UVec8::ToFloat() const +{ + return _mm256_cvtepi32_ps(mValue); +} + +template +UVec8 UVec8::LogicalShiftLeft() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#ifdef JPH_USE_AVX2 + return _mm256_slli_epi32(mValue, Count); +#else + return UVec8(LowerVec4().LogicalShiftLeft(), UpperVec4().LogicalShiftLeft()); +#endif +} + +template +UVec8 UVec8::LogicalShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#ifdef JPH_USE_AVX2 + return _mm256_srli_epi32(mValue, Count); +#else + return UVec8(LowerVec4().LogicalShiftRight(), UpperVec4().LogicalShiftRight()); +#endif +} + +template +UVec8 UVec8::ArithmeticShiftRight() const +{ + static_assert(Count <= 31, "Invalid shift"); + +#ifdef JPH_USE_AVX2 + return _mm256_srai_epi32(mValue, Count); +#else + return UVec8(LowerVec4().ArithmeticShiftRight(), UpperVec4().ArithmeticShiftRight()); +#endif +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Vec3.cpp b/WickedEngine/Jolt/Math/Vec3.cpp new file mode 100644 index 000000000..c865387f7 --- /dev/null +++ b/WickedEngine/Jolt/Math/Vec3.cpp @@ -0,0 +1,71 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +static void sAddVertex(StaticArray &ioVertices, Vec3Arg inVertex) +{ + bool found = false; + for (const Vec3 &v : ioVertices) + if (v == inVertex) + { + found = true; + break; + } + if (!found) + ioVertices.push_back(inVertex); +} + +static void sCreateVertices(StaticArray &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel) +{ + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + sAddVertex(ioVertices, center1); + sAddVertex(ioVertices, center2); + sAddVertex(ioVertices, center3); + + if (inLevel > 0) + { + int new_level = inLevel - 1; + sCreateVertices(ioVertices, inDir1, center1, center3, new_level); + sCreateVertices(ioVertices, center1, center2, center3, new_level); + sCreateVertices(ioVertices, center1, inDir2, center2, new_level); + sCreateVertices(ioVertices, center3, center2, inDir3, new_level); + } +} + +const StaticArray Vec3::sUnitSphere = []() { + + const int level = 3; + + StaticArray verts; + + // Add unit axis + verts.push_back(Vec3::sAxisX()); + verts.push_back(-Vec3::sAxisX()); + verts.push_back(Vec3::sAxisY()); + verts.push_back(-Vec3::sAxisY()); + verts.push_back(Vec3::sAxisZ()); + verts.push_back(-Vec3::sAxisZ()); + + // Subdivide + sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level); + sCreateVertices(verts, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), level); + + return verts; +}(); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Vec3.h b/WickedEngine/Jolt/Math/Vec3.h new file mode 100644 index 000000000..94f6bf665 --- /dev/null +++ b/WickedEngine/Jolt/Math/Vec3.h @@ -0,0 +1,295 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// 3 component vector (stored as 4 vectors). +/// Note that we keep the 4th component the same as the 3rd component to avoid divisions by zero when JPH_FLOATING_POINT_EXCEPTIONS_ENABLED defined +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec3 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128; +#elif defined(JPH_USE_NEON) + using Type = float32x4_t; +#else + using Type = Vec4::Type; +#endif + + // Argument type + using ArgType = Vec3Arg; + + /// Constructor + Vec3() = default; ///< Intentionally not initialized for performance reasons + Vec3(const Vec3 &inRHS) = default; + Vec3 & operator = (const Vec3 &inRHS) = default; + explicit JPH_INLINE Vec3(Vec4Arg inRHS); + JPH_INLINE Vec3(Type inRHS) : mValue(inRHS) { CheckW(); } + + /// Load 3 floats from memory + explicit JPH_INLINE Vec3(const Float3 &inV); + + /// Create a vector from 3 components + JPH_INLINE Vec3(float inX, float inY, float inZ); + + /// Vector with all zeros + static JPH_INLINE Vec3 sZero(); + + /// Vector with all NaN's + static JPH_INLINE Vec3 sNaN(); + + /// Vectors with the principal axis + static JPH_INLINE Vec3 sAxisX() { return Vec3(1, 0, 0); } + static JPH_INLINE Vec3 sAxisY() { return Vec3(0, 1, 0); } + static JPH_INLINE Vec3 sAxisZ() { return Vec3(0, 0, 1); } + + /// Replicate inV across all components + static JPH_INLINE Vec3 sReplicate(float inV); + + /// Load 3 floats from memory (reads 32 bits extra which it doesn't use) + static JPH_INLINE Vec3 sLoadFloat3Unsafe(const Float3 &inV); + + /// Return the minimum value of each of the components + static JPH_INLINE Vec3 sMin(Vec3Arg inV1, Vec3Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE Vec3 sMax(Vec3Arg inV1, Vec3Arg inV2); + + /// Clamp a vector between min and max (component wise) + static JPH_INLINE Vec3 sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(Vec3Arg inV1, Vec3Arg inV2); + + /// Less than (component wise) + static JPH_INLINE UVec4 sLess(Vec3Arg inV1, Vec3Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE UVec4 sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE UVec4 sGreater(Vec3Arg inV1, Vec3Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE UVec4 sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE Vec3 sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd); + + /// Component wise select, returns inV1 when highest bit of inControl = 0 and inV2 when highest bit of inControl = 1 + static JPH_INLINE Vec3 sSelect(Vec3Arg inV1, Vec3Arg inV2, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE Vec3 sOr(Vec3Arg inV1, Vec3Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE Vec3 sXor(Vec3Arg inV1, Vec3Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE Vec3 sAnd(Vec3Arg inV1, Vec3Arg inV2); + + /// Get unit vector given spherical coordinates + /// inTheta \f$\in [0, \pi]\f$ is angle between vector and z-axis + /// inPhi \f$\in [0, 2 \pi]\f$ is the angle in the xy-plane starting from the x axis and rotating counter clockwise around the z-axis + static JPH_INLINE Vec3 sUnitSpherical(float inTheta, float inPhi); + + /// A set of vectors uniformly spanning the surface of a unit sphere, usable for debug purposes + JPH_EXPORT static const StaticArray sUnitSphere; + + /// Get random unit vector + template + static inline Vec3 sRandom(Random &inRandom); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE float GetX() const { return _mm_cvtss_f32(mValue); } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE float GetX() const { return vgetq_lane_f32(mValue, 0); } + JPH_INLINE float GetY() const { return vgetq_lane_f32(mValue, 1); } + JPH_INLINE float GetZ() const { return vgetq_lane_f32(mValue, 2); } +#else + JPH_INLINE float GetX() const { return mF32[0]; } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(float inX) { mF32[0] = inX; } + JPH_INLINE void SetY(float inY) { mF32[1] = inY; } + JPH_INLINE void SetZ(float inZ) { mF32[2] = mF32[3] = inZ; } // Assure Z and W are the same + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ) { *this = Vec3(inX, inY, inZ); } + + /// Get float component by index + JPH_INLINE float operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 3); return mF32[inCoordinate]; } + + /// Set float component by index + JPH_INLINE void SetComponent(uint inCoordinate, float inValue) { JPH_ASSERT(inCoordinate < 3); mF32[inCoordinate] = inValue; mValue = sFixW(mValue); } // Assure Z and W are the same + + /// Comparison + JPH_INLINE bool operator == (Vec3Arg inV2) const; + JPH_INLINE bool operator != (Vec3Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(Vec3Arg inV2, float inMaxDistSq = 1.0e-12f) const; + + /// Test if vector is near zero + JPH_INLINE bool IsNearZero(float inMaxDistSq = 1.0e-12f) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(float inTolerance = 1.0e-6f) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two float vectors (component wise) + JPH_INLINE Vec3 operator * (Vec3Arg inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec3 operator * (float inV2) const; + + /// Multiply vector with float + friend JPH_INLINE Vec3 operator * (float inV1, Vec3Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec3 operator / (float inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec3 & operator *= (float inV2); + + /// Multiply vector with vector + JPH_INLINE Vec3 & operator *= (Vec3Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec3 & operator /= (float inV2); + + /// Add two float vectors (component wise) + JPH_INLINE Vec3 operator + (Vec3Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec3 & operator += (Vec3Arg inV2); + + /// Negate + JPH_INLINE Vec3 operator - () const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec3 operator - (Vec3Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec3 & operator -= (Vec3Arg inV2); + + /// Divide (component wise) + JPH_INLINE Vec3 operator / (Vec3Arg inV2) const; + + /// Swizzle the elements in inV + template + JPH_INLINE Vec3 Swizzle() const; + + /// Replicate the X component to all components + JPH_INLINE Vec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE Vec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE Vec4 SplatZ() const; + + /// Get index of component with lowest value + JPH_INLINE int GetLowestComponentIndex() const; + + /// Get index of component with highest value + JPH_INLINE int GetHighestComponentIndex() const; + + /// Return the absolute value of each of the components + JPH_INLINE Vec3 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE Vec3 Reciprocal() const; + + /// Cross product + JPH_INLINE Vec3 Cross(Vec3Arg inV2) const; + + /// Dot product, returns the dot product in X, Y and Z components + JPH_INLINE Vec3 DotV(Vec3Arg inV2) const; + + /// Dot product, returns the dot product in X, Y, Z and W components + JPH_INLINE Vec4 DotV4(Vec3Arg inV2) const; + + /// Dot product + JPH_INLINE float Dot(Vec3Arg inV2) const; + + /// Squared length of vector + JPH_INLINE float LengthSq() const; + + /// Length of vector + JPH_INLINE float Length() const; + + /// Normalize vector + JPH_INLINE Vec3 Normalized() const; + + /// Normalize vector or return inZeroValue if the length of the vector is zero + JPH_INLINE Vec3 NormalizedOr(Vec3Arg inZeroValue) const; + + /// Store 3 floats to memory + JPH_INLINE void StoreFloat3(Float3 *outV) const; + + /// Convert each component from a float to an int + JPH_INLINE UVec4 ToInt() const; + + /// Reinterpret Vec3 as a UVec4 (doesn't change the bits) + JPH_INLINE UVec4 ReinterpretAsInt() const; + + /// Get the minimum of X, Y and Z + JPH_INLINE float ReduceMin() const; + + /// Get the maximum of X, Y and Z + JPH_INLINE float ReduceMax() const; + + /// Component wise square root + JPH_INLINE Vec3 Sqrt() const; + + /// Get normalized vector that is perpendicular to this vector + JPH_INLINE Vec3 GetNormalizedPerpendicular() const; + + /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) + JPH_INLINE Vec3 GetSign() const; + + /// To String + friend ostream & operator << (ostream &inStream, Vec3Arg inV) + { + inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2]; + return inStream; + } + + /// Internal helper function that checks that W is equal to Z, so e.g. dividing by it should not generate div by 0 + JPH_INLINE void CheckW() const; + + /// Internal helper function that ensures that the Z component is replicated to the W component to prevent divisions by zero + static JPH_INLINE Type sFixW(Type inValue); + + union + { + Type mValue; + float mF32[4]; + }; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Vec3.inl" diff --git a/WickedEngine/Jolt/Math/Vec3.inl b/WickedEngine/Jolt/Math/Vec3.inl new file mode 100644 index 000000000..4a7953ccc --- /dev/null +++ b/WickedEngine/Jolt/Math/Vec3.inl @@ -0,0 +1,853 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +// Create a std::hash for Vec3 +JPH_MAKE_HASHABLE(JPH::Vec3, t.GetX(), t.GetY(), t.GetZ()) + +JPH_NAMESPACE_BEGIN + +void Vec3::CheckW() const +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + // Avoid asserts when both components are NaN + JPH_ASSERT(reinterpret_cast(mF32)[2] == reinterpret_cast(mF32)[3]); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +JPH_INLINE Vec3::Type Vec3::sFixW(Type inValue) +{ +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + #if defined(JPH_USE_SSE) + return _mm_shuffle_ps(inValue, inValue, _MM_SHUFFLE(2, 2, 1, 0)); + #elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(inValue, inValue, 0, 1, 2, 2); + #else + Type value; + value.mData[0] = inValue.mData[0]; + value.mData[1] = inValue.mData[1]; + value.mData[2] = inValue.mData[2]; + value.mData[3] = inValue.mData[2]; + return value; + #endif +#else + return inValue; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +} + +Vec3::Vec3(Vec4Arg inRHS) : + mValue(sFixW(inRHS.mValue)) +{ +} + +Vec3::Vec3(const Float3 &inV) +{ +#if defined(JPH_USE_SSE) + Type x = _mm_load_ss(&inV.x); + Type y = _mm_load_ss(&inV.y); + Type z = _mm_load_ss(&inV.z); + Type xy = _mm_unpacklo_ps(x, y); + mValue = _mm_shuffle_ps(xy, z, _MM_SHUFFLE(0, 0, 1, 0)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + float32x2_t xy = vld1_f32(&inV.x); + float32x2_t zz = vdup_n_f32(inV.z); // Assure Z and W are the same + mValue = vcombine_f32(xy, zz); +#else + mF32[0] = inV[0]; + mF32[1] = inV[1]; + mF32[2] = inV[2]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = inV[2]; + #endif +#endif +} + +Vec3::Vec3(float inX, float inY, float inZ) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_ps(inZ, inZ, inY, inX); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_f32(static_cast(*reinterpret_cast(&inX)) | (static_cast(*reinterpret_cast(&inY)) << 32)); + uint32x2_t zz = vcreate_f32(static_cast(*reinterpret_cast(&inZ)) | (static_cast(*reinterpret_cast(&inZ)) << 32)); + mValue = vcombine_f32(xy, zz); +#else + mF32[0] = inX; + mF32[1] = inY; + mF32[2] = inZ; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = inZ; + #endif +#endif +} + +template +Vec3 Vec3::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleZ, SwizzleZ, SwizzleY, SwizzleX)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleZ); +#else + return Vec3(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ]); +#endif +} + +Vec3 Vec3::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_ps(); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(0); +#else + return Vec3(0, 0, 0); +#endif +} + +Vec3 Vec3::sReplicate(float inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_ps(inV); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(inV); +#else + return Vec3(inV, inV, inV); +#endif +} + +Vec3 Vec3::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +Vec3 Vec3::sLoadFloat3Unsafe(const Float3 &inV) +{ +#if defined(JPH_USE_SSE) + Type v = _mm_loadu_ps(&inV.x); +#elif defined(JPH_USE_NEON) + Type v = vld1q_f32(&inV.x); +#else + Type v = { inV.x, inV.y, inV.z }; +#endif + return sFixW(v); +} + +Vec3 Vec3::sMin(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_min_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_f32(inV1.mValue, inV2.mValue); +#else + return Vec3(min(inV1.mF32[0], inV2.mF32[0]), + min(inV1.mF32[1], inV2.mF32[1]), + min(inV1.mF32[2], inV2.mF32[2])); +#endif +} + +Vec3 Vec3::sMax(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_max_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_f32(inV1.mValue, inV2.mValue); +#else + return Vec3(max(inV1.mF32[0], inV2.mF32[0]), + max(inV1.mF32[1], inV2.mF32[1]), + max(inV1.mF32[2], inV2.mF32[2])); +#endif +} + +Vec3 Vec3::sClamp(Vec3Arg inV, Vec3Arg inMin, Vec3Arg inMax) +{ + return sMax(sMin(inV, inMax), inMin); +} + +UVec4 Vec3::sEquals(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vceqq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sLess(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcltq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sLessOrEqual(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcleq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sGreater(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgtq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +UVec4 Vec3::sGreaterOrEqual(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgeq_f32(inV1.mValue, inV2.mValue); +#else + uint32 z = inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0; + return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0, + z, + z); +#endif +} + +Vec3 Vec3::sFusedMultiplyAdd(Vec3Arg inMul1, Vec3Arg inMul2, Vec3Arg inAdd) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_FMADD + return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue); +#else + return Vec3(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0], + inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1], + inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2]); +#endif +} + +Vec3 Vec3::sSelect(Vec3Arg inV1, Vec3Arg inV2, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) + Type v = _mm_blendv_ps(inV1.mValue, inV2.mValue, _mm_castsi128_ps(inControl.mValue)); + return sFixW(v); +#elif defined(JPH_USE_NEON) + Type v = vbslq_f32(vshrq_n_s32(inControl.mValue, 31), inV2.mValue, inV1.mValue); + return sFixW(v); +#else + Vec3 result; + for (int i = 0; i < 3; i++) + result.mF32[i] = inControl.mU32[i] ? inV2.mF32[i] : inV1.mF32[i]; +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + result.mF32[3] = result.mF32[2]; +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + return result; +#endif +} + +Vec3 Vec3::sOr(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vorrq_s32(inV1.mValue, inV2.mValue); +#else + return Vec3(UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sXor(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return veorq_s32(inV1.mValue, inV2.mValue); +#else + return Vec3(UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sAnd(Vec3Arg inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vandq_s32(inV1.mValue, inV2.mValue); +#else + return Vec3(UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat()); +#endif +} + +Vec3 Vec3::sUnitSpherical(float inTheta, float inPhi) +{ + Vec4 s, c; + Vec4(inTheta, inPhi, 0, 0).SinCos(s, c); + return Vec3(s.GetX() * c.GetY(), s.GetX() * s.GetY(), c.GetX()); +} + +template +Vec3 Vec3::sRandom(Random &inRandom) +{ + std::uniform_real_distribution zero_to_one(0.0f, 1.0f); + float theta = JPH_PI * zero_to_one(inRandom); + float phi = 2.0f * JPH_PI * zero_to_one(inRandom); + return sUnitSpherical(theta, phi); +} + +bool Vec3::operator == (Vec3Arg inV2) const +{ + return sEquals(*this, inV2).TestAllXYZTrue(); +} + +bool Vec3::IsClose(Vec3Arg inV2, float inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool Vec3::IsNearZero(float inMaxDistSq) const +{ + return LengthSq() <= inMaxDistSq; +} + +Vec3 Vec3::operator * (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] * inV2.mF32[0], mF32[1] * inV2.mF32[1], mF32[2] * inV2.mF32[2]); +#endif +} + +Vec3 Vec3::operator * (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(mValue, inV2); +#else + return Vec3(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2); +#endif +} + +Vec3 operator * (float inV1, Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(inV2.mValue, inV1); +#else + return Vec3(inV1 * inV2.mF32[0], inV1 * inV2.mF32[1], inV1 * inV2.mF32[2]); +#endif +} + +Vec3 Vec3::operator / (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + return Vec3(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2); +#endif +} + +Vec3 &Vec3::operator *= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vmulq_n_f32(mValue, inV2); +#else + for (int i = 0; i < 3; ++i) + mF32[i] *= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 &Vec3::operator *= (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vmulq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] *= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 &Vec3::operator /= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + for (int i = 0; i < 3; ++i) + mF32[i] /= inV2; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator + (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] + inV2.mF32[0], mF32[1] + inV2.mF32[1], mF32[2] + inV2.mF32[2]); +#endif +} + +Vec3 &Vec3::operator += (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] += inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator - () const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(_mm_setzero_ps(), mValue); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return vsubq_f32(vdupq_n_f32(0), mValue); + #else + return vnegq_f32(mValue); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return Vec3(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2]); + #else + return Vec3(-mF32[0], -mF32[1], -mF32[2]); + #endif +#endif +} + +Vec3 Vec3::operator - (Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vsubq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] - inV2.mF32[0], mF32[1] - inV2.mF32[1], mF32[2] - inV2.mF32[2]); +#endif +} + +Vec3 &Vec3::operator -= (Vec3Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vsubq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 3; ++i) + mF32[i] -= inV2.mF32[i]; + #ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + mF32[3] = mF32[2]; + #endif +#endif + return *this; +} + +Vec3 Vec3::operator / (Vec3Arg inV2) const +{ + inV2.CheckW(); // Check W equals Z to avoid div by zero +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, inV2.mValue); +#else + return Vec3(mF32[0] / inV2.mF32[0], mF32[1] / inV2.mF32[1], mF32[2] / inV2.mF32[2]); +#endif +} + +Vec4 Vec3::SplatX() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 0); +#else + return Vec4(mF32[0], mF32[0], mF32[0], mF32[0]); +#endif +} + +Vec4 Vec3::SplatY() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 1); +#else + return Vec4(mF32[1], mF32[1], mF32[1], mF32[1]); +#endif +} + +Vec4 Vec3::SplatZ() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 2); +#else + return Vec4(mF32[2], mF32[2], mF32[2], mF32[2]); +#endif +} + +int Vec3::GetLowestComponentIndex() const +{ + return GetX() < GetY() ? (GetZ() < GetX() ? 2 : 0) : (GetZ() < GetY() ? 2 : 1); +} + +int Vec3::GetHighestComponentIndex() const +{ + return GetX() > GetY() ? (GetZ() > GetX() ? 2 : 0) : (GetZ() > GetY() ? 2 : 1); +} + +Vec3 Vec3::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm_range_ps(mValue, mValue, 0b1000); +#elif defined(JPH_USE_SSE) + return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue); +#elif defined(JPH_USE_NEON) + return vabsq_f32(mValue); +#else + return Vec3(abs(mF32[0]), abs(mF32[1]), abs(mF32[2])); +#endif +} + +Vec3 Vec3::Reciprocal() const +{ + return sReplicate(1.0f) / mValue; +} + +Vec3 Vec3::Cross(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE) + Type t1 = _mm_shuffle_ps(inV2.mValue, inV2.mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t1 = _mm_mul_ps(t1, mValue); + Type t2 = _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same + t2 = _mm_mul_ps(t2, inV2.mValue); + Type t3 = _mm_sub_ps(t1, t2); + return _mm_shuffle_ps(t3, t3, _MM_SHUFFLE(0, 0, 2, 1)); // Assure Z and W are the same +#elif defined(JPH_USE_NEON) + Type t1 = JPH_NEON_SHUFFLE_F32x4(inV2.mValue, inV2.mValue, 1, 2, 0, 0); // Assure Z and W are the same + t1 = vmulq_f32(t1, mValue); + Type t2 = JPH_NEON_SHUFFLE_F32x4(mValue, mValue, 1, 2, 0, 0); // Assure Z and W are the same + t2 = vmulq_f32(t2, inV2.mValue); + Type t3 = vsubq_f32(t1, t2); + return JPH_NEON_SHUFFLE_F32x4(t3, t3, 1, 2, 0, 0); // Assure Z and W are the same +#else + return Vec3(mF32[1] * inV2.mF32[2] - mF32[2] * inV2.mF32[1], + mF32[2] * inV2.mF32[0] - mF32[0] * inV2.mF32[2], + mF32[0] * inV2.mF32[1] - mF32[1] * inV2.mF32[0]); +#endif +} + +Vec3 Vec3::DotV(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0x7f); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return Vec3::sReplicate(dot); +#endif +} + +Vec4 Vec3::DotV4(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0x7f); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return Vec4::sReplicate(dot); +#endif +} + +float Vec3::Dot(Vec3Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0x7f)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vaddvq_f32(mul); +#else + float dot = 0.0f; + for (int i = 0; i < 3; i++) + dot += mF32[i] * inV2.mF32[i]; + return dot; +#endif +} + +float Vec3::LengthSq() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0x7f)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + return vaddvq_f32(mul); +#else + float len_sq = 0.0f; + for (int i = 0; i < 3; i++) + len_sq += mF32[i] * mF32[i]; + return len_sq; +#endif +} + +float Vec3::Length() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0x7f))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x2_t sum = vdup_n_f32(vaddvq_f32(mul)); + return vget_lane_f32(vsqrt_f32(sum), 0); +#else + return sqrt(LengthSq()); +#endif +} + +Vec3 Vec3::Sqrt() const +{ +#if defined(JPH_USE_SSE) + return _mm_sqrt_ps(mValue); +#elif defined(JPH_USE_NEON) + return vsqrtq_f32(mValue); +#else + return Vec3(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2])); +#endif +} + +Vec3 Vec3::Normalized() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0x7f))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul)); + return vdivq_f32(mValue, vsqrtq_f32(sum)); +#else + return *this / Length(); +#endif +} + +Vec3 Vec3::NormalizedOr(Vec3Arg inZeroValue) const +{ +#if defined(JPH_USE_SSE4_1) + Type len_sq = _mm_dp_ps(mValue, mValue, 0x7f); + Type is_zero = _mm_cmpeq_ps(len_sq, _mm_setzero_ps()); +#ifdef JPH_FLOATING_POINT_EXCEPTIONS_ENABLED + if (_mm_movemask_ps(is_zero) == 0xf) + return inZeroValue; + else + return _mm_div_ps(mValue, _mm_sqrt_ps(len_sq)); +#else + return _mm_blendv_ps(_mm_div_ps(mValue, _mm_sqrt_ps(len_sq)), inZeroValue.mValue, is_zero); +#endif // JPH_FLOATING_POINT_EXCEPTIONS_ENABLED +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + mul = vsetq_lane_f32(0, mul, 3); + float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul)); + float32x4_t len = vsqrtq_f32(sum); + float32x4_t is_zero = vceqq_f32(len, vdupq_n_f32(0)); + return vbslq_f32(is_zero, inZeroValue.mValue, vdivq_f32(mValue, len)); +#else + float len_sq = LengthSq(); + if (len_sq == 0.0f) + return inZeroValue; + else + return *this / sqrt(len_sq); +#endif +} + +bool Vec3::IsNormalized(float inTolerance) const +{ + return abs(LengthSq() - 1.0f) <= inTolerance; +} + +bool Vec3::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return (_mm_fpclass_ps_mask(mValue, 0b10000001) & 0x7) != 0; +#elif defined(JPH_USE_SSE) + return (_mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) & 0x7) != 0; +#elif defined(JPH_USE_NEON) + uint32x4_t mask = JPH_NEON_UINT32x4(1, 1, 1, 0); + uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN + return vaddvq_u32(vandq_u32(is_equal, mask)) != 3; +#else + return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]); +#endif +} + +void Vec3::StoreFloat3(Float3 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_store_ss(&outV->x, mValue); + Vec3 t = Swizzle(); + _mm_store_ss(&outV->y, t.mValue); + t = t.Swizzle(); + _mm_store_ss(&outV->z, t.mValue); +#elif defined(JPH_USE_NEON) + float32x2_t xy = vget_low_f32(mValue); + vst1_f32(&outV->x, xy); + vst1q_lane_f32(&outV->z, mValue, 2); +#else + outV->x = mF32[0]; + outV->y = mF32[1]; + outV->z = mF32[2]; +#endif +} + +UVec4 Vec3::ToInt() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvttps_epi32(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_u32_f32(mValue); +#else + return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3])); +#endif +} + +UVec4 Vec3::ReinterpretAsInt() const +{ +#if defined(JPH_USE_SSE) + return UVec4(_mm_castps_si128(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_f32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +float Vec3::ReduceMin() const +{ + Vec3 v = sMin(mValue, Swizzle()); + v = sMin(v, v.Swizzle()); + return v.GetX(); +} + +float Vec3::ReduceMax() const +{ + Vec3 v = sMax(mValue, Swizzle()); + v = sMax(v, v.Swizzle()); + return v.GetX(); +} + +Vec3 Vec3::GetNormalizedPerpendicular() const +{ + if (abs(mF32[0]) > abs(mF32[1])) + { + float len = sqrt(mF32[0] * mF32[0] + mF32[2] * mF32[2]); + return Vec3(mF32[2], 0.0f, -mF32[0]) / len; + } + else + { + float len = sqrt(mF32[1] * mF32[1] + mF32[2] * mF32[2]); + return Vec3(0.0f, mF32[2], -mF32[1]) / len; + } +} + +Vec3 Vec3::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_SSE) + Type minus_one = _mm_set1_ps(-1.0f); + Type one = _mm_set1_ps(1.0f); + return _mm_or_ps(_mm_and_ps(mValue, minus_one), one); +#elif defined(JPH_USE_NEON) + Type minus_one = vdupq_n_f32(-1.0f); + Type one = vdupq_n_f32(1.0f); + return vorrq_s32(vandq_s32(mValue, minus_one), one); +#else + return Vec3(signbit(mF32[0])? -1.0f : 1.0f, + signbit(mF32[1])? -1.0f : 1.0f, + signbit(mF32[2])? -1.0f : 1.0f); +#endif +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Vec4.h b/WickedEngine/Jolt/Math/Vec4.h new file mode 100644 index 000000000..b369f8e3c --- /dev/null +++ b/WickedEngine/Jolt/Math/Vec4.h @@ -0,0 +1,283 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] alignas(JPH_VECTOR_ALIGNMENT) Vec4 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Underlying vector type +#if defined(JPH_USE_SSE) + using Type = __m128; +#elif defined(JPH_USE_NEON) + using Type = float32x4_t; +#else + using Type = struct { float mData[4]; }; +#endif + + /// Constructor + Vec4() = default; ///< Intentionally not initialized for performance reasons + Vec4(const Vec4 &inRHS) = default; + Vec4 & operator = (const Vec4 &inRHS) = default; + explicit JPH_INLINE Vec4(Vec3Arg inRHS); ///< WARNING: W component undefined! + JPH_INLINE Vec4(Vec3Arg inRHS, float inW); + JPH_INLINE Vec4(Type inRHS) : mValue(inRHS) { } + + /// Create a vector from 4 components + JPH_INLINE Vec4(float inX, float inY, float inZ, float inW); + + /// Vector with all zeros + static JPH_INLINE Vec4 sZero(); + + /// Vector with all NaN's + static JPH_INLINE Vec4 sNaN(); + + /// Replicate inV across all components + static JPH_INLINE Vec4 sReplicate(float inV); + + /// Load 4 floats from memory + static JPH_INLINE Vec4 sLoadFloat4(const Float4 *inV); + + /// Load 4 floats from memory, 16 bytes aligned + static JPH_INLINE Vec4 sLoadFloat4Aligned(const Float4 *inV); + + /// Gather 4 floats from memory at inBase + inOffsets[i] * Scale + template + static JPH_INLINE Vec4 sGatherFloat4(const float *inBase, UVec4Arg inOffsets); + + /// Return the minimum value of each of the components + static JPH_INLINE Vec4 sMin(Vec4Arg inV1, Vec4Arg inV2); + + /// Return the maximum of each of the components + static JPH_INLINE Vec4 sMax(Vec4Arg inV1, Vec4Arg inV2); + + /// Equals (component wise) + static JPH_INLINE UVec4 sEquals(Vec4Arg inV1, Vec4Arg inV2); + + /// Less than (component wise) + static JPH_INLINE UVec4 sLess(Vec4Arg inV1, Vec4Arg inV2); + + /// Less than or equal (component wise) + static JPH_INLINE UVec4 sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2); + + /// Greater than (component wise) + static JPH_INLINE UVec4 sGreater(Vec4Arg inV1, Vec4Arg inV2); + + /// Greater than or equal (component wise) + static JPH_INLINE UVec4 sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE Vec4 sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd); + + /// Component wise select, returns inV1 when highest bit of inControl = 0 and inV2 when highest bit of inControl = 1 + static JPH_INLINE Vec4 sSelect(Vec4Arg inV1, Vec4Arg inV2, UVec4Arg inControl); + + /// Logical or (component wise) + static JPH_INLINE Vec4 sOr(Vec4Arg inV1, Vec4Arg inV2); + + /// Logical xor (component wise) + static JPH_INLINE Vec4 sXor(Vec4Arg inV1, Vec4Arg inV2); + + /// Logical and (component wise) + static JPH_INLINE Vec4 sAnd(Vec4Arg inV1, Vec4Arg inV2); + + /// Sort the four elements of ioValue and sort ioIndex at the same time. + /// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network + static JPH_INLINE void sSort4(Vec4 &ioValue, UVec4 &ioIndex); + + /// Reverse sort the four elements of ioValue (highest first) and sort ioIndex at the same time. + /// Based on a sorting network: http://en.wikipedia.org/wiki/Sorting_network + static JPH_INLINE void sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex); + + /// Get individual components +#if defined(JPH_USE_SSE) + JPH_INLINE float GetX() const { return _mm_cvtss_f32(mValue); } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } + JPH_INLINE float GetW() const { return mF32[3]; } +#elif defined(JPH_USE_NEON) + JPH_INLINE float GetX() const { return vgetq_lane_f32(mValue, 0); } + JPH_INLINE float GetY() const { return vgetq_lane_f32(mValue, 1); } + JPH_INLINE float GetZ() const { return vgetq_lane_f32(mValue, 2); } + JPH_INLINE float GetW() const { return vgetq_lane_f32(mValue, 3); } +#else + JPH_INLINE float GetX() const { return mF32[0]; } + JPH_INLINE float GetY() const { return mF32[1]; } + JPH_INLINE float GetZ() const { return mF32[2]; } + JPH_INLINE float GetW() const { return mF32[3]; } +#endif + + /// Set individual components + JPH_INLINE void SetX(float inX) { mF32[0] = inX; } + JPH_INLINE void SetY(float inY) { mF32[1] = inY; } + JPH_INLINE void SetZ(float inZ) { mF32[2] = inZ; } + JPH_INLINE void SetW(float inW) { mF32[3] = inW; } + + /// Set all components + JPH_INLINE void Set(float inX, float inY, float inZ, float inW) { *this = Vec4(inX, inY, inZ, inW); } + + /// Get float component by index + JPH_INLINE float operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; } + JPH_INLINE float & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 4); return mF32[inCoordinate]; } + + /// Comparison + JPH_INLINE bool operator == (Vec4Arg inV2) const; + JPH_INLINE bool operator != (Vec4Arg inV2) const { return !(*this == inV2); } + + /// Test if two vectors are close + JPH_INLINE bool IsClose(Vec4Arg inV2, float inMaxDistSq = 1.0e-12f) const; + + /// Test if vector is normalized + JPH_INLINE bool IsNormalized(float inTolerance = 1.0e-6f) const; + + /// Test if vector contains NaN elements + JPH_INLINE bool IsNaN() const; + + /// Multiply two float vectors (component wise) + JPH_INLINE Vec4 operator * (Vec4Arg inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec4 operator * (float inV2) const; + + /// Multiply vector with float + friend JPH_INLINE Vec4 operator * (float inV1, Vec4Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec4 operator / (float inV2) const; + + /// Multiply vector with float + JPH_INLINE Vec4 & operator *= (float inV2); + + /// Multiply vector with vector + JPH_INLINE Vec4 & operator *= (Vec4Arg inV2); + + /// Divide vector by float + JPH_INLINE Vec4 & operator /= (float inV2); + + /// Add two float vectors (component wise) + JPH_INLINE Vec4 operator + (Vec4Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec4 & operator += (Vec4Arg inV2); + + /// Negate + JPH_INLINE Vec4 operator - () const; + + /// Subtract two float vectors (component wise) + JPH_INLINE Vec4 operator - (Vec4Arg inV2) const; + + /// Add two float vectors (component wise) + JPH_INLINE Vec4 & operator -= (Vec4Arg inV2); + + /// Divide (component wise) + JPH_INLINE Vec4 operator / (Vec4Arg inV2) const; + + /// Swizzle the elements in inV + template + JPH_INLINE Vec4 Swizzle() const; + + /// Replicate the X component to all components + JPH_INLINE Vec4 SplatX() const; + + /// Replicate the Y component to all components + JPH_INLINE Vec4 SplatY() const; + + /// Replicate the Z component to all components + JPH_INLINE Vec4 SplatZ() const; + + /// Replicate the W component to all components + JPH_INLINE Vec4 SplatW() const; + + /// Return the absolute value of each of the components + JPH_INLINE Vec4 Abs() const; + + /// Reciprocal vector (1 / value) for each of the components + JPH_INLINE Vec4 Reciprocal() const; + + /// Dot product, returns the dot product in X, Y and Z components + JPH_INLINE Vec4 DotV(Vec4Arg inV2) const; + + /// Dot product + JPH_INLINE float Dot(Vec4Arg inV2) const; + + /// Squared length of vector + JPH_INLINE float LengthSq() const; + + /// Length of vector + JPH_INLINE float Length() const; + + /// Normalize vector + JPH_INLINE Vec4 Normalized() const; + + /// Store 4 floats to memory + JPH_INLINE void StoreFloat4(Float4 *outV) const; + + /// Convert each component from a float to an int + JPH_INLINE UVec4 ToInt() const; + + /// Reinterpret Vec4 as a UVec4 (doesn't change the bits) + JPH_INLINE UVec4 ReinterpretAsInt() const; + + /// Store if X is negative in bit 0, Y in bit 1, Z in bit 2 and W in bit 3 + JPH_INLINE int GetSignBits() const; + + /// Get the minimum of X, Y, Z and W + JPH_INLINE float ReduceMin() const; + + /// Get the maximum of X, Y, Z and W + JPH_INLINE float ReduceMax() const; + + /// Component wise square root + JPH_INLINE Vec4 Sqrt() const; + + /// Get vector that contains the sign of each element (returns 1.0f if positive, -1.0f if negative) + JPH_INLINE Vec4 GetSign() const; + + /// Calculate the sine and cosine for each element of this vector (input in radians) + inline void SinCos(Vec4 &outSin, Vec4 &outCos) const; + + /// Calculate the tangent for each element of this vector (input in radians) + inline Vec4 Tan() const; + + /// Calculate the arc sine for each element of this vector (returns value in the range [-PI / 2, PI / 2]) + /// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::asin + inline Vec4 ASin() const; + + /// Calculate the arc cosine for each element of this vector (returns value in the range [0, PI]) + /// Note that all input values will be clamped to the range [-1, 1] and this function will not return NaNs like std::acos + inline Vec4 ACos() const; + + /// Calculate the arc tangent for each element of this vector (returns value in the range [-PI / 2, PI / 2]) + inline Vec4 ATan() const; + + /// Calculate the arc tangent of y / x using the signs of the arguments to determine the correct quadrant (returns value in the range [-PI, PI]) + inline static Vec4 sATan2(Vec4Arg inY, Vec4Arg inX); + + /// To String + friend ostream & operator << (ostream &inStream, Vec4Arg inV) + { + inStream << inV.mF32[0] << ", " << inV.mF32[1] << ", " << inV.mF32[2] << ", " << inV.mF32[3]; + return inStream; + } + + union + { + Type mValue; + float mF32[4]; + }; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Vec4.inl" diff --git a/WickedEngine/Jolt/Math/Vec4.inl b/WickedEngine/Jolt/Math/Vec4.inl new file mode 100644 index 000000000..1090ca822 --- /dev/null +++ b/WickedEngine/Jolt/Math/Vec4.inl @@ -0,0 +1,978 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +// Constructor +Vec4::Vec4(Vec3Arg inRHS) : + mValue(inRHS.mValue) +{ +} + +Vec4::Vec4(Vec3Arg inRHS, float inW) +{ +#if defined(JPH_USE_SSE4_1) + mValue = _mm_blend_ps(inRHS.mValue, _mm_set1_ps(inW), 8); +#elif defined(JPH_USE_NEON) + mValue = vsetq_lane_f32(inW, inRHS.mValue, 3); +#else + for (int i = 0; i < 3; i++) + mF32[i] = inRHS.mF32[i]; + mF32[3] = inW; +#endif +} + +Vec4::Vec4(float inX, float inY, float inZ, float inW) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_set_ps(inW, inZ, inY, inX); +#elif defined(JPH_USE_NEON) + uint32x2_t xy = vcreate_f32(static_cast(*reinterpret_cast(&inX)) | (static_cast(*reinterpret_cast(&inY)) << 32)); + uint32x2_t zw = vcreate_f32(static_cast(*reinterpret_cast(&inZ)) | (static_cast(*reinterpret_cast(&inW)) << 32)); + mValue = vcombine_f32(xy, zw); +#else + mF32[0] = inX; + mF32[1] = inY; + mF32[2] = inZ; + mF32[3] = inW; +#endif +} + +template +Vec4 Vec4::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX)); +#elif defined(JPH_USE_NEON) + return JPH_NEON_SHUFFLE_F32x4(mValue, mValue, SwizzleX, SwizzleY, SwizzleZ, SwizzleW); +#else + return Vec4(mF32[SwizzleX], mF32[SwizzleY], mF32[SwizzleZ], mF32[SwizzleW]); +#endif +} + +Vec4 Vec4::sZero() +{ +#if defined(JPH_USE_SSE) + return _mm_setzero_ps(); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(0); +#else + return Vec4(0, 0, 0, 0); +#endif +} + +Vec4 Vec4::sReplicate(float inV) +{ +#if defined(JPH_USE_SSE) + return _mm_set1_ps(inV); +#elif defined(JPH_USE_NEON) + return vdupq_n_f32(inV); +#else + return Vec4(inV, inV, inV, inV); +#endif +} + +Vec4 Vec4::sNaN() +{ + return sReplicate(numeric_limits::quiet_NaN()); +} + +Vec4 Vec4::sLoadFloat4(const Float4 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_loadu_ps(&inV->x); +#elif defined(JPH_USE_NEON) + return vld1q_f32(&inV->x); +#else + return Vec4(inV->x, inV->y, inV->z, inV->w); +#endif +} + +Vec4 Vec4::sLoadFloat4Aligned(const Float4 *inV) +{ +#if defined(JPH_USE_SSE) + return _mm_load_ps(&inV->x); +#elif defined(JPH_USE_NEON) + return vld1q_f32(&inV->x); +#else + return Vec4(inV->x, inV->y, inV->z, inV->w); +#endif +} + +template +Vec4 Vec4::sGatherFloat4(const float *inBase, UVec4Arg inOffsets) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_AVX2 + return _mm_i32gather_ps(inBase, inOffsets.mValue, Scale); + #else + const uint8 *base = reinterpret_cast(inBase); + Type x = _mm_load_ss(reinterpret_cast(base + inOffsets.GetX() * Scale)); + Type y = _mm_load_ss(reinterpret_cast(base + inOffsets.GetY() * Scale)); + Type xy = _mm_unpacklo_ps(x, y); + Type z = _mm_load_ss(reinterpret_cast(base + inOffsets.GetZ() * Scale)); + Type w = _mm_load_ss(reinterpret_cast(base + inOffsets.GetW() * Scale)); + Type zw = _mm_unpacklo_ps(z, w); + return _mm_movelh_ps(xy, zw); + #endif +#else + const uint8 *base = reinterpret_cast(inBase); + float x = *reinterpret_cast(base + inOffsets.GetX() * Scale); + float y = *reinterpret_cast(base + inOffsets.GetY() * Scale); + float z = *reinterpret_cast(base + inOffsets.GetZ() * Scale); + float w = *reinterpret_cast(base + inOffsets.GetW() * Scale); + return Vec4(x, y, z, w); +#endif +} + +Vec4 Vec4::sMin(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_min_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vminq_f32(inV1.mValue, inV2.mValue); +#else + return Vec4(min(inV1.mF32[0], inV2.mF32[0]), + min(inV1.mF32[1], inV2.mF32[1]), + min(inV1.mF32[2], inV2.mF32[2]), + min(inV1.mF32[3], inV2.mF32[3])); +#endif +} + +Vec4 Vec4::sMax(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_max_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmaxq_f32(inV1.mValue, inV2.mValue); +#else + return Vec4(max(inV1.mF32[0], inV2.mF32[0]), + max(inV1.mF32[1], inV2.mF32[1]), + max(inV1.mF32[2], inV2.mF32[2]), + max(inV1.mF32[3], inV2.mF32[3])); +#endif +} + +UVec4 Vec4::sEquals(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpeq_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vceqq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] == inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] == inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] == inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] == inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sLess(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmplt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcltq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] < inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] < inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] < inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] < inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sLessOrEqual(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmple_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcleq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] <= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] <= inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] <= inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] <= inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sGreater(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpgt_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgtq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] > inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] > inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] > inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] > inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +UVec4 Vec4::sGreaterOrEqual(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_castps_si128(_mm_cmpge_ps(inV1.mValue, inV2.mValue)); +#elif defined(JPH_USE_NEON) + return vcgeq_f32(inV1.mValue, inV2.mValue); +#else + return UVec4(inV1.mF32[0] >= inV2.mF32[0]? 0xffffffffu : 0, + inV1.mF32[1] >= inV2.mF32[1]? 0xffffffffu : 0, + inV1.mF32[2] >= inV2.mF32[2]? 0xffffffffu : 0, + inV1.mF32[3] >= inV2.mF32[3]? 0xffffffffu : 0); +#endif +} + +Vec4 Vec4::sFusedMultiplyAdd(Vec4Arg inMul1, Vec4Arg inMul2, Vec4Arg inAdd) +{ +#if defined(JPH_USE_SSE) + #ifdef JPH_USE_FMADD + return _mm_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue); + #else + return _mm_add_ps(_mm_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue); + #endif +#elif defined(JPH_USE_NEON) + return vmlaq_f32(inAdd.mValue, inMul1.mValue, inMul2.mValue); +#else + return Vec4(inMul1.mF32[0] * inMul2.mF32[0] + inAdd.mF32[0], + inMul1.mF32[1] * inMul2.mF32[1] + inAdd.mF32[1], + inMul1.mF32[2] * inMul2.mF32[2] + inAdd.mF32[2], + inMul1.mF32[3] * inMul2.mF32[3] + inAdd.mF32[3]); +#endif +} + +Vec4 Vec4::sSelect(Vec4Arg inV1, Vec4Arg inV2, UVec4Arg inControl) +{ +#if defined(JPH_USE_SSE4_1) + return _mm_blendv_ps(inV1.mValue, inV2.mValue, _mm_castsi128_ps(inControl.mValue)); +#elif defined(JPH_USE_NEON) + return vbslq_f32(vshrq_n_s32(inControl.mValue, 31), inV2.mValue, inV1.mValue); +#else + Vec4 result; + for (int i = 0; i < 4; i++) + result.mF32[i] = inControl.mU32[i] ? inV2.mF32[i] : inV1.mF32[i]; + return result; +#endif +} + +Vec4 Vec4::sOr(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_or_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vorrq_s32(inV1.mValue, inV2.mValue); +#else + return UVec4::sOr(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +Vec4 Vec4::sXor(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_xor_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return veorq_s32(inV1.mValue, inV2.mValue); +#else + return UVec4::sXor(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +Vec4 Vec4::sAnd(Vec4Arg inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_and_ps(inV1.mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vandq_s32(inV1.mValue, inV2.mValue); +#else + return UVec4::sAnd(inV1.ReinterpretAsInt(), inV2.ReinterpretAsInt()).ReinterpretAsFloat(); +#endif +} + +void Vec4::sSort4(Vec4 &ioValue, UVec4 &ioIndex) +{ + // Pass 1, test 1st vs 3rd, 2nd vs 4th + Vec4 v1 = ioValue.Swizzle(); + UVec4 i1 = ioIndex.Swizzle(); + UVec4 c1 = sLess(ioValue, v1).Swizzle(); + ioValue = sSelect(ioValue, v1, c1); + ioIndex = UVec4::sSelect(ioIndex, i1, c1); + + // Pass 2, test 1st vs 2nd, 3rd vs 4th + Vec4 v2 = ioValue.Swizzle(); + UVec4 i2 = ioIndex.Swizzle(); + UVec4 c2 = sLess(ioValue, v2).Swizzle(); + ioValue = sSelect(ioValue, v2, c2); + ioIndex = UVec4::sSelect(ioIndex, i2, c2); + + // Pass 3, test 2nd vs 3rd component + Vec4 v3 = ioValue.Swizzle(); + UVec4 i3 = ioIndex.Swizzle(); + UVec4 c3 = sLess(ioValue, v3).Swizzle(); + ioValue = sSelect(ioValue, v3, c3); + ioIndex = UVec4::sSelect(ioIndex, i3, c3); +} + +void Vec4::sSort4Reverse(Vec4 &ioValue, UVec4 &ioIndex) +{ + // Pass 1, test 1st vs 3rd, 2nd vs 4th + Vec4 v1 = ioValue.Swizzle(); + UVec4 i1 = ioIndex.Swizzle(); + UVec4 c1 = sGreater(ioValue, v1).Swizzle(); + ioValue = sSelect(ioValue, v1, c1); + ioIndex = UVec4::sSelect(ioIndex, i1, c1); + + // Pass 2, test 1st vs 2nd, 3rd vs 4th + Vec4 v2 = ioValue.Swizzle(); + UVec4 i2 = ioIndex.Swizzle(); + UVec4 c2 = sGreater(ioValue, v2).Swizzle(); + ioValue = sSelect(ioValue, v2, c2); + ioIndex = UVec4::sSelect(ioIndex, i2, c2); + + // Pass 3, test 2nd vs 3rd component + Vec4 v3 = ioValue.Swizzle(); + UVec4 i3 = ioIndex.Swizzle(); + UVec4 c3 = sGreater(ioValue, v3).Swizzle(); + ioValue = sSelect(ioValue, v3, c3); + ioIndex = UVec4::sSelect(ioIndex, i3, c3); +} + +bool Vec4::operator == (Vec4Arg inV2) const +{ + return sEquals(*this, inV2).TestAllTrue(); +} + +bool Vec4::IsClose(Vec4Arg inV2, float inMaxDistSq) const +{ + return (inV2 - *this).LengthSq() <= inMaxDistSq; +} + +bool Vec4::IsNormalized(float inTolerance) const +{ + return abs(LengthSq() - 1.0f) <= inTolerance; +} + +bool Vec4::IsNaN() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fpclass_ps_mask(mValue, 0b10000001) != 0; +#elif defined(JPH_USE_SSE) + return _mm_movemask_ps(_mm_cmpunord_ps(mValue, mValue)) != 0; +#elif defined(JPH_USE_NEON) + uint32x4_t is_equal = vceqq_f32(mValue, mValue); // If a number is not equal to itself it's a NaN + return vaddvq_u32(vshrq_n_u32(is_equal, 31)) != 4; +#else + return isnan(mF32[0]) || isnan(mF32[1]) || isnan(mF32[2]) || isnan(mF32[3]); +#endif +} + +Vec4 Vec4::operator * (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] * inV2.mF32[0], + mF32[1] * inV2.mF32[1], + mF32[2] * inV2.mF32[2], + mF32[3] * inV2.mF32[3]); +#endif +} + +Vec4 Vec4::operator * (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(mValue, inV2); +#else + return Vec4(mF32[0] * inV2, mF32[1] * inV2, mF32[2] * inV2, mF32[3] * inV2); +#endif +} + +/// Multiply vector with float +Vec4 operator * (float inV1, Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + return _mm_mul_ps(_mm_set1_ps(inV1), inV2.mValue); +#elif defined(JPH_USE_NEON) + return vmulq_n_f32(inV2.mValue, inV1); +#else + return Vec4(inV1 * inV2.mF32[0], + inV1 * inV2.mF32[1], + inV1 * inV2.mF32[2], + inV1 * inV2.mF32[3]); +#endif +} + +Vec4 Vec4::operator / (float inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + return Vec4(mF32[0] / inV2, mF32[1] / inV2, mF32[2] / inV2, mF32[3] / inV2); +#endif +} + +Vec4 &Vec4::operator *= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vmulq_n_f32(mValue, inV2); +#else + for (int i = 0; i < 4; ++i) + mF32[i] *= inV2; +#endif + return *this; +} + +Vec4 &Vec4::operator *= (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_mul_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vmulq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] *= inV2.mF32[i]; +#endif + return *this; +} + +Vec4 &Vec4::operator /= (float inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_div_ps(mValue, _mm_set1_ps(inV2)); +#elif defined(JPH_USE_NEON) + mValue = vdivq_f32(mValue, vdupq_n_f32(inV2)); +#else + for (int i = 0; i < 4; ++i) + mF32[i] /= inV2; +#endif + return *this; +} + +Vec4 Vec4::operator + (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vaddq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] + inV2.mF32[0], + mF32[1] + inV2.mF32[1], + mF32[2] + inV2.mF32[2], + mF32[3] + inV2.mF32[3]); +#endif +} + +Vec4 &Vec4::operator += (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_add_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vaddq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] += inV2.mF32[i]; +#endif + return *this; +} + +Vec4 Vec4::operator - () const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(_mm_setzero_ps(), mValue); +#elif defined(JPH_USE_NEON) + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return vsubq_f32(vdupq_n_f32(0), mValue); + #else + return vnegq_f32(mValue); + #endif +#else + #ifdef JPH_CROSS_PLATFORM_DETERMINISTIC + return Vec4(0.0f - mF32[0], 0.0f - mF32[1], 0.0f - mF32[2], 0.0f - mF32[3]); + #else + return Vec4(-mF32[0], -mF32[1], -mF32[2], -mF32[3]); + #endif +#endif +} + +Vec4 Vec4::operator - (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vsubq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] - inV2.mF32[0], + mF32[1] - inV2.mF32[1], + mF32[2] - inV2.mF32[2], + mF32[3] - inV2.mF32[3]); +#endif +} + +Vec4 &Vec4::operator -= (Vec4Arg inV2) +{ +#if defined(JPH_USE_SSE) + mValue = _mm_sub_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + mValue = vsubq_f32(mValue, inV2.mValue); +#else + for (int i = 0; i < 4; ++i) + mF32[i] -= inV2.mF32[i]; +#endif + return *this; +} + +Vec4 Vec4::operator / (Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE) + return _mm_div_ps(mValue, inV2.mValue); +#elif defined(JPH_USE_NEON) + return vdivq_f32(mValue, inV2.mValue); +#else + return Vec4(mF32[0] / inV2.mF32[0], + mF32[1] / inV2.mF32[1], + mF32[2] / inV2.mF32[2], + mF32[3] / inV2.mF32[3]); +#endif +} + +Vec4 Vec4::SplatX() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(0, 0, 0, 0)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 0); +#else + return Vec4(mF32[0], mF32[0], mF32[0], mF32[0]); +#endif +} + +Vec4 Vec4::SplatY() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(1, 1, 1, 1)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 1); +#else + return Vec4(mF32[1], mF32[1], mF32[1], mF32[1]); +#endif +} + +Vec4 Vec4::SplatZ() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(2, 2, 2, 2)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 2); +#else + return Vec4(mF32[2], mF32[2], mF32[2], mF32[2]); +#endif +} + +Vec4 Vec4::SplatW() const +{ +#if defined(JPH_USE_SSE) + return _mm_shuffle_ps(mValue, mValue, _MM_SHUFFLE(3, 3, 3, 3)); +#elif defined(JPH_USE_NEON) + return vdupq_laneq_f32(mValue, 3); +#else + return Vec4(mF32[3], mF32[3], mF32[3], mF32[3]); +#endif +} + +Vec4 Vec4::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm_range_ps(mValue, mValue, 0b1000); +#elif defined(JPH_USE_SSE) + return _mm_max_ps(_mm_sub_ps(_mm_setzero_ps(), mValue), mValue); +#elif defined(JPH_USE_NEON) + return vabsq_f32(mValue); +#else + return Vec4(abs(mF32[0]), abs(mF32[1]), abs(mF32[2]), abs(mF32[3])); +#endif +} + +Vec4 Vec4::Reciprocal() const +{ + return sReplicate(1.0f) / mValue; +} + +Vec4 Vec4::DotV(Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_dp_ps(mValue, inV2.mValue, 0xff); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + return vdupq_n_f32(vaddvq_f32(mul)); +#else + // Brackets placed so that the order is consistent with the vectorized version + return Vec4::sReplicate((mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3])); +#endif +} + +float Vec4::Dot(Vec4Arg inV2) const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, inV2.mValue, 0xff)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, inV2.mValue); + return vaddvq_f32(mul); +#else + // Brackets placed so that the order is consistent with the vectorized version + return (mF32[0] * inV2.mF32[0] + mF32[1] * inV2.mF32[1]) + (mF32[2] * inV2.mF32[2] + mF32[3] * inV2.mF32[3]); +#endif +} + +float Vec4::LengthSq() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_dp_ps(mValue, mValue, 0xff)); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + return vaddvq_f32(mul); +#else + // Brackets placed so that the order is consistent with the vectorized version + return (mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3]); +#endif +} + +float Vec4::Length() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_cvtss_f32(_mm_sqrt_ss(_mm_dp_ps(mValue, mValue, 0xff))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + float32x2_t sum = vdup_n_f32(vaddvq_f32(mul)); + return vget_lane_f32(vsqrt_f32(sum), 0); +#else + // Brackets placed so that the order is consistent with the vectorized version + return sqrt((mF32[0] * mF32[0] + mF32[1] * mF32[1]) + (mF32[2] * mF32[2] + mF32[3] * mF32[3])); +#endif +} + +Vec4 Vec4::Sqrt() const +{ +#if defined(JPH_USE_SSE) + return _mm_sqrt_ps(mValue); +#elif defined(JPH_USE_NEON) + return vsqrtq_f32(mValue); +#else + return Vec4(sqrt(mF32[0]), sqrt(mF32[1]), sqrt(mF32[2]), sqrt(mF32[3])); +#endif +} + + +Vec4 Vec4::GetSign() const +{ +#if defined(JPH_USE_AVX512) + return _mm_fixupimm_ps(mValue, mValue, _mm_set1_epi32(0xA9A90A00), 0); +#elif defined(JPH_USE_SSE) + Type minus_one = _mm_set1_ps(-1.0f); + Type one = _mm_set1_ps(1.0f); + return _mm_or_ps(_mm_and_ps(mValue, minus_one), one); +#elif defined(JPH_USE_NEON) + Type minus_one = vdupq_n_f32(-1.0f); + Type one = vdupq_n_f32(1.0f); + return vorrq_s32(vandq_s32(mValue, minus_one), one); +#else + return Vec4(signbit(mF32[0])? -1.0f : 1.0f, + signbit(mF32[1])? -1.0f : 1.0f, + signbit(mF32[2])? -1.0f : 1.0f, + signbit(mF32[3])? -1.0f : 1.0f); +#endif +} + +Vec4 Vec4::Normalized() const +{ +#if defined(JPH_USE_SSE4_1) + return _mm_div_ps(mValue, _mm_sqrt_ps(_mm_dp_ps(mValue, mValue, 0xff))); +#elif defined(JPH_USE_NEON) + float32x4_t mul = vmulq_f32(mValue, mValue); + float32x4_t sum = vdupq_n_f32(vaddvq_f32(mul)); + return vdivq_f32(mValue, vsqrtq_f32(sum)); +#else + return *this / Length(); +#endif +} + +void Vec4::StoreFloat4(Float4 *outV) const +{ +#if defined(JPH_USE_SSE) + _mm_storeu_ps(&outV->x, mValue); +#elif defined(JPH_USE_NEON) + vst1q_f32(&outV->x, mValue); +#else + for (int i = 0; i < 4; ++i) + (&outV->x)[i] = mF32[i]; +#endif +} + +UVec4 Vec4::ToInt() const +{ +#if defined(JPH_USE_SSE) + return _mm_cvttps_epi32(mValue); +#elif defined(JPH_USE_NEON) + return vcvtq_u32_f32(mValue); +#else + return UVec4(uint32(mF32[0]), uint32(mF32[1]), uint32(mF32[2]), uint32(mF32[3])); +#endif +} + +UVec4 Vec4::ReinterpretAsInt() const +{ +#if defined(JPH_USE_SSE) + return UVec4(_mm_castps_si128(mValue)); +#elif defined(JPH_USE_NEON) + return vreinterpretq_u32_f32(mValue); +#else + return *reinterpret_cast(this); +#endif +} + +int Vec4::GetSignBits() const +{ +#if defined(JPH_USE_SSE) + return _mm_movemask_ps(mValue); +#elif defined(JPH_USE_NEON) + int32x4_t shift = JPH_NEON_INT32x4(0, 1, 2, 3); + return vaddvq_u32(vshlq_u32(vshrq_n_u32(vreinterpretq_u32_f32(mValue), 31), shift)); +#else + return (signbit(mF32[0])? 1 : 0) | (signbit(mF32[1])? 2 : 0) | (signbit(mF32[2])? 4 : 0) | (signbit(mF32[3])? 8 : 0); +#endif +} + +float Vec4::ReduceMin() const +{ + Vec4 v = sMin(mValue, Swizzle()); + v = sMin(v, v.Swizzle()); + return v.GetX(); +} + +float Vec4::ReduceMax() const +{ + Vec4 v = sMax(mValue, Swizzle()); + v = sMax(v, v.Swizzle()); + return v.GetX(); +} + +void Vec4::SinCos(Vec4 &outSin, Vec4 &outCos) const +{ + // Implementation based on sinf.c from the cephes library, combines sinf and cosf in a single function, changes octants to quadrants and vectorizes it + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive and remember sign for sin only since cos is symmetric around x (highest bit of a float is the sign bit) + UVec4 sin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, sin_sign.ReinterpretAsFloat()); + + // x / (PI / 2) rounded to nearest int gives us the quadrant closest to x + UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt(); + + // Make x relative to the closest quadrant. + // This does x = x - quadrant * PI / 2 using a two step Cody-Waite argument reduction. + // This improves the accuracy of the result by avoiding loss of significant bits in the subtraction. + // We start with x = x - quadrant * PI / 2, PI / 2 in hexadecimal notation is 0x3fc90fdb, we remove the lowest 16 bits to + // get 0x3fc90000 (= 1.5703125) this means we can now multiply with a number of up to 2^16 without losing any bits. + // This leaves us with: x = (x - quadrant * 1.5703125) - quadrant * (PI / 2 - 1.5703125). + // PI / 2 - 1.5703125 in hexadecimal is 0x39fdaa22, stripping the lowest 12 bits we get 0x39fda000 (= 0.0004837512969970703125) + // This leaves uw with: x = ((x - quadrant * 1.5703125) - quadrant * 0.0004837512969970703125) - quadrant * (PI / 2 - 1.5703125 - 0.0004837512969970703125) + // See: https://stackoverflow.com/questions/42455143/sine-cosine-modular-extended-precision-arithmetic + // After this we have x in the range [-PI / 4, PI / 4]. + Vec4 float_quadrant = quadrant.ToFloat(); + x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f; + + // Calculate x2 = x^2 + Vec4 x2 = x * x; + + // Taylor expansion: + // Cos(x) = 1 - x^2/2! + x^4/4! - x^6/6! + x^8/8! + ... = (((x2/8!- 1/6!) * x2 + 1/4!) * x2 - 1/2!) * x2 + 1 + Vec4 taylor_cos = ((2.443315711809948e-5f * x2 - Vec4::sReplicate(1.388731625493765e-3f)) * x2 + Vec4::sReplicate(4.166664568298827e-2f)) * x2 * x2 - 0.5f * x2 + Vec4::sReplicate(1.0f); + // Sin(x) = x - x^3/3! + x^5/5! - x^7/7! + ... = ((-x2/7! + 1/5!) * x2 - 1/3!) * x2 * x + x + Vec4 taylor_sin = ((-1.9515295891e-4f * x2 + Vec4::sReplicate(8.3321608736e-3f)) * x2 - Vec4::sReplicate(1.6666654611e-1f)) * x2 * x + x; + + // The lowest 2 bits of quadrant indicate the quadrant that we are in. + // Let x be the original input value and x' our value that has been mapped to the range [-PI / 4, PI / 4]. + // since cos(x) = sin(x - PI / 2) and since we want to use the Taylor expansion as close as possible to 0, + // we can alternate between using the Taylor expansion for sin and cos according to the following table: + // + // quadrant sin(x) cos(x) + // XXX00b sin(x') cos(x') + // XXX01b cos(x') -sin(x') + // XXX10b -sin(x') -cos(x') + // XXX11b -cos(x') sin(x') + // + // So: sin_sign = bit2, cos_sign = bit1 ^ bit2, bit1 determines if we use sin or cos Taylor expansion + UVec4 bit1 = quadrant.LogicalShiftLeft<31>(); + UVec4 bit2 = UVec4::sAnd(quadrant.LogicalShiftLeft<30>(), UVec4::sReplicate(0x80000000U)); + + // Select which one of the results is sin and which one is cos + Vec4 s = Vec4::sSelect(taylor_sin, taylor_cos, bit1); + Vec4 c = Vec4::sSelect(taylor_cos, taylor_sin, bit1); + + // Update the signs + sin_sign = UVec4::sXor(sin_sign, bit2); + UVec4 cos_sign = UVec4::sXor(bit1, bit2); + + // Correct the signs + outSin = Vec4::sXor(s, sin_sign.ReinterpretAsFloat()); + outCos = Vec4::sXor(c, cos_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::Tan() const +{ + // Implementation based on tanf.c from the cephes library, see Vec4::SinCos for further details + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 tan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, tan_sign.ReinterpretAsFloat()); + + // x / (PI / 2) rounded to nearest int gives us the quadrant closest to x + UVec4 quadrant = (0.6366197723675814f * x + Vec4::sReplicate(0.5f)).ToInt(); + + // Remap x to range [-PI / 4, PI / 4], see Vec4::SinCos + Vec4 float_quadrant = quadrant.ToFloat(); + x = ((x - float_quadrant * 1.5703125f) - float_quadrant * 0.0004837512969970703125f) - float_quadrant * 7.549789948768648e-8f; + + // Calculate x2 = x^2 + Vec4 x2 = x * x; + + // Roughly equivalent to the Taylor expansion: + // Tan(x) = x + x^3/3 + 2*x^5/15 + 17*x^7/315 + 62*x^9/2835 + ... + Vec4 tan = + (((((9.38540185543e-3f * x2 + Vec4::sReplicate(3.11992232697e-3f)) * x2 + Vec4::sReplicate(2.44301354525e-2f)) * x2 + + Vec4::sReplicate(5.34112807005e-2f)) * x2 + Vec4::sReplicate(1.33387994085e-1f)) * x2 + Vec4::sReplicate(3.33331568548e-1f)) * x2 * x + x; + + // For the 2nd and 4th quadrant we need to invert the value + UVec4 bit1 = quadrant.LogicalShiftLeft<31>(); + tan = Vec4::sSelect(tan, Vec4::sReplicate(-1.0f) / (tan JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))), bit1); // Add small epsilon to prevent div by zero, works because tan is always positive + + // Put the sign back + return Vec4::sXor(tan, tan_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::ASin() const +{ + // Implementation based on asinf.c from the cephes library + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 asin_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 a = Vec4::sXor(*this, asin_sign.ReinterpretAsFloat()); + + // ASin is not defined outside the range [-1, 1] but it often happens that a value is slightly above 1 so we just clamp here + a = Vec4::sMin(a, Vec4::sReplicate(1.0f)); + + // When |x| <= 0.5 we use the asin approximation as is + Vec4 z1 = a * a; + Vec4 x1 = a; + + // When |x| > 0.5 we use the identity asin(x) = PI / 2 - 2 * asin(sqrt((1 - x) / 2)) + Vec4 z2 = 0.5f * (Vec4::sReplicate(1.0f) - a); + Vec4 x2 = z2.Sqrt(); + + // Select which of the two situations we have + UVec4 greater = Vec4::sGreater(a, Vec4::sReplicate(0.5f)); + Vec4 z = Vec4::sSelect(z1, z2, greater); + Vec4 x = Vec4::sSelect(x1, x2, greater); + + // Polynomial approximation of asin + z = ((((4.2163199048e-2f * z + Vec4::sReplicate(2.4181311049e-2f)) * z + Vec4::sReplicate(4.5470025998e-2f)) * z + Vec4::sReplicate(7.4953002686e-2f)) * z + Vec4::sReplicate(1.6666752422e-1f)) * z * x + x; + + // If |x| > 0.5 we need to apply the remainder of the identity above + z = Vec4::sSelect(z, Vec4::sReplicate(0.5f * JPH_PI) - (z + z), greater); + + // Put the sign back + return Vec4::sXor(z, asin_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::ACos() const +{ + // Not the most accurate, but simple + return Vec4::sReplicate(0.5f * JPH_PI) - ASin(); +} + +Vec4 Vec4::ATan() const +{ + // Implementation based on atanf.c from the cephes library + // Original implementation by Stephen L. Moshier (See: http://www.moshier.net/) + + // Make argument positive + UVec4 atan_sign = UVec4::sAnd(ReinterpretAsInt(), UVec4::sReplicate(0x80000000U)); + Vec4 x = Vec4::sXor(*this, atan_sign.ReinterpretAsFloat()); + Vec4 y = Vec4::sZero(); + + // If x > Tan(PI / 8) + UVec4 greater1 = Vec4::sGreater(x, Vec4::sReplicate(0.4142135623730950f)); + Vec4 x1 = (x - Vec4::sReplicate(1.0f)) / (x + Vec4::sReplicate(1.0f)); + + // If x > Tan(3 * PI / 8) + UVec4 greater2 = Vec4::sGreater(x, Vec4::sReplicate(2.414213562373095f)); + Vec4 x2 = Vec4::sReplicate(-1.0f) / (x JPH_IF_FLOATING_POINT_EXCEPTIONS_ENABLED(+ Vec4::sReplicate(FLT_MIN))); // Add small epsilon to prevent div by zero, works because x is always positive + + // Apply first if + x = Vec4::sSelect(x, x1, greater1); + y = Vec4::sSelect(y, Vec4::sReplicate(0.25f * JPH_PI), greater1); + + // Apply second if + x = Vec4::sSelect(x, x2, greater2); + y = Vec4::sSelect(y, Vec4::sReplicate(0.5f * JPH_PI), greater2); + + // Polynomial approximation + Vec4 z = x * x; + y += (((8.05374449538e-2f * z - Vec4::sReplicate(1.38776856032e-1f)) * z + Vec4::sReplicate(1.99777106478e-1f)) * z - Vec4::sReplicate(3.33329491539e-1f)) * z * x + x; + + // Put the sign back + return Vec4::sXor(y, atan_sign.ReinterpretAsFloat()); +} + +Vec4 Vec4::sATan2(Vec4Arg inY, Vec4Arg inX) +{ + UVec4 sign_mask = UVec4::sReplicate(0x80000000U); + + // Determine absolute value and sign of y + UVec4 y_sign = UVec4::sAnd(inY.ReinterpretAsInt(), sign_mask); + Vec4 y_abs = Vec4::sXor(inY, y_sign.ReinterpretAsFloat()); + + // Determine absolute value and sign of x + UVec4 x_sign = UVec4::sAnd(inX.ReinterpretAsInt(), sign_mask); + Vec4 x_abs = Vec4::sXor(inX, x_sign.ReinterpretAsFloat()); + + // Always divide smallest / largest to avoid dividing by zero + UVec4 x_is_numerator = Vec4::sLess(x_abs, y_abs); + Vec4 numerator = Vec4::sSelect(y_abs, x_abs, x_is_numerator); + Vec4 denominator = Vec4::sSelect(x_abs, y_abs, x_is_numerator); + Vec4 atan = (numerator / denominator).ATan(); + + // If we calculated x / y instead of y / x the result is PI / 2 - result (note that this is true because we know the result is positive because the input was positive) + atan = Vec4::sSelect(atan, Vec4::sReplicate(0.5f * JPH_PI) - atan, x_is_numerator); + + // Now we need to map to the correct quadrant + // x_sign y_sign result + // +1 +1 atan + // -1 +1 -atan + PI + // -1 -1 atan - PI + // +1 -1 -atan + // This can be written as: x_sign * y_sign * (atan - (x_sign < 0? PI : 0)) + atan -= Vec4::sAnd(x_sign.ArithmeticShiftRight<31>().ReinterpretAsFloat(), Vec4::sReplicate(JPH_PI)); + atan = Vec4::sXor(atan, UVec4::sXor(x_sign, y_sign).ReinterpretAsFloat()); + return atan; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Vec8.h b/WickedEngine/Jolt/Math/Vec8.h new file mode 100644 index 000000000..639d0415c --- /dev/null +++ b/WickedEngine/Jolt/Math/Vec8.h @@ -0,0 +1,112 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class [[nodiscard]] Vec8 +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Vec8() = default; ///< Intentionally not initialized for performance reasons + Vec8(const Vec8 &inRHS) = default; + JPH_INLINE Vec8(__m256 inRHS) : mValue(inRHS) { } + + /// Set 256 bit vector from 2 128 bit vectors + JPH_INLINE Vec8(Vec4Arg inLo, Vec4Arg inHi); + + /// Vector with all zeros + static JPH_INLINE Vec8 sZero(); + + /// Replicate across all components + static JPH_INLINE Vec8 sReplicate(float inV); + + /// Replicate the X component of inV to all components + static JPH_INLINE Vec8 sSplatX(Vec4Arg inV); + + /// Replicate the Y component of inV to all components + static JPH_INLINE Vec8 sSplatY(Vec4Arg inV); + + /// Replicate the Z component of inV to all components + static JPH_INLINE Vec8 sSplatZ(Vec4Arg inV); + + /// Calculates inMul1 * inMul2 + inAdd + static JPH_INLINE Vec8 sFusedMultiplyAdd(Vec8Arg inMul1, Vec8Arg inMul2, Vec8Arg inAdd); + + /// Component wise select, returns inV1 when highest bit of inControl = 0 and inV2 when highest bit of inControl = 1 + static JPH_INLINE Vec8 sSelect(Vec8Arg inV1, Vec8Arg inV2, UVec8Arg inControl); + + /// Component wise min + static JPH_INLINE Vec8 sMin(Vec8Arg inV1, Vec8Arg inV2); + + /// Component wise max + static JPH_INLINE Vec8 sMax(Vec8Arg inV1, Vec8Arg inV2); + + /// Less than + static JPH_INLINE UVec8 sLess(Vec8Arg inV1, Vec8Arg inV2); + + /// Greater than + static JPH_INLINE UVec8 sGreater(Vec8Arg inV1, Vec8Arg inV2); + + /// Load from memory + static JPH_INLINE Vec8 sLoadFloat8(const float *inV); + + /// Load 8 floats from memory, 32 bytes aligned + static JPH_INLINE Vec8 sLoadFloat8Aligned(const float *inV); + + /// Get float component by index + JPH_INLINE float operator [] (uint inCoordinate) const { JPH_ASSERT(inCoordinate < 8); return mF32[inCoordinate]; } + JPH_INLINE float & operator [] (uint inCoordinate) { JPH_ASSERT(inCoordinate < 8); return mF32[inCoordinate]; } + + /// Multiply two float vectors + JPH_INLINE Vec8 operator * (Vec8Arg inV2) const; + + /// Multiply vector by float + JPH_INLINE Vec8 operator * (float inV2) const; + + /// Add two float vectors + JPH_INLINE Vec8 operator + (Vec8Arg inV2) const; + + /// Subtract two float vectors + JPH_INLINE Vec8 operator - (Vec8Arg inV2) const; + + /// Divide + JPH_INLINE Vec8 operator / (Vec8Arg inV2) const; + + /// Reciprocal vector + JPH_INLINE Vec8 Reciprocal() const; + + /// 256 bit variant of Vec::Swizzle (no cross 128 bit lane swizzle) + template + JPH_INLINE Vec8 Swizzle() const; + + /// Get absolute value of all components + JPH_INLINE Vec8 Abs() const; + + /// Fetch the lower 128 bit from a 256 bit variable + JPH_INLINE Vec4 LowerVec4() const; + + /// Fetch the higher 128 bit from a 256 bit variable + JPH_INLINE Vec4 UpperVec4() const; + + /// Get the minimum value of the 8 floats + JPH_INLINE float ReduceMin() const; + + union + { + __m256 mValue; + float mF32[8]; + }; +}; + +static_assert(is_trivial(), "Is supposed to be a trivial type!"); + +JPH_NAMESPACE_END + +#include "Vec8.inl" diff --git a/WickedEngine/Jolt/Math/Vec8.inl b/WickedEngine/Jolt/Math/Vec8.inl new file mode 100644 index 000000000..f4d462ad0 --- /dev/null +++ b/WickedEngine/Jolt/Math/Vec8.inl @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +JPH_NAMESPACE_BEGIN + +Vec8::Vec8(Vec4Arg inLo, Vec4Arg inHi) : + mValue(_mm256_insertf128_ps(_mm256_castps128_ps256(inLo.mValue), inHi.mValue, 1)) +{ +} + +Vec8 Vec8::sZero() +{ + return _mm256_setzero_ps(); +} + +Vec8 Vec8::sReplicate(float inV) +{ + return _mm256_set1_ps(inV); +} + +Vec8 Vec8::sSplatX(Vec4Arg inV) +{ + return _mm256_set1_ps(inV.GetX()); +} + +Vec8 Vec8::sSplatY(Vec4Arg inV) +{ + return _mm256_set1_ps(inV.GetY()); +} + +Vec8 Vec8::sSplatZ(Vec4Arg inV) +{ + return _mm256_set1_ps(inV.GetZ()); +} + +Vec8 Vec8::sFusedMultiplyAdd(Vec8Arg inMul1, Vec8Arg inMul2, Vec8Arg inAdd) +{ +#ifdef JPH_USE_FMADD + return _mm256_fmadd_ps(inMul1.mValue, inMul2.mValue, inAdd.mValue); +#else + return _mm256_add_ps(_mm256_mul_ps(inMul1.mValue, inMul2.mValue), inAdd.mValue); +#endif +} + +Vec8 Vec8::sSelect(Vec8Arg inV1, Vec8Arg inV2, UVec8Arg inControl) +{ + return _mm256_blendv_ps(inV1.mValue, inV2.mValue, _mm256_castsi256_ps(inControl.mValue)); +} + +Vec8 Vec8::sMin(Vec8Arg inV1, Vec8Arg inV2) +{ + return _mm256_min_ps(inV1.mValue, inV2.mValue); +} + +Vec8 Vec8::sMax(Vec8Arg inV1, Vec8Arg inV2) +{ + return _mm256_max_ps(inV1.mValue, inV2.mValue); +} + +UVec8 Vec8::sLess(Vec8Arg inV1, Vec8Arg inV2) +{ + return _mm256_castps_si256(_mm256_cmp_ps(inV1.mValue, inV2.mValue, _CMP_LT_OQ)); +} + +UVec8 Vec8::sGreater(Vec8Arg inV1, Vec8Arg inV2) +{ + return _mm256_castps_si256(_mm256_cmp_ps(inV1.mValue, inV2.mValue, _CMP_GT_OQ)); +} + +Vec8 Vec8::sLoadFloat8(const float *inV) +{ + return _mm256_loadu_ps(inV); +} + +Vec8 Vec8::sLoadFloat8Aligned(const float *inV) +{ + return _mm256_load_ps(inV); +} + +Vec8 Vec8::operator * (Vec8Arg inV2) const +{ + return _mm256_mul_ps(mValue, inV2.mValue); +} + +Vec8 Vec8::operator * (float inV2) const +{ + return _mm256_mul_ps(mValue, _mm256_set1_ps(inV2)); +} + +Vec8 Vec8::operator + (Vec8Arg inV2) const +{ + return _mm256_add_ps(mValue, inV2.mValue); +} + +Vec8 Vec8::operator - (Vec8Arg inV2) const +{ + return _mm256_sub_ps(mValue, inV2.mValue); +} + +Vec8 Vec8::operator / (Vec8Arg inV2) const +{ + return _mm256_div_ps(mValue, inV2.mValue); +} + +Vec8 Vec8::Reciprocal() const +{ + return Vec8::sReplicate(1.0f) / mValue; +} + +template +Vec8 Vec8::Swizzle() const +{ + static_assert(SwizzleX <= 3, "SwizzleX template parameter out of range"); + static_assert(SwizzleY <= 3, "SwizzleY template parameter out of range"); + static_assert(SwizzleZ <= 3, "SwizzleZ template parameter out of range"); + static_assert(SwizzleW <= 3, "SwizzleW template parameter out of range"); + + return _mm256_shuffle_ps(mValue, mValue, _MM_SHUFFLE(SwizzleW, SwizzleZ, SwizzleY, SwizzleX)); +} + +Vec8 Vec8::Abs() const +{ +#if defined(JPH_USE_AVX512) + return _mm256_range_ps(mValue, mValue, 0b1000); +#else + return _mm256_max_ps(_mm256_sub_ps(_mm256_setzero_ps(), mValue), mValue); +#endif +} + +Vec4 Vec8::LowerVec4() const +{ + return _mm256_castps256_ps128(mValue); +} + +Vec4 Vec8::UpperVec4() const +{ + return _mm256_extractf128_ps(mValue, 1); +} + +float Vec8::ReduceMin() const +{ + return Vec4::sMin(LowerVec4(), UpperVec4()).ReduceMin(); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Math/Vector.h b/WickedEngine/Jolt/Math/Vector.h new file mode 100644 index 000000000..7057f2e2a --- /dev/null +++ b/WickedEngine/Jolt/Math/Vector.h @@ -0,0 +1,211 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Templatized vector class +template +class [[nodiscard]] Vector +{ +public: + /// Constructor + inline Vector() = default; + inline Vector(const Vector &) = default; + + /// Dimensions + inline uint GetRows() const { return Rows; } + + /// Vector with all zeros + inline void SetZero() + { + for (uint r = 0; r < Rows; ++r) + mF32[r] = 0.0f; + } + + inline static Vector sZero() { Vector v; v.SetZero(); return v; } + + /// Copy a (part) of another vector into this vector + template + void CopyPart(const OtherVector &inV, uint inSourceRow, uint inNumRows, uint inDestRow) + { + for (uint r = 0; r < inNumRows; ++r) + mF32[inDestRow + r] = inV[inSourceRow + r]; + } + + /// Get float component by index + inline float operator [] (uint inCoordinate) const + { + JPH_ASSERT(inCoordinate < Rows); + return mF32[inCoordinate]; + } + + inline float & operator [] (uint inCoordinate) + { + JPH_ASSERT(inCoordinate < Rows); + return mF32[inCoordinate]; + } + + /// Comparison + inline bool operator == (const Vector &inV2) const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != inV2.mF32[r]) + return false; + return true; + } + + inline bool operator != (const Vector &inV2) const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != inV2.mF32[r]) + return true; + return false; + } + + /// Test if vector consists of all zeros + inline bool IsZero() const + { + for (uint r = 0; r < Rows; ++r) + if (mF32[r] != 0.0f) + return false; + return true; + } + + /// Test if two vectors are close to each other + inline bool IsClose(const Vector &inV2, float inMaxDistSq = 1.0e-12f) + { + return (inV2 - *this).LengthSq() <= inMaxDistSq; + } + + /// Assignment + inline Vector & operator = (const Vector &) = default; + + /// Multiply vector with float + inline Vector operator * (const float inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] * inV2; + return v; + } + + inline Vector & operator *= (const float inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] *= inV2; + return *this; + } + + /// Multiply vector with float + inline friend Vector operator * (const float inV1, const Vector &inV2) + { + return inV2 * inV1; + } + + /// Divide vector by float + inline Vector operator / (float inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] / inV2; + return v; + } + + inline Vector & operator /= (float inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] /= inV2; + return *this; + } + + /// Add two float vectors (component wise) + inline Vector operator + (const Vector &inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] + inV2.mF32[r]; + return v; + } + + inline Vector & operator += (const Vector &inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] += inV2.mF32[r]; + return *this; + } + + /// Negate + inline Vector operator - () const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = -mF32[r]; + return v; + } + + /// Subtract two float vectors (component wise) + inline Vector operator - (const Vector &inV2) const + { + Vector v; + for (uint r = 0; r < Rows; ++r) + v.mF32[r] = mF32[r] - inV2.mF32[r]; + return v; + } + + inline Vector & operator -= (const Vector &inV2) + { + for (uint r = 0; r < Rows; ++r) + mF32[r] -= inV2.mF32[r]; + return *this; + } + + /// Dot product + inline float Dot(const Vector &inV2) const + { + float dot = 0.0f; + for (uint r = 0; r < Rows; ++r) + dot += mF32[r] * inV2.mF32[r]; + return dot; + } + + /// Squared length of vector + inline float LengthSq() const + { + return Dot(*this); + } + + /// Length of vector + inline float Length() const + { + return sqrt(LengthSq()); + } + + /// Check if vector is normalized + inline bool IsNormalized(float inToleranceSq = 1.0e-6f) + { + return abs(LengthSq() - 1.0f) <= inToleranceSq; + } + + /// Normalize vector + inline Vector Normalized() const + { + return *this / Length(); + } + + /// To String + friend ostream & operator << (ostream &inStream, const Vector &inV) + { + inStream << "["; + for (uint i = 0; i < Rows - 1; ++i) + inStream << inV.mF32[i] << ", "; + inStream << inV.mF32[Rows - 1] << "]"; + return inStream; + } + + float mF32[Rows]; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/ObjectStream/GetPrimitiveTypeOfType.h b/WickedEngine/Jolt/ObjectStream/GetPrimitiveTypeOfType.h new file mode 100644 index 000000000..cc8507636 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/GetPrimitiveTypeOfType.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get the underlying RTTI type of a type (so e.g. Array will return sometype) +template +const RTTI *GetPrimitiveTypeOfType(T *) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(T **) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(Ref *) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(RefConst *) +{ + return GetRTTIOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(Array *) +{ + return GetPrimitiveTypeOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(StaticArray *) +{ + return GetPrimitiveTypeOfType((T *)nullptr); +} + +template +const RTTI *GetPrimitiveTypeOfType(T (*)[N]) +{ + return GetPrimitiveTypeOfType((T *)nullptr); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStream.cpp b/WickedEngine/Jolt/ObjectStream/ObjectStream.cpp new file mode 100644 index 000000000..359a9e554 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStream.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +// Define macro to declare functions for a specific primitive type +#define JPH_DECLARE_PRIMITIVE(name) \ + bool OSIsType(name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::T_##name; \ + } \ + bool OSReadData(IObjectStreamIn &ioStream, name &outPrimitive) \ + { \ + return ioStream.ReadPrimitiveData(outPrimitive); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, name *) \ + { \ + ioStream.WriteDataType(EOSDataType::T_##name); \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, const name &inPrimitive) \ + { \ + ioStream.HintNextItem(); \ + ioStream.WritePrimitiveData(inPrimitive); \ + } + +// This file uses the JPH_DECLARE_PRIMITIVE macro to define all types +#include + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStream.h b/WickedEngine/Jolt/ObjectStream/ObjectStream.h new file mode 100644 index 000000000..adbfbb773 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStream.h @@ -0,0 +1,333 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Base class for object stream input and output streams. +class JPH_EXPORT ObjectStream : public NonCopyable +{ +public: + /// Stream type + enum class EStreamType + { + Text, + Binary, + }; + +protected: + /// Destructor + virtual ~ObjectStream() = default; + + /// Identifier for objects + using Identifier = uint32; + + static constexpr int sVersion = 1; + static constexpr int sRevision = 0; + static constexpr Identifier sNullIdentifier = 0; +}; + +/// Interface class for reading from an object stream +class JPH_EXPORT IObjectStreamIn : public ObjectStream +{ +public: + ///@name Input type specific operations + virtual bool ReadDataType(EOSDataType &outType) = 0; + virtual bool ReadName(String &outName) = 0; + virtual bool ReadIdentifier(Identifier &outIdentifier) = 0; + virtual bool ReadCount(uint32 &outCount) = 0; + + ///@name Read primitives + virtual bool ReadPrimitiveData(uint8 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint16 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(int &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint32 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(uint64 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(float &outPrimitive) = 0; + virtual bool ReadPrimitiveData(double &outPrimitive) = 0; + virtual bool ReadPrimitiveData(bool &outPrimitive) = 0; + virtual bool ReadPrimitiveData(String &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Float3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Double3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Vec3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(DVec3 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Vec4 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Quat &outPrimitive) = 0; + virtual bool ReadPrimitiveData(Mat44 &outPrimitive) = 0; + virtual bool ReadPrimitiveData(DMat44 &outPrimitive) = 0; + + ///@name Read compounds + virtual bool ReadClassData(const char *inClassName, void *inInstance) = 0; + virtual bool ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset = -1) = 0; +}; + +/// Interface class for writing to an object stream +class JPH_EXPORT IObjectStreamOut : public ObjectStream +{ +public: + ///@name Output type specific operations + virtual void WriteDataType(EOSDataType inType) = 0; + virtual void WriteName(const char *inName) = 0; + virtual void WriteIdentifier(Identifier inIdentifier) = 0; + virtual void WriteCount(uint32 inCount) = 0; + + ///@name Write primitives + virtual void WritePrimitiveData(const uint8 &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint16 &inPrimitive) = 0; + virtual void WritePrimitiveData(const int &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint32 &inPrimitive) = 0; + virtual void WritePrimitiveData(const uint64 &inPrimitive) = 0; + virtual void WritePrimitiveData(const float &inPrimitive) = 0; + virtual void WritePrimitiveData(const double &inPrimitive) = 0; + virtual void WritePrimitiveData(const bool &inPrimitive) = 0; + virtual void WritePrimitiveData(const String &inPrimitive) = 0; + virtual void WritePrimitiveData(const Float3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Double3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Vec3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const DVec3 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Vec4 &inPrimitive) = 0; + virtual void WritePrimitiveData(const Quat &inPrimitive) = 0; + virtual void WritePrimitiveData(const Mat44 &inPrimitive) = 0; + virtual void WritePrimitiveData(const DMat44 &inPrimitive) = 0; + + ///@name Write compounds + virtual void WritePointerData(const RTTI *inRTTI, const void *inPointer) = 0; + virtual void WriteClassData(const RTTI *inRTTI, const void *inInstance) = 0; + + ///@name Layout hints (for text output) + virtual void HintNextItem() { /* Default is do nothing */ } + virtual void HintIndentUp() { /* Default is do nothing */ } + virtual void HintIndentDown() { /* Default is do nothing */ } +}; + +// Define macro to declare functions for a specific primitive type +#define JPH_DECLARE_PRIMITIVE(name) \ + JPH_EXPORT bool OSIsType(name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + JPH_EXPORT bool OSReadData(IObjectStreamIn &ioStream, name &outPrimitive); \ + JPH_EXPORT void OSWriteDataType(IObjectStreamOut &ioStream, name *); \ + JPH_EXPORT void OSWriteData(IObjectStreamOut &ioStream, const name &inPrimitive); + +// This file uses the JPH_DECLARE_PRIMITIVE macro to define all types +#include + +// Define serialization templates +template +bool OSIsType(Array *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(StaticArray *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(T (*)[N], int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return (inArrayDepth > 0 && OSIsType(static_cast(nullptr), inArrayDepth - 1, inDataType, inClassName)); +} + +template +bool OSIsType(Ref *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return OSIsType(static_cast(nullptr), inArrayDepth, inDataType, inClassName); +} + +template +bool OSIsType(RefConst *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + return OSIsType(static_cast(nullptr), inArrayDepth, inDataType, inClassName); +} + +/// Define serialization templates for dynamic arrays +template +bool OSReadData(IObjectStreamIn &ioStream, Array &inArray) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + + // Read array items + if (continue_reading) + { + inArray.clear(); + inArray.resize(array_length); + for (uint32 el = 0; el < array_length && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + } + + return continue_reading; +} + +/// Define serialization templates for static arrays +template +bool OSReadData(IObjectStreamIn &ioStream, StaticArray &inArray) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + + // Check if we can fit this many elements + if (array_length > N) + return false; + + // Read array items + if (continue_reading) + { + inArray.clear(); + inArray.resize(array_length); + for (uint32 el = 0; el < array_length && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + } + + return continue_reading; +} + +/// Define serialization templates for C style arrays +template +bool OSReadData(IObjectStreamIn &ioStream, T (&inArray)[N]) +{ + bool continue_reading = true; + + // Read array length + uint32 array_length; + continue_reading = ioStream.ReadCount(array_length); + if (array_length != N) + return false; + + // Read array items + for (uint32 el = 0; el < N && continue_reading; ++el) + continue_reading = OSReadData(ioStream, inArray[el]); + + return continue_reading; +} + +/// Define serialization templates for references +template +bool OSReadData(IObjectStreamIn &ioStream, Ref &inRef) +{ + return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset()); +} + +template +bool OSReadData(IObjectStreamIn &ioStream, RefConst &inRef) +{ + return ioStream.ReadPointerData(JPH_RTTI(T), inRef.InternalGetPointer(), T::sInternalGetRefCountOffset()); +} + +// Define serialization templates for dynamic arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, Array *) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const Array &inArray) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(static_cast(inArray.size())); + + // Write data in array + ioStream.HintIndentUp(); + for (const T &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for static arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, StaticArray *) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const StaticArray &inArray) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(inArray.size()); + + // Write data in array + ioStream.HintIndentUp(); + for (const typename StaticArray::value_type &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for C style arrays +template +void OSWriteDataType(IObjectStreamOut &ioStream, T (*)[N]) +{ + ioStream.WriteDataType(EOSDataType::Array); + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const T (&inArray)[N]) +{ + // Write size of array + ioStream.HintNextItem(); + ioStream.WriteCount(uint32(N)); + + // Write data in array + ioStream.HintIndentUp(); + for (const T &v : inArray) + OSWriteData(ioStream, v); + ioStream.HintIndentDown(); +} + +/// Define serialization templates for references +template +void OSWriteDataType(IObjectStreamOut &ioStream, Ref *) +{ + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const Ref &inRef) +{ + if (inRef != nullptr) + ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr()); + else + ioStream.WritePointerData(nullptr, nullptr); +} + +template +void OSWriteDataType(IObjectStreamOut &ioStream, RefConst *) +{ + OSWriteDataType(ioStream, static_cast(nullptr)); +} + +template +void OSWriteData(IObjectStreamOut &ioStream, const RefConst &inRef) +{ + if (inRef != nullptr) + ioStream.WritePointerData(GetRTTI(inRef.GetPtr()), inRef.GetPtr()); + else + ioStream.WritePointerData(nullptr, nullptr); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.cpp b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.cpp new file mode 100644 index 000000000..e359f0482 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.cpp @@ -0,0 +1,234 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamBinaryIn::ObjectStreamBinaryIn(istream &inStream) : + ObjectStreamIn(inStream) +{ +} + +bool ObjectStreamBinaryIn::ReadDataType(EOSDataType &outType) +{ + uint32 type; + mStream.read((char *)&type, sizeof(type)); + if (mStream.fail()) return false; + outType = (EOSDataType)type; + return true; +} + +bool ObjectStreamBinaryIn::ReadName(String &outName) +{ + return ReadPrimitiveData(outName); +} + +bool ObjectStreamBinaryIn::ReadIdentifier(Identifier &outIdentifier) +{ + Identifier id; + mStream.read((char *)&id, sizeof(id)); + if (mStream.fail()) return false; + outIdentifier = id; + return true; +} + +bool ObjectStreamBinaryIn::ReadCount(uint32 &outCount) +{ + uint32 count; + mStream.read((char *)&count, sizeof(count)); + if (mStream.fail()) return false; + outCount = count; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint8 &outPrimitive) +{ + uint8 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint16 &outPrimitive) +{ + uint16 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(int &outPrimitive) +{ + int primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint32 &outPrimitive) +{ + uint32 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(uint64 &outPrimitive) +{ + uint64 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(float &outPrimitive) +{ + float primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(double &outPrimitive) +{ + double primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(bool &outPrimitive) +{ + bool primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(String &outPrimitive) +{ + // Read length or ID of string + uint32 len; + if (!ReadPrimitiveData(len)) + return false; + + // Check empty string + if (len == 0) + { + outPrimitive.clear(); + return true; + } + + // Check if it is an ID in the string table + if (len & 0x80000000) + { + StringTable::iterator i = mStringTable.find(len); + if (i == mStringTable.end()) + return false; + outPrimitive = i->second; + return true; + } + + // Read the string + char *data = (char *)JPH_STACK_ALLOC(len + 1); + mStream.read(data, len); + if (mStream.fail()) return false; + data[len] = 0; + outPrimitive = data; + + // Insert string in table + mStringTable.try_emplace(mNextStringID, outPrimitive); + mNextStringID++; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Float3 &outPrimitive) +{ + Float3 primitive; + mStream.read((char *)&primitive, sizeof(Float3)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Double3 &outPrimitive) +{ + Double3 primitive; + mStream.read((char *)&primitive, sizeof(Double3)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Vec3 &outPrimitive) +{ + Float3 primitive; + mStream.read((char *)&primitive, sizeof(Float3)); + if (mStream.fail()) return false; + outPrimitive = Vec3(primitive); // Use Float3 constructor so that we initialize W too + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(DVec3 &outPrimitive) +{ + Double3 primitive; + mStream.read((char *)&primitive, sizeof(Double3)); + if (mStream.fail()) return false; + outPrimitive = DVec3(primitive); // Use Float3 constructor so that we initialize W too + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Vec4 &outPrimitive) +{ + Vec4 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Quat &outPrimitive) +{ + Quat primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(Mat44 &outPrimitive) +{ + Mat44 primitive; + mStream.read((char *)&primitive, sizeof(primitive)); + if (mStream.fail()) return false; + outPrimitive = primitive; + return true; +} + +bool ObjectStreamBinaryIn::ReadPrimitiveData(DMat44 &outPrimitive) +{ + Vec4 c0, c1, c2; + DVec3 c3; + if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3)) + return false; + outPrimitive = DMat44(c0, c1, c2, c3); + return true; +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.h b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.h new file mode 100644 index 000000000..310962b40 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryIn.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream binary input stream. +class JPH_EXPORT ObjectStreamBinaryIn : public ObjectStreamIn +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ObjectStreamBinaryIn(istream &inStream); + + ///@name Input type specific operations + virtual bool ReadDataType(EOSDataType &outType) override; + virtual bool ReadName(String &outName) override; + virtual bool ReadIdentifier(Identifier &outIdentifier) override; + virtual bool ReadCount(uint32 &outCount) override; + + virtual bool ReadPrimitiveData(uint8 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint16 &outPrimitive) override; + virtual bool ReadPrimitiveData(int &outPrimitive) override; + virtual bool ReadPrimitiveData(uint32 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint64 &outPrimitive) override; + virtual bool ReadPrimitiveData(float &outPrimitive) override; + virtual bool ReadPrimitiveData(double &outPrimitive) override; + virtual bool ReadPrimitiveData(bool &outPrimitive) override; + virtual bool ReadPrimitiveData(String &outPrimitive) override; + virtual bool ReadPrimitiveData(Float3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Double3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(DVec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec4 &outPrimitive) override; + virtual bool ReadPrimitiveData(Quat &outPrimitive) override; + virtual bool ReadPrimitiveData(Mat44 &outPrimitive) override; + virtual bool ReadPrimitiveData(DMat44 &outPrimitive) override; + +private: + using StringTable = UnorderedMap; + + StringTable mStringTable; + uint32 mNextStringID = 0x80000000; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.cpp b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.cpp new file mode 100644 index 000000000..042c8edb3 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.cpp @@ -0,0 +1,155 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamBinaryOut::ObjectStreamBinaryOut(ostream &inStream) : + ObjectStreamOut(inStream) +{ + String header; + header = StringFormat("BOS%2d.%02d", ObjectStream::sVersion, ObjectStream::sRevision); + mStream.write(header.c_str(), header.size()); +} + +void ObjectStreamBinaryOut::WriteDataType(EOSDataType inType) +{ + mStream.write((const char *)&inType, sizeof(inType)); +} + +void ObjectStreamBinaryOut::WriteName(const char *inName) +{ + WritePrimitiveData(String(inName)); +} + +void ObjectStreamBinaryOut::WriteIdentifier(Identifier inIdentifier) +{ + mStream.write((const char *)&inIdentifier, sizeof(inIdentifier)); +} + +void ObjectStreamBinaryOut::WriteCount(uint32 inCount) +{ + mStream.write((const char *)&inCount, sizeof(inCount)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint8 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint16 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const int &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint32 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const uint64 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const float &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const double &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const bool &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const String &inPrimitive) +{ + // Empty strings are trivial + if (inPrimitive.empty()) + { + WritePrimitiveData((uint32)0); + return; + } + + // Check if we've already written this string + StringTable::iterator i = mStringTable.find(inPrimitive); + if (i != mStringTable.end()) + { + WritePrimitiveData(i->second); + return; + } + + // Insert string in table + mStringTable.try_emplace(inPrimitive, mNextStringID); + mNextStringID++; + + // Write string + uint32 len = min((uint32)inPrimitive.size(), (uint32)0x7fffffff); + WritePrimitiveData(len); + mStream.write(inPrimitive.c_str(), len); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Float3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(Float3)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Double3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(Double3)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Vec3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, 3 * sizeof(float)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const DVec3 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, 3 * sizeof(double)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Vec4 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Quat &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const Mat44 &inPrimitive) +{ + mStream.write((const char *)&inPrimitive, sizeof(inPrimitive)); +} + +void ObjectStreamBinaryOut::WritePrimitiveData(const DMat44 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetColumn4(0)); + WritePrimitiveData(inPrimitive.GetColumn4(1)); + WritePrimitiveData(inPrimitive.GetColumn4(2)); + WritePrimitiveData(inPrimitive.GetTranslation()); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM + diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.h b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.h new file mode 100644 index 000000000..c35a4a59c --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamBinaryOut.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream binary output stream. +class JPH_EXPORT ObjectStreamBinaryOut : public ObjectStreamOut +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor and destructor + explicit ObjectStreamBinaryOut(ostream &inStream); + + ///@name Output type specific operations + virtual void WriteDataType(EOSDataType inType) override; + virtual void WriteName(const char *inName) override; + virtual void WriteIdentifier(Identifier inIdentifier) override; + virtual void WriteCount(uint32 inCount) override; + + virtual void WritePrimitiveData(const uint8 &inPrimitive) override; + virtual void WritePrimitiveData(const uint16 &inPrimitive) override; + virtual void WritePrimitiveData(const int &inPrimitive) override; + virtual void WritePrimitiveData(const uint32 &inPrimitive) override; + virtual void WritePrimitiveData(const uint64 &inPrimitive) override; + virtual void WritePrimitiveData(const float &inPrimitive) override; + virtual void WritePrimitiveData(const double &inPrimitive) override; + virtual void WritePrimitiveData(const bool &inPrimitive) override; + virtual void WritePrimitiveData(const String &inPrimitive) override; + virtual void WritePrimitiveData(const Float3 &inPrimitive) override; + virtual void WritePrimitiveData(const Double3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec3 &inPrimitive) override; + virtual void WritePrimitiveData(const DVec3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec4 &inPrimitive) override; + virtual void WritePrimitiveData(const Quat &inPrimitive) override; + virtual void WritePrimitiveData(const Mat44 &inPrimitive) override; + virtual void WritePrimitiveData(const DMat44 &inPrimitive) override; + +private: + using StringTable = UnorderedMap; + + StringTable mStringTable; + uint32 mNextStringID = 0x80000000; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamIn.cpp b/WickedEngine/Jolt/ObjectStream/ObjectStreamIn.cpp new file mode 100644 index 000000000..2f9d56cd8 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamIn.cpp @@ -0,0 +1,621 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamIn::ObjectStreamIn(istream &inStream) : + mStream(inStream) +{ +} + +bool ObjectStreamIn::GetInfo(istream &inStream, EStreamType &outType, int &outVersion, int &outRevision) +{ + // Read header and check if it is the correct format, e.g. "TOS 1.00" + char header[9]; + memset(header, 0, 9); + inStream.read(header, 8); + if ((header[0] == 'B' || header[0] == 'T') && header[1] == 'O' && header[2] == 'S' + && (header[3] == ' ' || isdigit(header[3])) && isdigit(header[4]) + && header[5] == '.' && isdigit(header[6]) && isdigit(header[7])) + { + // Check if this is a binary or text objectfile + switch (header[0]) + { + case 'T': outType = ObjectStream::EStreamType::Text; break; + case 'B': outType = ObjectStream::EStreamType::Binary; break; + default: JPH_ASSERT(false); break; + } + + // Extract version and revision + header[5] = '\0'; + outVersion = atoi(&header[3]); + outRevision = atoi(&header[6]); + + return true; + } + + Trace("ObjectStreamIn: Not a valid object stream."); + return false; +} + +ObjectStreamIn *ObjectStreamIn::Open(istream &inStream) +{ + // Check if file is an ObjectStream of the correct version and revision + EStreamType type; + int version; + int revision; + if (GetInfo(inStream, type, version, revision)) + { + if (version == sVersion && revision == sRevision) + { + // Create an input stream of the correct type + switch (type) + { + case EStreamType::Text: return new ObjectStreamTextIn(inStream); + case EStreamType::Binary: return new ObjectStreamBinaryIn(inStream); + default: JPH_ASSERT(false); + } + } + else + { + Trace("ObjectStreamIn: Different version stream (%d.%02d, expected %d.%02d).", version, revision, sVersion, sRevision); + } + } + + return nullptr; +} + +void *ObjectStreamIn::Read(const RTTI *inRTTI) +{ + using ObjectSet = UnorderedSet; + + // Read all information on the stream + void *main_object = nullptr; + bool continue_reading = true; + for (;;) + { + // Get type of next operation + EOSDataType data_type; + if (!ReadDataType(data_type)) + break; + + if (data_type == EOSDataType::Declare) + { + // Read type declaration + if (!ReadRTTI()) + { + Trace("ObjectStreamIn: Fatal error while reading class description for class %s.", inRTTI->GetName()); + continue_reading = false; + break; + } + } + else if (data_type == EOSDataType::Object) + { + const RTTI *rtti; + void *object = ReadObject(rtti); + if (!main_object && object) + { + // This is the first and thus main object of the file. + if (rtti->IsKindOf(inRTTI)) + { + // Object is of correct type + main_object = object; + } + else + { + Trace("ObjectStreamIn: Main object of different type. Expected %s, but found %s.", inRTTI->GetName(), rtti->GetName()); + continue_reading = false; + break; + } + } + } + else + { + // Invalid or out of place token found + Trace("ObjectStreamIn: Invalid or out of place token found."); + continue_reading = false; + break; + } + } + + // Resolve links (pointer, references) + if (continue_reading) + { + // Resolve links + ObjectSet referenced_objects; + for (Link &link : mUnresolvedLinks) + { + IdentifierMap::const_iterator j = mIdentifierMap.find(link.mIdentifier); + if (j != mIdentifierMap.end() && j->second.mRTTI->IsKindOf(link.mRTTI)) + { + const ObjectInfo &obj_info = j->second; + + // Set pointer + *link.mPointer = obj_info.mInstance; + + // Increment refcount if it was a referencing pointer + if (link.mRefCountOffset != -1) + ++(*(uint32 *)(((uint8 *)obj_info.mInstance) + link.mRefCountOffset)); + + // Add referenced object to the list + if (referenced_objects.find(obj_info.mInstance) == referenced_objects.end()) + referenced_objects.insert(obj_info.mInstance); + } + else + { + // Referenced object not found, set pointer to nullptr + Trace("ObjectStreamIn: Setting incorrect pointer to class of type %s to nullptr.", link.mRTTI->GetName()); + *link.mPointer = nullptr; + } + } + + // Release unreferenced objects except the main object + for (const IdentifierMap::value_type &j : mIdentifierMap) + { + const ObjectInfo &obj_info = j.second; + + if (obj_info.mInstance != main_object) + { + ObjectSet::const_iterator k = referenced_objects.find(obj_info.mInstance); + if (k == referenced_objects.end()) + { + Trace("ObjectStreamIn: Releasing unreferenced object of type %s.", obj_info.mRTTI->GetName()); + obj_info.mRTTI->DestructObject(obj_info.mInstance); + } + } + } + + return main_object; + } + else + { + // Release all objects if a fatal error occurred + for (const IdentifierMap::value_type &i : mIdentifierMap) + { + const ObjectInfo &obj_info = i.second; + obj_info.mRTTI->DestructObject(obj_info.mInstance); + } + + return nullptr; + } +} + +void *ObjectStreamIn::ReadObject(const RTTI *& outRTTI) +{ + // Read the object class + void *object = nullptr; + String class_name; + if (ReadName(class_name)) + { + // Get class description + ClassDescriptionMap::iterator i = mClassDescriptionMap.find(class_name); + if (i != mClassDescriptionMap.end()) + { + const ClassDescription &class_desc = i->second; + + // Read object identifier + Identifier identifier; + if (ReadIdentifier(identifier)) + { + // Check if this object can be read or must be skipped + if (identifier != sNullIdentifier + && class_desc.mRTTI + && !class_desc.mRTTI->IsAbstract()) + { + // Create object instance + outRTTI = class_desc.mRTTI; + object = outRTTI->CreateObject(); + + // Read object attributes + if (ReadClassData(class_desc, object)) + { + // Add object to identifier map + mIdentifierMap.try_emplace(identifier, object, outRTTI); + } + else + { + // Fatal error while reading attributes, release object + outRTTI->DestructObject(object); + object = nullptr; + } + } + else + { + // Skip this object + // TODO: This operation can fail, but there is no check yet + Trace("ObjectStreamIn: Found uncreatable object %s.", class_name.c_str()); + ReadClassData(class_desc, nullptr); + } + } + } + else + { + // TODO: This is a fatal error, but this function has no way of indicating this + Trace("ObjectStreamIn: Found object of unknown class %s.", class_name.c_str()); + } + } + + return object; +} + +bool ObjectStreamIn::ReadRTTI() +{ + // Read class name and find it's attribute info + String class_name; + if (!ReadName(class_name)) + return false; + + // Find class + const RTTI *rtti = Factory::sInstance->Find(class_name.c_str()); + if (rtti == nullptr) + Trace("ObjectStreamIn: Unknown class: \"%s\".", class_name.c_str()); + + // Insert class description + ClassDescription &class_desc = mClassDescriptionMap.try_emplace(class_name, rtti).first->second; + + // Read the number of entries in the description + uint32 count; + if (!ReadCount(count)) + return false; + + // Read the entries + for (uint32 i = 0; i < count; ++i) + { + AttributeDescription attribute; + + // Read name + String attribute_name; + if (!ReadName(attribute_name)) + return false; + + // Read type + if (!ReadDataType(attribute.mSourceType)) + return false; + + // Read array depth + while (attribute.mSourceType == EOSDataType::Array) + { + ++attribute.mArrayDepth; + if (!ReadDataType(attribute.mSourceType)) + return false; + } + + // Read instance/pointer class name + if ((attribute.mSourceType == EOSDataType::Instance || attribute.mSourceType == EOSDataType::Pointer) + && !ReadName(attribute.mClassName)) + return false; + + // Find attribute in rtti + if (rtti) + { + // Find attribute index + for (int idx = 0; idx < rtti->GetAttributeCount(); ++idx) + { + const SerializableAttribute &attr = rtti->GetAttribute(idx); + if (strcmp(attr.GetName(), attribute_name.c_str()) == 0) + { + attribute.mIndex = idx; + break; + } + } + + // Check if attribute is of expected type + if (attribute.mIndex >= 0) + { + const SerializableAttribute &attr = rtti->GetAttribute(attribute.mIndex); + if (attr.IsType(attribute.mArrayDepth, attribute.mSourceType, attribute.mClassName.c_str())) + { + // No conversion needed + attribute.mDestinationType = attribute.mSourceType; + } + else if (attribute.mArrayDepth == 0 && attribute.mClassName.empty()) + { + // Try to apply type conversions + if (attribute.mSourceType == EOSDataType::T_Vec3 && attr.IsType(0, EOSDataType::T_DVec3, "")) + attribute.mDestinationType = EOSDataType::T_DVec3; + else if (attribute.mSourceType == EOSDataType::T_DVec3 && attr.IsType(0, EOSDataType::T_Vec3, "")) + attribute.mDestinationType = EOSDataType::T_Vec3; + else + attribute.mIndex = -1; + } + else + { + // No conversion exists + attribute.mIndex = -1; + } + } + } + + // Add attribute to the class description + class_desc.mAttributes.push_back(attribute); + } + + return true; +} + +bool ObjectStreamIn::ReadClassData(const char *inClassName, void *inInstance) +{ + // Find the class description + ClassDescriptionMap::iterator i = mClassDescriptionMap.find(inClassName); + if (i != mClassDescriptionMap.end()) + return ReadClassData(i->second, inInstance); + + return false; +} + +bool ObjectStreamIn::ReadClassData(const ClassDescription &inClassDesc, void *inInstance) +{ + // Read data for this class + bool continue_reading = true; + + for (const AttributeDescription &attr_desc : inClassDesc.mAttributes) + { + // Read or skip the attribute data + if (attr_desc.mIndex >= 0 && inInstance) + { + const SerializableAttribute &attr = inClassDesc.mRTTI->GetAttribute(attr_desc.mIndex); + if (attr_desc.mSourceType == attr_desc.mDestinationType) + { + continue_reading = attr.ReadData(*this, inInstance); + } + else if (attr_desc.mSourceType == EOSDataType::T_Vec3 && attr_desc.mDestinationType == EOSDataType::T_DVec3) + { + // Vec3 to DVec3 + Vec3 tmp; + continue_reading = ReadPrimitiveData(tmp); + if (continue_reading) + *attr.GetMemberPointer(inInstance) = DVec3(tmp); + } + else if (attr_desc.mSourceType == EOSDataType::T_DVec3 && attr_desc.mDestinationType == EOSDataType::T_Vec3) + { + // DVec3 to Vec3 + DVec3 tmp; + continue_reading = ReadPrimitiveData(tmp); + if (continue_reading) + *attr.GetMemberPointer(inInstance) = Vec3(tmp); + } + else + { + JPH_ASSERT(false); // Unknown conversion + continue_reading = SkipAttributeData(attr_desc.mArrayDepth, attr_desc.mSourceType, attr_desc.mClassName.c_str()); + } + } + else + continue_reading = SkipAttributeData(attr_desc.mArrayDepth, attr_desc.mSourceType, attr_desc.mClassName.c_str()); + + if (!continue_reading) + break; + } + + return continue_reading; +} + +bool ObjectStreamIn::ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset) +{ + Identifier identifier; + if (ReadIdentifier(identifier)) + { + if (identifier == sNullIdentifier) + { + // Set nullptr pointer + inPointer = nullptr; + } + else + { + // Put pointer on the list to be resolved later on + Link &link = mUnresolvedLinks.emplace_back(); + link.mPointer = inPointer; + link.mRefCountOffset = inRefCountOffset; + link.mIdentifier = identifier; + link.mRTTI = inRTTI; + } + + return true; + } + + return false; +} + +bool ObjectStreamIn::SkipAttributeData(int inArrayDepth, EOSDataType inDataType, const char *inClassName) +{ + bool continue_reading = true; + + // Get number of items to read + uint32 count = 1; + for (; inArrayDepth > 0; --inArrayDepth) + { + uint32 temporary; + if (ReadCount(temporary)) + { + // Multiply for multi dimensional arrays + count *= temporary; + } + else + { + // Fatal error while reading array size + continue_reading = false; + break; + } + } + + // Read data for all items + if (continue_reading) + { + if (inDataType == EOSDataType::Instance) + { + // Get the class description + ClassDescriptionMap::iterator i = mClassDescriptionMap.find(inClassName); + if (i != mClassDescriptionMap.end()) + { + for (; count > 0 && continue_reading; --count) + continue_reading = ReadClassData(i->second, nullptr); + } + else + { + continue_reading = false; + Trace("ObjectStreamIn: Found instance of unknown class %s.", inClassName); + } + } + else + { + for (; count > 0 && continue_reading; --count) + { + switch (inDataType) + { + case EOSDataType::Pointer: + { + Identifier temporary; + continue_reading = ReadIdentifier(temporary); + break; + } + + case EOSDataType::T_uint8: + { + uint8 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_uint16: + { + uint16 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_int: + { + int temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_uint32: + { + uint32 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_uint64: + { + uint64 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_float: + { + float temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_double: + { + double temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_bool: + { + bool temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_String: + { + String temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Float3: + { + Float3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Double3: + { + Double3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Vec3: + { + Vec3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_DVec3: + { + DVec3 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Vec4: + { + Vec4 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Quat: + { + Quat temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_Mat44: + { + Mat44 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::T_DMat44: + { + DMat44 temporary; + continue_reading = ReadPrimitiveData(temporary); + break; + } + + case EOSDataType::Array: + case EOSDataType::Object: + case EOSDataType::Declare: + case EOSDataType::Instance: + case EOSDataType::Invalid: + default: + continue_reading = false; + break; + } + } + } + } + + return continue_reading; +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamIn.h b/WickedEngine/Jolt/ObjectStream/ObjectStreamIn.h new file mode 100644 index 000000000..9b99cc2bf --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamIn.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// ObjectStreamIn contains all logic for reading an object from disk. It is the base +/// class for the text and binary input streams (ObjectStreamTextIn and ObjectStreamBinaryIn). +class JPH_EXPORT ObjectStreamIn : public IObjectStreamIn +{ +private: + struct ClassDescription; + +public: + /// Main function to read an object from a stream + template + static bool sReadObject(istream &inStream, T *&outObject) + { + // Create the input stream + bool result = false; + ObjectStreamIn *stream = ObjectStreamIn::Open(inStream); + if (stream) + { + // Read the object + outObject = (T *)stream->Read(JPH_RTTI(T)); + result = (outObject != nullptr); + delete stream; + } + return result; + } + + /// Main function to read an object from a stream (reference counting pointer version) + template + static bool sReadObject(istream &inStream, Ref &outObject) + { + T *object = nullptr; + bool result = sReadObject(inStream, object); + outObject = object; + return result; + } + + /// Main function to read an object from a file + template + static bool sReadObject(const char *inFileName, T *&outObject) + { + std::ifstream stream; + stream.open(inFileName, std::ifstream::in | std::ifstream::binary); + if (!stream.is_open()) + return false; + return sReadObject(stream, outObject); + } + + /// Main function to read an object from a file (reference counting pointer version) + template + static bool sReadObject(const char *inFileName, Ref &outObject) + { + T *object = nullptr; + bool result = sReadObject(inFileName, object); + outObject = object; + return result; + } + + ////////////////////////////////////////////////////// + // EVERYTHING BELOW THIS SHOULD NOT DIRECTLY BE CALLED + ////////////////////////////////////////////////////// + + ///@name Serialization operations + void * Read(const RTTI *inRTTI); + void * ReadObject(const RTTI *& outRTTI); + bool ReadRTTI(); + virtual bool ReadClassData(const char *inClassName, void *inInstance) override; + bool ReadClassData(const ClassDescription &inClassDesc, void *inInstance); + virtual bool ReadPointerData(const RTTI *inRTTI, void **inPointer, int inRefCountOffset = -1) override; + bool SkipAttributeData(int inArrayDepth, EOSDataType inDataType, const char *inClassName); + +protected: + /// Constructor + explicit ObjectStreamIn(istream &inStream); + + /// Determine the type and version of an object stream + static bool GetInfo(istream &inStream, EStreamType &outType, int &outVersion, int &outRevision); + + /// Static constructor + static ObjectStreamIn * Open(istream &inStream); + + istream & mStream; + +private: + /// Class descriptions + struct AttributeDescription + { + int mArrayDepth = 0; + EOSDataType mSourceType = EOSDataType::Invalid; + EOSDataType mDestinationType = EOSDataType::Invalid; + String mClassName; + int mIndex = -1; + }; + + struct ClassDescription + { + ClassDescription() = default; + explicit ClassDescription(const RTTI *inRTTI) : mRTTI(inRTTI) { } + + const RTTI * mRTTI = nullptr; + Array mAttributes; + }; + + struct ObjectInfo + { + ObjectInfo() = default; + ObjectInfo(void *inInstance, const RTTI *inRTTI) : mInstance(inInstance), mRTTI(inRTTI) { } + + void * mInstance = nullptr; + const RTTI * mRTTI = nullptr; + }; + + struct Link + { + void ** mPointer; + int mRefCountOffset; + Identifier mIdentifier; + const RTTI * mRTTI; + }; + + using IdentifierMap = UnorderedMap; + using ClassDescriptionMap = UnorderedMap; + + ClassDescriptionMap mClassDescriptionMap; + IdentifierMap mIdentifierMap; ///< Links identifier to an object pointer + Array mUnresolvedLinks; ///< All pointers (links) are resolved after reading the entire file, e.g. when all object exist +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamOut.cpp b/WickedEngine/Jolt/ObjectStream/ObjectStreamOut.cpp new file mode 100644 index 000000000..f44f9ea75 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamOut.cpp @@ -0,0 +1,168 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +ObjectStreamOut::ObjectStreamOut(ostream &inStream) : + mStream(inStream) +{ +// Add all primitives to the class set +#define JPH_DECLARE_PRIMITIVE(name) mClassSet.insert(JPH_RTTI(name)); +#include +} + +ObjectStreamOut *ObjectStreamOut::Open(EStreamType inType, ostream &inStream) +{ + switch (inType) + { + case EStreamType::Text: return new ObjectStreamTextOut(inStream); + case EStreamType::Binary: return new ObjectStreamBinaryOut(inStream); + default: JPH_ASSERT(false); + } + return nullptr; +} + +bool ObjectStreamOut::Write(const void *inObject, const RTTI *inRTTI) +{ + // Assign a new identifier to the object and write it + mIdentifierMap.try_emplace(inObject, mNextIdentifier, inRTTI); + mNextIdentifier++; + WriteObject(inObject); + + // Write all linked objects + while (!mObjectQueue.empty() && !mStream.fail()) + { + const void *linked_object = mObjectQueue.front(); + WriteObject(linked_object); + mObjectQueue.pop(); + } + return !mStream.fail(); +} + +void ObjectStreamOut::WriteObject(const void *inObject) +{ + // Find object identifier + IdentifierMap::iterator i = mIdentifierMap.find(inObject); + JPH_ASSERT(i != mIdentifierMap.end()); + + // Write class description and associated descriptions + QueueRTTI(i->second.mRTTI); + while (!mClassQueue.empty() && !mStream.fail()) + { + WriteRTTI(mClassQueue.front()); + mClassQueue.pop(); + } + + HintNextItem(); + HintNextItem(); + + // Write object header. + WriteDataType(EOSDataType::Object); + WriteName(i->second.mRTTI->GetName()); + WriteIdentifier(i->second.mIdentifier); + + // Write attribute data + WriteClassData(i->second.mRTTI, inObject); +} + +void ObjectStreamOut::QueueRTTI(const RTTI *inRTTI) +{ + ClassSet::const_iterator i = mClassSet.find(inRTTI); + if (i == mClassSet.end()) + { + mClassSet.insert(inRTTI); + mClassQueue.push(inRTTI); + } +} + +void ObjectStreamOut::WriteRTTI(const RTTI *inRTTI) +{ + HintNextItem(); + HintNextItem(); + + // Write class header. E.g. in text mode: "class " + WriteDataType(EOSDataType::Declare); + WriteName(inRTTI->GetName()); + WriteCount(inRTTI->GetAttributeCount()); + + // Write class attribute info + HintIndentUp(); + for (int attr_index = 0; attr_index < inRTTI->GetAttributeCount(); ++attr_index) + { + // Get attribute + const SerializableAttribute &attr = inRTTI->GetAttribute(attr_index); + + // Write definition of attribute class if undefined + const RTTI *rtti = attr.GetMemberPrimitiveType(); + if (rtti != nullptr) + QueueRTTI(rtti); + + HintNextItem(); + + // Write attribute information. + WriteName(attr.GetName()); + attr.WriteDataType(*this); + } + HintIndentDown(); +} + +void ObjectStreamOut::WriteClassData(const RTTI *inRTTI, const void *inInstance) +{ + JPH_ASSERT(inInstance); + + // Write attributes + HintIndentUp(); + for (int attr_index = 0; attr_index < inRTTI->GetAttributeCount(); ++attr_index) + { + // Get attribute + const SerializableAttribute &attr = inRTTI->GetAttribute(attr_index); + attr.WriteData(*this, inInstance); + } + HintIndentDown(); +} + +void ObjectStreamOut::WritePointerData(const RTTI *inRTTI, const void *inPointer) +{ + Identifier identifier; + + if (inPointer) + { + // Check if this object has an identifier + IdentifierMap::iterator i = mIdentifierMap.find(inPointer); + if (i != mIdentifierMap.end()) + { + // Object already has an identifier + identifier = i->second.mIdentifier; + } + else + { + // Assign a new identifier to this object and queue it for serialization + identifier = mNextIdentifier++; + mIdentifierMap.try_emplace(inPointer, identifier, inRTTI); + mObjectQueue.push(inPointer); + } + } + else + { + // Write nullptr pointer + identifier = sNullIdentifier; + } + + // Write the identifier + HintNextItem(); + WriteIdentifier(identifier); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamOut.h b/WickedEngine/Jolt/ObjectStream/ObjectStreamOut.h new file mode 100644 index 000000000..2d5a476bc --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamOut.h @@ -0,0 +1,104 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +template using Queue = std::queue>>; + +/// ObjectStreamOut contains all logic for writing an object to disk. It is the base +/// class for the text and binary output streams (ObjectStreamTextOut and ObjectStreamBinaryOut). +class JPH_EXPORT ObjectStreamOut : public IObjectStreamOut +{ +private: + struct ObjectInfo; + +public: + /// Main function to write an object to a stream + template + static bool sWriteObject(ostream &inStream, ObjectStream::EStreamType inType, const T &inObject) + { + // Create the output stream + bool result = false; + ObjectStreamOut *stream = ObjectStreamOut::Open(inType, inStream); + if (stream) + { + // Write the object to the stream + result = stream->Write((void *)&inObject, GetRTTI(&inObject)); + delete stream; + } + + return result; + } + + /// Main function to write an object to a file + template + static bool sWriteObject(const char *inFileName, ObjectStream::EStreamType inType, const T &inObject) + { + std::ofstream stream; + stream.open(inFileName, std::ofstream::out | std::ofstream::trunc | std::ofstream::binary); + if (!stream.is_open()) + return false; + return sWriteObject(stream, inType, inObject); + } + + ////////////////////////////////////////////////////// + // EVERYTHING BELOW THIS SHOULD NOT DIRECTLY BE CALLED + ////////////////////////////////////////////////////// + + ///@name Serialization operations + bool Write(const void *inObject, const RTTI *inRTTI); + void WriteObject(const void *inObject); + void QueueRTTI(const RTTI *inRTTI); + void WriteRTTI(const RTTI *inRTTI); + virtual void WriteClassData(const RTTI *inRTTI, const void *inInstance) override; + virtual void WritePointerData(const RTTI *inRTTI, const void *inPointer) override; + +protected: + /// Static constructor + static ObjectStreamOut * Open(EStreamType inType, ostream &inStream); + + /// Constructor + explicit ObjectStreamOut(ostream &inStream); + + ostream & mStream; + +private: + struct ObjectInfo + { + ObjectInfo() : mIdentifier(0), mRTTI(nullptr) { } + ObjectInfo(Identifier inIdentifier, const RTTI *inRTTI) : mIdentifier(inIdentifier), mRTTI(inRTTI) { } + + Identifier mIdentifier; + const RTTI * mRTTI; + }; + + using IdentifierMap = UnorderedMap; + using ClassSet = UnorderedSet; + using ObjectQueue = Queue; + using ClassQueue = Queue; + + Identifier mNextIdentifier = sNullIdentifier + 1; ///< Next free identifier for this stream + IdentifierMap mIdentifierMap; ///< Links object pointer to an identifier + ObjectQueue mObjectQueue; ///< Queue of objects to be written + ClassSet mClassSet; ///< List of classes already written + ClassQueue mClassQueue; ///< List of classes waiting to be written +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.cpp b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.cpp new file mode 100644 index 000000000..4160df865 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.cpp @@ -0,0 +1,396 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamTextIn::ObjectStreamTextIn(istream &inStream) : + ObjectStreamIn(inStream) +{ +} + +bool ObjectStreamTextIn::ReadDataType(EOSDataType &outType) +{ + String token; + if (ReadWord(token)) + { + transform(token.begin(), token.end(), token.begin(), [](char inValue) { return (char)tolower(inValue); }); + if (token == "declare") + outType = EOSDataType::Declare; + else if (token == "object") + outType = EOSDataType::Object; + else if (token == "instance") + outType = EOSDataType::Instance; + else if (token == "pointer") + outType = EOSDataType::Pointer; + else if (token == "array") + outType = EOSDataType::Array; + else if (token == "uint8") + outType = EOSDataType::T_uint8; + else if (token == "uint16") + outType = EOSDataType::T_uint16; + else if (token == "int") + outType = EOSDataType::T_int; + else if (token == "uint32") + outType = EOSDataType::T_uint32; + else if (token == "uint64") + outType = EOSDataType::T_uint64; + else if (token == "float") + outType = EOSDataType::T_float; + else if (token == "double") + outType = EOSDataType::T_double; + else if (token == "bool") + outType = EOSDataType::T_bool; + else if (token == "string") + outType = EOSDataType::T_String; + else if (token == "float3") + outType = EOSDataType::T_Float3; + else if (token == "double3") + outType = EOSDataType::T_Double3; + else if (token == "vec3") + outType = EOSDataType::T_Vec3; + else if (token == "dvec3") + outType = EOSDataType::T_DVec3; + else if (token == "vec4") + outType = EOSDataType::T_Vec4; + else if (token == "quat") + outType = EOSDataType::T_Quat; + else if (token == "mat44") + outType = EOSDataType::T_Mat44; + else if (token == "dmat44") + outType = EOSDataType::T_DMat44; + else + { + Trace("ObjectStreamTextIn: Found unknown data type."); + return false; + } + return true; + } + return false; +} + +bool ObjectStreamTextIn::ReadName(String &outName) +{ + return ReadWord(outName); +} + +bool ObjectStreamTextIn::ReadIdentifier(Identifier &outIdentifier) +{ + String token; + if (!ReadWord(token)) + return false; + outIdentifier = (uint32)std::strtoul(token.c_str(), nullptr, 16); + if (errno == ERANGE) + { + outIdentifier = sNullIdentifier; + return false; + } + return true; +} + +bool ObjectStreamTextIn::ReadCount(uint32 &outCount) +{ + return ReadPrimitiveData(outCount); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint8 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + uint32 temporary; + IStringStream stream(token); + stream >> temporary; + if (!stream.fail()) + { + outPrimitive = (uint8)temporary; + return true; + } + return false; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint16 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + uint32 temporary; + IStringStream stream(token); + stream >> temporary; + if (!stream.fail()) + { + outPrimitive = (uint16)temporary; + return true; + } + return false; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(int &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint32 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(uint64 &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(float &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(double &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + IStringStream stream(token); + stream >> outPrimitive; + return !stream.fail(); +} + +bool ObjectStreamTextIn::ReadPrimitiveData(bool &outPrimitive) +{ + String token; + if (!ReadWord(token)) + return false; + transform(token.begin(), token.end(), token.begin(), [](char inValue) { return (char)tolower(inValue); }); + outPrimitive = token == "true"; + return outPrimitive || token == "false"; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(String &outPrimitive) +{ + outPrimitive.clear(); + + char c; + + // Skip whitespace + for (;;) + { + if (!ReadChar(c)) + return false; + + if (!isspace(c)) + break; + } + + // Check if it is a opening quote + if (c != '\"') + return false; + + // Read string and interpret special characters + String result; + bool escaped = false; + for (;;) + { + if (!ReadChar(c)) + break; + + switch (c) + { + case '\n': + case '\t': + break; + + case '\\': + if (escaped) + { + result += '\\'; + escaped = false; + } + else + escaped = true; + break; + + case 'n': + if (escaped) + { + result += '\n'; + escaped = false; + } + else + result += 'n'; + break; + + case 't': + if (escaped) + { + result += '\t'; + escaped = false; + } + else + result += 't'; + break; + + case '\"': + if (escaped) + { + result += '\"'; + escaped = false; + } + else + { + // Found closing double quote + outPrimitive = result; + return true; + } + break; + + default: + if (escaped) + escaped = false; + else + result += c; + break; + } + } + + return false; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Float3 &outPrimitive) +{ + float x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = Float3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Double3 &outPrimitive) +{ + double x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = Double3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Vec3 &outPrimitive) +{ + float x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = Vec3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(DVec3 &outPrimitive) +{ + double x, y, z; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z)) + return false; + outPrimitive = DVec3(x, y, z); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Vec4 &outPrimitive) +{ + float x, y, z, w; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z) || !ReadPrimitiveData(w)) + return false; + outPrimitive = Vec4(x, y, z, w); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Quat &outPrimitive) +{ + float x, y, z, w; + if (!ReadPrimitiveData(x) || !ReadPrimitiveData(y) || !ReadPrimitiveData(z) || !ReadPrimitiveData(w)) + return false; + outPrimitive = Quat(x, y, z, w); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(Mat44 &outPrimitive) +{ + Vec4 c0, c1, c2, c3; + if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3)) + return false; + outPrimitive = Mat44(c0, c1, c2, c3); + return true; +} + +bool ObjectStreamTextIn::ReadPrimitiveData(DMat44 &outPrimitive) +{ + Vec4 c0, c1, c2; + DVec3 c3; + if (!ReadPrimitiveData(c0) || !ReadPrimitiveData(c1) || !ReadPrimitiveData(c2) || !ReadPrimitiveData(c3)) + return false; + outPrimitive = DMat44(c0, c1, c2, c3); + return true; +} + +bool ObjectStreamTextIn::ReadChar(char &outChar) +{ + mStream.get(outChar); + return !mStream.eof(); +} + +bool ObjectStreamTextIn::ReadWord(String &outWord) +{ + outWord.clear(); + + char c; + + // Skip whitespace + for (;;) + { + if (!ReadChar(c)) + return false; + + if (!isspace(c)) + break; + } + + // Read word + for (;;) + { + outWord += c; + + if (!ReadChar(c)) + break; + + if (isspace(c)) + break; + } + + return !outWord.empty(); +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.h b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.h new file mode 100644 index 000000000..c6a164e73 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextIn.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream text input stream. +class JPH_EXPORT ObjectStreamTextIn : public ObjectStreamIn +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ObjectStreamTextIn(istream &inStream); + + ///@name Input type specific operations + virtual bool ReadDataType(EOSDataType &outType) override; + virtual bool ReadName(String &outName) override; + virtual bool ReadIdentifier(Identifier &outIdentifier) override; + virtual bool ReadCount(uint32 &outCount) override; + + virtual bool ReadPrimitiveData(uint8 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint16 &outPrimitive) override; + virtual bool ReadPrimitiveData(int &outPrimitive) override; + virtual bool ReadPrimitiveData(uint32 &outPrimitive) override; + virtual bool ReadPrimitiveData(uint64 &outPrimitive) override; + virtual bool ReadPrimitiveData(float &outPrimitive) override; + virtual bool ReadPrimitiveData(double &outPrimitive) override; + virtual bool ReadPrimitiveData(bool &outPrimitive) override; + virtual bool ReadPrimitiveData(String &outPrimitive) override; + virtual bool ReadPrimitiveData(Float3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Double3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(DVec3 &outPrimitive) override; + virtual bool ReadPrimitiveData(Vec4 &outPrimitive) override; + virtual bool ReadPrimitiveData(Quat &outPrimitive) override; + virtual bool ReadPrimitiveData(Mat44 &outPrimitive) override; + virtual bool ReadPrimitiveData(DMat44 &outPrimitive) override; + +private: + bool ReadChar(char &outChar); + bool ReadWord(String &outWord); +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.cpp b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.cpp new file mode 100644 index 000000000..7d5b13e33 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.cpp @@ -0,0 +1,231 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_OBJECT_STREAM + +#include +#include + +JPH_NAMESPACE_BEGIN + +ObjectStreamTextOut::ObjectStreamTextOut(ostream &inStream) : + ObjectStreamOut(inStream) +{ + WriteWord(StringFormat("TOS%2d.%02d", ObjectStream::sVersion, ObjectStream::sRevision)); +} + +void ObjectStreamTextOut::WriteDataType(EOSDataType inType) +{ + switch (inType) + { + case EOSDataType::Declare: WriteWord("declare "); break; + case EOSDataType::Object: WriteWord("object "); break; + case EOSDataType::Instance: WriteWord("instance "); break; + case EOSDataType::Pointer: WriteWord("pointer "); break; + case EOSDataType::Array: WriteWord("array "); break; + case EOSDataType::T_uint8: WriteWord("uint8"); break; + case EOSDataType::T_uint16: WriteWord("uint16"); break; + case EOSDataType::T_int: WriteWord("int"); break; + case EOSDataType::T_uint32: WriteWord("uint32"); break; + case EOSDataType::T_uint64: WriteWord("uint64"); break; + case EOSDataType::T_float: WriteWord("float"); break; + case EOSDataType::T_double: WriteWord("double"); break; + case EOSDataType::T_bool: WriteWord("bool"); break; + case EOSDataType::T_String: WriteWord("string"); break; + case EOSDataType::T_Float3: WriteWord("float3"); break; + case EOSDataType::T_Double3: WriteWord("double3"); break; + case EOSDataType::T_Vec3: WriteWord("vec3"); break; + case EOSDataType::T_DVec3: WriteWord("dvec3"); break; + case EOSDataType::T_Vec4: WriteWord("vec4"); break; + case EOSDataType::T_Quat: WriteWord("quat"); break; + case EOSDataType::T_Mat44: WriteWord("mat44"); break; + case EOSDataType::T_DMat44: WriteWord("dmat44"); break; + case EOSDataType::Invalid: + default: JPH_ASSERT(false); break; + } +} + +void ObjectStreamTextOut::WriteName(const char *inName) +{ + WriteWord(String(inName) + " "); +} + +void ObjectStreamTextOut::WriteIdentifier(Identifier inIdentifier) +{ + WriteWord(StringFormat("%08X", inIdentifier)); +} + +void ObjectStreamTextOut::WriteCount(uint32 inCount) +{ + WriteWord(std::to_string(inCount)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint8 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint16 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const int &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint32 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const uint64 &inPrimitive) +{ + WriteWord(std::to_string(inPrimitive)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const float &inPrimitive) +{ + std::ostringstream stream; + stream.precision(9); + stream << inPrimitive; + WriteWord(stream.str()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const double &inPrimitive) +{ + std::ostringstream stream; + stream.precision(17); + stream << inPrimitive; + WriteWord(stream.str()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const bool &inPrimitive) +{ + WriteWord(inPrimitive? "true" : "false"); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Float3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.x); + WriteChar(' '); + WritePrimitiveData(inPrimitive.y); + WriteChar(' '); + WritePrimitiveData(inPrimitive.z); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Double3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.x); + WriteChar(' '); + WritePrimitiveData(inPrimitive.y); + WriteChar(' '); + WritePrimitiveData(inPrimitive.z); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Vec3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const DVec3 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Vec4 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetW()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Quat &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetX()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetY()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetZ()); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetW()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const Mat44 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetColumn4(0)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(1)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(2)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(3)); +} + +void ObjectStreamTextOut::WritePrimitiveData(const DMat44 &inPrimitive) +{ + WritePrimitiveData(inPrimitive.GetColumn4(0)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(1)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetColumn4(2)); + WriteChar(' '); + WritePrimitiveData(inPrimitive.GetTranslation()); +} + +void ObjectStreamTextOut::WritePrimitiveData(const String &inPrimitive) +{ + String temporary(inPrimitive); + StringReplace(temporary, "\\", "\\\\"); + StringReplace(temporary, "\n", "\\n"); + StringReplace(temporary, "\t", "\\t"); + StringReplace(temporary, "\"", "\\\""); + WriteWord(String("\"") + temporary + String("\"")); +} + +void ObjectStreamTextOut::HintNextItem() +{ + WriteWord("\r\n"); + for (int i = 0; i < mIndentation; ++i) + WriteWord(" "); +} + +void ObjectStreamTextOut::HintIndentUp() +{ + ++mIndentation; +} + +void ObjectStreamTextOut::HintIndentDown() +{ + --mIndentation; +} + +void ObjectStreamTextOut::WriteChar(char inChar) +{ + mStream.put(inChar); +} + +void ObjectStreamTextOut::WriteWord(const string_view &inWord) +{ + mStream << inWord; +} + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.h b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.h new file mode 100644 index 000000000..5a71cfb47 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamTextOut.h @@ -0,0 +1,60 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +/// Implementation of ObjectStream text output stream. +class JPH_EXPORT ObjectStreamTextOut : public ObjectStreamOut +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor and destructor + explicit ObjectStreamTextOut(ostream &inStream); + + ///@name Output type specific operations + virtual void WriteDataType(EOSDataType inType) override; + virtual void WriteName(const char *inName) override; + virtual void WriteIdentifier(Identifier inIdentifier) override; + virtual void WriteCount(uint32 inCount) override; + + virtual void WritePrimitiveData(const uint8 &inPrimitive) override; + virtual void WritePrimitiveData(const uint16 &inPrimitive) override; + virtual void WritePrimitiveData(const int &inPrimitive) override; + virtual void WritePrimitiveData(const uint32 &inPrimitive) override; + virtual void WritePrimitiveData(const uint64 &inPrimitive) override; + virtual void WritePrimitiveData(const float &inPrimitive) override; + virtual void WritePrimitiveData(const double &inPrimitive) override; + virtual void WritePrimitiveData(const bool &inPrimitive) override; + virtual void WritePrimitiveData(const String &inPrimitive) override; + virtual void WritePrimitiveData(const Float3 &inPrimitive) override; + virtual void WritePrimitiveData(const Double3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec3 &inPrimitive) override; + virtual void WritePrimitiveData(const DVec3 &inPrimitive) override; + virtual void WritePrimitiveData(const Vec4 &inPrimitive) override; + virtual void WritePrimitiveData(const Quat &inPrimitive) override; + virtual void WritePrimitiveData(const Mat44 &inPrimitive) override; + virtual void WritePrimitiveData(const DMat44 &inPrimitive) override; + + ///@name Layout hints (for text output) + virtual void HintNextItem() override; + virtual void HintIndentUp() override; + virtual void HintIndentDown() override; + +private: + void WriteChar(char inChar); + void WriteWord(const string_view &inWord); + + int mIndentation = 0; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/ObjectStreamTypes.h b/WickedEngine/Jolt/ObjectStream/ObjectStreamTypes.h new file mode 100644 index 000000000..6fb08af17 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/ObjectStreamTypes.h @@ -0,0 +1,24 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +// Note: Order is important, an enum is created and its value is stored in a binary stream! +JPH_DECLARE_PRIMITIVE(uint8) +JPH_DECLARE_PRIMITIVE(uint16) +JPH_DECLARE_PRIMITIVE(int) +JPH_DECLARE_PRIMITIVE(uint32) +JPH_DECLARE_PRIMITIVE(uint64) +JPH_DECLARE_PRIMITIVE(float) +JPH_DECLARE_PRIMITIVE(bool) +JPH_DECLARE_PRIMITIVE(String) +JPH_DECLARE_PRIMITIVE(Float3) +JPH_DECLARE_PRIMITIVE(Vec3) +JPH_DECLARE_PRIMITIVE(Vec4) +JPH_DECLARE_PRIMITIVE(Quat) +JPH_DECLARE_PRIMITIVE(Mat44) +JPH_DECLARE_PRIMITIVE(double) +JPH_DECLARE_PRIMITIVE(DVec3) +JPH_DECLARE_PRIMITIVE(DMat44) +JPH_DECLARE_PRIMITIVE(Double3) + +#undef JPH_DECLARE_PRIMITIVE diff --git a/WickedEngine/Jolt/ObjectStream/SerializableAttribute.h b/WickedEngine/Jolt/ObjectStream/SerializableAttribute.h new file mode 100644 index 000000000..9a6d88021 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/SerializableAttribute.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +class RTTI; +class IObjectStreamIn; +class IObjectStreamOut; + +/// Data type +enum class EOSDataType +{ + /// Control codes + Declare, ///< Used to declare the attributes of a new object type + Object, ///< Start of a new object + Instance, ///< Used in attribute declaration, indicates that an object is an instanced attribute (no pointer) + Pointer, ///< Used in attribute declaration, indicates that an object is a pointer attribute + Array, ///< Used in attribute declaration, indicates that this is an array of objects + + // Basic types (primitives) + #define JPH_DECLARE_PRIMITIVE(name) T_##name, + + // This file uses the JPH_DECLARE_PRIMITIVE macro to define all types + #include + + // Error values for read functions + Invalid, ///< Next token on the stream was not a valid data type +}; + +/// Attributes are members of classes that need to be serialized. +class SerializableAttribute +{ +public: + ///@ Serialization functions + using pGetMemberPrimitiveType = const RTTI * (*)(); + using pIsType = bool (*)(int inArrayDepth, EOSDataType inDataType, const char *inClassName); + using pReadData = bool (*)(IObjectStreamIn &ioStream, void *inObject); + using pWriteData = void (*)(IObjectStreamOut &ioStream, const void *inObject); + using pWriteDataType = void (*)(IObjectStreamOut &ioStream); + + /// Constructor + SerializableAttribute(const char *inName, uint inMemberOffset, pGetMemberPrimitiveType inGetMemberPrimitiveType, pIsType inIsType, pReadData inReadData, pWriteData inWriteData, pWriteDataType inWriteDataType) : mName(inName), mMemberOffset(inMemberOffset), mGetMemberPrimitiveType(inGetMemberPrimitiveType), mIsType(inIsType), mReadData(inReadData), mWriteData(inWriteData), mWriteDataType(inWriteDataType) { } + + /// Construct from other attribute with base class offset + SerializableAttribute(const SerializableAttribute &inOther, int inBaseOffset) : mName(inOther.mName), mMemberOffset(inOther.mMemberOffset + inBaseOffset), mGetMemberPrimitiveType(inOther.mGetMemberPrimitiveType), mIsType(inOther.mIsType), mReadData(inOther.mReadData), mWriteData(inOther.mWriteData), mWriteDataType(inOther.mWriteDataType) { } + + /// Name of the attribute + void SetName(const char *inName) { mName = inName; } + const char * GetName() const { return mName; } + + /// Access to the memory location that contains the member + template + inline T * GetMemberPointer(void *inObject) const { return reinterpret_cast(reinterpret_cast(inObject) + mMemberOffset); } + template + inline const T * GetMemberPointer(const void *inObject) const { return reinterpret_cast(reinterpret_cast(inObject) + mMemberOffset); } + + /// In case this attribute contains an RTTI type, return it (note that a Array will return the rtti of sometype) + const RTTI * GetMemberPrimitiveType() const + { + return mGetMemberPrimitiveType(); + } + + /// Check if this attribute is of a specific type + bool IsType(int inArrayDepth, EOSDataType inDataType, const char *inClassName) const + { + return mIsType(inArrayDepth, inDataType, inClassName); + } + + /// Read the data for this attribute into attribute containing class inObject + bool ReadData(IObjectStreamIn &ioStream, void *inObject) const + { + return mReadData(ioStream, GetMemberPointer(inObject)); + } + + /// Write the data for this attribute from attribute containing class inObject + void WriteData(IObjectStreamOut &ioStream, const void *inObject) const + { + mWriteData(ioStream, GetMemberPointer(inObject)); + } + + /// Write the data type of this attribute to a stream + void WriteDataType(IObjectStreamOut &ioStream) const + { + mWriteDataType(ioStream); + } + +private: + // Name of the attribute + const char * mName; + + // Offset of the member relative to the class + uint mMemberOffset; + + // In case this attribute contains an RTTI type, return it (note that a Array will return the rtti of sometype) + pGetMemberPrimitiveType mGetMemberPrimitiveType; + + // Serialization operations + pIsType mIsType; + pReadData mReadData; + pWriteData mWriteData; + pWriteDataType mWriteDataType; +}; + +JPH_NAMESPACE_END + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/SerializableAttributeEnum.h b/WickedEngine/Jolt/ObjectStream/SerializableAttributeEnum.h new file mode 100644 index 000000000..b506ff981 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/SerializableAttributeEnum.h @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to add properties to be serialized +////////////////////////////////////////////////////////////////////////////////////////// + +template +inline void AddSerializableAttributeEnum(RTTI &inRTTI, uint inOffset, const char *inName) +{ + inRTTI.AddAttribute(SerializableAttribute(inName, inOffset, + []() -> const RTTI * + { + return nullptr; + }, + [](int inArrayDepth, EOSDataType inDataType, [[maybe_unused]] const char *inClassName) + { + return inArrayDepth == 0 && inDataType == EOSDataType::T_uint32; + }, + [](IObjectStreamIn &ioStream, void *inObject) + { + uint32 temporary; + if (OSReadData(ioStream, temporary)) + { + *reinterpret_cast(inObject) = static_cast(temporary); + return true; + } + return false; + }, + [](IObjectStreamOut &ioStream, const void *inObject) + { + static_assert(sizeof(MemberType) <= sizeof(uint32)); + uint32 temporary = uint32(*reinterpret_cast(inObject)); + OSWriteData(ioStream, temporary); + }, + [](IObjectStreamOut &ioStream) + { + ioStream.WriteDataType(EOSDataType::T_uint32); + })); +} + +// JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS +#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \ + AddSerializableAttributeEnum(inRTTI, offsetof(class_name, member_name), alias_name); + +// JPH_ADD_ENUM_ATTRIBUTE +#define JPH_ADD_ENUM_ATTRIBUTE(class_name, member_name) \ + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name); + +JPH_NAMESPACE_END + +#else + +#define JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(...) +#define JPH_ADD_ENUM_ATTRIBUTE(...) + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/SerializableAttributeTyped.h b/WickedEngine/Jolt/ObjectStream/SerializableAttributeTyped.h new file mode 100644 index 000000000..296495eeb --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/SerializableAttributeTyped.h @@ -0,0 +1,60 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +#ifdef JPH_OBJECT_STREAM + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Macros to add properties to be serialized +////////////////////////////////////////////////////////////////////////////////////////// + +template +inline void AddSerializableAttributeTyped(RTTI &inRTTI, uint inOffset, const char *inName) +{ + inRTTI.AddAttribute(SerializableAttribute(inName, inOffset, + []() + { + return GetPrimitiveTypeOfType((MemberType *)nullptr); + }, + [](int inArrayDepth, EOSDataType inDataType, const char *inClassName) + { + return OSIsType((MemberType *)nullptr, inArrayDepth, inDataType, inClassName); + }, + [](IObjectStreamIn &ioStream, void *inObject) + { + return OSReadData(ioStream, *reinterpret_cast(inObject)); + }, + [](IObjectStreamOut &ioStream, const void *inObject) + { + OSWriteData(ioStream, *reinterpret_cast(inObject)); + }, + [](IObjectStreamOut &ioStream) + { + OSWriteDataType(ioStream, (MemberType *)nullptr); + })); +} + +// JPH_ADD_ATTRIBUTE +#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, alias_name) \ + AddSerializableAttributeTyped(inRTTI, offsetof(class_name, member_name), alias_name); + +// JPH_ADD_ATTRIBUTE +#define JPH_ADD_ATTRIBUTE(class_name, member_name) \ + JPH_ADD_ATTRIBUTE_WITH_ALIAS(class_name, member_name, #member_name) + +JPH_NAMESPACE_END + +#else + +#define JPH_ADD_ATTRIBUTE_WITH_ALIAS(...) +#define JPH_ADD_ATTRIBUTE(...) + +#endif // JPH_OBJECT_STREAM diff --git a/WickedEngine/Jolt/ObjectStream/SerializableObject.cpp b/WickedEngine/Jolt/ObjectStream/SerializableObject.cpp new file mode 100644 index 000000000..98d3b3cf8 --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/SerializableObject.cpp @@ -0,0 +1,15 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(SerializableObject) +{ +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/ObjectStream/SerializableObject.h b/WickedEngine/Jolt/ObjectStream/SerializableObject.h new file mode 100644 index 000000000..8428c5dae --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/SerializableObject.h @@ -0,0 +1,164 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +////////////////////////////////////////////////////////////////////////////////////////// +// Helper macros +////////////////////////////////////////////////////////////////////////////////////////// + +#ifdef JPH_OBJECT_STREAM + +// JPH_DECLARE_SERIALIZATION_FUNCTIONS +#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, prefix, class_name) \ + linkage prefix bool OSReadData(IObjectStreamIn &ioStream, class_name &inInstance); \ + linkage prefix bool OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer); \ + linkage prefix bool OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + linkage prefix bool OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName); \ + linkage prefix void OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance); \ + linkage prefix void OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer); \ + linkage prefix void OSWriteDataType(IObjectStreamOut &ioStream, class_name *); \ + linkage prefix void OSWriteDataType(IObjectStreamOut &ioStream, class_name **); + +// JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS +#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + bool OSReadData(IObjectStreamIn &ioStream, class_name &inInstance) \ + { \ + return ioStream.ReadClassData(#class_name, (void *)&inInstance); \ + } \ + bool OSReadData(IObjectStreamIn &ioStream, class_name *&inPointer) \ + { \ + return ioStream.ReadPointerData(JPH_RTTI(class_name), (void **)&inPointer); \ + } \ + bool OSIsType(class_name *, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::Instance && strcmp(inClassName, #class_name) == 0; \ + } \ + bool OSIsType(class_name **, int inArrayDepth, EOSDataType inDataType, const char *inClassName) \ + { \ + return inArrayDepth == 0 && inDataType == EOSDataType::Pointer && strcmp(inClassName, #class_name) == 0; \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, const class_name &inInstance) \ + { \ + ioStream.WriteClassData(JPH_RTTI(class_name), (void *)&inInstance); \ + } \ + void OSWriteData(IObjectStreamOut &ioStream, class_name *const &inPointer) \ + { \ + if (inPointer) \ + ioStream.WritePointerData(GetRTTI(inPointer), (void *)inPointer); \ + else \ + ioStream.WritePointerData(nullptr, nullptr); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, class_name *) \ + { \ + ioStream.WriteDataType(EOSDataType::Instance); \ + ioStream.WriteName(#class_name); \ + } \ + void OSWriteDataType(IObjectStreamOut &ioStream, class_name **) \ + { \ + ioStream.WriteDataType(EOSDataType::Pointer); \ + ioStream.WriteName(#class_name); \ + } + +#else + +#define JPH_DECLARE_SERIALIZATION_FUNCTIONS(...) +#define JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(...) + +#endif // JPH_OBJECT_STREAM + +////////////////////////////////////////////////////////////////////////////////////////// +// Use these macros on non-virtual objects to make them serializable +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL +#define JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_NON_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL +#define JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_NON_VIRTUAL(class_name) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but when you cannot insert the declaration in the class itself +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS +#define JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(linkage, class_name) \ + JPH_DECLARE_RTTI_OUTSIDE_CLASS(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, extern, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS +#define JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(class_name) \ + +////////////////////////////////////////////////////////////////////////////////////////// +// Same as above, but for classes that have virtual functions +////////////////////////////////////////////////////////////////////////////////////////// + +// JPH_DECLARE_SERIALIZABLE_VIRTUAL - Use for concrete, non-base classes +#define JPH_DECLARE_SERIALIZABLE_VIRTUAL(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_VIRTUAL(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL +#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_ABSTRACT - Use for abstract, non-base classes +#define JPH_DECLARE_SERIALIZABLE_ABSTRACT(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_ABSTRACT(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT +#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE - Use for concrete base classes +#define JPH_DECLARE_SERIALIZABLE_VIRTUAL_BASE(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_VIRTUAL_BASE(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE +#define JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL_BASE(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_VIRTUAL_BASE(class_name) \ + +// JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE - Use for abstract base class +#define JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(linkage, class_name) \ +public: \ + JPH_DECLARE_RTTI_ABSTRACT_BASE(linkage, class_name) \ + JPH_DECLARE_SERIALIZATION_FUNCTIONS(linkage, friend, class_name) \ + +// JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE +#define JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(class_name) \ + JPH_IMPLEMENT_SERIALIZATION_FUNCTIONS(class_name) \ + JPH_IMPLEMENT_RTTI_ABSTRACT_BASE(class_name) + +/// Classes must be derived from SerializableObject if you want to be able to save pointers or +/// reference counting pointers to objects of this or derived classes. The type will automatically +/// be determined during serialization and upon deserialization it will be restored correctly. +class JPH_EXPORT SerializableObject : public NonCopyable +{ + JPH_DECLARE_SERIALIZABLE_ABSTRACT_BASE(JPH_EXPORT, SerializableObject) + +public: + /// Constructor + virtual ~SerializableObject() = default; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/ObjectStream/TypeDeclarations.cpp b/WickedEngine/Jolt/ObjectStream/TypeDeclarations.cpp new file mode 100644 index 000000000..a1d5ac6ac --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/TypeDeclarations.cpp @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint8) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint16) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(int) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint32) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(uint64) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(float) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(double) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(bool) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(String) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Float3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Double3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Vec3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(DVec3) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Vec4) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Quat) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(Mat44) { } +JPH_IMPLEMENT_RTTI_OUTSIDE_CLASS(DMat44) { } + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(Color) +{ + JPH_ADD_ATTRIBUTE(Color, r) + JPH_ADD_ATTRIBUTE(Color, g) + JPH_ADD_ATTRIBUTE(Color, b) + JPH_ADD_ATTRIBUTE(Color, a) +} + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(AABox) +{ + JPH_ADD_ATTRIBUTE(AABox, mMin) + JPH_ADD_ATTRIBUTE(AABox, mMax) +} + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(Triangle) +{ + JPH_ADD_ATTRIBUTE(Triangle, mV) + JPH_ADD_ATTRIBUTE(Triangle, mMaterialIndex) +} + +JPH_IMPLEMENT_SERIALIZABLE_OUTSIDE_CLASS(IndexedTriangle) +{ + JPH_ADD_ATTRIBUTE(IndexedTriangle, mIdx) + JPH_ADD_ATTRIBUTE(IndexedTriangle, mMaterialIndex) +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/ObjectStream/TypeDeclarations.h b/WickedEngine/Jolt/ObjectStream/TypeDeclarations.h new file mode 100644 index 000000000..2a61c306b --- /dev/null +++ b/WickedEngine/Jolt/ObjectStream/TypeDeclarations.h @@ -0,0 +1,41 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint8); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint16); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, int); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint32); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, uint64); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, float); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, double); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, bool); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, String); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Float3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Double3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DVec3); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Vec4); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Quat); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, Mat44); +JPH_DECLARE_RTTI_OUTSIDE_CLASS(JPH_EXPORT, DMat44); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Color); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, AABox); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, Triangle); +JPH_DECLARE_SERIALIZABLE_OUTSIDE_CLASS(JPH_EXPORT, IndexedTriangle); + +JPH_NAMESPACE_END + +// These need to be added after all types have been registered or else clang under linux will not find GetRTTIOfType for the type +#include +#include diff --git a/WickedEngine/Jolt/Physics/Body/AllowedDOFs.h b/WickedEngine/Jolt/Physics/Body/AllowedDOFs.h new file mode 100644 index 000000000..8445cb186 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/AllowedDOFs.h @@ -0,0 +1,68 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used in BodyCreationSettings and MotionProperties to indicate which degrees of freedom a body has +enum class EAllowedDOFs : uint8 +{ + None = 0b000000, ///< No degrees of freedom are allowed. Note that this is not valid and will crash. Use a static body instead. + All = 0b111111, ///< All degrees of freedom are allowed + TranslationX = 0b000001, ///< Body can move in world space X axis + TranslationY = 0b000010, ///< Body can move in world space Y axis + TranslationZ = 0b000100, ///< Body can move in world space Z axis + RotationX = 0b001000, ///< Body can rotate around world space X axis + RotationY = 0b010000, ///< Body can rotate around world space Y axis + RotationZ = 0b100000, ///< Body can rotate around world space Z axis + Plane2D = TranslationX | TranslationY | RotationZ, ///< Body can only move in X and Y axis and rotate around Z axis +}; + +/// Bitwise OR operator for EAllowedDOFs +constexpr EAllowedDOFs operator | (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) | uint8(inRHS)); +} + +/// Bitwise AND operator for EAllowedDOFs +constexpr EAllowedDOFs operator & (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) & uint8(inRHS)); +} + +/// Bitwise XOR operator for EAllowedDOFs +constexpr EAllowedDOFs operator ^ (EAllowedDOFs inLHS, EAllowedDOFs inRHS) +{ + return EAllowedDOFs(uint8(inLHS) ^ uint8(inRHS)); +} + +/// Bitwise NOT operator for EAllowedDOFs +constexpr EAllowedDOFs operator ~ (EAllowedDOFs inAllowedDOFs) +{ + return EAllowedDOFs(~uint8(inAllowedDOFs)); +} + +/// Bitwise OR assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator |= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS | inRHS; + return ioLHS; +} + +/// Bitwise AND assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator &= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS & inRHS; + return ioLHS; +} + +/// Bitwise XOR assignment operator for EAllowedDOFs +constexpr EAllowedDOFs & operator ^= (EAllowedDOFs &ioLHS, EAllowedDOFs inRHS) +{ + ioLHS = ioLHS ^ inRHS; + return ioLHS; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/Body.cpp b/WickedEngine/Jolt/Physics/Body/Body.cpp new file mode 100644 index 000000000..085ae427c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/Body.cpp @@ -0,0 +1,413 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +static const SphereShape sFixedToWorldShape(FLT_EPSILON); +Body Body::sFixedToWorld(false); + +Body::Body(bool) : + mPosition(Vec3::sZero()), + mRotation(Quat::sIdentity()), + mShape(&sFixedToWorldShape), // Dummy shape + mFriction(0.0f), + mRestitution(0.0f), + mObjectLayer(cObjectLayerInvalid), + mMotionType(EMotionType::Static) +{ + sFixedToWorldShape.SetEmbedded(); +} + +void Body::SetMotionType(EMotionType inMotionType) +{ + if (mMotionType == inMotionType) + return; + + JPH_ASSERT(inMotionType == EMotionType::Static || mMotionProperties != nullptr, "Body needs to be created with mAllowDynamicOrKinematic set tot true"); + JPH_ASSERT(inMotionType != EMotionType::Static || !IsActive(), "Deactivate body first"); + JPH_ASSERT(inMotionType == EMotionType::Dynamic || !IsSoftBody(), "Soft bodies can only be dynamic, you can make individual vertices kinematic by setting their inverse mass to 0"); + + // Store new motion type + mMotionType = inMotionType; + + if (mMotionProperties != nullptr) + { + // Update cache + JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = inMotionType;) + + switch (inMotionType) + { + case EMotionType::Static: + // Stop the object + mMotionProperties->mLinearVelocity = Vec3::sZero(); + mMotionProperties->mAngularVelocity = Vec3::sZero(); + [[fallthrough]]; + + case EMotionType::Kinematic: + // Cancel forces + mMotionProperties->ResetForce(); + mMotionProperties->ResetTorque(); + break; + + case EMotionType::Dynamic: + break; + } + } +} + +void Body::SetAllowSleeping(bool inAllow) +{ + mMotionProperties->mAllowSleeping = inAllow; + if (inAllow) + ResetSleepTimer(); +} + +void Body::MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies + JPH_ASSERT(!IsStatic()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); + + // Calculate center of mass at end situation + RVec3 new_com = inTargetPosition + inTargetRotation * mShape->GetCenterOfMass(); + + // Calculate delta position and rotation + Vec3 delta_pos = Vec3(new_com - mPosition); + Quat delta_rotation = inTargetRotation * mRotation.Conjugated(); + + mMotionProperties->MoveKinematic(delta_pos, delta_rotation, inDeltaTime); +} + +void Body::CalculateWorldSpaceBoundsInternal() +{ + mBounds = mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); +} + +void Body::SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); + + mPosition = inPosition + inRotation * mShape->GetCenterOfMass(); + mRotation = inRotation; + + // Initialize bounding box + CalculateWorldSpaceBoundsInternal(); + + // Reset sleeping test + if (inResetSleepTimer && mMotionProperties != nullptr) + ResetSleepTimer(); +} + +void Body::UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties) +{ + // Update center of mass position so the world position for this body stays the same + mPosition += mRotation * (mShape->GetCenterOfMass() - inPreviousCenterOfMass); + + // Recalculate mass and inertia if requested + if (inUpdateMassProperties && mMotionProperties != nullptr) + mMotionProperties->SetMassProperties(mMotionProperties->GetAllowedDOFs(), mShape->GetMassProperties()); +} + +void Body::SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties) +{ + JPH_ASSERT(IsRigidBody()); // Only valid for rigid bodies + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); + + // Get the old center of mass + Vec3 old_com = mShape->GetCenterOfMass(); + + // Update the shape + mShape = inShape; + + // Update center of mass + UpdateCenterOfMassInternal(old_com, inUpdateMassProperties); + + // Recalculate bounding box + CalculateWorldSpaceBoundsInternal(); +} + +ECanSleep Body::UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep) +{ + // Check override & sensors will never go to sleep (they would stop detecting collisions with sleeping bodies) + if (!mMotionProperties->mAllowSleeping || IsSensor()) + return ECanSleep::CannotSleep; + + // Get the points to test + RVec3 points[3]; + GetSleepTestPoints(points); + +#ifdef JPH_DOUBLE_PRECISION + // Get base offset for spheres + DVec3 offset = mMotionProperties->GetSleepTestOffset(); +#endif // JPH_DOUBLE_PRECISION + + for (int i = 0; i < 3; ++i) + { + Sphere &sphere = mMotionProperties->mSleepTestSpheres[i]; + + // Make point relative to base offset +#ifdef JPH_DOUBLE_PRECISION + Vec3 p = Vec3(points[i] - offset); +#else + Vec3 p = points[i]; +#endif // JPH_DOUBLE_PRECISION + + // Encapsulate the point in a sphere + sphere.EncapsulatePoint(p); + + // Test if it exceeded the max movement + if (sphere.GetRadius() > inMaxMovement) + { + // Body is not sleeping, reset test + mMotionProperties->ResetSleepTestSpheres(points); + return ECanSleep::CannotSleep; + } + } + + return mMotionProperties->AccumulateSleepTime(inDeltaTime, inTimeBeforeSleep); +} + +bool Body::ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(IsRigidBody()); // Only implemented for rigid bodies currently + + // We follow the approach from 'Game Programming Gems 6' 2.5 Exact Buoyancy for Polyhedra + // All quantities below are in world space + + // For GetSubmergedVolume we transform the surface relative to the body position for increased precision + Mat44 rotation = Mat44::sRotation(mRotation); + Plane surface_relative_to_body = Plane::sFromPointAndNormal(inSurfacePosition - mPosition, inSurfaceNormal); + + // Calculate amount of volume that is submerged and what the center of buoyancy is + float total_volume, submerged_volume; + Vec3 relative_center_of_buoyancy; + mShape->GetSubmergedVolume(rotation, Vec3::sReplicate(1.0f), surface_relative_to_body, total_volume, submerged_volume, relative_center_of_buoyancy JPH_IF_DEBUG_RENDERER(, mPosition)); + + // If we're not submerged, there's no point in doing the rest of the calculations + if (submerged_volume > 0.0f) + { + #ifdef JPH_DEBUG_RENDERER + // Draw submerged volume properties + if (Shape::sDrawSubmergedVolumes) + { + RVec3 center_of_buoyancy = mPosition + relative_center_of_buoyancy; + DebugRenderer::sInstance->DrawMarker(center_of_buoyancy, Color::sWhite, 2.0f); + DebugRenderer::sInstance->DrawText3D(center_of_buoyancy, StringFormat("%.3f / %.3f", (double)submerged_volume, (double)total_volume)); + } + #endif // JPH_DEBUG_RENDERER + + // When buoyancy is 1 we want neutral buoyancy, this means that the density of the liquid is the same as the density of the body at that point. + // Buoyancy > 1 should make the object float, < 1 should make it sink. + float inverse_mass = mMotionProperties->GetInverseMass(); + float fluid_density = inBuoyancy / (total_volume * inverse_mass); + + // Buoyancy force = Density of Fluid * Submerged volume * Magnitude of gravity * Up direction (eq 2.5.1) + // Impulse = Force * Delta time + // We should apply this at the center of buoyancy (= center of mass of submerged volume) + Vec3 buoyancy_impulse = -fluid_density * submerged_volume * mMotionProperties->GetGravityFactor() * inGravity * inDeltaTime; + + // Calculate the velocity of the center of buoyancy relative to the fluid + Vec3 linear_velocity = mMotionProperties->GetLinearVelocity(); + Vec3 angular_velocity = mMotionProperties->GetAngularVelocity(); + Vec3 center_of_buoyancy_velocity = linear_velocity + angular_velocity.Cross(relative_center_of_buoyancy); + Vec3 relative_center_of_buoyancy_velocity = inFluidVelocity - center_of_buoyancy_velocity; + + // Here we deviate from the article, instead of eq 2.5.14 we use a quadratic drag formula: https://en.wikipedia.org/wiki/Drag_%28physics%29 + // Drag force = 0.5 * Fluid Density * (Velocity of fluid - Velocity of center of buoyancy)^2 * Linear Drag * Area Facing the Relative Fluid Velocity + // Again Impulse = Force * Delta Time + // We should apply this at the center of buoyancy (= center of mass for submerged volume with no center of mass offset) + + // Get size of local bounding box + Vec3 size = mShape->GetLocalBounds().GetSize(); + + // Determine area of the local space bounding box in the direction of the relative velocity between the fluid and the center of buoyancy + float area = 0.0f; + float relative_center_of_buoyancy_velocity_len_sq = relative_center_of_buoyancy_velocity.LengthSq(); + if (relative_center_of_buoyancy_velocity_len_sq > 1.0e-12f) + { + Vec3 local_relative_center_of_buoyancy_velocity = GetRotation().Conjugated() * relative_center_of_buoyancy_velocity; + area = local_relative_center_of_buoyancy_velocity.Abs().Dot(size.Swizzle() * size.Swizzle()) / sqrt(relative_center_of_buoyancy_velocity_len_sq); + } + + // Calculate the impulse + Vec3 drag_impulse = (0.5f * fluid_density * inLinearDrag * area * inDeltaTime) * relative_center_of_buoyancy_velocity * relative_center_of_buoyancy_velocity.Length(); + + // Clamp magnitude against current linear velocity to prevent overshoot + float linear_velocity_len_sq = linear_velocity.LengthSq(); + float drag_delta_linear_velocity_len_sq = (drag_impulse * inverse_mass).LengthSq(); + if (drag_delta_linear_velocity_len_sq > linear_velocity_len_sq) + drag_impulse *= sqrt(linear_velocity_len_sq / drag_delta_linear_velocity_len_sq); + + // Calculate the resulting delta linear velocity due to buoyancy and drag + Vec3 delta_linear_velocity = (drag_impulse + buoyancy_impulse) * inverse_mass; + mMotionProperties->AddLinearVelocityStep(delta_linear_velocity); + + // Determine average width of the body (across the three axis) + float l = (size.GetX() + size.GetY() + size.GetZ()) / 3.0f; + + // Drag torque = -Angular Drag * Mass * Submerged volume / Total volume * (Average width of body)^2 * Angular velocity (eq 2.5.15) + Vec3 drag_angular_impulse = (-inAngularDrag * submerged_volume / total_volume * inDeltaTime * Square(l) / inverse_mass) * angular_velocity; + Mat44 inv_inertia = GetInverseInertia(); + Vec3 drag_delta_angular_velocity = inv_inertia * drag_angular_impulse; + + // Clamp magnitude against the current angular velocity to prevent overshoot + float angular_velocity_len_sq = angular_velocity.LengthSq(); + float drag_delta_angular_velocity_len_sq = drag_delta_angular_velocity.LengthSq(); + if (drag_delta_angular_velocity_len_sq > angular_velocity_len_sq) + drag_delta_angular_velocity *= sqrt(angular_velocity_len_sq / drag_delta_angular_velocity_len_sq); + + // Calculate total delta angular velocity due to drag and buoyancy + Vec3 delta_angular_velocity = drag_delta_angular_velocity + inv_inertia * relative_center_of_buoyancy.Cross(buoyancy_impulse + drag_impulse); + mMotionProperties->AddAngularVelocityStep(delta_angular_velocity); + return true; + } + + return false; +} + +void Body::SaveState(StateRecorder &inStream) const +{ + // Only write properties that can change at runtime + inStream.Write(mPosition); + inStream.Write(mRotation); + + if (mMotionProperties != nullptr) + { + if (IsSoftBody()) + static_cast(mMotionProperties)->SaveState(inStream); + else + mMotionProperties->SaveState(inStream); + } +} + +void Body::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + + if (mMotionProperties != nullptr) + { + if (IsSoftBody()) + static_cast(mMotionProperties)->RestoreState(inStream); + else + mMotionProperties->RestoreState(inStream); + + JPH_IF_ENABLE_ASSERTS(mMotionProperties->mCachedMotionType = mMotionType); + } + + // Initialize bounding box + CalculateWorldSpaceBoundsInternal(); +} + +BodyCreationSettings Body::GetBodyCreationSettings() const +{ + JPH_ASSERT(IsRigidBody()); + + BodyCreationSettings result; + + result.mPosition = GetPosition(); + result.mRotation = GetRotation(); + result.mLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); + result.mAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); + result.mObjectLayer = GetObjectLayer(); + result.mUserData = mUserData; + result.mCollisionGroup = GetCollisionGroup(); + result.mMotionType = GetMotionType(); + result.mAllowedDOFs = mMotionProperties != nullptr? mMotionProperties->GetAllowedDOFs() : EAllowedDOFs::All; + result.mAllowDynamicOrKinematic = mMotionProperties != nullptr; + result.mIsSensor = IsSensor(); + result.mCollideKinematicVsNonDynamic = GetCollideKinematicVsNonDynamic(); + result.mUseManifoldReduction = GetUseManifoldReduction(); + result.mApplyGyroscopicForce = GetApplyGyroscopicForce(); + result.mMotionQuality = mMotionProperties != nullptr? mMotionProperties->GetMotionQuality() : EMotionQuality::Discrete; + result.mEnhancedInternalEdgeRemoval = GetEnhancedInternalEdgeRemoval(); + result.mAllowSleeping = mMotionProperties != nullptr? GetAllowSleeping() : true; + result.mFriction = GetFriction(); + result.mRestitution = GetRestitution(); + result.mLinearDamping = mMotionProperties != nullptr? mMotionProperties->GetLinearDamping() : 0.0f; + result.mAngularDamping = mMotionProperties != nullptr? mMotionProperties->GetAngularDamping() : 0.0f; + result.mMaxLinearVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxLinearVelocity() : 0.0f; + result.mMaxAngularVelocity = mMotionProperties != nullptr? mMotionProperties->GetMaxAngularVelocity() : 0.0f; + result.mGravityFactor = mMotionProperties != nullptr? mMotionProperties->GetGravityFactor() : 1.0f; + result.mNumVelocityStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumVelocityStepsOverride() : 0; + result.mNumPositionStepsOverride = mMotionProperties != nullptr? mMotionProperties->GetNumPositionStepsOverride() : 0; + result.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + + // Invert inertia and mass + if (mMotionProperties != nullptr) + { + float inv_mass = mMotionProperties->GetInverseMassUnchecked(); + Mat44 inv_inertia = mMotionProperties->GetLocalSpaceInverseInertiaUnchecked(); + + // Get mass + result.mMassPropertiesOverride.mMass = inv_mass != 0.0f? 1.0f / inv_mass : FLT_MAX; + + // Get inertia + Mat44 inertia; + if (inertia.SetInversed3x3(inv_inertia)) + { + // Inertia was invertible, we can use it + result.mMassPropertiesOverride.mInertia = inertia; + } + else + { + // Prevent division by zero + Vec3 diagonal = Vec3::sMax(inv_inertia.GetDiagonal3(), Vec3::sReplicate(FLT_MIN)); + result.mMassPropertiesOverride.mInertia = Mat44::sScale(diagonal.Reciprocal()); + } + } + else + { + result.mMassPropertiesOverride.mMass = FLT_MAX; + result.mMassPropertiesOverride.mInertia = Mat44::sScale(Vec3::sReplicate(FLT_MAX)); + } + + result.SetShape(GetShape()); + + return result; +} + +SoftBodyCreationSettings Body::GetSoftBodyCreationSettings() const +{ + JPH_ASSERT(IsSoftBody()); + + SoftBodyCreationSettings result; + + result.mPosition = GetPosition(); + result.mRotation = GetRotation(); + result.mUserData = mUserData; + result.mObjectLayer = GetObjectLayer(); + result.mCollisionGroup = GetCollisionGroup(); + result.mFriction = GetFriction(); + result.mRestitution = GetRestitution(); + const SoftBodyMotionProperties *mp = static_cast(mMotionProperties); + result.mNumIterations = mp->GetNumIterations(); + result.mLinearDamping = mp->GetLinearDamping(); + result.mMaxLinearVelocity = mp->GetMaxLinearVelocity(); + result.mGravityFactor = mp->GetGravityFactor(); + result.mPressure = mp->GetPressure(); + result.mUpdatePosition = mp->GetUpdatePosition(); + result.mSettings = mp->GetSettings(); + + return result; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/Body.h b/WickedEngine/Jolt/Physics/Body/Body.h new file mode 100644 index 000000000..509f55f66 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/Body.h @@ -0,0 +1,388 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StateRecorder; +class BodyCreationSettings; +class SoftBodyCreationSettings; + +/// A rigid body that can be simulated using the physics system +/// +/// Note that internally all properties (position, velocity etc.) are tracked relative to the center of mass of the object to simplify the simulation of the object. +/// +/// The offset between the position of the body and the center of mass position of the body is GetShape()->GetCenterOfMass(). +/// The functions that get/set the position of the body all indicate if they are relative to the center of mass or to the original position in which the shape was created. +/// +/// The linear velocity is also velocity of the center of mass, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$. +class alignas(JPH_RVECTOR_ALIGNMENT) JPH_EXPORT_GCC_BUG_WORKAROUND Body : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor + Body() = default; + + /// Destructor + ~Body() { JPH_ASSERT(mMotionProperties == nullptr); } + + /// Get the id of this body + inline const BodyID & GetID() const { return mID; } + + /// Get the type of body (rigid or soft) + inline EBodyType GetBodyType() const { return mBodyType; } + + /// Check if this body is a rigid body + inline bool IsRigidBody() const { return mBodyType == EBodyType::RigidBody; } + + /// Check if this body is a soft body + inline bool IsSoftBody() const { return mBodyType == EBodyType::SoftBody; } + + /// If this body is currently actively simulating (true) or sleeping (false) + inline bool IsActive() const { return mMotionProperties != nullptr && mMotionProperties->mIndexInActiveBodies != cInactiveIndex; } + + /// Check if this body is static (not movable) + inline bool IsStatic() const { return mMotionType == EMotionType::Static; } + + /// Check if this body is kinematic (keyframed), which means that it will move according to its current velocity, but forces don't affect it + inline bool IsKinematic() const { return mMotionType == EMotionType::Kinematic; } + + /// Check if this body is dynamic, which means that it moves and forces can act on it + inline bool IsDynamic() const { return mMotionType == EMotionType::Dynamic; } + + /// Check if a body could be made kinematic or dynamic (if it was created dynamic or with mAllowDynamicOrKinematic set to true) + inline bool CanBeKinematicOrDynamic() const { return mMotionProperties != nullptr; } + + /// Change the body to a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. + /// The cheapest sensor (in terms of CPU usage) is a sensor with motion type Static (they can be moved around using BodyInterface::SetPosition/SetPositionAndRotation). + /// These sensors will only detect collisions with active Dynamic or Kinematic bodies. As soon as a body go to sleep, the contact point with the sensor will be lost. + /// If you make a sensor Dynamic or Kinematic and activate them, the sensor will be able to detect collisions with sleeping bodies too. An active sensor will never go to sleep automatically. + /// When you make a Dynamic or Kinematic sensor, make sure it is in an ObjectLayer that does not collide with Static bodies or other sensors to avoid extra overhead in the broad phase. + inline void SetIsSensor(bool inIsSensor) { JPH_ASSERT(IsRigidBody()); if (inIsSensor) mFlags.fetch_or(uint8(EFlags::IsSensor), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsSensor)), memory_order_relaxed); } + + /// Check if this body is a sensor. + inline bool IsSensor() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsSensor)) != 0; } + + /// If kinematic objects can generate contact points against other kinematic or static objects. + /// Note that turning this on can be CPU intensive as much more collision detection work will be done without any effect on the simulation (kinematic objects are not affected by other kinematic/static objects). + /// This can be used to make sensors detect static objects. Note that the sensor must be kinematic and active for it to detect static objects. + inline void SetCollideKinematicVsNonDynamic(bool inCollide) { JPH_ASSERT(IsRigidBody()); if (inCollide) mFlags.fetch_or(uint8(EFlags::CollideKinematicVsNonDynamic), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::CollideKinematicVsNonDynamic)), memory_order_relaxed); } + + /// Check if kinematic objects can generate contact points against other kinematic or static objects. + inline bool GetCollideKinematicVsNonDynamic() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::CollideKinematicVsNonDynamic)) != 0; } + + /// If PhysicsSettings::mUseManifoldReduction is true, this allows turning off manifold reduction for this specific body. + /// Manifold reduction by default will combine contacts with similar normals that come from different SubShapeIDs (e.g. different triangles in a mesh shape or different compound shapes). + /// If the application requires tracking exactly which SubShapeIDs are in contact, you can turn off manifold reduction. Note that this comes at a performance cost. + /// Consider using BodyInterface::SetUseManifoldReduction if the body could already be in contact with other bodies to ensure that the contact cache is invalidated and you get the correct contact callbacks. + inline void SetUseManifoldReduction(bool inUseReduction) { JPH_ASSERT(IsRigidBody()); if (inUseReduction) mFlags.fetch_or(uint8(EFlags::UseManifoldReduction), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::UseManifoldReduction)), memory_order_relaxed); } + + /// Check if this body can use manifold reduction. + inline bool GetUseManifoldReduction() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::UseManifoldReduction)) != 0; } + + /// Checks if the combination of this body and inBody2 should use manifold reduction + inline bool GetUseManifoldReductionWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) & inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::UseManifoldReduction)) != 0; } + + /// Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + inline void SetApplyGyroscopicForce(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::ApplyGyroscopicForce), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::ApplyGyroscopicForce)), memory_order_relaxed); } + + /// Check if the gyroscopic force is being applied for this body + inline bool GetApplyGyroscopicForce() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::ApplyGyroscopicForce)) != 0; } + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + inline void SetEnhancedInternalEdgeRemoval(bool inApply) { JPH_ASSERT(IsRigidBody()); if (inApply) mFlags.fetch_or(uint8(EFlags::EnhancedInternalEdgeRemoval), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::EnhancedInternalEdgeRemoval)), memory_order_relaxed); } + + /// Check if enhanced internal edge removal is turned on + inline bool GetEnhancedInternalEdgeRemoval() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; } + + /// Checks if the combination of this body and inBody2 should use enhanced internal edge removal + inline bool GetEnhancedInternalEdgeRemovalWithBody(const Body &inBody2) const { return ((mFlags.load(memory_order_relaxed) | inBody2.mFlags.load(memory_order_relaxed)) & uint8(EFlags::EnhancedInternalEdgeRemoval)) != 0; } + + /// Get the bodies motion type. + inline EMotionType GetMotionType() const { return mMotionType; } + + /// Set the motion type of this body. Consider using BodyInterface::SetMotionType instead of this function if the body may be active or if it needs to be activated. + void SetMotionType(EMotionType inMotionType); + + /// Get broadphase layer, this determines in which broad phase sub-tree the object is placed + inline BroadPhaseLayer GetBroadPhaseLayer() const { return mBroadPhaseLayer; } + + /// Get object layer, this determines which other objects it collides with + inline ObjectLayer GetObjectLayer() const { return mObjectLayer; } + + /// Collision group and sub-group ID, determines which other objects it collides with + const CollisionGroup & GetCollisionGroup() const { return mCollisionGroup; } + CollisionGroup & GetCollisionGroup() { return mCollisionGroup; } + void SetCollisionGroup(const CollisionGroup &inGroup) { mCollisionGroup = inGroup; } + + /// If this body can go to sleep. Note that disabling sleeping on a sleeping object will not wake it up. + bool GetAllowSleeping() const { return mMotionProperties->mAllowSleeping; } + void SetAllowSleeping(bool inAllow); + + /// Resets the sleep timer. This does not wake up the body if it is sleeping, but allows resetting the system that detects when a body is sleeping. + inline void ResetSleepTimer(); + + /// Friction (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + inline float GetFriction() const { return mFriction; } + void SetFriction(float inFriction) { mFriction = inFriction; } + + /// Restitution (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + inline float GetRestitution() const { return mRestitution; } + void SetRestitution(float inRestitution) { mRestitution = inRestitution; } + + /// Get world space linear velocity of the center of mass (unit: m/s) + inline Vec3 GetLinearVelocity() const { return !IsStatic()? mMotionProperties->GetLinearVelocity() : Vec3::sZero(); } + + /// Set world space linear velocity of the center of mass (unit: m/s) + void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocity(inLinearVelocity); } + + /// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity + void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetLinearVelocityClamped(inLinearVelocity); } + + /// Get world space angular velocity of the center of mass (unit: rad/s) + inline Vec3 GetAngularVelocity() const { return !IsStatic()? mMotionProperties->GetAngularVelocity() : Vec3::sZero(); } + + /// Set world space angular velocity of the center of mass (unit: rad/s) + void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocity(inAngularVelocity); } + + /// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity + void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { JPH_ASSERT(!IsStatic()); mMotionProperties->SetAngularVelocityClamped(inAngularVelocity); } + + /// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s) + inline Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return !IsStatic()? mMotionProperties->GetPointVelocityCOM(inPointRelativeToCOM) : Vec3::sZero(); } + + /// Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body (unit: m/s) + inline Vec3 GetPointVelocity(RVec3Arg inPoint) const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return GetPointVelocityCOM(Vec3(inPoint - mPosition)); } + + /// Add force (unit: N) at center of mass for the next time step, will be reset after the next call to PhysicsSystem::Update + inline void AddForce(Vec3Arg inForce) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mForce) + inForce).StoreFloat3(&mMotionProperties->mForce); } + + /// Add force (unit: N) at inPosition for the next time step, will be reset after the next call to PhysicsSystem::Update + inline void AddForce(Vec3Arg inForce, RVec3Arg inPosition); + + /// Add torque (unit: N m) for the next time step, will be reset after the next call to PhysicsSystem::Update + inline void AddTorque(Vec3Arg inTorque) { JPH_ASSERT(IsDynamic()); (Vec3::sLoadFloat3Unsafe(mMotionProperties->mTorque) + inTorque).StoreFloat3(&mMotionProperties->mTorque); } + + // Get the total amount of force applied to the center of mass this time step (through AddForce calls). Note that it will reset to zero after PhysicsSystem::Update. + inline Vec3 GetAccumulatedForce() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedForce(); } + + // Get the total amount of torque applied to the center of mass this time step (through AddForce/AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update. + inline Vec3 GetAccumulatedTorque() const { JPH_ASSERT(IsDynamic()); return mMotionProperties->GetAccumulatedTorque(); } + + // Reset the total accumulated force, not that this will be done automatically after every time step. + JPH_INLINE void ResetForce() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetForce(); } + + // Reset the total accumulated torque, not that this will be done automatically after every time step. + JPH_INLINE void ResetTorque() { JPH_ASSERT(IsDynamic()); return mMotionProperties->ResetTorque(); } + + // Reset the current velocity and accumulated force and torque. + JPH_INLINE void ResetMotion() { JPH_ASSERT(!IsStatic()); return mMotionProperties->ResetMotion(); } + + /// Get inverse inertia tensor in world space + inline Mat44 GetInverseInertia() const; + + /// Add impulse to center of mass (unit: kg m/s) + inline void AddImpulse(Vec3Arg inImpulse); + + /// Add impulse to point in world space (unit: kg m/s) + inline void AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition); + + /// Add angular impulse in world space (unit: N m s) + inline void AddAngularImpulse(Vec3Arg inAngularImpulse); + + /// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds. + void MoveKinematic(RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime); + + /// Applies an impulse to the body that simulates fluid buoyancy and drag + /// @param inSurfacePosition Position of the fluid surface in world space + /// @param inSurfaceNormal Normal of the fluid surface (should point up) + /// @param inBuoyancy The buoyancy factor for the body. 1 = neutral body, < 1 sinks, > 1 floats. Note that we don't use the fluid density since it is harder to configure than a simple number between [0, 2] + /// @param inLinearDrag Linear drag factor that slows down the body when in the fluid (approx. 0.5) + /// @param inAngularDrag Angular drag factor that slows down rotation when the body is in the fluid (approx. 0.01) + /// @param inFluidVelocity The average velocity of the fluid (in m/s) in which the body resides + /// @param inGravity The gravity vector (pointing down) + /// @param inDeltaTime Delta time of the next simulation step (in s) + /// @return true if an impulse was applied, false if the body was not in the fluid + bool ApplyBuoyancyImpulse(RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + + /// Check if this body has been added to the physics system + inline bool IsInBroadPhase() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::IsInBroadPhase)) != 0; } + + /// Check if this body has been changed in such a way that the collision cache should be considered invalid for any body interacting with this body + inline bool IsCollisionCacheInvalid() const { return (mFlags.load(memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) != 0; } + + /// Get the shape of this body + inline const Shape * GetShape() const { return mShape; } + + /// World space position of the body + inline RVec3 GetPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mPosition - mRotation * mShape->GetCenterOfMass(); } + + /// World space rotation of the body + inline Quat GetRotation() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mRotation; } + + /// Calculates the transform of this body + inline RMat44 GetWorldTransform() const; + + /// Gets the world space position of this body's center of mass + inline RVec3 GetCenterOfMassPosition() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return mPosition; } + + /// Calculates the transform for this body's center of mass + inline RMat44 GetCenterOfMassTransform() const; + + /// Calculates the inverse of the transform for this body's center of mass + inline RMat44 GetInverseCenterOfMassTransform() const; + + /// Get world space bounding box + inline const AABox & GetWorldSpaceBounds() const { return mBounds; } + + /// Access to the motion properties + const MotionProperties *GetMotionProperties() const { JPH_ASSERT(!IsStatic()); return mMotionProperties; } + MotionProperties * GetMotionProperties() { JPH_ASSERT(!IsStatic()); return mMotionProperties; } + + /// Access to the motion properties (version that does not check if the object is kinematic or dynamic) + const MotionProperties *GetMotionPropertiesUnchecked() const { return mMotionProperties; } + MotionProperties * GetMotionPropertiesUnchecked() { return mMotionProperties; } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Get surface normal of a particular sub shape and its world space surface position on this body + inline Vec3 GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const; + + /// Get the transformed shape of this body, which can be used to do collision detection outside of a body lock + inline TransformedShape GetTransformedShape() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); return TransformedShape(mPosition, mRotation, mShape, mID); } + + /// Debug function to convert a body back to a body creation settings object to be able to save/recreate the body later + BodyCreationSettings GetBodyCreationSettings() const; + + /// Debug function to convert a soft body back to a soft body creation settings object to be able to save/recreate the body later + SoftBodyCreationSettings GetSoftBodyCreationSettings() const; + + /// A dummy body that can be used by constraints to attach a constraint to the world instead of another body + static Body sFixedToWorld; + + ///@name THESE FUNCTIONS ARE FOR INTERNAL USE ONLY AND SHOULD NOT BE CALLED BY THE APPLICATION + ///@{ + + /// Helper function for BroadPhase::FindCollidingPairs that returns true when two bodies can collide + /// It assumes that body 1 is dynamic and active and guarantees that it body 1 collides with body 2 that body 2 will not collide with body 1 in order to avoid finding duplicate collision pairs + static inline bool sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2); + + /// Update position using an Euler step (used during position integrate & constraint solving) + inline void AddPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition += mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); } + inline void SubPositionStep(Vec3Arg inLinearVelocityTimesDeltaTime) { JPH_ASSERT(IsRigidBody()); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); mPosition -= mMotionProperties->LockTranslation(inLinearVelocityTimesDeltaTime); JPH_ASSERT(!mPosition.IsNaN()); } + + /// Update rotation using an Euler step (using during position integrate & constraint solving) + inline void AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime); + inline void SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime); + + /// Flag if body is in the broadphase (should only be called by the BroadPhase) + inline void SetInBroadPhaseInternal(bool inInBroadPhase) { if (inInBroadPhase) mFlags.fetch_or(uint8(EFlags::IsInBroadPhase), memory_order_relaxed); else mFlags.fetch_and(uint8(~uint8(EFlags::IsInBroadPhase)), memory_order_relaxed); } + + /// Invalidate the contact cache (should only be called by the BodyManager), will be reset the next simulation step. Returns true if the contact cache was still valid. + inline bool InvalidateContactCacheInternal() { return (mFlags.fetch_or(uint8(EFlags::InvalidateContactCache), memory_order_relaxed) & uint8(EFlags::InvalidateContactCache)) == 0; } + + /// Reset the collision cache invalid flag (should only be called by the BodyManager). + inline void ValidateContactCacheInternal() { JPH_IF_ENABLE_ASSERTS(uint8 old_val = ) mFlags.fetch_and(uint8(~uint8(EFlags::InvalidateContactCache)), memory_order_relaxed); JPH_ASSERT((old_val & uint8(EFlags::InvalidateContactCache)) != 0); } + + /// Updates world space bounding box (should only be called by the PhysicsSystem) + void CalculateWorldSpaceBoundsInternal(); + + /// Function to update body's position (should only be called by the BodyInterface since it also requires updating the broadphase) + void SetPositionAndRotationInternal(RVec3Arg inPosition, QuatArg inRotation, bool inResetSleepTimer = true); + + /// Updates the center of mass and optionally mass properties after shifting the center of mass or changes to the shape (should only be called by the BodyInterface since it also requires updating the broadphase) + /// @param inPreviousCenterOfMass Center of mass of the shape before the alterations + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + void UpdateCenterOfMassInternal(Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties); + + /// Function to update a body's shape (should only be called by the BodyInterface since it also requires updating the broadphase) + /// @param inShape The new shape for this body + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + void SetShapeInternal(const Shape *inShape, bool inUpdateMassProperties); + + /// Access to the index in the BodyManager::mActiveBodies list + uint32 GetIndexInActiveBodiesInternal() const { return mMotionProperties != nullptr? mMotionProperties->mIndexInActiveBodies : cInactiveIndex; } + + /// Update eligibility for sleeping + ECanSleep UpdateSleepStateInternal(float inDeltaTime, float inMaxMovement, float inTimeBeforeSleep); + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + ///@} + + static constexpr uint32 cInactiveIndex = MotionProperties::cInactiveIndex; ///< Constant indicating that body is not active + +private: + friend class BodyManager; + + explicit Body(bool); ///< Alternative constructor that initializes all members + + inline void GetSleepTestPoints(RVec3 *outPoints) const; ///< Determine points to test for checking if body is sleeping: COM, COM + largest bounding box axis, COM + second largest bounding box axis + + enum class EFlags : uint8 + { + IsSensor = 1 << 0, ///< If this object is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. + CollideKinematicVsNonDynamic = 1 << 1, ///< If kinematic objects can generate contact points against other kinematic or static objects. + IsInBroadPhase = 1 << 2, ///< Set this bit to indicate that the body is in the broadphase + InvalidateContactCache = 1 << 3, ///< Set this bit to indicate that all collision caches for this body are invalid, will be reset the next simulation step. + UseManifoldReduction = 1 << 4, ///< Set this bit to indicate that this body can use manifold reduction (if PhysicsSettings::mUseManifoldReduction is true) + ApplyGyroscopicForce = 1 << 5, ///< Set this bit to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + EnhancedInternalEdgeRemoval = 1 << 6, ///< Set this bit to indicate that enhanced internal edge removal should be used for this body (see BodyCreationSettings::mEnhancedInternalEdgeRemoval) + }; + + // 16 byte aligned + RVec3 mPosition; ///< World space position of center of mass + Quat mRotation; ///< World space rotation of center of mass + AABox mBounds; ///< World space bounding box of the body + + // 8 byte aligned + RefConst mShape; ///< Shape representing the volume of this body + MotionProperties * mMotionProperties = nullptr; ///< If this is a keyframed or dynamic object, this object holds all information about the movement + uint64 mUserData = 0; ///< User data, can be used for anything by the application + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + // 4 byte aligned + float mFriction; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + float mRestitution; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + BodyID mID; ///< ID of the body (index in the bodies array) + + // 2 or 4 bytes aligned + ObjectLayer mObjectLayer; ///< The collision layer this body belongs to (determines if two objects can collide) + + // 1 byte aligned + EBodyType mBodyType; ///< Type of body (rigid or soft) + BroadPhaseLayer mBroadPhaseLayer; ///< The broad phase layer this body belongs to + EMotionType mMotionType; ///< Type of motion (static, dynamic or kinematic) + atomic mFlags = 0; ///< See EFlags for possible flags + + // 122 bytes up to here (64-bit mode, single precision, 16-bit ObjectLayer) +}; + +static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(Body) == JPH_IF_SINGLE_PRECISION_ELSE(128, 160), "Body size is incorrect"); +static_assert(alignof(Body) == JPH_RVECTOR_ALIGNMENT, "Body should properly align"); + +JPH_NAMESPACE_END + +#include "Body.inl" diff --git a/WickedEngine/Jolt/Physics/Body/Body.inl b/WickedEngine/Jolt/Physics/Body/Body.inl new file mode 100644 index 000000000..51bc64878 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/Body.inl @@ -0,0 +1,197 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +RMat44 Body::GetWorldTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); + + return RMat44::sRotationTranslation(mRotation, mPosition).PreTranslated(-mShape->GetCenterOfMass()); +} + +RMat44 Body::GetCenterOfMassTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); + + return RMat44::sRotationTranslation(mRotation, mPosition); +} + +RMat44 Body::GetInverseCenterOfMassTransform() const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); + + return RMat44::sInverseRotationTranslation(mRotation, mPosition); +} + +inline bool Body::sFindCollidingPairsCanCollide(const Body &inBody1, const Body &inBody2) +{ + // First body should never be a soft body + JPH_ASSERT(!inBody1.IsSoftBody()); + + // One of these conditions must be true + // - We always allow detecting collisions between kinematic and non-dynamic bodies + // - One of the bodies must be dynamic to collide + // - A kinematic object can collide with a sensor + if (!inBody1.GetCollideKinematicVsNonDynamic() + && !inBody2.GetCollideKinematicVsNonDynamic() + && (!inBody1.IsDynamic() && !inBody2.IsDynamic()) + && !(inBody1.IsKinematic() && inBody2.IsSensor()) + && !(inBody2.IsKinematic() && inBody1.IsSensor())) + return false; + + // Check that body 1 is active + uint32 body1_index_in_active_bodies = inBody1.GetIndexInActiveBodiesInternal(); + JPH_ASSERT(!inBody1.IsStatic() && body1_index_in_active_bodies != Body::cInactiveIndex, "This function assumes that Body 1 is active"); + + // If the pair A, B collides we need to ensure that the pair B, A does not collide or else we will handle the collision twice. + // If A is the same body as B we don't want to collide (1) + // If A is dynamic / kinematic and B is static we should collide (2) + // If A is dynamic / kinematic and B is dynamic / kinematic we should only collide if + // - A is active and B is not active (3) + // - A is active and B will become active during this simulation step (4) + // - A is active and B is active, we require a condition that makes A, B collide and B, A not (5) + // + // In order to implement this we use the index in the active body list and make use of the fact that + // a body not in the active list has Body.Index = 0xffffffff which is the highest possible value for an uint32. + // + // Because we know that A is active we know that A.Index != 0xffffffff: + // (1) Because A.Index != 0xffffffff, if A.Index = B.Index then A = B, so to collide A.Index != B.Index + // (2) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's static and cannot be in the active list), so to collide A.Index != B.Index + // (3) A.Index != 0xffffffff, B.Index = 0xffffffff (because it's not yet active), so to collide A.Index != B.Index + // (4) A.Index != 0xffffffff, B.Index = 0xffffffff currently. But it can activate during the Broad/NarrowPhase step at which point it + // will be added to the end of the active list which will make B.Index > A.Index (this holds only true when we don't deactivate + // bodies during the Broad/NarrowPhase step), so to collide A.Index < B.Index. + // (5) As tie breaker we can use the same condition A.Index < B.Index to collide, this means that if A, B collides then B, A won't + static_assert(Body::cInactiveIndex == 0xffffffff, "The algorithm below uses this value"); + if (!inBody2.IsSoftBody() && body1_index_in_active_bodies >= inBody2.GetIndexInActiveBodiesInternal()) + return false; + JPH_ASSERT(inBody1.GetID() != inBody2.GetID(), "Read the comment above, A and B are the same body which should not be possible!"); + + // Check collision group filter + if (!inBody1.GetCollisionGroup().CanCollide(inBody2.GetCollisionGroup())) + return false; + + return true; +} + +void Body::AddRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); + + // This used to use the equation: d/dt R(t) = 1/2 * w(t) * R(t) so that R(t + dt) = R(t) + 1/2 * w(t) * R(t) * dt + // See: Appendix B of An Introduction to Physically Based Modeling: Rigid Body Simulation II-Nonpenetration Constraints + // URL: https://www.cs.cmu.edu/~baraff/sigcourse/notesd2.pdf + // But this is a first order approximation and does not work well for kinematic ragdolls that are driven to a new + // pose if the poses differ enough. So now we split w(t) * dt into an axis and angle part and create a quaternion with it. + // Note that the resulting quaternion is normalized since otherwise numerical drift will eventually make the rotation non-normalized. + float len = inAngularVelocityTimesDeltaTime.Length(); + if (len > 1.0e-6f) + { + mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, len) * mRotation).Normalized(); + JPH_ASSERT(!mRotation.IsNaN()); + } +} + +void Body::SubRotationStep(Vec3Arg inAngularVelocityTimesDeltaTime) +{ + JPH_ASSERT(IsRigidBody()); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::ReadWrite)); + + // See comment at Body::AddRotationStep + float len = inAngularVelocityTimesDeltaTime.Length(); + if (len > 1.0e-6f) + { + mRotation = (Quat::sRotation(inAngularVelocityTimesDeltaTime / len, -len) * mRotation).Normalized(); + JPH_ASSERT(!mRotation.IsNaN()); + } +} + +Vec3 Body::GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const +{ + RMat44 inv_com = GetInverseCenterOfMassTransform(); + return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(inSubShapeID, Vec3(inv_com * inPosition))).Normalized(); +} + +Mat44 Body::GetInverseInertia() const +{ + JPH_ASSERT(IsDynamic()); + + return GetMotionProperties()->GetInverseInertiaForRotation(Mat44::sRotation(mRotation)); +} + +void Body::AddForce(Vec3Arg inForce, RVec3Arg inPosition) +{ + AddForce(inForce); + AddTorque(Vec3(inPosition - mPosition).Cross(inForce)); +} + +void Body::AddImpulse(Vec3Arg inImpulse) +{ + JPH_ASSERT(IsDynamic()); + + SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass()); +} + +void Body::AddImpulse(Vec3Arg inImpulse, RVec3Arg inPosition) +{ + JPH_ASSERT(IsDynamic()); + + SetLinearVelocityClamped(mMotionProperties->GetLinearVelocity() + inImpulse * mMotionProperties->GetInverseMass()); + + SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, Vec3(inPosition - mPosition).Cross(inImpulse))); +} + +void Body::AddAngularImpulse(Vec3Arg inAngularImpulse) +{ + JPH_ASSERT(IsDynamic()); + + SetAngularVelocityClamped(mMotionProperties->GetAngularVelocity() + mMotionProperties->MultiplyWorldSpaceInverseInertiaByVector(mRotation, inAngularImpulse)); +} + +void Body::GetSleepTestPoints(RVec3 *outPoints) const +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); + + // Center of mass is the first position + outPoints[0] = mPosition; + + // The second and third position are on the largest axis of the bounding box + Vec3 extent = mShape->GetLocalBounds().GetExtent(); + int lowest_component = extent.GetLowestComponentIndex(); + Mat44 rotation = Mat44::sRotation(mRotation); + switch (lowest_component) + { + case 0: + outPoints[1] = mPosition + extent.GetY() * rotation.GetColumn3(1); + outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2); + break; + + case 1: + outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0); + outPoints[2] = mPosition + extent.GetZ() * rotation.GetColumn3(2); + break; + + case 2: + outPoints[1] = mPosition + extent.GetX() * rotation.GetColumn3(0); + outPoints[2] = mPosition + extent.GetY() * rotation.GetColumn3(1); + break; + + default: + JPH_ASSERT(false); + break; + } +} + +void Body::ResetSleepTimer() +{ + RVec3 points[3]; + GetSleepTestPoints(points); + mMotionProperties->ResetSleepTestSpheres(points); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyAccess.cpp b/WickedEngine/Jolt/Physics/Body/BodyAccess.cpp new file mode 100644 index 000000000..b343e717c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyAccess.cpp @@ -0,0 +1,18 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_BEGIN + +thread_local BodyAccess::EAccess BodyAccess::sVelocityAccess = BodyAccess::EAccess::ReadWrite; +thread_local BodyAccess::EAccess BodyAccess::sPositionAccess = BodyAccess::EAccess::ReadWrite; + +JPH_NAMESPACE_END + +#endif diff --git a/WickedEngine/Jolt/Physics/Body/BodyAccess.h b/WickedEngine/Jolt/Physics/Body/BodyAccess.h new file mode 100644 index 000000000..426460aac --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyAccess.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifdef JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_BEGIN + +class BodyAccess +{ +public: + /// Access rules, used to detect race conditions during simulation + enum class EAccess : uint8 + { + None = 0, + Read = 1, + ReadWrite = 3, + }; + + /// Grant a scope specific access rights on the current thread + class Grant + { + public: + inline Grant(EAccess inVelocity, EAccess inPosition) + { + JPH_ASSERT(sVelocityAccess == EAccess::ReadWrite); + JPH_ASSERT(sPositionAccess == EAccess::ReadWrite); + + sVelocityAccess = inVelocity; + sPositionAccess = inPosition; + } + + inline ~Grant() + { + sVelocityAccess = EAccess::ReadWrite; + sPositionAccess = EAccess::ReadWrite; + } + }; + + /// Check if we have permission + static bool sCheckRights(EAccess inRights, EAccess inDesiredRights) + { + return (uint8(inRights) & uint8(inDesiredRights)) == uint8(inDesiredRights); + } + + // Various permissions that can be granted + static thread_local EAccess sVelocityAccess; + static thread_local EAccess sPositionAccess; +}; + +JPH_NAMESPACE_END + +#endif // JPH_ENABLE_ASSERTS diff --git a/WickedEngine/Jolt/Physics/Body/BodyActivationListener.h b/WickedEngine/Jolt/Physics/Body/BodyActivationListener.h new file mode 100644 index 000000000..2c8808a15 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyActivationListener.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class BodyID; + +/// A listener class that receives events when a body activates or deactivates. +/// It can be registered with the BodyManager (or PhysicsSystem). +class BodyActivationListener +{ +public: + /// Ensure virtual destructor + virtual ~BodyActivationListener() = default; + + /// Called whenever a body activates, note this can be called from any thread so make sure your code is thread safe. + /// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback. + virtual void OnBodyActivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0; + + /// Called whenever a body deactivates, note this can be called from any thread so make sure your code is thread safe. + /// At the time of the callback the body inBodyID will be locked and no bodies can be written/activated/deactivated from the callback. + virtual void OnBodyDeactivated(const BodyID &inBodyID, uint64 inBodyUserData) = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyCreationSettings.cpp b/WickedEngine/Jolt/Physics/Body/BodyCreationSettings.cpp new file mode 100644 index 000000000..9b6d3f92c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyCreationSettings.cpp @@ -0,0 +1,234 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(BodyCreationSettings) +{ + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mPosition) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRotation) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUserData) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mShape) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mCollisionGroup) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mObjectLayer) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionType) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mAllowedDOFs) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowDynamicOrKinematic) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mIsSensor) + JPH_ADD_ATTRIBUTE_WITH_ALIAS(BodyCreationSettings, mCollideKinematicVsNonDynamic, "mSensorDetectsStatic") // This is the old name to keep backwards compatibility + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mUseManifoldReduction) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mApplyGyroscopicForce) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mMotionQuality) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mEnhancedInternalEdgeRemoval) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAllowSleeping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mFriction) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mRestitution) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mLinearDamping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mAngularDamping) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxLinearVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMaxAngularVelocity) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mGravityFactor) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumVelocityStepsOverride) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mNumPositionStepsOverride) + JPH_ADD_ENUM_ATTRIBUTE(BodyCreationSettings, mOverrideMassProperties) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mInertiaMultiplier) + JPH_ADD_ATTRIBUTE(BodyCreationSettings, mMassPropertiesOverride) +} + +void BodyCreationSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mLinearVelocity); + inStream.Write(mAngularVelocity); + mCollisionGroup.SaveBinaryState(inStream); + inStream.Write(mObjectLayer); + inStream.Write(mMotionType); + inStream.Write(mAllowedDOFs); + inStream.Write(mAllowDynamicOrKinematic); + inStream.Write(mIsSensor); + inStream.Write(mCollideKinematicVsNonDynamic); + inStream.Write(mUseManifoldReduction); + inStream.Write(mApplyGyroscopicForce); + inStream.Write(mMotionQuality); + inStream.Write(mEnhancedInternalEdgeRemoval); + inStream.Write(mAllowSleeping); + inStream.Write(mFriction); + inStream.Write(mRestitution); + inStream.Write(mLinearDamping); + inStream.Write(mAngularDamping); + inStream.Write(mMaxLinearVelocity); + inStream.Write(mMaxAngularVelocity); + inStream.Write(mGravityFactor); + inStream.Write(mNumVelocityStepsOverride); + inStream.Write(mNumPositionStepsOverride); + inStream.Write(mOverrideMassProperties); + inStream.Write(mInertiaMultiplier); + mMassPropertiesOverride.SaveBinaryState(inStream); +} + +void BodyCreationSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mLinearVelocity); + inStream.Read(mAngularVelocity); + mCollisionGroup.RestoreBinaryState(inStream); + inStream.Read(mObjectLayer); + inStream.Read(mMotionType); + inStream.Read(mAllowedDOFs); + inStream.Read(mAllowDynamicOrKinematic); + inStream.Read(mIsSensor); + inStream.Read(mCollideKinematicVsNonDynamic); + inStream.Read(mUseManifoldReduction); + inStream.Read(mApplyGyroscopicForce); + inStream.Read(mMotionQuality); + inStream.Read(mEnhancedInternalEdgeRemoval); + inStream.Read(mAllowSleeping); + inStream.Read(mFriction); + inStream.Read(mRestitution); + inStream.Read(mLinearDamping); + inStream.Read(mAngularDamping); + inStream.Read(mMaxLinearVelocity); + inStream.Read(mMaxAngularVelocity); + inStream.Read(mGravityFactor); + inStream.Read(mNumVelocityStepsOverride); + inStream.Read(mNumPositionStepsOverride); + inStream.Read(mOverrideMassProperties); + inStream.Read(mInertiaMultiplier); + mMassPropertiesOverride.RestoreBinaryState(inStream); +} + +Shape::ShapeResult BodyCreationSettings::ConvertShapeSettings() +{ + // If we already have a shape, return it + if (mShapePtr != nullptr) + { + mShape = nullptr; + + Shape::ShapeResult result; + result.Set(const_cast(mShapePtr.GetPtr())); + return result; + } + + // Check if we have shape settings + if (mShape == nullptr) + { + Shape::ShapeResult result; + result.SetError("No shape present!"); + return result; + } + + // Create the shape + Shape::ShapeResult result = mShape->Create(); + if (result.IsValid()) + mShapePtr = result.Get(); + mShape = nullptr; + return result; +} + +const Shape *BodyCreationSettings::GetShape() const +{ + // If we already have a shape, return it + if (mShapePtr != nullptr) + return mShapePtr; + + // Check if we have shape settings + if (mShape == nullptr) + return nullptr; + + // Create the shape + Shape::ShapeResult result = mShape->Create(); + if (result.IsValid()) + return result.Get(); + + Trace("Error: %s", result.GetError().c_str()); + JPH_ASSERT(false, "An error occurred during shape creation. Use ConvertShapeSettings() to convert the shape and get the error!"); + return nullptr; +} + +MassProperties BodyCreationSettings::GetMassProperties() const +{ + // Calculate mass properties + MassProperties mass_properties; + switch (mOverrideMassProperties) + { + case EOverrideMassProperties::CalculateMassAndInertia: + mass_properties = GetShape()->GetMassProperties(); + mass_properties.mInertia *= mInertiaMultiplier; + mass_properties.mInertia(3, 3) = 1.0f; + break; + case EOverrideMassProperties::CalculateInertia: + mass_properties = GetShape()->GetMassProperties(); + mass_properties.ScaleToMass(mMassPropertiesOverride.mMass); + mass_properties.mInertia *= mInertiaMultiplier; + mass_properties.mInertia(3, 3) = 1.0f; + break; + case EOverrideMassProperties::MassAndInertiaProvided: + mass_properties = mMassPropertiesOverride; + break; + } + return mass_properties; +} + +void BodyCreationSettings::SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const +{ + // Save creation settings + SaveBinaryState(inStream); + + // Save shape + if (ioShapeMap != nullptr && ioMaterialMap != nullptr) + GetShape()->SaveWithChildren(inStream, *ioShapeMap, *ioMaterialMap); + else + inStream.Write(~uint32(0)); + + // Save group filter + StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap); +} + +BodyCreationSettings::BCSResult BodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap) +{ + BCSResult result; + + // Read creation settings + BodyCreationSettings settings; + settings.RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Error reading body creation settings"); + return result; + } + + // Read shape + Shape::ShapeResult shape_result = Shape::sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap); + if (shape_result.HasError()) + { + result.SetError(shape_result.GetError()); + return result; + } + settings.SetShape(shape_result.Get()); + + // Read group filter + Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap); + if (gfresult.HasError()) + { + result.SetError(gfresult.GetError()); + return result; + } + settings.mCollisionGroup.SetGroupFilter(gfresult.Get()); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyCreationSettings.h b/WickedEngine/Jolt/Physics/Body/BodyCreationSettings.h new file mode 100644 index 000000000..a0395473f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyCreationSettings.h @@ -0,0 +1,124 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Enum used in BodyCreationSettings to indicate how mass and inertia should be calculated +enum class EOverrideMassProperties : uint8 +{ + CalculateMassAndInertia, ///< Tells the system to calculate the mass and inertia based on density + CalculateInertia, ///< Tells the system to take the mass from mMassPropertiesOverride and to calculate the inertia based on density of the shapes and to scale it to the provided mass + MassAndInertiaProvided ///< Tells the system to take the mass and inertia from mMassPropertiesOverride +}; + +/// Settings for constructing a rigid body +class JPH_EXPORT BodyCreationSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, BodyCreationSettings) + + /// Constructor + BodyCreationSettings() = default; + BodyCreationSettings(const ShapeSettings *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShape(inShape) { } + BodyCreationSettings(const Shape *inShape, RVec3Arg inPosition, QuatArg inRotation, EMotionType inMotionType, ObjectLayer inObjectLayer) : mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer), mMotionType(inMotionType), mShapePtr(inShape) { } + + /// Access to the shape settings object. This contains serializable (non-runtime optimized) information about the Shape. + const ShapeSettings * GetShapeSettings() const { return mShape; } + void SetShapeSettings(const ShapeSettings *inShape) { mShape = inShape; mShapePtr = nullptr; } + + /// Convert ShapeSettings object into a Shape object. This will free the ShapeSettings object and make the object ready for runtime. Serialization is no longer possible after this. + Shape::ShapeResult ConvertShapeSettings(); + + /// Access to the run-time shape object. Will convert from ShapeSettings object if needed. + const Shape * GetShape() const; + void SetShape(const Shape *inShape) { mShapePtr = inShape; mShape = nullptr; } + + /// Check if the mass properties of this body will be calculated (only relevant for kinematic or dynamic objects that need a MotionProperties object) + bool HasMassProperties() const { return mAllowDynamicOrKinematic || mMotionType != EMotionType::Static; } + + /// Calculate (or return when overridden) the mass and inertia for this body + MassProperties GetMassProperties() const; + + /// Saves the state of this object in binary form to inStream. Doesn't store the shape nor the group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the shape nor the group filter. + void RestoreBinaryState(StreamIn &inStream); + + using GroupFilterToIDMap = StreamUtils::ObjectToIDMap; + using IDToGroupFilterMap = StreamUtils::IDToObjectMap; + using ShapeToIDMap = Shape::ShapeToIDMap; + using IDToShapeMap = Shape::IDToShapeMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + /// Pass nullptr to ioShapeMap and ioMaterial map to skip saving shapes + /// Pass nullptr to ioGroupFilterMap to skip saving group filters + void SaveWithChildren(StreamOut &inStream, ShapeToIDMap *ioShapeMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const; + + using BCSResult = Result; + + /// Restore body creation settings, its shape, materials and group filter. Pass in an empty map in ioShapeMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static BCSResult sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap); + + RVec3 mPosition = RVec3::sZero(); ///< Position of the body (not of the center of mass) + Quat mRotation = Quat::sIdentity(); ///< Rotation of the body + Vec3 mLinearVelocity = Vec3::sZero(); ///< World space linear velocity of the center of mass (m/s) + Vec3 mAngularVelocity = Vec3::sZero(); ///< World space angular velocity (rad/s) + + /// User data value (can be used by application) + uint64 mUserData = 0; + + ///@name Collision settings + ObjectLayer mObjectLayer = 0; ///< The collision layer this body belongs to (determines if two objects can collide) + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + ///@name Simulation properties + EMotionType mMotionType = EMotionType::Dynamic; ///< Motion type, determines if the object is static, dynamic or kinematic + EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Which degrees of freedom this body has (can be used to limit simulation to 2D) + bool mAllowDynamicOrKinematic = false; ///< When this body is created as static, this setting tells the system to create a MotionProperties object so that the object can be switched to kinematic or dynamic + bool mIsSensor = false; ///< If this body is a sensor. A sensor will receive collision callbacks, but will not cause any collision responses and can be used as a trigger volume. See description at Body::SetIsSensor. + bool mCollideKinematicVsNonDynamic = false; ///< If kinematic objects can generate contact points against other kinematic or static objects. See description at Body::SetCollideKinematicVsNonDynamic. + bool mUseManifoldReduction = true; ///< If this body should use manifold reduction (see description at Body::SetUseManifoldReduction) + bool mApplyGyroscopicForce = false; ///< Set to indicate that the gyroscopic force should be applied to this body (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + EMotionQuality mMotionQuality = EMotionQuality::Discrete; ///< Motion quality, or how well it detects collisions when it has a high velocity + bool mEnhancedInternalEdgeRemoval = false; ///< Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool mAllowSleeping = true; ///< If this body can go to sleep or not + float mFriction = 0.2f; ///< Friction of the body (dimensionless number, usually between 0 and 1, 0 = no friction, 1 = friction force equals force that presses the two bodies together). Note that bodies can have negative friction but the combined friction (see PhysicsSystem::SetCombineFriction) should never go below zero. + float mRestitution = 0.0f; ///< Restitution of body (dimensionless number, usually between 0 and 1, 0 = completely inelastic collision response, 1 = completely elastic collision response). Note that bodies can have negative restitution but the combined restitution (see PhysicsSystem::SetCombineRestitution) should never go below zero. + float mLinearDamping = 0.05f; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + float mAngularDamping = 0.05f; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + float mMaxLinearVelocity = 500.0f; ///< Maximum linear velocity that this body can reach (m/s) + float mMaxAngularVelocity = 0.25f * JPH_PI * 60.0f; ///< Maximum angular velocity that this body can reach (rad/s) + float mGravityFactor = 1.0f; ///< Value to multiply gravity with for this body + uint mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + + ///@name Mass properties of the body (by default calculated by the shape) + EOverrideMassProperties mOverrideMassProperties = EOverrideMassProperties::CalculateMassAndInertia; ///< Determines how mMassPropertiesOverride will be used + float mInertiaMultiplier = 1.0f; ///< When calculating the inertia (not when it is provided) the calculated inertia will be multiplied by this value + MassProperties mMassPropertiesOverride; ///< Contains replacement mass settings which override the automatically calculated values + +private: + /// Collision volume for the body + RefConst mShape; ///< Shape settings, can be serialized. Mutually exclusive with mShapePtr + RefConst mShapePtr; ///< Actual shape, cannot be serialized. Mutually exclusive with mShape +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyFilter.h b/WickedEngine/Jolt/Physics/Body/BodyFilter.h new file mode 100644 index 000000000..c876e9d09 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyFilter.h @@ -0,0 +1,102 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; + +/// Class function to filter out bodies, returns true if test should collide with body +class BodyFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BodyFilter() = default; + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide([[maybe_unused]] const BodyID &inBodyID) const + { + return true; + } + + /// Filter function. Returns true if we should collide with inBody (this is called after the body is locked and makes it possible to filter based on body members) + virtual bool ShouldCollideLocked([[maybe_unused]] const Body &inBody) const + { + return true; + } +}; + +/// A simple body filter implementation that ignores a single, specified body +class IgnoreSingleBodyFilter : public BodyFilter +{ +public: + /// Constructor, pass the body you want to ignore + explicit IgnoreSingleBodyFilter(const BodyID &inBodyID) : + mBodyID(inBodyID) + { + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return mBodyID != inBodyID; + } + +private: + BodyID mBodyID; +}; + +/// A simple body filter implementation that ignores multiple, specified bodies +class IgnoreMultipleBodiesFilter : public BodyFilter +{ +public: + /// Remove all bodies from the filter + void Clear() + { + mBodyIDs.clear(); + } + + /// Reserve space for inSize body ID's + void Reserve(uint inSize) + { + mBodyIDs.reserve(inSize); + } + + /// Add a body to be ignored + void IgnoreBody(const BodyID &inBodyID) + { + mBodyIDs.push_back(inBodyID); + } + + /// Filter function. Returns true if we should collide with inBodyID + virtual bool ShouldCollide(const BodyID &inBodyID) const override + { + return std::find(mBodyIDs.begin(), mBodyIDs.end(), inBodyID) == mBodyIDs.end(); + } + +private: + Array mBodyIDs; +}; + +#ifdef JPH_DEBUG_RENDERER +/// Class function to filter out bodies for debug rendering, returns true if body should be rendered +class BodyDrawFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BodyDrawFilter() = default; + + /// Filter function. Returns true if inBody should be rendered + virtual bool ShouldDraw([[maybe_unused]] const Body& inBody) const + { + return true; + } +}; +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyID.h b/WickedEngine/Jolt/Physics/Body/BodyID.h new file mode 100644 index 000000000..f56d96b7b --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyID.h @@ -0,0 +1,100 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// ID of a body. This is a way of reasoning about bodies in a multithreaded simulation while avoiding race conditions. +class BodyID +{ +public: + JPH_OVERRIDE_NEW_DELETE + + static constexpr uint32 cInvalidBodyID = 0xffffffff; ///< The value for an invalid body ID + static constexpr uint32 cBroadPhaseBit = 0x00800000; ///< This bit is used by the broadphase + static constexpr uint32 cMaxBodyIndex = 0x7fffff; ///< Maximum value for body index (also the maximum amount of bodies supported - 1) + static constexpr uint8 cMaxSequenceNumber = 0xff; ///< Maximum value for the sequence number + + /// Construct invalid body ID + BodyID() : + mID(cInvalidBodyID) + { + } + + /// Construct from index and sequence number combined in a single uint32 (use with care!) + explicit BodyID(uint32 inID) : + mID(inID) + { + JPH_ASSERT((inID & cBroadPhaseBit) == 0 || inID == cInvalidBodyID); // Check bit used by broadphase + } + + /// Construct from index and sequence number + explicit BodyID(uint32 inID, uint8 inSequenceNumber) : + mID((uint32(inSequenceNumber) << 24) | inID) + { + JPH_ASSERT(inID < cMaxBodyIndex); // Should not use bit pattern for invalid ID and should not use the broadphase bit + } + + /// Get index in body array + inline uint32 GetIndex() const + { + return mID & cMaxBodyIndex; + } + + /// Get sequence number of body. + /// The sequence number can be used to check if a body ID with the same body index has been reused by another body. + /// It is mainly used in multi threaded situations where a body is removed and its body index is immediately reused by a body created from another thread. + /// Functions querying the broadphase can (after acquiring a body lock) detect that the body has been removed (we assume that this won't happen more than 128 times in a row). + inline uint8 GetSequenceNumber() const + { + return uint8(mID >> 24); + } + + /// Returns the index and sequence number combined in an uint32 + inline uint32 GetIndexAndSequenceNumber() const + { + return mID; + } + + /// Check if the ID is valid + inline bool IsInvalid() const + { + return mID == cInvalidBodyID; + } + + /// Equals check + inline bool operator == (const BodyID &inRHS) const + { + return mID == inRHS.mID; + } + + /// Not equals check + inline bool operator != (const BodyID &inRHS) const + { + return mID != inRHS.mID; + } + + /// Smaller than operator, can be used for sorting bodies + inline bool operator < (const BodyID &inRHS) const + { + return mID < inRHS.mID; + } + + /// Greater than operator, can be used for sorting bodies + inline bool operator > (const BodyID &inRHS) const + { + return mID > inRHS.mID; + } + +private: + uint32 mID; +}; + +JPH_NAMESPACE_END + +// Create a std::hash for BodyID +JPH_MAKE_HASHABLE(JPH::BodyID, t.GetIndexAndSequenceNumber()) diff --git a/WickedEngine/Jolt/Physics/Body/BodyInterface.cpp b/WickedEngine/Jolt/Physics/Body/BodyInterface.cpp new file mode 100644 index 000000000..cc5f27de4 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyInterface.cpp @@ -0,0 +1,1034 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void BodyInterface::ActivateBodyInternal(Body &ioBody) const +{ + // Activate body or reset its sleep timer. + // Note that BodyManager::ActivateBodies also resets the sleep timer internally, but we avoid a mutex lock if the body is already active by calling ResetSleepTimer directly. + if (!ioBody.IsActive()) + mBodyManager->ActivateBodies(&ioBody.GetID(), 1); + else + ioBody.ResetSleepTimer(); +} + +Body *BodyInterface::CreateBody(const BodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateBody(inSettings); + if (!mBodyManager->AddBody(body)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateSoftBody(const SoftBodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateSoftBody(inSettings); + if (!mBodyManager->AddBody(body)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateBody(inSettings); + if (!mBodyManager->AddBodyWithCustomID(body, inBodyID)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings) +{ + Body *body = mBodyManager->AllocateSoftBody(inSettings); + if (!mBodyManager->AddBodyWithCustomID(body, inBodyID)) + { + mBodyManager->FreeBody(body); + return nullptr; + } + return body; +} + +Body *BodyInterface::CreateBodyWithoutID(const BodyCreationSettings &inSettings) const +{ + return mBodyManager->AllocateBody(inSettings); +} + +Body *BodyInterface::CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const +{ + return mBodyManager->AllocateSoftBody(inSettings); +} + +void BodyInterface::DestroyBodyWithoutID(Body *inBody) const +{ + mBodyManager->FreeBody(inBody); +} + +bool BodyInterface::AssignBodyID(Body *ioBody) +{ + return mBodyManager->AddBody(ioBody); +} + +bool BodyInterface::AssignBodyID(Body *ioBody, const BodyID &inBodyID) +{ + return mBodyManager->AddBodyWithCustomID(ioBody, inBodyID); +} + +Body *BodyInterface::UnassignBodyID(const BodyID &inBodyID) +{ + Body *body = nullptr; + mBodyManager->RemoveBodies(&inBodyID, 1, &body); + return body; +} + +void BodyInterface::UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies) +{ + mBodyManager->RemoveBodies(inBodyIDs, inNumber, outBodies); +} + +void BodyInterface::DestroyBody(const BodyID &inBodyID) +{ + mBodyManager->DestroyBodies(&inBodyID, 1); +} + +void BodyInterface::DestroyBodies(const BodyID *inBodyIDs, int inNumber) +{ + mBodyManager->DestroyBodies(inBodyIDs, inNumber); +} + +void BodyInterface::AddBody(const BodyID &inBodyID, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Add to broadphase + BodyID id = inBodyID; + BroadPhase::AddState add_state = mBroadPhase->AddBodiesPrepare(&id, 1); + mBroadPhase->AddBodiesFinalize(&id, 1, add_state); + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::RemoveBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Deactivate body + if (body.IsActive()) + mBodyManager->DeactivateBodies(&inBodyID, 1); + + // Remove from broadphase + BodyID id = inBodyID; + mBroadPhase->RemoveBodies(&id, 1); + } +} + +bool BodyInterface::IsAdded(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + return lock.SucceededAndIsInBroadPhase(); +} + +BodyID BodyInterface::CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode) +{ + const Body *b = CreateBody(inSettings); + if (b == nullptr) + return BodyID(); // Out of bodies + AddBody(b->GetID(), inActivationMode); + return b->GetID(); +} + +BodyID BodyInterface::CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode) +{ + const Body *b = CreateSoftBody(inSettings); + if (b == nullptr) + return BodyID(); // Out of bodies + AddBody(b->GetID(), inActivationMode); + return b->GetID(); +} + +BodyInterface::AddState BodyInterface::AddBodiesPrepare(BodyID *ioBodies, int inNumber) +{ + return mBroadPhase->AddBodiesPrepare(ioBodies, inNumber); +} + +void BodyInterface::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber); + + // Add to broadphase + mBroadPhase->AddBodiesFinalize(ioBodies, inNumber, inAddState); + + // Optionally activate bodies + if (inActivationMode == EActivation::Activate) + mBodyManager->ActivateBodies(ioBodies, inNumber); +} + +void BodyInterface::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + mBroadPhase->AddBodiesAbort(ioBodies, inNumber, inAddState); +} + +void BodyInterface::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, ioBodies, inNumber); + + // Deactivate bodies + mBodyManager->DeactivateBodies(ioBodies, inNumber); + + // Remove from broadphase + mBroadPhase->RemoveBodies(ioBodies, inNumber); +} + +void BodyInterface::ActivateBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + ActivateBodyInternal(body); + } +} + +void BodyInterface::ActivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber); + + mBodyManager->ActivateBodies(inBodyIDs, inNumber); +} + +void BodyInterface::ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) +{ + AllHitCollisionCollector collector; + mBroadPhase->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); + ActivateBodies(collector.mHits.data(), (int)collector.mHits.size()); +} + +void BodyInterface::DeactivateBody(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + if (body.IsActive()) + mBodyManager->DeactivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::DeactivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + BodyLockMultiWrite lock(*mBodyLockInterface, inBodyIDs, inNumber); + + mBodyManager->DeactivateBodies(inBodyIDs, inNumber); +} + +bool BodyInterface::IsActive(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + return lock.Succeeded() && lock.GetBody().IsActive(); +} + +void BodyInterface::ResetSleepTimer(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().ResetSleepTimer(); +} + +TwoBodyConstraint *BodyInterface::CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2) +{ + BodyID constraint_bodies[] = { inBodyID1, inBodyID2 }; + BodyLockMultiWrite lock(*mBodyLockInterface, constraint_bodies, 2); + + Body *body1 = lock.GetBody(0); + Body *body2 = lock.GetBody(1); + + JPH_ASSERT(body1 != body2); + JPH_ASSERT(body1 != nullptr || body2 != nullptr); + + return inSettings->Create(body1 != nullptr? *body1 : Body::sFixedToWorld, body2 != nullptr? *body2 : Body::sFixedToWorld); +} + +void BodyInterface::ActivateConstraint(const TwoBodyConstraint *inConstraint) +{ + BodyID bodies[] = { inConstraint->GetBody1()->GetID(), inConstraint->GetBody2()->GetID() }; + ActivateBodies(bodies, 2); +} + +RefConst BodyInterface::GetShape(const BodyID &inBodyID) const +{ + RefConst shape; + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + shape = lock.GetBody().GetShape(); + return shape; +} + +void BodyInterface::SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if shape actually changed + if (body.GetShape() != inShape) + { + // Update the shape + body.SetShapeInternal(inShape, inUpdateMassProperties); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update center of mass, mass and inertia + body.UpdateCenterOfMassInternal(inPreviousCenterOfMass, inUpdateMassProperties); + + // Recalculate bounding box + body.CalculateWorldSpaceBoundsInternal(); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +void BodyInterface::SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if layer actually changed, updating the broadphase is rather expensive + if (body.GetObjectLayer() != inLayer) + { + // Update the layer on the body + mBodyManager->SetBodyObjectLayerInternal(body, inLayer); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesLayerChanged(&id, 1); + } + } + } +} + +ObjectLayer BodyInterface::GetObjectLayer(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetObjectLayer(); + else + return cObjectLayerInvalid; +} + +void BodyInterface::SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +void BodyInterface::SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Check if there is enough change + if (!body.GetPosition().IsClose(inPosition) + || !body.GetRotation().IsClose(inRotation)) + { + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + outPosition = body.GetPosition(); + outRotation = body.GetRotation(); + } + else + { + outPosition = RVec3::sZero(); + outRotation = Quat::sIdentity(); + } +} + +void BodyInterface::SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, body.GetRotation()); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +RVec3 BodyInterface::GetPosition(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetPosition(); + else + return RVec3::sZero(); +} + +RVec3 BodyInterface::GetCenterOfMassPosition(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetCenterOfMassPosition(); + else + return RVec3::sZero(); +} + +void BodyInterface::SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(body.GetPosition(), inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + // Optionally activate body + if (inActivationMode == EActivation::Activate && !body.IsStatic()) + ActivateBodyInternal(body); + } +} + +Quat BodyInterface::GetRotation(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetRotation(); + else + return Quat::sIdentity(); +} + +RMat44 BodyInterface::GetWorldTransform(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetWorldTransform(); + else + return RMat44::sIdentity(); +} + +RMat44 BodyInterface::GetCenterOfMassTransform(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetCenterOfMassTransform(); + else + return RMat44::sIdentity(); +} + +void BodyInterface::MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + body.MoveKinematic(inTargetPosition, inTargetRotation, inDeltaTime); + + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } +} + +void BodyInterface::SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + body.SetAngularVelocityClamped(inAngularVelocity); + + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + outLinearVelocity = body.GetLinearVelocity(); + outAngularVelocity = body.GetAngularVelocity(); + return; + } + } + + outLinearVelocity = outAngularVelocity = Vec3::sZero(); +} + +void BodyInterface::SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + + if (!body.IsActive() && !inLinearVelocity.IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +Vec3 BodyInterface::GetLinearVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetLinearVelocity(); + } + + return Vec3::sZero(); +} + +void BodyInterface::AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); + + if (!body.IsActive() && !body.GetLinearVelocity().IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(body.GetLinearVelocity() + inLinearVelocity); + body.SetAngularVelocityClamped(body.GetAngularVelocity() + inAngularVelocity); + + if (!body.IsActive() && (!body.GetLinearVelocity().IsNearZero() || !body.GetAngularVelocity().IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (!body.IsStatic()) + { + body.SetAngularVelocityClamped(inAngularVelocity); + + if (!body.IsActive() && !inAngularVelocity.IsNearZero()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +Vec3 BodyInterface::GetAngularVelocity(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetAngularVelocity(); + } + + return Vec3::sZero(); +} + +Vec3 BodyInterface::GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + if (!body.IsStatic()) + return body.GetPointVelocity(inPoint); + } + + return Vec3::sZero(); +} + +void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce, inPoint); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddTorque(inTorque); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() && (inActivationMode == EActivation::Activate || body.IsActive())) + { + body.AddForce(inForce); + body.AddTorque(inTorque); + + if (inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } + } +} + +void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddImpulse(inImpulse); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddImpulse(inImpulse, inPoint); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic()) + { + body.AddAngularImpulse(inAngularImpulse); + + if (!body.IsActive()) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +bool BodyInterface::ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.IsDynamic() + && body.ApplyBuoyancyImpulse(inSurfacePosition, inSurfaceNormal, inBuoyancy, inLinearDrag, inAngularDrag, inFluidVelocity, inGravity, inDeltaTime)) + { + ActivateBodyInternal(body); + return true; + } + } + + return false; +} + +void BodyInterface::SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Update the position + body.SetPositionAndRotationInternal(inPosition, inRotation); + + // Notify broadphase of change + if (body.IsInBroadPhase()) + { + BodyID id = body.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); + } + + if (!body.IsStatic()) + { + body.SetLinearVelocityClamped(inLinearVelocity); + body.SetAngularVelocityClamped(inAngularVelocity); + + // Optionally activate body + if (!body.IsActive() && (!inLinearVelocity.IsNearZero() || !inAngularVelocity.IsNearZero())) + mBodyManager->ActivateBodies(&inBodyID, 1); + } + } +} + +void BodyInterface::SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + + // Deactivate if we're making the body static + if (body.IsActive() && inMotionType == EMotionType::Static) + mBodyManager->DeactivateBodies(&inBodyID, 1); + + body.SetMotionType(inMotionType); + + // Activate body if requested + if (inMotionType != EMotionType::Static && inActivationMode == EActivation::Activate) + ActivateBodyInternal(body); + } +} + +EBodyType BodyInterface::GetBodyType(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetBodyType(); + else + return EBodyType::RigidBody; +} + +EMotionType BodyInterface::GetMotionType(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetMotionType(); + else + return EMotionType::Static; +} + +void BodyInterface::SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + mBodyManager->SetMotionQuality(lock.GetBody(), inMotionQuality); +} + +EMotionQuality BodyInterface::GetMotionQuality(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && !lock.GetBody().IsStatic()) + return lock.GetBody().GetMotionProperties()->GetMotionQuality(); + else + return EMotionQuality::Discrete; +} + +Mat44 BodyInterface::GetInverseInertia(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetInverseInertia(); + else + return Mat44::sIdentity(); +} + +void BodyInterface::SetRestitution(const BodyID &inBodyID, float inRestitution) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetRestitution(inRestitution); +} + +float BodyInterface::GetRestitution(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetRestitution(); + else + return 0.0f; +} + +void BodyInterface::SetFriction(const BodyID &inBodyID, float inFriction) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetFriction(inFriction); +} + +float BodyInterface::GetFriction(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetFriction(); + else + return 0.0f; +} + +void BodyInterface::SetGravityFactor(const BodyID &inBodyID, float inGravityFactor) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + lock.GetBody().GetMotionPropertiesUnchecked()->SetGravityFactor(inGravityFactor); +} + +float BodyInterface::GetGravityFactor(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded() && lock.GetBody().GetMotionPropertiesUnchecked() != nullptr) + return lock.GetBody().GetMotionPropertiesUnchecked()->GetGravityFactor(); + else + return 1.0f; +} + +void BodyInterface::SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + { + Body &body = lock.GetBody(); + if (body.GetUseManifoldReduction() != inUseReduction) + { + body.SetUseManifoldReduction(inUseReduction); + + // Flag collision cache invalid for this body + mBodyManager->InvalidateContactCacheForBody(body); + } + } +} + +bool BodyInterface::GetUseManifoldReduction(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetUseManifoldReduction(); + else + return true; +} + +TransformedShape BodyInterface::GetTransformedShape(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetTransformedShape(); + else + return TransformedShape(); +} + +uint64 BodyInterface::GetUserData(const BodyID &inBodyID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetUserData(); + else + return 0; +} + +void BodyInterface::SetUserData(const BodyID &inBodyID, uint64 inUserData) const +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + lock.GetBody().SetUserData(inUserData); +} + +const PhysicsMaterial *BodyInterface::GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const +{ + BodyLockRead lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + return lock.GetBody().GetShape()->GetMaterial(inSubShapeID); + else + return PhysicsMaterial::sDefault; +} + +void BodyInterface::InvalidateContactCache(const BodyID &inBodyID) +{ + BodyLockWrite lock(*mBodyLockInterface, inBodyID); + if (lock.Succeeded()) + mBodyManager->InvalidateContactCacheForBody(lock.GetBody()); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyInterface.h b/WickedEngine/Jolt/Physics/Body/BodyInterface.h new file mode 100644 index 000000000..2b18245c0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyInterface.h @@ -0,0 +1,283 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class BodyCreationSettings; +class SoftBodyCreationSettings; +class BodyLockInterface; +class BroadPhase; +class BodyManager; +class TransformedShape; +class PhysicsMaterial; +class SubShapeID; +class Shape; +class TwoBodyConstraintSettings; +class TwoBodyConstraint; +class BroadPhaseLayerFilter; +class AABox; + +/// Class that provides operations on bodies using a body ID. Note that if you need to do multiple operations on a single body, it is more efficient to lock the body once and combine the operations. +/// All quantities are in world space unless otherwise specified. +class JPH_EXPORT BodyInterface : public NonCopyable +{ +public: + /// Initialize the interface (should only be called by PhysicsSystem) + void Init(BodyLockInterface &inBodyLockInterface, BodyManager &inBodyManager, BroadPhase &inBroadPhase) { mBodyLockInterface = &inBodyLockInterface; mBodyManager = &inBodyManager; mBroadPhase = &inBroadPhase; } + + /// Create a rigid body + /// @return Created body or null when out of bodies + Body * CreateBody(const BodyCreationSettings &inSettings); + + /// Create a soft body + /// @return Created body or null when out of bodies + Body * CreateSoftBody(const SoftBodyCreationSettings &inSettings); + + /// Create a rigid body with specified ID. This function can be used if a simulation is to run in sync between clients or if a simulation needs to be restored exactly. + /// The ID created on the server can be replicated to the client and used to create a deterministic simulation. + /// @return Created body or null when the body ID is invalid or a body of the same ID already exists. + Body * CreateBodyWithID(const BodyID &inBodyID, const BodyCreationSettings &inSettings); + + /// Create a soft body with specified ID. See comments at CreateBodyWithID. + Body * CreateSoftBodyWithID(const BodyID &inBodyID, const SoftBodyCreationSettings &inSettings); + + /// Advanced use only. Creates a rigid body without specifying an ID. This body cannot be added to the physics system until it has been assigned a body ID. + /// This can be used to decouple allocation from registering the body. A call to CreateBodyWithoutID followed by AssignBodyID is equivalent to calling CreateBodyWithID. + /// @return Created body + Body * CreateBodyWithoutID(const BodyCreationSettings &inSettings) const; + + /// Advanced use only. Creates a body without specifying an ID. See comments at CreateBodyWithoutID. + Body * CreateSoftBodyWithoutID(const SoftBodyCreationSettings &inSettings) const; + + /// Advanced use only. Destroy a body previously created with CreateBodyWithoutID that hasn't gotten an ID yet through the AssignBodyID function, + /// or a body that has had its body ID unassigned through UnassignBodyIDs. Bodies that have an ID should be destroyed through DestroyBody. + void DestroyBodyWithoutID(Body *inBody) const; + + /// Advanced use only. Assigns the next available body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system. + /// @return false if the body already has an ID or out of body ids. + bool AssignBodyID(Body *ioBody); + + /// Advanced use only. Assigns a body ID to a body that was created using CreateBodyWithoutID. After this call, the body can be added to the physics system. + /// @return false if the body already has an ID or if the ID is not valid. + bool AssignBodyID(Body *ioBody, const BodyID &inBodyID); + + /// Advanced use only. See UnassignBodyIDs. Unassigns the ID of a single body. + Body * UnassignBodyID(const BodyID &inBodyID); + + /// Advanced use only. Removes a number of body IDs from their bodies and returns the body pointers. Before calling this, the body should have been removed from the physics system. + /// The body can be destroyed through DestroyBodyWithoutID. This can be used to decouple deallocation. A call to UnassignBodyIDs followed by calls to DestroyBodyWithoutID is equivalent to calling DestroyBodies. + /// @param inBodyIDs A list of body IDs + /// @param inNumber Number of bodies in the list + /// @param outBodies If not null on input, this will contain a list of body pointers corresponding to inBodyIDs that can be destroyed afterwards (caller assumes ownership over these). + void UnassignBodyIDs(const BodyID *inBodyIDs, int inNumber, Body **outBodies); + + /// Destroy a body + void DestroyBody(const BodyID &inBodyID); + + /// Destroy multiple bodies + void DestroyBodies(const BodyID *inBodyIDs, int inNumber); + + /// Add body to the physics system. + /// Note that if you need to add multiple bodies, use the AddBodiesPrepare/AddBodiesFinalize function. + /// Adding many bodies, one at a time, results in a really inefficient broadphase until PhysicsSystem::OptimizeBroadPhase is called or when PhysicsSystem::Update rebuilds the tree! + /// After adding, to get a body by ID use the BodyLockRead or BodyLockWrite interface! + void AddBody(const BodyID &inBodyID, EActivation inActivationMode); + + /// Remove body from the physics system. + void RemoveBody(const BodyID &inBodyID); + + /// Check if a body has been added to the physics system. + bool IsAdded(const BodyID &inBodyID) const; + + /// Combines CreateBody and AddBody + /// @return Created body ID or an invalid ID when out of bodies + BodyID CreateAndAddBody(const BodyCreationSettings &inSettings, EActivation inActivationMode); + + /// Combines CreateSoftBody and AddBody + /// @return Created body ID or an invalid ID when out of bodies + BodyID CreateAndAddSoftBody(const SoftBodyCreationSettings &inSettings, EActivation inActivationMode); + + /// Broadphase add state handle, used to keep track of a batch while adding to the broadphase. + using AddState = void *; + + ///@name Batch adding interface, see Broadphase for further documentation. + /// Note that ioBodies array must be kept constant while the add is in progress. + ///@{ + AddState AddBodiesPrepare(BodyID *ioBodies, int inNumber); + void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState, EActivation inActivationMode); + void AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState); + void RemoveBodies(BodyID *ioBodies, int inNumber); + ///@} + + ///@name Activate / deactivate a body + ///@{ + void ActivateBody(const BodyID &inBodyID); + void ActivateBodies(const BodyID *inBodyIDs, int inNumber); + void ActivateBodiesInAABox(const AABox &inBox, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter); + void DeactivateBody(const BodyID &inBodyID); + void DeactivateBodies(const BodyID *inBodyIDs, int inNumber); + bool IsActive(const BodyID &inBodyID) const; + void ResetSleepTimer(const BodyID &inBodyID); + ///@} + + /// Create a two body constraint + TwoBodyConstraint * CreateConstraint(const TwoBodyConstraintSettings *inSettings, const BodyID &inBodyID1, const BodyID &inBodyID2); + + /// Activate non-static bodies attached to a constraint + void ActivateConstraint(const TwoBodyConstraint *inConstraint); + + ///@name Access to the shape of a body + ///@{ + + /// Get the current shape + RefConst GetShape(const BodyID &inBodyID) const; + + /// Set a new shape on the body + /// @param inBodyID Body ID of body that had its shape changed + /// @param inShape The new shape + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + /// @param inActivationMode Weather or not to activate the body + void SetShape(const BodyID &inBodyID, const Shape *inShape, bool inUpdateMassProperties, EActivation inActivationMode) const; + + /// Notify all systems to indicate that a shape has changed (usable for MutableCompoundShapes) + /// @param inBodyID Body ID of body that had its shape changed + /// @param inPreviousCenterOfMass Center of mass of the shape before the alterations + /// @param inUpdateMassProperties When true, the mass and inertia tensor is recalculated + /// @param inActivationMode Weather or not to activate the body + void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inPreviousCenterOfMass, bool inUpdateMassProperties, EActivation inActivationMode) const; + ///@} + + ///@name Object layer of a body + ///@{ + void SetObjectLayer(const BodyID &inBodyID, ObjectLayer inLayer); + ObjectLayer GetObjectLayer(const BodyID &inBodyID) const; + ///@} + + ///@name Position and rotation of a body + ///@{ + void SetPositionAndRotation(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); + void SetPositionAndRotationWhenChanged(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode); ///< Will only update the position/rotation and activate the body when the difference is larger than a very small number. This avoids updating the broadphase/waking up a body when the resulting position/orientation doesn't really change. + void GetPositionAndRotation(const BodyID &inBodyID, RVec3 &outPosition, Quat &outRotation) const; + void SetPosition(const BodyID &inBodyID, RVec3Arg inPosition, EActivation inActivationMode); + RVec3 GetPosition(const BodyID &inBodyID) const; + RVec3 GetCenterOfMassPosition(const BodyID &inBodyID) const; + void SetRotation(const BodyID &inBodyID, QuatArg inRotation, EActivation inActivationMode); + Quat GetRotation(const BodyID &inBodyID) const; + RMat44 GetWorldTransform(const BodyID &inBodyID) const; + RMat44 GetCenterOfMassTransform(const BodyID &inBodyID) const; + ///@} + + /// Set velocity of body such that it will be positioned at inTargetPosition/Rotation in inDeltaTime seconds (will activate body if needed) + void MoveKinematic(const BodyID &inBodyID, RVec3Arg inTargetPosition, QuatArg inTargetRotation, float inDeltaTime); + + /// Linear or angular velocity (functions will activate body if needed). + /// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$ + void SetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); + void GetLinearAndAngularVelocity(const BodyID &inBodyID, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const; + void SetLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); + Vec3 GetLinearVelocity(const BodyID &inBodyID) const; + void AddLinearVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity); ///< Add velocity to current velocity + void AddLinearAndAngularVelocity(const BodyID &inBodyID, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); ///< Add linear and angular to current velocities + void SetAngularVelocity(const BodyID &inBodyID, Vec3Arg inAngularVelocity); + Vec3 GetAngularVelocity(const BodyID &inBodyID) const; + Vec3 GetPointVelocity(const BodyID &inBodyID, RVec3Arg inPoint) const; ///< Velocity of point inPoint (in world space, e.g. on the surface of the body) of the body + + /// Set the complete motion state of a body. + /// Note that the linear velocity is the velocity of the center of mass, which may not coincide with the position of your object, to correct for this: \f$VelocityCOM = Velocity - AngularVelocity \times ShapeCOM\f$ + void SetPositionRotationAndVelocity(const BodyID &inBodyID, RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity); + + ///@name Add forces to the body + ///@{ + void AddForce(const BodyID &inBodyID, Vec3Arg inForce, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddForce + void AddForce(const BodyID &inBodyID, Vec3Arg inForce, RVec3Arg inPoint, EActivation inActivationMode = EActivation::Activate); ///< Applied at inPoint + void AddTorque(const BodyID &inBodyID, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< See Body::AddTorque + void AddForceAndTorque(const BodyID &inBodyID, Vec3Arg inForce, Vec3Arg inTorque, EActivation inActivationMode = EActivation::Activate); ///< A combination of Body::AddForce and Body::AddTorque + ///@} + + ///@name Add an impulse to the body + ///@{ + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse); ///< Applied at center of mass + void AddImpulse(const BodyID &inBodyID, Vec3Arg inImpulse, RVec3Arg inPoint); ///< Applied at inPoint + void AddAngularImpulse(const BodyID &inBodyID, Vec3Arg inAngularImpulse); + bool ApplyBuoyancyImpulse(const BodyID &inBodyID, RVec3Arg inSurfacePosition, Vec3Arg inSurfaceNormal, float inBuoyancy, float inLinearDrag, float inAngularDrag, Vec3Arg inFluidVelocity, Vec3Arg inGravity, float inDeltaTime); + ///@} + + ///@name Body type + ///@{ + EBodyType GetBodyType(const BodyID &inBodyID) const; + ///@} + + ///@name Body motion type + ///@{ + void SetMotionType(const BodyID &inBodyID, EMotionType inMotionType, EActivation inActivationMode); + EMotionType GetMotionType(const BodyID &inBodyID) const; + ///@} + + ///@name Body motion quality + ///@{ + void SetMotionQuality(const BodyID &inBodyID, EMotionQuality inMotionQuality); + EMotionQuality GetMotionQuality(const BodyID &inBodyID) const; + ///@} + + /// Get inverse inertia tensor in world space + Mat44 GetInverseInertia(const BodyID &inBodyID) const; + + ///@name Restitution + ///@{ + void SetRestitution(const BodyID &inBodyID, float inRestitution); + float GetRestitution(const BodyID &inBodyID) const; + ///@} + + ///@name Friction + ///@{ + void SetFriction(const BodyID &inBodyID, float inFriction); + float GetFriction(const BodyID &inBodyID) const; + ///@} + + ///@name Gravity factor + ///@{ + void SetGravityFactor(const BodyID &inBodyID, float inGravityFactor); + float GetGravityFactor(const BodyID &inBodyID) const; + ///@} + + ///@name Manifold reduction + ///@{ + void SetUseManifoldReduction(const BodyID &inBodyID, bool inUseReduction); + bool GetUseManifoldReduction(const BodyID &inBodyID) const; + ///@} + + /// Get transform and shape for this body, used to perform collision detection + TransformedShape GetTransformedShape(const BodyID &inBodyID) const; + + /// Get the user data for a body + uint64 GetUserData(const BodyID &inBodyID) const; + void SetUserData(const BodyID &inBodyID, uint64 inUserData) const; + + /// Get the material for a particular sub shape + const PhysicsMaterial * GetMaterial(const BodyID &inBodyID, const SubShapeID &inSubShapeID) const; + + /// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step. + void InvalidateContactCache(const BodyID &inBodyID); + +private: + /// Helper function to activate a single body + JPH_INLINE void ActivateBodyInternal(Body &ioBody) const; + + BodyLockInterface * mBodyLockInterface = nullptr; + BodyManager * mBodyManager = nullptr; + BroadPhase * mBroadPhase = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyLock.h b/WickedEngine/Jolt/Physics/Body/BodyLock.h new file mode 100644 index 000000000..eccfec4d1 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyLock.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Base class for locking bodies for the duration of the scope of this class (do not use directly) +template +class BodyLockBase : public NonCopyable +{ +public: + /// Constructor will lock the body + BodyLockBase(const BodyLockInterface &inBodyLockInterface, const BodyID &inBodyID) : + mBodyLockInterface(inBodyLockInterface) + { + if (inBodyID == BodyID()) + { + // Invalid body id + mBodyLockMutex = nullptr; + mBody = nullptr; + } + else + { + // Get mutex + mBodyLockMutex = Write? inBodyLockInterface.LockWrite(inBodyID) : inBodyLockInterface.LockRead(inBodyID); + + // Get a reference to the body or nullptr when it is no longer valid + mBody = inBodyLockInterface.TryGetBody(inBodyID); + } + } + + /// Explicitly release the lock (normally this is done in the destructor) + inline void ReleaseLock() + { + if (mBodyLockMutex != nullptr) + { + if (Write) + mBodyLockInterface.UnlockWrite(mBodyLockMutex); + else + mBodyLockInterface.UnlockRead(mBodyLockMutex); + + mBodyLockMutex = nullptr; + mBody = nullptr; + } + } + + /// Destructor will unlock the body + ~BodyLockBase() + { + ReleaseLock(); + } + + /// Test if the lock was successful (if the body ID was valid) + inline bool Succeeded() const + { + return mBody != nullptr; + } + + /// Test if the lock was successful (if the body ID was valid) and the body is still in the broad phase + inline bool SucceededAndIsInBroadPhase() const + { + return mBody != nullptr && mBody->IsInBroadPhase(); + } + + /// Access the body + inline BodyType & GetBody() const + { + JPH_ASSERT(mBody != nullptr, "Should check Succeeded() first"); + return *mBody; + } + +private: + const BodyLockInterface & mBodyLockInterface; + SharedMutex * mBodyLockMutex; + BodyType * mBody; +}; + +/// A body lock takes a body ID and locks the underlying body so that other threads cannot access its members +/// +/// The common usage pattern is: +/// +/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken +/// BodyID body_id = ...; // Obtain ID to body +/// +/// // Scoped lock +/// { +/// BodyLockRead lock(lock_interface, body_id); +/// if (lock.Succeeded()) // body_id may no longer be valid +/// { +/// const Body &body = lock.GetBody(); +/// +/// // Do something with body +/// ... +/// } +/// } +class BodyLockRead : public BodyLockBase +{ + using BodyLockBase::BodyLockBase; +}; + +/// Specialization that locks a body for writing to. @see BodyLockRead for usage patterns. +class BodyLockWrite : public BodyLockBase +{ + using BodyLockBase::BodyLockBase; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyLockInterface.h b/WickedEngine/Jolt/Physics/Body/BodyLockInterface.h new file mode 100644 index 000000000..b65951fc8 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyLockInterface.h @@ -0,0 +1,134 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Base class interface for locking a body. Usually you will use BodyLockRead / BodyLockWrite / BodyLockMultiRead / BodyLockMultiWrite instead. +class BodyLockInterface : public NonCopyable +{ +public: + /// Redefine MutexMask + using MutexMask = BodyManager::MutexMask; + + /// Constructor + explicit BodyLockInterface(BodyManager &inBodyManager) : mBodyManager(inBodyManager) { } + virtual ~BodyLockInterface() = default; + + ///@name Locking functions + ///@{ + virtual SharedMutex * LockRead(const BodyID &inBodyID) const = 0; + virtual void UnlockRead(SharedMutex *inMutex) const = 0; + virtual SharedMutex * LockWrite(const BodyID &inBodyID) const = 0; + virtual void UnlockWrite(SharedMutex *inMutex) const = 0; + ///@} + + /// Get the mask needed to lock all bodies + inline MutexMask GetAllBodiesMutexMask() const + { + return mBodyManager.GetAllBodiesMutexMask(); + } + + ///@name Batch locking functions + ///@{ + virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const = 0; + virtual void LockRead(MutexMask inMutexMask) const = 0; + virtual void UnlockRead(MutexMask inMutexMask) const = 0; + virtual void LockWrite(MutexMask inMutexMask) const = 0; + virtual void UnlockWrite(MutexMask inMutexMask) const = 0; + ///@} + + /// Convert body ID to body + inline Body * TryGetBody(const BodyID &inBodyID) const { return mBodyManager.TryGetBody(inBodyID); } + +protected: + BodyManager & mBodyManager; +}; + +/// Implementation that performs no locking (assumes the lock has already been taken) +class BodyLockInterfaceNoLock final : public BodyLockInterface +{ +public: + using BodyLockInterface::BodyLockInterface; + + ///@name Locking functions + virtual SharedMutex * LockRead([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; } + virtual void UnlockRead([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ } + virtual SharedMutex * LockWrite([[maybe_unused]] const BodyID &inBodyID) const override { return nullptr; } + virtual void UnlockWrite([[maybe_unused]] SharedMutex *inMutex) const override { /* Nothing to do */ } + + ///@name Batch locking functions + virtual MutexMask GetMutexMask([[maybe_unused]] const BodyID *inBodies, [[maybe_unused]] int inNumber) const override { return 0; } + virtual void LockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void UnlockRead([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void LockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } + virtual void UnlockWrite([[maybe_unused]] MutexMask inMutexMask) const override { /* Nothing to do */ } +}; + +/// Implementation that uses the body manager to lock the correct mutex for a body +class BodyLockInterfaceLocking final : public BodyLockInterface +{ +public: + using BodyLockInterface::BodyLockInterface; + + ///@name Locking functions + virtual SharedMutex * LockRead(const BodyID &inBodyID) const override + { + SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID); + PhysicsLock::sLockShared(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + return &mutex; + } + + virtual void UnlockRead(SharedMutex *inMutex) const override + { + PhysicsLock::sUnlockShared(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + } + + virtual SharedMutex * LockWrite(const BodyID &inBodyID) const override + { + SharedMutex &mutex = mBodyManager.GetMutexForBody(inBodyID); + PhysicsLock::sLock(mutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + return &mutex; + } + + virtual void UnlockWrite(SharedMutex *inMutex) const override + { + PhysicsLock::sUnlock(*inMutex JPH_IF_ENABLE_ASSERTS(, &mBodyManager, EPhysicsLockTypes::PerBody)); + } + + ///@name Batch locking functions + virtual MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const override + { + return mBodyManager.GetMutexMask(inBodies, inNumber); + } + + virtual void LockRead(MutexMask inMutexMask) const override + { + mBodyManager.LockRead(inMutexMask); + } + + virtual void UnlockRead(MutexMask inMutexMask) const override + { + mBodyManager.UnlockRead(inMutexMask); + } + + virtual void LockWrite(MutexMask inMutexMask) const override + { + mBodyManager.LockWrite(inMutexMask); + } + + virtual void UnlockWrite(MutexMask inMutexMask) const override + { + mBodyManager.UnlockWrite(inMutexMask); + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyLockMulti.h b/WickedEngine/Jolt/Physics/Body/BodyLockMulti.h new file mode 100644 index 000000000..5872729c0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyLockMulti.h @@ -0,0 +1,104 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Base class for locking multiple bodies for the duration of the scope of this class (do not use directly) +template +class BodyLockMultiBase : public NonCopyable +{ +public: + /// Redefine MutexMask + using MutexMask = BodyLockInterface::MutexMask; + + /// Constructor will lock the bodies + BodyLockMultiBase(const BodyLockInterface &inBodyLockInterface, const BodyID *inBodyIDs, int inNumber) : + mBodyLockInterface(inBodyLockInterface), + mMutexMask(inBodyLockInterface.GetMutexMask(inBodyIDs, inNumber)), + mBodyIDs(inBodyIDs), + mNumBodyIDs(inNumber) + { + if (mMutexMask != 0) + { + // Get mutex + if (Write) + inBodyLockInterface.LockWrite(mMutexMask); + else + inBodyLockInterface.LockRead(mMutexMask); + } + } + + /// Destructor will unlock the bodies + ~BodyLockMultiBase() + { + if (mMutexMask != 0) + { + if (Write) + mBodyLockInterface.UnlockWrite(mMutexMask); + else + mBodyLockInterface.UnlockRead(mMutexMask); + } + } + + /// Access the body (returns null if body was not properly locked) + inline BodyType * GetBody(int inBodyIndex) const + { + // Range check + JPH_ASSERT(inBodyIndex >= 0 && inBodyIndex < mNumBodyIDs); + + // Get body ID + const BodyID &body_id = mBodyIDs[inBodyIndex]; + if (body_id.IsInvalid()) + return nullptr; + + // Get a reference to the body or nullptr when it is no longer valid + return mBodyLockInterface.TryGetBody(body_id); + } + +private: + const BodyLockInterface & mBodyLockInterface; + MutexMask mMutexMask; + const BodyID * mBodyIDs; + int mNumBodyIDs; +}; + +/// A multi body lock takes a number of body IDs and locks the underlying bodies so that other threads cannot access its members +/// +/// The common usage pattern is: +/// +/// BodyLockInterface lock_interface = physics_system.GetBodyLockInterface(); // Or non-locking interface if the lock is already taken +/// const BodyID *body_id = ...; // Obtain IDs to bodies +/// int num_body_ids = ...; +/// +/// // Scoped lock +/// { +/// BodyLockMultiRead lock(lock_interface, body_ids, num_body_ids); +/// for (int i = 0; i < num_body_ids; ++i) +/// { +/// const Body *body = lock.GetBody(i); +/// if (body != nullptr) +/// { +/// const Body &body = lock.Body(); +/// +/// // Do something with body +/// ... +/// } +/// } +/// } +class BodyLockMultiRead : public BodyLockMultiBase +{ + using BodyLockMultiBase::BodyLockMultiBase; +}; + +/// Specialization that locks multiple bodies for writing to. @see BodyLockMultiRead for usage patterns. +class BodyLockMultiWrite : public BodyLockMultiBase +{ + using BodyLockMultiBase::BodyLockMultiBase; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyManager.cpp b/WickedEngine/Jolt/Physics/Body/BodyManager.cpp new file mode 100644 index 000000000..c65f5b6bf --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyManager.cpp @@ -0,0 +1,1152 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_ENABLE_ASSERTS + static thread_local bool sOverrideAllowActivation = false; + static thread_local bool sOverrideAllowDeactivation = false; + + bool BodyManager::sGetOverrideAllowActivation() + { + return sOverrideAllowActivation; + } + + void BodyManager::sSetOverrideAllowActivation(bool inValue) + { + sOverrideAllowActivation = inValue; + } + + bool BodyManager::sGetOverrideAllowDeactivation() + { + return sOverrideAllowDeactivation; + } + + void BodyManager::sSetOverrideAllowDeactivation(bool inValue) + { + sOverrideAllowDeactivation = inValue; + } +#endif + +// Helper class that combines a body and its motion properties +class BodyWithMotionProperties : public Body +{ +public: + JPH_OVERRIDE_NEW_DELETE + + MotionProperties mMotionProperties; +}; + +// Helper class that combines a soft body its motion properties and shape +class SoftBodyWithMotionPropertiesAndShape : public Body +{ +public: + SoftBodyWithMotionPropertiesAndShape() + { + mShape.SetEmbedded(); + } + + SoftBodyMotionProperties mMotionProperties; + SoftBodyShape mShape; +}; + +inline void BodyManager::sDeleteBody(Body *inBody) +{ + if (inBody->mMotionProperties != nullptr) + { + JPH_IF_ENABLE_ASSERTS(inBody->mMotionProperties = nullptr;) + if (inBody->IsSoftBody()) + { + inBody->mShape = nullptr; // Release the shape to avoid assertion on shape destruction because of embedded object with refcount > 0 + delete static_cast(inBody); + } + else + delete static_cast(inBody); + } + else + delete inBody; +} + +BodyManager::~BodyManager() +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Destroy any bodies that are still alive + for (Body *b : mBodies) + if (sIsValidBodyPointer(b)) + sDeleteBody(b); + + for (BodyID *active_bodies : mActiveBodies) + delete [] active_bodies; +} + +void BodyManager::Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface) +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Num body mutexes must be a power of two and not bigger than our MutexMask + uint num_body_mutexes = Clamp(GetNextPowerOf2(inNumBodyMutexes == 0? 2 * thread::hardware_concurrency() : inNumBodyMutexes), 1, sizeof(MutexMask) * 8); + + // Allocate the body mutexes + mBodyMutexes.Init(num_body_mutexes); + + // Allocate space for bodies + mBodies.reserve(inMaxBodies); + + // Allocate space for active bodies + for (BodyID *&active_bodies : mActiveBodies) + { + JPH_ASSERT(active_bodies == nullptr); + active_bodies = new BodyID [inMaxBodies]; + } + + // Allocate space for sequence numbers + mBodySequenceNumbers.resize(inMaxBodies, 0); + + // Keep layer interface + mBroadPhaseLayerInterface = &inLayerInterface; +} + +uint BodyManager::GetNumBodies() const +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + return mNumBodies; +} + +BodyManager::BodyStats BodyManager::GetBodyStats() const +{ + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + BodyStats stats; + stats.mNumBodies = mNumBodies; + stats.mMaxBodies = uint(mBodies.capacity()); + + for (const Body *body : mBodies) + if (sIsValidBodyPointer(body)) + { + if (body->IsSoftBody()) + { + stats.mNumSoftBodies++; + if (body->IsActive()) + stats.mNumActiveSoftBodies++; + } + else + { + switch (body->GetMotionType()) + { + case EMotionType::Static: + stats.mNumBodiesStatic++; + break; + + case EMotionType::Dynamic: + stats.mNumBodiesDynamic++; + if (body->IsActive()) + stats.mNumActiveBodiesDynamic++; + break; + + case EMotionType::Kinematic: + stats.mNumBodiesKinematic++; + if (body->IsActive()) + stats.mNumActiveBodiesKinematic++; + break; + } + } + } + + return stats; +} + +Body *BodyManager::AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const +{ + // Fill in basic properties + Body *body; + if (inBodyCreationSettings.HasMassProperties()) + { + BodyWithMotionProperties *bmp = new BodyWithMotionProperties; + body = bmp; + body->mMotionProperties = &bmp->mMotionProperties; + } + else + { + body = new Body; + } + body->mBodyType = EBodyType::RigidBody; + body->mShape = inBodyCreationSettings.GetShape(); + body->mUserData = inBodyCreationSettings.mUserData; + body->SetFriction(inBodyCreationSettings.mFriction); + body->SetRestitution(inBodyCreationSettings.mRestitution); + body->mMotionType = inBodyCreationSettings.mMotionType; + if (inBodyCreationSettings.mIsSensor) + body->SetIsSensor(true); + if (inBodyCreationSettings.mCollideKinematicVsNonDynamic) + body->SetCollideKinematicVsNonDynamic(true); + if (inBodyCreationSettings.mUseManifoldReduction) + body->SetUseManifoldReduction(true); + if (inBodyCreationSettings.mApplyGyroscopicForce) + body->SetApplyGyroscopicForce(true); + if (inBodyCreationSettings.mEnhancedInternalEdgeRemoval) + body->SetEnhancedInternalEdgeRemoval(true); + SetBodyObjectLayerInternal(*body, inBodyCreationSettings.mObjectLayer); + body->mObjectLayer = inBodyCreationSettings.mObjectLayer; + body->mCollisionGroup = inBodyCreationSettings.mCollisionGroup; + + if (inBodyCreationSettings.HasMassProperties()) + { + MotionProperties *mp = body->mMotionProperties; + mp->SetLinearDamping(inBodyCreationSettings.mLinearDamping); + mp->SetAngularDamping(inBodyCreationSettings.mAngularDamping); + mp->SetMaxLinearVelocity(inBodyCreationSettings.mMaxLinearVelocity); + mp->SetMaxAngularVelocity(inBodyCreationSettings.mMaxAngularVelocity); + mp->SetMassProperties(inBodyCreationSettings.mAllowedDOFs, inBodyCreationSettings.GetMassProperties()); + mp->SetLinearVelocity(inBodyCreationSettings.mLinearVelocity); // Needs to happen after setting the max linear/angular velocity and setting allowed DOFs + mp->SetAngularVelocity(inBodyCreationSettings.mAngularVelocity); + mp->SetGravityFactor(inBodyCreationSettings.mGravityFactor); + mp->SetNumVelocityStepsOverride(inBodyCreationSettings.mNumVelocityStepsOverride); + mp->SetNumPositionStepsOverride(inBodyCreationSettings.mNumPositionStepsOverride); + mp->mMotionQuality = inBodyCreationSettings.mMotionQuality; + mp->mAllowSleeping = inBodyCreationSettings.mAllowSleeping; + JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;) + JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;) + } + + // Position body + body->SetPositionAndRotationInternal(inBodyCreationSettings.mPosition, inBodyCreationSettings.mRotation); + + return body; +} + +/// Create a soft body using creation settings. The returned body will not be part of the body manager yet. +Body *BodyManager::AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const +{ + // Fill in basic properties + SoftBodyWithMotionPropertiesAndShape *bmp = new SoftBodyWithMotionPropertiesAndShape; + SoftBodyMotionProperties *mp = &bmp->mMotionProperties; + SoftBodyShape *shape = &bmp->mShape; + Body *body = bmp; + shape->mSoftBodyMotionProperties = mp; + body->mBodyType = EBodyType::SoftBody; + body->mMotionProperties = mp; + body->mShape = shape; + body->mUserData = inSoftBodyCreationSettings.mUserData; + body->SetFriction(inSoftBodyCreationSettings.mFriction); + body->SetRestitution(inSoftBodyCreationSettings.mRestitution); + body->mMotionType = EMotionType::Dynamic; + SetBodyObjectLayerInternal(*body, inSoftBodyCreationSettings.mObjectLayer); + body->mObjectLayer = inSoftBodyCreationSettings.mObjectLayer; + body->mCollisionGroup = inSoftBodyCreationSettings.mCollisionGroup; + mp->SetLinearDamping(inSoftBodyCreationSettings.mLinearDamping); + mp->SetAngularDamping(0); + mp->SetMaxLinearVelocity(inSoftBodyCreationSettings.mMaxLinearVelocity); + mp->SetMaxAngularVelocity(FLT_MAX); + mp->SetLinearVelocity(Vec3::sZero()); + mp->SetAngularVelocity(Vec3::sZero()); + mp->SetGravityFactor(inSoftBodyCreationSettings.mGravityFactor); + mp->mMotionQuality = EMotionQuality::Discrete; + mp->mAllowSleeping = inSoftBodyCreationSettings.mAllowSleeping; + JPH_IF_ENABLE_ASSERTS(mp->mCachedBodyType = body->mBodyType;) + JPH_IF_ENABLE_ASSERTS(mp->mCachedMotionType = body->mMotionType;) + mp->Initialize(inSoftBodyCreationSettings); + + body->SetPositionAndRotationInternal(inSoftBodyCreationSettings.mPosition, inSoftBodyCreationSettings.mMakeRotationIdentity? Quat::sIdentity() : inSoftBodyCreationSettings.mRotation); + + return body; +} + +void BodyManager::FreeBody(Body *inBody) const +{ + JPH_ASSERT(inBody->GetID().IsInvalid(), "This function should only be called on a body that doesn't have an ID yet, use DestroyBody otherwise"); + + sDeleteBody(inBody); +} + +bool BodyManager::AddBody(Body *ioBody) +{ + // Return error when body was already added + if (!ioBody->GetID().IsInvalid()) + return false; + + // Determine next free index + uint32 idx; + { + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + if (mBodyIDFreeListStart != cBodyIDFreeListEnd) + { + // Pop an item from the freelist + JPH_ASSERT(mBodyIDFreeListStart & cIsFreedBody); + idx = uint32(mBodyIDFreeListStart >> cFreedBodyIndexShift); + JPH_ASSERT(!sIsValidBodyPointer(mBodies[idx])); + mBodyIDFreeListStart = uintptr_t(mBodies[idx]); + mBodies[idx] = ioBody; + } + else + { + if (mBodies.size() < mBodies.capacity()) + { + // Allocate a new entry, note that the array should not actually resize since we've reserved it at init time + idx = uint32(mBodies.size()); + mBodies.push_back(ioBody); + } + else + { + // Out of bodies + return false; + } + } + + // Update cached number of bodies + mNumBodies++; + } + + // Get next sequence number and assign the ID + uint8 seq_no = GetNextSequenceNumber(idx); + ioBody->mID = BodyID(idx, seq_no); + return true; +} + +bool BodyManager::AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID) +{ + // Return error when body was already added + if (!ioBody->GetID().IsInvalid()) + return false; + + { + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Check if index is beyond the max body ID + uint32 idx = inBodyID.GetIndex(); + if (idx >= mBodies.capacity()) + return false; // Return error + + if (idx < mBodies.size()) + { + // Body array entry has already been allocated, check if there's a free body here + if (sIsValidBodyPointer(mBodies[idx])) + return false; // Return error + + // Remove the entry from the freelist + uintptr_t idx_start = mBodyIDFreeListStart >> cFreedBodyIndexShift; + if (idx == idx_start) + { + // First entry, easy to remove, the start of the list is our next + mBodyIDFreeListStart = uintptr_t(mBodies[idx]); + } + else + { + // Loop over the freelist and find the entry in the freelist pointing to our index + // TODO: This is O(N), see if this becomes a performance problem (don't want to put the freed bodies in a double linked list) + uintptr_t cur, next; + for (cur = idx_start; cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift; cur = next) + { + next = uintptr_t(mBodies[cur]) >> cFreedBodyIndexShift; + if (next == idx) + { + mBodies[cur] = mBodies[idx]; + break; + } + } + JPH_ASSERT(cur != cBodyIDFreeListEnd >> cFreedBodyIndexShift); + } + + // Put the body in the slot + mBodies[idx] = ioBody; + } + else + { + // Ensure that all body IDs up to this body ID have been allocated and added to the free list + while (idx > mBodies.size()) + { + // Push the id onto the freelist + mBodies.push_back((Body *)mBodyIDFreeListStart); + mBodyIDFreeListStart = (uintptr_t(mBodies.size() - 1) << cFreedBodyIndexShift) | cIsFreedBody; + } + + // Add the element to the list + mBodies.push_back(ioBody); + } + + // Update cached number of bodies + mNumBodies++; + } + + // Assign the ID + ioBody->mID = inBodyID; + return true; +} + +Body *BodyManager::RemoveBodyInternal(const BodyID &inBodyID) +{ + // Get body + uint32 idx = inBodyID.GetIndex(); + Body *body = mBodies[idx]; + + // Validate that it can be removed + JPH_ASSERT(body->GetID() == inBodyID); + JPH_ASSERT(!body->IsActive()); + JPH_ASSERT(!body->IsInBroadPhase()); + + // Push the id onto the freelist + mBodies[idx] = (Body *)mBodyIDFreeListStart; + mBodyIDFreeListStart = (uintptr_t(idx) << cFreedBodyIndexShift) | cIsFreedBody; + + return body; +} + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + +void BodyManager::ValidateFreeList() const +{ + // Check that the freelist is correct + size_t num_freed = 0; + for (uintptr_t start = mBodyIDFreeListStart; start != cBodyIDFreeListEnd; start = uintptr_t(mBodies[start >> cFreedBodyIndexShift])) + { + JPH_ASSERT(start & cIsFreedBody); + num_freed++; + } + JPH_ASSERT(mNumBodies == mBodies.size() - num_freed); +} + +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) + +void BodyManager::RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies) +{ + // Don't take lock if no bodies are to be destroyed + if (inNumber <= 0) + return; + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Update cached number of bodies + JPH_ASSERT(mNumBodies >= (uint)inNumber); + mNumBodies -= inNumber; + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + { + // Remove body + Body *body = RemoveBodyInternal(*b); + + // Clear the ID + body->mID = BodyID(); + + // Return the body to the caller + if (outBodies != nullptr) + { + *outBodies = body; + ++outBodies; + } + } + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + ValidateFreeList(); +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) +} + +void BodyManager::DestroyBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be destroyed + if (inNumber <= 0) + return; + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Update cached number of bodies + JPH_ASSERT(mNumBodies >= (uint)inNumber); + mNumBodies -= inNumber; + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + { + // Remove body + Body *body = RemoveBodyInternal(*b); + + // Free the body + sDeleteBody(body); + } + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + ValidateFreeList(); +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) +} + +void BodyManager::AddBodyToActiveBodies(Body &ioBody) +{ + // Select the correct array to use + int type = (int)ioBody.GetBodyType(); + atomic &num_active_bodies = mNumActiveBodies[type]; + BodyID *active_bodies = mActiveBodies[type]; + + MotionProperties *mp = ioBody.mMotionProperties; + mp->mIndexInActiveBodies = num_active_bodies; + JPH_ASSERT(num_active_bodies < GetMaxBodies()); + active_bodies[num_active_bodies] = ioBody.GetID(); + num_active_bodies++; // Increment atomic after setting the body ID so that PhysicsSystem::JobFindCollisions (which doesn't lock the mActiveBodiesMutex) will only read valid IDs + + // Count CCD bodies + if (mp->GetMotionQuality() == EMotionQuality::LinearCast) + mNumActiveCCDBodies++; +} + +void BodyManager::RemoveBodyFromActiveBodies(Body &ioBody) +{ + // Select the correct array to use + int type = (int)ioBody.GetBodyType(); + atomic &num_active_bodies = mNumActiveBodies[type]; + BodyID *active_bodies = mActiveBodies[type]; + + uint32 last_body_index = num_active_bodies - 1; + MotionProperties *mp = ioBody.mMotionProperties; + if (mp->mIndexInActiveBodies != last_body_index) + { + // This is not the last body, use the last body to fill the hole + BodyID last_body_id = active_bodies[last_body_index]; + active_bodies[mp->mIndexInActiveBodies] = last_body_id; + + // Update that body's index in the active list + Body &last_body = *mBodies[last_body_id.GetIndex()]; + JPH_ASSERT(last_body.mMotionProperties->mIndexInActiveBodies == last_body_index); + last_body.mMotionProperties->mIndexInActiveBodies = mp->mIndexInActiveBodies; + } + + // Mark this body as no longer active + mp->mIndexInActiveBodies = Body::cInactiveIndex; + + // Remove unused element from active bodies list + --num_active_bodies; + + // Count CCD bodies + if (mp->GetMotionQuality() == EMotionQuality::LinearCast) + mNumActiveCCDBodies--; +} + +void BodyManager::ActivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be activated + if (inNumber <= 0) + return; + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation); + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + if (!b->IsInvalid()) + { + BodyID body_id = *b; + Body &body = *mBodies[body_id.GetIndex()]; + + JPH_ASSERT(body.GetID() == body_id); + JPH_ASSERT(body.IsInBroadPhase()); + + if (!body.IsStatic()) + { + // Reset sleeping timer so that we don't immediately go to sleep again + body.ResetSleepTimer(); + + // Check if we're sleeping + if (body.mMotionProperties->mIndexInActiveBodies == Body::cInactiveIndex) + { + AddBodyToActiveBodies(body); + + // Call activation listener + if (mActivationListener != nullptr) + mActivationListener->OnBodyActivated(body_id, body.GetUserData()); + } + } + } +} + +void BodyManager::DeactivateBodies(const BodyID *inBodyIDs, int inNumber) +{ + // Don't take lock if no bodies are to be deactivated + if (inNumber <= 0) + return; + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowDeactivation); + + for (const BodyID *b = inBodyIDs, *b_end = inBodyIDs + inNumber; b < b_end; b++) + if (!b->IsInvalid()) + { + BodyID body_id = *b; + Body &body = *mBodies[body_id.GetIndex()]; + + JPH_ASSERT(body.GetID() == body_id); + JPH_ASSERT(body.IsInBroadPhase()); + + if (body.mMotionProperties != nullptr + && body.mMotionProperties->mIndexInActiveBodies != Body::cInactiveIndex) + { + // Remove the body from the active bodies list + RemoveBodyFromActiveBodies(body); + + // Mark this body as no longer active + body.mMotionProperties->mIslandIndex = Body::cInactiveIndex; + + // Reset velocity + body.mMotionProperties->mLinearVelocity = Vec3::sZero(); + body.mMotionProperties->mAngularVelocity = Vec3::sZero(); + + // Call activation listener + if (mActivationListener != nullptr) + mActivationListener->OnBodyDeactivated(body_id, body.GetUserData()); + } + } +} + +void BodyManager::SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality) +{ + MotionProperties *mp = ioBody.GetMotionPropertiesUnchecked(); + if (mp != nullptr && mp->GetMotionQuality() != inMotionQuality) + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked); + + bool is_active = ioBody.IsActive(); + if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast) + --mNumActiveCCDBodies; + + mp->mMotionQuality = inMotionQuality; + + if (is_active && mp->GetMotionQuality() == EMotionQuality::LinearCast) + ++mNumActiveCCDBodies; + } +} + +void BodyManager::GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + const BodyID *active_bodies = mActiveBodies[(int)inType]; + outBodyIDs.assign(active_bodies, active_bodies + mNumActiveBodies[(int)inType]); +} + +void BodyManager::GetBodyIDs(BodyIDVector &outBodies) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + // Reserve space for all bodies + outBodies.clear(); + outBodies.reserve(mNumBodies); + + // Iterate the list and find the bodies that are not null + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b)) + outBodies.push_back(b->GetID()); + + // Validate that our reservation was correct + JPH_ASSERT(outBodies.size() == mNumBodies); +} + +void BodyManager::SetBodyActivationListener(BodyActivationListener *inListener) +{ + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + mActivationListener = inListener; +} + +BodyManager::MutexMask BodyManager::GetMutexMask(const BodyID *inBodies, int inNumber) const +{ + JPH_ASSERT(sizeof(MutexMask) * 8 >= mBodyMutexes.GetNumMutexes(), "MutexMask must have enough bits"); + + if (inNumber >= (int)mBodyMutexes.GetNumMutexes()) + { + // Just lock everything if there are too many bodies + return GetAllBodiesMutexMask(); + } + else + { + MutexMask mask = 0; + for (const BodyID *b = inBodies, *b_end = inBodies + inNumber; b < b_end; ++b) + if (!b->IsInvalid()) + { + uint32 index = mBodyMutexes.GetMutexIndex(b->GetIndex()); + mask |= (MutexMask(1) << index); + } + return mask; + } +} + +void BodyManager::LockRead(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).lock_shared(); +} + +void BodyManager::UnlockRead(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).unlock_shared(); +} + +void BodyManager::LockWrite(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).lock(); +} + +void BodyManager::UnlockWrite(MutexMask inMutexMask) const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + + int index = 0; + for (MutexMask mask = inMutexMask; mask != 0; mask >>= 1, index++) + if (mask & 1) + mBodyMutexes.GetMutexByIndex(index).unlock(); +} + +void BodyManager::LockAllBodies() const +{ + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckLock(this, EPhysicsLockTypes::PerBody)); + mBodyMutexes.LockAll(); + + PhysicsLock::sLock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); +} + +void BodyManager::UnlockAllBodies() const +{ + PhysicsLock::sUnlock(mBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::BodiesList)); + + JPH_IF_ENABLE_ASSERTS(PhysicsLock::sCheckUnlock(this, EPhysicsLockTypes::PerBody)); + mBodyMutexes.UnlockAll(); +} + +void BodyManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + { + LockAllBodies(); + + // Determine which bodies to save + Array bodies; + bodies.reserve(mNumBodies); + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase() && (inFilter == nullptr || inFilter->ShouldSaveBody(*b))) + bodies.push_back(b); + + // Write state of bodies + uint32 num_bodies = (uint32)bodies.size(); + inStream.Write(num_bodies); + for (const Body *b : bodies) + { + inStream.Write(b->GetID()); + inStream.Write(b->IsActive()); + b->SaveState(inStream); + } + + UnlockAllBodies(); + } +} + +bool BodyManager::RestoreState(StateRecorder &inStream) +{ + BodyIDVector bodies_to_activate, bodies_to_deactivate; + + { + LockAllBodies(); + + if (inStream.IsValidating()) + { + // Read state of bodies, note this reads it in a way to be consistent with validation + uint32 old_num_bodies = 0; + for (const Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase()) + ++old_num_bodies; + uint32 num_bodies = old_num_bodies; // Initialize to current value for validation + inStream.Read(num_bodies); + if (num_bodies != old_num_bodies) + { + JPH_ASSERT(false, "Cannot handle adding/removing bodies"); + UnlockAllBodies(); + return false; + } + + for (Body *b : mBodies) + if (sIsValidBodyPointer(b) && b->IsInBroadPhase()) + { + BodyID body_id = b->GetID(); // Initialize to current value for validation + inStream.Read(body_id); + if (body_id != b->GetID()) + { + JPH_ASSERT(false, "Cannot handle adding/removing bodies"); + UnlockAllBodies(); + return false; + } + bool is_active = b->IsActive(); // Initialize to current value for validation + inStream.Read(is_active); + if (is_active != b->IsActive()) + { + if (is_active) + bodies_to_activate.push_back(body_id); + else + bodies_to_deactivate.push_back(body_id); + } + b->RestoreState(inStream); + } + } + else + { + // Not validating, we can be a bit more loose, read number of bodies + uint32 num_bodies = 0; + inStream.Read(num_bodies); + + // Iterate over the stored bodies and restore their state + for (uint32 idx = 0; idx < num_bodies; ++idx) + { + BodyID body_id; + inStream.Read(body_id); + Body *b = TryGetBody(body_id); + if (b == nullptr) + { + JPH_ASSERT(false, "Restoring state for non-existing body"); + UnlockAllBodies(); + return false; + } + bool is_active; + inStream.Read(is_active); + if (is_active != b->IsActive()) + { + if (is_active) + bodies_to_activate.push_back(body_id); + else + bodies_to_deactivate.push_back(body_id); + } + b->RestoreState(inStream); + } + } + + UnlockAllBodies(); + } + + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + for (BodyID body_id : bodies_to_activate) + { + Body *body = TryGetBody(body_id); + AddBodyToActiveBodies(*body); + } + + for (BodyID body_id : bodies_to_deactivate) + { + Body *body = TryGetBody(body_id); + RemoveBodyFromActiveBodies(*body); + } + } + + return true; +} + +void BodyManager::SaveBodyState(const Body &inBody, StateRecorder &inStream) const +{ + inStream.Write(inBody.IsActive()); + + inBody.SaveState(inStream); +} + +void BodyManager::RestoreBodyState(Body &ioBody, StateRecorder &inStream) +{ + bool is_active = ioBody.IsActive(); + inStream.Read(is_active); + + ioBody.RestoreState(inStream); + + if (is_active != ioBody.IsActive()) + { + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + JPH_ASSERT(!mActiveBodiesLocked || sOverrideAllowActivation); + + if (is_active) + AddBodyToActiveBodies(ioBody); + else + RemoveBodyFromActiveBodies(ioBody); + } +} + +#ifdef JPH_DEBUG_RENDERER +void BodyManager::Draw(const DrawSettings &inDrawSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter) +{ + JPH_PROFILE_FUNCTION(); + + LockAllBodies(); + + for (const Body *body : mBodies) + if (sIsValidBodyPointer(body) && body->IsInBroadPhase() && (!inBodyFilter || inBodyFilter->ShouldDraw(*body))) + { + JPH_ASSERT(mBodies[body->GetID().GetIndex()] == body); + + bool is_sensor = body->IsSensor(); + + // Determine drawing mode + Color color; + if (is_sensor) + color = Color::sYellow; + else + switch (inDrawSettings.mDrawShapeColor) + { + case EShapeColor::InstanceColor: + // Each instance has own color + color = Color::sGetDistinctColor(body->mID.GetIndex()); + break; + + case EShapeColor::ShapeTypeColor: + color = ShapeFunctions::sGet(body->GetShape()->GetSubType()).mColor; + break; + + case EShapeColor::MotionTypeColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + color = Color::sGreen; + break; + + case EMotionType::Dynamic: + color = Color::sGetDistinctColor(body->mID.GetIndex()); + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::SleepColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + color = body->IsActive()? Color::sGreen : Color::sRed; + break; + + case EMotionType::Dynamic: + color = body->IsActive()? Color::sYellow : Color::sRed; + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::IslandColor: + // Determine color based on motion type + switch (body->mMotionType) + { + case EMotionType::Static: + color = Color::sGrey; + break; + + case EMotionType::Kinematic: + case EMotionType::Dynamic: + { + uint32 idx = body->GetMotionProperties()->GetIslandIndexInternal(); + color = idx != Body::cInactiveIndex? Color::sGetDistinctColor(idx) : Color::sLightGrey; + } + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + break; + + case EShapeColor::MaterialColor: + color = Color::sWhite; + break; + + default: + JPH_ASSERT(false); + color = Color::sBlack; + break; + } + + // Draw the results of GetSupportFunction + if (inDrawSettings.mDrawGetSupportFunction) + body->mShape->DrawGetSupportFunction(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawSupportDirection); + + // Draw the results of GetSupportingFace + if (inDrawSettings.mDrawGetSupportingFace) + body->mShape->DrawGetSupportingFace(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + + // Draw the shape + if (inDrawSettings.mDrawShape) + body->mShape->Draw(inRenderer, body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f), color, inDrawSettings.mDrawShapeColor == EShapeColor::MaterialColor, inDrawSettings.mDrawShapeWireframe || is_sensor); + + // Draw bounding box + if (inDrawSettings.mDrawBoundingBox) + inRenderer->DrawWireBox(body->mBounds, color); + + // Draw center of mass transform + if (inDrawSettings.mDrawCenterOfMassTransform) + inRenderer->DrawCoordinateSystem(body->GetCenterOfMassTransform(), 0.2f); + + // Draw world transform + if (inDrawSettings.mDrawWorldTransform) + inRenderer->DrawCoordinateSystem(body->GetWorldTransform(), 0.2f); + + // Draw world space linear and angular velocity + if (inDrawSettings.mDrawVelocity) + { + RVec3 pos = body->GetCenterOfMassPosition(); + inRenderer->DrawArrow(pos, pos + body->GetLinearVelocity(), Color::sGreen, 0.1f); + inRenderer->DrawArrow(pos, pos + body->GetAngularVelocity(), Color::sRed, 0.1f); + } + + if (inDrawSettings.mDrawMassAndInertia && body->IsDynamic()) + { + const MotionProperties *mp = body->GetMotionProperties(); + if (mp->GetInverseMass() > 0.0f + && !Vec3::sEquals(mp->GetInverseInertiaDiagonal(), Vec3::sZero()).TestAnyXYZTrue()) + { + // Invert mass again + float mass = 1.0f / mp->GetInverseMass(); + + // Invert diagonal again + Vec3 diagonal = mp->GetInverseInertiaDiagonal().Reciprocal(); + + // Determine how big of a box has the equivalent inertia + Vec3 box_size = MassProperties::sGetEquivalentSolidBoxSize(mass, diagonal); + + // Draw box with equivalent inertia + inRenderer->DrawWireBox(body->GetCenterOfMassTransform() * Mat44::sRotation(mp->GetInertiaRotation()), AABox(-0.5f * box_size, 0.5f * box_size), Color::sOrange); + + // Draw mass + inRenderer->DrawText3D(body->GetCenterOfMassPosition(), StringFormat("%.2f", (double)mass), Color::sOrange, 0.2f); + } + } + + if (inDrawSettings.mDrawSleepStats && body->IsDynamic() && body->IsActive()) + { + // Draw stats to know which bodies could go to sleep + String text = StringFormat("t: %.1f", (double)body->mMotionProperties->mSleepTestTimer); + uint8 g = uint8(Clamp(255.0f * body->mMotionProperties->mSleepTestTimer / inPhysicsSettings.mTimeBeforeSleep, 0.0f, 255.0f)); + Color sleep_color = Color(0, 255 - g, g); + inRenderer->DrawText3D(body->GetCenterOfMassPosition(), text, sleep_color, 0.2f); + for (int i = 0; i < 3; ++i) + inRenderer->DrawWireSphere(JPH_IF_DOUBLE_PRECISION(body->mMotionProperties->GetSleepTestOffset() +) body->mMotionProperties->mSleepTestSpheres[i].GetCenter(), body->mMotionProperties->mSleepTestSpheres[i].GetRadius(), sleep_color); + } + + if (body->IsSoftBody()) + { + const SoftBodyMotionProperties *mp = static_cast(body->GetMotionProperties()); + RMat44 com = body->GetCenterOfMassTransform(); + + if (inDrawSettings.mDrawSoftBodyVertices) + mp->DrawVertices(inRenderer, com); + + if (inDrawSettings.mDrawSoftBodyVertexVelocities) + mp->DrawVertexVelocities(inRenderer, com); + + if (inDrawSettings.mDrawSoftBodyEdgeConstraints) + mp->DrawEdgeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyBendConstraints) + mp->DrawBendConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyVolumeConstraints) + mp->DrawVolumeConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodySkinConstraints) + mp->DrawSkinConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyLRAConstraints) + mp->DrawLRAConstraints(inRenderer, com, inDrawSettings.mDrawSoftBodyConstraintColor); + + if (inDrawSettings.mDrawSoftBodyPredictedBounds) + mp->DrawPredictedBounds(inRenderer, com); + } + } + + UnlockAllBodies(); +} +#endif // JPH_DEBUG_RENDERER + +void BodyManager::InvalidateContactCacheForBody(Body &ioBody) +{ + // If this is the first time we flip the collision cache invalid flag, we need to add it to an internal list to ensure we reset the flag at the end of the physics update + if (ioBody.InvalidateContactCacheInternal()) + { + lock_guard lock(mBodiesCacheInvalidMutex); + mBodiesCacheInvalid.push_back(ioBody.GetID()); + } +} + +void BodyManager::ValidateContactCacheForAllBodies() +{ + lock_guard lock(mBodiesCacheInvalidMutex); + + for (const BodyID &b : mBodiesCacheInvalid) + { + // The body may have been removed between the call to InvalidateContactCacheForBody and this call, so check if it still exists + Body *body = TryGetBody(b); + if (body != nullptr) + body->ValidateContactCacheInternal(); + } + mBodiesCacheInvalid.clear(); +} + +#ifdef JPH_DEBUG +void BodyManager::ValidateActiveBodyBounds() +{ + UniqueLock lock(mActiveBodiesMutex JPH_IF_ENABLE_ASSERTS(, this, EPhysicsLockTypes::ActiveBodiesList)); + + for (uint type = 0; type < cBodyTypeCount; ++type) + for (BodyID *id = mActiveBodies[type], *id_end = mActiveBodies[type] + mNumActiveBodies[type]; id < id_end; ++id) + { + const Body *body = mBodies[id->GetIndex()]; + AABox cached = body->GetWorldSpaceBounds(); + AABox calculated = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + JPH_ASSERT(cached == calculated); + } +} +#endif // JPH_DEBUG + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyManager.h b/WickedEngine/Jolt/Physics/Body/BodyManager.h new file mode 100644 index 000000000..fa1fb1117 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyManager.h @@ -0,0 +1,377 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +// Classes +class BodyCreationSettings; +class SoftBodyCreationSettings; +class BodyActivationListener; +class StateRecorderFilter; +struct PhysicsSettings; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +class BodyDrawFilter; +#endif // JPH_DEBUG_RENDERER + +#ifdef JPH_DEBUG_RENDERER + +/// Defines how to color soft body constraints +enum class ESoftBodyConstraintColor +{ + ConstraintType, /// Draw different types of constraints in different colors + ConstraintGroup, /// Draw constraints in the same group in the same color, non-parallel group will be red + ConstraintOrder, /// Draw constraints in the same group in the same color, non-parallel group will be red, and order within each group will be indicated with gradient +}; + +#endif // JPH_DEBUG_RENDERER + +/// Array of bodies +using BodyVector = Array; + +/// Array of body ID's +using BodyIDVector = Array; + +/// Class that contains all bodies +class JPH_EXPORT BodyManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + ~BodyManager(); + + /// Initialize the manager + void Init(uint inMaxBodies, uint inNumBodyMutexes, const BroadPhaseLayerInterface &inLayerInterface); + + /// Gets the current amount of bodies that are in the body manager + uint GetNumBodies() const; + + /// Gets the max bodies that we can support + uint GetMaxBodies() const { return uint(mBodies.capacity()); } + + /// Helper struct that counts the number of bodies of each type + struct BodyStats + { + uint mNumBodies = 0; ///< Total number of bodies in the body manager + uint mMaxBodies = 0; ///< Max allowed number of bodies in the body manager (as configured in Init(...)) + + uint mNumBodiesStatic = 0; ///< Number of static bodies + + uint mNumBodiesDynamic = 0; ///< Number of dynamic bodies + uint mNumActiveBodiesDynamic = 0; ///< Number of dynamic bodies that are currently active + + uint mNumBodiesKinematic = 0; ///< Number of kinematic bodies + uint mNumActiveBodiesKinematic = 0; ///< Number of kinematic bodies that are currently active + + uint mNumSoftBodies = 0; ///< Number of soft bodies + uint mNumActiveSoftBodies = 0; ///< Number of soft bodies that are currently active + }; + + /// Get stats about the bodies in the body manager (slow, iterates through all bodies) + BodyStats GetBodyStats() const; + + /// Create a body using creation settings. The returned body will not be part of the body manager yet. + Body * AllocateBody(const BodyCreationSettings &inBodyCreationSettings) const; + + /// Create a soft body using creation settings. The returned body will not be part of the body manager yet. + Body * AllocateSoftBody(const SoftBodyCreationSettings &inSoftBodyCreationSettings) const; + + /// Free a body that has not been added to the body manager yet (if it has, use DestroyBodies). + void FreeBody(Body *inBody) const; + + /// Add a body to the body manager, assigning it the next available ID. Returns false if no more IDs are available. + bool AddBody(Body *ioBody); + + /// Add a body to the body manager, assigning it a custom ID. Returns false if the ID is not valid. + bool AddBodyWithCustomID(Body *ioBody, const BodyID &inBodyID); + + /// Remove a list of bodies from the body manager + void RemoveBodies(const BodyID *inBodyIDs, int inNumber, Body **outBodies); + + /// Remove a set of bodies from the body manager and frees them. + void DestroyBodies(const BodyID *inBodyIDs, int inNumber); + + /// Activate a list of bodies. + /// This function should only be called when an exclusive lock for the bodies are held. + void ActivateBodies(const BodyID *inBodyIDs, int inNumber); + + /// Deactivate a list of bodies. + /// This function should only be called when an exclusive lock for the bodies are held. + void DeactivateBodies(const BodyID *inBodyIDs, int inNumber); + + /// Update the motion quality for a body + void SetMotionQuality(Body &ioBody, EMotionQuality inMotionQuality); + + /// Get copy of the list of active bodies under protection of a lock. + void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const; + + /// Get the list of active bodies. Note: Not thread safe. The active bodies list can change at any moment. + const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mActiveBodies[int(inType)]; } + + /// Get the number of active bodies. + uint32 GetNumActiveBodies(EBodyType inType) const { return mNumActiveBodies[int(inType)]; } + + /// Get the number of active bodies that are using continuous collision detection + uint32 GetNumActiveCCDBodies() const { return mNumActiveCCDBodies; } + + /// Listener that is notified whenever a body is activated/deactivated + void SetBodyActivationListener(BodyActivationListener *inListener); + BodyActivationListener * GetBodyActivationListener() const { return mActivationListener; } + + /// Check if this is a valid body pointer. When a body is freed the memory that the pointer occupies is reused to store a freelist. + static inline bool sIsValidBodyPointer(const Body *inBody) { return (uintptr_t(inBody) & cIsFreedBody) == 0; } + + /// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check. + const BodyVector & GetBodies() const { return mBodies; } + + /// Get all bodies. Note that this can contain invalid body pointers, call sIsValidBodyPointer to check. + BodyVector & GetBodies() { return mBodies; } + + /// Get all body IDs under the protection of a lock + void GetBodyIDs(BodyIDVector &outBodies) const; + + /// Access a body (not protected by lock) + const Body & GetBody(const BodyID &inID) const { return *mBodies[inID.GetIndex()]; } + + /// Access a body (not protected by lock) + Body & GetBody(const BodyID &inID) { return *mBodies[inID.GetIndex()]; } + + /// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock) + const Body * TryGetBody(const BodyID &inID) const + { + uint32 idx = inID.GetIndex(); + if (idx >= mBodies.size()) + return nullptr; + + const Body *body = mBodies[idx]; + if (sIsValidBodyPointer(body) && body->GetID() == inID) + return body; + + return nullptr; + } + + /// Access a body, will return a nullptr if the body ID is no longer valid (not protected by lock) + Body * TryGetBody(const BodyID &inID) + { + uint32 idx = inID.GetIndex(); + if (idx >= mBodies.size()) + return nullptr; + + Body *body = mBodies[idx]; + if (sIsValidBodyPointer(body) && body->GetID() == inID) + return body; + + return nullptr; + } + + /// Access the mutex for a single body + SharedMutex & GetMutexForBody(const BodyID &inID) const { return mBodyMutexes.GetMutexByObjectIndex(inID.GetIndex()); } + + /// Bodies are protected using an array of mutexes (so a fixed number, not 1 per body). Each bit in this mask indicates a locked mutex. + using MutexMask = uint64; + + ///@name Batch body mutex access (do not use directly) + ///@{ + MutexMask GetAllBodiesMutexMask() const { return mBodyMutexes.GetNumMutexes() == sizeof(MutexMask) * 8? ~MutexMask(0) : (MutexMask(1) << mBodyMutexes.GetNumMutexes()) - 1; } + MutexMask GetMutexMask(const BodyID *inBodies, int inNumber) const; + void LockRead(MutexMask inMutexMask) const; + void UnlockRead(MutexMask inMutexMask) const; + void LockWrite(MutexMask inMutexMask) const; + void UnlockWrite(MutexMask inMutexMask) const; + ///@} + + /// Lock all bodies. This should only be done during PhysicsSystem::Update(). + void LockAllBodies() const; + + /// Unlock all bodies. This should only be done during PhysicsSystem::Update(). + void UnlockAllBodies() const; + + /// Function to update body's layer (should only be called by the BodyInterface since it also requires updating the broadphase) + inline void SetBodyObjectLayerInternal(Body &ioBody, ObjectLayer inLayer) const { ioBody.mObjectLayer = inLayer; ioBody.mBroadPhaseLayer = mBroadPhaseLayerInterface->GetBroadPhaseLayer(inLayer); } + + /// Set the Body::EFlags::InvalidateContactCache flag for the specified body. This means that the collision cache is invalid for any body pair involving that body until the next physics step. + void InvalidateContactCacheForBody(Body &ioBody); + + /// Reset the Body::EFlags::InvalidateContactCache flag for all bodies. All contact pairs in the contact cache will now by valid again. + void ValidateContactCacheForAllBodies(); + + /// Saving state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restoring state for replay. Returns false if failed. + bool RestoreState(StateRecorder &inStream); + + /// Save the state of a single body for replay + void SaveBodyState(const Body &inBody, StateRecorder &inStream) const; + + /// Save the state of a single body for replay + void RestoreBodyState(Body &inBody, StateRecorder &inStream); + +#ifdef JPH_DEBUG_RENDERER + enum class EShapeColor + { + InstanceColor, ///< Random color per instance + ShapeTypeColor, ///< Convex = green, scaled = yellow, compound = orange, mesh = red + MotionTypeColor, ///< Static = grey, keyframed = green, dynamic = random color per instance + SleepColor, ///< Static = grey, keyframed = green, dynamic = yellow, sleeping = red + IslandColor, ///< Static = grey, active = random color per island, sleeping = light grey + MaterialColor, ///< Color as defined by the PhysicsMaterial of the shape + }; + + /// Draw settings + struct DrawSettings + { + bool mDrawGetSupportFunction = false; ///< Draw the GetSupport() function, used for convex collision detection + bool mDrawSupportDirection = false; ///< When drawing the support function, also draw which direction mapped to a specific support point + bool mDrawGetSupportingFace = false; ///< Draw the faces that were found colliding during collision detection + bool mDrawShape = true; ///< Draw the shapes of all bodies + bool mDrawShapeWireframe = false; ///< When mDrawShape is true and this is true, the shapes will be drawn in wireframe instead of solid. + EShapeColor mDrawShapeColor = EShapeColor::MotionTypeColor; ///< Coloring scheme to use for shapes + bool mDrawBoundingBox = false; ///< Draw a bounding box per body + bool mDrawCenterOfMassTransform = false; ///< Draw the center of mass for each body + bool mDrawWorldTransform = false; ///< Draw the world transform (which can be different than the center of mass) for each body + bool mDrawVelocity = false; ///< Draw the velocity vector for each body + bool mDrawMassAndInertia = false; ///< Draw the mass and inertia (as the box equivalent) for each body + bool mDrawSleepStats = false; ///< Draw stats regarding the sleeping algorithm of each body + bool mDrawSoftBodyVertices = false; ///< Draw the vertices of soft bodies + bool mDrawSoftBodyVertexVelocities = false; ///< Draw the velocities of the vertices of soft bodies + bool mDrawSoftBodyEdgeConstraints = false; ///< Draw the edge constraints of soft bodies + bool mDrawSoftBodyBendConstraints = false; ///< Draw the bend constraints of soft bodies + bool mDrawSoftBodyVolumeConstraints = false; ///< Draw the volume constraints of soft bodies + bool mDrawSoftBodySkinConstraints = false; ///< Draw the skin constraints of soft bodies + bool mDrawSoftBodyLRAConstraints = false; ///< Draw the LRA constraints of soft bodies + bool mDrawSoftBodyPredictedBounds = false; ///< Draw the predicted bounds of soft bodies + ESoftBodyConstraintColor mDrawSoftBodyConstraintColor = ESoftBodyConstraintColor::ConstraintType; ///< Coloring scheme to use for soft body constraints + }; + + /// Draw the state of the bodies (debugging purposes) + void Draw(const DrawSettings &inSettings, const PhysicsSettings &inPhysicsSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr); +#endif // JPH_DEBUG_RENDERER + +#ifdef JPH_ENABLE_ASSERTS + /// Lock the active body list, asserts when Activate/DeactivateBody is called. + void SetActiveBodiesLocked(bool inLocked) { mActiveBodiesLocked = inLocked; } + + /// Per thread override of the locked state, to be used by the PhysicsSystem only! + class GrantActiveBodiesAccess + { + public: + inline GrantActiveBodiesAccess(bool inAllowActivation, bool inAllowDeactivation) + { + JPH_ASSERT(!sGetOverrideAllowActivation()); + sSetOverrideAllowActivation(inAllowActivation); + + JPH_ASSERT(!sGetOverrideAllowDeactivation()); + sSetOverrideAllowDeactivation(inAllowDeactivation); + } + + inline ~GrantActiveBodiesAccess() + { + sSetOverrideAllowActivation(false); + sSetOverrideAllowDeactivation(false); + } + }; +#endif + +#ifdef JPH_DEBUG + /// Validate if the cached bounding boxes are correct for all active bodies + void ValidateActiveBodyBounds(); +#endif // JPH_DEBUG + +private: + /// Increment and get the sequence number of the body +#ifdef JPH_COMPILER_CLANG + __attribute__((no_sanitize("implicit-conversion"))) // We intentionally overflow the uint8 sequence number +#endif + inline uint8 GetNextSequenceNumber(int inBodyIndex) { return ++mBodySequenceNumbers[inBodyIndex]; } + + /// Add a single body to mActiveBodies, note doesn't lock the active body mutex! + inline void AddBodyToActiveBodies(Body &ioBody); + + /// Remove a single body from mActiveBodies, note doesn't lock the active body mutex! + inline void RemoveBodyFromActiveBodies(Body &ioBody); + + /// Helper function to remove a body from the manager + JPH_INLINE Body * RemoveBodyInternal(const BodyID &inBodyID); + + /// Helper function to delete a body (which could actually be a BodyWithMotionProperties) + inline static void sDeleteBody(Body *inBody); + +#if defined(JPH_DEBUG) && defined(JPH_ENABLE_ASSERTS) + /// Function to check that the free list is not corrupted + void ValidateFreeList() const; +#endif // defined(JPH_DEBUG) && _defined(JPH_ENABLE_ASSERTS) + + /// List of pointers to all bodies. Contains invalid pointers for deleted bodies, check with sIsValidBodyPointer. Note that this array is reserved to the max bodies that is passed in the Init function so that adding bodies will not reallocate the array. + BodyVector mBodies; + + /// Current number of allocated bodies + uint mNumBodies = 0; + + /// Indicates that there are no more freed body IDs + static constexpr uintptr_t cBodyIDFreeListEnd = ~uintptr_t(0); + + /// Bit that indicates a pointer in mBodies is actually the index of the next freed body. We use the lowest bit because we know that Bodies need to be 16 byte aligned so addresses can never end in a 1 bit. + static constexpr uintptr_t cIsFreedBody = uintptr_t(1); + + /// Amount of bits to shift to get an index to the next freed body + static constexpr uint cFreedBodyIndexShift = 1; + + /// Index of first entry in mBodies that is unused + uintptr_t mBodyIDFreeListStart = cBodyIDFreeListEnd; + + /// Protects mBodies array (but not the bodies it points to), mNumBodies and mBodyIDFreeListStart + mutable Mutex mBodiesMutex; + + /// An array of mutexes protecting the bodies in the mBodies array + using BodyMutexes = MutexArray; + mutable BodyMutexes mBodyMutexes; + + /// List of next sequence number for a body ID + Array mBodySequenceNumbers; + + /// Mutex that protects the mActiveBodies array + mutable Mutex mActiveBodiesMutex; + + /// List of all active dynamic bodies (size is equal to max amount of bodies) + BodyID * mActiveBodies[cBodyTypeCount] = { }; + + /// How many bodies there are in the list of active bodies + atomic mNumActiveBodies[cBodyTypeCount] = { }; + + /// How many of the active bodies have continuous collision detection enabled + uint32 mNumActiveCCDBodies = 0; + + /// Mutex that protects the mBodiesCacheInvalid array + mutable Mutex mBodiesCacheInvalidMutex; + + /// List of all bodies that should have their cache invalidated + BodyIDVector mBodiesCacheInvalid; + + /// Listener that is notified whenever a body is activated/deactivated + BodyActivationListener * mActivationListener = nullptr; + + /// Cached broadphase layer interface + const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr; + +#ifdef JPH_ENABLE_ASSERTS + static bool sGetOverrideAllowActivation(); + static void sSetOverrideAllowActivation(bool inValue); + + static bool sGetOverrideAllowDeactivation(); + static void sSetOverrideAllowDeactivation(bool inValue); + + /// Debug system that tries to limit changes to active bodies during the PhysicsSystem::Update() + bool mActiveBodiesLocked = false; +#endif +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyPair.h b/WickedEngine/Jolt/Physics/Body/BodyPair.h new file mode 100644 index 000000000..8ac849a49 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyPair.h @@ -0,0 +1,36 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a body pair +struct alignas(uint64) BodyPair +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BodyPair() = default; + BodyPair(BodyID inA, BodyID inB) : mBodyA(inA), mBodyB(inB) { } + + /// Equals operator + bool operator == (const BodyPair &inRHS) const { return *reinterpret_cast(this) == *reinterpret_cast(&inRHS); } + + /// Smaller than operator, used for consistently ordering body pairs + bool operator < (const BodyPair &inRHS) const { return *reinterpret_cast(this) < *reinterpret_cast(&inRHS); } + + /// Get the hash value of this object + uint64 GetHash() const { return Hash64(*reinterpret_cast(this)); } + + BodyID mBodyA; + BodyID mBodyB; +}; + +static_assert(sizeof(BodyPair) == sizeof(uint64), "Mismatch in class size"); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/BodyType.h b/WickedEngine/Jolt/Physics/Body/BodyType.h new file mode 100644 index 000000000..984af06ad --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/BodyType.h @@ -0,0 +1,19 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Type of body +enum class EBodyType : uint8 +{ + RigidBody, ///< Rigid body consisting of a rigid shape + SoftBody, ///< Soft body consisting of a deformable shape +}; + +/// How many types of bodies there are +static constexpr uint cBodyTypeCount = 2; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/MassProperties.cpp b/WickedEngine/Jolt/Physics/Body/MassProperties.cpp new file mode 100644 index 000000000..91df6b0df --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/MassProperties.cpp @@ -0,0 +1,185 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MassProperties) +{ + JPH_ADD_ATTRIBUTE(MassProperties, mMass) + JPH_ADD_ATTRIBUTE(MassProperties, mInertia) +} + +bool MassProperties::DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const +{ + // Using eigendecomposition to get the principal components of the inertia tensor + // See: https://en.wikipedia.org/wiki/Eigendecomposition_of_a_matrix + Matrix<3, 3> inertia; + inertia.CopyPart(mInertia, 0, 0, 3, 3, 0, 0); + Matrix<3, 3> eigen_vec = Matrix<3, 3>::sIdentity(); + Vector<3> eigen_val; + if (!EigenValueSymmetric(inertia, eigen_vec, eigen_val)) + return false; + + // Sort so that the biggest value goes first + int indices[] = { 0, 1, 2 }; + InsertionSort(indices, indices + 3, [&eigen_val](int inLeft, int inRight) { return eigen_val[inLeft] > eigen_val[inRight]; }); + + // Convert to a regular Mat44 and Vec3 + outRotation = Mat44::sIdentity(); + for (int i = 0; i < 3; ++i) + { + outRotation.SetColumn3(i, Vec3(reinterpret_cast(eigen_vec.GetColumn(indices[i])))); + outDiagonal.SetComponent(i, eigen_val[indices[i]]); + } + + // Make sure that the rotation matrix is a right handed matrix + if (outRotation.GetAxisX().Cross(outRotation.GetAxisY()).Dot(outRotation.GetAxisZ()) < 0.0f) + outRotation.SetAxisZ(-outRotation.GetAxisZ()); + +#ifdef JPH_ENABLE_ASSERTS + // Validate that the solution is correct, for each axis we want to make sure that the difference in inertia is + // smaller than some fraction of the inertia itself in that axis + Mat44 new_inertia = outRotation * Mat44::sScale(outDiagonal) * outRotation.Inversed(); + for (int i = 0; i < 3; ++i) + JPH_ASSERT(new_inertia.GetColumn3(i).IsClose(mInertia.GetColumn3(i), mInertia.GetColumn3(i).LengthSq() * 1.0e-10f)); +#endif + + return true; +} + +void MassProperties::SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity) +{ + // Calculate mass + mMass = inBoxSize.GetX() * inBoxSize.GetY() * inBoxSize.GetZ() * inDensity; + + // Calculate inertia + Vec3 size_sq = inBoxSize * inBoxSize; + Vec3 scale = (size_sq.Swizzle() + size_sq.Swizzle()) * (mMass / 12.0f); + mInertia = Mat44::sScale(scale); +} + +void MassProperties::ScaleToMass(float inMass) +{ + if (mMass > 0.0f) + { + // Calculate how much we have to scale the inertia tensor + float mass_scale = inMass / mMass; + + // Update mass + mMass = inMass; + + // Update inertia tensor + for (int i = 0; i < 3; ++i) + mInertia.SetColumn4(i, mInertia.GetColumn4(i) * mass_scale); + } + else + { + // Just set the mass + mMass = inMass; + } +} + +Vec3 MassProperties::sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal) +{ + // Moment of inertia of a solid box has diagonal: + // mass / 12 * [size_y^2 + size_z^2, size_x^2 + size_z^2, size_x^2 + size_y^2] + // Solving for size_x, size_y and size_y (diagonal and mass are known): + Vec3 diagonal = inInertiaDiagonal * (12.0f / inMass); + return Vec3(sqrt(0.5f * (-diagonal[0] + diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] - diagonal[1] + diagonal[2])), sqrt(0.5f * (diagonal[0] + diagonal[1] - diagonal[2]))); +} + +void MassProperties::Scale(Vec3Arg inScale) +{ + // See: https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_tensor + // The diagonal of the inertia tensor can be calculated like this: + // Ixx = sum_{k = 1 to n}(m_k * (y_k^2 + z_k^2)) + // Iyy = sum_{k = 1 to n}(m_k * (x_k^2 + z_k^2)) + // Izz = sum_{k = 1 to n}(m_k * (x_k^2 + y_k^2)) + // + // We want to isolate the terms x_k, y_k and z_k: + // d = [0.5, 0.5, 0.5].[Ixx, Iyy, Izz] + // [sum_{k = 1 to n}(m_k * x_k^2), sum_{k = 1 to n}(m_k * y_k^2), sum_{k = 1 to n}(m_k * z_k^2)] = [d, d, d] - [Ixx, Iyy, Izz] + Vec3 diagonal = mInertia.GetDiagonal3(); + Vec3 xyz_sq = Vec3::sReplicate(Vec3::sReplicate(0.5f).Dot(diagonal)) - diagonal; + + // When scaling a shape these terms change like this: + // sum_{k = 1 to n}(m_k * (scale_x * x_k)^2) = scale_x^2 * sum_{k = 1 to n}(m_k * x_k^2) + // Same for y_k and z_k + // Using these terms we can calculate the new diagonal of the inertia tensor: + Vec3 xyz_scaled_sq = inScale * inScale * xyz_sq; + float i_xx = xyz_scaled_sq.GetY() + xyz_scaled_sq.GetZ(); + float i_yy = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetZ(); + float i_zz = xyz_scaled_sq.GetX() + xyz_scaled_sq.GetY(); + + // The off diagonal elements are calculated like: + // Ixy = -sum_{k = 1 to n}(x_k y_k) + // Ixz = -sum_{k = 1 to n}(x_k z_k) + // Iyz = -sum_{k = 1 to n}(y_k z_k) + // Scaling these is simple: + float i_xy = inScale.GetX() * inScale.GetY() * mInertia(0, 1); + float i_xz = inScale.GetX() * inScale.GetZ() * mInertia(0, 2); + float i_yz = inScale.GetY() * inScale.GetZ() * mInertia(1, 2); + + // Update inertia tensor + mInertia(0, 0) = i_xx; + mInertia(0, 1) = i_xy; + mInertia(1, 0) = i_xy; + mInertia(1, 1) = i_yy; + mInertia(0, 2) = i_xz; + mInertia(2, 0) = i_xz; + mInertia(1, 2) = i_yz; + mInertia(2, 1) = i_yz; + mInertia(2, 2) = i_zz; + + // Mass scales linear with volume (note that the scaling can be negative and we don't want the mass to become negative) + float mass_scale = abs(inScale.GetX() * inScale.GetY() * inScale.GetZ()); + mMass *= mass_scale; + + // Inertia scales linear with mass. This updates the m_k terms above. + mInertia *= mass_scale; + + // Ensure that the bottom right element is a 1 again + mInertia(3, 3) = 1.0f; +} + +void MassProperties::Rotate(Mat44Arg inRotation) +{ + mInertia = inRotation.Multiply3x3(mInertia).Multiply3x3RightTransposed(inRotation); +} + +void MassProperties::Translate(Vec3Arg inTranslation) +{ + // Transform the inertia using the parallel axis theorem: I' = I + m * (translation^2 E - translation translation^T) + // Where I is the original body's inertia and E the identity matrix + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + mInertia += mMass * (Mat44::sScale(inTranslation.Dot(inTranslation)) - Mat44::sOuterProduct(inTranslation, inTranslation)); + + // Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change + mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); +} + +void MassProperties::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMass); + inStream.Write(mInertia); +} + +void MassProperties::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMass); + inStream.Read(mInertia); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/MassProperties.h b/WickedEngine/Jolt/Physics/Body/MassProperties.h new file mode 100644 index 000000000..a72a901db --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/MassProperties.h @@ -0,0 +1,58 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Describes the mass and inertia properties of a body. Used during body construction only. +class JPH_EXPORT MassProperties +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MassProperties) + + /// Using eigendecomposition, decompose the inertia tensor into a diagonal matrix D and a right-handed rotation matrix R so that the inertia tensor is \f$R \: D \: R^{-1}\f$. + /// @see https://en.wikipedia.org/wiki/Moment_of_inertia section 'Principal axes' + /// @param outRotation The rotation matrix R + /// @param outDiagonal The diagonal of the diagonal matrix D + /// @return True if successful, false if failed + bool DecomposePrincipalMomentsOfInertia(Mat44 &outRotation, Vec3 &outDiagonal) const; + + /// Set the mass and inertia of a box with edge size inBoxSize and density inDensity + void SetMassAndInertiaOfSolidBox(Vec3Arg inBoxSize, float inDensity); + + /// Set the mass and scale the inertia tensor to match the mass + void ScaleToMass(float inMass); + + /// Calculates the size of the solid box that has an inertia tensor diagonal inInertiaDiagonal + static Vec3 sGetEquivalentSolidBoxSize(float inMass, Vec3Arg inInertiaDiagonal); + + /// Rotate the inertia by 3x3 matrix inRotation + void Rotate(Mat44Arg inRotation); + + /// Translate the inertia by a vector inTranslation + void Translate(Vec3Arg inTranslation); + + /// Scale the mass and inertia by inScale, note that elements can be < 0 to flip the shape + void Scale(Vec3Arg inScale); + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Mass of the shape (kg) + float mMass = 0.0f; + + /// Inertia tensor of the shape (kg m^2) + Mat44 mInertia = Mat44::sZero(); +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/MotionProperties.cpp b/WickedEngine/Jolt/Physics/Body/MotionProperties.cpp new file mode 100644 index 000000000..1acc274bb --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/MotionProperties.cpp @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +void MotionProperties::SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties) +{ + // Store allowed DOFs + mAllowedDOFs = inAllowedDOFs; + + // Decompose DOFs + uint allowed_translation_axis = uint(inAllowedDOFs) & 0b111; + uint allowed_rotation_axis = (uint(inAllowedDOFs) >> 3) & 0b111; + + // Set inverse mass + if (allowed_translation_axis == 0) + { + // No translation possible + mInvMass = 0.0f; + } + else + { + JPH_ASSERT(inMassProperties.mMass > 0.0f); + mInvMass = 1.0f / inMassProperties.mMass; + } + + if (allowed_rotation_axis == 0) + { + // No rotation possible + mInvInertiaDiagonal = Vec3::sZero(); + mInertiaRotation = Quat::sIdentity(); + } + else + { + // Set inverse inertia + Mat44 rotation; + Vec3 diagonal; + if (inMassProperties.DecomposePrincipalMomentsOfInertia(rotation, diagonal) + && !diagonal.IsNearZero()) + { + mInvInertiaDiagonal = diagonal.Reciprocal(); + mInertiaRotation = rotation.GetQuaternion(); + } + else + { + // Failed! Fall back to inertia tensor of sphere with radius 1. + mInvInertiaDiagonal = Vec3::sReplicate(2.5f * mInvMass); + mInertiaRotation = Quat::sIdentity(); + } + } + + JPH_ASSERT(mInvMass != 0.0f || mInvInertiaDiagonal != Vec3::sZero(), "Can't lock all axes, use a static body for this. This will crash with a division by zero later!"); +} + +void MotionProperties::SaveState(StateRecorder &inStream) const +{ + // Only write properties that can change at runtime + inStream.Write(mLinearVelocity); + inStream.Write(mAngularVelocity); + inStream.Write(mForce); + inStream.Write(mTorque); +#ifdef JPH_DOUBLE_PRECISION + inStream.Write(mSleepTestOffset); +#endif // JPH_DOUBLE_PRECISION + inStream.Write(mSleepTestSpheres); + inStream.Write(mSleepTestTimer); + inStream.Write(mAllowSleeping); +} + +void MotionProperties::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mLinearVelocity); + inStream.Read(mAngularVelocity); + inStream.Read(mForce); + inStream.Read(mTorque); +#ifdef JPH_DOUBLE_PRECISION + inStream.Read(mSleepTestOffset); +#endif // JPH_DOUBLE_PRECISION + inStream.Read(mSleepTestSpheres); + inStream.Read(mSleepTestTimer); + inStream.Read(mAllowSleeping); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/MotionProperties.h b/WickedEngine/Jolt/Physics/Body/MotionProperties.h new file mode 100644 index 000000000..f3e6bc32e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/MotionProperties.h @@ -0,0 +1,278 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StateRecorder; + +/// Enum that determines if an object can go to sleep +enum class ECanSleep +{ + CannotSleep = 0, ///< Object cannot go to sleep + CanSleep = 1, ///< Object can go to sleep +}; + +/// The Body class only keeps track of state for static bodies, the MotionProperties class keeps the additional state needed for a moving Body. It has a 1-on-1 relationship with the body. +class JPH_EXPORT MotionProperties +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Motion quality, or how well it detects collisions when it has a high velocity + EMotionQuality GetMotionQuality() const { return mMotionQuality; } + + /// Get the allowed degrees of freedom that this body has (this can be changed by calling SetMassProperties) + inline EAllowedDOFs GetAllowedDOFs() const { return mAllowedDOFs; } + + /// If this body can go to sleep. + inline bool GetAllowSleeping() const { return mAllowSleeping; } + + /// Get world space linear velocity of the center of mass + inline Vec3 GetLinearVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::Read)); return mLinearVelocity; } + + /// Set world space linear velocity of the center of mass + void SetLinearVelocity(Vec3Arg inLinearVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inLinearVelocity.Length() <= mMaxLinearVelocity); mLinearVelocity = LockTranslation(inLinearVelocity); } + + /// Set world space linear velocity of the center of mass, will make sure the value is clamped against the maximum linear velocity + void SetLinearVelocityClamped(Vec3Arg inLinearVelocity) { mLinearVelocity = LockTranslation(inLinearVelocity); ClampLinearVelocity(); } + + /// Get world space angular velocity of the center of mass + inline Vec3 GetAngularVelocity() const { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::Read)); return mAngularVelocity; } + + /// Set world space angular velocity of the center of mass + void SetAngularVelocity(Vec3Arg inAngularVelocity) { JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); JPH_ASSERT(inAngularVelocity.Length() <= mMaxAngularVelocity); mAngularVelocity = LockAngular(inAngularVelocity); } + + /// Set world space angular velocity of the center of mass, will make sure the value is clamped against the maximum angular velocity + void SetAngularVelocityClamped(Vec3Arg inAngularVelocity) { mAngularVelocity = LockAngular(inAngularVelocity); ClampAngularVelocity(); } + + /// Set velocity of body such that it will be rotate/translate by inDeltaPosition/Rotation in inDeltaTime seconds. + inline void MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime); + + ///@name Velocity limits + ///@{ + + /// Maximum linear velocity that a body can achieve. Used to prevent the system from exploding. + inline float GetMaxLinearVelocity() const { return mMaxLinearVelocity; } + inline void SetMaxLinearVelocity(float inLinearVelocity) { JPH_ASSERT(inLinearVelocity >= 0.0f); mMaxLinearVelocity = inLinearVelocity; } + + /// Maximum angular velocity that a body can achieve. Used to prevent the system from exploding. + inline float GetMaxAngularVelocity() const { return mMaxAngularVelocity; } + inline void SetMaxAngularVelocity(float inAngularVelocity) { JPH_ASSERT(inAngularVelocity >= 0.0f); mMaxAngularVelocity = inAngularVelocity; } + ///@} + + /// Clamp velocity according to limit + inline void ClampLinearVelocity(); + inline void ClampAngularVelocity(); + + /// Get linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + inline float GetLinearDamping() const { return mLinearDamping; } + void SetLinearDamping(float inLinearDamping) { JPH_ASSERT(inLinearDamping >= 0.0f); mLinearDamping = inLinearDamping; } + + /// Get angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + inline float GetAngularDamping() const { return mAngularDamping; } + void SetAngularDamping(float inAngularDamping) { JPH_ASSERT(inAngularDamping >= 0.0f); mAngularDamping = inAngularDamping; } + + /// Get gravity factor (1 = normal gravity, 0 = no gravity) + inline float GetGravityFactor() const { return mGravityFactor; } + void SetGravityFactor(float inGravityFactor) { mGravityFactor = inGravityFactor; } + + /// Set the mass and inertia tensor + void SetMassProperties(EAllowedDOFs inAllowedDOFs, const MassProperties &inMassProperties); + + /// Get inverse mass (1 / mass). Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as 1 / mass = 0) + inline float GetInverseMass() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvMass; } + inline float GetInverseMassUnchecked() const { return mInvMass; } + + /// Set the inverse mass (1 / mass). + /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$). + /// If you change mass, inertia should probably change as well. See MassProperties::ScaleToMass. + /// If all your translation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs). + void SetInverseMass(float inInverseMass) { mInvMass = inInverseMass; } + + /// Diagonal of inverse inertia matrix: D. Should only be called on a dynamic object (static or kinematic bodies have infinite mass so should be treated as D = 0) + inline Vec3 GetInverseInertiaDiagonal() const { JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); return mInvInertiaDiagonal; } + + /// Rotation (R) that takes inverse inertia diagonal to local space: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$ + inline Quat GetInertiaRotation() const { return mInertiaRotation; } + + /// Set the inverse inertia tensor in local space by setting the diagonal and the rotation: \f$I_{body}^{-1} = R \: D \: R^{-1}\f$. + /// Note that mass and inertia are linearly related (e.g. inertia of a sphere with mass m and radius r is \f$2/5 \: m \: r^2\f$). + /// If you change inertia, mass should probably change as well. See MassProperties::ScaleToMass. + /// If all your rotation degrees of freedom are restricted, make sure this is zero (see EAllowedDOFs). + void SetInverseInertia(Vec3Arg inDiagonal, QuatArg inRot) { mInvInertiaDiagonal = inDiagonal; mInertiaRotation = inRot; } + + /// Get inverse inertia matrix (\f$I_{body}^{-1}\f$). Will be a matrix of zeros for a static or kinematic object. + inline Mat44 GetLocalSpaceInverseInertia() const; + + /// Same as GetLocalSpaceInverseInertia() but doesn't check if the body is dynamic + inline Mat44 GetLocalSpaceInverseInertiaUnchecked() const; + + /// Get inverse inertia matrix (\f$I^{-1}\f$) for a given object rotation (translation will be ignored). Zero if object is static or kinematic. + inline Mat44 GetInverseInertiaForRotation(Mat44Arg inRotation) const; + + /// Multiply a vector with the inverse world space inertia tensor (\f$I_{world}^{-1}\f$). Zero if object is static or kinematic. + JPH_INLINE Vec3 MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const; + + /// Velocity of point inPoint (in center of mass space, e.g. on the surface of the body) of the body (unit: m/s) + JPH_INLINE Vec3 GetPointVelocityCOM(Vec3Arg inPointRelativeToCOM) const { return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); } + + // Get the total amount of force applied to the center of mass this time step (through Body::AddForce calls). Note that it will reset to zero after PhysicsSystem::Update. + JPH_INLINE Vec3 GetAccumulatedForce() const { return Vec3::sLoadFloat3Unsafe(mForce); } + + // Get the total amount of torque applied to the center of mass this time step (through Body::AddForce/Body::AddTorque calls). Note that it will reset to zero after PhysicsSystem::Update. + JPH_INLINE Vec3 GetAccumulatedTorque() const { return Vec3::sLoadFloat3Unsafe(mTorque); } + + // Reset the total accumulated force, note that this will be done automatically after every time step. + JPH_INLINE void ResetForce() { mForce = Float3(0, 0, 0); } + + // Reset the total accumulated torque, note that this will be done automatically after every time step. + JPH_INLINE void ResetTorque() { mTorque = Float3(0, 0, 0); } + + // Reset the current velocity and accumulated force and torque. + JPH_INLINE void ResetMotion() + { + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); + mLinearVelocity = mAngularVelocity = Vec3::sZero(); + mForce = mTorque = Float3(0, 0, 0); + } + + /// Returns a vector where the linear components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff + JPH_INLINE UVec4 GetLinearDOFsMask() const + { + UVec4 mask(uint32(EAllowedDOFs::TranslationX), uint32(EAllowedDOFs::TranslationY), uint32(EAllowedDOFs::TranslationZ), 0); + return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask); + } + + /// Takes a translation vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0 + JPH_INLINE Vec3 LockTranslation(Vec3Arg inV) const + { + return Vec3::sAnd(inV, Vec3(GetLinearDOFsMask().ReinterpretAsFloat())); + } + + /// Returns a vector where the angular components that are not allowed by mAllowedDOFs are set to 0 and the rest to 0xffffffff + JPH_INLINE UVec4 GetAngularDOFsMask() const + { + UVec4 mask(uint32(EAllowedDOFs::RotationX), uint32(EAllowedDOFs::RotationY), uint32(EAllowedDOFs::RotationZ), 0); + return UVec4::sEquals(UVec4::sAnd(UVec4::sReplicate(uint32(mAllowedDOFs)), mask), mask); + } + + /// Takes an angular velocity / torque vector inV and returns a vector where the components that are not allowed by mAllowedDOFs are set to 0 + JPH_INLINE Vec3 LockAngular(Vec3Arg inV) const + { + return Vec3::sAnd(inV, Vec3(GetAngularDOFsMask().ReinterpretAsFloat())); + } + + /// Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumVelocityStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); } + uint GetNumVelocityStepsOverride() const { return mNumVelocityStepsOverride; } + + /// Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); } + uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; } + + //////////////////////////////////////////////////////////// + // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY + //////////////////////////////////////////////////////////// + + ///@name Update linear and angular velocity (used during constraint solving) + ///@{ + inline void AddLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("AddLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity + inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); } + inline void SubLinearVelocityStep(Vec3Arg inLinearVelocityChange) { JPH_DET_LOG("SubLinearVelocityStep: " << inLinearVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mLinearVelocity = LockTranslation(mLinearVelocity - inLinearVelocityChange); JPH_ASSERT(!mLinearVelocity.IsNaN()); } + inline void AddAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("AddAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mAngularVelocity += inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); } + inline void SubAngularVelocityStep(Vec3Arg inAngularVelocityChange) { JPH_DET_LOG("SubAngularVelocityStep: " << inAngularVelocityChange); JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); mAngularVelocity -= inAngularVelocityChange; JPH_ASSERT(!mAngularVelocity.IsNaN()); } + ///@} + + /// Apply the gyroscopic force (aka Dzhanibekov effect, see https://en.wikipedia.org/wiki/Tennis_racket_theorem) + inline void ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime); + + /// Apply all accumulated forces, torques and drag (should only be called by the PhysicsSystem) + inline void ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime); + + /// Access to the island index + uint32 GetIslandIndexInternal() const { return mIslandIndex; } + void SetIslandIndexInternal(uint32 inIndex) { mIslandIndex = inIndex; } + + /// Access to the index in the active bodies array + uint32 GetIndexInActiveBodiesInternal() const { return mIndexInActiveBodies; } + +#ifdef JPH_DOUBLE_PRECISION + inline DVec3 GetSleepTestOffset() const { return DVec3::sLoadDouble3Unsafe(mSleepTestOffset); } +#endif // JPH_DOUBLE_PRECISION + + /// Reset spheres to center around inPoints with radius 0 + inline void ResetSleepTestSpheres(const RVec3 *inPoints); + + /// Reset the sleep test timer without resetting the sleep test spheres + inline void ResetSleepTestTimer() { mSleepTestTimer = 0.0f; } + + /// Accumulate sleep time and return if a body can go to sleep + inline ECanSleep AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep); + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + static constexpr uint32 cInactiveIndex = uint32(-1); ///< Constant indicating that body is not active + +private: + friend class BodyManager; + friend class Body; + + // 1st cache line + // 16 byte aligned + Vec3 mLinearVelocity { Vec3::sZero() }; ///< World space linear velocity of the center of mass (m/s) + Vec3 mAngularVelocity { Vec3::sZero() }; ///< World space angular velocity (rad/s) + Vec3 mInvInertiaDiagonal; ///< Diagonal of inverse inertia matrix: D + Quat mInertiaRotation; ///< Rotation (R) that takes inverse inertia diagonal to local space: Ibody^-1 = R * D * R^-1 + + // 2nd cache line + // 4 byte aligned + Float3 mForce { 0, 0, 0 }; ///< Accumulated world space force (N). Note loaded through intrinsics so ensure that the 4 bytes after this are readable! + Float3 mTorque { 0, 0, 0 }; ///< Accumulated world space torque (N m). Note loaded through intrinsics so ensure that the 4 bytes after this are readable! + float mInvMass; ///< Inverse mass of the object (1/kg) + float mLinearDamping; ///< Linear damping: dv/dt = -c * v. c must be between 0 and 1 but is usually close to 0. + float mAngularDamping; ///< Angular damping: dw/dt = -c * w. c must be between 0 and 1 but is usually close to 0. + float mMaxLinearVelocity; ///< Maximum linear velocity that this body can reach (m/s) + float mMaxAngularVelocity; ///< Maximum angular velocity that this body can reach (rad/s) + float mGravityFactor; ///< Factor to multiply gravity with + uint32 mIndexInActiveBodies = cInactiveIndex; ///< If the body is active, this is the index in the active body list or cInactiveIndex if it is not active (note that there are 2 lists, one for rigid and one for soft bodies) + uint32 mIslandIndex = cInactiveIndex; ///< Index of the island that this body is part of, when the body has not yet been updated or is not active this is cInactiveIndex + + // 1 byte aligned + EMotionQuality mMotionQuality; ///< Motion quality, or how well it detects collisions when it has a high velocity + bool mAllowSleeping; ///< If this body can go to sleep + EAllowedDOFs mAllowedDOFs = EAllowedDOFs::All; ///< Allowed degrees of freedom for this body + uint8 mNumVelocityStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumPositionStepsOverride = 0; ///< Used only when this body is dynamic and colliding. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + + // 3rd cache line (least frequently used) + // 4 byte aligned (or 8 byte if running in double precision) +#ifdef JPH_DOUBLE_PRECISION + Double3 mSleepTestOffset; ///< mSleepTestSpheres are relative to this offset to prevent floating point inaccuracies. Warning: Loaded using sLoadDouble3Unsafe which will read 8 extra bytes. +#endif // JPH_DOUBLE_PRECISION + Sphere mSleepTestSpheres[3]; ///< Measure motion for 3 points on the body to see if it is resting: COM, COM + largest bounding box axis, COM + second largest bounding box axis + float mSleepTestTimer; ///< How long this body has been within the movement tolerance + +#ifdef JPH_ENABLE_ASSERTS + EBodyType mCachedBodyType; ///< Copied from Body::mBodyType and cached for asserting purposes + EMotionType mCachedMotionType; ///< Copied from Body::mMotionType and cached for asserting purposes +#endif +}; + +JPH_NAMESPACE_END + +#include "MotionProperties.inl" diff --git a/WickedEngine/Jolt/Physics/Body/MotionProperties.inl b/WickedEngine/Jolt/Physics/Body/MotionProperties.inl new file mode 100644 index 000000000..1fb044ef4 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/MotionProperties.inl @@ -0,0 +1,168 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRotation, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sPositionAccess, BodyAccess::EAccess::Read)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType != EMotionType::Static); + + // Calculate required linear velocity + mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime); + + // Calculate required angular velocity + Vec3 axis; + float angle; + inDeltaRotation.GetAxisAngle(axis, angle); + mAngularVelocity = LockAngular(axis * (angle / inDeltaTime)); +} + +void MotionProperties::ClampLinearVelocity() +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); + + float len_sq = mLinearVelocity.LengthSq(); + JPH_ASSERT(isfinite(len_sq)); + if (len_sq > Square(mMaxLinearVelocity)) + mLinearVelocity *= mMaxLinearVelocity / sqrt(len_sq); +} + +void MotionProperties::ClampAngularVelocity() +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); + + float len_sq = mAngularVelocity.LengthSq(); + JPH_ASSERT(isfinite(len_sq)); + if (len_sq > Square(mMaxAngularVelocity)) + mAngularVelocity *= mMaxAngularVelocity / sqrt(len_sq); +} + +inline Mat44 MotionProperties::GetLocalSpaceInverseInertiaUnchecked() const +{ + Mat44 rotation = Mat44::sRotation(mInertiaRotation); + Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1)); + return rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed); +} + +inline Mat44 MotionProperties::GetLocalSpaceInverseInertia() const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + return GetLocalSpaceInverseInertiaUnchecked(); +} + +Mat44 MotionProperties::GetInverseInertiaForRotation(Mat44Arg inRotation) const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + Mat44 rotation = inRotation.Multiply3x3(Mat44::sRotation(mInertiaRotation)); + Mat44 rotation_mul_scale_transposed(mInvInertiaDiagonal.SplatX() * rotation.GetColumn4(0), mInvInertiaDiagonal.SplatY() * rotation.GetColumn4(1), mInvInertiaDiagonal.SplatZ() * rotation.GetColumn4(2), Vec4(0, 0, 0, 1)); + Mat44 inverse_inertia = rotation.Multiply3x3RightTransposed(rotation_mul_scale_transposed); + + // We need to mask out both the rows and columns of DOFs that are not allowed + Vec4 angular_dofs_mask = GetAngularDOFsMask().ReinterpretAsFloat(); + inverse_inertia.SetColumn4(0, Vec4::sAnd(inverse_inertia.GetColumn4(0), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatX()))); + inverse_inertia.SetColumn4(1, Vec4::sAnd(inverse_inertia.GetColumn4(1), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatY()))); + inverse_inertia.SetColumn4(2, Vec4::sAnd(inverse_inertia.GetColumn4(2), Vec4::sAnd(angular_dofs_mask, angular_dofs_mask.SplatZ()))); + + return inverse_inertia; +} + +Vec3 MotionProperties::MultiplyWorldSpaceInverseInertiaByVector(QuatArg inBodyRotation, Vec3Arg inV) const +{ + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Mask out columns of DOFs that are not allowed + Vec3 angular_dofs_mask = Vec3(GetAngularDOFsMask().ReinterpretAsFloat()); + Vec3 v = Vec3::sAnd(inV, angular_dofs_mask); + + // Multiply vector by inverse inertia + Mat44 rotation = Mat44::sRotation(inBodyRotation * mInertiaRotation); + Vec3 result = rotation.Multiply3x3(mInvInertiaDiagonal * rotation.Multiply3x3Transposed(v)); + + // Mask out rows of DOFs that are not allowed + return Vec3::sAnd(result, angular_dofs_mask); +} + +void MotionProperties::ApplyGyroscopicForceInternal(QuatArg inBodyRotation, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Calculate local space inertia tensor (a diagonal in local space) + UVec4 is_zero = Vec3::sEquals(mInvInertiaDiagonal, Vec3::sZero()); + Vec3 denominator = Vec3::sSelect(mInvInertiaDiagonal, Vec3::sReplicate(1.0f), is_zero); + Vec3 nominator = Vec3::sSelect(Vec3::sReplicate(1.0f), Vec3::sZero(), is_zero); + Vec3 local_inertia = nominator / denominator; // Avoid dividing by zero, inertia in this axis will be zero + + // Calculate local space angular momentum + Quat inertia_space_to_world_space = inBodyRotation * mInertiaRotation; + Vec3 local_angular_velocity = inertia_space_to_world_space.Conjugated() * mAngularVelocity; + Vec3 local_momentum = local_inertia * local_angular_velocity; + + // The gyroscopic force applies a torque: T = -w x I w where w is angular velocity and I the inertia tensor + // Calculate the new angular momentum by applying the gyroscopic force and make sure the new magnitude is the same as the old one + // to avoid introducing energy into the system due to the Euler step + Vec3 new_local_momentum = local_momentum - inDeltaTime * local_angular_velocity.Cross(local_momentum); + float new_local_momentum_len_sq = new_local_momentum.LengthSq(); + new_local_momentum = new_local_momentum_len_sq > 0.0f? new_local_momentum * sqrt(local_momentum.LengthSq() / new_local_momentum_len_sq) : Vec3::sZero(); + + // Convert back to world space angular velocity + mAngularVelocity = inertia_space_to_world_space * (mInvInertiaDiagonal * new_local_momentum); +} + +void MotionProperties::ApplyForceTorqueAndDragInternal(QuatArg inBodyRotation, Vec3Arg inGravity, float inDeltaTime) +{ + JPH_ASSERT(BodyAccess::sCheckRights(BodyAccess::sVelocityAccess, BodyAccess::EAccess::ReadWrite)); + JPH_ASSERT(mCachedBodyType == EBodyType::RigidBody); + JPH_ASSERT(mCachedMotionType == EMotionType::Dynamic); + + // Update linear velocity + mLinearVelocity = LockTranslation(mLinearVelocity + inDeltaTime * (mGravityFactor * inGravity + mInvMass * GetAccumulatedForce())); + + // Update angular velocity + mAngularVelocity += inDeltaTime * MultiplyWorldSpaceInverseInertiaByVector(inBodyRotation, GetAccumulatedTorque()); + + // Linear damping: dv/dt = -c * v + // Solution: v(t) = v(0) * e^(-c * t) or v2 = v1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mLinearVelocity *= max(0.0f, 1.0f - mLinearDamping * inDeltaTime); + mAngularVelocity *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime); + + // Clamp velocities + ClampLinearVelocity(); + ClampAngularVelocity(); +} + +void MotionProperties::ResetSleepTestSpheres(const RVec3 *inPoints) +{ +#ifdef JPH_DOUBLE_PRECISION + // Make spheres relative to the first point and initialize them to zero radius + DVec3 offset = inPoints[0]; + offset.StoreDouble3(&mSleepTestOffset); + mSleepTestSpheres[0] = Sphere(Vec3::sZero(), 0.0f); + for (int i = 1; i < 3; ++i) + mSleepTestSpheres[i] = Sphere(Vec3(inPoints[i] - offset), 0.0f); +#else + // Initialize the spheres to zero radius around the supplied points + for (int i = 0; i < 3; ++i) + mSleepTestSpheres[i] = Sphere(inPoints[i], 0.0f); +#endif + + mSleepTestTimer = 0.0f; +} + +ECanSleep MotionProperties::AccumulateSleepTime(float inDeltaTime, float inTimeBeforeSleep) +{ + mSleepTestTimer += inDeltaTime; + return mSleepTestTimer >= inTimeBeforeSleep? ECanSleep::CanSleep : ECanSleep::CannotSleep; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/MotionQuality.h b/WickedEngine/Jolt/Physics/Body/MotionQuality.h new file mode 100644 index 000000000..b1ba343c0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/MotionQuality.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Motion quality, or how well it detects collisions when it has a high velocity +enum class EMotionQuality : uint8 +{ + /// Update the body in discrete steps. Body will tunnel throuh thin objects if its velocity is high enough. + /// This is the cheapest way of simulating a body. + Discrete, + + /// Update the body using linear casting. When stepping the body, its collision shape is cast from + /// start to destination using the starting rotation. The body will not be able to tunnel through thin + /// objects at high velocity, but tunneling is still possible if the body is long and thin and has high + /// angular velocity. Time is stolen from the object (which means it will move up to the first collision + /// and will not bounce off the surface until the next integration step). This will make the body appear + /// to go slower when it collides with high velocity. In order to not get stuck, the body is always + /// allowed to move by a fraction of it's inner radius, which may eventually lead it to pass through geometry. + /// + /// Note that if you're using a collision listener, you can receive contact added/persisted notifications of contacts + /// that may in the end not happen. This happens between bodies that are using casting: If bodies A and B collide at t1 + /// and B and C collide at t2 where t2 < t1 and A and C don't collide. In this case you may receive an incorrect contact + /// point added callback between A and B (which will be removed the next frame). + LinearCast, +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Body/MotionType.h b/WickedEngine/Jolt/Physics/Body/MotionType.h new file mode 100644 index 000000000..6de0d8c8e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Body/MotionType.h @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Motion type of a physics body +enum class EMotionType : uint8 +{ + Static, ///< Non movable + Kinematic, ///< Movable using velocities only, does not respond to forces + Dynamic, ///< Responds to forces as a normal physics object +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Character/Character.cpp b/WickedEngine/Jolt/Physics/Character/Character.cpp new file mode 100644 index 000000000..73914962c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Character/Character.cpp @@ -0,0 +1,318 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? static_cast(inSystem->GetBodyLockInterface()) : static_cast(inSystem->GetBodyLockInterfaceNoLock()); +} + +static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock(); +} + +static inline const NarrowPhaseQuery &sGetNarrowPhaseQuery(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetNarrowPhaseQuery() : inSystem->GetNarrowPhaseQueryNoLock(); +} + +Character::Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) : + CharacterBase(inSettings, inSystem), + mLayer(inSettings->mLayer) +{ + // Construct rigid body + BodyCreationSettings settings(mShape, inPosition, inRotation, EMotionType::Dynamic, mLayer); + settings.mAllowedDOFs = EAllowedDOFs::TranslationX | EAllowedDOFs::TranslationY | EAllowedDOFs::TranslationZ; + settings.mEnhancedInternalEdgeRemoval = inSettings->mEnhancedInternalEdgeRemoval; + settings.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + settings.mMassPropertiesOverride.mMass = inSettings->mMass; + settings.mFriction = inSettings->mFriction; + settings.mGravityFactor = inSettings->mGravityFactor; + settings.mUserData = inUserData; + const Body *body = mSystem->GetBodyInterface().CreateBody(settings); + if (body != nullptr) + mBodyID = body->GetID(); +} + +Character::~Character() +{ + // Destroy the body + mSystem->GetBodyInterface().DestroyBody(mBodyID); +} + +void Character::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddBody(mBodyID, inActivationMode); +} + +void Character::RemoveFromPhysicsSystem(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).RemoveBody(mBodyID); +} + +void Character::Activate(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).ActivateBody(mBodyID); +} + +void Character::CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Create query broadphase layer filter + DefaultBroadPhaseLayerFilter broadphase_layer_filter = mSystem->GetDefaultBroadPhaseLayerFilter(mLayer); + + // Create query object layer filter + DefaultObjectLayerFilter object_layer_filter = mSystem->GetDefaultLayerFilter(mLayer); + + // Ignore my own body + IgnoreSingleBodyFilter body_filter(mBodyID); + + // Settings for collide shape + CollideShapeSettings settings; + settings.mMaxSeparationDistance = inMaxSeparationDistance; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + settings.mActiveEdgeMovementDirection = inMovementDirection; + settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces; + + sGetNarrowPhaseQuery(mSystem, inLockBodies).CollideShape(inShape, Vec3::sReplicate(1.0f), inCenterOfMassTransform, settings, inBaseOffset, ioCollector, broadphase_layer_filter, object_layer_filter, body_filter); +} + +void Character::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Calculate center of mass transform + RMat44 center_of_mass = RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(inShape->GetCenterOfMass()); + + CheckCollision(center_of_mass, inMovementDirection, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies); +} + +void Character::CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const +{ + // Determine position and velocity of body + RMat44 query_transform; + Vec3 velocity; + { + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID); + if (!lock.Succeeded()) + return; + + const Body &body = lock.GetBody(); + + // Correct the center of mass transform for the difference between the old and new center of mass shape + query_transform = body.GetCenterOfMassTransform().PreTranslated(inShape->GetCenterOfMass() - mShape->GetCenterOfMass()); + velocity = body.GetLinearVelocity(); + } + + CheckCollision(query_transform, velocity, inMaxSeparationDistance, inShape, inBaseOffset, ioCollector, inLockBodies); +} + +void Character::PostSimulation(float inMaxSeparationDistance, bool inLockBodies) +{ + // Get character position, rotation and velocity + RVec3 char_pos; + Quat char_rot; + Vec3 char_vel; + { + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyID); + if (!lock.Succeeded()) + return; + const Body &body = lock.GetBody(); + char_pos = body.GetPosition(); + char_rot = body.GetRotation(); + char_vel = body.GetLinearVelocity(); + } + + // Collector that finds the hit with the normal that is the most 'up' + class MyCollector : public CollideShapeCollector + { + public: + // Constructor + explicit MyCollector(Vec3Arg inUp, RVec3 inBaseOffset) : mBaseOffset(inBaseOffset), mUp(inUp) { } + + // See: CollectorType::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + Vec3 normal = -inResult.mPenetrationAxis.Normalized(); + float dot = normal.Dot(mUp); + if (dot > mBestDot) // Find the hit that is most aligned with the up vector + { + mGroundBodyID = inResult.mBodyID2; + mGroundBodySubShapeID = inResult.mSubShapeID2; + mGroundPosition = mBaseOffset + inResult.mContactPointOn2; + mGroundNormal = normal; + mBestDot = dot; + } + } + + BodyID mGroundBodyID; + SubShapeID mGroundBodySubShapeID; + RVec3 mGroundPosition = RVec3::sZero(); + Vec3 mGroundNormal = Vec3::sZero(); + + private: + RVec3 mBaseOffset; + Vec3 mUp; + float mBestDot = -FLT_MAX; + }; + + // Collide shape + MyCollector collector(mUp, char_pos); + CheckCollision(char_pos, char_rot, char_vel, inMaxSeparationDistance, mShape, char_pos, collector, inLockBodies); + + // Copy results + mGroundBodyID = collector.mGroundBodyID; + mGroundBodySubShapeID = collector.mGroundBodySubShapeID; + mGroundPosition = collector.mGroundPosition; + mGroundNormal = collector.mGroundNormal; + + // Get additional data from body + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mGroundBodyID); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + + // Update ground state + RMat44 inv_transform = RMat44::sInverseRotationTranslation(char_rot, char_pos); + if (mSupportingVolume.SignedDistance(Vec3(inv_transform * mGroundPosition)) > 0.0f) + mGroundState = EGroundState::NotSupported; + else if (IsSlopeTooSteep(mGroundNormal)) + mGroundState = EGroundState::OnSteepGround; + else + mGroundState = EGroundState::OnGround; + + // Copy other body properties + mGroundMaterial = body.GetShape()->GetMaterial(mGroundBodySubShapeID); + mGroundVelocity = body.GetPointVelocity(mGroundPosition); + mGroundUserData = body.GetUserData(); + } + else + { + mGroundState = EGroundState::InAir; + mGroundMaterial = PhysicsMaterial::sDefault; + mGroundVelocity = Vec3::sZero(); + mGroundUserData = 0; + } +} + +void Character::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetLinearAndAngularVelocity(mBodyID, inLinearVelocity, inAngularVelocity); +} + +Vec3 Character::GetLinearVelocity(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetLinearVelocity(mBodyID); +} + +void Character::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetLinearVelocity(mBodyID, inLinearVelocity); +} + +void Character::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddLinearVelocity(mBodyID, inLinearVelocity); +} + +void Character::AddImpulse(Vec3Arg inImpulse, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).AddImpulse(mBodyID, inImpulse); +} + +void Character::GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const +{ + sGetBodyInterface(mSystem, inLockBodies).GetPositionAndRotation(mBodyID, outPosition, outRotation); +} + +void Character::SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) const +{ + sGetBodyInterface(mSystem, inLockBodies).SetPositionAndRotation(mBodyID, inPosition, inRotation, inActivationMode); +} + +RVec3 Character::GetPosition(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetPosition(mBodyID); +} + +void Character::SetPosition(RVec3Arg inPosition, EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetPosition(mBodyID, inPosition, inActivationMode); +} + +Quat Character::GetRotation(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetRotation(mBodyID); +} + +void Character::SetRotation(QuatArg inRotation, EActivation inActivationMode, bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).SetRotation(mBodyID, inRotation, inActivationMode); +} + +RVec3 Character::GetCenterOfMassPosition(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetCenterOfMassPosition(mBodyID); +} + +RMat44 Character::GetWorldTransform(bool inLockBodies) const +{ + return sGetBodyInterface(mSystem, inLockBodies).GetWorldTransform(mBodyID); +} + +void Character::SetLayer(ObjectLayer inLayer, bool inLockBodies) +{ + mLayer = inLayer; + + sGetBodyInterface(mSystem, inLockBodies).SetObjectLayer(mBodyID, inLayer); +} + +bool Character::SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies) +{ + if (inMaxPenetrationDepth < FLT_MAX) + { + // Collector that checks if there is anything in the way while switching to inShape + class MyCollector : public CollideShapeCollector + { + public: + // Constructor + explicit MyCollector(float inMaxPenetrationDepth) : mMaxPenetrationDepth(inMaxPenetrationDepth) { } + + // See: CollectorType::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + if (inResult.mPenetrationDepth > mMaxPenetrationDepth) + { + mHadCollision = true; + ForceEarlyOut(); + } + } + + float mMaxPenetrationDepth; + bool mHadCollision = false; + }; + + // Test if anything is in the way of switching + RVec3 char_pos = GetPosition(inLockBodies); + MyCollector collector(inMaxPenetrationDepth); + CheckCollision(inShape, 0.0f, char_pos, collector, inLockBodies); + if (collector.mHadCollision) + return false; + } + + // Switch the shape + mShape = inShape; + sGetBodyInterface(mSystem, inLockBodies).SetShape(mBodyID, mShape, false, EActivation::Activate); + return true; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Character/Character.h b/WickedEngine/Jolt/Physics/Character/Character.h new file mode 100644 index 000000000..b886045e4 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Character/Character.h @@ -0,0 +1,140 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Contains the configuration of a character +class JPH_EXPORT CharacterSettings : public CharacterBaseSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Layer that this character will be added to + ObjectLayer mLayer = 0; + + /// Mass of the character + float mMass = 80.0f; + + /// Friction for the character + float mFriction = 0.2f; + + /// Value to multiply gravity with for this character + float mGravityFactor = 1.0f; +}; + +/// Runtime character object. +/// This object usually represents the player or a humanoid AI. It uses a single rigid body, +/// usually with a capsule shape to simulate movement and collision for the character. +/// The character is a keyframed object, the application controls it by setting the velocity. +class JPH_EXPORT Character : public CharacterBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inSettings The settings for the character + /// @param inPosition Initial position for the character + /// @param inRotation Initial rotation for the character (usually only around Y) + /// @param inUserData Application specific value + /// @param inSystem Physics system that this character will be added to later + Character(const CharacterSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem); + + /// Destructor + virtual ~Character() override; + + /// Add bodies and constraints to the system and optionally activate the bodies + void AddToPhysicsSystem(EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Remove bodies and constraints from the system + void RemoveFromPhysicsSystem(bool inLockBodies = true); + + /// Wake up the character + void Activate(bool inLockBodies = true); + + /// Needs to be called after every PhysicsSystem::Update + /// @param inMaxSeparationDistance Max distance between the floor and the character to still consider the character standing on the floor + /// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false) + void PostSimulation(float inMaxSeparationDistance, bool inLockBodies = true); + + /// Control the velocity of the character + void SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true); + + /// Get the linear velocity of the character (m / s) + Vec3 GetLinearVelocity(bool inLockBodies = true) const; + + /// Set the linear velocity of the character (m / s) + void SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add world space linear velocity to current velocity (m / s) + void AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add impulse to the center of mass of the character + void AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true); + + /// Get the body associated with this character + BodyID GetBodyID() const { return mBodyID; } + + /// Get position / rotation of the body + void GetPositionAndRotation(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const; + + /// Set the position / rotation of the body, optionally activating it. + void SetPositionAndRotation(RVec3Arg inPosition, QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true) const; + + /// Get the position of the character + RVec3 GetPosition(bool inLockBodies = true) const; + + /// Set the position of the character, optionally activating it. + void SetPosition(RVec3Arg inPostion, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Get the rotation of the character + Quat GetRotation(bool inLockBodies = true) const; + + /// Set the rotation of the character, optionally activating it. + void SetRotation(QuatArg inRotation, EActivation inActivationMode = EActivation::Activate, bool inLockBodies = true); + + /// Position of the center of mass of the underlying rigid body + RVec3 GetCenterOfMassPosition(bool inLockBodies = true) const; + + /// Calculate the world transform of the character + RMat44 GetWorldTransform(bool inLockBodies = true) const; + + /// Update the layer of the character + void SetLayer(ObjectLayer inLayer, bool inLockBodies = true); + + /// Switch the shape of the character (e.g. for stance). When inMaxPenetrationDepth is not FLT_MAX, it checks + /// if the new shape collides before switching shape. Returns true if the switch succeeded. + bool SetShape(const Shape *inShape, float inMaxPenetrationDepth, bool inLockBodies = true); + + /// @brief Get all contacts for the character at a particular location + /// @param inPosition Position to test. + /// @param inRotation Rotation at which to test the shape. + /// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal. + /// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly). + /// @param inShape Shape to test collision with. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + /// @param inLockBodies If the collision query should use the locking body interface (true) or the non locking body interface (false) + void CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies = true) const; + +private: + /// Check collisions between inShape and the world using the center of mass transform + void CheckCollision(RMat44Arg inCenterOfMassTransform, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const; + + /// Check collisions between inShape and the world using the current position / rotation of the character + void CheckCollision(const Shape *inShape, float inMaxSeparationDistance, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, bool inLockBodies) const; + + /// The body of this character + BodyID mBodyID; + + /// The layer the body is in + ObjectLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Character/CharacterBase.cpp b/WickedEngine/Jolt/Physics/Character/CharacterBase.cpp new file mode 100644 index 000000000..d31425065 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Character/CharacterBase.cpp @@ -0,0 +1,59 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +CharacterBase::CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem) : + mSystem(inSystem), + mShape(inSettings->mShape), + mUp(inSettings->mUp), + mSupportingVolume(inSettings->mSupportingVolume) +{ + // Initialize max slope angle + SetMaxSlopeAngle(inSettings->mMaxSlopeAngle); +} + +const char *CharacterBase::sToString(EGroundState inState) +{ + switch (inState) + { + case EGroundState::OnGround: return "OnGround"; + case EGroundState::OnSteepGround: return "OnSteepGround"; + case EGroundState::NotSupported: return "NotSupported"; + case EGroundState::InAir: return "InAir"; + } + + JPH_ASSERT(false); + return "Unknown"; +} + +void CharacterBase::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mGroundState); + inStream.Write(mGroundBodyID); + inStream.Write(mGroundBodySubShapeID); + inStream.Write(mGroundPosition); + inStream.Write(mGroundNormal); + inStream.Write(mGroundVelocity); + // Can't save user data (may be a pointer) and material +} + +void CharacterBase::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mGroundState); + inStream.Read(mGroundBodyID); + inStream.Read(mGroundBodySubShapeID); + inStream.Read(mGroundPosition); + inStream.Read(mGroundNormal); + inStream.Read(mGroundVelocity); + mGroundUserData = 0; // Cannot restore user data + mGroundMaterial = PhysicsMaterial::sDefault; // Cannot restore material +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Character/CharacterBase.h b/WickedEngine/Jolt/Physics/Character/CharacterBase.h new file mode 100644 index 000000000..c5804af97 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Character/CharacterBase.h @@ -0,0 +1,157 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class StateRecorder; + +/// Base class for configuration of a character +class JPH_EXPORT CharacterBaseSettings : public RefTarget +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CharacterBaseSettings() = default; + CharacterBaseSettings(const CharacterBaseSettings &inSettings) = default; + CharacterBaseSettings & operator = (const CharacterBaseSettings &inSettings) = default; + + /// Virtual destructor + virtual ~CharacterBaseSettings() = default; + + /// Vector indicating the up direction of the character + Vec3 mUp = Vec3::sAxisY(); + + /// Plane, defined in local space relative to the character. Every contact behind this plane can support the + /// character, every contact in front of this plane is treated as only colliding with the player. + /// Default: Accept any contact. + Plane mSupportingVolume { Vec3::sAxisY(), -1.0e10f }; + + /// Maximum angle of slope that character can still walk on (radians). + float mMaxSlopeAngle = DegreesToRadians(50.0f); + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool mEnhancedInternalEdgeRemoval = false; + + /// Initial shape that represents the character's volume. + /// Usually this is a capsule, make sure the shape is made so that the bottom of the shape is at (0, 0, 0). + RefConst mShape; +}; + +/// Base class for character class +class JPH_EXPORT CharacterBase : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CharacterBase(const CharacterBaseSettings *inSettings, PhysicsSystem *inSystem); + + /// Destructor + virtual ~CharacterBase() = default; + + /// Set the maximum angle of slope that character can still walk on (radians) + void SetMaxSlopeAngle(float inMaxSlopeAngle) { mCosMaxSlopeAngle = Cos(inMaxSlopeAngle); } + float GetCosMaxSlopeAngle() const { return mCosMaxSlopeAngle; } + + /// Set the up vector for the character + void SetUp(Vec3Arg inUp) { mUp = inUp; } + Vec3 GetUp() const { return mUp; } + + /// Check if the normal of the ground surface is too steep to walk on + bool IsSlopeTooSteep(Vec3Arg inNormal) const + { + // If cos max slope angle is close to one the system is turned off, + // otherwise check the angle between the up and normal vector + return mCosMaxSlopeAngle < cNoMaxSlopeAngle && inNormal.Dot(mUp) < mCosMaxSlopeAngle; + } + + /// Get the current shape that the character is using. + const Shape * GetShape() const { return mShape; } + + enum class EGroundState + { + OnGround, ///< Character is on the ground and can move freely. + OnSteepGround, ///< Character is on a slope that is too steep and can't climb up any further. The caller should start applying downward velocity if sliding from the slope is desired. + NotSupported, ///< Character is touching an object, but is not supported by it and should fall. The GetGroundXXX functions will return information about the touched object. + InAir, ///< Character is in the air and is not touching anything. + }; + + /// Debug function to convert enum values to string + static const char * sToString(EGroundState inState); + + ///@name Properties of the ground this character is standing on + + /// Current ground state + EGroundState GetGroundState() const { return mGroundState; } + + /// Returns true if the player is supported by normal or steep ground + bool IsSupported() const { return mGroundState == EGroundState::OnGround || mGroundState == EGroundState::OnSteepGround; } + + /// Get the contact point with the ground + RVec3 GetGroundPosition() const { return mGroundPosition; } + + /// Get the contact normal with the ground + Vec3 GetGroundNormal() const { return mGroundNormal; } + + /// Velocity in world space of ground + Vec3 GetGroundVelocity() const { return mGroundVelocity; } + + /// Material that the character is standing on + const PhysicsMaterial * GetGroundMaterial() const { return mGroundMaterial; } + + /// BodyID of the object the character is standing on. Note may have been removed! + BodyID GetGroundBodyID() const { return mGroundBodyID; } + + /// Sub part of the body that we're standing on. + SubShapeID GetGroundSubShapeID() const { return mGroundBodySubShapeID; } + + /// User data value of the body that we're standing on + uint64 GetGroundUserData() const { return mGroundUserData; } + + // Saving / restoring state for replay + virtual void SaveState(StateRecorder &inStream) const; + virtual void RestoreState(StateRecorder &inStream); + +protected: + // Cached physics system + PhysicsSystem * mSystem; + + // The shape that the body currently has + RefConst mShape; + + // The character's world space up axis + Vec3 mUp; + + // Every contact behind this plane can support the character + Plane mSupportingVolume; + + // Beyond this value there is no max slope + static constexpr float cNoMaxSlopeAngle = 0.9999f; + + // Cosine of the maximum angle of slope that character can still walk on + float mCosMaxSlopeAngle; + + // Ground properties + EGroundState mGroundState = EGroundState::InAir; + BodyID mGroundBodyID; + SubShapeID mGroundBodySubShapeID; + RVec3 mGroundPosition = RVec3::sZero(); + Vec3 mGroundNormal = Vec3::sZero(); + Vec3 mGroundVelocity = Vec3::sZero(); + RefConst mGroundMaterial = PhysicsMaterial::sDefault; + uint64 mGroundUserData = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Character/CharacterVirtual.cpp b/WickedEngine/Jolt/Physics/Character/CharacterVirtual.cpp new file mode 100644 index 000000000..dc851832b --- /dev/null +++ b/WickedEngine/Jolt/Physics/Character/CharacterVirtual.cpp @@ -0,0 +1,1581 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem) : + CharacterBase(inSettings, inSystem), + mBackFaceMode(inSettings->mBackFaceMode), + mPredictiveContactDistance(inSettings->mPredictiveContactDistance), + mMaxCollisionIterations(inSettings->mMaxCollisionIterations), + mMaxConstraintIterations(inSettings->mMaxConstraintIterations), + mMinTimeRemaining(inSettings->mMinTimeRemaining), + mCollisionTolerance(inSettings->mCollisionTolerance), + mCharacterPadding(inSettings->mCharacterPadding), + mMaxNumHits(inSettings->mMaxNumHits), + mHitReductionCosMaxAngle(inSettings->mHitReductionCosMaxAngle), + mPenetrationRecoverySpeed(inSettings->mPenetrationRecoverySpeed), + mEnhancedInternalEdgeRemoval(inSettings->mEnhancedInternalEdgeRemoval), + mShapeOffset(inSettings->mShapeOffset), + mPosition(inPosition), + mRotation(inRotation), + mUserData(inUserData) +{ + // Copy settings + SetMaxStrength(inSettings->mMaxStrength); + SetMass(inSettings->mMass); +} + +void CharacterVirtual::GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const +{ + // Get real velocity of body + if (!inBody.IsStatic()) + { + const MotionProperties *mp = inBody.GetMotionPropertiesUnchecked(); + outLinearVelocity = mp->GetLinearVelocity(); + outAngularVelocity = mp->GetAngularVelocity(); + } + else + { + outLinearVelocity = outAngularVelocity = Vec3::sZero(); + } + + // Allow application to override + if (mListener != nullptr) + mListener->OnAdjustBodyVelocity(this, inBody, outLinearVelocity, outAngularVelocity); +} + +Vec3 CharacterVirtual::CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const +{ + // Get angular velocity + float angular_velocity_len_sq = inAngularVelocity.LengthSq(); + if (angular_velocity_len_sq < 1.0e-12f) + return inLinearVelocity; + float angular_velocity_len = sqrt(angular_velocity_len_sq); + + // Calculate the rotation that the object will make in the time step + Quat rotation = Quat::sRotation(inAngularVelocity / angular_velocity_len, angular_velocity_len * inDeltaTime); + + // Calculate where the new character position will be + RVec3 new_position = inCenterOfMass + rotation * Vec3(mPosition - inCenterOfMass); + + // Calculate the velocity + return inLinearVelocity + Vec3(new_position - mPosition) / inDeltaTime; +} + +template +void CharacterVirtual::sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult) +{ + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + inCharacter->GetAdjustedBodyVelocity(inBody, linear_velocity, angular_velocity); + + outContact.mPosition = inBaseOffset + inResult.mContactPointOn2; + outContact.mLinearVelocity = linear_velocity + angular_velocity.Cross(Vec3(outContact.mPosition - inBody.GetCenterOfMassPosition())); // Calculate point velocity + outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()); + outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, outContact.mPosition); + if (outContact.mContactNormal.Dot(outContact.mSurfaceNormal) < 0.0f) + outContact.mSurfaceNormal = -outContact.mSurfaceNormal; // Flip surface normal if we're hitting a back face + if (outContact.mContactNormal.Dot(inUp) > outContact.mSurfaceNormal.Dot(inUp)) + outContact.mSurfaceNormal = outContact.mContactNormal; // Replace surface normal with contact normal if the contact normal is pointing more upwards + outContact.mDistance = -inResult.mPenetrationDepth; + outContact.mBodyB = inResult.mBodyID2; + outContact.mSubShapeIDB = inResult.mSubShapeID2; + outContact.mMotionTypeB = inBody.GetMotionType(); + outContact.mIsSensorB = inBody.IsSensor(); + outContact.mUserData = inBody.GetUserData(); + outContact.mMaterial = inCollector.GetContext()->GetMaterial(inResult.mSubShapeID2); +} + +void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResult) +{ + // If we exceed our contact limit, try to clean up near-duplicate contacts + if (mContacts.size() == mMaxHits) + { + // Flag that we hit this code path + mMaxHitsExceeded = true; + + // Check if we can do reduction + if (mHitReductionCosMaxAngle > -1.0f) + { + // Loop all contacts and find similar contacts + for (int i = (int)mContacts.size() - 1; i >= 0; --i) + { + Contact &contact_i = mContacts[i]; + for (int j = i - 1; j >= 0; --j) + { + Contact &contact_j = mContacts[j]; + if (contact_i.mBodyB == contact_j.mBodyB // Same body + && contact_i.mContactNormal.Dot(contact_j.mContactNormal) > mHitReductionCosMaxAngle) // Very similar contact normals + { + // Remove the contact with the biggest distance + bool i_is_last = i == (int)mContacts.size() - 1; + if (contact_i.mDistance > contact_j.mDistance) + { + // Remove i + if (!i_is_last) + contact_i = mContacts.back(); + mContacts.pop_back(); + + // Break out of the loop, i is now an element that we already processed + break; + } + else + { + // Remove j + contact_j = mContacts.back(); + mContacts.pop_back(); + + // If i was the last element, we just moved it into position j. Break out of the loop, we'll see it again later. + if (i_is_last) + break; + } + } + } + } + } + + if (mContacts.size() == mMaxHits) + { + // There are still too many hits, give up! + ForceEarlyOut(); + return; + } + } + + BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2); + if (lock.SucceededAndIsInBroadPhase()) + { + mContacts.emplace_back(); + Contact &contact = mContacts.back(); + sFillContactProperties(mCharacter, contact, lock.GetBody(), mUp, mBaseOffset, *this, inResult); + contact.mFraction = 0.0f; + } +} + +void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inResult) +{ + // Should not have gotten here without a lower fraction + JPH_ASSERT(inResult.mFraction < mContact.mFraction); + + if (inResult.mFraction > 0.0f // Ignore collisions at fraction = 0 + && inResult.mPenetrationAxis.Dot(mDisplacement) > 0.0f) // Ignore penetrations that we're moving away from + { + // Test if this contact should be ignored + for (const IgnoredContact &c : mIgnoredContacts) + if (c.mBodyID == inResult.mBodyID2 && c.mSubShapeID == inResult.mSubShapeID2) + return; + + Contact contact; + + // Lock body only while we fetch contact properties + { + BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2); + if (!lock.SucceededAndIsInBroadPhase()) + return; + + // Sweeps don't result in OnContactAdded callbacks so we can ignore sensors here + const Body &body = lock.GetBody(); + if (body.IsSensor()) + return; + + // Convert the hit result into a contact + sFillContactProperties(mCharacter, contact, body, mUp, mBaseOffset, *this, inResult); + } + + contact.mFraction = inResult.mFraction; + + // Check if the contact that will make us penetrate more than the allowed tolerance + if (contact.mDistance + contact.mContactNormal.Dot(mDisplacement) < -mCharacter->mCollisionTolerance + && mCharacter->ValidateContact(contact)) + { + mContact = contact; + UpdateEarlyOutFraction(contact.mFraction); + } + } +} + +void CharacterVirtual::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Query shape transform + RMat44 transform = GetCenterOfMassTransform(inPosition, inRotation, inShape); + + // Settings for collide shape + CollideShapeSettings settings; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + settings.mBackFaceMode = mBackFaceMode; + settings.mActiveEdgeMovementDirection = inMovementDirection; + settings.mMaxSeparationDistance = mCharacterPadding + inMaxSeparationDistance; + + // Collide shape + if (mEnhancedInternalEdgeRemoval) + { + // Version that does additional work to remove internal edges + settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + + // This is a copy of NarrowPhaseQuery::CollideShape with additional logic to wrap the collector in an InternalEdgeRemovingCollector and flushing that collector after every body + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const Shape *inShape, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mShape(inShape), + mCenterOfMassTransform(inCenterOfMassTransform), + mBaseOffset(inBaseOffset), + mCollideShapeSettings(inCollideShapeSettings), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter), + mCollector(ioCollector) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // See NarrowPhaseQuery::CollideShape + if (mBodyFilter.ShouldCollide(inResult)) + { + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) + { + const Body &body = lock.GetBody(); + if (mBodyFilter.ShouldCollideLocked(body)) + { + TransformedShape ts = body.GetTransformedShape(); + mCollector.OnBody(body); + lock.ReleaseLock(); + ts.CollideShape(mShape, Vec3::sReplicate(1.0f), mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter); + + // After each body, we need to flush the InternalEdgeRemovingCollector because it uses 'ts' as context and it will go out of scope at the end of this block + mCollector.Flush(); + + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const Shape * mShape; + RMat44 mCenterOfMassTransform; + RVec3 mBaseOffset; + const CollideShapeSettings & mCollideShapeSettings; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + InternalEdgeRemovingCollector mCollector; + }; + + // Calculate bounds for shape and expand by max separation distance + AABox bounds = inShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f)); + bounds.ExpandBy(Vec3::sReplicate(settings.mMaxSeparationDistance)); + + // Do broadphase test + MyCollector collector(inShape, transform, settings, inBaseOffset, ioCollector, mSystem->GetBodyLockInterface(), inBodyFilter, inShapeFilter); + mSystem->GetBroadPhaseQuery().CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); + } + else + mSystem->GetNarrowPhaseQuery().CollideShape(inShape, Vec3::sReplicate(1.0f), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); +} + +void CharacterVirtual::GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Remove previous results + outContacts.clear(); + + // Collide shape + ContactCollector collector(mSystem, this, mMaxNumHits, mHitReductionCosMaxAngle, mUp, mPosition, outContacts); + CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, mPosition, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + // The broadphase bounding boxes will not be deterministic, which means that the order in which the contacts are received by the collector is not deterministic. + // Therefore we need to sort the contacts to preserve determinism. Note that currently this will fail if we exceed mMaxNumHits hits. + QuickSort(outContacts.begin(), outContacts.end(), ContactOrderingPredicate()); + + // Flag if we exceeded the max number of hits + mMaxHitsExceeded = collector.mMaxHitsExceeded; + + // Reduce distance to contact by padding to ensure we stay away from the object by a little margin + // (this will make collision detection cheaper - especially for sweep tests as they won't hit the surface if we're properly sliding) + for (Contact &c : outContacts) + c.mDistance -= mCharacterPadding; +} + +void CharacterVirtual::RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const +{ + // Only use this algorithm if we're penetrating further than this (due to numerical precision issues we can always penetrate a little bit and we don't want to discard contacts if they just have a tiny penetration) + // We do need to account for padding (see GetContactsAtPosition) that is removed from the contact distances, to compensate we add it to the cMinRequiredPenetration + const float cMinRequiredPenetration = 1.25f * mCharacterPadding; + + // Discard conflicting penetrating contacts + for (size_t c1 = 0; c1 < ioContacts.size(); c1++) + { + Contact &contact1 = ioContacts[c1]; + if (contact1.mDistance <= -cMinRequiredPenetration) // Only for penetrations + for (size_t c2 = c1 + 1; c2 < ioContacts.size(); c2++) + { + Contact &contact2 = ioContacts[c2]; + if (contact1.mBodyB == contact2.mBodyB // Only same body + && contact2.mDistance <= -cMinRequiredPenetration // Only for penetrations + && contact1.mContactNormal.Dot(contact2.mContactNormal) < 0.0f) // Only opposing normals + { + // Discard contacts with the least amount of penetration + if (contact1.mDistance < contact2.mDistance) + { + // Discard the 2nd contact + outIgnoredContacts.emplace_back(contact2.mBodyB, contact2.mSubShapeIDB); + ioContacts.erase(ioContacts.begin() + c2); + c2--; + } + else + { + // Discard the first contact + outIgnoredContacts.emplace_back(contact1.mBodyB, contact1.mSubShapeIDB); + ioContacts.erase(ioContacts.begin() + c1); + c1--; + break; + } + } + } + } +} + +bool CharacterVirtual::ValidateContact(const Contact &inContact) const +{ + if (mListener == nullptr) + return true; + + return mListener->OnContactValidate(this, inContact.mBodyB, inContact.mSubShapeIDB); +} + +template +inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat44Arg inStart, Vec3Arg inDisplacement, const T &inPolygon, float &ioFraction) +{ + if (inShape->GetType() == EShapeType::Convex) + { + // Get the support function for the shape we're casting + const ConvexShape *convex_shape = static_cast(inShape); + ConvexShape::SupportBuffer buffer; + const ConvexShape::Support *support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + + // Cast the shape against the polygon + GJKClosestPoint gjk; + return gjk.CastShape(inStart, inDisplacement, cDefaultCollisionTolerance, *support, inPolygon, ioFraction); + } + else if (inShape->GetSubType() == EShapeSubType::RotatedTranslated) + { + const RotatedTranslatedShape *rt_shape = static_cast(inShape); + return sCorrectFractionForCharacterPadding(rt_shape->GetInnerShape(), inStart * Mat44::sRotation(rt_shape->GetRotation()), inDisplacement, inPolygon, ioFraction); + } + else + { + JPH_ASSERT(false, "Not supported yet!"); + return false; + } +} + +bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + // Too small distance -> skip checking + float displacement_len_sq = inDisplacement.LengthSq(); + if (displacement_len_sq < 1.0e-8f) + return false; + + // Calculate start transform + RMat44 start = GetCenterOfMassTransform(inPosition, mRotation, mShape); + + // Settings for the cast + ShapeCastSettings settings; + settings.mBackFaceModeTriangles = mBackFaceMode; + settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = false; + + // Calculate how much extra fraction we need to add to the cast to account for the character padding + float character_padding_fraction = mCharacterPadding / sqrt(displacement_len_sq); + + // Cast shape + Contact contact; + contact.mFraction = 1.0f + character_padding_fraction; + ContactCastCollector collector(mSystem, this, inDisplacement, mUp, inIgnoredContacts, start.GetTranslation(), contact); + collector.ResetEarlyOutFraction(contact.mFraction); + RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement); + mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, start.GetTranslation(), collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + if (contact.mBodyB.IsInvalid()) + return false; + + // Store contact + outContact = contact; + + // Fetch the face we're colliding with + TransformedShape ts = mSystem->GetBodyInterface().GetTransformedShape(outContact.mBodyB); + Shape::SupportingFace face; + ts.GetSupportingFace(outContact.mSubShapeIDB, -outContact.mContactNormal, start.GetTranslation(), face); + + bool corrected = false; + if (face.size() >= 2) + { + // Inflate the colliding face by the character padding + PolygonConvexSupport polygon(face); + AddConvexRadius add_cvx(polygon, mCharacterPadding); + + // Correct fraction to hit this inflated face instead of the inner shape + corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, add_cvx, outContact.mFraction); + } + if (!corrected) + { + // When there's only a single contact point or when we were unable to correct the fraction, + // we can just move the fraction back so that the character and its padding don't hit the contact point anymore + outContact.mFraction = max(0.0f, outContact.mFraction - character_padding_fraction); + } + + // Ensure that we never return a fraction that's bigger than 1 (which could happen due to float precision issues). + outContact.mFraction = min(outContact.mFraction, 1.0f); + + return true; +} + +void CharacterVirtual::DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const +{ + for (Contact &c : inContacts) + { + Vec3 contact_velocity = c.mLinearVelocity; + + // Penetrating contact: Add a contact velocity that pushes the character out at the desired speed + if (c.mDistance < 0.0f) + contact_velocity -= c.mContactNormal * c.mDistance * mPenetrationRecoverySpeed / inDeltaTime; + + // Convert to a constraint + outConstraints.emplace_back(); + Constraint &constraint = outConstraints.back(); + constraint.mContact = &c; + constraint.mLinearVelocity = contact_velocity; + constraint.mPlane = Plane(c.mContactNormal, c.mDistance); + + // Next check if the angle is too steep and if it is add an additional constraint that holds the character back + if (IsSlopeTooSteep(c.mSurfaceNormal)) + { + // Only take planes that point up. + // Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement. + float dot = c.mContactNormal.Dot(mUp); + if (dot > 1.0e-3f) // Add a little slack, if the normal is perfectly horizontal we already have our vertical plane. + { + // Mark the slope constraint as steep + constraint.mIsSteepSlope = true; + + // Make horizontal normal + Vec3 normal = (c.mContactNormal - dot * mUp).Normalized(); + + // Create a secondary constraint that blocks horizontal movement + outConstraints.emplace_back(); + Constraint &vertical_constraint = outConstraints.back(); + vertical_constraint.mContact = &c; + vertical_constraint.mLinearVelocity = contact_velocity.Dot(normal) * normal; // Project the contact velocity on the new normal so that both planes push at an equal rate + vertical_constraint.mPlane = Plane(normal, c.mDistance / normal.Dot(c.mContactNormal)); // Calculate the distance we have to travel horizontally to hit the contact plane + } + } + } +} + +bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const +{ + Contact &contact = *ioConstraint.mContact; + + // Validate the contact point + if (!ValidateContact(contact)) + return false; + + // Send contact added event + CharacterContactSettings settings; + if (mListener != nullptr) + mListener->OnContactAdded(this, contact.mBodyB, contact.mSubShapeIDB, contact.mPosition, -contact.mContactNormal, settings); + contact.mCanPushCharacter = settings.mCanPushCharacter; + + // We don't have any further interaction with sensors beyond an OnContactAdded notification + if (contact.mIsSensorB) + return false; + + // If body B cannot receive an impulse, we're done + if (!settings.mCanReceiveImpulses || contact.mMotionTypeB != EMotionType::Dynamic) + return true; + + // Lock the body we're colliding with + BodyLockWrite lock(mSystem->GetBodyLockInterface(), contact.mBodyB); + if (!lock.SucceededAndIsInBroadPhase()) + return false; // Body has been removed, we should not collide with it anymore + const Body &body = lock.GetBody(); + + // Calculate the velocity that we want to apply at B so that it will start moving at the character's speed at the contact point + constexpr float cDamping = 0.9f; + constexpr float cPenetrationResolution = 0.4f; + Vec3 relative_velocity = inVelocity - contact.mLinearVelocity; + float projected_velocity = relative_velocity.Dot(contact.mContactNormal); + float delta_velocity = -projected_velocity * cDamping - min(contact.mDistance, 0.0f) * cPenetrationResolution / inDeltaTime; + + // Don't apply impulses if we're separating + if (delta_velocity < 0.0f) + return true; + + // Determine mass properties of the body we're colliding with + const MotionProperties *motion_properties = body.GetMotionProperties(); + RVec3 center_of_mass = body.GetCenterOfMassPosition(); + Mat44 inverse_inertia = body.GetInverseInertia(); + float inverse_mass = motion_properties->GetInverseMass(); + + // Calculate the inverse of the mass of body B as seen at the contact point in the direction of the contact normal + Vec3 jacobian = Vec3(contact.mPosition - center_of_mass).Cross(contact.mContactNormal); + float inv_effective_mass = inverse_inertia.Multiply3x3(jacobian).Dot(jacobian) + inverse_mass; + + // Impulse P = M dv + float impulse = delta_velocity / inv_effective_mass; + + // Clamp the impulse according to the character strength, character strength is a force in newtons, P = F dt + float max_impulse = mMaxStrength * inDeltaTime; + impulse = min(impulse, max_impulse); + + // Calculate the world space impulse to apply + Vec3 world_impulse = -impulse * contact.mContactNormal; + + // Cancel impulse in down direction (we apply gravity later) + float impulse_dot_up = world_impulse.Dot(mUp); + if (impulse_dot_up < 0.0f) + world_impulse -= impulse_dot_up * mUp; + + // Now apply the impulse (body is already locked so we use the no-lock interface) + mSystem->GetBodyInterfaceNoLock().AddImpulse(contact.mBodyB, world_impulse, contact.mPosition); + return true; +} + +void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator +#ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints +#endif // JPH_DEBUG_RENDERER + ) const +{ + // If there are no constraints we can immediately move to our target + if (ioConstraints.empty()) + { + outDisplacement = inVelocity * inTimeRemaining; + outTimeSimulated = inTimeRemaining; + return; + } + + // Create array that holds the constraints in order of time of impact (sort will happen later) + Array> sorted_constraints(inAllocator); + sorted_constraints.resize(ioConstraints.size()); + for (size_t index = 0; index < sorted_constraints.size(); index++) + sorted_constraints[index] = &ioConstraints[index]; + + // This is the velocity we use for the displacement, if we hit something it will be shortened + Vec3 velocity = inVelocity; + + // Keep track of the last velocity that was applied to the character so that we can detect when the velocity reverses + Vec3 last_velocity = inVelocity; + + // Start with no displacement + outDisplacement = Vec3::sZero(); + outTimeSimulated = 0.0f; + + // These are the contacts that we hit previously without moving a significant distance + Array> previous_contacts(inAllocator); + previous_contacts.resize(mMaxConstraintIterations); + int num_previous_contacts = 0; + + // Loop for a max amount of iterations + for (uint iteration = 0; iteration < mMaxConstraintIterations; iteration++) + { + // Calculate time of impact for all constraints + for (Constraint &c : ioConstraints) + { + // Project velocity on plane direction + c.mProjectedVelocity = c.mPlane.GetNormal().Dot(c.mLinearVelocity - velocity); + if (c.mProjectedVelocity < 1.0e-6f) + { + c.mTOI = FLT_MAX; + } + else + { + // Distance to plane + float dist = c.mPlane.SignedDistance(outDisplacement); + + if (dist - c.mProjectedVelocity * inTimeRemaining > -1.0e-4f) + { + // Too little penetration, accept the movement + c.mTOI = FLT_MAX; + } + else + { + // Calculate time of impact + c.mTOI = max(0.0f, dist / c.mProjectedVelocity); + } + } + } + + // Sort constraints on proximity + QuickSort(sorted_constraints.begin(), sorted_constraints.end(), [](const Constraint *inLHS, const Constraint *inRHS) { + // If both constraints hit at t = 0 then order the one that will push the character furthest first + // Note that because we add velocity to penetrating contacts, this will also resolve contacts that penetrate the most + if (inLHS->mTOI <= 0.0f && inRHS->mTOI <= 0.0f) + return inLHS->mProjectedVelocity > inRHS->mProjectedVelocity; + + // Then sort on time of impact + if (inLHS->mTOI != inRHS->mTOI) + return inLHS->mTOI < inRHS->mTOI; + + // As a tie breaker sort static first so it has the most influence + return inLHS->mContact->mMotionTypeB > inRHS->mContact->mMotionTypeB; + }); + + // Find the first valid constraint + Constraint *constraint = nullptr; + for (Constraint *c : sorted_constraints) + { + // Take the first contact and see if we can reach it + if (c->mTOI >= inTimeRemaining) + { + // We can reach our goal! + outDisplacement += velocity * inTimeRemaining; + outTimeSimulated += inTimeRemaining; + return; + } + + // Test if this contact was discarded by the contact callback before + if (c->mContact->mWasDiscarded) + continue; + + // Check if we made contact with this before + if (!c->mContact->mHadCollision) + { + // Handle the contact + if (!HandleContact(velocity, *c, inDeltaTime)) + { + // Constraint should be ignored, remove it from the list + c->mContact->mWasDiscarded = true; + + // Mark it as ignored for GetFirstContactForSweep + ioIgnoredContacts.emplace_back(c->mContact->mBodyB, c->mContact->mSubShapeIDB); + continue; + } + + c->mContact->mHadCollision = true; + } + + // Cancel velocity of constraint if it cannot push the character + if (!c->mContact->mCanPushCharacter) + c->mLinearVelocity = Vec3::sZero(); + + // We found the first constraint that we want to collide with + constraint = c; + break; + } + + if (constraint == nullptr) + { + // All constraints were discarded, we can reach our goal! + outDisplacement += velocity * inTimeRemaining; + outTimeSimulated += inTimeRemaining; + return; + } + + // Move to the contact + outDisplacement += velocity * constraint->mTOI; + inTimeRemaining -= constraint->mTOI; + outTimeSimulated += constraint->mTOI; + + // If there's not enough time left to be simulated, bail + if (inTimeRemaining < mMinTimeRemaining) + return; + + // If we've moved significantly, clear all previous contacts + if (constraint->mTOI > 1.0e-4f) + num_previous_contacts = 0; + + // Get the normal of the plane we're hitting + Vec3 plane_normal = constraint->mPlane.GetNormal(); + + // If we're hitting a steep slope we cancel the velocity towards the slope first so that we don't end up sliding up the slope + // (we may hit the slope before the vertical wall constraint we added which will result in a small movement up causing jitter in the character movement) + if (constraint->mIsSteepSlope) + { + // We're hitting a steep slope, create a vertical plane that blocks any further movement up the slope (note: not normalized) + Vec3 vertical_plane_normal = plane_normal - plane_normal.Dot(mUp) * mUp; + + // Get the relative velocity between the character and the constraint + Vec3 relative_velocity = velocity - constraint->mLinearVelocity; + + // Remove velocity towards the slope + velocity = velocity - min(0.0f, relative_velocity.Dot(vertical_plane_normal)) * vertical_plane_normal / vertical_plane_normal.LengthSq(); + } + + // Get the relative velocity between the character and the constraint + Vec3 relative_velocity = velocity - constraint->mLinearVelocity; + + // Calculate new velocity if we cancel the relative velocity in the normal direction + Vec3 new_velocity = velocity - relative_velocity.Dot(plane_normal) * plane_normal; + + // Find the normal of the previous contact that we will violate the most if we move in this new direction + float highest_penetration = 0.0f; + Constraint *other_constraint = nullptr; + for (Constraint **c = previous_contacts.data(); c < previous_contacts.data() + num_previous_contacts; ++c) + if (*c != constraint) + { + // Calculate how much we will penetrate if we move in this direction + Vec3 other_normal = (*c)->mPlane.GetNormal(); + float penetration = ((*c)->mLinearVelocity - new_velocity).Dot(other_normal); + if (penetration > highest_penetration) + { + // We don't want parallel or anti-parallel normals as that will cause our cross product below to become zero. Slack is approx 10 degrees. + float dot = other_normal.Dot(plane_normal); + if (dot < 0.984f && dot > -0.984f) + { + highest_penetration = penetration; + other_constraint = *c; + } + } + } + + // Check if we found a 2nd constraint + if (other_constraint != nullptr) + { + // Calculate the sliding direction and project the new velocity onto that sliding direction + Vec3 other_normal = other_constraint->mPlane.GetNormal(); + Vec3 slide_dir = plane_normal.Cross(other_normal).Normalized(); + Vec3 velocity_in_slide_dir = new_velocity.Dot(slide_dir) * slide_dir; + + // Cancel the constraint velocity in the other constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes + constraint->mLinearVelocity -= min(0.0f, constraint->mLinearVelocity.Dot(other_normal)) * other_normal; + + // Cancel the other constraints velocity in this constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes + other_constraint->mLinearVelocity -= min(0.0f, other_constraint->mLinearVelocity.Dot(plane_normal)) * plane_normal; + + // Calculate the velocity of this constraint perpendicular to the slide direction + Vec3 perpendicular_velocity = constraint->mLinearVelocity - constraint->mLinearVelocity.Dot(slide_dir) * slide_dir; + + // Calculate the velocity of the other constraint perpendicular to the slide direction + Vec3 other_perpendicular_velocity = other_constraint->mLinearVelocity - other_constraint->mLinearVelocity.Dot(slide_dir) * slide_dir; + + // Add all components together + new_velocity = velocity_in_slide_dir + perpendicular_velocity + other_perpendicular_velocity; + } + + // Allow application to modify calculated velocity + if (mListener != nullptr) + mListener->OnContactSolve(this, constraint->mContact->mBodyB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity); + +#ifdef JPH_DEBUG_RENDERER + if (inDrawConstraints) + { + // Calculate where to draw + RVec3 offset = mPosition + Vec3(0, 0, 2.5f * (iteration + 1)); + + // Draw constraint plane + DebugRenderer::sInstance->DrawPlane(offset, constraint->mPlane.GetNormal(), Color::sCyan, 1.0f); + + // Draw 2nd constraint plane + if (other_constraint != nullptr) + DebugRenderer::sInstance->DrawPlane(offset, other_constraint->mPlane.GetNormal(), Color::sBlue, 1.0f); + + // Draw starting velocity + DebugRenderer::sInstance->DrawArrow(offset, offset + velocity, Color::sGreen, 0.05f); + + // Draw resulting velocity + DebugRenderer::sInstance->DrawArrow(offset, offset + new_velocity, Color::sRed, 0.05f); + } +#endif // JPH_DEBUG_RENDERER + + // Update the velocity + velocity = new_velocity; + + // Add the contact to the list so that next iteration we can avoid violating it again + previous_contacts[num_previous_contacts] = constraint; + num_previous_contacts++; + + // Check early out + if (constraint->mProjectedVelocity < 1.0e-8f // Constraint should not be pushing, otherwise there may be other constraints that are pushing us + && velocity.LengthSq() < 1.0e-8f) // There's not enough velocity left + return; + + // If the constraint has velocity we accept the new velocity, otherwise check that we didn't reverse velocity + if (!constraint->mLinearVelocity.IsNearZero(1.0e-8f)) + last_velocity = constraint->mLinearVelocity; + else if (velocity.Dot(last_velocity) < 0.0f) + return; + } +} + +void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator) +{ + // Flag contacts as having a collision if they're close enough but ignore contacts we're moving away from. + // Note that if we did MoveShape before we want to preserve any contacts that it marked as colliding + for (Contact &c : mActiveContacts) + if (!c.mWasDiscarded + && !c.mHadCollision + && c.mDistance < mCollisionTolerance + && (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 1.0e-4f)) + { + if (ValidateContact(c) && !c.mIsSensorB) + c.mHadCollision = true; + else + c.mWasDiscarded = true; + } + + // Calculate transform that takes us to character local space + RMat44 inv_transform = RMat44::sInverseRotationTranslation(mRotation, mPosition); + + // Determine if we're supported or not + int num_supported = 0; + int num_sliding = 0; + int num_avg_normal = 0; + Vec3 avg_normal = Vec3::sZero(); + Vec3 avg_velocity = Vec3::sZero(); + const Contact *supporting_contact = nullptr; + float max_cos_angle = -FLT_MAX; + const Contact *deepest_contact = nullptr; + float smallest_distance = FLT_MAX; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + { + // Calculate the angle between the plane normal and the up direction + float cos_angle = c.mSurfaceNormal.Dot(mUp); + + // Find the deepest contact + if (c.mDistance < smallest_distance) + { + deepest_contact = &c; + smallest_distance = c.mDistance; + } + + // If this contact is in front of our plane, we cannot be supported by it + if (mSupportingVolume.SignedDistance(Vec3(inv_transform * c.mPosition)) > 0.0f) + continue; + + // Find the contact with the normal that is pointing most upwards and store it + if (max_cos_angle < cos_angle) + { + supporting_contact = &c; + max_cos_angle = cos_angle; + } + + // Check if this is a sliding or supported contact + bool is_supported = mCosMaxSlopeAngle > cNoMaxSlopeAngle || cos_angle >= mCosMaxSlopeAngle; + if (is_supported) + num_supported++; + else + num_sliding++; + + // If the angle between the two is less than 85 degrees we also use it to calculate the average normal + if (cos_angle >= 0.08f) + { + avg_normal += c.mSurfaceNormal; + num_avg_normal++; + + // For static or dynamic objects or for contacts that don't support us just take the contact velocity + if (c.mMotionTypeB != EMotionType::Kinematic || !is_supported) + avg_velocity += c.mLinearVelocity; + else + { + // For keyframed objects that support us calculate the velocity at our position rather than at the contact position so that we properly follow the object + BodyLockRead lock(mSystem->GetBodyLockInterface(), c.mBodyB); + if (lock.SucceededAndIsInBroadPhase()) + { + const Body &body = lock.GetBody(); + + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity); + + // Calculate the ground velocity + avg_velocity += CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime); + } + else + { + // Fall back to contact velocity + avg_velocity += c.mLinearVelocity; + } + } + } + } + + // Take either the most supporting contact or the deepest contact + const Contact *best_contact = supporting_contact != nullptr? supporting_contact : deepest_contact; + + // Calculate average normal and velocity + if (num_avg_normal >= 1) + { + mGroundNormal = avg_normal.Normalized(); + mGroundVelocity = avg_velocity / float(num_avg_normal); + } + else if (best_contact != nullptr) + { + mGroundNormal = best_contact->mSurfaceNormal; + mGroundVelocity = best_contact->mLinearVelocity; + } + else + { + mGroundNormal = Vec3::sZero(); + mGroundVelocity = Vec3::sZero(); + } + + // Copy contact properties + if (best_contact != nullptr) + { + mGroundBodyID = best_contact->mBodyB; + mGroundBodySubShapeID = best_contact->mSubShapeIDB; + mGroundPosition = best_contact->mPosition; + mGroundMaterial = best_contact->mMaterial; + mGroundUserData = best_contact->mUserData; + } + else + { + mGroundBodyID = BodyID(); + mGroundBodySubShapeID = SubShapeID(); + mGroundPosition = RVec3::sZero(); + mGroundMaterial = PhysicsMaterial::sDefault; + mGroundUserData = 0; + } + + // Determine ground state + if (num_supported > 0) + { + // We made contact with something that supports us + mGroundState = EGroundState::OnGround; + } + else if (num_sliding > 0) + { + if ((mLinearVelocity - deepest_contact->mLinearVelocity).Dot(mUp) > 1.0e-4f) + { + // We cannot be on ground if we're moving upwards relative to the ground + mGroundState = EGroundState::OnSteepGround; + } + else + { + // If we're sliding down, we may actually be standing on multiple sliding contacts in such a way that we can't slide off, in this case we're also supported + + // Convert the contacts into constraints + TempContactList contacts(mActiveContacts.begin(), mActiveContacts.end(), inAllocator); + ConstraintList constraints(inAllocator); + constraints.reserve(contacts.size() * 2); + DetermineConstraints(contacts, mLastDeltaTime, constraints); + + // Solve the displacement using these constraints, this is used to check if we didn't move at all because we are supported + Vec3 displacement; + float time_simulated; + IgnoredContactList ignored_contacts(inAllocator); + ignored_contacts.reserve(contacts.size()); + SolveConstraints(-mUp, 1.0f, 1.0f, constraints, ignored_contacts, time_simulated, displacement, inAllocator); + + // If we're blocked then we're supported, otherwise we're sliding + float min_required_displacement_sq = Square(0.6f * mLastDeltaTime); + if (time_simulated < 0.001f || displacement.LengthSq() < min_required_displacement_sq) + mGroundState = EGroundState::OnGround; + else + mGroundState = EGroundState::OnSteepGround; + } + } + else + { + // Not supported by anything + mGroundState = best_contact != nullptr? EGroundState::NotSupported : EGroundState::InAir; + } +} + +void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator) +{ + mActiveContacts.assign(inContacts.begin(), inContacts.end()); + + UpdateSupportingContact(true, inAllocator); +} + +void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator +#ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints +#endif // JPH_DEBUG_RENDERER + ) const +{ + JPH_DET_LOG("CharacterVirtual::MoveShape: pos: " << ioPosition << " vel: " << inVelocity << " dt: " << inDeltaTime); + + Vec3 movement_direction = inVelocity.NormalizedOr(Vec3::sZero()); + + float time_remaining = inDeltaTime; + for (uint iteration = 0; iteration < mMaxCollisionIterations && time_remaining >= mMinTimeRemaining; iteration++) + { + JPH_DET_LOG("iter: " << iteration << " time: " << time_remaining); + + // Determine contacts in the neighborhood + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(ioPosition, movement_direction, mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + +#ifdef JPH_ENABLE_DETERMINISM_LOG + for (const Contact &c : contacts) + JPH_DET_LOG("contact: " << c.mPosition << " vel: " << c.mLinearVelocity << " cnormal: " << c.mContactNormal << " snormal: " << c.mSurfaceNormal << " dist: " << c.mDistance << " fraction: " << c.mFraction << " body: " << c.mBodyB << " subshape: " << c.mSubShapeIDB); +#endif // JPH_ENABLE_DETERMINISM_LOG + + // Remove contacts with the same body that have conflicting normals + IgnoredContactList ignored_contacts(inAllocator); + ignored_contacts.reserve(contacts.size()); + RemoveConflictingContacts(contacts, ignored_contacts); + + // Convert contacts into constraints + ConstraintList constraints(inAllocator); + constraints.reserve(contacts.size() * 2); + DetermineConstraints(contacts, inDeltaTime, constraints); + +#ifdef JPH_DEBUG_RENDERER + bool draw_constraints = inDrawConstraints && iteration == 0; + if (draw_constraints) + { + for (const Constraint &c : constraints) + { + // Draw contact point + DebugRenderer::sInstance->DrawMarker(c.mContact->mPosition, Color::sYellow, 0.05f); + Vec3 dist_to_plane = -c.mPlane.GetConstant() * c.mPlane.GetNormal(); + + // Draw arrow towards surface that we're hitting + DebugRenderer::sInstance->DrawArrow(c.mContact->mPosition, c.mContact->mPosition - dist_to_plane, Color::sYellow, 0.05f); + + // Draw plane around the player position indicating the space that we can move + DebugRenderer::sInstance->DrawPlane(mPosition + dist_to_plane, c.mPlane.GetNormal(), Color::sCyan, 1.0f); + DebugRenderer::sInstance->DrawArrow(mPosition + dist_to_plane, mPosition + dist_to_plane + c.mContact->mSurfaceNormal, Color::sRed, 0.05f); + } + } +#endif // JPH_DEBUG_RENDERER + + // Solve the displacement using these constraints + Vec3 displacement; + float time_simulated; + SolveConstraints(inVelocity, inDeltaTime, time_remaining, constraints, ignored_contacts, time_simulated, displacement, inAllocator + #ifdef JPH_DEBUG_RENDERER + , draw_constraints + #endif // JPH_DEBUG_RENDERER + ); + + // Store the contacts now that the colliding ones have been marked + if (outActiveContacts != nullptr) + outActiveContacts->assign(contacts.begin(), contacts.end()); + + // Do a sweep to test if the path is really unobstructed + Contact cast_contact; + if (GetFirstContactForSweep(ioPosition, displacement, cast_contact, ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + { + displacement *= cast_contact.mFraction; + time_simulated *= cast_contact.mFraction; + } + + // Update the position + ioPosition += displacement; + time_remaining -= time_simulated; + + // If the displacement during this iteration was too small we assume we cannot further progress this update + if (displacement.LengthSq() < 1.0e-8f) + break; + } +} + +Vec3 CharacterVirtual::CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const +{ + // If we're not pushing against a steep slope, return the desired velocity + // Note: This is important as WalkStairs overrides the ground state to OnGround when its first check fails but the second succeeds + if (mGroundState == CharacterVirtual::EGroundState::OnGround + || mGroundState == CharacterVirtual::EGroundState::InAir) + return inDesiredVelocity; + + Vec3 desired_velocity = inDesiredVelocity; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && IsSlopeTooSteep(c.mSurfaceNormal)) + { + // Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement. + Vec3 normal = c.mContactNormal; + + // Remove normal vertical component + normal -= normal.Dot(mUp) * mUp; + + // Cancel horizontal movement in opposite direction + float dot = normal.Dot(desired_velocity); + if (dot < 0.0f) + desired_velocity -= (dot * normal) / normal.LengthSq(); + } + return desired_velocity; +} + +void CharacterVirtual::Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // If there's no delta time, we don't need to do anything + if (inDeltaTime <= 0.0f) + return; + + // Remember delta time for checking if we're supported by the ground + mLastDeltaTime = inDeltaTime; + + // Slide the shape through the world + MoveShape(mPosition, mLinearVelocity, inDeltaTime, &mActiveContacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator + #ifdef JPH_DEBUG_RENDERER + , sDrawConstraints + #endif // JPH_DEBUG_RENDERER + ); + + // Determine the object that we're standing on + UpdateSupportingContact(false, inAllocator); + + // If we're on the ground + if (!mGroundBodyID.IsInvalid() && mMass > 0.0f) + { + // Add the impulse to the ground due to gravity: P = F dt = M g dt + float normal_dot_gravity = mGroundNormal.Dot(inGravity); + if (normal_dot_gravity < 0.0f) + { + Vec3 world_impulse = -(mMass * normal_dot_gravity / inGravity.Length() * inDeltaTime) * inGravity; + mSystem->GetBodyInterface().AddImpulse(mGroundBodyID, world_impulse, mGroundPosition); + } + } +} + +void CharacterVirtual::RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Determine the contacts + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + StoreActiveContacts(contacts, inAllocator); +} + +void CharacterVirtual::UpdateGroundVelocity() +{ + BodyLockRead lock(mSystem->GetBodyLockInterface(), mGroundBodyID); + if (lock.SucceededAndIsInBroadPhase()) + { + const Body &body = lock.GetBody(); + + // Get adjusted body velocity + Vec3 linear_velocity, angular_velocity; + GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity); + + // Calculate the ground velocity + mGroundVelocity = CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime); + } +} + +void CharacterVirtual::MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Set the new position + SetPosition(inPosition); + + // Determine the contacts + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits + 1); // +1 because we can add one extra below + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + // Ensure that we mark inContact as colliding + bool found_contact = false; + for (Contact &c : contacts) + if (c.mBodyB == inContact.mBodyB + && c.mSubShapeIDB == inContact.mSubShapeIDB) + { + c.mHadCollision = true; + found_contact = true; + } + if (!found_contact) + { + contacts.push_back(inContact); + + Contact © = contacts.back(); + copy.mHadCollision = true; + } + + StoreActiveContacts(contacts, inAllocator); + JPH_ASSERT(mGroundState != EGroundState::InAir); +} + +bool CharacterVirtual::SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + if (mShape == nullptr || mSystem == nullptr) + { + // It hasn't been initialized yet + mShape = inShape; + return true; + } + + if (inShape != mShape && inShape != nullptr) + { + if (inMaxPenetrationDepth < FLT_MAX) + { + // Check collision around the new shape + TempContactList contacts(inAllocator); + contacts.reserve(mMaxNumHits); + GetContactsAtPosition(mPosition, mLinearVelocity.NormalizedOr(Vec3::sZero()), inShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter); + + // Test if this results in penetration, if so cancel the transition + for (const Contact &c : contacts) + if (c.mDistance < -inMaxPenetrationDepth + && !c.mIsSensorB) + return false; + + StoreActiveContacts(contacts, inAllocator); + } + + // Set new shape + mShape = inShape; + } + + return mShape == inShape; +} + +bool CharacterVirtual::CanWalkStairs(Vec3Arg inLinearVelocity) const +{ + // We can only walk stairs if we're supported + if (!IsSupported()) + return false; + + // Check if there's enough horizontal velocity to trigger a stair walk + Vec3 horizontal_velocity = inLinearVelocity - inLinearVelocity.Dot(mUp) * mUp; + if (horizontal_velocity.IsNearZero(1.0e-6f)) + return false; + + // Check contacts for steep slopes + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact + && IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep + return true; + + return false; +} + +bool CharacterVirtual::WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Move up + Vec3 up = inStepUp; + Contact contact; + IgnoredContactList dummy_ignored_contacts(inAllocator); + if (GetFirstContactForSweep(mPosition, up, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + { + if (contact.mFraction < 1.0e-6f) + return false; // No movement, cancel + + // Limit up movement to the first contact point + up *= contact.mFraction; + } + RVec3 up_position = mPosition + up; + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep up + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(mPosition, up_position, Color::sWhite, 0.01f); +#endif // JPH_DEBUG_RENDERER + + // Collect normals of steep slopes that we would like to walk stairs on. + // We need to do this before calling MoveShape because it will update mActiveContacts. + Vec3 character_velocity = inStepForward / inDeltaTime; + Vec3 horizontal_velocity = character_velocity - character_velocity.Dot(mUp) * mUp; + Array> steep_slope_normals(inAllocator); + steep_slope_normals.reserve(mActiveContacts.size()); + for (const Contact &c : mActiveContacts) + if (c.mHadCollision + && c.mSurfaceNormal.Dot(horizontal_velocity - c.mLinearVelocity) < 0.0f // Pushing into the contact + && IsSlopeTooSteep(c.mSurfaceNormal)) // Slope too steep + steep_slope_normals.push_back(c.mSurfaceNormal); + if (steep_slope_normals.empty()) + return false; // No steep slopes, cancel + + // Horizontal movement + RVec3 new_position = up_position; + MoveShape(new_position, character_velocity, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + Vec3 horizontal_movement = Vec3(new_position - up_position); + float horizontal_movement_sq = horizontal_movement.LengthSq(); + if (horizontal_movement_sq < 1.0e-8f) + return false; // No movement, cancel + + // Check if we made any progress towards any of the steep slopes, if not we just slid along the slope + // so we need to cancel the stair walk or else we will move faster than we should as we've done + // normal movement first and then stair walk. + bool made_progress = false; + float max_dot = -0.05f * inStepForward.Length(); + for (const Vec3 &normal : steep_slope_normals) + if (normal.Dot(horizontal_movement) < max_dot) + { + // We moved more than 5% of the forward step against a steep slope, accept this as progress + made_progress = true; + break; + } + if (!made_progress) + return false; + +#ifdef JPH_DEBUG_RENDERER + // Draw horizontal sweep + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(up_position, new_position, Color::sWhite, 0.01f); +#endif // JPH_DEBUG_RENDERER + + // Move down towards the floor. + // Note that we travel the same amount down as we traveled up with the specified extra + Vec3 down = -up + inStepDownExtra; + if (!GetFirstContactForSweep(new_position, down, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; // No floor found, we're in mid air, cancel stair walk + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep down + if (sDrawWalkStairs) + { + RVec3 debug_pos = new_position + contact.mFraction * down; + DebugRenderer::sInstance->DrawArrow(new_position, debug_pos, Color::sWhite, 0.01f); + DebugRenderer::sInstance->DrawArrow(contact.mPosition, contact.mPosition + contact.mSurfaceNormal, Color::sWhite, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sWhite, false, true); + } +#endif // JPH_DEBUG_RENDERER + + // Test for floor that will support the character + if (IsSlopeTooSteep(contact.mSurfaceNormal)) + { + // If no test position was provided, we cancel the stair walk + if (inStepForwardTest.IsNearZero()) + return false; + + // Delta time may be very small, so it may be that we hit the edge of a step and the normal is too horizontal. + // In order to judge if the floor is flat further along the sweep, we test again for a floor at inStepForwardTest + // and check if the normal is valid there. + RVec3 test_position = up_position; + MoveShape(test_position, inStepForwardTest / inDeltaTime, inDeltaTime, nullptr, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + float test_horizontal_movement_sq = Vec3(test_position - up_position).LengthSq(); + if (test_horizontal_movement_sq <= horizontal_movement_sq + 1.0e-8f) + return false; // We didn't move any further than in the previous test + + #ifdef JPH_DEBUG_RENDERER + // Draw 2nd sweep horizontal + if (sDrawWalkStairs) + DebugRenderer::sInstance->DrawArrow(up_position, test_position, Color::sCyan, 0.01f); + #endif // JPH_DEBUG_RENDERER + + // Then sweep down + Contact test_contact; + if (!GetFirstContactForSweep(test_position, down, test_contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; + + #ifdef JPH_DEBUG_RENDERER + // Draw 2nd sweep down + if (sDrawWalkStairs) + { + RVec3 debug_pos = test_position + test_contact.mFraction * down; + DebugRenderer::sInstance->DrawArrow(test_position, debug_pos, Color::sCyan, 0.01f); + DebugRenderer::sInstance->DrawArrow(test_contact.mPosition, test_contact.mPosition + test_contact.mSurfaceNormal, Color::sCyan, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(debug_pos, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sCyan, false, true); + } + #endif // JPH_DEBUG_RENDERER + + if (IsSlopeTooSteep(test_contact.mSurfaceNormal)) + return false; + } + + // Calculate new down position + down *= contact.mFraction; + new_position += down; + + // Move the character to the new location + MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + + // Override ground state to 'on ground', it is possible that the contact normal is too steep, but in this case the inStepForwardTest has found a contact normal that is not too steep + mGroundState = EGroundState::OnGround; + + return true; +} + +bool CharacterVirtual::StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Try to find the floor + Contact contact; + IgnoredContactList dummy_ignored_contacts(inAllocator); + if (!GetFirstContactForSweep(mPosition, inStepDown, contact, dummy_ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter)) + return false; // If no floor found, don't update our position + + // Calculate new position + RVec3 new_position = mPosition + contact.mFraction * inStepDown; + +#ifdef JPH_DEBUG_RENDERER + // Draw sweep down + if (sDrawStickToFloor) + { + DebugRenderer::sInstance->DrawArrow(mPosition, new_position, Color::sOrange, 0.01f); + mShape->Draw(DebugRenderer::sInstance, GetCenterOfMassTransform(new_position, mRotation, mShape), Vec3::sReplicate(1.0f), Color::sOrange, false, true); + } +#endif // JPH_DEBUG_RENDERER + + // Move the character to the new location + MoveToContact(new_position, contact, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + return true; +} + +void CharacterVirtual::ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator) +{ + // Update the velocity + Vec3 desired_velocity = mLinearVelocity; + mLinearVelocity = CancelVelocityTowardsSteepSlopes(desired_velocity); + + // Remember old position + RVec3 old_position = mPosition; + + // Track if on ground before the update + bool ground_to_air = IsSupported(); + + // Update the character position (instant, do not have to wait for physics update) + Update(inDeltaTime, inGravity, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + + // ... and that we got into air after + if (IsSupported()) + ground_to_air = false; + + // If stick to floor enabled and we're going from supported to not supported + if (ground_to_air && !inSettings.mStickToFloorStepDown.IsNearZero()) + { + // If we're not moving up, stick to the floor + float velocity = Vec3(mPosition - old_position).Dot(mUp) / inDeltaTime; + if (velocity <= 1.0e-6f) + StickToFloor(inSettings.mStickToFloorStepDown, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + } + + // If walk stairs enabled + if (!inSettings.mWalkStairsStepUp.IsNearZero()) + { + // Calculate how much we wanted to move horizontally + Vec3 desired_horizontal_step = desired_velocity * inDeltaTime; + desired_horizontal_step -= desired_horizontal_step.Dot(mUp) * mUp; + float desired_horizontal_step_len = desired_horizontal_step.Length(); + if (desired_horizontal_step_len > 0.0f) + { + // Calculate how much we moved horizontally + Vec3 achieved_horizontal_step = Vec3(mPosition - old_position); + achieved_horizontal_step -= achieved_horizontal_step.Dot(mUp) * mUp; + + // Only count movement in the direction of the desired movement + // (otherwise we find it ok if we're sliding downhill while we're trying to climb uphill) + Vec3 step_forward_normalized = desired_horizontal_step / desired_horizontal_step_len; + achieved_horizontal_step = max(0.0f, achieved_horizontal_step.Dot(step_forward_normalized)) * step_forward_normalized; + float achieved_horizontal_step_len = achieved_horizontal_step.Length(); + + // If we didn't move as far as we wanted and we're against a slope that's too steep + if (achieved_horizontal_step_len + 1.0e-4f < desired_horizontal_step_len + && CanWalkStairs(desired_velocity)) + { + // Calculate how much we should step forward + // Note that we clamp the step forward to a minimum distance. This is done because at very high frame rates the delta time + // may be very small, causing a very small step forward. If the step becomes small enough, we may not move far enough + // horizontally to actually end up at the top of the step. + Vec3 step_forward = step_forward_normalized * max(inSettings.mWalkStairsMinStepForward, desired_horizontal_step_len - achieved_horizontal_step_len); + + // Calculate how far to scan ahead for a floor. This is only used in case the floor normal at step_forward is too steep. + // In that case an additional check will be performed at this distance to check if that normal is not too steep. + // Start with the ground normal in the horizontal plane and normalizing it + Vec3 step_forward_test = -mGroundNormal; + step_forward_test -= step_forward_test.Dot(mUp) * mUp; + step_forward_test = step_forward_test.NormalizedOr(step_forward_normalized); + + // If this normalized vector and the character forward vector is bigger than a preset angle, we use the character forward vector instead of the ground normal + // to do our forward test + if (step_forward_test.Dot(step_forward_normalized) < inSettings.mWalkStairsCosAngleForwardContact) + step_forward_test = step_forward_normalized; + + // Calculate the correct magnitude for the test vector + step_forward_test *= inSettings.mWalkStairsStepForwardTest; + + WalkStairs(inDeltaTime, inSettings.mWalkStairsStepUp, step_forward, step_forward_test, inSettings.mWalkStairsStepDownExtra, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter, inAllocator); + } + } + } +} + +void CharacterVirtual::Contact::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mLinearVelocity); + inStream.Write(mContactNormal); + inStream.Write(mSurfaceNormal); + inStream.Write(mDistance); + inStream.Write(mFraction); + inStream.Write(mBodyB); + inStream.Write(mSubShapeIDB); + inStream.Write(mMotionTypeB); + inStream.Write(mHadCollision); + inStream.Write(mWasDiscarded); + inStream.Write(mCanPushCharacter); + // Cannot store user data (may be a pointer) and material +} + +void CharacterVirtual::Contact::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mLinearVelocity); + inStream.Read(mContactNormal); + inStream.Read(mSurfaceNormal); + inStream.Read(mDistance); + inStream.Read(mFraction); + inStream.Read(mBodyB); + inStream.Read(mSubShapeIDB); + inStream.Read(mMotionTypeB); + inStream.Read(mHadCollision); + inStream.Read(mWasDiscarded); + inStream.Read(mCanPushCharacter); + mUserData = 0; // Cannot restore user data + mMaterial = PhysicsMaterial::sDefault; // Cannot restore material +} + +void CharacterVirtual::SaveState(StateRecorder &inStream) const +{ + CharacterBase::SaveState(inStream); + + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mLinearVelocity); + inStream.Write(mLastDeltaTime); + inStream.Write(mMaxHitsExceeded); + + // Store contacts that had collision, we're using it at the beginning of the step in CancelVelocityTowardsSteepSlopes + uint32 num_contacts = 0; + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + ++num_contacts; + inStream.Write(num_contacts); + for (const Contact &c : mActiveContacts) + if (c.mHadCollision) + c.SaveState(inStream); +} + +void CharacterVirtual::RestoreState(StateRecorder &inStream) +{ + CharacterBase::RestoreState(inStream); + + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mLinearVelocity); + inStream.Read(mLastDeltaTime); + inStream.Read(mMaxHitsExceeded); + + // When validating remove contacts that don't have collision since we didn't save them + if (inStream.IsValidating()) + for (int i = (int)mActiveContacts.size() - 1; i >= 0; --i) + if (!mActiveContacts[i].mHadCollision) + mActiveContacts.erase(mActiveContacts.begin() + i); + + uint32 num_contacts = (uint32)mActiveContacts.size(); + inStream.Read(num_contacts); + mActiveContacts.resize(num_contacts); + for (Contact &c : mActiveContacts) + c.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Character/CharacterVirtual.h b/WickedEngine/Jolt/Physics/Character/CharacterVirtual.h new file mode 100644 index 000000000..8df82f1a9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Character/CharacterVirtual.h @@ -0,0 +1,507 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CharacterVirtual; + +/// Contains the configuration of a character +class JPH_EXPORT CharacterVirtualSettings : public CharacterBaseSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Character mass (kg). Used to push down objects with gravity when the character is standing on top. + float mMass = 70.0f; + + /// Maximum force with which the character can push other bodies (N). + float mMaxStrength = 100.0f; + + /// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. + Vec3 mShapeOffset = Vec3::sZero(); + + ///@name Movement settings + EBackFaceMode mBackFaceMode = EBackFaceMode::CollideWithBackFaces; ///< When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides. + float mPredictiveContactDistance = 0.1f; ///< How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions. + uint mMaxCollisionIterations = 5; ///< Max amount of collision loops + uint mMaxConstraintIterations = 15; ///< How often to try stepping in the constraint solving + float mMinTimeRemaining = 1.0e-4f; ///< Early out condition: If this much time is left to simulate we are done + float mCollisionTolerance = 1.0e-3f; ///< How far we're willing to penetrate geometry + float mCharacterPadding = 0.02f; ///< How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck + uint mMaxNumHits = 256; ///< Max num hits to collect in order to avoid excess of contact points collection + float mHitReductionCosMaxAngle = 0.999f; ///< Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float mPenetrationRecoverySpeed = 1.0f; ///< This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update +}; + +/// This class contains settings that allow you to override the behavior of a character's collision response +class CharacterContactSettings +{ +public: + bool mCanPushCharacter = true; ///< True when the object can push the virtual character + bool mCanReceiveImpulses = true; ///< True when the virtual character can apply impulses (push) the body +}; + +/// This class receives callbacks when a virtual character hits something. +class JPH_EXPORT CharacterContactListener +{ +public: + /// Destructor + virtual ~CharacterContactListener() = default; + + /// Callback to adjust the velocity of a body as seen by the character. Can be adjusted to e.g. implement a conveyor belt or an inertial dampener system of a sci-fi space ship. + /// Note that inBody2 is locked during the callback so you can read its properties freely. + virtual void OnAdjustBodyVelocity(const CharacterVirtual *inCharacter, const Body &inBody2, Vec3 &ioLinearVelocity, Vec3 &ioAngularVelocity) { /* Do nothing, the linear and angular velocity are already filled in */ } + + /// Checks if a character can collide with specified body. Return true if the contact is valid. + virtual bool OnContactValidate(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2) { return true; } + + /// Called whenever the character collides with a body. + /// @param inCharacter Character that is being solved + /// @param inBodyID2 Body ID of body that is being hit + /// @param inSubShapeID2 Sub shape ID of shape that is being hit + /// @param inContactPosition World space contact position + /// @param inContactNormal World space contact normal + /// @param ioSettings Settings returned by the contact callback to indicate how the character should behave + virtual void OnContactAdded(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, CharacterContactSettings &ioSettings) { /* Default do nothing */ } + + /// Called whenever a contact is being used by the solver. Allows the listener to override the resulting character velocity (e.g. by preventing sliding along certain surfaces). + /// @param inCharacter Character that is being solved + /// @param inBodyID2 Body ID of body that is being hit + /// @param inSubShapeID2 Sub shape ID of shape that is being hit + /// @param inContactPosition World space contact position + /// @param inContactNormal World space contact normal + /// @param inContactVelocity World space velocity of contact point (e.g. for a moving platform) + /// @param inContactMaterial Material of contact point + /// @param inCharacterVelocity World space velocity of the character prior to hitting this contact + /// @param ioNewCharacterVelocity Contains the calculated world space velocity of the character after hitting this contact, this velocity slides along the surface of the contact. Can be modified by the listener to provide an alternative velocity. + virtual void OnContactSolve(const CharacterVirtual *inCharacter, const BodyID &inBodyID2, const SubShapeID &inSubShapeID2, RVec3Arg inContactPosition, Vec3Arg inContactNormal, Vec3Arg inContactVelocity, const PhysicsMaterial *inContactMaterial, Vec3Arg inCharacterVelocity, Vec3 &ioNewCharacterVelocity) { /* Default do nothing */ } +}; + +/// Runtime character object. +/// This object usually represents the player. Contrary to the Character class it doesn't use a rigid body but moves doing collision checks only (hence the name virtual). +/// The advantage of this is that you can determine when the character moves in the frame (usually this has to happen at a very particular point in the frame) +/// but the downside is that other objects don't see this virtual character. In order to make this work it is recommended to pair a CharacterVirtual with a Character that +/// moves along. This Character should be keyframed (or at least have no gravity) and move along with the CharacterVirtual so that other rigid bodies can collide with it. +class JPH_EXPORT CharacterVirtual : public CharacterBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inSettings The settings for the character + /// @param inPosition Initial position for the character + /// @param inRotation Initial rotation for the character (usually only around the up-axis) + /// @param inUserData Application specific value + /// @param inSystem Physics system that this character will be added to later + CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, uint64 inUserData, PhysicsSystem *inSystem); + + /// Constructor without user data + CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) : CharacterVirtual(inSettings, inPosition, inRotation, 0, inSystem) { } + + /// Set the contact listener + void SetListener(CharacterContactListener *inListener) { mListener = inListener; } + + /// Get the current contact listener + CharacterContactListener * GetListener() const { return mListener; } + + /// Get the linear velocity of the character (m / s) + Vec3 GetLinearVelocity() const { return mLinearVelocity; } + + /// Set the linear velocity of the character (m / s) + void SetLinearVelocity(Vec3Arg inLinearVelocity) { mLinearVelocity = inLinearVelocity; } + + /// Get the position of the character + RVec3 GetPosition() const { return mPosition; } + + /// Set the position of the character + void SetPosition(RVec3Arg inPosition) { mPosition = inPosition; } + + /// Get the rotation of the character + Quat GetRotation() const { return mRotation; } + + /// Set the rotation of the character + void SetRotation(QuatArg inRotation) { mRotation = inRotation; } + + /// Calculate the world transform of the character + RMat44 GetWorldTransform() const { return RMat44::sRotationTranslation(mRotation, mPosition); } + + /// Calculates the transform for this character's center of mass + RMat44 GetCenterOfMassTransform() const { return GetCenterOfMassTransform(mPosition, mRotation, mShape); } + + /// Character mass (kg) + float GetMass() const { return mMass; } + void SetMass(float inMass) { mMass = inMass; } + + /// Maximum force with which the character can push other bodies (N) + float GetMaxStrength() const { return mMaxStrength; } + void SetMaxStrength(float inMaxStrength) { mMaxStrength = inMaxStrength; } + + /// This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + float GetPenetrationRecoverySpeed() const { return mPenetrationRecoverySpeed; } + void SetPenetrationRecoverySpeed(float inSpeed) { mPenetrationRecoverySpeed = inSpeed; } + + /// Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + bool GetEnhancedInternalEdgeRemoval() const { return mEnhancedInternalEdgeRemoval; } + void SetEnhancedInternalEdgeRemoval(bool inApply) { mEnhancedInternalEdgeRemoval = inApply; } + + /// Character padding + float GetCharacterPadding() const { return mCharacterPadding; } + + /// Max num hits to collect in order to avoid excess of contact points collection + uint GetMaxNumHits() const { return mMaxNumHits; } + void SetMaxNumHits(uint inMaxHits) { mMaxNumHits = inMaxHits; } + + /// Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float GetHitReductionCosMaxAngle() const { return mHitReductionCosMaxAngle; } + void SetHitReductionCosMaxAngle(float inCosMaxAngle) { mHitReductionCosMaxAngle = inCosMaxAngle; } + + /// Returns if we exceeded the maximum number of hits during the last collision check and had to discard hits based on distance. + /// This can be used to find areas that have too complex geometry for the character to navigate properly. + /// To solve you can either increase the max number of hits or simplify the geometry. Note that the character simulation will + /// try to do its best to select the most relevant contacts to avoid the character from getting stuck. + bool GetMaxHitsExceeded() const { return mMaxHitsExceeded; } + + /// An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. Note that setting it on the fly can cause the shape to teleport into collision. + Vec3 GetShapeOffset() const { return mShapeOffset; } + void SetShapeOffset(Vec3Arg inShapeOffset) { mShapeOffset = inShapeOffset; } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// This function can be called prior to calling Update() to convert a desired velocity into a velocity that won't make the character move further onto steep slopes. + /// This velocity can then be set on the character using SetLinearVelocity() + /// @param inDesiredVelocity Velocity to clamp against steep walls + /// @return A new velocity vector that won't make the character move up steep slopes + Vec3 CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const; + + /// This is the main update function. It moves the character according to its current velocity (the character is similar to a kinematic body in the sense + /// that you set the velocity and the character will follow unless collision is blocking the way). Note it's your own responsibility to apply gravity to the character velocity! + /// Different surface materials (like ice) can be emulated by getting the ground material and adjusting the velocity and/or the max slope angle accordingly every frame. + /// @param inDeltaTime Time step to simulate. + /// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + void Update(float inDeltaTime, Vec3Arg inGravity, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function will return true if the character has moved into a slope that is too steep (e.g. a vertical wall). + /// You would call WalkStairs to attempt to step up stairs. + /// @param inLinearVelocity The linear velocity that the player desired. This is used to determine if we're pushing into a step. + bool CanWalkStairs(Vec3Arg inLinearVelocity) const; + + /// When stair walking is needed, you can call the WalkStairs function to cast up, forward and down again to try to find a valid position + /// @param inDeltaTime Time step to simulate. + /// @param inStepUp The direction and distance to step up (this corresponds to the max step height) + /// @param inStepForward The direction and distance to step forward after the step up + /// @param inStepForwardTest When running at a high frequency, inStepForward can be very small and it's likely that you hit the side of the stairs on the way down. This could produce a normal that violates the max slope angle. If this happens, we test again using this distance from the up position to see if we find a valid slope. + /// @param inStepDownExtra An additional translation that is added when stepping down at the end. Allows you to step further down than up. Set to zero if you don't want this. Should be in the opposite direction of up. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return true if the stair walk was successful + bool WalkStairs(float inDeltaTime, Vec3Arg inStepUp, Vec3Arg inStepForward, Vec3Arg inStepForwardTest, Vec3Arg inStepDownExtra, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function can be used to artificially keep the character to the floor. Normally when a character is on a small step and starts moving horizontally, the character will + /// lose contact with the floor because the initial vertical velocity is zero while the horizontal velocity is quite high. To prevent the character from losing contact with the floor, + /// we do an additional collision check downwards and if we find the floor within a certain distance, we project the character onto the floor. + /// @param inStepDown Max amount to project the character downwards (if no floor is found within this distance, the function will return false) + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return True if the character was successfully projected onto the floor. + bool StickToFloor(Vec3Arg inStepDown, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Settings struct with settings for ExtendedUpdate + struct ExtendedUpdateSettings + { + Vec3 mStickToFloorStepDown { 0, -0.5f, 0 }; ///< See StickToFloor inStepDown parameter. Can be zero to turn off. + Vec3 mWalkStairsStepUp { 0, 0.4f, 0 }; ///< See WalkStairs inStepUp parameter. Can be zero to turn off. + float mWalkStairsMinStepForward { 0.02f }; ///< See WalkStairs inStepForward parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity. + float mWalkStairsStepForwardTest { 0.15f }; ///< See WalkStairs inStepForwardTest parameter. Note that the parameter only indicates a magnitude, direction is taken from current velocity. + float mWalkStairsCosAngleForwardContact { Cos(DegreesToRadians(75.0f)) }; ///< Cos(angle) where angle is the maximum angle between the ground normal in the horizontal plane and the character forward vector where we're willing to adjust the step forward test towards the contact normal. + Vec3 mWalkStairsStepDownExtra { Vec3::sZero() }; ///< See WalkStairs inStepDownExtra + }; + + /// This function combines Update, StickToFloor and WalkStairs. This function serves as an example of how these functions could be combined. + /// Before calling, call SetLinearVelocity to update the horizontal/vertical speed of the character, typically this is: + /// - When on OnGround and not moving away from ground: velocity = GetGroundVelocity() + horizontal speed as input by player + optional vertical jump velocity + delta time * gravity + /// - Else: velocity = current vertical velocity + horizontal speed as input by player + delta time * gravity + /// @param inDeltaTime Time step to simulate. + /// @param inGravity Gravity vector (m/s^2). This gravity vector is only used when the character is standing on top of another object to apply downward force. + /// @param inSettings A structure containing settings for the algorithm. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + void ExtendedUpdate(float inDeltaTime, Vec3Arg inGravity, const ExtendedUpdateSettings &inSettings, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// This function can be used after a character has teleported to determine the new contacts with the world. + void RefreshContacts(const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// Use the ground body ID to get an updated estimate of the ground velocity. This function can be used if the ground body has moved / changed velocity and you want a new estimate of the ground velocity. + /// It will not perform collision detection, so is less accurate than RefreshContacts but a lot faster. + void UpdateGroundVelocity(); + + /// Switch the shape of the character (e.g. for stance). + /// @param inShape The shape to switch to. + /// @param inMaxPenetrationDepth When inMaxPenetrationDepth is not FLT_MAX, it checks if the new shape collides before switching shape. This is the max penetration we're willing to accept after the switch. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + /// @param inAllocator An allocator for temporary allocations. All memory will be freed by the time this function returns. + /// @return Returns true if the switch succeeded. + bool SetShape(const Shape *inShape, float inMaxPenetrationDepth, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + /// @brief Get all contacts for the character at a particular location + /// @param inPosition Position to test, note that this position will be corrected for the character padding. + /// @param inRotation Rotation at which to test the shape. + /// @param inMovementDirection A hint in which direction the character is moving, will be used to calculate a proper normal. + /// @param inMaxSeparationDistance How much distance around the character you want to report contacts in (can be 0 to match the character exactly). + /// @param inShape Shape to test collision with. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. GetPosition() since floats are most accurate near the origin + /// @param ioCollector Collision collector that receives the collision results. + /// @param inBroadPhaseLayerFilter Filter that is used to check if the character collides with something in the broadphase. + /// @param inObjectLayerFilter Filter that is used to check if a character collides with a layer. + /// @param inBodyFilter Filter that is used to check if a character collides with a body. + /// @param inShapeFilter Filter that is used to check if a character collides with a subshape. + void CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Saving / restoring state for replay + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + +#ifdef JPH_DEBUG_RENDERER + static inline bool sDrawConstraints = false; ///< Draw the current state of the constraints for iteration 0 when creating them + static inline bool sDrawWalkStairs = false; ///< Draw the state of the walk stairs algorithm + static inline bool sDrawStickToFloor = false; ///< Draw the state of the stick to floor algorithm +#endif + + // Encapsulates a collision contact + struct Contact + { + // Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + RVec3 mPosition; ///< Position where the character makes contact + Vec3 mLinearVelocity; ///< Velocity of the contact point + Vec3 mContactNormal; ///< Contact normal, pointing towards the character + Vec3 mSurfaceNormal; ///< Surface normal of the contact + float mDistance; ///< Distance to the contact <= 0 means that it is an actual contact, > 0 means predictive + float mFraction; ///< Fraction along the path where this contact takes place + BodyID mBodyB; ///< ID of body we're colliding with + SubShapeID mSubShapeIDB; ///< Sub shape ID of body we're colliding with + EMotionType mMotionTypeB; ///< Motion type of B, used to determine the priority of the contact + bool mIsSensorB; ///< If B is a sensor + uint64 mUserData; ///< User data of B + const PhysicsMaterial * mMaterial; ///< Material of B + bool mHadCollision = false; ///< If the character actually collided with the contact (can be false if a predictive contact never becomes a real one) + bool mWasDiscarded = false; ///< If the contact validate callback chose to discard this contact + bool mCanPushCharacter = true; ///< When true, the velocity of the contact point can push the character + }; + + using TempContactList = Array>; + using ContactList = Array; + + /// Access to the internal list of contacts that the character has found. + const ContactList & GetActiveContacts() const { return mActiveContacts; } + +private: + // Sorting predicate for making contact order deterministic + struct ContactOrderingPredicate + { + inline bool operator () (const Contact &inLHS, const Contact &inRHS) const + { + if (inLHS.mBodyB != inRHS.mBodyB) + return inLHS.mBodyB < inRHS.mBodyB; + + return inLHS.mSubShapeIDB.GetValue() < inRHS.mSubShapeIDB.GetValue(); + } + }; + + // A contact that needs to be ignored + struct IgnoredContact + { + IgnoredContact() = default; + IgnoredContact(const BodyID &inBodyID, const SubShapeID &inSubShapeID) : mBodyID(inBodyID), mSubShapeID(inSubShapeID) { } + + BodyID mBodyID; ///< ID of body we're colliding with + SubShapeID mSubShapeID; ///< Sub shape of body we're colliding with + }; + + using IgnoredContactList = Array>; + + // A constraint that limits the movement of the character + struct Constraint + { + Contact * mContact; ///< Contact that this constraint was generated from + float mTOI; ///< Calculated time of impact (can be negative if penetrating) + float mProjectedVelocity; ///< Velocity of the contact projected on the contact normal (negative if separating) + Vec3 mLinearVelocity; ///< Velocity of the contact (can contain a corrective velocity to resolve penetration) + Plane mPlane; ///< Plane around the origin that describes how far we can displace (from the origin) + bool mIsSteepSlope = false; ///< If this constraint belongs to a steep slope + }; + + using ConstraintList = Array>; + + // Collision collector that collects hits for CollideShape + class ContactCollector : public CollideShapeCollector + { + public: + ContactCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, uint inMaxHits, float inHitReductionCosMaxAngle, Vec3Arg inUp, RVec3Arg inBaseOffset, TempContactList &outContacts) : mBaseOffset(inBaseOffset), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mContacts(outContacts), mMaxHits(inMaxHits), mHitReductionCosMaxAngle(inHitReductionCosMaxAngle) { } + + virtual void AddHit(const CollideShapeResult &inResult) override; + + RVec3 mBaseOffset; + Vec3 mUp; + PhysicsSystem * mSystem; + const CharacterVirtual * mCharacter; + TempContactList & mContacts; + uint mMaxHits; + float mHitReductionCosMaxAngle; + bool mMaxHitsExceeded = false; + }; + + // A collision collector that collects hits for CastShape + class ContactCastCollector : public CastShapeCollector + { + public: + ContactCastCollector(PhysicsSystem *inSystem, const CharacterVirtual *inCharacter, Vec3Arg inDisplacement, Vec3Arg inUp, const IgnoredContactList &inIgnoredContacts, RVec3Arg inBaseOffset, Contact &outContact) : mBaseOffset(inBaseOffset), mDisplacement(inDisplacement), mUp(inUp), mSystem(inSystem), mCharacter(inCharacter), mIgnoredContacts(inIgnoredContacts), mContact(outContact) { } + + virtual void AddHit(const ShapeCastResult &inResult) override; + + RVec3 mBaseOffset; + Vec3 mDisplacement; + Vec3 mUp; + PhysicsSystem * mSystem; + const CharacterVirtual * mCharacter; + const IgnoredContactList & mIgnoredContacts; + Contact & mContact; + }; + + // Helper function to convert a Jolt collision result into a contact + template + inline static void sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult); + + // Move the shape from ioPosition and try to displace it by inVelocity * inDeltaTime, this will try to slide the shape along the world geometry + void MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator + #ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints = false + #endif // JPH_DEBUG_RENDERER + ) const; + + // Ask the callback if inContact is a valid contact point + bool ValidateContact(const Contact &inContact) const; + + // Tests the shape for collision around inPosition + void GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Remove penetrating contacts with the same body that have conflicting normals, leaving these will make the character mover get stuck + void RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const; + + // Convert contacts into constraints. The character is assumed to start at the origin and the constraints are planes around the origin that confine the movement of the character. + void DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const; + + // Use the constraints to solve the displacement of the character. This will slide the character on the planes around the origin for as far as possible. + void SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator + #ifdef JPH_DEBUG_RENDERER + , bool inDrawConstraints = false + #endif // JPH_DEBUG_RENDERER + ) const; + + // Get the velocity of a body adjusted by the contact listener + void GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const; + + // Calculate the ground velocity of the character assuming it's standing on an object with specified linear and angular velocity and with specified center of mass. + // Note that we don't just take the point velocity because a point on an object with angular velocity traces an arc, + // so if you just take point velocity * delta time you get an error that accumulates over time + Vec3 CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const; + + // Handle contact with physics object that we're colliding against + bool HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const; + + // Does a swept test of the shape from inPosition with displacement inDisplacement, returns true if there was a collision + bool GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const; + + // Store contacts so that we have proper ground information + void StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator); + + // This function will determine which contacts are touching the character and will calculate the one that is supporting us + void UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator); + + /// This function can be called after moving the character to a new colliding position + void MoveToContact(RVec3Arg inPosition, const Contact &inContact, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator); + + // This function returns the actual center of mass of the shape, not corrected for the character padding + inline RMat44 GetCenterOfMassTransform(RVec3Arg inPosition, QuatArg inRotation, const Shape *inShape) const + { + return RMat44::sRotationTranslation(inRotation, inPosition).PreTranslated(mShapeOffset + inShape->GetCenterOfMass()).PostTranslated(mCharacterPadding * mUp); + } + + // Our main listener for contacts + CharacterContactListener * mListener = nullptr; + + // Movement settings + EBackFaceMode mBackFaceMode; // When colliding with back faces, the character will not be able to move through back facing triangles. Use this if you have triangles that need to collide on both sides. + float mPredictiveContactDistance; // How far to scan outside of the shape for predictive contacts. A value of 0 will most likely cause the character to get stuck as it cannot properly calculate a sliding direction anymore. A value that's too high will cause ghost collisions. + uint mMaxCollisionIterations; // Max amount of collision loops + uint mMaxConstraintIterations; // How often to try stepping in the constraint solving + float mMinTimeRemaining; // Early out condition: If this much time is left to simulate we are done + float mCollisionTolerance; // How far we're willing to penetrate geometry + float mCharacterPadding; // How far we try to stay away from the geometry, this ensures that the sweep will hit as little as possible lowering the collision cost and reducing the risk of getting stuck + uint mMaxNumHits; // Max num hits to collect in order to avoid excess of contact points collection + float mHitReductionCosMaxAngle; // Cos(angle) where angle is the maximum angle between two hits contact normals that are allowed to be merged during hit reduction. Default is around 2.5 degrees. Set to -1 to turn off. + float mPenetrationRecoverySpeed; // This value governs how fast a penetration will be resolved, 0 = nothing is resolved, 1 = everything in one update + bool mEnhancedInternalEdgeRemoval; // Set to indicate that extra effort should be made to try to remove ghost contacts (collisions with internal edges of a mesh). This is more expensive but makes bodies move smoother over a mesh with convex edges. + + // Character mass (kg) + float mMass; + + // Maximum force with which the character can push other bodies (N) + float mMaxStrength; + + // An extra offset applied to the shape in local space. This allows applying an extra offset to the shape in local space. + Vec3 mShapeOffset = Vec3::sZero(); + + // Current position (of the base, not the center of mass) + RVec3 mPosition = RVec3::sZero(); + + // Current rotation (of the base, not of the center of mass) + Quat mRotation = Quat::sIdentity(); + + // Current linear velocity + Vec3 mLinearVelocity = Vec3::sZero(); + + // List of contacts that were active in the last frame + ContactList mActiveContacts; + + // Remembers the delta time of the last update + float mLastDeltaTime = 1.0f / 60.0f; + + // Remember if we exceeded the maximum number of hits and had to remove similar contacts + mutable bool mMaxHitsExceeded = false; + + // User data, can be used for anything by the application + uint64 mUserData = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/AABoxCast.h b/WickedEngine/Jolt/Physics/Collision/AABoxCast.h new file mode 100644 index 000000000..a1cedf1ad --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/AABoxCast.h @@ -0,0 +1,20 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds AABox moving linearly through 3d space +struct AABoxCast +{ + JPH_OVERRIDE_NEW_DELETE + + AABox mBox; ///< Axis aligned box at starting location + Vec3 mDirection; ///< Direction and length of the cast (anything beyond this length will not be reported as a hit) +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ActiveEdgeMode.h b/WickedEngine/Jolt/Physics/Collision/ActiveEdgeMode.h new file mode 100644 index 000000000..30f96aeb1 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ActiveEdgeMode.h @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// How to treat active/inactive edges. +/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large, see: ActiveEdges +enum class EActiveEdgeMode : uint8 +{ + CollideOnlyWithActive, ///< Do not collide with inactive edges. For physics simulation, this gives less ghost collisions. + CollideWithAll, ///< Collide with all edges. Use this when you're interested in all collisions. +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ActiveEdges.h b/WickedEngine/Jolt/Physics/Collision/ActiveEdges.h new file mode 100644 index 000000000..7e51d2a63 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ActiveEdges.h @@ -0,0 +1,114 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// An active edge is an edge that either has no neighbouring edge or if the angle between the two connecting faces is too large. +namespace ActiveEdges +{ + /// Helper function to check if an edge is active or not + /// @param inNormal1 Triangle normal of triangle on the left side of the edge (when looking along the edge from the top) + /// @param inNormal2 Triangle normal of triangle on the right side of the edge + /// @param inEdgeDirection Vector that points along the edge + /// @param inCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive) + inline static bool IsEdgeActive(Vec3Arg inNormal1, Vec3Arg inNormal2, Vec3Arg inEdgeDirection, float inCosThresholdAngle) + { + // If normals are opposite the edges are active (the triangles are back to back) + float cos_angle_normals = inNormal1.Dot(inNormal2); + if (cos_angle_normals < -0.999848f) // cos(179 degrees) + return true; + + // Check if concave edge, if so we are not active + if (inNormal1.Cross(inNormal2).Dot(inEdgeDirection) < 0.0f) + return false; + + // Convex edge, active when angle bigger than threshold + return cos_angle_normals < inCosThresholdAngle; + } + + /// Replace normal by triangle normal if a hit is hitting an inactive edge + /// @param inV0 , inV1 , inV2 form the triangle + /// @param inTriangleNormal is the normal of the provided triangle (does not need to be normalized) + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// @param inPoint Collision point on the triangle + /// @param inNormal Collision normal on the triangle (does not need to be normalized) + /// @param inMovementDirection Can be zero. This gives an indication of in which direction the motion is to determine if when we hit an inactive edge/triangle we should return the triangle normal. + /// @return Returns inNormal if an active edge was hit, otherwise returns inTriangleNormal + inline static Vec3 FixNormal(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inTriangleNormal, uint8 inActiveEdges, Vec3Arg inPoint, Vec3Arg inNormal, Vec3Arg inMovementDirection) + { + // Check: All of the edges are active, we have the correct normal already. No need to call this function! + JPH_ASSERT(inActiveEdges != 0b111); + + // If inNormal would affect movement less than inTriangleNormal use inNormal + // This is done since it is really hard to make a distinction between sliding over a horizontal triangulated grid and hitting an edge (in this case you want to use the triangle normal) + // and sliding over a triangulated grid and grazing a vertical triangle with an inactive edge (in this case using the triangle normal will cause the object to bounce back so we want to use the calculated normal). + // To solve this we take a movement hint to give an indication of what direction our object is moving. If the edge normal results in less motion difference than the triangle normal we use the edge normal. + float normal_length = inNormal.Length(); + float triangle_normal_length = inTriangleNormal.Length(); + if (inMovementDirection.Dot(inNormal) * triangle_normal_length < inMovementDirection.Dot(inTriangleNormal) * normal_length) + return inNormal; + + // Check: None of the edges are active, we need to use the triangle normal + if (inActiveEdges == 0) + return inTriangleNormal; + + // Some edges are active. + // If normal is parallel to the triangle normal we don't need to check the active edges. + if (inTriangleNormal.Dot(inNormal) > 0.999848f * normal_length * triangle_normal_length) // cos(1 degree) + return inNormal; + + const float cEpsilon = 1.0e-4f; + const float cOneMinusEpsilon = 1.0f - cEpsilon; + + uint colliding_edge; + + // Test where the contact point is in the triangle + float u, v, w; + ClosestPoint::GetBaryCentricCoordinates(inV0 - inPoint, inV1 - inPoint, inV2 - inPoint, u, v, w); + if (u > cOneMinusEpsilon) + { + // Colliding with v0, edge 0 or 2 needs to be active + colliding_edge = 0b101; + } + else if (v > cOneMinusEpsilon) + { + // Colliding with v1, edge 0 or 1 needs to be active + colliding_edge = 0b011; + } + else if (w > cOneMinusEpsilon) + { + // Colliding with v2, edge 1 or 2 needs to be active + colliding_edge = 0b110; + } + else if (u < cEpsilon) + { + // Colliding with edge v1, v2, edge 1 needs to be active + colliding_edge = 0b010; + } + else if (v < cEpsilon) + { + // Colliding with edge v0, v2, edge 2 needs to be active + colliding_edge = 0b100; + } + else if (w < cEpsilon) + { + // Colliding with edge v0, v1, edge 0 needs to be active + colliding_edge = 0b001; + } + else + { + // Interior hit + return inTriangleNormal; + } + + // If this edge is active, use the provided normal instead of the triangle normal + return (inActiveEdges & colliding_edge) != 0? inNormal : inTriangleNormal; + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BackFaceMode.h b/WickedEngine/Jolt/Physics/Collision/BackFaceMode.h new file mode 100644 index 000000000..441dcd89a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BackFaceMode.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// How collision detection functions will treat back facing triangles +enum class EBackFaceMode : uint8 +{ + IgnoreBackFaces, ///< Ignore collision with back facing surfaces/triangles + CollideWithBackFaces, ///< Collide with back facing surfaces/triangles +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp new file mode 100644 index 000000000..1317d1339 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.cpp @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void BroadPhase::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) +{ + mBodyManager = inBodyManager; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.h new file mode 100644 index 000000000..8b6506e90 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhase.h @@ -0,0 +1,112 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +// Shorthand function to ifdef out code if broadphase stats tracking is off +#ifdef JPH_TRACK_BROADPHASE_STATS + #define JPH_IF_TRACK_BROADPHASE_STATS(...) __VA_ARGS__ +#else + #define JPH_IF_TRACK_BROADPHASE_STATS(...) +#endif // JPH_TRACK_BROADPHASE_STATS + +class BodyManager; +struct BodyPair; + +using BodyPairCollector = CollisionCollector; + +/// Used to do coarse collision detection operations to quickly prune out bodies that will not collide. +class JPH_EXPORT BroadPhase : public BroadPhaseQuery +{ +public: + /// Initialize the broadphase. + /// @param inBodyManager The body manager singleton + /// @param inLayerInterface Interface that maps object layers to broadphase layers. + /// Note that the broadphase takes a pointer to the data inside inObjectToBroadPhaseLayer so this object should remain static. + virtual void Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface); + + /// Should be called after many objects have been inserted to make the broadphase more efficient, usually done on startup only + virtual void Optimize() { /* Optionally overridden by implementation */ } + + /// Must be called just before updating the broadphase when none of the body mutexes are locked + virtual void FrameSync() { /* Optionally overridden by implementation */ } + + /// Must be called before UpdatePrepare to prevent modifications from being made to the tree + virtual void LockModifications() { /* Optionally overridden by implementation */ } + + /// Context used during broadphase update + struct UpdateState { void *mData[4]; }; + + /// Update the broadphase, needs to be called frequently to update the internal state when bodies have been modified. + /// The UpdatePrepare() function can run in a background thread without influencing the broadphase + virtual UpdateState UpdatePrepare() { return UpdateState(); } + + /// Finalizing the update will quickly apply the changes + virtual void UpdateFinalize([[maybe_unused]] const UpdateState &inUpdateState) { /* Optionally overridden by implementation */ } + + /// Must be called after UpdateFinalize to allow modifications to the broadphase + virtual void UnlockModifications() { /* Optionally overridden by implementation */ } + + /// Handle used during adding bodies to the broadphase + using AddState = void *; + + /// Prepare adding inNumber bodies at ioBodies to the broadphase, returns a handle that should be used in AddBodiesFinalize/Abort. + /// This can be done on a background thread without influencing the broadphase. + /// ioBodies may be shuffled around by this function and should be kept that way until AddBodiesFinalize/Abort is called. + virtual AddState AddBodiesPrepare([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber) { return nullptr; } // By default the broadphase doesn't support this + + /// Finalize adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) = 0; + + /// Abort adding bodies to the broadphase, supply the return value of AddBodiesPrepare in inAddState. + /// This can be done on a background thread without influencing the broadphase. + /// Please ensure that the ioBodies array passed to AddBodiesPrepare is unmodified and passed again to this function. + virtual void AddBodiesAbort([[maybe_unused]] BodyID *ioBodies, [[maybe_unused]] int inNumber, [[maybe_unused]] AddState inAddState) { /* By default nothing needs to be done */ } + + /// Remove inNumber bodies in ioBodies from the broadphase. + /// ioBodies may be shuffled around by this function. + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) = 0; + + /// Call whenever the aabb of a body changes (can change order of ioBodies array) + /// inTakeLock should be false if we're between LockModifications/UnlockModificiations in which case care needs to be taken to not call this between UpdatePrepare/UpdateFinalize + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock = true) = 0; + + /// Call whenever the layer (and optionally the aabb as well) of a body changes (can change order of ioBodies array) + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) = 0; + + /// Find all colliding pairs between dynamic bodies + /// Note that this function is very specifically tailored for the PhysicsSystem::Update function, hence it is not part of the BroadPhaseQuery interface. + /// One of the assumptions it can make is that no locking is needed during the query as it will only be called during a very particular part of the update. + /// @param ioActiveBodies is a list of bodies for which we need to find colliding pairs (this function can change the order of the ioActiveBodies array). This can be a subset of the set of active bodies in the system. + /// @param inNumActiveBodies is the size of the ioActiveBodies array. + /// @param inSpeculativeContactDistance Distance at which speculative contact points will be created. + /// @param inObjectVsBroadPhaseLayerFilter is the filter that determines if an object can collide with a broadphase layer. + /// @param inObjectLayerPairFilter is the filter that determines if two objects can collide. + /// @param ioPairCollector receives callbacks for every body pair found. + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const = 0; + + /// Same as BroadPhaseQuery::CastAABox but can be implemented in a way to take no broad phase locks. + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const = 0; + + /// Get the bounding box of all objects in the broadphase + virtual AABox GetBounds() const = 0; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Trace the collected broadphase stats in CSV form. + /// This report can be used to judge and tweak the efficiency of the broadphase. + virtual void ReportStats() { /* Can be implemented by derived classes */ } +#endif // JPH_TRACK_BROADPHASE_STATS + +protected: + /// Link to the body manager that manages the bodies in this broadphase + BodyManager * mBodyManager = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp new file mode 100644 index 000000000..fc2332182 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.cpp @@ -0,0 +1,313 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void BroadPhaseBruteForce::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + lock_guard lock(mMutex); + + BodyVector &bodies = mBodyManager->GetBodies(); + + // Allocate space + uint32 idx = (uint32)mBodyIDs.size(); + mBodyIDs.resize(idx + inNumber); + + // Add bodies + for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b) + { + Body &body = *bodies[b->GetIndex()]; + + // Validate that body ID is consistent with array index + JPH_ASSERT(body.GetID() == *b); + JPH_ASSERT(!body.IsInBroadPhase()); + + // Add it to the list + mBodyIDs[idx] = body.GetID(); + ++idx; + + // Indicate body is in the broadphase + body.SetInBroadPhaseInternal(true); + } + + // Resort + QuickSort(mBodyIDs.begin(), mBodyIDs.end()); +} + +void BroadPhaseBruteForce::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + lock_guard lock(mMutex); + + BodyVector &bodies = mBodyManager->GetBodies(); + + JPH_ASSERT((int)mBodyIDs.size() >= inNumber); + + // Remove bodies + for (const BodyID *b = ioBodies, *b_end = ioBodies + inNumber; b < b_end; ++b) + { + Body &body = *bodies[b->GetIndex()]; + + // Validate that body ID is consistent with array index + JPH_ASSERT(body.GetID() == *b); + JPH_ASSERT(body.IsInBroadPhase()); + + // Find body id + Array::const_iterator it = std::lower_bound(mBodyIDs.begin(), mBodyIDs.end(), body.GetID()); + JPH_ASSERT(it != mBodyIDs.end()); + + // Remove element + mBodyIDs.erase(it); + + // Indicate body is no longer in the broadphase + body.SetInBroadPhaseInternal(false); + } +} + +void BroadPhaseBruteForce::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) +{ + // Do nothing, we directly reference the body +} + +void BroadPhaseBruteForce::NotifyBodiesLayerChanged(BodyID * ioBodies, int inNumber) +{ + // Do nothing, we directly reference the body +} + +void BroadPhaseBruteForce::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // Load ray + Vec3 origin(inRay.mOrigin); + RayInvDirection inv_direction(inRay.mDirection); + + // For all bodies + float early_out_fraction = ioCollector.GetEarlyOutFraction(); + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with ray + const AABox &bounds = body.GetWorldSpaceBounds(); + float fraction = RayAABox(origin, inv_direction, bounds.mMin, bounds.mMax); + if (fraction < early_out_fraction) + { + // Store hit + BroadPhaseCastResult result { b, fraction }; + ioCollector.AddHit(result); + if (ioCollector.ShouldEarlyOut()) + break; + early_out_fraction = ioCollector.GetEarlyOutFraction(); + } + } + } +} + +void BroadPhaseBruteForce::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.Overlaps(inBox)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + float radius_sq = Square(inRadius); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.GetSqDistanceTo(inCenter) <= radius_sq) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (bounds.Contains(inPoint)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // For all bodies + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with box + const AABox &bounds = body.GetWorldSpaceBounds(); + if (inBox.Overlaps(bounds)) + { + // Store hit + ioCollector.AddHit(b); + if (ioCollector.ShouldEarlyOut()) + break; + } + } + } +} + +void BroadPhaseBruteForce::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + shared_lock lock(mMutex); + + // Load box + Vec3 origin(inBox.mBox.GetCenter()); + Vec3 extent(inBox.mBox.GetExtent()); + RayInvDirection inv_direction(inBox.mDirection); + + // For all bodies + float early_out_fraction = ioCollector.GetPositiveEarlyOutFraction(); + for (BodyID b : mBodyIDs) + { + const Body &body = mBodyManager->GetBody(b); + + // Test layer + if (inObjectLayerFilter.ShouldCollide(body.GetObjectLayer())) + { + // Test intersection with ray + const AABox &bounds = body.GetWorldSpaceBounds(); + float fraction = RayAABox(origin, inv_direction, bounds.mMin - extent, bounds.mMax + extent); + if (fraction < early_out_fraction) + { + // Store hit + BroadPhaseCastResult result { b, fraction }; + ioCollector.AddHit(result); + if (ioCollector.ShouldEarlyOut()) + break; + early_out_fraction = ioCollector.GetPositiveEarlyOutFraction(); + } + } + } +} + +void BroadPhaseBruteForce::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void BroadPhaseBruteForce::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const +{ + shared_lock lock(mMutex); + + // Loop through all active bodies + size_t num_bodies = mBodyIDs.size(); + for (int b1 = 0; b1 < inNumActiveBodies; ++b1) + { + BodyID b1_id = ioActiveBodies[b1]; + const Body &body1 = mBodyManager->GetBody(b1_id); + const ObjectLayer layer1 = body1.GetObjectLayer(); + + // Expand the bounding box by the speculative contact distance + AABox bounds1 = body1.GetWorldSpaceBounds(); + bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance)); + + // For all other bodies + for (size_t b2 = 0; b2 < num_bodies; ++b2) + { + // Check if bodies can collide + BodyID b2_id = mBodyIDs[b2]; + const Body &body2 = mBodyManager->GetBody(b2_id); + if (!Body::sFindCollidingPairsCanCollide(body1, body2)) + continue; + + // Check if layers can collide + const ObjectLayer layer2 = body2.GetObjectLayer(); + if (!inObjectLayerPairFilter.ShouldCollide(layer1, layer2)) + continue; + + // Check if bounds overlap + const AABox &bounds2 = body2.GetWorldSpaceBounds(); + if (!bounds1.Overlaps(bounds2)) + continue; + + // Store overlapping pair + ioPairCollector.AddHit({ b1_id, b2_id }); + } + } +} + +AABox BroadPhaseBruteForce::GetBounds() const +{ + shared_lock lock(mMutex); + + AABox bounds; + for (BodyID b : mBodyIDs) + bounds.Encapsulate(mBodyManager->GetBody(b).GetWorldSpaceBounds()); + return bounds; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h new file mode 100644 index 000000000..c3e20f5c8 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseBruteForce.h @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Test BroadPhase implementation that does not do anything to speed up the operations. Can be used as a reference implementation. +class JPH_EXPORT BroadPhaseBruteForce final : public BroadPhase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + // Implementing interface of BroadPhase (see BroadPhase for documentation) + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) override; + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override; + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override; + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override; + virtual AABox GetBounds() const override; + +private: + Array mBodyIDs; + mutable SharedMutex mMutex; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h new file mode 100644 index 000000000..bd22cc3a5 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayer.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// An object layer can be mapped to a broadphase layer. Objects with the same broadphase layer will end up in the same sub structure (usually a tree) of the broadphase. +/// When there are many layers, this reduces the total amount of sub structures the broad phase needs to manage. Usually you want objects that don't collide with each other +/// in different broad phase layers, but there could be exceptions if objects layers only contain a minor amount of objects so it is not beneficial to give each layer its +/// own sub structure in the broadphase. +/// Note: This class requires explicit casting from and to Type to avoid confusion with ObjectLayer +class BroadPhaseLayer +{ +public: + using Type = uint8; + + JPH_INLINE BroadPhaseLayer() = default; + JPH_INLINE explicit constexpr BroadPhaseLayer(Type inValue) : mValue(inValue) { } + JPH_INLINE constexpr BroadPhaseLayer(const BroadPhaseLayer &) = default; + JPH_INLINE BroadPhaseLayer & operator = (const BroadPhaseLayer &) = default; + + JPH_INLINE constexpr bool operator == (const BroadPhaseLayer &inRHS) const + { + return mValue == inRHS.mValue; + } + + JPH_INLINE constexpr bool operator != (const BroadPhaseLayer &inRHS) const + { + return mValue != inRHS.mValue; + } + + JPH_INLINE constexpr bool operator < (const BroadPhaseLayer &inRHS) const + { + return mValue < inRHS.mValue; + } + + JPH_INLINE explicit constexpr operator Type() const + { + return mValue; + } + + JPH_INLINE Type GetValue() const + { + return mValue; + } + +private: + Type mValue; +}; + +/// Constant value used to indicate an invalid broad phase layer +static constexpr BroadPhaseLayer cBroadPhaseLayerInvalid(0xff); + +/// Interface that the application should implement to allow mapping object layers to broadphase layers +class BroadPhaseLayerInterface : public NonCopyable +{ +public: + /// Destructor + virtual ~BroadPhaseLayerInterface() = default; + + /// Return the number of broadphase layers there are + virtual uint GetNumBroadPhaseLayers() const = 0; + + /// Convert an object layer to the corresponding broadphase layer + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const = 0; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Get the user readable name of a broadphase layer (debugging purposes) + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const = 0; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +/// Class to test if an object can collide with a broadphase layer. Used while finding collision pairs. +class ObjectVsBroadPhaseLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectVsBroadPhaseLayerFilter() = default; + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] BroadPhaseLayer inLayer2) const + { + return true; + } +}; + +/// Filter class for broadphase layers +class BroadPhaseLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~BroadPhaseLayerFilter() = default; + + /// Function to filter out broadphase layers when doing collision query test (return true to allow testing against objects with this layer) + virtual bool ShouldCollide([[maybe_unused]] BroadPhaseLayer inLayer) const + { + return true; + } +}; + +/// Default filter class that uses the pair filter in combination with a specified layer to filter layers +class DefaultBroadPhaseLayerFilter : public BroadPhaseLayerFilter +{ +public: + /// Constructor + DefaultBroadPhaseLayerFilter(const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, ObjectLayer inLayer) : + mObjectVsBroadPhaseLayerFilter(inObjectVsBroadPhaseLayerFilter), + mLayer(inLayer) + { + } + + // See BroadPhaseLayerFilter::ShouldCollide + virtual bool ShouldCollide(BroadPhaseLayer inLayer) const override + { + return mObjectVsBroadPhaseLayerFilter.ShouldCollide(mLayer, inLayer); + } + +private: + const ObjectVsBroadPhaseLayerFilter &mObjectVsBroadPhaseLayerFilter; + ObjectLayer mLayer; +}; + +/// Allows objects from a specific broad phase layer only +class SpecifiedBroadPhaseLayerFilter : public BroadPhaseLayerFilter +{ +public: + /// Constructor + explicit SpecifiedBroadPhaseLayerFilter(BroadPhaseLayer inLayer) : + mLayer(inLayer) + { + } + + // See BroadPhaseLayerFilter::ShouldCollide + virtual bool ShouldCollide(BroadPhaseLayer inLayer) const override + { + return mLayer == inLayer; + } + +private: + BroadPhaseLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h new file mode 100644 index 000000000..15a894bb1 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceMask.h @@ -0,0 +1,92 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// BroadPhaseLayerInterface implementation. +/// This defines a mapping between object and broadphase layers. +/// This implementation works together with ObjectLayerPairFilterMask and ObjectVsBroadPhaseLayerFilterMask. +/// A broadphase layer is suitable for an object if its group & inGroupsToInclude is not zero and its group & inGroupsToExclude is zero. +/// The broadphase layers are iterated from lowest to highest value and the first one that matches is taken. If none match then it takes the last layer. +class BroadPhaseLayerInterfaceMask : public BroadPhaseLayerInterface +{ +public: + JPH_OVERRIDE_NEW_DELETE + + explicit BroadPhaseLayerInterfaceMask(uint inNumBroadPhaseLayers) + { + JPH_ASSERT(inNumBroadPhaseLayers > 0); + mMapping.resize(inNumBroadPhaseLayers); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined"); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } + + // Configures a broadphase layer. + void ConfigureLayer(BroadPhaseLayer inBroadPhaseLayer, uint32 inGroupsToInclude, uint32 inGroupsToExclude) + { + JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < (uint)mMapping.size()); + Mapping &m = mMapping[(BroadPhaseLayer::Type)inBroadPhaseLayer]; + m.mGroupsToInclude = inGroupsToInclude; + m.mGroupsToExclude = inGroupsToExclude; + } + + virtual uint GetNumBroadPhaseLayers() const override + { + return (uint)mMapping.size(); + } + + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override + { + // Try to find the first broadphase layer that matches + uint32 group = ObjectLayerPairFilterMask::sGetGroup(inLayer); + for (const Mapping &m : mMapping) + if ((group & m.mGroupsToInclude) != 0 && (group & m.mGroupsToExclude) == 0) + return BroadPhaseLayer(BroadPhaseLayer::Type(&m - mMapping.data())); + + // Fall back to the last broadphase layer + return BroadPhaseLayer(BroadPhaseLayer::Type(mMapping.size() - 1)); + } + + /// Returns true if an object layer should collide with a broadphase layer, this function is being called from ObjectVsBroadPhaseLayerFilterMask + inline bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const + { + uint32 mask = ObjectLayerPairFilterMask::sGetMask(inLayer1); + const Mapping &m = mMapping[(BroadPhaseLayer::Type)inLayer2]; + return &m == &mMapping.back() // Last layer may collide with anything + || (m.mGroupsToInclude & mask) != 0; // Mask allows it to collide with objects that could reside in this layer + } + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + void SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName) + { + mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName; + } + + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override + { + return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer]; + } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + +private: + struct Mapping + { + uint32 mGroupsToInclude = 0; + uint32 mGroupsToExclude = ~uint32(0); + }; + Array mMapping; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + Array mBroadPhaseLayerNames; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h new file mode 100644 index 000000000..e777a0858 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseLayerInterfaceTable.h @@ -0,0 +1,64 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// BroadPhaseLayerInterface implementation. +/// This defines a mapping between object and broadphase layers. +/// This implementation uses a simple table +class BroadPhaseLayerInterfaceTable : public BroadPhaseLayerInterface +{ +public: + JPH_OVERRIDE_NEW_DELETE + + BroadPhaseLayerInterfaceTable(uint inNumObjectLayers, uint inNumBroadPhaseLayers) : + mNumBroadPhaseLayers(inNumBroadPhaseLayers) + { + mObjectToBroadPhase.resize(inNumObjectLayers, BroadPhaseLayer(0)); +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + mBroadPhaseLayerNames.resize(inNumBroadPhaseLayers, "Undefined"); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } + + void MapObjectToBroadPhaseLayer(ObjectLayer inObjectLayer, BroadPhaseLayer inBroadPhaseLayer) + { + JPH_ASSERT((BroadPhaseLayer::Type)inBroadPhaseLayer < mNumBroadPhaseLayers); + mObjectToBroadPhase[inObjectLayer] = inBroadPhaseLayer; + } + + virtual uint GetNumBroadPhaseLayers() const override + { + return mNumBroadPhaseLayers; + } + + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override + { + return mObjectToBroadPhase[inLayer]; + } + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + void SetBroadPhaseLayerName(BroadPhaseLayer inLayer, const char *inName) + { + mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer] = inName; + } + + virtual const char * GetBroadPhaseLayerName(BroadPhaseLayer inLayer) const override + { + return mBroadPhaseLayerNames[(BroadPhaseLayer::Type)inLayer]; + } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + +private: + uint mNumBroadPhaseLayers; + Array mObjectToBroadPhase; +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + Array mBroadPhaseLayerNames; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp new file mode 100644 index 000000000..0c3d480ba --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.cpp @@ -0,0 +1,609 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +BroadPhaseQuadTree::~BroadPhaseQuadTree() +{ + delete [] mLayers; +} + +void BroadPhaseQuadTree::Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) +{ + BroadPhase::Init(inBodyManager, inLayerInterface); + + // Store input parameters + mBroadPhaseLayerInterface = &inLayerInterface; + mNumLayers = inLayerInterface.GetNumBroadPhaseLayers(); + JPH_ASSERT(mNumLayers < (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + +#ifdef JPH_ENABLE_ASSERTS + // Store lock context + mLockContext = inBodyManager; +#endif // JPH_ENABLE_ASSERTS + + // Store max bodies + mMaxBodies = inBodyManager->GetMaxBodies(); + + // Initialize tracking data + mTracking.resize(mMaxBodies); + + // Init allocator + // Estimate the amount of nodes we're going to need + uint32 num_leaves = (uint32)(mMaxBodies + 1) / 2; // Assume 50% fill + uint32 num_leaves_plus_internal_nodes = num_leaves + (num_leaves + 2) / 3; // = Sum(num_leaves * 4^-i) with i = [0, Inf]. + mAllocator.Init(2 * num_leaves_plus_internal_nodes, 256); // We use double the amount of nodes while rebuilding the tree during Update() + + // Init sub trees + mLayers = new QuadTree [mNumLayers]; + for (uint l = 0; l < mNumLayers; ++l) + { + mLayers[l].Init(mAllocator); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + // Set the name of the layer + mLayers[l].SetName(inLayerInterface.GetBroadPhaseLayerName(BroadPhaseLayer(BroadPhaseLayer::Type(l)))); +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + } +} + +void BroadPhaseQuadTree::FrameSync() +{ + JPH_PROFILE_FUNCTION(); + + // Take a unique lock on the old query lock so that we know no one is using the old nodes anymore. + // Note that nothing should be locked at this point to avoid risking a lock inversion deadlock. + // Note that in other places where we lock this mutex we don't use SharedLock to detect lock inversions. As long as + // nothing else is locked this is safe. This is why BroadPhaseQuery should be the highest priority lock. + UniqueLock root_lock(mQueryLocks[mQueryLockIdx ^ 1] JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseQuery)); + + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + mLayers[l].DiscardOldTree(); +} + +void BroadPhaseQuadTree::Optimize() +{ + JPH_PROFILE_FUNCTION(); + + FrameSync(); + + LockModifications(); + + for (uint l = 0; l < mNumLayers; ++l) + { + QuadTree &tree = mLayers[l]; + if (tree.HasBodies()) + { + QuadTree::UpdateState update_state; + tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state, true); + tree.UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state); + } + } + + UnlockModifications(); + + mNextLayerToUpdate = 0; +} + +void BroadPhaseQuadTree::LockModifications() +{ + // From this point on we prevent modifications to the tree + PhysicsLock::sLock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +BroadPhase::UpdateState BroadPhaseQuadTree::UpdatePrepare() +{ + // LockModifications should have been called + JPH_ASSERT(mUpdateMutex.is_locked()); + + // Create update state + UpdateState update_state; + UpdateStateImpl *update_state_impl = reinterpret_cast(&update_state); + + // Loop until we've seen all layers + for (uint iteration = 0; iteration < mNumLayers; ++iteration) + { + // Get the layer + QuadTree &tree = mLayers[mNextLayerToUpdate]; + mNextLayerToUpdate = (mNextLayerToUpdate + 1) % mNumLayers; + + // If it is dirty we update this one + if (tree.HasBodies() && tree.IsDirty() && tree.CanBeUpdated()) + { + update_state_impl->mTree = &tree; + tree.UpdatePrepare(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState, false); + return update_state; + } + } + + // Nothing to update + update_state_impl->mTree = nullptr; + return update_state; +} + +void BroadPhaseQuadTree::UpdateFinalize(const UpdateState &inUpdateState) +{ + // LockModifications should have been called + JPH_ASSERT(mUpdateMutex.is_locked()); + + // Test if a tree was updated + const UpdateStateImpl *update_state_impl = reinterpret_cast(&inUpdateState); + if (update_state_impl->mTree == nullptr) + return; + + update_state_impl->mTree->UpdateFinalize(mBodyManager->GetBodies(), mTracking, update_state_impl->mUpdateState); + + // Make all queries from now on use the new lock + mQueryLockIdx = mQueryLockIdx ^ 1; +} + +void BroadPhaseQuadTree::UnlockModifications() +{ + // From this point on we allow modifications to the tree again + PhysicsLock::sUnlock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +BroadPhase::AddState BroadPhaseQuadTree::AddBodiesPrepare(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = new LayerState [mNumLayers]; + + // Sort bodies on layer + Body * const * const bodies_ptr = bodies.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [bodies_ptr](BodyID inLHS, BodyID inRHS) { return bodies_ptr[inLHS.GetIndex()]->GetBroadPhaseLayer() < bodies_ptr[inRHS.GetIndex()]->GetBroadPhaseLayer(); }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broadphase layer + BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)bodies[b_start->GetIndex()]->GetBroadPhaseLayer(); + JPH_ASSERT(broadphase_layer < mNumLayers); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [bodies_ptr](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < (BroadPhaseLayer::Type)bodies_ptr[inBodyID.GetIndex()]->GetBroadPhaseLayer(); }); + + // Keep track of state for this layer + LayerState &layer_state = state[broadphase_layer]; + layer_state.mBodyStart = b_start; + layer_state.mBodyEnd = b_mid; + + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesPrepare(bodies, mTracking, b_start, int(b_mid - b_start), layer_state.mAddState); + + // Keep track in which tree we placed the object + for (const BodyID *b = b_start; b < b_mid; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + Tracking &t = mTracking[index]; + JPH_ASSERT(t.mBroadPhaseLayer == (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + t.mBroadPhaseLayer = broadphase_layer; + JPH_ASSERT(t.mObjectLayer == cObjectLayerInvalid); + t.mObjectLayer = bodies[index]->GetObjectLayer(); + } + + // Repeat + b_start = b_mid; + } + + return state; +} + +void BroadPhaseQuadTree::AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + JPH_PROFILE_FUNCTION(); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + + BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = (LayerState *)inAddState; + + for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++) + { + const LayerState &l = state[broadphase_layer]; + if (l.mBodyStart != nullptr) + { + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesFinalize(mTracking, int(l.mBodyEnd - l.mBodyStart), l.mAddState); + + // Mark added to broadphase + for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(mTracking[index].mBroadPhaseLayer == broadphase_layer); + JPH_ASSERT(mTracking[index].mObjectLayer == bodies[index]->GetObjectLayer()); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + bodies[index]->SetInBroadPhaseInternal(true); + } + } + } + + delete [] state; +} + +void BroadPhaseQuadTree::AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) +{ + JPH_PROFILE_FUNCTION(); + + JPH_IF_ENABLE_ASSERTS(const BodyVector &bodies = mBodyManager->GetBodies();) + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + LayerState *state = (LayerState *)inAddState; + + for (BroadPhaseLayer::Type broadphase_layer = 0; broadphase_layer < mNumLayers; broadphase_layer++) + { + const LayerState &l = state[broadphase_layer]; + if (l.mBodyStart != nullptr) + { + // Insert all bodies of the same layer + mLayers[broadphase_layer].AddBodiesAbort(mTracking, l.mAddState); + + // Reset bookkeeping + for (const BodyID *b = l.mBodyStart; b < l.mBodyEnd; ++b) + { + uint32 index = b->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *b, "Provided BodyID doesn't match BodyID in body manager"); + JPH_ASSERT(!bodies[index]->IsInBroadPhase()); + Tracking &t = mTracking[index]; + JPH_ASSERT(t.mBroadPhaseLayer == broadphase_layer); + t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + t.mObjectLayer = cObjectLayerInvalid; + } + } + } + + delete [] state; +} + +void BroadPhaseQuadTree::RemoveBodies(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + SharedLock lock(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + + JPH_ASSERT(inNumber > 0); + + BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Sort bodies on layer + Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broad phase layer + BroadPhaseLayer::Type broadphase_layer = mTracking[b_start->GetIndex()].mBroadPhaseLayer; + JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; }); + + // Remove all bodies of the same layer + mLayers[broadphase_layer].RemoveBodies(bodies, mTracking, b_start, int(b_mid - b_start)); + + for (const BodyID *b = b_start; b < b_mid; ++b) + { + // Reset bookkeeping + uint32 index = b->GetIndex(); + Tracking &t = tracking[index]; + t.mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + t.mObjectLayer = cObjectLayerInvalid; + + // Mark removed from broadphase + JPH_ASSERT(bodies[index]->IsInBroadPhase()); + bodies[index]->SetInBroadPhaseInternal(false); + } + + // Repeat + b_start = b_mid; + } +} + +void BroadPhaseQuadTree::NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + // This cannot run concurrently with UpdatePrepare()/UpdateFinalize() + if (inTakeLock) + PhysicsLock::sLockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); + else + JPH_ASSERT(mUpdateMutex.is_locked()); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Sort bodies on layer + const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioBodies, ioBodies + inNumber, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mBroadPhaseLayer < tracking[inRHS.GetIndex()].mBroadPhaseLayer; }); + + BodyID *b_start = ioBodies, *b_end = ioBodies + inNumber; + while (b_start < b_end) + { + // Get broadphase layer + BroadPhaseLayer::Type broadphase_layer = tracking[b_start->GetIndex()].mBroadPhaseLayer; + JPH_ASSERT(broadphase_layer != (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, broadphase_layer, [tracking](BroadPhaseLayer::Type inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mBroadPhaseLayer; }); + + // Nodify all bodies of the same layer changed + mLayers[broadphase_layer].NotifyBodiesAABBChanged(bodies, mTracking, b_start, int(b_mid - b_start)); + + // Repeat + b_start = b_mid; + } + + if (inTakeLock) + PhysicsLock::sUnlockShared(mUpdateMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::BroadPhaseUpdate)); +} + +void BroadPhaseQuadTree::NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inNumber > 0); + + // First sort the bodies that actually changed layer to beginning of the array + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + for (BodyID *body_id = ioBodies + inNumber - 1; body_id >= ioBodies; --body_id) + { + uint32 index = body_id->GetIndex(); + JPH_ASSERT(bodies[index]->GetID() == *body_id, "Provided BodyID doesn't match BodyID in body manager"); + const Body *body = bodies[index]; + BroadPhaseLayer::Type broadphase_layer = (BroadPhaseLayer::Type)body->GetBroadPhaseLayer(); + JPH_ASSERT(broadphase_layer < mNumLayers); + if (mTracking[index].mBroadPhaseLayer == broadphase_layer) + { + // Update tracking information + mTracking[index].mObjectLayer = body->GetObjectLayer(); + + // Move the body to the end, layer didn't change + swap(*body_id, ioBodies[inNumber - 1]); + --inNumber; + } + } + + if (inNumber > 0) + { + // Changing layer requires us to remove from one tree and add to another, so this is equivalent to removing all bodies first and then adding them again + RemoveBodies(ioBodies, inNumber); + AddState add_state = AddBodiesPrepare(ioBodies, inNumber); + AddBodiesFinalize(ioBodies, inNumber, add_state); + } +} + +void BroadPhaseQuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CastRay(inRay, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideAABox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideSphere(inCenter, inRadius, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollidePoint(inPoint, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CollideOrientedBox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inBroadPhaseLayerFilter.ShouldCollide(BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.CastAABox(inBox, ioCollector, inObjectLayerFilter, mTracking); + if (ioCollector.ShouldEarlyOut()) + break; + } + } +} + +void BroadPhaseQuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const +{ + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + CastAABoxNoLock(inBox, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void BroadPhaseQuadTree::FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const +{ + JPH_PROFILE_FUNCTION(); + + const BodyVector &bodies = mBodyManager->GetBodies(); + JPH_ASSERT(mMaxBodies == mBodyManager->GetMaxBodies()); + + // Note that we don't take any locks at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update. + + // Sort bodies on layer + const Tracking *tracking = mTracking.data(); // C pointer or else sort is incredibly slow in debug mode + QuickSort(ioActiveBodies, ioActiveBodies + inNumActiveBodies, [tracking](BodyID inLHS, BodyID inRHS) { return tracking[inLHS.GetIndex()].mObjectLayer < tracking[inRHS.GetIndex()].mObjectLayer; }); + + BodyID *b_start = ioActiveBodies, *b_end = ioActiveBodies + inNumActiveBodies; + while (b_start < b_end) + { + // Get broadphase layer + ObjectLayer object_layer = tracking[b_start->GetIndex()].mObjectLayer; + JPH_ASSERT(object_layer != cObjectLayerInvalid); + + // Find first body with different layer + BodyID *b_mid = std::upper_bound(b_start, b_end, object_layer, [tracking](ObjectLayer inLayer, BodyID inBodyID) { return inLayer < tracking[inBodyID.GetIndex()].mObjectLayer; }); + + // Loop over all layers and test the ones that could hit + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + { + const QuadTree &tree = mLayers[l]; + if (tree.HasBodies() && inObjectVsBroadPhaseLayerFilter.ShouldCollide(object_layer, BroadPhaseLayer(l))) + { + JPH_PROFILE(tree.GetName()); + tree.FindCollidingPairs(bodies, b_start, int(b_mid - b_start), inSpeculativeContactDistance, ioPairCollector, inObjectLayerPairFilter); + } + } + + // Repeat + b_start = b_mid; + } +} + +AABox BroadPhaseQuadTree::GetBounds() const +{ + // Prevent this from running in parallel with node deletion in FrameSync(), see notes there + shared_lock lock(mQueryLocks[mQueryLockIdx]); + + AABox bounds; + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + bounds.Encapsulate(mLayers[l].GetBounds()); + return bounds; +} + +#ifdef JPH_TRACK_BROADPHASE_STATS + +void BroadPhaseQuadTree::ReportStats() +{ + Trace("Query Type, Filter Description, Tree Name, Num Queries, Total Time (%%), Total Time Excl. Collector (%%), Nodes Visited, Bodies Visited, Hits Reported, Hits Reported vs Bodies Visited (%%), Hits Reported vs Nodes Visited"); + + uint64 total_ticks = 0; + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + total_ticks += mLayers[l].GetTicks100Pct(); + + for (BroadPhaseLayer::Type l = 0; l < mNumLayers; ++l) + mLayers[l].ReportStats(total_ticks); +} + +#endif // JPH_TRACK_BROADPHASE_STATS + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h new file mode 100644 index 000000000..ae97c10f0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h @@ -0,0 +1,108 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Fast SIMD based quad tree BroadPhase that is multithreading aware and tries to do a minimal amount of locking. +class JPH_EXPORT BroadPhaseQuadTree final : public BroadPhase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Destructor + virtual ~BroadPhaseQuadTree() override; + + // Implementing interface of BroadPhase (see BroadPhase for documentation) + virtual void Init(BodyManager *inBodyManager, const BroadPhaseLayerInterface &inLayerInterface) override; + virtual void Optimize() override; + virtual void FrameSync() override; + virtual void LockModifications() override; + virtual UpdateState UpdatePrepare() override; + virtual void UpdateFinalize(const UpdateState &inUpdateState) override; + virtual void UnlockModifications() override; + virtual AddState AddBodiesPrepare(BodyID *ioBodies, int inNumber) override; + virtual void AddBodiesFinalize(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void AddBodiesAbort(BodyID *ioBodies, int inNumber, AddState inAddState) override; + virtual void RemoveBodies(BodyID *ioBodies, int inNumber) override; + virtual void NotifyBodiesAABBChanged(BodyID *ioBodies, int inNumber, bool inTakeLock) override; + virtual void NotifyBodiesLayerChanged(BodyID *ioBodies, int inNumber) override; + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABoxNoLock(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter) const override; + virtual void FindCollidingPairs(BodyID *ioActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter, BodyPairCollector &ioPairCollector) const override; + virtual AABox GetBounds() const override; +#ifdef JPH_TRACK_BROADPHASE_STATS + virtual void ReportStats() override; +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + /// Helper struct for AddBodies handle + struct LayerState + { + JPH_OVERRIDE_NEW_DELETE + + BodyID * mBodyStart = nullptr; + BodyID * mBodyEnd; + QuadTree::AddState mAddState; + }; + + using Tracking = QuadTree::Tracking; + using TrackingVector = QuadTree::TrackingVector; + +#ifdef JPH_ENABLE_ASSERTS + /// Context used to lock a physics lock + PhysicsLockContext mLockContext = nullptr; +#endif // JPH_ENABLE_ASSERTS + + /// Max amount of bodies we support + size_t mMaxBodies = 0; + + /// Array that for each BodyID keeps track of where it is located in which tree + TrackingVector mTracking; + + /// Node allocator for all trees + QuadTree::Allocator mAllocator; + + /// Information about broad phase layers + const BroadPhaseLayerInterface *mBroadPhaseLayerInterface = nullptr; + + /// One tree per object layer + QuadTree * mLayers; + uint mNumLayers; + + /// UpdateState implementation for this tree used during UpdatePrepare/Finalize() + struct UpdateStateImpl + { + QuadTree * mTree; + QuadTree::UpdateState mUpdateState; + }; + + static_assert(sizeof(UpdateStateImpl) <= sizeof(UpdateState)); + static_assert(alignof(UpdateStateImpl) <= alignof(UpdateState)); + + /// Mutex that prevents object modification during UpdatePrepare/Finalize() + SharedMutex mUpdateMutex; + + /// We double buffer all trees so that we can query while building the next one and we destroy the old tree the next physics update. + /// This structure ensures that we wait for queries that are still using the old tree. + mutable SharedMutex mQueryLocks[2]; + + /// This index indicates which lock is currently active, it alternates between 0 and 1 + atomic mQueryLockIdx { 0 }; + + /// This is the next tree to update in UpdatePrepare() + uint32 mNextLayerToUpdate = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h new file mode 100644 index 000000000..10085e66f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/BroadPhaseQuery.h @@ -0,0 +1,53 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RayCast; +class BroadPhaseCastResult; +class AABox; +class OrientedBox; +struct AABoxCast; + +// Various collector configurations +using RayCastBodyCollector = CollisionCollector; +using CastShapeBodyCollector = CollisionCollector; +using CollideShapeBodyCollector = CollisionCollector; + +/// Interface to the broadphase that can perform collision queries. These queries will only test the bounding box of the body to quickly determine a potential set of colliding bodies. +/// The shapes of the bodies are not tested, if you want this then you should use the NarrowPhaseQuery interface. +class JPH_EXPORT BroadPhaseQuery : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~BroadPhaseQuery() = default; + + /// Cast a ray and add any hits to ioCollector + virtual void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with inBox and any hits to ioCollector + virtual void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with a sphere and any hits to ioCollector + virtual void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with a point and any hits to ioCollector + virtual void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Get bodies intersecting with an oriented box and any hits to ioCollector + virtual void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; + + /// Cast a box and add any hits to ioCollector + virtual void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }) const = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h new file mode 100644 index 000000000..20305a5f0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterMask.h @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that determines if an object layer can collide with a broadphase layer. +/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectLayerPairFilterMask +class ObjectVsBroadPhaseLayerFilterMask : public ObjectVsBroadPhaseLayerFilter +{ +public: + JPH_OVERRIDE_NEW_DELETE + +/// Constructor + ObjectVsBroadPhaseLayerFilterMask(const BroadPhaseLayerInterfaceMask &inBroadPhaseLayerInterface) : + mBroadPhaseLayerInterface(inBroadPhaseLayerInterface) + { + } + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override + { + // Just defer to BroadPhaseLayerInterface + return mBroadPhaseLayerInterface.ShouldCollide(inLayer1, inLayer2); + } + +private: + const BroadPhaseLayerInterfaceMask &mBroadPhaseLayerInterface; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h new file mode 100644 index 000000000..532ce6da0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/ObjectVsBroadPhaseLayerFilterTable.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that determines if an object layer can collide with a broadphase layer. +/// This implementation uses a table and constructs itself from an ObjectLayerPairFilter and a BroadPhaseLayerInterface. +class ObjectVsBroadPhaseLayerFilterTable : public ObjectVsBroadPhaseLayerFilter +{ +private: + /// Get which bit corresponds to the pair (inLayer1, inLayer2) + uint GetBit(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const + { + // Calculate at which bit the entry for this pair resides + return inLayer1 * mNumBroadPhaseLayers + (BroadPhaseLayer::Type)inLayer2; + } + +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct the table + /// @param inBroadPhaseLayerInterface The broad phase layer interface that maps object layers to broad phase layers + /// @param inNumBroadPhaseLayers Number of broad phase layers + /// @param inObjectLayerPairFilter The object layer pair filter that determines which object layers can collide + /// @param inNumObjectLayers Number of object layers + ObjectVsBroadPhaseLayerFilterTable(const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, uint inNumBroadPhaseLayers, const ObjectLayerPairFilter &inObjectLayerPairFilter, uint inNumObjectLayers) : + mNumBroadPhaseLayers(inNumBroadPhaseLayers) + { + // Resize table and set all entries to false + mTable.resize((inNumBroadPhaseLayers * inNumObjectLayers + 7) / 8, 0); + + // Loop over all object layer pairs + for (ObjectLayer o1 = 0; o1 < inNumObjectLayers; ++o1) + for (ObjectLayer o2 = 0; o2 < inNumObjectLayers; ++o2) + { + // Get the broad phase layer for the second object layer + BroadPhaseLayer b2 = inBroadPhaseLayerInterface.GetBroadPhaseLayer(o2); + JPH_ASSERT((BroadPhaseLayer::Type)b2 < inNumBroadPhaseLayers); + + // If the object layers collide then so should the object and broadphase layer + if (inObjectLayerPairFilter.ShouldCollide(o1, o2)) + { + uint bit = GetBit(o1, b2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + } + } + + /// Returns true if an object layer should collide with a broadphase layer + virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override + { + uint bit = GetBit(inLayer1, inLayer2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + +private: + uint mNumBroadPhaseLayers; ///< The total number of broadphase layers + Array mTable; ///< The table of bits that indicates which layers collide +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp b/WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp new file mode 100644 index 000000000..86d79bcb6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.cpp @@ -0,0 +1,1686 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef JPH_DUMP_BROADPHASE_TREE +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END +#endif // JPH_DUMP_BROADPHASE_TREE + +JPH_NAMESPACE_BEGIN + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// QuadTree::Node +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +QuadTree::Node::Node(bool inIsChanged) : + mIsChanged(inIsChanged) +{ + // First reset bounds + Vec4 val = Vec4::sReplicate(cLargeFloat); + val.StoreFloat4((Float4 *)&mBoundsMinX); + val.StoreFloat4((Float4 *)&mBoundsMinY); + val.StoreFloat4((Float4 *)&mBoundsMinZ); + val = Vec4::sReplicate(-cLargeFloat); + val.StoreFloat4((Float4 *)&mBoundsMaxX); + val.StoreFloat4((Float4 *)&mBoundsMaxY); + val.StoreFloat4((Float4 *)&mBoundsMaxZ); + + // Reset child node ids + mChildNodeID[0] = NodeID::sInvalid(); + mChildNodeID[1] = NodeID::sInvalid(); + mChildNodeID[2] = NodeID::sInvalid(); + mChildNodeID[3] = NodeID::sInvalid(); +} + +void QuadTree::Node::GetChildBounds(int inChildIndex, AABox &outBounds) const +{ + // Read bounding box in order min -> max + outBounds.mMin = Vec3(mBoundsMinX[inChildIndex], mBoundsMinY[inChildIndex], mBoundsMinZ[inChildIndex]); + outBounds.mMax = Vec3(mBoundsMaxX[inChildIndex], mBoundsMaxY[inChildIndex], mBoundsMaxZ[inChildIndex]); +} + +void QuadTree::Node::SetChildBounds(int inChildIndex, const AABox &inBounds) +{ + // Set max first (this keeps the bounding box invalid for reading threads) + mBoundsMaxZ[inChildIndex] = inBounds.mMax.GetZ(); + mBoundsMaxY[inChildIndex] = inBounds.mMax.GetY(); + mBoundsMaxX[inChildIndex] = inBounds.mMax.GetX(); + + // Then set min (and make box valid) + mBoundsMinZ[inChildIndex] = inBounds.mMin.GetZ(); + mBoundsMinY[inChildIndex] = inBounds.mMin.GetY(); + mBoundsMinX[inChildIndex] = inBounds.mMin.GetX(); // Min X becomes valid last +} + +void QuadTree::Node::InvalidateChildBounds(int inChildIndex) +{ + // First we make the box invalid by setting the min to cLargeFloat + mBoundsMinX[inChildIndex] = cLargeFloat; // Min X becomes invalid first + mBoundsMinY[inChildIndex] = cLargeFloat; + mBoundsMinZ[inChildIndex] = cLargeFloat; + + // Then we reset the max values too + mBoundsMaxX[inChildIndex] = -cLargeFloat; + mBoundsMaxY[inChildIndex] = -cLargeFloat; + mBoundsMaxZ[inChildIndex] = -cLargeFloat; +} + +void QuadTree::Node::GetNodeBounds(AABox &outBounds) const +{ + // Get first child bounds + GetChildBounds(0, outBounds); + + // Encapsulate other child bounds + for (int child_idx = 1; child_idx < 4; ++child_idx) + { + AABox tmp; + GetChildBounds(child_idx, tmp); + outBounds.Encapsulate(tmp); + } +} + +bool QuadTree::Node::EncapsulateChildBounds(int inChildIndex, const AABox &inBounds) +{ + bool changed = AtomicMin(mBoundsMinX[inChildIndex], inBounds.mMin.GetX()); + changed |= AtomicMin(mBoundsMinY[inChildIndex], inBounds.mMin.GetY()); + changed |= AtomicMin(mBoundsMinZ[inChildIndex], inBounds.mMin.GetZ()); + changed |= AtomicMax(mBoundsMaxX[inChildIndex], inBounds.mMax.GetX()); + changed |= AtomicMax(mBoundsMaxY[inChildIndex], inBounds.mMax.GetY()); + changed |= AtomicMax(mBoundsMaxZ[inChildIndex], inBounds.mMax.GetZ()); + return changed; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// QuadTree +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +const float QuadTree::cLargeFloat = 1.0e30f; +const AABox QuadTree::cInvalidBounds(Vec3::sReplicate(cLargeFloat), Vec3::sReplicate(-cLargeFloat)); + +void QuadTree::GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const +{ + uint32 body_location = inTracking[inBodyID.GetIndex()].mBodyLocation; + JPH_ASSERT(body_location != Tracking::cInvalidBodyLocation); + outNodeIdx = body_location & 0x3fffffff; + outChildIdx = body_location >> 30; + JPH_ASSERT(mAllocator->Get(outNodeIdx).mChildNodeID[outChildIdx] == inBodyID, "Make sure that the body is in the node where it should be"); +} + +void QuadTree::SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const +{ + JPH_ASSERT(inNodeIdx <= 0x3fffffff); + JPH_ASSERT(inChildIdx < 4); + JPH_ASSERT(mAllocator->Get(inNodeIdx).mChildNodeID[inChildIdx] == inBodyID, "Make sure that the body is in the node where it should be"); + ioTracking[inBodyID.GetIndex()].mBodyLocation = inNodeIdx + (inChildIdx << 30); + +#ifdef JPH_ENABLE_ASSERTS + uint32 v1, v2; + GetBodyLocation(ioTracking, inBodyID, v1, v2); + JPH_ASSERT(v1 == inNodeIdx); + JPH_ASSERT(v2 == inChildIdx); +#endif +} + +void QuadTree::sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID) +{ + ioTracking[inBodyID.GetIndex()].mBodyLocation = Tracking::cInvalidBodyLocation; +} + +QuadTree::~QuadTree() +{ + // Get rid of any nodes that are still to be freed + DiscardOldTree(); + + // Get the current root node + const RootNode &root_node = GetCurrentRoot(); + + // Collect all bodies + Allocator::Batch free_batch; + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + JPH_ASSERT(node_stack[0].IsValid()); + if (node_stack[0].IsNode()) + { + int top = 0; + do + { + // Process node + NodeID node_id = node_stack[top]; + JPH_ASSERT(!node_id.IsBody()); + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + // Recurse and get all child nodes + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid() && child_node_id.IsNode()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = child_node_id; + top++; + } + + // Mark node to be freed + mAllocator->AddObjectToBatch(free_batch, node_idx); + --top; + } + while (top >= 0); + } + + // Now free all nodes + mAllocator->DestructObjectBatch(free_batch); +} + +uint32 QuadTree::AllocateNode(bool inIsChanged) +{ + uint32 index = mAllocator->ConstructObject(inIsChanged); + if (index == Allocator::cInvalidObjectIndex) + { + Trace("QuadTree: Out of nodes!"); + std::abort(); + } + return index; +} + +void QuadTree::Init(Allocator &inAllocator) +{ + // Store allocator + mAllocator = &inAllocator; + + // Allocate root node + mRootNode[mRootNodeIndex].mIndex = AllocateNode(false); +} + +void QuadTree::DiscardOldTree() +{ + // Check if there is an old tree + RootNode &old_root_node = mRootNode[mRootNodeIndex ^ 1]; + if (old_root_node.mIndex != cInvalidNodeIndex) + { + // Clear the root + old_root_node.mIndex = cInvalidNodeIndex; + + // Now free all old nodes + mAllocator->DestructObjectBatch(mFreeNodeBatch); + + // Clear the batch + mFreeNodeBatch = Allocator::Batch(); + } +} + +AABox QuadTree::GetBounds() const +{ + uint32 node_idx = GetCurrentRoot().mIndex; + JPH_ASSERT(node_idx != cInvalidNodeIndex); + const Node &node = mAllocator->Get(node_idx); + + AABox bounds; + node.GetNodeBounds(bounds); + return bounds; +} + +void QuadTree::UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); +#endif + + // Assert we have no nodes pending deletion, this means DiscardOldTree wasn't called yet + JPH_ASSERT(mFreeNodeBatch.mNumObjects == 0); + + // Mark tree non-dirty + mIsDirty = false; + + // Get the current root node + const RootNode &root_node = GetCurrentRoot(); + +#ifdef JPH_DUMP_BROADPHASE_TREE + DumpTree(root_node.GetNodeID(), StringFormat("%s_PRE", mName).c_str()); +#endif + + // Assert sane data +#ifdef JPH_DEBUG + ValidateTree(inBodies, ioTracking, root_node.mIndex, mNumBodies); +#endif + + // Create space for all body ID's + NodeID *node_ids = new NodeID [mNumBodies]; + NodeID *cur_node_id = node_ids; + + // Collect all bodies + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID node_id = node_stack[top]; + if (node_id.IsBody()) + { + // Validate that we're still in the right layer + #ifdef JPH_ENABLE_ASSERTS + uint32 body_index = node_id.GetBodyID().GetIndex(); + JPH_ASSERT(ioTracking[body_index].mObjectLayer == inBodies[body_index]->GetObjectLayer()); + #endif + + // Store body + *cur_node_id = node_id; + ++cur_node_id; + } + else + { + // Process normal node + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + if (!node.mIsChanged && !inFullRebuild) + { + // Node is unchanged, treat it as a whole + *cur_node_id = node_id; + ++cur_node_id; + } + else + { + // Node is changed, recurse and get all children + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid()) + { + if (top < cStackSize) + { + node_stack[top] = child_node_id; + top++; + } + else + { + JPH_ASSERT(false); // Out of stack space, this must be a very deep tree. Are you batch adding bodies to the broadphase? + + // Falling back to adding the node as a whole + *cur_node_id = child_node_id; + ++cur_node_id; + } + } + + // Mark node to be freed + mAllocator->AddObjectToBatch(mFreeNodeBatch, node_idx); + } + } + --top; + } + while (top >= 0); + + // Check that our book keeping matches + uint32 num_node_ids = uint32(cur_node_id - node_ids); + JPH_ASSERT(inFullRebuild? num_node_ids == mNumBodies : num_node_ids <= mNumBodies); + + // This will be the new root node id + NodeID root_node_id; + + if (num_node_ids > 0) + { + // We mark the first 5 levels (max 1024 nodes) of the newly built tree as 'changed' so that + // those nodes get recreated every time when we rebuild the tree. This balances the amount of + // time we spend on rebuilding the tree ('unchanged' nodes will be put in the new tree as a whole) + // vs the quality of the built tree. + constexpr uint cMaxDepthMarkChanged = 5; + + // Build new tree + AABox root_bounds; + root_node_id = BuildTree(inBodies, ioTracking, node_ids, num_node_ids, cMaxDepthMarkChanged, root_bounds); + + if (root_node_id.IsBody()) + { + // For a single body we need to allocate a new root node + uint32 root_idx = AllocateNode(false); + Node &root = mAllocator->Get(root_idx); + root.SetChildBounds(0, root_bounds); + root.mChildNodeID[0] = root_node_id; + SetBodyLocation(ioTracking, root_node_id.GetBodyID(), root_idx, 0); + root_node_id = NodeID::sFromNodeIndex(root_idx); + } + } + else + { + // Empty tree, create root node + uint32 root_idx = AllocateNode(false); + root_node_id = NodeID::sFromNodeIndex(root_idx); + } + + // Delete temporary data + delete [] node_ids; + + outUpdateState.mRootNodeID = root_node_id; +} + +void QuadTree::UpdateFinalize([[maybe_unused]] const BodyVector &inBodies, [[maybe_unused]] const TrackingVector &inTracking, const UpdateState &inUpdateState) +{ + // Tree building is complete, now we switch the old with the new tree + uint32 new_root_idx = mRootNodeIndex ^ 1; + RootNode &new_root_node = mRootNode[new_root_idx]; + { + // Note: We don't need to lock here as the old tree stays available so any queries + // that use it can continue using it until DiscardOldTree is called. This slot + // should be empty and unused at this moment. + JPH_ASSERT(new_root_node.mIndex == cInvalidNodeIndex); + new_root_node.mIndex = inUpdateState.mRootNodeID.GetNodeIndex(); + } + + // All queries that start from now on will use this new tree + mRootNodeIndex = new_root_idx; + +#ifdef JPH_DUMP_BROADPHASE_TREE + DumpTree(new_root_node.GetNodeID(), StringFormat("%s_POST", mName).c_str()); +#endif + +#ifdef JPH_DEBUG + ValidateTree(inBodies, inTracking, new_root_node.mIndex, mNumBodies); +#endif +} + +void QuadTree::sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint) +{ + // Handle trivial case + if (inNumber <= 4) + { + outMidPoint = inNumber / 2; + return; + } + + // Calculate bounding box of box centers + Vec3 center_min = Vec3::sReplicate(cLargeFloat); + Vec3 center_max = Vec3::sReplicate(-cLargeFloat); + for (const Vec3 *c = ioNodeCenters, *c_end = ioNodeCenters + inNumber; c < c_end; ++c) + { + Vec3 center = *c; + center_min = Vec3::sMin(center_min, center); + center_max = Vec3::sMax(center_max, center); + } + + // Calculate split plane + int dimension = (center_max - center_min).GetHighestComponentIndex(); + float split = 0.5f * (center_min + center_max)[dimension]; + + // Divide bodies + int start = 0, end = inNumber; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && ioNodeCenters[start][dimension] < split) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && ioNodeCenters[end - 1][dimension] >= split) + --end; + + if (start < end) + { + // Swap the two elements + swap(ioNodeIDs[start], ioNodeIDs[end - 1]); + swap(ioNodeCenters[start], ioNodeCenters[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + + if (start > 0 && start < inNumber) + { + // Success! + outMidPoint = start; + } + else + { + // Failed to divide bodies + outMidPoint = inNumber / 2; + } +} + +void QuadTree::sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit) +{ + NodeID *node_ids = ioNodeIDs + inBegin; + Vec3 *node_centers = ioNodeCenters + inBegin; + int number = inEnd - inBegin; + + // Partition entire range + sPartition(node_ids, node_centers, number, outSplit[2]); + + // Partition lower half + sPartition(node_ids, node_centers, outSplit[2], outSplit[1]); + + // Partition upper half + sPartition(node_ids + outSplit[2], node_centers + outSplit[2], number - outSplit[2], outSplit[3]); + + // Convert to proper range + outSplit[0] = inBegin; + outSplit[1] += inBegin; + outSplit[2] += inBegin; + outSplit[3] += outSplit[2]; + outSplit[4] = inEnd; +} + +AABox QuadTree::GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const +{ + if (inNodeID.IsNode()) + { + // It is a node + uint32 node_idx = inNodeID.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + AABox bounds; + node.GetNodeBounds(bounds); + return bounds; + } + else + { + // It is a body + return inBodies[inNodeID.GetBodyID().GetIndex()]->GetWorldSpaceBounds(); + } +} + +QuadTree::NodeID QuadTree::BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds) +{ + // Trivial case: No bodies in tree + if (inNumber == 0) + { + outBounds = cInvalidBounds; + return NodeID::sInvalid(); + } + + // Trivial case: When we have 1 body or node, return it + if (inNumber == 1) + { + if (ioNodeIDs->IsNode()) + { + // When returning an existing node as root, ensure that no parent has been set + Node &node = mAllocator->Get(ioNodeIDs->GetNodeIndex()); + node.mParentNodeIndex = cInvalidNodeIndex; + } + outBounds = GetNodeOrBodyBounds(inBodies, *ioNodeIDs); + return *ioNodeIDs; + } + + // Calculate centers of all bodies that are to be inserted + Vec3 *centers = new Vec3 [inNumber]; + JPH_ASSERT(IsAligned(centers, JPH_VECTOR_ALIGNMENT)); + Vec3 *c = centers; + for (const NodeID *n = ioNodeIDs, *n_end = ioNodeIDs + inNumber; n < n_end; ++n, ++c) + *c = GetNodeOrBodyBounds(inBodies, *n).GetCenter(); + + // The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here + struct StackEntry + { + uint32 mNodeIdx; // Node index of node that is generated + int mChildIdx; // Index of child that we're currently processing + int mSplit[5]; // Indices where the node ID's have been split to form 4 partitions + uint32 mDepth; // Depth of this node in the tree + Vec3 mNodeBoundsMin; // Bounding box of this node, accumulated while iterating over children + Vec3 mNodeBoundsMax; + }; + static_assert(sizeof(StackEntry) == 64); + StackEntry stack[cStackSize / 4]; // We don't process 4 at a time in this loop but 1, so the stack can be 4x as small + int top = 0; + + // Create root node + stack[0].mNodeIdx = AllocateNode(inMaxDepthMarkChanged > 0); + stack[0].mChildIdx = -1; + stack[0].mDepth = 0; + stack[0].mNodeBoundsMin = Vec3::sReplicate(cLargeFloat); + stack[0].mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat); + sPartition4(ioNodeIDs, centers, 0, inNumber, stack[0].mSplit); + + for (;;) + { + StackEntry &cur_stack = stack[top]; + + // Next child + cur_stack.mChildIdx++; + + // Check if all children processed + if (cur_stack.mChildIdx >= 4) + { + // Terminate if there's nothing left to pop + if (top <= 0) + break; + + // Add our bounds to our parents bounds + StackEntry &prev_stack = stack[top - 1]; + prev_stack.mNodeBoundsMin = Vec3::sMin(prev_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMin); + prev_stack.mNodeBoundsMax = Vec3::sMax(prev_stack.mNodeBoundsMax, cur_stack.mNodeBoundsMax); + + // Store parent node + Node &node = mAllocator->Get(cur_stack.mNodeIdx); + node.mParentNodeIndex = prev_stack.mNodeIdx; + + // Store this node's properties in the parent node + Node &parent_node = mAllocator->Get(prev_stack.mNodeIdx); + parent_node.mChildNodeID[prev_stack.mChildIdx] = NodeID::sFromNodeIndex(cur_stack.mNodeIdx); + parent_node.SetChildBounds(prev_stack.mChildIdx, AABox(cur_stack.mNodeBoundsMin, cur_stack.mNodeBoundsMax)); + + // Pop entry from stack + --top; + } + else + { + // Get low and high index to bodies to process + int low = cur_stack.mSplit[cur_stack.mChildIdx]; + int high = cur_stack.mSplit[cur_stack.mChildIdx + 1]; + int num_bodies = high - low; + + if (num_bodies == 1) + { + // Get body info + NodeID child_node_id = ioNodeIDs[low]; + AABox bounds = GetNodeOrBodyBounds(inBodies, child_node_id); + + // Update node + Node &node = mAllocator->Get(cur_stack.mNodeIdx); + node.mChildNodeID[cur_stack.mChildIdx] = child_node_id; + node.SetChildBounds(cur_stack.mChildIdx, bounds); + + if (child_node_id.IsNode()) + { + // Update parent for this node + Node &child_node = mAllocator->Get(child_node_id.GetNodeIndex()); + child_node.mParentNodeIndex = cur_stack.mNodeIdx; + } + else + { + // Set location in tracking + SetBodyLocation(ioTracking, child_node_id.GetBodyID(), cur_stack.mNodeIdx, cur_stack.mChildIdx); + } + + // Encapsulate bounding box in parent + cur_stack.mNodeBoundsMin = Vec3::sMin(cur_stack.mNodeBoundsMin, bounds.mMin); + cur_stack.mNodeBoundsMax = Vec3::sMax(cur_stack.mNodeBoundsMax, bounds.mMax); + } + else if (num_bodies > 1) + { + // Allocate new node + StackEntry &new_stack = stack[++top]; + JPH_ASSERT(top < cStackSize / 4); + uint32 next_depth = cur_stack.mDepth + 1; + new_stack.mNodeIdx = AllocateNode(inMaxDepthMarkChanged > next_depth); + new_stack.mChildIdx = -1; + new_stack.mDepth = next_depth; + new_stack.mNodeBoundsMin = Vec3::sReplicate(cLargeFloat); + new_stack.mNodeBoundsMax = Vec3::sReplicate(-cLargeFloat); + sPartition4(ioNodeIDs, centers, low, high, new_stack.mSplit); + } + } + } + + // Delete temporary data + delete [] centers; + + // Store bounding box of root + outBounds.mMin = stack[0].mNodeBoundsMin; + outBounds.mMax = stack[0].mNodeBoundsMax; + + // Return root + return NodeID::sFromNodeIndex(stack[0].mNodeIdx); +} + +void QuadTree::MarkNodeAndParentsChanged(uint32 inNodeIndex) +{ + uint32 node_idx = inNodeIndex; + + do + { + // If node has changed, parent will be too + Node &node = mAllocator->Get(node_idx); + if (node.mIsChanged) + break; + + // Mark node as changed + node.mIsChanged = true; + + // Get our parent + node_idx = node.mParentNodeIndex; + } + while (node_idx != cInvalidNodeIndex); +} + +void QuadTree::WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds) +{ + uint32 node_idx = inNodeIndex; + + for (;;) + { + // Mark node as changed + Node &node = mAllocator->Get(node_idx); + node.mIsChanged = true; + + // Get our parent + uint32 parent_idx = node.mParentNodeIndex; + if (parent_idx == cInvalidNodeIndex) + break; + + // Find which child of the parent we're in + Node &parent_node = mAllocator->Get(parent_idx); + NodeID node_id = NodeID::sFromNodeIndex(node_idx); + int child_idx = -1; + for (int i = 0; i < 4; ++i) + if (parent_node.mChildNodeID[i] == node_id) + { + // Found one, set the node index and child index and update the bounding box too + child_idx = i; + break; + } + JPH_ASSERT(child_idx != -1, "Nodes don't get removed from the tree, we must have found it"); + + // To avoid any race conditions with other threads we only enlarge bounding boxes + if (!parent_node.EncapsulateChildBounds(child_idx, inNewBounds)) + { + // No changes to bounding box, only marking as changed remains to be done + if (!parent_node.mIsChanged) + MarkNodeAndParentsChanged(parent_idx); + break; + } + + // Update node index + node_idx = parent_idx; + } +} + +bool QuadTree::TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies) +{ + // Tentively assign the node as parent + bool leaf_is_node = inLeafID.IsNode(); + if (leaf_is_node) + { + uint32 leaf_idx = inLeafID.GetNodeIndex(); + mAllocator->Get(leaf_idx).mParentNodeIndex = inNodeIndex; + } + + // Fetch node that we're adding to + Node &node = mAllocator->Get(inNodeIndex); + + // Find an empty child + for (uint32 child_idx = 0; child_idx < 4; ++child_idx) + if (node.mChildNodeID[child_idx].CompareExchange(NodeID::sInvalid(), inLeafID)) // Check if we can claim it + { + // We managed to add it to the node + + // If leaf was a body, we need to update its bookkeeping + if (!leaf_is_node) + SetBodyLocation(ioTracking, inLeafID.GetBodyID(), inNodeIndex, child_idx); + + // Now set the bounding box making the child valid for queries + node.SetChildBounds(child_idx, inLeafBounds); + + // Widen the bounds for our parents too + WidenAndMarkNodeAndParentsChanged(inNodeIndex, inLeafBounds); + + // Update body counter + mNumBodies += inLeafNumBodies; + + // And we're done + return true; + } + + return false; +} + +bool QuadTree::TryCreateNewRoot(TrackingVector &ioTracking, atomic &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies) +{ + // Fetch old root + uint32 root_idx = ioRootNodeIndex; + Node &root = mAllocator->Get(root_idx); + + // Create new root, mark this new root as changed as we're not creating a very efficient tree at this point + uint32 new_root_idx = AllocateNode(true); + Node &new_root = mAllocator->Get(new_root_idx); + + // First child is current root, note that since the tree may be modified concurrently we cannot assume that the bounds of our child will be correct so we set a very large bounding box + new_root.mChildNodeID[0] = NodeID::sFromNodeIndex(root_idx); + new_root.SetChildBounds(0, AABox(Vec3::sReplicate(-cLargeFloat), Vec3::sReplicate(cLargeFloat))); + + // Second child is new leaf + new_root.mChildNodeID[1] = inLeafID; + new_root.SetChildBounds(1, inLeafBounds); + + // Tentatively assign new root as parent + bool leaf_is_node = inLeafID.IsNode(); + if (leaf_is_node) + { + uint32 leaf_idx = inLeafID.GetNodeIndex(); + mAllocator->Get(leaf_idx).mParentNodeIndex = new_root_idx; + } + + // Try to swap it + if (ioRootNodeIndex.compare_exchange_strong(root_idx, new_root_idx)) + { + // We managed to set the new root + + // If leaf was a body, we need to update its bookkeeping + if (!leaf_is_node) + SetBodyLocation(ioTracking, inLeafID.GetBodyID(), new_root_idx, 1); + + // Store parent node for old root + root.mParentNodeIndex = new_root_idx; + + // Update body counter + mNumBodies += inLeafNumBodies; + + // And we're done + return true; + } + + // Failed to swap, someone else must have created a new root, try again + mAllocator->DestructObject(new_root_idx); + return false; +} + +void QuadTree::AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + +#ifdef JPH_ENABLE_ASSERTS + // Below we just cast the body ID's to node ID's, check here that that is valid + for (const BodyID *b = ioBodyIDs, *b_end = ioBodyIDs + inNumber; b < b_end; ++b) + NodeID::sFromBodyID(*b); +#endif + + // Build subtree for the new bodies, note that we mark all nodes as 'not changed' + // so they will stay together as a batch and will make the tree rebuild cheaper + outState.mLeafID = BuildTree(inBodies, ioTracking, (NodeID *)ioBodyIDs, inNumber, 0, outState.mLeafBounds); + +#ifdef JPH_DEBUG + if (outState.mLeafID.IsNode()) + ValidateTree(inBodies, ioTracking, outState.mLeafID.GetNodeIndex(), inNumber); +#endif +} + +void QuadTree::AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState) +{ + // Assert sane input + JPH_ASSERT(inNumberBodies > 0); + + // Mark tree dirty + mIsDirty = true; + + // Get the current root node + RootNode &root_node = GetCurrentRoot(); + + for (;;) + { + // Check if we can insert the body in the root + if (TryInsertLeaf(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies)) + return; + + // Check if we can create a new root + if (TryCreateNewRoot(ioTracking, root_node.mIndex, inState.mLeafID, inState.mLeafBounds, inNumberBodies)) + return; + } +} + +void QuadTree::AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState) +{ + // Collect all bodies + Allocator::Batch free_batch; + NodeID node_stack[cStackSize]; + node_stack[0] = inState.mLeafID; + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Reset location of body + sInvalidateBodyLocation(ioTracking, child_node_id.GetBodyID()); + } + else + { + // Process normal node + uint32 node_idx = child_node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + for (NodeID sub_child_node_id : node.mChildNodeID) + if (sub_child_node_id.IsValid()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = sub_child_node_id; + top++; + } + + // Mark it to be freed + mAllocator->AddObjectToBatch(free_batch, node_idx); + } + --top; + } + while (top >= 0); + + // Now free all nodes as a single batch + mAllocator->DestructObjectBatch(free_batch); +} + +void QuadTree::RemoveBodies([[maybe_unused]] const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + + // Mark tree dirty + mIsDirty = true; + + for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur) + { + // Check if BodyID is correct + JPH_ASSERT(inBodies[cur->GetIndex()]->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager"); + + // Get location of body + uint32 node_idx, child_idx; + GetBodyLocation(ioTracking, *cur, node_idx, child_idx); + + // First we reset our internal bookkeeping + sInvalidateBodyLocation(ioTracking, *cur); + + // Then we make the bounding box invalid, no queries can find this node anymore + Node &node = mAllocator->Get(node_idx); + node.InvalidateChildBounds(child_idx); + + // Finally we reset the child id, this makes the node available for adds again + node.mChildNodeID[child_idx] = NodeID::sInvalid(); + + // We don't need to bubble up our bounding box changes to our parents since we never make volumes smaller, only bigger + // But we do need to mark the nodes as changed so that the tree can be rebuilt + MarkNodeAndParentsChanged(node_idx); + } + + mNumBodies -= inNumber; +} + +void QuadTree::NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber) +{ + // Assert sane input + JPH_ASSERT(ioBodyIDs != nullptr); + JPH_ASSERT(inNumber > 0); + + for (const BodyID *cur = ioBodyIDs, *end = ioBodyIDs + inNumber; cur < end; ++cur) + { + // Check if BodyID is correct + const Body *body = inBodies[cur->GetIndex()]; + JPH_ASSERT(body->GetID() == *cur, "Provided BodyID doesn't match BodyID in body manager"); + + // Get the new bounding box + const AABox &new_bounds = body->GetWorldSpaceBounds(); + + // Get location of body + uint32 node_idx, child_idx; + GetBodyLocation(inTracking, *cur, node_idx, child_idx); + + // Widen bounds for node + Node &node = mAllocator->Get(node_idx); + if (node.EncapsulateChildBounds(child_idx, new_bounds)) + { + // Mark tree dirty + mIsDirty = true; + + // If bounds changed, widen the bounds for our parents too + WidenAndMarkNodeAndParentsChanged(node_idx, new_bounds); + } + } +} + +template +JPH_INLINE void QuadTree::WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const +{ + // Get the root + const RootNode &root_node = GetCurrentRoot(); + +#ifdef JPH_TRACK_BROADPHASE_STATS + // Start tracking stats + int bodies_visited = 0; + int hits_collected = 0; + int nodes_visited = 0; + uint64 collector_ticks = 0; + + uint64 start = GetProcessorTickCount(); +#endif // JPH_TRACK_BROADPHASE_STATS + + NodeID node_stack[cStackSize]; + node_stack[0] = root_node.GetNodeID(); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Track amount of bodies visited + JPH_IF_TRACK_BROADPHASE_STATS(++bodies_visited;) + + BodyID body_id = child_node_id.GetBodyID(); + ObjectLayer object_layer = inTracking[body_id.GetIndex()].mObjectLayer; // We're not taking a lock on the body, so it may be in the process of being removed so check if the object layer is invalid + if (object_layer != cObjectLayerInvalid && inObjectLayerFilter.ShouldCollide(object_layer)) + { + JPH_PROFILE("VisitBody"); + + // Track amount of hits + JPH_IF_TRACK_BROADPHASE_STATS(++hits_collected;) + + // Start track time the collector takes + JPH_IF_TRACK_BROADPHASE_STATS(uint64 collector_start = GetProcessorTickCount();) + + // We found a body we collide with, call our visitor + ioVisitor.VisitBody(body_id, top); + + // End track time the collector takes + JPH_IF_TRACK_BROADPHASE_STATS(collector_ticks += GetProcessorTickCount() - collector_start;) + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + } + } + else if (child_node_id.IsValid()) + { + JPH_IF_TRACK_BROADPHASE_STATS(++nodes_visited;) + + // Check if stack can hold more nodes + if (top + 4 < cStackSize) + { + // Process normal node + const Node &node = mAllocator->Get(child_node_id.GetNodeIndex()); + JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE)); + + // Load bounds of 4 children + Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX); + Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY); + Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ); + Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX); + Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY); + Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ); + + // Load ids for 4 children + UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, child_ids, top); + child_ids.StoreInt4((uint32 *)&node_stack[top]); + top += num_results; + } + else + JPH_ASSERT(false, "Stack full!"); + } + + // Fetch next node until we find one that the visitor wants to see + do + --top; + while (top >= 0 && !ioVisitor.ShouldVisitNode(top)); + } + while (top >= 0); + +#ifdef JPH_TRACK_BROADPHASE_STATS + // Calculate total time the broadphase walk took + uint64 total_ticks = GetProcessorTickCount() - start; + + // Update stats under lock protection (slow!) + { + unique_lock lock(mStatsMutex); + Stat &s = ioStats[inObjectLayerFilter.GetDescription()]; + s.mNumQueries++; + s.mNodesVisited += nodes_visited; + s.mBodiesVisited += bodies_visited; + s.mHitsReported += hits_collected; + s.mTotalTicks += total_ticks; + s.mCollectorTicks += collector_ticks; + } +#endif // JPH_TRACK_BROADPHASE_STATS +} + +void QuadTree::CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const RayCast &inRay, RayCastBodyCollector &ioCollector) : + mOrigin(inRay.mOrigin), + mInvDirection(inRay.mDirection), + mCollector(ioCollector) + { + mFractionStack[0] = -1; + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mFractionStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) + { + // Test the ray against 4 bounding boxes + Vec4 fraction = RayAABox4(mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(fraction, mCollector.GetEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] }; + mCollector.AddHit(result); + } + + private: + Vec3 mOrigin; + RayInvDirection mInvDirection; + RayCastBodyCollector & mCollector; + float mFractionStack[cStackSize]; + }; + + Visitor visitor(inRay, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastRayStats)); +} + +void QuadTree::CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const AABox &inBox, CollideShapeBodyCollector &ioCollector) : + mBox(inBox), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test the box vs 4 boxes + UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + const AABox & mBox; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideAABoxStats)); +} + +void QuadTree::CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector) : + mCenterX(inCenter.SplatX()), + mCenterY(inCenter.SplatY()), + mCenterZ(inCenter.SplatZ()), + mRadiusSq(Vec4::sReplicate(Square(inRadius))), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test 4 boxes vs sphere + UVec4 hitting = AABox4VsSphere(mCenterX, mCenterY, mCenterZ, mRadiusSq, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + Vec4 mCenterX; + Vec4 mCenterY; + Vec4 mCenterZ; + Vec4 mRadiusSq; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inCenter, inRadius, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideSphereStats)); +} + +void QuadTree::CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector) : + mPoint(inPoint), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test if point overlaps with box + UVec4 hitting = AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + Vec3 mPoint; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inPoint, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollidePointStats)); +} + +void QuadTree::CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector) : + mBox(inBox), + mCollector(ioCollector) + { + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) const + { + // Test if point overlaps with box + UVec4 hitting = AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(hitting, ioChildNodeIDs); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + mCollector.AddHit(inBodyID); + } + + private: + OrientedBox mBox; + CollideShapeBodyCollector & mCollector; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCollideOrientedBoxStats)); +} + +void QuadTree::CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const +{ + class Visitor + { + public: + /// Constructor + JPH_INLINE Visitor(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector) : + mOrigin(inBox.mBox.GetCenter()), + mExtent(inBox.mBox.GetExtent()), + mInvDirection(inBox.mDirection), + mCollector(ioCollector) + { + mFractionStack[0] = -1; + } + + /// Returns true if further processing of the tree should be aborted + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Returns true if this node / body should be visited, false if no hit can be generated + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mFractionStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + /// Visit nodes, returns number of hits found and sorts ioChildNodeIDs so that they are at the beginning of the vector. + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioChildNodeIDs, int inStackTop) + { + // Enlarge them by the casted aabox extents + Vec4 bounds_min_x = inBoundsMinX, bounds_min_y = inBoundsMinY, bounds_min_z = inBoundsMinZ, bounds_max_x = inBoundsMaxX, bounds_max_y = inBoundsMaxY, bounds_max_z = inBoundsMaxZ; + AABox4EnlargeWithExtent(mExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test 4 children + Vec4 fraction = RayAABox4(mOrigin, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(fraction, mCollector.GetPositiveEarlyOutFraction(), ioChildNodeIDs, &mFractionStack[inStackTop]); + } + + /// Visit a body, returns false if the algorithm should terminate because no hits can be generated anymore + JPH_INLINE void VisitBody(const BodyID &inBodyID, int inStackTop) + { + // Store potential hit with body + BroadPhaseCastResult result { inBodyID, mFractionStack[inStackTop] }; + mCollector.AddHit(result); + } + + private: + Vec3 mOrigin; + Vec3 mExtent; + RayInvDirection mInvDirection; + CastShapeBodyCollector & mCollector; + float mFractionStack[cStackSize]; + }; + + Visitor visitor(inBox, ioCollector); + WalkTree(inObjectLayerFilter, inTracking, visitor JPH_IF_TRACK_BROADPHASE_STATS(, mCastAABoxStats)); +} + +void QuadTree::FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const +{ + // Note that we don't lock the tree at this point. We know that the tree is not going to be swapped or deleted while finding collision pairs due to the way the jobs are scheduled in the PhysicsSystem::Update. + // We double check this at the end of the function. + const RootNode &root_node = GetCurrentRoot(); + JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex); + + // Assert sane input + JPH_ASSERT(inActiveBodies != nullptr); + JPH_ASSERT(inNumActiveBodies > 0); + + NodeID node_stack[cStackSize]; + + // Loop over all active bodies + for (int b1 = 0; b1 < inNumActiveBodies; ++b1) + { + BodyID b1_id = inActiveBodies[b1]; + const Body &body1 = *inBodies[b1_id.GetIndex()]; + JPH_ASSERT(!body1.IsStatic()); + + // Expand the bounding box by the speculative contact distance + AABox bounds1 = body1.GetWorldSpaceBounds(); + bounds1.ExpandBy(Vec3::sReplicate(inSpeculativeContactDistance)); + + // Test each body with the tree + node_stack[0] = root_node.GetNodeID(); + int top = 0; + do + { + // Check if node is a body + NodeID child_node_id = node_stack[top]; + if (child_node_id.IsBody()) + { + // Don't collide with self + BodyID b2_id = child_node_id.GetBodyID(); + if (b1_id != b2_id) + { + // Collision between dynamic pairs need to be picked up only once + const Body &body2 = *inBodies[b2_id.GetIndex()]; + if (inObjectLayerPairFilter.ShouldCollide(body1.GetObjectLayer(), body2.GetObjectLayer()) + && Body::sFindCollidingPairsCanCollide(body1, body2) + && bounds1.Overlaps(body2.GetWorldSpaceBounds())) // In the broadphase we widen the bounding box when a body moves, do a final check to see if the bounding boxes actually overlap + { + // Store potential hit between bodies + ioPairCollector.AddHit({ b1_id, b2_id }); + } + } + } + else if (child_node_id.IsValid()) + { + // Process normal node + const Node &node = mAllocator->Get(child_node_id.GetNodeIndex()); + JPH_ASSERT(IsAligned(&node, JPH_CACHE_LINE_SIZE)); + + // Get bounds of 4 children + Vec4 bounds_minx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinX); + Vec4 bounds_miny = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinY); + Vec4 bounds_minz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMinZ); + Vec4 bounds_maxx = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxX); + Vec4 bounds_maxy = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxY); + Vec4 bounds_maxz = Vec4::sLoadFloat4Aligned((const Float4 *)&node.mBoundsMaxZ); + + // Test overlap + UVec4 overlap = AABox4VsBox(bounds1, bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz); + int num_results = overlap.CountTrues(); + if (num_results > 0) + { + // Load ids for 4 children + UVec4 child_ids = UVec4::sLoadInt4Aligned((const uint32 *)&node.mChildNodeID[0]); + + // Sort so that overlaps are first + child_ids = UVec4::sSort4True(overlap, child_ids); + + // Push them onto the stack + if (top + 4 < cStackSize) + { + child_ids.StoreInt4((uint32 *)&node_stack[top]); + top += num_results; + } + else + JPH_ASSERT(false, "Stack full!"); + } + } + --top; + } + while (top >= 0); + } + + // Test that the root node was not swapped while finding collision pairs. + // This would mean that UpdateFinalize/DiscardOldTree ran during collision detection which should not be possible due to the way the jobs are scheduled. + JPH_ASSERT(root_node.mIndex != cInvalidNodeIndex); + JPH_ASSERT(&root_node == &GetCurrentRoot()); +} + +#ifdef JPH_DEBUG + +void QuadTree::ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const +{ + JPH_PROFILE_FUNCTION(); + + // Root should be valid + JPH_ASSERT(inNodeIndex != cInvalidNodeIndex); + + // To avoid call overhead, create a stack in place + struct StackEntry + { + uint32 mNodeIndex; + uint32 mParentNodeIndex; + }; + StackEntry stack[cStackSize]; + stack[0].mNodeIndex = inNodeIndex; + stack[0].mParentNodeIndex = cInvalidNodeIndex; + int top = 0; + + uint32 num_bodies = 0; + + do + { + // Copy entry from the stack + StackEntry cur_stack = stack[top]; + + // Validate parent + const Node &node = mAllocator->Get(cur_stack.mNodeIndex); + JPH_ASSERT(node.mParentNodeIndex == cur_stack.mParentNodeIndex); + + // Validate that when a parent is not-changed that all of its children are also + JPH_ASSERT(cur_stack.mParentNodeIndex == cInvalidNodeIndex || mAllocator->Get(cur_stack.mParentNodeIndex).mIsChanged || !node.mIsChanged); + + // Loop children + for (uint32 i = 0; i < 4; ++i) + { + NodeID child_node_id = node.mChildNodeID[i]; + if (child_node_id.IsValid()) + { + if (child_node_id.IsNode()) + { + // Child is a node, recurse + uint32 child_idx = child_node_id.GetNodeIndex(); + JPH_ASSERT(top < cStackSize); + StackEntry &new_entry = stack[top++]; + new_entry.mNodeIndex = child_idx; + new_entry.mParentNodeIndex = cur_stack.mNodeIndex; + + // Validate that the bounding box is bigger or equal to the bounds in the tree + // Bounding box could also be invalid if all children of our child were removed + AABox child_bounds; + node.GetChildBounds(i, child_bounds); + AABox real_child_bounds; + mAllocator->Get(child_idx).GetNodeBounds(real_child_bounds); + JPH_ASSERT(child_bounds.Contains(real_child_bounds) || !real_child_bounds.IsValid()); + } + else + { + // Increment number of bodies found + ++num_bodies; + + // Check if tracker matches position of body + uint32 node_idx, child_idx; + GetBodyLocation(inTracking, child_node_id.GetBodyID(), node_idx, child_idx); + JPH_ASSERT(node_idx == cur_stack.mNodeIndex); + JPH_ASSERT(child_idx == i); + + // Validate that the body bounds are bigger or equal to the bounds in the tree + AABox body_bounds; + node.GetChildBounds(i, body_bounds); + const Body *body = inBodies[child_node_id.GetBodyID().GetIndex()]; + AABox cached_body_bounds = body->GetWorldSpaceBounds(); + AABox real_body_bounds = body->GetShape()->GetWorldSpaceBounds(body->GetCenterOfMassTransform(), Vec3::sReplicate(1.0f)); + JPH_ASSERT(cached_body_bounds == real_body_bounds); // Check that cached body bounds are up to date + JPH_ASSERT(body_bounds.Contains(real_body_bounds)); + } + } + } + --top; + } + while (top >= 0); + + // Check that the amount of bodies in the tree matches our counter + JPH_ASSERT(num_bodies == inNumExpectedBodies); +} + +#endif + +#ifdef JPH_DUMP_BROADPHASE_TREE + +void QuadTree::DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const +{ + // Open DOT file + std::ofstream f; + f.open(StringFormat("%s.dot", inFileNamePrefix).c_str(), std::ofstream::out | std::ofstream::trunc); + if (!f.is_open()) + return; + + // Write header + f << "digraph {\n"; + + // Iterate the entire tree + NodeID node_stack[cStackSize]; + node_stack[0] = inRoot; + JPH_ASSERT(node_stack[0].IsValid()); + int top = 0; + do + { + // Check if node is a body + NodeID node_id = node_stack[top]; + if (node_id.IsBody()) + { + // Output body + String body_id = ConvertToString(node_id.GetBodyID().GetIndex()); + f << "body" << body_id << "[label = \"Body " << body_id << "\"]\n"; + } + else + { + // Process normal node + uint32 node_idx = node_id.GetNodeIndex(); + const Node &node = mAllocator->Get(node_idx); + + // Get bounding box + AABox bounds; + node.GetNodeBounds(bounds); + + // Output node + String node_str = ConvertToString(node_idx); + f << "node" << node_str << "[label = \"Node " << node_str << "\nVolume: " << ConvertToString(bounds.GetVolume()) << "\" color=" << (node.mIsChanged? "red" : "black") << "]\n"; + + // Recurse and get all children + for (NodeID child_node_id : node.mChildNodeID) + if (child_node_id.IsValid()) + { + JPH_ASSERT(top < cStackSize); + node_stack[top] = child_node_id; + top++; + + // Output link + f << "node" << node_str << " -> "; + if (child_node_id.IsBody()) + f << "body" << ConvertToString(child_node_id.GetBodyID().GetIndex()); + else + f << "node" << ConvertToString(child_node_id.GetNodeIndex()); + f << "\n"; + } + } + --top; + } + while (top >= 0); + + // Finish DOT file + f << "}\n"; + f.close(); + + // Convert to svg file + String cmd = StringFormat("dot %s.dot -Tsvg -o %s.svg", inFileNamePrefix, inFileNamePrefix); + system(cmd.c_str()); +} + +#endif // JPH_DUMP_BROADPHASE_TREE + +#ifdef JPH_TRACK_BROADPHASE_STATS + +uint64 QuadTree::GetTicks100Pct(const LayerToStats &inLayer) const +{ + uint64 total_ticks = 0; + for (const LayerToStats::value_type &kv : inLayer) + total_ticks += kv.second.mTotalTicks; + return total_ticks; +} + +void QuadTree::ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const +{ + for (const LayerToStats::value_type &kv : inLayer) + { + double total_pct = 100.0 * double(kv.second.mTotalTicks) / double(inTicks100Pct); + double total_pct_excl_collector = 100.0 * double(kv.second.mTotalTicks - kv.second.mCollectorTicks) / double(inTicks100Pct); + double hits_reported_vs_bodies_visited = kv.second.mBodiesVisited > 0? 100.0 * double(kv.second.mHitsReported) / double(kv.second.mBodiesVisited) : 100.0; + double hits_reported_vs_nodes_visited = kv.second.mNodesVisited > 0? double(kv.second.mHitsReported) / double(kv.second.mNodesVisited) : -1.0; + + std::stringstream str; + str << inName << ", " << kv.first << ", " << mName << ", " << kv.second.mNumQueries << ", " << total_pct << ", " << total_pct_excl_collector << ", " << kv.second.mNodesVisited << ", " << kv.second.mBodiesVisited << ", " << kv.second.mHitsReported << ", " << hits_reported_vs_bodies_visited << ", " << hits_reported_vs_nodes_visited; + Trace(str.str().c_str()); + } +} + +uint64 QuadTree::GetTicks100Pct() const +{ + uint64 total_ticks = 0; + total_ticks += GetTicks100Pct(mCastRayStats); + total_ticks += GetTicks100Pct(mCollideAABoxStats); + total_ticks += GetTicks100Pct(mCollideSphereStats); + total_ticks += GetTicks100Pct(mCollidePointStats); + total_ticks += GetTicks100Pct(mCollideOrientedBoxStats); + total_ticks += GetTicks100Pct(mCastAABoxStats); + return total_ticks; +} + +void QuadTree::ReportStats(uint64 inTicks100Pct) const +{ + unique_lock lock(mStatsMutex); + ReportStats("RayCast", mCastRayStats, inTicks100Pct); + ReportStats("CollideAABox", mCollideAABoxStats, inTicks100Pct); + ReportStats("CollideSphere", mCollideSphereStats, inTicks100Pct); + ReportStats("CollidePoint", mCollidePointStats, inTicks100Pct); + ReportStats("CollideOrientedBox", mCollideOrientedBoxStats, inTicks100Pct); + ReportStats("CastAABox", mCastAABoxStats, inTicks100Pct); +} + +#endif // JPH_TRACK_BROADPHASE_STATS + +uint QuadTree::GetMaxTreeDepth(const NodeID &inNodeID) const +{ + // Reached a leaf? + if (!inNodeID.IsValid() || inNodeID.IsBody()) + return 0; + + // Recurse to children + uint max_depth = 0; + const Node &node = mAllocator->Get(inNodeID.GetNodeIndex()); + for (NodeID child_node_id : node.mChildNodeID) + max_depth = max(max_depth, GetMaxTreeDepth(child_node_id)); + return max_depth + 1; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.h b/WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.h new file mode 100644 index 000000000..94d36bc3b --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/BroadPhase/QuadTree.h @@ -0,0 +1,390 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +//#define JPH_DUMP_BROADPHASE_TREE + +JPH_NAMESPACE_BEGIN + +/// Internal tree structure in broadphase, is essentially a quad AABB tree. +/// Tree is lockless (except for UpdatePrepare/Finalize() function), modifying objects in the tree will widen the aabbs of parent nodes to make the node fit. +/// During the UpdatePrepare/Finalize() call the tree is rebuilt to achieve a tight fit again. +class JPH_EXPORT QuadTree : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + +private: + // Forward declare + class AtomicNodeID; + + /// Class that points to either a body or a node in the tree + class NodeID + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor does not initialize + inline NodeID() = default; + + /// Construct a node ID + static inline NodeID sInvalid() { return NodeID(cInvalidNodeIndex); } + static inline NodeID sFromBodyID(BodyID inID) { NodeID node_id(inID.GetIndexAndSequenceNumber()); JPH_ASSERT(node_id.IsBody()); return node_id; } + static inline NodeID sFromNodeIndex(uint32 inIdx) { NodeID node_id(inIdx | cIsNode); JPH_ASSERT(node_id.IsNode()); return node_id; } + + /// Check what type of ID it is + inline bool IsValid() const { return mID != cInvalidNodeIndex; } + inline bool IsBody() const { return (mID & cIsNode) == 0; } + inline bool IsNode() const { return (mID & cIsNode) != 0; } + + /// Get body or node index + inline BodyID GetBodyID() const { JPH_ASSERT(IsBody()); return BodyID(mID); } + inline uint32 GetNodeIndex() const { JPH_ASSERT(IsNode()); return mID & ~cIsNode; } + + /// Comparison + inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); } + inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; } + + private: + friend class AtomicNodeID; + + inline explicit NodeID(uint32 inID) : mID(inID) { } + + static const uint32 cIsNode = BodyID::cBroadPhaseBit; ///< If this bit is set it means that the ID refers to a node, otherwise it refers to a body + + uint32 mID; + }; + + static_assert(sizeof(NodeID) == sizeof(BodyID), "Body id's should have the same size as NodeIDs"); + + /// A NodeID that uses atomics to store the value + class AtomicNodeID + { + public: + /// Constructor + AtomicNodeID() = default; + explicit AtomicNodeID(const NodeID &inRHS) : mID(inRHS.mID) { } + + /// Assignment + inline void operator = (const NodeID &inRHS) { mID = inRHS.mID; } + + /// Getting the value + inline operator NodeID () const { return NodeID(mID); } + + /// Check if the ID is valid + inline bool IsValid() const { return mID != cInvalidNodeIndex; } + + /// Comparison + inline bool operator == (const BodyID &inRHS) const { return mID == inRHS.GetIndexAndSequenceNumber(); } + inline bool operator == (const NodeID &inRHS) const { return mID == inRHS.mID; } + + /// Atomically compare and swap value. Expects inOld value, replaces with inNew value or returns false + inline bool CompareExchange(NodeID inOld, NodeID inNew) { return mID.compare_exchange_strong(inOld.mID, inNew.mID); } + + private: + atomic mID; + }; + + /// Class that represents a node in the tree + class Node + { + public: + /// Construct node + explicit Node(bool inIsChanged); + + /// Get bounding box encapsulating all children + void GetNodeBounds(AABox &outBounds) const; + + /// Get bounding box in a consistent way with the functions below (check outBounds.IsValid() before using the box) + void GetChildBounds(int inChildIndex, AABox &outBounds) const; + + /// Set the bounds in such a way that other threads will either see a fully correct bounding box or a bounding box with no volume + void SetChildBounds(int inChildIndex, const AABox &inBounds); + + /// Invalidate bounding box in such a way that other threads will not temporarily see a very large bounding box + void InvalidateChildBounds(int inChildIndex); + + /// Encapsulate inBounds in node bounds, returns true if there were changes + bool EncapsulateChildBounds(int inChildIndex, const AABox &inBounds); + + /// Bounding box for child nodes or bodies (all initially set to invalid so no collision test will ever traverse to the leaf) + atomic mBoundsMinX[4]; + atomic mBoundsMinY[4]; + atomic mBoundsMinZ[4]; + atomic mBoundsMaxX[4]; + atomic mBoundsMaxY[4]; + atomic mBoundsMaxZ[4]; + + /// Index of child node or body ID. + AtomicNodeID mChildNodeID[4]; + + /// Index of the parent node. + /// Note: This value is unreliable during the UpdatePrepare/Finalize() function as a node may be relinked to the newly built tree. + atomic mParentNodeIndex = cInvalidNodeIndex; + + /// If this part of the tree has changed, if not, we will treat this sub tree as a single body during the UpdatePrepare/Finalize(). + /// If any changes are made to an object inside this sub tree then the direct path from the body to the top of the tree will become changed. + atomic mIsChanged; + + // Padding to align to 124 bytes + uint32 mPadding = 0; + }; + + // Maximum size of the stack during tree walk + static constexpr int cStackSize = 128; + + static_assert(sizeof(atomic) == 4, "Assuming that an atomic doesn't add any additional storage"); + static_assert(sizeof(atomic) == 4, "Assuming that an atomic doesn't add any additional storage"); + static_assert(is_trivially_destructible(), "Assuming that we don't have a destructor"); + +public: + /// Class that allocates tree nodes, can be shared between multiple trees + using Allocator = FixedSizeFreeList; + + static_assert(Allocator::ObjectStorageSize == 128, "Node should be 128 bytes"); + + /// Data to track location of a Body in the tree + struct Tracking + { + /// Constructor to satisfy the vector class + Tracking() = default; + Tracking(const Tracking &inRHS) : mBroadPhaseLayer(inRHS.mBroadPhaseLayer.load()), mObjectLayer(inRHS.mObjectLayer.load()), mBodyLocation(inRHS.mBodyLocation.load()) { } + + /// Invalid body location identifier + static const uint32 cInvalidBodyLocation = 0xffffffff; + + atomic mBroadPhaseLayer = (BroadPhaseLayer::Type)cBroadPhaseLayerInvalid; + atomic mObjectLayer = cObjectLayerInvalid; + atomic mBodyLocation { cInvalidBodyLocation }; + }; + + using TrackingVector = Array; + + /// Destructor + ~QuadTree(); + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Name of the tree for debugging purposes + void SetName(const char *inName) { mName = inName; } + inline const char * GetName() const { return mName; } +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED + + /// Check if there is anything in the tree + inline bool HasBodies() const { return mNumBodies != 0; } + + /// Check if the tree needs an UpdatePrepare/Finalize() + inline bool IsDirty() const { return mIsDirty; } + + /// Check if this tree can get an UpdatePrepare/Finalize() or if it needs a DiscardOldTree() first + inline bool CanBeUpdated() const { return mFreeNodeBatch.mNumObjects == 0; } + + /// Initialization + void Init(Allocator &inAllocator); + + struct UpdateState + { + NodeID mRootNodeID; ///< This will be the new root node id + }; + + /// Will throw away the previous frame's nodes so that we can start building a new tree in the background + void DiscardOldTree(); + + /// Get the bounding box for this tree + AABox GetBounds() const; + + /// Update the broadphase, needs to be called regularly to achieve a tight fit of the tree when bodies have been modified. + /// UpdatePrepare() will build the tree, UpdateFinalize() will lock the root of the tree shortly and swap the trees and afterwards clean up temporary data structures. + void UpdatePrepare(const BodyVector &inBodies, TrackingVector &ioTracking, UpdateState &outUpdateState, bool inFullRebuild); + void UpdateFinalize(const BodyVector &inBodies, const TrackingVector &inTracking, const UpdateState &inUpdateState); + + /// Temporary data structure to pass information between AddBodiesPrepare and AddBodiesFinalize/Abort + struct AddState + { + NodeID mLeafID = NodeID::sInvalid(); + AABox mLeafBounds; + }; + + /// Prepare adding inNumber bodies at ioBodyIDs to the quad tree, returns the state in outState that should be used in AddBodiesFinalize. + /// This can be done on a background thread without influencing the broadphase. + /// ioBodyIDs may be shuffled around by this function. + void AddBodiesPrepare(const BodyVector &inBodies, TrackingVector &ioTracking, BodyID *ioBodyIDs, int inNumber, AddState &outState); + + /// Finalize adding bodies to the quadtree, supply the same number of bodies as in AddBodiesPrepare. + void AddBodiesFinalize(TrackingVector &ioTracking, int inNumberBodies, const AddState &inState); + + /// Abort adding bodies to the quadtree, supply the same bodies and state as in AddBodiesPrepare. + /// This can be done on a background thread without influencing the broadphase. + void AddBodiesAbort(TrackingVector &ioTracking, const AddState &inState); + + /// Remove inNumber bodies in ioBodyIDs from the quadtree. + void RemoveBodies(const BodyVector &inBodies, TrackingVector &ioTracking, const BodyID *ioBodyIDs, int inNumber); + + /// Call whenever the aabb of a body changes. + void NotifyBodiesAABBChanged(const BodyVector &inBodies, const TrackingVector &inTracking, const BodyID *ioBodyIDs, int inNumber); + + /// Cast a ray and get the intersecting bodies in ioCollector. + void CastRay(const RayCast &inRay, RayCastBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with inBox in ioCollector + void CollideAABox(const AABox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with a sphere in ioCollector + void CollideSphere(Vec3Arg inCenter, float inRadius, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with a point and any hits to ioCollector + void CollidePoint(Vec3Arg inPoint, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Get bodies intersecting with an oriented box and any hits to ioCollector + void CollideOrientedBox(const OrientedBox &inBox, CollideShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Cast a box and get intersecting bodies in ioCollector + void CastAABox(const AABoxCast &inBox, CastShapeBodyCollector &ioCollector, const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking) const; + + /// Find all colliding pairs between dynamic bodies, calls ioPairCollector for every pair found + void FindCollidingPairs(const BodyVector &inBodies, const BodyID *inActiveBodies, int inNumActiveBodies, float inSpeculativeContactDistance, BodyPairCollector &ioPairCollector, const ObjectLayerPairFilter &inObjectLayerPairFilter) const; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Sum up all the ticks spent in the various layers + uint64 GetTicks100Pct() const; + + /// Trace the stats of this tree to the TTY + void ReportStats(uint64 inTicks100Pct) const; +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + /// Constants + static const uint32 cInvalidNodeIndex = 0xffffffff; ///< Value used to indicate node index is invalid + static const float cLargeFloat; ///< A large floating point number that is small enough to not cause any overflows + static const AABox cInvalidBounds; ///< Invalid bounding box using cLargeFloat + + /// We alternate between two trees in order to let collision queries complete in parallel to adding/removing objects to the tree + struct RootNode + { + /// Get the ID of the root node + inline NodeID GetNodeID() const { return NodeID::sFromNodeIndex(mIndex); } + + /// Index of the root node of the tree (this is always a node, never a body id) + atomic mIndex { cInvalidNodeIndex }; + }; + + /// Caches location of body inBodyID in the tracker, body can be found in mNodes[inNodeIdx].mChildNodeID[inChildIdx] + void GetBodyLocation(const TrackingVector &inTracking, BodyID inBodyID, uint32 &outNodeIdx, uint32 &outChildIdx) const; + void SetBodyLocation(TrackingVector &ioTracking, BodyID inBodyID, uint32 inNodeIdx, uint32 inChildIdx) const; + static void sInvalidateBodyLocation(TrackingVector &ioTracking, BodyID inBodyID); + + /// Get the current root of the tree + JPH_INLINE const RootNode & GetCurrentRoot() const { return mRootNode[mRootNodeIndex]; } + JPH_INLINE RootNode & GetCurrentRoot() { return mRootNode[mRootNodeIndex]; } + + /// Depending on if inNodeID is a body or tree node return the bounding box + inline AABox GetNodeOrBodyBounds(const BodyVector &inBodies, NodeID inNodeID) const; + + /// Mark node and all of its parents as changed + inline void MarkNodeAndParentsChanged(uint32 inNodeIndex); + + /// Widen parent bounds of node inNodeIndex to encapsulate inNewBounds, also mark node and all of its parents as changed + inline void WidenAndMarkNodeAndParentsChanged(uint32 inNodeIndex, const AABox &inNewBounds); + + /// Allocate a new node + inline uint32 AllocateNode(bool inIsChanged); + + /// Try to insert a new leaf to the tree at inNodeIndex + inline bool TryInsertLeaf(TrackingVector &ioTracking, int inNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies); + + /// Try to replace the existing root with a new root that contains both the existing root and the new leaf + inline bool TryCreateNewRoot(TrackingVector &ioTracking, atomic &ioRootNodeIndex, NodeID inLeafID, const AABox &inLeafBounds, int inLeafNumBodies); + + /// Build a tree for ioBodyIDs, returns the NodeID of the root (which will be the ID of a single body if inNumber = 1). All tree levels up to inMaxDepthMarkChanged will be marked as 'changed'. + NodeID BuildTree(const BodyVector &inBodies, TrackingVector &ioTracking, NodeID *ioNodeIDs, int inNumber, uint inMaxDepthMarkChanged, AABox &outBounds); + + /// Sorts ioNodeIDs spatially into 2 groups. Second groups starts at ioNodeIDs + outMidPoint. + /// After the function returns ioNodeIDs and ioNodeCenters will be shuffled + static void sPartition(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inNumber, int &outMidPoint); + + /// Sorts ioNodeIDs from inBegin to (but excluding) inEnd spatially into 4 groups. + /// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1] + /// After the function returns ioNodeIDs and ioNodeCenters will be shuffled + static void sPartition4(NodeID *ioNodeIDs, Vec3 *ioNodeCenters, int inBegin, int inEnd, int *outSplit); + +#ifdef JPH_DEBUG + /// Validate that the tree is consistent. + /// Note: This function only works if the tree is not modified while we're traversing it. + void ValidateTree(const BodyVector &inBodies, const TrackingVector &inTracking, uint32 inNodeIndex, uint32 inNumExpectedBodies) const; +#endif + +#ifdef JPH_DUMP_BROADPHASE_TREE + /// Dump the tree in DOT format (see: https://graphviz.org/) + void DumpTree(const NodeID &inRoot, const char *inFileNamePrefix) const; +#endif + + /// Allocator that controls adding / freeing nodes + Allocator * mAllocator = nullptr; + + /// This is a list of nodes that must be deleted after the trees are swapped and the old tree is no longer in use + Allocator::Batch mFreeNodeBatch; + + /// Number of bodies currently in the tree + /// This is aligned to be in a different cache line from the `Allocator` pointer to prevent cross-thread syncs + /// when reading nodes. + alignas(JPH_CACHE_LINE_SIZE) atomic mNumBodies { 0 }; + + /// We alternate between two tree root nodes. When updating, we activate the new tree and we keep the old tree alive. + /// for queries that are in progress until the next time DiscardOldTree() is called. + RootNode mRootNode[2]; + atomic mRootNodeIndex { 0 }; + + /// Flag to keep track of changes to the broadphase, if false, we don't need to UpdatePrepare/Finalize() + atomic mIsDirty = false; + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Mutex protecting the various LayerToStats members + mutable Mutex mStatsMutex; + + struct Stat + { + uint64 mNumQueries = 0; + uint64 mNodesVisited = 0; + uint64 mBodiesVisited = 0; + uint64 mHitsReported = 0; + uint64 mTotalTicks = 0; + uint64 mCollectorTicks = 0; + }; + + using LayerToStats = UnorderedMap; + + /// Sum up all the ticks in a layer + uint64 GetTicks100Pct(const LayerToStats &inLayer) const; + + /// Trace the stats of a single query type to the TTY + void ReportStats(const char *inName, const LayerToStats &inLayer, uint64 inTicks100Pct) const; + + mutable LayerToStats mCastRayStats; + mutable LayerToStats mCollideAABoxStats; + mutable LayerToStats mCollideSphereStats; + mutable LayerToStats mCollidePointStats; + mutable LayerToStats mCollideOrientedBoxStats; + mutable LayerToStats mCastAABoxStats; +#endif // JPH_TRACK_BROADPHASE_STATS + + /// Debug function to get the depth of the tree from node inNodeID + uint GetMaxTreeDepth(const NodeID &inNodeID) const; + + /// Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitBody for each body encountered + template + JPH_INLINE void WalkTree(const ObjectLayerFilter &inObjectLayerFilter, const TrackingVector &inTracking, Visitor &ioVisitor JPH_IF_TRACK_BROADPHASE_STATS(, LayerToStats &ioStats)) const; + +#if defined(JPH_EXTERNAL_PROFILE) || defined(JPH_PROFILE_ENABLED) + /// Name of this tree for debugging purposes + const char * mName = "Layer"; +#endif // JPH_EXTERNAL_PROFILE || JPH_PROFILE_ENABLED +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.cpp b/WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.cpp new file mode 100644 index 000000000..a69208a04 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.cpp @@ -0,0 +1,109 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CastConvexVsTriangles::CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) : + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mScale(inScale), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mCollector(ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + + // Determine if shape is inside out or not + mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; +} + +void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle + Vec3 v0 = mScale * inV0; + Vec3 v1 = mScale * inV1; + Vec3 v2 = mScale * inV2; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(mShapeCast.mDirection) > 0.0f; + if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Create triangle support function + TriangleConvexSupport triangle { v0, v1, v2 }; + + // Check if we already created the cast shape support function + if (mSupport == nullptr) + { + // Determine if we want to use the actual shape or a shrunken shape with convex radius + ConvexShape::ESupportMode support_mode = mShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default; + + // Create support function + mSupport = static_cast(mShapeCast.mShape)->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale); + } + + EPAPenetrationDepth epa; + float fraction = mCollector.GetEarlyOutFraction(); + Vec3 contact_point_a, contact_point_b, contact_normal; + if (epa.CastShape(mShapeCast.mCenterOfMassStart, mShapeCast.mDirection, mShapeCastSettings.mCollisionTolerance, mShapeCastSettings.mPenetrationTolerance, *mSupport, triangle, mSupport->GetConvexRadius(), 0.0f, mShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal)) + { + // Check if we have enabled active edge detection + if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection); + + // Update the contact normal to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + contact_normal = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, contact_point_b, contact_normal, active_edge_movement_direction); + } + + // Convert to world space + contact_point_a = mCenterOfMassTransform2 * contact_point_a; + contact_point_b = mCenterOfMassTransform2 * contact_point_b; + Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(contact_normal); + + // Its a hit, store the sub shape id's + ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, back_facing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Early out if this hit is deeper than the collector's early out value + if (fraction == 0.0f && -result.mPenetrationDepth >= mCollector.GetEarlyOutFraction()) + return; + + // Gather faces + if (mShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + Mat44 transform_1_to_2 = mShapeCast.mCenterOfMassStart; + transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * mShapeCast.mDirection); + static_cast(mShapeCast.mShape)->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, mCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face); + + // Get face of the triangle + triangle.GetSupportingFace(contact_normal, result.mShape2Face); + + // Convert to world space + for (Vec3 &p : result.mShape2Face) + p = mCenterOfMassTransform2 * p; + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.h b/WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.h new file mode 100644 index 000000000..6a6c161cd --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CastConvexVsTriangles.h @@ -0,0 +1,46 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that casts a convex object vs one or more triangles +class JPH_EXPORT CastConvexVsTriangles +{ +public: + /// Constructor + /// @param inShapeCast The shape to cast against the triangles and its start and direction + /// @param inShapeCastSettings Settings for performing the cast + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param ioCollector The collector that receives the results. + CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector); + + /// Cast convex object with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const ShapeCast & mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + const Mat44 & mCenterOfMassTransform2; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator1; + CastShapeCollector & mCollector; + +private: + ConvexShape::SupportBuffer mSupportBuffer; ///< Buffer that holds the support function of the cast shape + const ConvexShape::Support * mSupport = nullptr; ///< Support function of the cast shape + float mScaleSign; ///< Sign of the scale, -1 if object is inside out, 1 if not +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CastResult.h b/WickedEngine/Jolt/Physics/Collision/CastResult.h new file mode 100644 index 000000000..6bb49feb7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CastResult.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a ray cast or other object cast hit +class BroadPhaseCastResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction. + inline float GetEarlyOutFraction() const { return mFraction; } + + /// Reset this result so it can be reused for a new cast. + inline void Reset() { mBodyID = BodyID(); mFraction = 1.0f + FLT_EPSILON; } + + BodyID mBodyID; ///< Body that was hit + float mFraction = 1.0f + FLT_EPSILON; ///< Hit fraction of the ray/object [0, 1], HitPoint = Start + mFraction * (End - Start) +}; + +/// Specialization of cast result against a shape +class RayCastResult : public BroadPhaseCastResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + SubShapeID mSubShapeID2; ///< Sub shape ID of shape that we collided against +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.cpp b/WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.cpp new file mode 100644 index 000000000..166883be8 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.cpp @@ -0,0 +1,223 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CastSphereVsTriangles::CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) : + mStart(inShapeCast.mCenterOfMassStart.GetTranslation()), + mDirection(inShapeCast.mDirection), + mShapeCastSettings(inShapeCastSettings), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mScale(inScale), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mCollector(ioCollector) +{ + // Cast to sphere shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Sphere); + const SphereShape *sphere = static_cast(inShapeCast.mShape); + + // Scale the radius + mRadius = sphere->GetRadius() * abs(inShapeCast.mScale.GetX()); + + // Determine if shape is inside out or not + mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; +} + +void CastSphereVsTriangles::AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal) +{ + // Convert to world space + Vec3 contact_point_a = mCenterOfMassTransform2 * (mStart + inContactPointA); + Vec3 contact_point_b = mCenterOfMassTransform2 * (mStart + inContactPointB); + Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(inContactNormal); + + // Its a hit, store the sub shape id's + ShapeCastResult result(inFraction, contact_point_a, contact_point_b, contact_normal_world, inBackFacing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Note: We don't gather faces here because that's only useful if both shapes have a face. Since the sphere always has only 1 contact point, the manifold is always a point. + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +void CastSphereVsTriangles::AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal) +{ + // Check if we have enabled active edge detection + Vec3 contact_normal = inContactNormal; + if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection); + + // Update the contact normal to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + contact_normal = ActiveEdges::FixNormal(inV0, inV1, inV2, inBackFacing? inTriangleNormal : -inTriangleNormal, inActiveEdges, inContactPointB, inContactNormal, active_edge_movement_direction); + } + + AddHit(inBackFacing, inSubShapeID2, inFraction, inContactPointA, inContactPointB, contact_normal); +} + +// This is a simplified version of the ray cylinder test from: Real Time Collision Detection - Christer Ericson +// Chapter 5.3.7, page 194-197. Some conditions have been removed as we're not interested in hitting the caps of the cylinder. +// Note that the ray origin is assumed to be the origin here. +float CastSphereVsTriangles::RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const +{ + // Calculate cylinder axis + Vec3 axis = inCylinderB - inCylinderA; + + // Make ray start relative to cylinder side A (moving cylinder A to the origin) + Vec3 start = -inCylinderA; + + // Test if segment is fully on the A side of the cylinder + float start_dot_axis = start.Dot(axis); + float direction_dot_axis = inRayDirection.Dot(axis); + float end_dot_axis = start_dot_axis + direction_dot_axis; + if (start_dot_axis < 0.0f && end_dot_axis < 0.0f) + return FLT_MAX; + + // Test if segment is fully on the B side of the cylinder + float axis_len_sq = axis.LengthSq(); + if (start_dot_axis > axis_len_sq && end_dot_axis > axis_len_sq) + return FLT_MAX; + + // Calculate a, b and c, the factors for quadratic equation + // We're basically solving the ray: x = start + direction * t + // The closest point to x on the segment A B is: w = (x . axis) * axis / (axis . axis) + // The distance between x and w should be radius: (x - w) . (x - w) = radius^2 + // Solving this gives the following: + float a = axis_len_sq * inRayDirection.LengthSq() - Square(direction_dot_axis); + if (abs(a) < 1.0e-6f) + return FLT_MAX; // Segment runs parallel to cylinder axis, stop processing, we will either hit at fraction = 0 or we'll hit a vertex + float b = axis_len_sq * start.Dot(inRayDirection) - direction_dot_axis * start_dot_axis; // should be multiplied by 2, instead we'll divide a and c by 2 when we solve the quadratic equation + float c = axis_len_sq * (start.LengthSq() - Square(inRadius)) - Square(start_dot_axis); + float det = Square(b) - a * c; // normally 4 * a * c but since both a and c need to be divided by 2 we lose the 4 + if (det < 0.0f) + return FLT_MAX; // No solution to quadractic equation + + // Solve fraction t where the ray hits the cylinder + float t = -(b + sqrt(det)) / a; // normally divided by 2 * a but since a should be divided by 2 we lose the 2 + if (t < 0.0f || t > 1.0f) + return FLT_MAX; // Intersection lies outside segment + if (start_dot_axis + t * direction_dot_axis < 0.0f || start_dot_axis + t * direction_dot_axis > axis_len_sq) + return FLT_MAX; // Intersection outside the end point of the cylinder, stop processing, we will possibly hit a vertex + return t; +} + +void CastSphereVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle and make it relative to the start of the cast + Vec3 v0 = mScale * inV0 - mStart; + Vec3 v1 = mScale * inV1 - mStart; + Vec3 v2 = mScale * inV2 - mStart; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0); + float triangle_normal_len = triangle_normal.Length(); + if (triangle_normal_len == 0.0f) + return; // Degenerate triangle + triangle_normal /= triangle_normal_len; + + // Backface check + float normal_dot_direction = triangle_normal.Dot(mDirection); + bool back_facing = normal_dot_direction > 0.0f; + if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Test if distance between the sphere and plane of triangle is smaller or equal than the radius + if (abs(v0.Dot(triangle_normal)) <= mRadius) + { + // Check if the sphere intersects at the start of the cast + uint32 closest_feature; + Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature); + float q_len_sq = q.LengthSq(); + if (q_len_sq <= Square(mRadius)) + { + // Early out if this hit is deeper than the collector's early out value + float q_len = sqrt(q_len_sq); + float penetration_depth = mRadius - q_len; + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Generate contact point + Vec3 contact_normal = q_len > 0.0f? q / q_len : Vec3::sAxisY(); + Vec3 contact_point_a = q + contact_normal * penetration_depth; + Vec3 contact_point_b = q; + AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, 0.0f, contact_point_a, contact_point_b, contact_normal); + return; + } + } + else + { + // Check if cast is not parallel to the plane of the triangle + float abs_normal_dot_direction = abs(normal_dot_direction); + if (abs_normal_dot_direction > 1.0e-6f) + { + // Calculate the point on the sphere that will hit the triangle's plane first and calculate a fraction where it will do so + Vec3 d = Sign(normal_dot_direction) * mRadius * triangle_normal; + float plane_intersection = (v0 - d).Dot(triangle_normal) / normal_dot_direction; + + // Check if sphere will hit in the interval that we're interested in + if (plane_intersection * abs_normal_dot_direction < -mRadius // Sphere hits the plane before the sweep, cannot intersect + || plane_intersection >= mCollector.GetEarlyOutFraction()) // Sphere hits the plane after the sweep / early out fraction, cannot intersect + return; + + // We can only report an interior hit if we're hitting the plane during our sweep and not before + if (plane_intersection >= 0.0f) + { + // Calculate the point of contact on the plane + Vec3 p = d + plane_intersection * mDirection; + + // Check if this is an interior point + float u, v, w; + if (ClosestPoint::GetBaryCentricCoordinates(v0 - p, v1 - p, v2 - p, u, v, w) + && u >= 0.0f && v >= 0.0f && w >= 0.0f) + { + // Interior point, we found the collision point. We don't need to check active edges. + AddHit(back_facing, inSubShapeID2, plane_intersection, p, p, back_facing? triangle_normal : -triangle_normal); + return; + } + } + } + } + + // Test 3 edges + float fraction = RayCylinder(mDirection, v0, v1, mRadius); + fraction = min(fraction, RayCylinder(mDirection, v1, v2, mRadius)); + fraction = min(fraction, RayCylinder(mDirection, v2, v0, mRadius)); + + // Test 3 vertices + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v0, mRadius)); + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v1, mRadius)); + fraction = min(fraction, RaySphere(Vec3::sZero(), mDirection, v2, mRadius)); + + // Check if we have a collision + JPH_ASSERT(fraction >= 0.0f); + if (fraction < mCollector.GetEarlyOutFraction()) + { + // Calculate the center of the sphere at the point of contact + Vec3 p = fraction * mDirection; + + // Get contact point and normal + uint32 closest_feature; + Vec3 q = ClosestPoint::GetClosestPointOnTriangle(v0 - p, v1 - p, v2 - p, closest_feature); + Vec3 contact_normal = q.Normalized(); + Vec3 contact_point_ab = p + q; + AddHitWithActiveEdgeDetection(v0, v1, v2, back_facing, triangle_normal, inActiveEdges, inSubShapeID2, fraction, contact_point_ab, contact_point_ab, contact_normal); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.h b/WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.h new file mode 100644 index 000000000..e37bf6829 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CastSphereVsTriangles.h @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that casts a sphere vs one or more triangles +class JPH_EXPORT CastSphereVsTriangles +{ +public: + /// Constructor + /// @param inShapeCast The sphere to cast against the triangles and its start and direction + /// @param inShapeCastSettings Settings for performing the cast + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param ioCollector The collector that receives the results. + CastSphereVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector); + + /// Cast sphere with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + Vec3 mStart; ///< Starting location of the sphere + Vec3 mDirection; ///< Direction and length of movement of sphere + float mRadius; ///< Scaled radius of sphere + const ShapeCastSettings & mShapeCastSettings; + const Mat44 & mCenterOfMassTransform2; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator1; + CastShapeCollector & mCollector; + +private: + void AddHit(bool inBackFacing, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal); + void AddHitWithActiveEdgeDetection(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, bool inBackFacing, Vec3Arg inTriangleNormal, uint8 inActiveEdges, const SubShapeID &inSubShapeID2, float inFraction, Vec3Arg inContactPointA, Vec3Arg inContactPointB, Vec3Arg inContactNormal); + float RayCylinder(Vec3Arg inRayDirection, Vec3Arg inCylinderA, Vec3Arg inCylinderB, float inRadius) const; + + float mScaleSign; ///< Sign of the scale, -1 if object is inside out, 1 if not +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollectFacesMode.h b/WickedEngine/Jolt/Physics/Collision/CollectFacesMode.h new file mode 100644 index 000000000..4cac6256d --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollectFacesMode.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Whether or not to collect faces, used by CastShape and CollideShape +enum class ECollectFacesMode : uint8 +{ + CollectFaces, ///< mShape1/2Face is desired + NoFaces ///< mShape1/2Face is not desired +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp new file mode 100644 index 000000000..f00b0023a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp @@ -0,0 +1,150 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +CollideConvexVsTriangles::CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mSubShapeID1(inSubShapeID1) +{ + // Get transforms + Mat44 inverse_transform2 = inCenterOfMassTransform2.InversedRotationTranslation(); + Mat44 transform1_to_2 = inverse_transform2 * inCenterOfMassTransform1; + mTransform2To1 = transform1_to_2.InversedRotationTranslation(); + + // Calculate bounds + mBoundsOf1 = inShape1->GetLocalBounds().Scaled(inScale1); + mBoundsOf1.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + mBoundsOf1InSpaceOf2 = mBoundsOf1.Transformed(transform1_to_2); // Convert bounding box of 1 into space of 2 + + // Determine if shape 2 is inside out or not + mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f; +} + +void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle and transform it to the space of 1 + Vec3 v0 = mTransform2To1 * (mScale2 * inV0); + Vec3 v1 = mTransform2To1 * (mScale2 * inV1); + Vec3 v2 = mTransform2To1 * (mScale2 * inV2); + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(v0) > 0.0f; + if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Get bounding box for triangle + AABox triangle_bbox = AABox::sFromTwoPoints(v0, v1); + triangle_bbox.Encapsulate(v2); + + // Get intersection between triangle and shape box, if there is none, we're done + if (!triangle_bbox.Overlaps(mBoundsOf1)) + return; + + // Create triangle support function + TriangleConvexSupport triangle(v0, v1, v2); + + // Perform collision detection + // Note: As we don't remember the penetration axis from the last iteration, and it is likely that the shape (A) we're colliding the triangle (B) against is in front of the triangle, + // and the penetration axis is the shortest distance along to push B out of collision, we use the inverse of the triangle normal as an initial penetration axis. This has been seen + // to improve performance by approx. 5% over using a fixed axis like (1, 0, 0). + Vec3 penetration_axis = -triangle_normal, point1, point2; + EPAPenetrationDepth pen_depth; + EPAPenetrationDepth::EStatus status; + + // Get the support function + if (mShape1ExCvxRadius == nullptr) + mShape1ExCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, mBufferExCvxRadius, mScale1); + + // Perform GJK step + status = pen_depth.GetPenetrationDepthStepGJK(*mShape1ExCvxRadius, mShape1ExCvxRadius->GetConvexRadius() + mCollideShapeSettings.mMaxSeparationDistance, triangle, 0.0f, mCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + + // Check result of collision detection + if (status == EPAPenetrationDepth::EStatus::NotColliding) + return; + else if (status == EPAPenetrationDepth::EStatus::Indeterminate) + { + // Need to run expensive EPA algorithm + + // Get the support function + if (mShape1IncCvxRadius == nullptr) + mShape1IncCvxRadius = mShape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, mBufferIncCvxRadius, mScale1); + + // Add convex radius + AddConvexRadius shape1_add_max_separation_distance(*mShape1IncCvxRadius, mCollideShapeSettings.mMaxSeparationDistance); + + // Perform EPA step + if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, triangle, mCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) + return; + } + + // Check if the penetration is bigger than the early out fraction + float penetration_depth = (point2 - point1).Length() - mCollideShapeSettings.mMaxSeparationDistance; + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Correct point1 for the added separation distance + float penetration_axis_len = penetration_axis.Length(); + if (penetration_axis_len > 0.0f) + point1 -= penetration_axis * (mCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + + // Check if we have enabled active edge detection + if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111) + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mTransform1.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection); + + // Update the penetration axis to account for active edges + // Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away + penetration_axis = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, point2, penetration_axis, active_edge_movement_direction); + } + + // Convert to world space + point1 = mTransform1 * point1; + point2 = mTransform1 * point2; + Vec3 penetration_axis_world = mTransform1.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Gather faces + if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + mShape1->GetSupportingFace(SubShapeID(), -penetration_axis, mScale1, mTransform1, result.mShape1Face); + + // Get face of the triangle + result.mShape2Face.resize(3); + result.mShape2Face[0] = mTransform1 * v0; + result.mShape2Face[1] = mTransform1 * v1; + result.mShape2Face[2] = mTransform1 * v2; + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.h b/WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.h new file mode 100644 index 000000000..8adfa3b41 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollideConvexVsTriangles.h @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Collision detection helper that collides a convex object vs one or more triangles +class JPH_EXPORT CollideConvexVsTriangles +{ +public: + /// Constructor + /// @param inShape1 The convex shape to collide against triangles + /// @param inScale1 Local space scale for the convex object (scales relative to its center of mass) + /// @param inScale2 Local space scale for the triangles + /// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space + /// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space + /// @param inSubShapeID1 Sub shape ID of the convex object + /// @param inCollideShapeSettings Settings for the collide shape query + /// @param ioCollector The collector that will receive the results + CollideConvexVsTriangles(const ConvexShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector); + + /// Collide convex object with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const CollideShapeSettings & mCollideShapeSettings; ///< Settings for this collision operation + CollideShapeCollector & mCollector; ///< The collector that will receive the results + const ConvexShape * mShape1; ///< The shape that we're colliding with + Vec3 mScale1; ///< The scale of the shape (in shape local space) of the shape we're colliding with + Vec3 mScale2; ///< The scale of the shape (in shape local space) of the shape we're colliding against + Mat44 mTransform1; ///< Transform of the shape we're colliding with + Mat44 mTransform2To1; ///< Transform that takes a point in space of the colliding shape to the shape we're colliding with + AABox mBoundsOf1; ///< Bounds of the colliding shape in local space + AABox mBoundsOf1InSpaceOf2; ///< Bounds of the colliding shape in space of shape we're colliding with + SubShapeID mSubShapeID1; ///< Sub shape ID of colliding shape + float mScaleSign2; ///< Sign of the scale of object 2, -1 if object is inside out, 1 if not + ConvexShape::SupportBuffer mBufferExCvxRadius; ///< Buffer that holds the support function data excluding convex radius + ConvexShape::SupportBuffer mBufferIncCvxRadius; ///< Buffer that holds the support function data including convex radius + const ConvexShape::Support * mShape1ExCvxRadius = nullptr; ///< Actual support function object excluding convex radius + const ConvexShape::Support * mShape1IncCvxRadius = nullptr; ///< Actual support function object including convex radius +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollidePointResult.h b/WickedEngine/Jolt/Physics/Collision/CollidePointResult.h new file mode 100644 index 000000000..8601b3c40 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollidePointResult.h @@ -0,0 +1,25 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds the result of colliding a point against a shape +class CollidePointResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For point queries there is no sensible return value. + inline float GetEarlyOutFraction() const { return 0.0f; } + + BodyID mBodyID; ///< Body that was hit + SubShapeID mSubShapeID2; ///< Sub shape ID of shape that we collided against +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollideShape.h b/WickedEngine/Jolt/Physics/Collision/CollideShape.h new file mode 100644 index 000000000..2c5f8f217 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollideShape.h @@ -0,0 +1,105 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that contains all information of two colliding shapes +class CollideShapeResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor + CollideShapeResult() = default; + + /// Constructor + CollideShapeResult(Vec3Arg inContactPointOn1, Vec3Arg inContactPointOn2, Vec3Arg inPenetrationAxis, float inPenetrationDepth, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) : + mContactPointOn1(inContactPointOn1), + mContactPointOn2(inContactPointOn2), + mPenetrationAxis(inPenetrationAxis), + mPenetrationDepth(inPenetrationDepth), + mSubShapeID1(inSubShapeID1), + mSubShapeID2(inSubShapeID2), + mBodyID2(inBodyID2) + { + } + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. We use -penetration depth to get the hit with the biggest penetration depth + inline float GetEarlyOutFraction() const { return -mPenetrationDepth; } + + /// Reverses the hit result, swapping contact point 1 with contact point 2 etc. + inline CollideShapeResult Reversed() const + { + CollideShapeResult result; + result.mContactPointOn2 = mContactPointOn1; + result.mContactPointOn1 = mContactPointOn2; + result.mPenetrationAxis = -mPenetrationAxis; + result.mPenetrationDepth = mPenetrationDepth; + result.mSubShapeID2 = mSubShapeID1; + result.mSubShapeID1 = mSubShapeID2; + result.mBodyID2 = mBodyID2; + result.mShape2Face = mShape1Face; + result.mShape1Face = mShape2Face; + return result; + } + + using Face = StaticArray; + + Vec3 mContactPointOn1; ///< Contact point on the surface of shape 1 (in world space or relative to base offset) + Vec3 mContactPointOn2; ///< Contact point on the surface of shape 2 (in world space or relative to base offset). If the penetration depth is 0, this will be the same as mContactPointOn1. + Vec3 mPenetrationAxis; ///< Direction to move shape 2 out of collision along the shortest path (magnitude is meaningless, in world space). You can use -mPenetrationAxis.Normalized() as contact normal. + float mPenetrationDepth; ///< Penetration depth (move shape 2 by this distance to resolve the collision) + SubShapeID mSubShapeID1; ///< Sub shape ID that identifies the face on shape 1 + SubShapeID mSubShapeID2; ///< Sub shape ID that identifies the face on shape 2 + BodyID mBodyID2; ///< BodyID to which shape 2 belongs to + Face mShape1Face; ///< Colliding face on shape 1 (optional result, in world space or relative to base offset) + Face mShape2Face; ///< Colliding face on shape 2 (optional result, in world space or relative to base offset) +}; + +/// Settings to be passed with a collision query +class CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// How active edges (edges that a moving object should bump into) are handled + EActiveEdgeMode mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive; + + /// If colliding faces should be collected or only the collision point + ECollectFacesMode mCollectFacesMode = ECollectFacesMode::NoFaces; + + /// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter) + float mCollisionTolerance = cDefaultCollisionTolerance; + + /// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless) + float mPenetrationTolerance = cDefaultPenetrationTolerance; + + /// When mActiveEdgeMode is CollideOnlyWithActive a movement direction can be provided. When hitting an inactive edge, the system will select the triangle normal as penetration depth only if it impedes the movement less than with the calculated penetration depth. + Vec3 mActiveEdgeMovementDirection = Vec3::sZero(); +}; + +/// Settings to be passed with a collision query +class CollideShapeSettings : public CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// When > 0 contacts in the vicinity of the query shape can be found. All nearest contacts that are not further away than this distance will be found (unit: meter) + float mMaxSeparationDistance = 0.0f; + + /// How backfacing triangles should be treated + EBackFaceMode mBackFaceMode = EBackFaceMode::IgnoreBackFaces; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h b/WickedEngine/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h new file mode 100644 index 000000000..4385e15cd --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollideSoftBodyVerticesVsTriangles.h @@ -0,0 +1,98 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Collision detection helper that collides soft body vertices vs triangles +class JPH_EXPORT CollideSoftBodyVerticesVsTriangles +{ +public: + CollideSoftBodyVerticesVsTriangles(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) : + mTransform(inCenterOfMassTransform * Mat44::sScale(inScale)), + mInvTransform(mTransform.Inversed()), + mNormalSign(ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f) + { + } + + JPH_INLINE void StartVertex(const SoftBodyVertex &inVertex) + { + mLocalPosition = mInvTransform * inVertex.mPosition; + mClosestDistanceSq = FLT_MAX; + } + + JPH_INLINE void ProcessTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Get the closest point from the vertex to the triangle + uint32 set; + Vec3 closest_point = ClosestPoint::GetClosestPointOnTriangle(inV0 - mLocalPosition, inV1 - mLocalPosition, inV2 - mLocalPosition, set); + float dist_sq = closest_point.LengthSq(); + if (dist_sq < mClosestDistanceSq) + { + mV0 = inV0; + mV1 = inV1; + mV2 = inV2; + mClosestPoint = closest_point; + mClosestDistanceSq = dist_sq; + mSet = set; + } + } + + JPH_INLINE void FinishVertex(SoftBodyVertex &ioVertex, int inCollidingShapeIndex) const + { + if (mClosestDistanceSq < FLT_MAX) + { + // Convert triangle to world space + Vec3 v0 = mTransform * mV0; + Vec3 v1 = mTransform * mV1; + Vec3 v2 = mTransform * mV2; + Vec3 triangle_normal = mNormalSign * (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sAxisY()); + + if (mSet == 0b111) + { + // Closest is interior to the triangle, use plane as collision plane but don't allow more than 0.1 m penetration + // because otherwise a triangle half a level a way will have a huge penetration if it is back facing + float penetration = min(triangle_normal.Dot(v0 - ioVertex.mPosition), 0.1f); + if (penetration > ioVertex.mLargestPenetration) + { + ioVertex.mLargestPenetration = penetration; + ioVertex.mCollisionPlane = Plane::sFromPointAndNormal(v0, triangle_normal); + ioVertex.mCollidingShapeIndex = inCollidingShapeIndex; + } + } + else + { + // Closest point is on an edge or vertex, use closest point as collision plane + Vec3 closest_point = mTransform * (mLocalPosition + mClosestPoint); + Vec3 normal = ioVertex.mPosition - closest_point; + if (normal.Dot(triangle_normal) > 0.0f) // Ignore back facing edges + { + float normal_length = normal.Length(); + float penetration = -normal_length; + if (penetration > ioVertex.mLargestPenetration) + { + ioVertex.mLargestPenetration = penetration; + ioVertex.mCollisionPlane = Plane::sFromPointAndNormal(closest_point, normal_length > 0.0f? normal / normal_length : triangle_normal); + ioVertex.mCollidingShapeIndex = inCollidingShapeIndex; + } + } + } + } + } + + Mat44 mTransform; + Mat44 mInvTransform; + Vec3 mLocalPosition; + Vec3 mV0, mV1, mV2; + Vec3 mClosestPoint; + float mNormalSign; + float mClosestDistanceSq; + uint32 mSet; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp b/WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp new file mode 100644 index 000000000..92ed28a7b --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp @@ -0,0 +1,123 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +static constexpr uint8 sClosestFeatureToActiveEdgesMask[] = { + 0b000, // 0b000: Invalid, guarded by an assert + 0b101, // 0b001: Vertex 1 -> edge 1 or 3 + 0b011, // 0b010: Vertex 2 -> edge 1 or 2 + 0b001, // 0b011: Vertex 1 & 2 -> edge 1 + 0b110, // 0b100: Vertex 3 -> edge 2 or 3 + 0b100, // 0b101: Vertex 1 & 3 -> edge 3 + 0b010, // 0b110: Vertex 2 & 3 -> edge 2 + // 0b111: Vertex 1, 2 & 3 -> interior, guarded by an if +}; + +CollideSphereVsTriangles::CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale2(inScale2), + mTransform2(inCenterOfMassTransform2), + mSubShapeID1(inSubShapeID1) +{ + // Calculate the center of the sphere in the space of 2 + mSphereCenterIn2 = inCenterOfMassTransform2.Multiply3x3Transposed(inCenterOfMassTransform1.GetTranslation() - inCenterOfMassTransform2.GetTranslation()); + + // Determine if shape 2 is inside out or not + mScaleSign2 = ScaleHelpers::IsInsideOut(inScale2)? -1.0f : 1.0f; + + // Check that the sphere is uniformly scaled + JPH_ASSERT(ScaleHelpers::IsUniformScale(inScale1.Abs())); + mRadius = abs(inScale1.GetX()) * inShape1->GetRadius(); + mRadiusPlusMaxSeparationSq = Square(mRadius + inCollideShapeSettings.mMaxSeparationDistance); +} + +void CollideSphereVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2) +{ + JPH_PROFILE_FUNCTION(); + + // Scale triangle and make it relative to the center of the sphere + Vec3 v0 = mScale2 * inV0 - mSphereCenterIn2; + Vec3 v1 = mScale2 * inV1 - mSphereCenterIn2; + Vec3 v2 = mScale2 * inV2 - mSphereCenterIn2; + + // Calculate triangle normal + Vec3 triangle_normal = mScaleSign2 * (v1 - v0).Cross(v2 - v0); + + // Backface check + bool back_facing = triangle_normal.Dot(v0) > 0.0f; + if (mCollideShapeSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && back_facing) + return; + + // Check if we collide with the sphere + uint32 closest_feature; + Vec3 point2 = ClosestPoint::GetClosestPointOnTriangle(v0, v1, v2, closest_feature); + float point2_len_sq = point2.LengthSq(); + if (point2_len_sq > mRadiusPlusMaxSeparationSq) + return; + + // Calculate penetration depth + float penetration_depth = mRadius - sqrt(point2_len_sq); + if (-penetration_depth >= mCollector.GetEarlyOutFraction()) + return; + + // Calculate penetration axis, direction along which to push 2 to move it out of collision (this is always away from the sphere center) + Vec3 penetration_axis = point2.NormalizedOr(Vec3::sAxisY()); + + // Calculate the point on the sphere + Vec3 point1 = mRadius * penetration_axis; + + // Check if we have enabled active edge detection + JPH_ASSERT(closest_feature != 0); + if (mCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive + && closest_feature != 0b111 // For an interior hit we should already have the right normal + && (inActiveEdges & sClosestFeatureToActiveEdgesMask[closest_feature]) == 0) // If we didn't hit an active edge we should take the triangle normal + { + // Convert the active edge velocity hint to local space + Vec3 active_edge_movement_direction = mTransform2.Multiply3x3Transposed(mCollideShapeSettings.mActiveEdgeMovementDirection); + + // See ActiveEdges::FixNormal. If penetration_axis affects the movement less than the triangle normal we keep penetration_axis. + Vec3 new_penetration_axis = back_facing? triangle_normal : -triangle_normal; + if (active_edge_movement_direction.Dot(penetration_axis) * new_penetration_axis.Length() >= active_edge_movement_direction.Dot(new_penetration_axis)) + penetration_axis = new_penetration_axis; + } + + // Convert to world space + point1 = mTransform2 * (mSphereCenterIn2 + point1); + point2 = mTransform2 * (mSphereCenterIn2 + point2); + Vec3 penetration_axis_world = mTransform2.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, mSubShapeID1, inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext())); + + // Gather faces + if (mCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // The sphere doesn't have a supporting face + + // Get face of triangle 2 + result.mShape2Face.resize(3); + result.mShape2Face[0] = mTransform2 * (mSphereCenterIn2 + v0); + result.mShape2Face[1] = mTransform2 * (mSphereCenterIn2 + v1); + result.mShape2Face[2] = mTransform2 * (mSphereCenterIn2 + v2); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + mCollector.AddHit(result); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.h b/WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.h new file mode 100644 index 000000000..5d16d9a88 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollideSphereVsTriangles.h @@ -0,0 +1,50 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Collision detection helper that collides a sphere vs one or more triangles +class JPH_EXPORT CollideSphereVsTriangles +{ +public: + /// Constructor + /// @param inShape1 The sphere to collide against triangles + /// @param inScale1 Local space scale for the sphere (scales relative to its center of mass) + /// @param inScale2 Local space scale for the triangles + /// @param inCenterOfMassTransform1 Transform that takes the center of mass of 1 into world space + /// @param inCenterOfMassTransform2 Transform that takes the center of mass of 2 into world space + /// @param inSubShapeID1 Sub shape ID of the convex object + /// @param inCollideShapeSettings Settings for the collide shape query + /// @param ioCollector The collector that will receive the results + CollideSphereVsTriangles(const SphereShape *inShape1, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeID &inSubShapeID1, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector); + + /// Collide sphere with a single triangle + /// @param inV0 , inV1 , inV2: CCW triangle vertices + /// @param inActiveEdges bit 0 = edge v0..v1 is active, bit 1 = edge v1..v2 is active, bit 2 = edge v2..v0 is active + /// An active edge is an edge that is not connected to another triangle in such a way that it is impossible to collide with the edge + /// @param inSubShapeID2 The sub shape ID for the triangle + void Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2); + +protected: + const CollideShapeSettings & mCollideShapeSettings; ///< Settings for this collision operation + CollideShapeCollector & mCollector; ///< The collector that will receive the results + const SphereShape * mShape1; ///< The shape that we're colliding with + Vec3 mScale2; ///< The scale of the shape (in shape local space) of the shape we're colliding against + Mat44 mTransform2; ///< Transform of the shape we're colliding against + Vec3 mSphereCenterIn2; ///< The center of the sphere in the space of 2 + SubShapeID mSubShapeID1; ///< Sub shape ID of colliding shape + float mScaleSign2; ///< Sign of the scale of object 2, -1 if object is inside out, 1 if not + float mRadius; ///< Radius of the sphere + float mRadiusPlusMaxSeparationSq; ///< (Radius + Max SeparationDistance)^2 +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollisionCollector.h b/WickedEngine/Jolt/Physics/Collision/CollisionCollector.h new file mode 100644 index 000000000..274e8f032 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollisionCollector.h @@ -0,0 +1,102 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Body; +class TransformedShape; + +/// Traits to use for CastRay +class CollisionCollectorTraitsCastRay +{ +public: + /// For rays the early out fraction is the fraction along the line to order hits. + static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON; ///< Furthest hit: Fraction is 1 + epsilon + static constexpr float ShouldEarlyOutFraction = 0.0f; ///< Closest hit: Fraction is 0 +}; + +/// Traits to use for CastShape +class CollisionCollectorTraitsCastShape +{ +public: + /// For rays the early out fraction is the fraction along the line to order hits. + static constexpr float InitialEarlyOutFraction = 1.0f + FLT_EPSILON; ///< Furthest hit: Fraction is 1 + epsilon + static constexpr float ShouldEarlyOutFraction = -FLT_MAX; ///< Deepest hit: Penetration is infinite +}; + +/// Traits to use for CollideShape +class CollisionCollectorTraitsCollideShape +{ +public: + /// For shape collisions we use -penetration depth to order hits. + static constexpr float InitialEarlyOutFraction = FLT_MAX; ///< Most shallow hit: Separation is infinite + static constexpr float ShouldEarlyOutFraction = -FLT_MAX; ///< Deepest hit: Penetration is infinite +}; + +/// Traits to use for CollidePoint +using CollisionCollectorTraitsCollidePoint = CollisionCollectorTraitsCollideShape; + +/// Virtual interface that allows collecting multiple collision results +template +class CollisionCollector +{ +public: + /// Declare ResultType so that derived classes can use it + using ResultType = ResultTypeArg; + + /// Default constructor + CollisionCollector() = default; + + /// Constructor to initialize from another collector + template + explicit CollisionCollector(const CollisionCollector &inRHS) : mEarlyOutFraction(inRHS.GetEarlyOutFraction()), mContext(inRHS.GetContext()) { } + CollisionCollector(const CollisionCollector &inRHS) = default; + + /// Destructor + virtual ~CollisionCollector() = default; + + /// If you want to reuse this collector, call Reset() + virtual void Reset() { mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; } + + /// When running a query through the NarrowPhaseQuery class, this will be called for every body that is potentially colliding. + /// It allows collecting additional information needed by the collision collector implementation from the body under lock protection + /// before AddHit is called (e.g. the user data pointer or the velocity of the body). + virtual void OnBody([[maybe_unused]] const Body &inBody) { /* Collects nothing by default */ } + + /// Set by the collision detection functions to the current TransformedShape that we're colliding against before calling the AddHit function + void SetContext(const TransformedShape *inContext) { mContext = inContext; } + const TransformedShape *GetContext() const { return mContext; } + + /// This function will be called for every hit found, it's up to the application to decide how to store the hit + virtual void AddHit(const ResultType &inResult) = 0; + + /// Update the early out fraction (should be lower than before) + inline void UpdateEarlyOutFraction(float inFraction) { JPH_ASSERT(inFraction <= mEarlyOutFraction); mEarlyOutFraction = inFraction; } + + /// Reset the early out fraction to a specific value + inline void ResetEarlyOutFraction(float inFraction = TraitsType::InitialEarlyOutFraction) { mEarlyOutFraction = inFraction; } + + /// Force the collision detection algorithm to terminate as soon as possible. Call this from the AddHit function when a satisfying hit is found. + inline void ForceEarlyOut() { mEarlyOutFraction = TraitsType::ShouldEarlyOutFraction; } + + /// When true, the collector will no longer accept any additional hits and the collision detection routine should early out as soon as possible + inline bool ShouldEarlyOut() const { return mEarlyOutFraction <= TraitsType::ShouldEarlyOutFraction; } + + /// Get the current early out value + inline float GetEarlyOutFraction() const { return mEarlyOutFraction; } + + /// Get the current early out value but make sure it's bigger than zero, this is used for shape casting as negative values are used for penetration + inline float GetPositiveEarlyOutFraction() const { return max(FLT_MIN, mEarlyOutFraction); } + +private: + /// The early out fraction determines the fraction below which the collector is still accepting a hit (can be used to reduce the amount of work) + float mEarlyOutFraction = TraitsType::InitialEarlyOutFraction; + + /// Set by the collision detection functions to the current TransformedShape of the body that we're colliding against before calling the AddHit function + const TransformedShape *mContext = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollisionCollectorImpl.h b/WickedEngine/Jolt/Physics/Collision/CollisionCollectorImpl.h new file mode 100644 index 000000000..7bdd5da27 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollisionCollectorImpl.h @@ -0,0 +1,134 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Simple implementation that collects all hits and optionally sorts them on distance +template +class AllHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHits.clear(); + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + mHits.push_back(inResult); + } + + /// Order hits on closest first + void Sort() + { + QuickSort(mHits.begin(), mHits.end(), [](const ResultType &inLHS, const ResultType &inRHS) { return inLHS.GetEarlyOutFraction() < inRHS.GetEarlyOutFraction(); }); + } + + /// Check if any hits were collected + inline bool HadHit() const + { + return !mHits.empty(); + } + + Array mHits; +}; + +/// Simple implementation that collects the closest / deepest hit +template +class ClosestHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHadHit = false; + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + float early_out = inResult.GetEarlyOutFraction(); + if (!mHadHit || early_out < mHit.GetEarlyOutFraction()) + { + // Update early out fraction + CollectorType::UpdateEarlyOutFraction(early_out); + + // Store hit + mHit = inResult; + mHadHit = true; + } + } + + /// Check if this collector has had a hit + inline bool HadHit() const + { + return mHadHit; + } + + ResultType mHit; + +private: + bool mHadHit = false; +}; + +/// Simple implementation that collects any hit +template +class AnyHitCollisionCollector : public CollectorType +{ +public: + /// Redeclare ResultType + using ResultType = typename CollectorType::ResultType; + + // See: CollectorType::Reset + virtual void Reset() override + { + CollectorType::Reset(); + + mHadHit = false; + } + + // See: CollectorType::AddHit + virtual void AddHit(const ResultType &inResult) override + { + // Test that the collector is not collecting more hits after forcing an early out + JPH_ASSERT(!mHadHit); + + // Abort any further testing + CollectorType::ForceEarlyOut(); + + // Store hit + mHit = inResult; + mHadHit = true; + } + + /// Check if this collector has had a hit + inline bool HadHit() const + { + return mHadHit; + } + + ResultType mHit; + +private: + bool mHadHit = false; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollisionDispatch.cpp b/WickedEngine/Jolt/Physics/Collision/CollisionDispatch.cpp new file mode 100644 index 000000000..259a9526e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollisionDispatch.cpp @@ -0,0 +1,107 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +CollisionDispatch::CollideShape CollisionDispatch::sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; +CollisionDispatch::CastShape CollisionDispatch::sCastShape[NumSubShapeTypes][NumSubShapeTypes]; + +void CollisionDispatch::sInit() +{ + for (uint i = 0; i < NumSubShapeTypes; ++i) + for (uint j = 0; j < NumSubShapeTypes; ++j) + { + if (sCollideShape[i][j] == nullptr) + sCollideShape[i][j] = [](const Shape *, const Shape *, Vec3Arg, Vec3Arg, Mat44Arg, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, const CollideShapeSettings &, CollideShapeCollector &, const ShapeFilter &) + { + JPH_ASSERT(false, "Unsupported shape pair"); + }; + + if (sCastShape[i][j] == nullptr) + sCastShape[i][j] = [](const ShapeCast &, const ShapeCastSettings &, const Shape *, Vec3Arg, const ShapeFilter &, Mat44Arg, const SubShapeIDCreator &, const SubShapeIDCreator &, CastShapeCollector &) + { + JPH_ASSERT(false, "Unsupported shape pair"); + }; + } +} + +void CollisionDispatch::sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + // A collision collector that flips the collision results + class ReversedCollector : public CollideShapeCollector + { + public: + explicit ReversedCollector(CollideShapeCollector &ioCollector) : + CollideShapeCollector(ioCollector), + mCollector(ioCollector) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // Add the reversed hit + mCollector.AddHit(inResult.Reversed()); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + private: + CollideShapeCollector & mCollector; + }; + + ReversedShapeFilter shape_filter(inShapeFilter); + ReversedCollector collector(ioCollector); + sCollideShapeVsShape(inShape2, inShape1, inScale2, inScale1, inCenterOfMassTransform2, inCenterOfMassTransform1, inSubShapeIDCreator2, inSubShapeIDCreator1, inCollideShapeSettings, collector, shape_filter); +} + +void CollisionDispatch::sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // A collision collector that flips the collision results + class ReversedCollector : public CastShapeCollector + { + public: + explicit ReversedCollector(CastShapeCollector &ioCollector, Vec3Arg inWorldDirection) : + CastShapeCollector(ioCollector), + mCollector(ioCollector), + mWorldDirection(inWorldDirection) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Add the reversed hit + mCollector.AddHit(inResult.Reversed(mWorldDirection)); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + private: + CastShapeCollector & mCollector; + Vec3 mWorldDirection; + }; + + // Reverse the shape cast (shape cast is in local space to shape 2) + Mat44 com_start_inv = inShapeCast.mCenterOfMassStart.InversedRotationTranslation(); + ShapeCast local_shape_cast(inShape, inScale, com_start_inv, -com_start_inv.Multiply3x3(inShapeCast.mDirection)); + + // Calculate the center of mass of shape 1 at start of sweep + Mat44 shape1_com = inCenterOfMassTransform2 * inShapeCast.mCenterOfMassStart; + + // Calculate the world space direction vector of the shape cast + Vec3 world_direction = -inCenterOfMassTransform2.Multiply3x3(inShapeCast.mDirection); + + // Forward the cast + ReversedShapeFilter shape_filter(inShapeFilter); + ReversedCollector collector(ioCollector, world_direction); + sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShapeCast.mShape, inShapeCast.mScale, shape_filter, shape1_com, inSubShapeIDCreator2, inSubShapeIDCreator1, collector); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollisionDispatch.h b/WickedEngine/Jolt/Physics/Collision/CollisionDispatch.h new file mode 100644 index 000000000..6842c8154 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollisionDispatch.h @@ -0,0 +1,97 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Dispatch function, main function to handle collisions between shapes +class JPH_EXPORT CollisionDispatch +{ +public: + /// Collide 2 shapes and pass any collision on to ioCollector + /// @param inShape1 The first shape + /// @param inShape2 The second shape + /// @param inScale1 Local space scale of shape 1 (scales relative to its center of mass) + /// @param inScale2 Local space scale of shape 2 (scales relative to its center of mass) + /// @param inCenterOfMassTransform1 Transform to transform center of mass of shape 1 into world space + /// @param inCenterOfMassTransform2 Transform to transform center of mass of shape 2 into world space + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for shape 1 + /// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for shape 2 + /// @param inCollideShapeSettings Options for the CollideShape test + /// @param ioCollector The collector that receives the results. + /// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes. + static inline void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) + { + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()]);) + + // Only test shape if it passes the shape filter + if (inShapeFilter.ShouldCollide(inShape1, inSubShapeIDCreator1.GetID(), inShape2, inSubShapeIDCreator2.GetID())) + sCollideShape[(int)inShape1->GetSubType()][(int)inShape2->GetSubType()](inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + } + + /// Cast a shape against this shape, passes any hits found to ioCollector. + /// Note: This version takes the shape cast in local space relative to the center of mass of inShape, take a look at sCastShapeVsShapeWorldSpace if you have a shape cast in world space. + /// @param inShapeCastLocal The shape to cast against the other shape and its start and direction. + /// @param inShapeCastSettings Settings for performing the cast + /// @param inShape The shape to cast against. + /// @param inScale Local space scale for the shape to cast against (scales relative to its center of mass). + /// @param inShapeFilter allows selectively disabling collisions between pairs of (sub) shapes. + /// @param inCenterOfMassTransform2 Is the center of mass transform of shape 2 (excluding scale), this is used to provide a transform to the shape cast result so that local hit result quantities can be transformed into world space. + /// @param inSubShapeIDCreator1 Class that tracks the current sub shape ID for the casting shape + /// @param inSubShapeIDCreator2 Class that tracks the current sub shape ID for the shape we're casting against + /// @param ioCollector The collector that receives the results. + static inline void sCastShapeVsShapeLocalSpace(const ShapeCast &inShapeCastLocal, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) + { + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseStat track(NarrowPhaseStat::sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()]);) + + // Only test shape if it passes the shape filter + if (inShapeFilter.ShouldCollide(inShapeCastLocal.mShape, inSubShapeIDCreator1.GetID(), inShape, inSubShapeIDCreator2.GetID())) + sCastShape[(int)inShapeCastLocal.mShape->GetSubType()][(int)inShape->GetSubType()](inShapeCastLocal, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + } + + /// See: sCastShapeVsShapeLocalSpace. + /// The only difference is that the shape cast (inShapeCastWorld) is provided in world space. + /// Note: A shape cast contains the center of mass start of the shape, if you have the world transform of the shape you probably want to construct it using ShapeCast::sFromWorldTransform. + static inline void sCastShapeVsShapeWorldSpace(const ShapeCast &inShapeCastWorld, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) + { + ShapeCast local_shape_cast = inShapeCastWorld.PostTransformed(inCenterOfMassTransform2.InversedRotationTranslation()); + sCastShapeVsShapeLocalSpace(local_shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + } + + /// Function that collides 2 shapes (see sCollideShapeVsShape) + using CollideShape = void (*)(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + + /// Function that casts a shape vs another shape (see sCastShapeVsShapeLocalSpace) + using CastShape = void (*)(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Initialize all collision functions with a function that asserts and returns no collision + static void sInit(); + + /// Register a collide shape function in the collision table + static void sRegisterCollideShape(EShapeSubType inType1, EShapeSubType inType2, CollideShape inFunction) { sCollideShape[(int)inType1][(int)inType2] = inFunction; } + + /// Register a cast shape function in the collision table + static void sRegisterCastShape(EShapeSubType inType1, EShapeSubType inType2, CastShape inFunction) { sCastShape[(int)inType1][(int)inType2] = inFunction; } + + /// An implementation of CollideShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around + static void sReversedCollideShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + + /// An implementation of CastShape that swaps inShape1 and inShape2 and swaps the result back, can be registered if the collision function only exists the other way around + static void sReversedCastShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + +private: + static CollideShape sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; + static CastShape sCastShape[NumSubShapeTypes][NumSubShapeTypes]; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollisionGroup.cpp b/WickedEngine/Jolt/Physics/Collision/CollisionGroup.cpp new file mode 100644 index 000000000..a8c98b6dc --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollisionGroup.cpp @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CollisionGroup) +{ + JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupFilter) + JPH_ADD_ATTRIBUTE(CollisionGroup, mGroupID) + JPH_ADD_ATTRIBUTE(CollisionGroup, mSubGroupID) +} + +void CollisionGroup::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mGroupID); + inStream.Write(mSubGroupID); +} + +void CollisionGroup::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mGroupID); + inStream.Read(mSubGroupID); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/CollisionGroup.h b/WickedEngine/Jolt/Physics/Collision/CollisionGroup.h new file mode 100644 index 000000000..266242ff0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/CollisionGroup.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Two objects collide with each other if: +/// - Both don't have a group filter +/// - The first group filter says that the objects can collide +/// - Or if there's no filter for the first object, the second group filter says the objects can collide +class JPH_EXPORT CollisionGroup +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, CollisionGroup) + + using GroupID = uint32; + using SubGroupID = uint32; + + static const GroupID cInvalidGroup = ~GroupID(0); + static const SubGroupID cInvalidSubGroup = ~SubGroupID(0); + + /// Default constructor + CollisionGroup() = default; + + /// Construct with all properties + CollisionGroup(const GroupFilter *inFilter, GroupID inGroupID, SubGroupID inSubGroupID) : mGroupFilter(inFilter), mGroupID(inGroupID), mSubGroupID(inSubGroupID) { } + + /// Set the collision group filter + inline void SetGroupFilter(const GroupFilter *inFilter) + { + mGroupFilter = inFilter; + } + + /// Get the collision group filter + inline const GroupFilter *GetGroupFilter() const + { + return mGroupFilter; + } + + /// Set the main group id for this object + inline void SetGroupID(GroupID inID) + { + mGroupID = inID; + } + + inline GroupID GetGroupID() const + { + return mGroupID; + } + + /// Add this object to a sub group + inline void SetSubGroupID(SubGroupID inID) + { + mSubGroupID = inID; + } + + inline SubGroupID GetSubGroupID() const + { + return mSubGroupID; + } + + /// Check if this object collides with another object + bool CanCollide(const CollisionGroup &inOther) const + { + // Call the CanCollide function of the first group filter that's not null + if (mGroupFilter != nullptr) + return mGroupFilter->CanCollide(*this, inOther); + else if (inOther.mGroupFilter != nullptr) + return inOther.mGroupFilter->CanCollide(inOther, *this); + else + return true; + } + + /// Saves the state of this object in binary form to inStream. Does not save group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Does not save group filter. + void RestoreBinaryState(StreamIn &inStream); + +private: + RefConst mGroupFilter; + GroupID mGroupID = cInvalidGroup; + SubGroupID mSubGroupID = cInvalidSubGroup; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ContactListener.h b/WickedEngine/Jolt/Physics/Collision/ContactListener.h new file mode 100644 index 000000000..d347dc475 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ContactListener.h @@ -0,0 +1,114 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class CollideShapeResult; + +/// Array of contact points +using ContactPoints = StaticArray; + +/// Manifold class, describes the contact surface between two bodies +class ContactManifold +{ +public: + /// Swaps shape 1 and 2 + ContactManifold SwapShapes() const { return { mBaseOffset, -mWorldSpaceNormal, mPenetrationDepth, mSubShapeID2, mSubShapeID1, mRelativeContactPointsOn2, mRelativeContactPointsOn1 }; } + + /// Access to the world space contact positions + inline RVec3 GetWorldSpaceContactPointOn1(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn1[inIndex]; } + inline RVec3 GetWorldSpaceContactPointOn2(uint inIndex) const { return mBaseOffset + mRelativeContactPointsOn2[inIndex]; } + + RVec3 mBaseOffset; ///< Offset to which all the contact points are relative + Vec3 mWorldSpaceNormal; ///< Normal for this manifold, direction along which to move body 2 out of collision along the shortest path + float mPenetrationDepth; ///< Penetration depth (move shape 2 by this distance to resolve the collision). If this value is negative, this is a speculative contact point and may not actually result in a velocity change as during solving the bodies may not actually collide. + SubShapeID mSubShapeID1; ///< Sub shapes that formed this manifold (note that when multiple manifolds are combined because they're coplanar, we lose some information here because we only keep track of one sub shape pair that we encounter, see description at Body::SetUseManifoldReduction) + SubShapeID mSubShapeID2; + ContactPoints mRelativeContactPointsOn1; ///< Contact points on the surface of shape 1 relative to mBaseOffset. + ContactPoints mRelativeContactPointsOn2; ///< Contact points on the surface of shape 2 relative to mBaseOffset. If there's no penetration, this will be the same as mRelativeContactPointsOn1. If there is penetration they will be different. +}; + +/// When a contact point is added or persisted, the callback gets a chance to override certain properties of the contact constraint. +/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to. +class ContactSettings +{ +public: + float mCombinedFriction; ///< Combined friction for the body pair (see: PhysicsSystem::SetCombineFriction) + float mCombinedRestitution; ///< Combined restitution for the body pair (see: PhysicsSystem::SetCombineRestitution) + float mInvMassScale1 = 1.0f; ///< Scale factor for the inverse mass of body 1 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale1 = 1.0f; ///< Scale factor for the inverse inertia of body 1 (usually same as mInvMassScale1) + float mInvMassScale2 = 1.0f; ///< Scale factor for the inverse mass of body 2 (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale2 = 1.0f; ///< Scale factor for the inverse inertia of body 2 (usually same as mInvMassScale2) + bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) + Vec3 mRelativeLinearSurfaceVelocity = Vec3::sZero(); ///< Relative linear surface velocity between the bodies (world space surface velocity of body 2 - world space surface velocity of body 1), can be used to create a conveyor belt effect + Vec3 mRelativeAngularSurfaceVelocity = Vec3::sZero(); ///< Relative angular surface velocity between the bodies (world space angular surface velocity of body 2 - world space angular surface velocity of body 1). Note that this angular velocity is relative to the center of mass of body 1, so if you want it relative to body 2's center of mass you need to add body 2 angular velocity x (body 1 world space center of mass - body 2 world space center of mass) to mRelativeLinearSurfaceVelocity. +}; + +/// Return value for the OnContactValidate callback. Determines if the contact is being processed or not. +/// Results are ordered so that the strongest accept has the lowest number and the strongest reject the highest number (which allows for easy combining of results) +enum class ValidateResult +{ + AcceptAllContactsForThisBodyPair, ///< Accept this and any further contact points for this body pair + AcceptContact, ///< Accept this contact only (and continue calling this callback for every contact manifold for the same body pair) + RejectContact, ///< Reject this contact only (but process any other contact manifolds for the same body pair) + RejectAllContactsForThisBodyPair ///< Rejects this and any further contact points for this body pair +}; + +/// A listener class that receives collision contact events. +/// It can be registered with the ContactConstraintManager (or PhysicsSystem). +/// Note that contact listener callbacks are called from multiple threads at the same time when all bodies are locked, you're only allowed to read from the bodies and you can't change physics state. +class ContactListener +{ +public: + /// Ensure virtual destructor + virtual ~ContactListener() = default; + + /// Called after detecting a collision between a body pair, but before calling OnContactAdded and before adding the contact constraint. + /// If the function rejects the contact, the contact will not be added and any other contacts between this body pair will not be processed. + /// This function will only be called once per PhysicsSystem::Update per body pair and may not be called again the next update + /// if a contact persists and no new contact pairs between sub shapes are found. + /// This is a rather expensive time to reject a contact point since a lot of the collision detection has happened already, make sure you + /// filter out the majority of undesired body pairs through the ObjectLayerPairFilter that is registered on the PhysicsSystem. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// Body 1 will have a motion type that is larger or equal than body 2's motion type (order from large to small: dynamic -> kinematic -> static). When motion types are equal, they are ordered by BodyID. + /// The collision result (inCollisionResult) is reported relative to inBaseOffset. + virtual ValidateResult OnContactValidate([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] RVec3Arg inBaseOffset, [[maybe_unused]] const CollideShapeResult &inCollisionResult) { return ValidateResult::AcceptAllContactsForThisBodyPair; } + + /// Called whenever a new contact point is detected. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// Note that only active bodies will report contacts, as soon as a body goes to sleep the contacts between that body and all other + /// bodies will receive an OnContactRemoved callback, if this is the case then Body::IsActive() will return false during the callback. + /// When contacts are added, the constraint solver has not run yet, so the collision impulse is unknown at that point. + /// The velocities of inBody1 and inBody2 are the velocities before the contact has been resolved, so you can use this to + /// estimate the collision impulse to e.g. determine the volume of the impact sound to play (see: EstimateCollisionResponse). + virtual void OnContactAdded([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ } + + /// Called whenever a contact is detected that was also detected last update. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// If the structure of the shape of a body changes between simulation steps (e.g. by adding/removing a child shape of a compound shape), + /// it is possible that the same sub shape ID used to identify the removed child shape is now reused for a different child shape. The physics + /// system cannot detect this, so may send a 'contact persisted' callback even though the contact is now on a different child shape. You can + /// detect this by keeping the old shape (before adding/removing a part) around until the next PhysicsSystem::Update (when the OnContactPersisted + /// callbacks are triggered) and resolving the sub shape ID against both the old and new shape to see if they still refer to the same child shape. + virtual void OnContactPersisted([[maybe_unused]] const Body &inBody1, [[maybe_unused]] const Body &inBody2, [[maybe_unused]] const ContactManifold &inManifold, [[maybe_unused]] ContactSettings &ioSettings) { /* Do nothing */ } + + /// Called whenever a contact was detected last update but is not detected anymore. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// Note that we're using BodyID's since the bodies may have been removed at the time of callback. + /// Body 1 and 2 will be sorted such that body 1 ID < body 2 ID, so body 1 may not be dynamic. + /// The sub shape ID were created in the previous simulation step too, so if the structure of a shape changes (e.g. by adding/removing a child shape of a compound shape), + /// the sub shape ID may not be valid / may not point to the same sub shape anymore. + /// If you want to know if this is the last contact between the two bodies, use PhysicsSystem::WereBodiesInContact. + virtual void OnContactRemoved([[maybe_unused]] const SubShapeIDPair &inSubShapePair) { /* Do nothing */ } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.cpp b/WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.cpp new file mode 100644 index 000000000..53cf12b5d --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.cpp @@ -0,0 +1,213 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution, uint inNumIterations) +{ + // Note this code is based on AxisConstraintPart, see that class for more comments on the math + + ContactPoints::size_type num_points = inManifold.mRelativeContactPointsOn1.size(); + JPH_ASSERT(num_points == inManifold.mRelativeContactPointsOn2.size()); + + // Start with zero impulses + outResult.mImpulses.resize(num_points); + memset(outResult.mImpulses.data(), 0, num_points * sizeof(CollisionEstimationResult::Impulse)); + + // Calculate friction directions + outResult.mTangent1 = inManifold.mWorldSpaceNormal.GetNormalizedPerpendicular(); + outResult.mTangent2 = inManifold.mWorldSpaceNormal.Cross(outResult.mTangent1); + + // Get body velocities + EMotionType motion_type1 = inBody1.GetMotionType(); + const MotionProperties *motion_properties1 = inBody1.GetMotionPropertiesUnchecked(); + if (motion_type1 != EMotionType::Static) + { + outResult.mLinearVelocity1 = motion_properties1->GetLinearVelocity(); + outResult.mAngularVelocity1 = motion_properties1->GetAngularVelocity(); + } + else + outResult.mLinearVelocity1 = outResult.mAngularVelocity1 = Vec3::sZero(); + + EMotionType motion_type2 = inBody2.GetMotionType(); + const MotionProperties *motion_properties2 = inBody2.GetMotionPropertiesUnchecked(); + if (motion_type2 != EMotionType::Static) + { + outResult.mLinearVelocity2 = motion_properties2->GetLinearVelocity(); + outResult.mAngularVelocity2 = motion_properties2->GetAngularVelocity(); + } + else + outResult.mLinearVelocity2 = outResult.mAngularVelocity2 = Vec3::sZero(); + + // Get inverse mass and inertia + float inv_m1, inv_m2; + Mat44 inv_i1, inv_i2; + if (motion_type1 == EMotionType::Dynamic) + { + inv_m1 = motion_properties1->GetInverseMass(); + inv_i1 = inBody1.GetInverseInertia(); + } + else + { + inv_m1 = 0.0f; + inv_i1 = Mat44::sZero(); + } + + if (motion_type2 == EMotionType::Dynamic) + { + inv_m2 = motion_properties2->GetInverseMass(); + inv_i2 = inBody2.GetInverseInertia(); + } + else + { + inv_m2 = 0.0f; + inv_i2 = Mat44::sZero(); + } + + // Get center of masses relative to the base offset + Vec3 com1 = Vec3(inBody1.GetCenterOfMassPosition() - inManifold.mBaseOffset); + Vec3 com2 = Vec3(inBody2.GetCenterOfMassPosition() - inManifold.mBaseOffset); + + struct AxisConstraint + { + inline void Initialize(Vec3Arg inR1, Vec3Arg inR2, Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2) + { + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + mR1PlusUxAxis = inR1.Cross(inWorldSpaceNormal); + mR2xAxis = inR2.Cross(inWorldSpaceNormal); + mInvI1_R1PlusUxAxis = inInvI1.Multiply3x3(mR1PlusUxAxis); + mInvI2_R2xAxis = inInvI2.Multiply3x3(mR2xAxis); + mEffectiveMass = 1.0f / (inInvM1 + mInvI1_R1PlusUxAxis.Dot(mR1PlusUxAxis) + inInvM2 + mInvI2_R2xAxis.Dot(mR2xAxis)); + mBias = 0.0f; + } + + inline float SolveGetLambda(Vec3Arg inWorldSpaceNormal, const CollisionEstimationResult &inResult) const + { + // Calculate jacobian multiplied by linear/angular velocity + float jv = inWorldSpaceNormal.Dot(inResult.mLinearVelocity1 - inResult.mLinearVelocity2) + mR1PlusUxAxis.Dot(inResult.mAngularVelocity1) - mR2xAxis.Dot(inResult.mAngularVelocity2); + + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + return mEffectiveMass * (jv - mBias); + } + + inline void SolveApplyLambda(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inLambda, CollisionEstimationResult &ioResult) const + { + // Apply impulse to body velocities + ioResult.mLinearVelocity1 -= (inLambda * inInvM1) * inWorldSpaceNormal; + ioResult.mAngularVelocity1 -= inLambda * mInvI1_R1PlusUxAxis; + ioResult.mLinearVelocity2 += (inLambda * inInvM2) * inWorldSpaceNormal; + ioResult.mAngularVelocity2 += inLambda * mInvI2_R2xAxis; + } + + inline void Solve(Vec3Arg inWorldSpaceNormal, float inInvM1, float inInvM2, float inMinLambda, float inMaxLambda, float &ioTotalLambda, CollisionEstimationResult &ioResult) const + { + // Calculate new total lambda + float total_lambda = ioTotalLambda + SolveGetLambda(inWorldSpaceNormal, ioResult); + + // Clamp impulse + total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda); + + SolveApplyLambda(inWorldSpaceNormal, inInvM1, inInvM2, total_lambda - ioTotalLambda, ioResult); + + ioTotalLambda = total_lambda; + } + + Vec3 mR1PlusUxAxis; + Vec3 mR2xAxis; + Vec3 mInvI1_R1PlusUxAxis; + Vec3 mInvI2_R2xAxis; + float mEffectiveMass; + float mBias; + }; + + struct Constraint + { + AxisConstraint mContact; + AxisConstraint mFriction1; + AxisConstraint mFriction2; + }; + + // Initialize the constraint properties + Constraint constraints[ContactPoints::Capacity]; + for (uint c = 0; c < num_points; ++c) + { + Constraint &constraint = constraints[c]; + + // Calculate contact points relative to body 1 and 2 + Vec3 p = 0.5f * (inManifold.mRelativeContactPointsOn1[c] + inManifold.mRelativeContactPointsOn2[c]); + Vec3 r1 = p - com1; + Vec3 r2 = p - com2; + + // Initialize contact constraint + constraint.mContact.Initialize(r1, r2, inManifold.mWorldSpaceNormal, inv_m1, inv_m2, inv_i1, inv_i2); + + // Handle elastic collisions + if (inCombinedRestitution > 0.0f) + { + // Calculate velocity of contact point + Vec3 relative_velocity = outResult.mLinearVelocity2 + outResult.mAngularVelocity2.Cross(r2) - outResult.mLinearVelocity1 - outResult.mAngularVelocity1.Cross(r1); + float normal_velocity = relative_velocity.Dot(inManifold.mWorldSpaceNormal); + + // If it is big enough, apply restitution + if (normal_velocity < -inMinVelocityForRestitution) + constraint.mContact.mBias = inCombinedRestitution * normal_velocity; + } + + if (inCombinedFriction > 0.0f) + { + // Initialize friction constraints + constraint.mFriction1.Initialize(r1, r2, outResult.mTangent1, inv_m1, inv_m2, inv_i1, inv_i2); + constraint.mFriction2.Initialize(r1, r2, outResult.mTangent2, inv_m1, inv_m2, inv_i1, inv_i2); + } + } + + // If there's only 1 contact point, we only need 1 iteration + int num_iterations = inCombinedFriction <= 0.0f && num_points == 1? 1 : inNumIterations; + + // Solve iteratively + for (int iteration = 0; iteration < num_iterations; ++iteration) + { + // Solve friction constraints first + if (inCombinedFriction > 0.0f && iteration > 0) // For first iteration the contact impulse is zero so there's no point in applying friction + for (uint c = 0; c < num_points; ++c) + { + const Constraint &constraint = constraints[c]; + CollisionEstimationResult::Impulse &impulse = outResult.mImpulses[c]; + + float lambda1 = impulse.mFrictionImpulse1 + constraint.mFriction1.SolveGetLambda(outResult.mTangent1, outResult); + float lambda2 = impulse.mFrictionImpulse2 + constraint.mFriction2.SolveGetLambda(outResult.mTangent2, outResult); + + // Calculate max impulse based on contact impulse + float max_impulse = inCombinedFriction * impulse.mContactImpulse; + + // If the total lambda that we will apply is too large, scale it back + float total_lambda_sq = Square(lambda1) + Square(lambda2); + if (total_lambda_sq > Square(max_impulse)) + { + float scale = max_impulse / sqrt(total_lambda_sq); + lambda1 *= scale; + lambda2 *= scale; + } + + constraint.mFriction1.SolveApplyLambda(outResult.mTangent1, inv_m1, inv_m2, lambda1 - impulse.mFrictionImpulse1, outResult); + constraint.mFriction2.SolveApplyLambda(outResult.mTangent2, inv_m1, inv_m2, lambda2 - impulse.mFrictionImpulse2, outResult); + + impulse.mFrictionImpulse1 = lambda1; + impulse.mFrictionImpulse2 = lambda2; + } + + // Solve contact constraints last + for (uint c = 0; c < num_points; ++c) + constraints[c].mContact.Solve(inManifold.mWorldSpaceNormal, inv_m1, inv_m2, 0.0f, FLT_MAX, outResult.mImpulses[c].mContactImpulse, outResult); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.h b/WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.h new file mode 100644 index 000000000..45098d147 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/EstimateCollisionResponse.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A structure that contains the estimated contact and friction impulses and the resulting body velocities +struct CollisionEstimationResult +{ + Vec3 mLinearVelocity1; ///< The estimated linear velocity of body 1 after collision + Vec3 mAngularVelocity1; ///< The estimated angular velocity of body 1 after collision + Vec3 mLinearVelocity2; ///< The estimated linear velocity of body 2 after collision + Vec3 mAngularVelocity2; ///< The estimated angular velocity of body 2 after collision + + Vec3 mTangent1; ///< Normalized tangent of contact normal + Vec3 mTangent2; ///< Second normalized tangent of contact normal (forms a basis with mTangent1 and mWorldSpaceNormal) + + struct Impulse + { + float mContactImpulse; ///< Estimated contact impulses (kg m / s) + float mFrictionImpulse1; ///< Estimated friction impulses in the direction of tangent 1 (kg m / s) + float mFrictionImpulse2; ///< Estimated friction impulses in the direction of tangent 2 (kg m / s) + }; + + using Impulses = StaticArray; + + Impulses mImpulses; +}; + +/// This function estimates the contact impulses and body velocity changes as a result of a collision. +/// It can be used in the ContactListener::OnContactAdded to determine the strength of the collision to e.g. play a sound or trigger a particle system. +/// This function is accurate when two bodies collide but will not be accurate when more than 2 bodies collide at the same time as it does not know about these other collisions. +/// +/// @param inBody1 Colliding body 1 +/// @param inBody2 Colliding body 2 +/// @param inManifold The collision manifold +/// @param outResult A structure that contains the estimated contact and friction impulses and the resulting body velocities +/// @param inCombinedFriction The combined friction of body 1 and body 2 (see ContactSettings::mCombinedFriction) +/// @param inCombinedRestitution The combined restitution of body 1 and body 2 (see ContactSettings::mCombinedRestitution) +/// @param inMinVelocityForRestitution Minimal velocity required for restitution to be applied (see PhysicsSettings::mMinVelocityForRestitution) +/// @param inNumIterations Number of iterations to use for the impulse estimation (see PhysicsSettings::mNumVelocitySteps, note you can probably use a lower number for a decent estimate). If you set the number of iterations to 1 then no friction will be calculated. +JPH_EXPORT void EstimateCollisionResponse(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, CollisionEstimationResult &outResult, float inCombinedFriction, float inCombinedRestitution, float inMinVelocityForRestitution = 1.0f, uint inNumIterations = 10); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/GroupFilter.cpp b/WickedEngine/Jolt/Physics/Collision/GroupFilter.cpp new file mode 100644 index 000000000..80c7620fd --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/GroupFilter.cpp @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(GroupFilter) +{ + JPH_ADD_BASE_CLASS(GroupFilter, SerializableObject) +} + +void GroupFilter::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); +} + +void GroupFilter::RestoreBinaryState(StreamIn &inStream) +{ + // RTTI hash is read in sRestoreFromBinaryState +} + +GroupFilter::GroupFilterResult GroupFilter::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &GroupFilter::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/GroupFilter.h b/WickedEngine/Jolt/Physics/Collision/GroupFilter.h new file mode 100644 index 000000000..f0ad835e0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/GroupFilter.h @@ -0,0 +1,41 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollisionGroup; +class StreamIn; +class StreamOut; + +/// Abstract class that checks if two CollisionGroups collide +class JPH_EXPORT GroupFilter : public SerializableObject, public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, GroupFilter) + + /// Virtual destructor + virtual ~GroupFilter() override = default; + + /// Check if two groups collide + virtual bool CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const = 0; + + /// Saves the contents of the group filter in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + using GroupFilterResult = Result>; + + /// Creates a GroupFilter of the correct type and restores its contents from the binary stream inStream. + static GroupFilterResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/GroupFilterTable.cpp b/WickedEngine/Jolt/Physics/Collision/GroupFilterTable.cpp new file mode 100644 index 000000000..dd69d27c8 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/GroupFilterTable.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GroupFilterTable) +{ + JPH_ADD_BASE_CLASS(GroupFilterTable, GroupFilter) + + JPH_ADD_ATTRIBUTE(GroupFilterTable, mNumSubGroups) + JPH_ADD_ATTRIBUTE(GroupFilterTable, mTable) +} + +void GroupFilterTable::SaveBinaryState(StreamOut &inStream) const +{ + GroupFilter::SaveBinaryState(inStream); + + inStream.Write(mNumSubGroups); + inStream.Write(mTable); +} + +void GroupFilterTable::RestoreBinaryState(StreamIn &inStream) +{ + GroupFilter::RestoreBinaryState(inStream); + + inStream.Read(mNumSubGroups); + inStream.Read(mTable); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/GroupFilterTable.h b/WickedEngine/Jolt/Physics/Collision/GroupFilterTable.h new file mode 100644 index 000000000..62ce34ce5 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/GroupFilterTable.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of GroupFilter that stores a bit table with one bit per sub shape ID pair to determine if they collide or not +/// +/// The collision rules: +/// - If one of the objects is in the cInvalidGroup the objects will collide. +/// - If the objects are in different groups they will collide. +/// - If they're in the same group but their collision filter is different they will not collide. +/// - If they're in the same group and their collision filters match, we'll use the SubGroupID and the table below. +/// +/// For N = 6 sub groups the table will look like: +/// +/// sub group 1 ---> +/// sub group 2 x..... +/// | ox.... +/// | oox... +/// V ooox.. +/// oooox. +/// ooooox +/// +/// * 'x' means sub group 1 == sub group 2 and we define this to never collide. +/// * 'o' is a bit that we have to store that defines if the sub groups collide or not. +/// * '.' is a bit we don't need to store because the table is symmetric, we take care that group 2 > group 1 by swapping sub group 1 and sub group 2 if needed. +/// +/// The total number of bits we need to store is (N * (N - 1)) / 2 +class JPH_EXPORT GroupFilterTable final : public GroupFilter +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GroupFilterTable) + +private: + using GroupID = CollisionGroup::GroupID; + using SubGroupID = CollisionGroup::SubGroupID; + + /// Get which bit corresponds to the pair (inSubGroup1, inSubGroup2) + int GetBit(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const + { + JPH_ASSERT(inSubGroup1 != inSubGroup2); + + // We store the lower left half only, so swap the inputs when trying to access the top right half + if (inSubGroup1 > inSubGroup2) + swap(inSubGroup1, inSubGroup2); + + JPH_ASSERT(inSubGroup2 < mNumSubGroups); + + // Calculate at which bit the entry for this pair resides + // We use the fact that a row always starts at inSubGroup2 * (inSubGroup2 - 1) / 2 + // (this is the amount of bits needed to store a table of inSubGroup2 entries) + return (inSubGroup2 * (inSubGroup2 - 1)) / 2 + inSubGroup1; + } + +public: + /// Constructs the table with inNumSubGroups subgroups, initially all collision pairs are enabled except when the sub group ID is the same + explicit GroupFilterTable(uint inNumSubGroups = 0) : + mNumSubGroups(inNumSubGroups) + { + // By default everything collides + int table_size = ((inNumSubGroups * (inNumSubGroups - 1)) / 2 + 7) / 8; + mTable.resize(table_size, 0xff); + } + + /// Copy constructor + GroupFilterTable(const GroupFilterTable &inRHS) : mNumSubGroups(inRHS.mNumSubGroups), mTable(inRHS.mTable) { } + + /// Disable collision between two sub groups + void DisableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2) + { + int bit = GetBit(inSubGroup1, inSubGroup2); + mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111))); + } + + /// Enable collision between two sub groups + void EnableCollision(SubGroupID inSubGroup1, SubGroupID inSubGroup2) + { + int bit = GetBit(inSubGroup1, inSubGroup2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + + /// Check if the collision between two subgroups is enabled + inline bool IsCollisionEnabled(SubGroupID inSubGroup1, SubGroupID inSubGroup2) const + { + // Test if the bit is set for this group pair + int bit = GetBit(inSubGroup1, inSubGroup2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + + /// Checks if two CollisionGroups collide + virtual bool CanCollide(const CollisionGroup &inGroup1, const CollisionGroup &inGroup2) const override + { + // If one of the groups is cInvalidGroup the objects will collide (note that the if following this if will ensure that group2 is not cInvalidGroup) + if (inGroup1.GetGroupID() == CollisionGroup::cInvalidGroup) + return true; + + // If the objects are in different groups, they collide + if (inGroup1.GetGroupID() != inGroup2.GetGroupID()) + return true; + + // If the collision filters do not match, but they're in the same group we ignore the collision + if (inGroup1.GetGroupFilter() != inGroup2.GetGroupFilter()) + return false; + + // If they are in the same sub group, they don't collide + if (inGroup1.GetSubGroupID() == inGroup2.GetSubGroupID()) + return false; + + // Check the bit table + return IsCollisionEnabled(inGroup1.GetSubGroupID(), inGroup2.GetSubGroupID()); + } + + // See: GroupFilter::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + +protected: + // See: GroupFilter::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + uint mNumSubGroups; ///< The number of subgroups that this group filter supports + Array mTable; ///< The table of bits that indicates which pairs collide +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h b/WickedEngine/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h new file mode 100644 index 000000000..e879487fd --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/InternalEdgeRemovingCollector.h @@ -0,0 +1,237 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + +#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG +#include +#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + +JPH_NAMESPACE_BEGIN + +/// Removes internal edges from collision results. Can be used to filter out 'ghost collisions'. +/// Based on: Contact generation for meshes - Pierre Terdiman (https://www.codercorner.com/MeshContacts.pdf) +class InternalEdgeRemovingCollector : public CollideShapeCollector +{ + static constexpr uint cMaxDelayedResults = 16; + static constexpr uint cMaxVoidedFeatures = 128; + + /// Check if a vertex is voided + inline bool IsVoided(Vec3 inV) const + { + for (const Float3 &vf : mVoidedFeatures) + if (inV.IsClose(Vec3::sLoadFloat3Unsafe(vf), 1.0e-8f)) + return true; + return false; + } + + /// Add all vertices of a face to the voided features + inline void VoidFeatures(const CollideShapeResult &inResult) + { + for (const Vec3 &v : inResult.mShape2Face) + if (!IsVoided(v)) + { + if (mVoidedFeatures.size() == cMaxVoidedFeatures) + break; + Float3 f; + v.StoreFloat3(&f); + mVoidedFeatures.push_back(f); + } + } + + /// Call the chained collector + inline void Chain(const CollideShapeResult &inResult) + { + // Make sure the chained collector has the same context as we do + mChainedCollector.SetContext(GetContext()); + + // Forward the hit + mChainedCollector.AddHit(inResult); + + // If our chained collector updated its early out fraction, we need to follow + UpdateEarlyOutFraction(mChainedCollector.GetEarlyOutFraction()); + } + + /// Call the chained collector and void all features of inResult + inline void ChainAndVoid(const CollideShapeResult &inResult) + { + Chain(inResult); + VoidFeatures(inResult); + + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), inResult.mShape2Face, Color::sGreen); + DebugRenderer::sInstance->DrawArrow(RVec3(inResult.mContactPointOn2), RVec3(inResult.mContactPointOn2) + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f); + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + } + +public: + /// Constructor, configures a collector to be called with all the results that do not hit internal edges + explicit InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector) : + mChainedCollector(inChainedCollector) + { + } + + // See: CollideShapeCollector::Reset + virtual void Reset() override + { + CollideShapeCollector::Reset(); + + mChainedCollector.Reset(); + + mVoidedFeatures.clear(); + mDelayedResults.clear(); + } + + // See: CollideShapeCollector::OnBody + virtual void OnBody(const Body &inBody) override + { + // Just forward the call to our chained collector + mChainedCollector.OnBody(inBody); + } + + // See: CollideShapeCollector::AddHit + virtual void AddHit(const CollideShapeResult &inResult) override + { + // We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal + if (inResult.mShape2Face.size() < 3) + return ChainAndVoid(inResult); + + // Get the triangle normal of shape 2 face + Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]); + float triangle_normal_len = triangle_normal.Length(); + if (triangle_normal_len < 1e-6f) + return ChainAndVoid(inResult); + + // If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately + // We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact' + Vec3 contact_normal = -inResult.mPenetrationAxis; + float contact_normal_len = inResult.mPenetrationAxis.Length(); + if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree) + return ChainAndVoid(inResult); + + // Delayed processing + if (mDelayedResults.size() == cMaxDelayedResults) + return ChainAndVoid(inResult); + mDelayedResults.push_back(inResult); + } + + /// After all hits have been added, call this function to process the delayed results + void Flush() + { + // Sort on biggest penetration depth first + uint sorted_indices[cMaxDelayedResults]; + for (uint i = 0; i < uint(mDelayedResults.size()); ++i) + sorted_indices[i] = i; + QuickSort(sorted_indices, sorted_indices + mDelayedResults.size(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; }); + + // Loop over all results + for (uint i = 0; i < uint(mDelayedResults.size()); ++i) + { + const CollideShapeResult &r = mDelayedResults[sorted_indices[i]]; + + // Determine which vertex or which edge is the closest to the contact point + float best_dist_sq = FLT_MAX; + uint best_v1_idx = 0; + uint best_v2_idx = 0; + uint num_v = uint(r.mShape2Face.size()); + uint v1_idx = num_v - 1; + Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2; + for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx) + { + Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2; + Vec3 v1_v2 = v2 - v1; + float denominator = v1_v2.LengthSq(); + if (denominator < Square(FLT_EPSILON)) + { + // Degenerate, assume v1 is closest, v2 will be tested in a later iteration + float v1_len_sq = v1.LengthSq(); + if (v1_len_sq < best_dist_sq) + { + best_dist_sq = v1_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v1_idx; + } + } + else + { + // Taken from ClosestPoint::GetBaryCentricCoordinates + float fraction = -v1.Dot(v1_v2) / denominator; + if (fraction < 1.0e-6f) + { + // Closest lies on v1 + float v1_len_sq = v1.LengthSq(); + if (v1_len_sq < best_dist_sq) + { + best_dist_sq = v1_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v1_idx; + } + } + else if (fraction < 1.0f - 1.0e-6f) + { + // Closest lies on the line segment v1, v2 + Vec3 closest = v1 + fraction * v1_v2; + float closest_len_sq = closest.LengthSq(); + if (closest_len_sq < best_dist_sq) + { + best_dist_sq = closest_len_sq; + best_v1_idx = v1_idx; + best_v2_idx = v2_idx; + } + } + // else closest is v2, but v2 will be tested in a later iteration + } + + v1_idx = v2_idx; + v1 = v2; + } + + // Check if this vertex/edge is voided + bool voided = IsVoided(r.mShape2Face[best_v1_idx]) + && (best_v1_idx == best_v2_idx || IsVoided(r.mShape2Face[best_v2_idx])); + + #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + Color color = voided? Color::sRed : Color::sYellow; + DebugRenderer::sInstance->DrawText3D(RVec3(r.mContactPointOn2), StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f); + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sIdentity(), r.mShape2Face, color); + DebugRenderer::sInstance->DrawArrow(RVec3(r.mContactPointOn2), RVec3(r.mContactPointOn2) + r.mPenetrationAxis.NormalizedOr(Vec3::sZero()), color, 0.1f); + DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v1_idx]), IsVoided(r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f); + DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v2_idx]), IsVoided(r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f); + #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG + + // No voided features, accept the contact + if (!voided) + Chain(r); + + // Void the features of this face + VoidFeatures(r); + } + + // All delayed results have been processed + mVoidedFeatures.clear(); + mDelayedResults.clear(); + } + + /// Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges + static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) + { + JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces + + InternalEdgeRemovingCollector wrapper(ioCollector); + CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter); + wrapper.Flush(); + } + +private: + CollideShapeCollector & mChainedCollector; + StaticArray mVoidedFeatures; // Read with Vec3::sLoadFloat3Unsafe so must not be the last member + StaticArray mDelayedResults; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp b/WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp new file mode 100644 index 000000000..3331f5d7a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.cpp @@ -0,0 +1,237 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass)) +{ + // Makes no sense to call this with 4 or less points + JPH_ASSERT(ioContactPointsOn1.size() > 4); + + // Both arrays should have the same size + JPH_ASSERT(ioContactPointsOn1.size() == ioContactPointsOn2.size()); + + // Penetration axis must be normalized + JPH_ASSERT(inPenetrationAxis.IsNormalized()); + + // We use a heuristic of (distance to center of mass) * (penetration depth) to find the contact point that we should keep + // Neither of those two terms should ever become zero, so we clamp against this minimum value + constexpr float cMinDistanceSq = 1.0e-6f; // 1 mm + + ContactPoints projected; + StaticArray penetration_depth_sq; + for (ContactPoints::size_type i = 0; i < ioContactPointsOn1.size(); ++i) + { + // Project contact points on the plane through inCenterOfMass with normal inPenetrationAxis and center around the center of mass of body 1 + // (note that since all points are relative to inCenterOfMass we can project onto the plane through the origin) + Vec3 v1 = ioContactPointsOn1[i]; + projected.push_back(v1 - v1.Dot(inPenetrationAxis) * inPenetrationAxis); + + // Calculate penetration depth^2 of each point and clamp against the minimal distance + Vec3 v2 = ioContactPointsOn2[i]; + penetration_depth_sq.push_back(max(cMinDistanceSq, (v2 - v1).LengthSq())); + } + + // Find the point that is furthest away from the center of mass (its torque will have the biggest influence) + // and the point that has the deepest penetration depth. Use the heuristic (distance to center of mass) * (penetration depth) for this. + uint point1 = 0; + float val = max(cMinDistanceSq, projected[0].LengthSq()) * penetration_depth_sq[0]; + for (uint i = 0; i < projected.size(); ++i) + { + float v = max(cMinDistanceSq, projected[i].LengthSq()) * penetration_depth_sq[i]; + if (v > val) + { + val = v; + point1 = i; + } + } + Vec3 point1v = projected[point1]; + + // Find point furthest from the first point forming a line segment with point1. Again combine this with the heuristic + // for deepest point as per above. + uint point2 = uint(-1); + val = -FLT_MAX; + for (uint i = 0; i < projected.size(); ++i) + if (i != point1) + { + float v = max(cMinDistanceSq, (projected[i] - point1v).LengthSq()) * penetration_depth_sq[i]; + if (v > val) + { + val = v; + point2 = i; + } + } + JPH_ASSERT(point2 != uint(-1)); + Vec3 point2v = projected[point2]; + + // Find furthest points on both sides of the line segment in order to maximize the area + uint point3 = uint(-1); + uint point4 = uint(-1); + float min_val = 0.0f; + float max_val = 0.0f; + Vec3 perp = (point2v - point1v).Cross(inPenetrationAxis); + for (uint i = 0; i < projected.size(); ++i) + if (i != point1 && i != point2) + { + float v = perp.Dot(projected[i] - point1v); + if (v < min_val) + { + min_val = v; + point3 = i; + } + else if (v > max_val) + { + max_val = v; + point4 = i; + } + } + + // Add points to array (in order so they form a polygon) + StaticArray points_to_keep_on_1, points_to_keep_on_2; + points_to_keep_on_1.push_back(ioContactPointsOn1[point1]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point1]); + if (point3 != uint(-1)) + { + points_to_keep_on_1.push_back(ioContactPointsOn1[point3]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point3]); + } + points_to_keep_on_1.push_back(ioContactPointsOn1[point2]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point2]); + if (point4 != uint(-1)) + { + JPH_ASSERT(point3 != point4); + points_to_keep_on_1.push_back(ioContactPointsOn1[point4]); + points_to_keep_on_2.push_back(ioContactPointsOn2[point4]); + } + +#ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawContactPointReduction) + { + // Draw input polygon + DebugRenderer::sInstance->DrawWirePolygon(RMat44::sTranslation(inCenterOfMass), ioContactPointsOn1, Color::sOrange, 0.05f); + + // Draw primary axis + DebugRenderer::sInstance->DrawArrow(inCenterOfMass + ioContactPointsOn1[point1], inCenterOfMass + ioContactPointsOn1[point2], Color::sRed, 0.05f); + + // Draw contact points we kept + for (Vec3 p : points_to_keep_on_1) + DebugRenderer::sInstance->DrawMarker(inCenterOfMass + p, Color::sGreen, 0.1f); + } +#endif // JPH_DEBUG_RENDERER + + // Copy the points back to the input buffer + ioContactPointsOn1 = points_to_keep_on_1; + ioContactPointsOn2 = points_to_keep_on_2; +} + +void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq , const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 JPH_IF_DEBUG_RENDERER(, RVec3Arg inCenterOfMass)) +{ +#ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawContactPoint) + { + RVec3 cp1 = inCenterOfMass + inContactPoint1; + RVec3 cp2 = inCenterOfMass + inContactPoint2; + + // Draw contact points + DebugRenderer::sInstance->DrawMarker(cp1, Color::sRed, 0.1f); + DebugRenderer::sInstance->DrawMarker(cp2, Color::sGreen, 0.1f); + + // Draw contact normal + DebugRenderer::sInstance->DrawArrow(cp1, cp1 + inPenetrationAxis.Normalized(), Color::sRed, 0.05f); + } +#endif // JPH_DEBUG_RENDERER + + // Remember size before adding new points, to check at the end if we added some + ContactPoints::size_type old_size = outContactPoints1.size(); + + // Check if both shapes have polygon faces + if (inShape1Face.size() >= 2 // The dynamic shape needs to have at least 2 points or else there can never be more than 1 contact point + && inShape2Face.size() >= 3) // The dynamic/static shape needs to have at least 3 points (in the case that it has 2 points only if the edges match exactly you can have 2 contact points, but this situation is unstable anyhow) + { + // Clip the polygon of face 2 against that of 1 + ConvexShape::SupportingFace clipped_face; + if (inShape1Face.size() >= 3) + ClipPolyVsPoly(inShape2Face, inShape1Face, inPenetrationAxis, clipped_face); + else if (inShape1Face.size() == 2) + ClipPolyVsEdge(inShape2Face, inShape1Face[0], inShape1Face[1], inPenetrationAxis, clipped_face); + + // Project the points back onto the plane of shape 1 face and only keep those that are behind the plane + Vec3 plane_origin = inShape1Face[0]; + Vec3 plane_normal; + Vec3 first_edge = inShape1Face[1] - plane_origin; + if (inShape1Face.size() >= 3) + { + // Three vertices, can just calculate the normal + plane_normal = first_edge.Cross(inShape1Face[2] - plane_origin); + } + else + { + // Two vertices, first find a perpendicular to the edge and penetration axis and then use the perpendicular together with the edge to form a normal + plane_normal = first_edge.Cross(inPenetrationAxis).Cross(first_edge); + } + + // Check if the plane normal has any length, if not the clipped shape is so small that we'll just use the contact points + float plane_normal_len_sq = plane_normal.LengthSq(); + if (plane_normal_len_sq > 0.0f) + { + // Discard points of faces that are too far away to collide + for (Vec3 p2 : clipped_face) + { + float distance = (p2 - plane_origin).Dot(plane_normal); // Note should divide by length of plane_normal (unnormalized here) + if (distance <= 0.0f || Square(distance) < inMaxContactDistanceSq * plane_normal_len_sq) // Must be close enough to plane, note we correct for not dividing by plane normal length here + { + // Project point back on shape 1 using the normal, note we correct for not dividing by plane normal length here: + // p1 = p2 - (distance / sqrt(plane_normal_len_sq)) * (plane_normal / sqrt(plane_normal_len_sq)); + Vec3 p1 = p2 - (distance / plane_normal_len_sq) * plane_normal; + + outContactPoints1.push_back(p1); + outContactPoints2.push_back(p2); + } + } + } + + #ifdef JPH_DEBUG_RENDERER + if (ContactConstraintManager::sDrawSupportingFaces) + { + RMat44 com = RMat44::sTranslation(inCenterOfMass); + + // Draw clipped poly + DebugRenderer::sInstance->DrawWirePolygon(com, clipped_face, Color::sOrange); + + // Draw supporting faces + DebugRenderer::sInstance->DrawWirePolygon(com, inShape1Face, Color::sRed, 0.05f); + DebugRenderer::sInstance->DrawWirePolygon(com, inShape2Face, Color::sGreen, 0.05f); + + // Draw normal + if (plane_normal_len_sq > 0.0f) + { + RVec3 plane_origin_ws = inCenterOfMass + plane_origin; + DebugRenderer::sInstance->DrawArrow(plane_origin_ws, plane_origin_ws + plane_normal / sqrt(plane_normal_len_sq), Color::sYellow, 0.05f); + } + + // Draw contact points that remain after distance check + for (ContactPoints::size_type p = old_size; p < outContactPoints1.size(); ++p) + DebugRenderer::sInstance->DrawMarker(inCenterOfMass + outContactPoints1[p], Color::sYellow, 0.1f); + } + #endif // JPH_DEBUG_RENDERER + } + + // If the clipping result is empty, use the contact point itself + if (outContactPoints1.size() == old_size) + { + outContactPoints1.push_back(inContactPoint1); + outContactPoints2.push_back(inContactPoint2); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h b/WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h new file mode 100644 index 000000000..72a5b8f0c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ManifoldBetweenTwoFaces.h @@ -0,0 +1,44 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Remove contact points if there are > 4 (no more than 4 are needed for a stable solution) +/// @param inPenetrationAxis is the world space penetration axis (must be normalized) +/// @param ioContactPointsOn1 The contact points on shape 1 relative to inCenterOfMass +/// @param ioContactPointsOn2 The contact points on shape 2 relative to inCenterOfMass +/// On output ioContactPointsOn1/2 are reduced to 4 or less points +#ifdef JPH_DEBUG_RENDERER +/// @param inCenterOfMass Center of mass position of body 1 +#endif +JPH_EXPORT void PruneContactPoints(Vec3Arg inPenetrationAxis, ContactPoints &ioContactPointsOn1, ContactPoints &ioContactPointsOn2 +#ifdef JPH_DEBUG_RENDERER + , RVec3Arg inCenterOfMass +#endif + ); + +/// Determine contact points between 2 faces of 2 shapes and return them in outContactPoints 1 & 2 +/// @param inContactPoint1 The contact point on shape 1 relative to inCenterOfMass +/// @param inContactPoint2 The contact point on shape 2 relative to inCenterOfMass +/// @param inPenetrationAxis The local space penetration axis in world space +/// @param inMaxContactDistanceSq After face 2 is clipped against face 1, each remaining point on face 2 is tested against the plane of face 1. If the distance^2 on the positive side of the plane is larger than this distance, the point will be discarded as a contact point. +/// @param inShape1Face The supporting faces on shape 1 relative to inCenterOfMass +/// @param inShape2Face The supporting faces on shape 2 relative to inCenterOfMass +/// @param outContactPoints1 Returns the contact points between the two shapes for shape 1 relative to inCenterOfMass (any existing points in the output array are left as is) +/// @param outContactPoints2 Returns the contact points between the two shapes for shape 2 relative to inCenterOfMass (any existing points in the output array are left as is) +#ifdef JPH_DEBUG_RENDERER +/// @param inCenterOfMass Center of mass position of body 1 +#endif +JPH_EXPORT void ManifoldBetweenTwoFaces(Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inPenetrationAxis, float inMaxContactDistanceSq, const ConvexShape::SupportingFace &inShape1Face, const ConvexShape::SupportingFace &inShape2Face, ContactPoints &outContactPoints1, ContactPoints &outContactPoints2 +#ifdef JPH_DEBUG_RENDERER + , RVec3Arg inCenterOfMass +#endif + ); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.cpp b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.cpp new file mode 100644 index 000000000..d2b9663e3 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.cpp @@ -0,0 +1,412 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool NarrowPhaseQuery::CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public RayCastBodyCollector + { + public: + MyCollector(const RRayCast &inRay, RayCastResult &ioHit, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter) : + mRay(inRay), + mHit(ioHit), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter) + { + UpdateEarlyOutFraction(ioHit.mFraction); + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction < mHit.mFraction, "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + if (ts.CastRay(mRay, mHit)) + { + // Test that we didn't find a further hit by accident + JPH_ASSERT(mHit.mFraction >= 0.0f && mHit.mFraction < GetEarlyOutFraction()); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mHit.mFraction); + } + } + } + } + } + + RRayCast mRay; + RayCastResult & mHit; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + }; + + // Do broadphase test, note that the broadphase uses floats so we drop precision here + MyCollector collector(inRay, ioHit, *mBodyLockInterface, inBodyFilter); + mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); + return ioHit.mFraction <= 1.0f; +} + +void NarrowPhaseQuery::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public RayCastBodyCollector + { + public: + MyCollector(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + RayCastBodyCollector(ioCollector), + mRay(inRay), + mRayCastSettings(inRayCastSettings), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction < mCollector.GetEarlyOutFraction(), "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CastRay(mRay, mRayCastSettings, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RRayCast mRay; + RayCastSettings mRayCastSettings; + CastRayCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test, note that the broadphase uses floats so we drop precision here + MyCollector collector(inRay, inRayCastSettings, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CastRay(RayCast(inRay), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mPoint(inPoint), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollidePoint(mPoint, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RVec3 mPoint; + CollidePointCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test (note: truncates double to single precision since the broadphase uses single precision) + MyCollector collector(inPoint, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollidePoint(Vec3(inPoint), collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mShape(inShape), + mShapeScale(inShapeScale), + mCenterOfMassTransform(inCenterOfMassTransform), + mCollideShapeSettings(inCollideShapeSettings), + mBaseOffset(inBaseOffset), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollideShape(mShape, mShapeScale, mCenterOfMassTransform, mCollideShapeSettings, mBaseOffset, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const Shape * mShape; + Vec3 mShapeScale; + RMat44 mCenterOfMassTransform; + const CollideShapeSettings & mCollideShapeSettings; + RVec3 mBaseOffset; + CollideShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Calculate bounds for shape and expand by max separation distance + AABox bounds = inShape->GetWorldSpaceBounds(inCenterOfMassTransform, inShapeScale); + bounds.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + + // Do broadphase test + MyCollector collector(inShape, inShapeScale, inCenterOfMassTransform, inCollideShapeSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(bounds, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + class MyCollector : public CastShapeBodyCollector + { + public: + MyCollector(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CastShapeBodyCollector(ioCollector), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mBaseOffset(inBaseOffset), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + JPH_ASSERT(inResult.mFraction <= max(0.0f, mCollector.GetEarlyOutFraction()), "This hit should not have been passed on to the collector"); + + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult.mBodyID)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult.mBodyID); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CastShape(mShapeCast, mShapeCastSettings, mBaseOffset, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + RShapeCast mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + RVec3 mBaseOffset; + CastShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test + MyCollector collector(inShapeCast, inShapeCastSettings, inBaseOffset, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CastAABox({ inShapeCast.mShapeWorldBounds, inShapeCast.mDirection }, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +void NarrowPhaseQuery::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const +{ + class MyCollector : public CollideShapeBodyCollector + { + public: + MyCollector(const AABox &inBox, TransformedShapeCollector &ioCollector, const BodyLockInterface &inBodyLockInterface, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) : + CollideShapeBodyCollector(ioCollector), + mBox(inBox), + mCollector(ioCollector), + mBodyLockInterface(inBodyLockInterface), + mBodyFilter(inBodyFilter), + mShapeFilter(inShapeFilter) + { + } + + virtual void AddHit(const ResultType &inResult) override + { + // Only test shape if it passes the body filter + if (mBodyFilter.ShouldCollide(inResult)) + { + // Lock the body + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.SucceededAndIsInBroadPhase()) // Race condition: body could have been removed since it has been found in the broadphase, ensures body is in the broadphase while we call the callbacks + { + const Body &body = lock.GetBody(); + + // Check body filter again now that we've locked the body + if (mBodyFilter.ShouldCollideLocked(body)) + { + // Collect the transformed shape + TransformedShape ts = body.GetTransformedShape(); + + // Notify collector of new body + mCollector.OnBody(body); + + // Release the lock now, we have all the info we need in the transformed shape + lock.ReleaseLock(); + + // Do narrow phase collision check + ts.CollectTransformedShapes(mBox, mCollector, mShapeFilter); + + // Update early out fraction based on narrow phase collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + } + } + } + + const AABox & mBox; + TransformedShapeCollector & mCollector; + const BodyLockInterface & mBodyLockInterface; + const BodyFilter & mBodyFilter; + const ShapeFilter & mShapeFilter; + }; + + // Do broadphase test + MyCollector collector(inBox, ioCollector, *mBodyLockInterface, inBodyFilter, inShapeFilter); + mBroadPhaseQuery->CollideAABox(inBox, collector, inBroadPhaseLayerFilter, inObjectLayerFilter); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.h b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.h new file mode 100644 index 000000000..0967b21a1 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseQuery.h @@ -0,0 +1,74 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Shape; +class CollideShapeSettings; +class RayCastResult; + +/// Class that provides an interface for doing precise collision detection against the broad and then the narrow phase. +/// Unlike a BroadPhaseQuery, the NarrowPhaseQuery will test against shapes and will return collision information against triangles, spheres etc. +class JPH_EXPORT NarrowPhaseQuery : public NonCopyable +{ +public: + /// Initialize the interface (should only be called by PhysicsSystem) + void Init(BodyLockInterface &inBodyLockInterface, BroadPhaseQuery &inBroadPhaseQuery) { mBodyLockInterface = &inBodyLockInterface; mBroadPhaseQuery = &inBroadPhaseQuery; } + + /// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned. + /// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on body with ID ioHit.mBodyID. + bool CastRay(const RRayCast &inRay, RayCastResult &ioHit, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }) const; + + /// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use Body::GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on body with collected body ID. + void CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid. + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit + void CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Collide a shape with the system + /// @param inShape Shape to test + /// @param inShapeScale Scale in local space of shape + /// @param inCenterOfMassTransform Center of mass transform for the shape + /// @param inCollideShapeSettings Settings + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inCenterOfMassTransform.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inBroadPhaseLayerFilter Filter that filters at broadphase level + /// @param inObjectLayerFilter Filter that filters at layer level + /// @param inBodyFilter Filter that filters at body level + /// @param inShapeFilter Filter that filters at shape level + void CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Cast a shape and report any hits to ioCollector + /// @param inShapeCast The shape cast and its position and direction + /// @param inShapeCastSettings Settings for the shape cast + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inBroadPhaseLayerFilter Filter that filters at broadphase level + /// @param inObjectLayerFilter Filter that filters at layer level + /// @param inBodyFilter Filter that filters at body level + /// @param inShapeFilter Filter that filters at shape level + void CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + + /// Collect all leaf transformed shapes that fall inside world space box inBox + void CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter = { }, const ObjectLayerFilter &inObjectLayerFilter = { }, const BodyFilter &inBodyFilter = { }, const ShapeFilter &inShapeFilter = { }) const; + +private: + BodyLockInterface * mBodyLockInterface = nullptr; + BroadPhaseQuery * mBroadPhaseQuery = nullptr; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.cpp b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.cpp new file mode 100644 index 000000000..69c611372 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.cpp @@ -0,0 +1,62 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_TRACK_NARROWPHASE_STATS + +JPH_NAMESPACE_BEGIN + +NarrowPhaseStat NarrowPhaseStat::sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; +NarrowPhaseStat NarrowPhaseStat::sCastShape[NumSubShapeTypes][NumSubShapeTypes]; + +thread_local TrackNarrowPhaseStat *TrackNarrowPhaseStat::sRoot = nullptr; + +void NarrowPhaseStat::ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const +{ + double total_pct = 100.0 * double(mTotalTicks) / double(inTicks100Pct); + double total_pct_excl_children = 100.0 * double(mTotalTicks - mChildTicks) / double(inTicks100Pct); + + std::stringstream str; + str << inName << ", " << sSubShapeTypeNames[(int)inType1] << ", " << sSubShapeTypeNames[(int)inType2] << ", " << mNumQueries << ", " << total_pct << ", " << total_pct_excl_children << ", " << total_pct_excl_children / mNumQueries << ", " << mHitsReported; + Trace(str.str().c_str()); +} + +void NarrowPhaseStat::sReportStats() +{ + Trace("Query Type, Shape Type 1, Shape Type 2, Num Queries, Total Time (%%), Total Time Excl Children (%%), Total Time Excl. Children / Query (%%), Hits Reported"); + + uint64 total_ticks = 0; + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &collide_stat = sCollideShape[(int)t1][(int)t2]; + total_ticks += collide_stat.mTotalTicks - collide_stat.mChildTicks; + + const NarrowPhaseStat &cast_stat = sCastShape[(int)t1][(int)t2]; + total_ticks += cast_stat.mTotalTicks - cast_stat.mChildTicks; + } + + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &stat = sCollideShape[(int)t1][(int)t2]; + if (stat.mNumQueries > 0) + stat.ReportStats("CollideShape", t1, t2, total_ticks); + } + + for (EShapeSubType t1 : sAllSubShapeTypes) + for (EShapeSubType t2 : sAllSubShapeTypes) + { + const NarrowPhaseStat &stat = sCastShape[(int)t1][(int)t2]; + if (stat.mNumQueries > 0) + stat.ReportStats("CastShape", t1, t2, total_ticks); + } +} + +JPH_NAMESPACE_END + +#endif // JPH_TRACK_NARROWPHASE_STATS diff --git a/WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.h b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.h new file mode 100644 index 000000000..813933bb7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/NarrowPhaseStats.h @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wc++98-compat-pedantic") + +// Shorthand function to ifdef out code if narrow phase stats tracking is off +#ifdef JPH_TRACK_NARROWPHASE_STATS + #define JPH_IF_TRACK_NARROWPHASE_STATS(...) __VA_ARGS__ +#else + #define JPH_IF_TRACK_NARROWPHASE_STATS(...) +#endif // JPH_TRACK_NARROWPHASE_STATS + +JPH_SUPPRESS_WARNING_POP + +#ifdef JPH_TRACK_NARROWPHASE_STATS + +JPH_NAMESPACE_BEGIN + +/// Structure that tracks narrow phase timing information for a particular combination of shapes +class NarrowPhaseStat +{ +public: + /// Trace an individual stat in CSV form. + void ReportStats(const char *inName, EShapeSubType inType1, EShapeSubType inType2, uint64 inTicks100Pct) const; + + /// Trace the collected broadphase stats in CSV form. + /// This report can be used to judge and tweak the efficiency of the broadphase. + static void sReportStats(); + + atomic mNumQueries = 0; + atomic mHitsReported = 0; + atomic mTotalTicks = 0; + atomic mChildTicks = 0; + + static NarrowPhaseStat sCollideShape[NumSubShapeTypes][NumSubShapeTypes]; + static NarrowPhaseStat sCastShape[NumSubShapeTypes][NumSubShapeTypes]; +}; + +/// Object that tracks the start and end of a narrow phase operation +class TrackNarrowPhaseStat +{ +public: + TrackNarrowPhaseStat(NarrowPhaseStat &inStat) : + mStat(inStat), + mParent(sRoot), + mStart(GetProcessorTickCount()) + { + // Make this the new root of the chain + sRoot = this; + } + + ~TrackNarrowPhaseStat() + { + uint64 delta_ticks = GetProcessorTickCount() - mStart; + + // Notify parent of time spent in child + if (mParent != nullptr) + mParent->mStat.mChildTicks += delta_ticks; + + // Increment stats at this level + mStat.mNumQueries++; + mStat.mTotalTicks += delta_ticks; + + // Restore root pointer + JPH_ASSERT(sRoot == this); + sRoot = mParent; + } + + NarrowPhaseStat & mStat; + TrackNarrowPhaseStat * mParent; + uint64 mStart; + + static thread_local TrackNarrowPhaseStat *sRoot; +}; + +/// Object that tracks the start and end of a hit being processed by a collision collector +class TrackNarrowPhaseCollector +{ +public: + TrackNarrowPhaseCollector() : + mStart(GetProcessorTickCount()) + { + } + + ~TrackNarrowPhaseCollector() + { + // Mark time spent in collector as 'child' time for the parent + uint64 delta_ticks = GetProcessorTickCount() - mStart; + if (TrackNarrowPhaseStat::sRoot != nullptr) + TrackNarrowPhaseStat::sRoot->mStat.mChildTicks += delta_ticks; + + // Notify all parents of a hit + for (TrackNarrowPhaseStat *track = TrackNarrowPhaseStat::sRoot; track != nullptr; track = track->mParent) + track->mStat.mHitsReported++; + } + +private: + uint64 mStart; +}; + +JPH_NAMESPACE_END + +#endif // JPH_TRACK_NARROWPHASE_STATS diff --git a/WickedEngine/Jolt/Physics/Collision/ObjectLayer.h b/WickedEngine/Jolt/Physics/Collision/ObjectLayer.h new file mode 100644 index 000000000..f684e09e6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ObjectLayer.h @@ -0,0 +1,111 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Layer that objects can be in, determines which other objects it can collide with +#ifndef JPH_OBJECT_LAYER_BITS + #define JPH_OBJECT_LAYER_BITS 16 +#endif // JPH_OBJECT_LAYER_BITS +#if JPH_OBJECT_LAYER_BITS == 16 + using ObjectLayer = uint16; +#elif JPH_OBJECT_LAYER_BITS == 32 + using ObjectLayer = uint32; +#else + #error "JPH_OBJECT_LAYER_BITS must be 16 or 32" +#endif + +/// Constant value used to indicate an invalid object layer +static constexpr ObjectLayer cObjectLayerInvalid = ObjectLayer(~ObjectLayer(0U)); + +/// Filter class for object layers +class ObjectLayerFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectLayerFilter() = default; + + /// Function to filter out object layers when doing collision query test (return true to allow testing against objects with this layer) + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer) const + { + return true; + } + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Get a string that describes this filter for stat tracking purposes + virtual String GetDescription() const + { + return "No Description"; + } +#endif // JPH_TRACK_BROADPHASE_STATS +}; + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +class ObjectLayerPairFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ObjectLayerPairFilter() = default; + + /// Returns true if two layers can collide + virtual bool ShouldCollide([[maybe_unused]] ObjectLayer inLayer1, [[maybe_unused]] ObjectLayer inLayer2) const + { + return true; + } +}; + +/// Default filter class that uses the pair filter in combination with a specified layer to filter layers +class DefaultObjectLayerFilter : public ObjectLayerFilter +{ +public: + /// Constructor + DefaultObjectLayerFilter(const ObjectLayerPairFilter &inObjectLayerPairFilter, ObjectLayer inLayer) : + mObjectLayerPairFilter(inObjectLayerPairFilter), + mLayer(inLayer) + { + } + + /// Copy constructor + DefaultObjectLayerFilter(const DefaultObjectLayerFilter &inRHS) : + mObjectLayerPairFilter(inRHS.mObjectLayerPairFilter), + mLayer(inRHS.mLayer) + { + } + + // See ObjectLayerFilter::ShouldCollide + virtual bool ShouldCollide(ObjectLayer inLayer) const override + { + return mObjectLayerPairFilter.ShouldCollide(mLayer, inLayer); + } + +private: + const ObjectLayerPairFilter & mObjectLayerPairFilter; + ObjectLayer mLayer; +}; + +/// Allows objects from a specific layer only +class SpecifiedObjectLayerFilter : public ObjectLayerFilter +{ +public: + /// Constructor + explicit SpecifiedObjectLayerFilter(ObjectLayer inLayer) : + mLayer(inLayer) + { + } + + // See ObjectLayerFilter::ShouldCollide + virtual bool ShouldCollide(ObjectLayer inLayer) const override + { + return mLayer == inLayer; + } + +private: + ObjectLayer mLayer; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h b/WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h new file mode 100644 index 000000000..dc3494c2e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterMask.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +/// Uses group bits and mask bits. Two layers can collide if Object1.Group & Object2.Mask is non-zero and Object2.Group & Object1.Mask is non-zero. +/// The behavior is similar to that in e.g. Bullet. +/// This implementation works together with BroadPhaseLayerInterfaceMask and ObjectVsBroadPhaseLayerFilterMask +class ObjectLayerPairFilterMask : public ObjectLayerPairFilter +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Number of bits for the group and mask bits + static constexpr uint32 cNumBits = JPH_OBJECT_LAYER_BITS / 2; + static constexpr uint32 cMask = (1 << cNumBits) - 1; + + /// Construct an ObjectLayer from a group and mask bits + static ObjectLayer sGetObjectLayer(uint32 inGroup, uint32 inMask = cMask) + { + JPH_ASSERT((inGroup & ~cMask) == 0); + JPH_ASSERT((inMask & ~cMask) == 0); + return ObjectLayer((inGroup & cMask) | (inMask << cNumBits)); + } + + /// Get the group bits from an ObjectLayer + static inline uint32 sGetGroup(ObjectLayer inObjectLayer) + { + return uint32(inObjectLayer) & cMask; + } + + /// Get the mask bits from an ObjectLayer + static inline uint32 sGetMask(ObjectLayer inObjectLayer) + { + return uint32(inObjectLayer) >> cNumBits; + } + + /// Returns true if two layers can collide + virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override + { + return (sGetGroup(inObject1) & sGetMask(inObject2)) != 0 + && (sGetGroup(inObject2) & sGetMask(inObject1)) != 0; + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h b/WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h new file mode 100644 index 000000000..1d62178af --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ObjectLayerPairFilterTable.h @@ -0,0 +1,78 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Filter class to test if two objects can collide based on their object layer. Used while finding collision pairs. +/// This implementation uses a table to determine if two layers can collide. +class ObjectLayerPairFilterTable : public ObjectLayerPairFilter +{ +private: + /// Get which bit corresponds to the pair (inLayer1, inLayer2) + uint GetBit(ObjectLayer inLayer1, ObjectLayer inLayer2) const + { + // We store the lower left half only, so swap the inputs when trying to access the top right half + if (inLayer1 > inLayer2) + swap(inLayer1, inLayer2); + + JPH_ASSERT(inLayer2 < mNumObjectLayers); + + // Calculate at which bit the entry for this pair resides + // We use the fact that a row always starts at inLayer2 * (inLayer2 + 1) / 2 + // (this is the amount of bits needed to store a table of inLayer2 entries) + return (inLayer2 * (inLayer2 + 1)) / 2 + inLayer1; + } + +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructs the table with inNumObjectLayers Layers, initially all layer pairs are disabled + explicit ObjectLayerPairFilterTable(uint inNumObjectLayers) : + mNumObjectLayers(inNumObjectLayers) + { + // By default nothing collides + // For the first layer we only need to store 1 bit, for the second 2 bits, for the third 3 bits, etc. + // We use the formula Sum_i=1^N i = N * (N + 1) / 2 to calculate the size of the table + int table_size = (inNumObjectLayers * (inNumObjectLayers + 1) / 2 + 7) / 8; + mTable.resize(table_size, 0); + } + + /// Get the number of object layers + uint GetNumObjectLayers() const + { + return mNumObjectLayers; + } + + /// Disable collision between two object layers + void DisableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2) + { + uint bit = GetBit(inLayer1, inLayer2); + mTable[bit >> 3] &= (0xff ^ (1 << (bit & 0b111))); + } + + /// Enable collision between two object layers + void EnableCollision(ObjectLayer inLayer1, ObjectLayer inLayer2) + { + uint bit = GetBit(inLayer1, inLayer2); + mTable[bit >> 3] |= 1 << (bit & 0b111); + } + + /// Returns true if two layers can collide + virtual bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override + { + // Test if the bit is set for this group pair + uint bit = GetBit(inObject1, inObject2); + return (mTable[bit >> 3] & (1 << (bit & 0b111))) != 0; + } + +private: + uint mNumObjectLayers; ///< The number of layers that this table supports + Array mTable; ///< The table of bits that indicates which layers collide +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.cpp b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.cpp new file mode 100644 index 000000000..17a982e74 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.cpp @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +RefConst PhysicsMaterial::sDefault; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterial) +{ + JPH_ADD_BASE_CLASS(PhysicsMaterial, SerializableObject) +} + +void PhysicsMaterial::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); +} + +void PhysicsMaterial::RestoreBinaryState(StreamIn &inStream) +{ + // RTTI hash is read in sRestoreFromBinaryState +} + +PhysicsMaterial::PhysicsMaterialResult PhysicsMaterial::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &PhysicsMaterial::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.h b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.h new file mode 100644 index 000000000..d24e344d1 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterial.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// This structure describes the surface of (part of) a shape. You should inherit from it to define additional +/// information that is interesting for the simulation. The 2 materials involved in a contact could be used +/// to decide which sound or particle effects to play. +/// +/// If you inherit from this material, don't forget to create a suitable default material in sDefault +class JPH_EXPORT PhysicsMaterial : public SerializableObject, public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterial) + + /// Virtual destructor + virtual ~PhysicsMaterial() override = default; + + /// Default material that is used when a shape has no materials defined + static RefConst sDefault; + + // Properties + virtual const char * GetDebugName() const { return "Unknown"; } + virtual Color GetDebugColor() const { return Color::sGrey; } + + /// Saves the contents of the material in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + using PhysicsMaterialResult = Result>; + + /// Creates a PhysicsMaterial of the correct type and restores its contents from the binary stream inStream. + static PhysicsMaterialResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +using PhysicsMaterialList = Array>; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp new file mode 100644 index 000000000..02a569822 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.cpp @@ -0,0 +1,38 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PhysicsMaterialSimple) +{ + JPH_ADD_BASE_CLASS(PhysicsMaterialSimple, PhysicsMaterial) + + JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugName) + JPH_ADD_ATTRIBUTE(PhysicsMaterialSimple, mDebugColor) +} + +void PhysicsMaterialSimple::SaveBinaryState(StreamOut &inStream) const +{ + PhysicsMaterial::SaveBinaryState(inStream); + + inStream.Write(mDebugName); + inStream.Write(mDebugColor); +} + +void PhysicsMaterialSimple::RestoreBinaryState(StreamIn &inStream) +{ + PhysicsMaterial::RestoreBinaryState(inStream); + + inStream.Read(mDebugName); + inStream.Read(mDebugColor); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.h b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.h new file mode 100644 index 000000000..21c1e634a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/PhysicsMaterialSimple.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Sample implementation of PhysicsMaterial that just holds the needed properties directly +class JPH_EXPORT PhysicsMaterialSimple : public PhysicsMaterial +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PhysicsMaterialSimple) + + /// Constructor + PhysicsMaterialSimple() = default; + PhysicsMaterialSimple(const string_view &inName, ColorArg inColor) : mDebugName(inName), mDebugColor(inColor) { } + + // Properties + virtual const char * GetDebugName() const override { return mDebugName.c_str(); } + virtual Color GetDebugColor() const override { return mDebugColor; } + + // See: PhysicsMaterial::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + +protected: + // See: PhysicsMaterial::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + String mDebugName; ///< Name of the material, used for debugging purposes + Color mDebugColor = Color::sGrey; ///< Color of the material, used to render the shapes +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/RayCast.h b/WickedEngine/Jolt/Physics/Collision/RayCast.h new file mode 100644 index 000000000..052c81529 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/RayCast.h @@ -0,0 +1,81 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a single ray cast +template +struct RayCastT +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructors + RayCastT() = default; // Allow raycast to be created uninitialized + RayCastT(typename Vec::ArgType inOrigin, Vec3Arg inDirection) : mOrigin(inOrigin), mDirection(inDirection) { } + RayCastT(const RayCastT &) = default; + + /// Transform this ray using inTransform + RayCastType Transformed(typename Mat::ArgType inTransform) const + { + Vec ray_origin = inTransform * mOrigin; + Vec3 ray_direction(inTransform * (mOrigin + mDirection) - ray_origin); + return { ray_origin, ray_direction }; + } + + /// Translate ray using inTranslation + RayCastType Translated(typename Vec::ArgType inTranslation) const + { + return { inTranslation + mOrigin, mDirection }; + } + + /// Get point with fraction inFraction on ray (0 = start of ray, 1 = end of ray) + inline Vec GetPointOnRay(float inFraction) const + { + return mOrigin + inFraction * mDirection; + } + + Vec mOrigin; ///< Origin of the ray + Vec3 mDirection; ///< Direction and length of the ray (anything beyond this length will not be reported as a hit) +}; + +struct RayCast : public RayCastT +{ + using RayCastT::RayCastT; +}; + +struct RRayCast : public RayCastT +{ + using RayCastT::RayCastT; + + /// Convert from RayCast, converts single to double precision + explicit RRayCast(const RayCast &inRay) : + RRayCast(RVec3(inRay.mOrigin), inRay.mDirection) + { + } + + /// Convert to RayCast, which implies casting from double precision to single precision + explicit operator RayCast() const + { + return RayCast(Vec3(mOrigin), mDirection); + } +}; + +/// Settings to be passed with a ray cast +class RayCastSettings +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// How backfacing triangles should be treated + EBackFaceMode mBackFaceMode = EBackFaceMode::IgnoreBackFaces; + + /// If convex shapes should be treated as solid. When true, a ray starting inside a convex shape will generate a hit at fraction 0. + bool mTreatConvexAsSolid = true; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.cpp new file mode 100644 index 000000000..da6b08c4f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.cpp @@ -0,0 +1,318 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(BoxShapeSettings) +{ + JPH_ADD_BASE_CLASS(BoxShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(BoxShapeSettings, mHalfExtent) + JPH_ADD_ATTRIBUTE(BoxShapeSettings, mConvexRadius) +} + +static const Vec3 sUnitBoxTriangles[] = { + Vec3(-1, 1, -1), Vec3(-1, 1, 1), Vec3(1, 1, 1), + Vec3(-1, 1, -1), Vec3(1, 1, 1), Vec3(1, 1, -1), + Vec3(-1, -1, -1), Vec3(1, -1, -1), Vec3(1, -1, 1), + Vec3(-1, -1, -1), Vec3(1, -1, 1), Vec3(-1, -1, 1), + Vec3(-1, 1, -1), Vec3(-1, -1, -1), Vec3(-1, -1, 1), + Vec3(-1, 1, -1), Vec3(-1, -1, 1), Vec3(-1, 1, 1), + Vec3(1, 1, 1), Vec3(1, -1, 1), Vec3(1, -1, -1), + Vec3(1, 1, 1), Vec3(1, -1, -1), Vec3(1, 1, -1), + Vec3(-1, 1, 1), Vec3(-1, -1, 1), Vec3(1, -1, 1), + Vec3(-1, 1, 1), Vec3(1, -1, 1), Vec3(1, 1, 1), + Vec3(-1, 1, -1), Vec3(1, 1, -1), Vec3(1, -1, -1), + Vec3(-1, 1, -1), Vec3(1, -1, -1), Vec3(-1, -1, -1) +}; + +ShapeSettings::ShapeResult BoxShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new BoxShape(*this, mCachedResult); + return mCachedResult; +} + +BoxShape::BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Box, inSettings, outResult), + mHalfExtent(inSettings.mHalfExtent), + mConvexRadius(inSettings.mConvexRadius) +{ + // Check convex radius + if (inSettings.mConvexRadius < 0.0f + || inSettings.mHalfExtent.ReduceMin() <= inSettings.mConvexRadius) + { + outResult.SetError("Invalid convex radius"); + return; + } + + // Result is valid + outResult.Set(this); +} + +class BoxShape::Box final : public Support +{ +public: + Box(const AABox &inBox, float inConvexRadius) : + mBox(inBox), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(Box) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(Box))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return mBox.GetSupport(inDirection); + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + AABox mBox; + float mConvexRadius; +}; + +const ConvexShape::Support *BoxShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + // Scale our half extents + Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + { + // Make box out of our half extents + AABox box = AABox(-scaled_half_extent, scaled_half_extent); + JPH_ASSERT(box.IsValid()); + return new (&inBuffer) Box(box, 0.0f); + } + + case ESupportMode::ExcludeConvexRadius: + { + // Reduce the box by our convex radius + float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + Vec3 convex_radius3 = Vec3::sReplicate(convex_radius); + Vec3 reduced_half_extent = scaled_half_extent - convex_radius3; + AABox box = AABox(-reduced_half_extent, reduced_half_extent); + JPH_ASSERT(box.IsValid()); + return new (&inBuffer) Box(box, convex_radius); + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void BoxShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 scaled_half_extent = inScale.Abs() * mHalfExtent; + AABox box(-scaled_half_extent, scaled_half_extent); + box.GetSupportingFace(inDirection, outVertices); + + // Transform to world space + for (Vec3 &v : outVertices) + v = inCenterOfMassTransform * v; +} + +MassProperties BoxShape::GetMassProperties() const +{ + MassProperties p; + p.SetMassAndInertiaOfSolidBox(2.0f * mHalfExtent, GetDensity()); + return p; +} + +Vec3 BoxShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Get component that is closest to the surface of the box + int index = (inLocalSurfacePosition.Abs() - mHalfExtent).Abs().GetLowestComponentIndex(); + + // Calculate normal + Vec3 normal = Vec3::sZero(); + normal.SetComponent(index, inLocalSurfacePosition[index] > 0.0f? 1.0f : -1.0f); + return normal; +} + +#ifdef JPH_DEBUG_RENDERER +void BoxShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawBox(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), GetLocalBounds(), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool BoxShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test hit against box + float fraction = max(RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent), 0.0f); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void BoxShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float min_fraction, max_fraction; + RayAABox(inRay.mOrigin, RayInvDirection(inRay.mDirection), -mHalfExtent, mHalfExtent, min_fraction, max_fraction); + if (min_fraction <= max_fraction // Ray should intersect + && max_fraction >= 0.0f // End of ray should be inside box + && min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = max(0.0f, min_fraction); + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces + && max_fraction < ioCollector.GetEarlyOutFraction()) + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void BoxShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + if (Vec3::sLessOrEqual(inPoint.Abs(), mHalfExtent).TestAllXYZTrue()) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void BoxShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + Vec3 half_extent = inScale.Abs() * mHalfExtent; + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + // Convert to local space + Vec3 local_pos = inverse_transform * v->mPosition; + + // Clamp point to inside box + Vec3 clamped_point = Vec3::sMax(Vec3::sMin(local_pos, half_extent), -half_extent); + + // Test if point was inside + if (clamped_point == local_pos) + { + // Calculate closest distance to surface + Vec3 delta = half_extent - local_pos.Abs(); + int index = delta.GetLowestComponentIndex(); + float penetration = delta[index]; + if (penetration > v->mLargestPenetration) + { + v->mLargestPenetration = penetration; + + // Calculate contact point and normal + Vec3 possible_normals[] = { Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ() }; + Vec3 normal = local_pos.GetSign() * possible_normals[index]; + Vec3 point = normal * half_extent; + + // Store collision + v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } + else + { + // Calculate normal + Vec3 normal = local_pos - clamped_point; + float normal_length = normal.Length(); + + // Penetration will be negative since we're not penetrating + float penetration = -normal_length; + if (penetration > v->mLargestPenetration) + { + normal /= normal_length; + + v->mLargestPenetration = penetration; + + // Store collision + v->mCollisionPlane = Plane::sFromPointAndNormal(clamped_point, normal).GetTransformed(inCenterOfMassTransform); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } + } +} + +void BoxShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, Mat44::sScale(mHalfExtent), sUnitBoxTriangles, sizeof(sUnitBoxTriangles) / sizeof(Vec3), GetMaterial()); +} + +int BoxShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void BoxShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mHalfExtent); + inStream.Write(mConvexRadius); +} + +void BoxShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mHalfExtent); + inStream.Read(mConvexRadius); +} + +void BoxShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Box); + f.mConstruct = []() -> Shape * { return new BoxShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.h new file mode 100644 index 000000000..dc53772d9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/BoxShape.h @@ -0,0 +1,115 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a BoxShape +class JPH_EXPORT BoxShapeSettings final : public ConvexShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, BoxShapeSettings) + + /// Default constructor for deserialization + BoxShapeSettings() = default; + + /// Create a box with half edge length inHalfExtent and convex radius inConvexRadius. + /// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius). + BoxShapeSettings(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mHalfExtent = Vec3::sZero(); ///< Half the size of the box (including convex radius) + float mConvexRadius = 0.0f; +}; + +/// A box, centered around the origin +class JPH_EXPORT BoxShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + BoxShape() : ConvexShape(EShapeSubType::Box) { } + BoxShape(const BoxShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a box with half edge length inHalfExtent and convex radius inConvexRadius. + /// (internally the convex radius will be subtracted from the half extent so the total box will not grow with the convex radius). + BoxShape(Vec3Arg inHalfExtent, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Box, inMaterial), mHalfExtent(inHalfExtent), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); JPH_ASSERT(inHalfExtent.ReduceMin() >= inConvexRadius); } + + /// Get half extent of box + Vec3 GetHalfExtent() const { return mHalfExtent; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return AABox(-mHalfExtent, mHalfExtent); } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mHalfExtent.ReduceMin(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 12); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return GetLocalBounds().GetVolume(); } + + /// Get the convex radius of this box + float GetConvexRadius() const { return mConvexRadius; } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class Box; + + Vec3 mHalfExtent = Vec3::sZero(); ///< Half the size of the box (including convex radius) + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.cpp new file mode 100644 index 000000000..1032505f2 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.cpp @@ -0,0 +1,446 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CapsuleShapeSettings) +{ + JPH_ADD_BASE_CLASS(CapsuleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mRadius) + JPH_ADD_ATTRIBUTE(CapsuleShapeSettings, mHalfHeightOfCylinder) +} + +static const int cCapsuleDetailLevel = 2; + +static const StaticArray sCapsuleTopTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, cCapsuleDetailLevel); + return verts; +}(); + +static const StaticArray sCapsuleMiddleTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateUnitOpenCylinder(verts, cCapsuleDetailLevel); + return verts; +}(); + +static const StaticArray sCapsuleBottomTriangles = []() { + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, cCapsuleDetailLevel); + return verts; +}(); + +ShapeSettings::ShapeResult CapsuleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (IsValid() && IsSphere()) + { + // If the capsule has no height, use a sphere instead + shape = new SphereShape(mRadius, mMaterial); + mCachedResult.Set(shape); + } + else + shape = new CapsuleShape(*this, mCachedResult); + } + return mCachedResult; +} + +CapsuleShape::CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Capsule, inSettings, outResult), + mRadius(inSettings.mRadius), + mHalfHeightOfCylinder(inSettings.mHalfHeightOfCylinder) +{ + if (inSettings.mHalfHeightOfCylinder <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mRadius <= 0.0f) + { + outResult.SetError("Invalid radius"); + return; + } + + outResult.Set(this); +} + +class CapsuleShape::CapsuleNoConvex final : public Support +{ +public: + CapsuleNoConvex(Vec3Arg inHalfHeightOfCylinder, float inConvexRadius) : + mHalfHeightOfCylinder(inHalfHeightOfCylinder), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(CapsuleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(CapsuleNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + if (inDirection.GetY() > 0) + return mHalfHeightOfCylinder; + else + return -mHalfHeightOfCylinder; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + Vec3 mHalfHeightOfCylinder; + float mConvexRadius; +}; + +class CapsuleShape::CapsuleWithConvex final : public Support +{ +public: + CapsuleWithConvex(Vec3Arg inHalfHeightOfCylinder, float inRadius) : + mHalfHeightOfCylinder(inHalfHeightOfCylinder), + mRadius(inRadius) + { + static_assert(sizeof(CapsuleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(CapsuleWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float len = inDirection.Length(); + Vec3 radius = len > 0.0f? inDirection * (mRadius / len) : Vec3::sZero(); + + if (inDirection.GetY() > 0) + return radius + mHalfHeightOfCylinder; + else + return radius - mHalfHeightOfCylinder; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + Vec3 mHalfHeightOfCylinder; + float mRadius; +}; + +const ConvexShape::Support *CapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled capsule + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0); + float scaled_radius = scale * mRadius; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) CapsuleWithConvex(scaled_half_height_of_cylinder, scaled_radius); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) CapsuleNoConvex(scaled_half_height_of_cylinder, scaled_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +void CapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get direction in horizontal plane + Vec3 direction = inDirection; + direction.SetComponent(1, 0.0f); + + // Check zero vector, in this case we're hitting from top/bottom so there's no supporting face + float len = direction.Length(); + if (len == 0.0f) + return; + + // Get scaled capsule + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 scaled_half_height_of_cylinder = Vec3(0, scale * mHalfHeightOfCylinder, 0); + float scaled_radius = scale * mRadius; + + // Get support point for top and bottom sphere in the opposite of 'direction' (including convex radius) + Vec3 support = (scaled_radius / len) * direction; + Vec3 support_top = scaled_half_height_of_cylinder - support; + Vec3 support_bottom = -scaled_half_height_of_cylinder - support; + + // Get projection on inDirection + // Note that inDirection is not normalized, so we need to divide by inDirection.Length() to get the actual projection + // We've multiplied both sides of the if below with inDirection.Length() + float proj_top = support_top.Dot(inDirection); + float proj_bottom = support_bottom.Dot(inDirection); + + // If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point + if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * inDirection.Length()) + { + outVertices.push_back(inCenterOfMassTransform * support_top); + outVertices.push_back(inCenterOfMassTransform * support_bottom); + } +} + +MassProperties CapsuleShape::GetMassProperties() const +{ + MassProperties p; + + float density = GetDensity(); + + // Calculate inertia and mass according to: + // https://www.gamedev.net/resources/_/technical/math-and-physics/capsule-inertia-tensor-r3856 + // Note that there is an error in eq 14, H^2/2 should be H^2/4 in Ixx and Izz, eq 12 does contain the correct value + float radius_sq = Square(mRadius); + float height = 2.0f * mHalfHeightOfCylinder; + float cylinder_mass = JPH_PI * height * radius_sq * density; + float hemisphere_mass = (2.0f * JPH_PI / 3.0f) * radius_sq * mRadius * density; + + // From cylinder + float height_sq = Square(height); + float inertia_y = radius_sq * cylinder_mass * 0.5f; + float inertia_xz = inertia_y * 0.5f + cylinder_mass * height_sq / 12.0f; + + // From hemispheres + float temp = hemisphere_mass * 4.0f * radius_sq / 5.0f; + inertia_y += temp; + inertia_xz += temp + hemisphere_mass * (0.5f * height_sq + (3.0f / 4.0f) * height * mRadius); + + // Mass is cylinder + hemispheres + p.mMass = cylinder_mass + hemisphere_mass * 2.0f; + + // Set inertia + p.mInertia = Mat44::sScale(Vec3(inertia_xz, inertia_y, inertia_xz)); + + return p; +} + +Vec3 CapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + if (inLocalSurfacePosition.GetY() > mHalfHeightOfCylinder) + return (inLocalSurfacePosition - Vec3(0, mHalfHeightOfCylinder, 0)).Normalized(); + else if (inLocalSurfacePosition.GetY() < -mHalfHeightOfCylinder) + return (inLocalSurfacePosition - Vec3(0, -mHalfHeightOfCylinder, 0)).Normalized(); + else + return Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX()); +} + +AABox CapsuleShape::GetLocalBounds() const +{ + Vec3 extent = Vec3::sReplicate(mRadius) + Vec3(0, mHalfHeightOfCylinder, 0); + return AABox(-extent, extent); +} + +AABox CapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + Vec3 extent = Vec3::sReplicate(scale * mRadius); + Vec3 height = Vec3(0, scale * mHalfHeightOfCylinder, 0); + Vec3 p1 = inCenterOfMassTransform * -height; + Vec3 p2 = inCenterOfMassTransform * height; + return AABox(Vec3::sMin(p1, p2) - extent, Vec3::sMax(p1, p2) + extent); +} + +#ifdef JPH_DEBUG_RENDERER +void CapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawCapsule(inCenterOfMassTransform * Mat44::sScale(inScale.Abs().GetX()), mHalfHeightOfCylinder, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool CapsuleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test ray against capsule + float fraction = RayCapsule(inRay.mOrigin, inRay.mDirection, mHalfHeightOfCylinder, mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void CapsuleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float radius_sq = Square(mRadius); + + // Get vertical distance to the top/bottom sphere centers + float delta_y = abs(inPoint.GetY()) - mHalfHeightOfCylinder; + + // Get distance in horizontal plane + float xz_sq = Square(inPoint.GetX()) + Square(inPoint.GetZ()); + + // Check if the point is in one of the two spheres + bool in_sphere = xz_sq + Square(delta_y) <= radius_sq; + + // Check if the point is in the cylinder in the middle + bool in_cylinder = delta_y <= 0.0f && xz_sq <= radius_sq; + + if (in_sphere || in_cylinder) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void CapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled capsule + float scale = abs(inScale.GetX()); + float half_height_of_cylinder = scale * mHalfHeightOfCylinder; + float radius = scale * mRadius; + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + // Calculate penetration + Vec3 local_pos = inverse_transform * v->mPosition; + if (abs(local_pos.GetY()) <= half_height_of_cylinder) + { + // Near cylinder + Vec3 normal = local_pos; + normal.SetY(0.0f); + float normal_length = normal.Length(); + float penetration = radius - normal_length; + if (penetration > v->mLargestPenetration) + { + v->mLargestPenetration = penetration; + + // Calculate contact point and normal + normal = normal_length > 0.0f? normal / normal_length : Vec3::sAxisX(); + Vec3 point = radius * normal; + + // Store collision + v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } + else + { + // Near cap + Vec3 center = Vec3(0, Sign(local_pos.GetY()) * half_height_of_cylinder, 0); + Vec3 delta = local_pos - center; + float distance = delta.Length(); + float penetration = radius - distance; + if (penetration > v->mLargestPenetration) + { + v->mLargestPenetration = penetration; + + // Calculate contact point and normal + Vec3 normal = delta / distance; + Vec3 point = center + radius * normal; + + // Store collision + v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal).GetTransformed(inCenterOfMassTransform); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } + } +} + +void CapsuleShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + ts.SetShapeScale(ScaleHelpers::MakeUniformScale(scale.Abs())); + ioCollector.AddHit(ts); +} + +void CapsuleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale = abs_scale.GetX(); + + GetTrianglesContextMultiVertexList *context = new (&ioContext) GetTrianglesContextMultiVertexList(false, GetMaterial()); + + Mat44 world_matrix = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale); + + Mat44 top_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, mHalfHeightOfCylinder, 0, 1)); + context->AddPart(top_matrix, sCapsuleTopTriangles.data(), sCapsuleTopTriangles.size()); + + Mat44 middle_matrix = world_matrix * Mat44::sScale(Vec3(mRadius, mHalfHeightOfCylinder, mRadius)); + context->AddPart(middle_matrix, sCapsuleMiddleTriangles.data(), sCapsuleMiddleTriangles.size()); + + Mat44 bottom_matrix = world_matrix * Mat44(Vec4(mRadius, 0, 0, 0), Vec4(0, mRadius, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, -mHalfHeightOfCylinder, 0, 1)); + context->AddPart(bottom_matrix, sCapsuleBottomTriangles.data(), sCapsuleBottomTriangles.size()); +} + +int CapsuleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextMultiVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void CapsuleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mRadius); + inStream.Write(mHalfHeightOfCylinder); +} + +void CapsuleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mRadius); + inStream.Read(mHalfHeightOfCylinder); +} + +bool CapsuleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +void CapsuleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Capsule); + f.mConstruct = []() -> Shape * { return new CapsuleShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.h new file mode 100644 index 000000000..73281d0fe --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/CapsuleShape.h @@ -0,0 +1,128 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a CapsuleShape +class JPH_EXPORT CapsuleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CapsuleShapeSettings) + + /// Default constructor for deserialization + CapsuleShapeSettings() = default; + + /// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0) + CapsuleShapeSettings(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { } + + /// Check if this is a valid capsule shape + bool IsValid() const { return mRadius > 0.0f && mHalfHeightOfCylinder >= 0.0f; } + + /// Checks if the settings of this capsule make this shape a sphere + bool IsSphere() const { return mHalfHeightOfCylinder == 0.0f; } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mRadius = 0.0f; + float mHalfHeightOfCylinder = 0.0f; +}; + +/// A capsule, implemented as a line segment with convex radius +class JPH_EXPORT CapsuleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CapsuleShape() : ConvexShape(EShapeSubType::Capsule) { } + CapsuleShape(const CapsuleShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfCylinder, 0) and the other at (0, inHalfHeightOfCylinder, 0) + CapsuleShape(float inHalfHeightOfCylinder, float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Capsule, inMaterial), mRadius(inRadius), mHalfHeightOfCylinder(inHalfHeightOfCylinder) { JPH_ASSERT(inHalfHeightOfCylinder > 0.0f); JPH_ASSERT(inRadius > 0.0f); } + + /// Radius of the cylinder + float GetRadius() const { return mRadius; } + + /// Get half of the height of the cylinder + float GetHalfHeightOfCylinder() const { return mHalfHeightOfCylinder; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + using ConvexShape::CastRay; + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 4.0f / 3.0f * JPH_PI * Cubed(mRadius) + 2.0f * JPH_PI * mHalfHeightOfCylinder * Square(mRadius); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Classes for GetSupportFunction + class CapsuleNoConvex; + class CapsuleWithConvex; + + float mRadius = 0.0f; + float mHalfHeightOfCylinder = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.cpp new file mode 100644 index 000000000..20a61f28a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.cpp @@ -0,0 +1,398 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(CompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(CompoundShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(CompoundShapeSettings, mSubShapes) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CompoundShapeSettings::SubShapeSettings) +{ + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mShape) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mPosition) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mRotation) + JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mUserData) +} + +void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData) +{ + // Add shape + SubShapeSettings shape; + shape.mPosition = inPosition; + shape.mRotation = inRotation; + shape.mShape = inShape; + shape.mUserData = inUserData; + mSubShapes.push_back(shape); +} + +void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData) +{ + // Add shape + SubShapeSettings shape; + shape.mPosition = inPosition; + shape.mRotation = inRotation; + shape.mShapePtr = inShape; + shape.mUserData = inUserData; + mSubShapes.push_back(shape); +} + +bool CompoundShape::MustBeStatic() const +{ + for (const SubShape &shape : mSubShapes) + if (shape.mShape->MustBeStatic()) + return true; + + return false; +} + +MassProperties CompoundShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass and inertia + p.mMass = 0.0f; + p.mInertia = Mat44::sZero(); + for (const SubShape &shape : mSubShapes) + { + // Rotate and translate inertia of child into place + MassProperties child = shape.mShape->GetMassProperties(); + child.Rotate(Mat44::sRotation(shape.GetRotation())); + child.Translate(shape.GetPositionCOM()); + + // Accumulate mass and inertia + p.mMass += child.mMass; + p.mInertia += child.mInertia; + } + + // Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change + p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); + + return p; +} + +AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + if (mSubShapes.size() <= 10) + { + AABox bounds; + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale); + bounds.Encapsulate(shape.mShape->GetWorldSpaceBounds(transform, shape.TransformScale(inScale))); + } + return bounds; + } + else + { + // If there are too many shapes, use the base class function (this will result in a slightly wider bounding box) + return Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale); + } +} + +uint CompoundShape::GetSubShapeIDBitsRecursive() const +{ + // Add max of child bits to our bits + uint child_bits = 0; + for (const SubShape &shape : mSubShapes) + child_bits = max(child_bits, shape.mShape->GetSubShapeIDBitsRecursive()); + return child_bits + GetSubShapeIDBits(); +} + +const PhysicsMaterial *CompoundShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Pass call on + return mSubShapes[index].mShape->GetMaterial(remainder); +} + +uint64 CompoundShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + if (index >= mSubShapes.size()) + return 0; // No longer valid index + + // Pass call on + return mSubShapes[index].mShape->GetSubShapeUserData(remainder); +} + +TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // Get the sub shape + const SubShape &sub_shape = mSubShapes[GetSubShapeIndexFromID(inSubShapeID, outRemainder)]; + + // Calculate transform for sub shape + Vec3 position = inPositionCOM + inRotation * (inScale * sub_shape.GetPositionCOM()); + Quat rotation = inRotation * sub_shape.GetRotation(); + Vec3 scale = sub_shape.TransformScale(inScale); + + // Return transformed shape + TransformedShape ts(RVec3(position), rotation, sub_shape.mShape, BodyID()); + ts.SetShapeScale(scale); + return ts; +} + +Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Transform surface position to local space and pass call on + const SubShape &shape = mSubShapes[index]; + Mat44 transform = Mat44::sInverseRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()); + Vec3 normal = shape.mShape->GetSurfaceNormal(remainder, transform * inLocalSurfacePosition); + + // Transform normal to this shape's space + return transform.Multiply3x3Transposed(normal); +} + +void CompoundShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode sub shape index + SubShapeID remainder; + uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder); + + // Apply transform and pass on to sub shape + const SubShape &shape = mSubShapes[index]; + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->GetSupportingFace(remainder, transform.Multiply3x3Transposed(inDirection), shape.TransformScale(inScale), inCenterOfMassTransform * transform, outVertices); +} + +void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + outTotalVolume = 0.0f; + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + + for (const SubShape &shape : mSubShapes) + { + // Get center of mass transform of child + Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale); + + // Recurse to child + float total_volume, submerged_volume; + Vec3 center_of_buoyancy; + shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + // Accumulate volumes + outTotalVolume += total_volume; + outSubmergedVolume += submerged_volume; + + // The center of buoyancy is the weighted average of the center of buoyancy of our child shapes + outCenterOfBuoyancy += submerged_volume * center_of_buoyancy; + } + + if (outSubmergedVolume > 0.0f) + outCenterOfBuoyancy /= outSubmergedVolume; + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void CompoundShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->Draw(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe); + } +} + +void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inDrawSupportDirection); + } +} + +void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale)); + } +} +#endif // JPH_DEBUG_RENDERER + +void CompoundShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + for (const SubShape &shape : mSubShapes) + { + Mat44 transform = shape.GetLocalTransformNoScale(inScale); + shape.mShape->CollideSoftBodyVertices(inCenterOfMassTransform * transform, shape.TransformScale(inScale), ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); + } +} + +void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + for (const SubShape &shape : mSubShapes) + shape.mShape->TransformShape(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioCollector); +} + +void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Fetch compound shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound); + const CompoundShape *compound = static_cast(inShapeCast.mShape); + + // Number of sub shapes + int n = (int)compound->mSubShapes.size(); + + // Determine amount of bits for sub shape + uint sub_shape_bits = compound->GetSubShapeIDBits(); + + // Recurse to sub shapes + for (int i = 0; i < n; ++i) + { + const SubShape &shape = compound->mSubShapes[i]; + + // Create ID for sub shape + SubShapeIDCreator shape1_sub_shape_id = inSubShapeIDCreator1.PushID(i, sub_shape_bits); + + // Transform the shape cast and update the shape + Mat44 transform = inShapeCast.mCenterOfMassStart * shape.GetLocalTransformNoScale(inShapeCast.mScale); + Vec3 scale = shape.TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector); + + if (ioCollector.ShouldEarlyOut()) + break; + } +} + +void CompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mInnerRadius); + + // Write sub shapes + inStream.Write(mSubShapes, [](const SubShape &inElement, StreamOut &inS) { + inS.Write(inElement.mUserData); + inS.Write(inElement.mPositionCOM); + inS.Write(inElement.mRotation); + }); +} + +void CompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mInnerRadius); + + // Read sub shapes + inStream.Read(mSubShapes, [](StreamIn &inS, SubShape &outElement) { + inS.Read(outElement.mUserData); + inS.Read(outElement.mPositionCOM); + inS.Read(outElement.mRotation); + outElement.mIsRotationIdentity = outElement.mRotation == Float3(0, 0, 0); + }); +} + +void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const +{ + outSubShapes.clear(); + outSubShapes.reserve(mSubShapes.size()); + for (const SubShape &shape : mSubShapes) + outSubShapes.push_back(shape.mShape); +} + +void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) +{ + JPH_ASSERT(mSubShapes.size() == inNumShapes); + for (uint i = 0; i < inNumShapes; ++i) + mSubShapes[i].mShape = inSubShapes[i]; +} + +Shape::Stats CompoundShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + // Get own stats + Stats stats = Shape::GetStatsRecursive(ioVisitedShapes); + + // Add child stats + for (const SubShape &shape : mSubShapes) + { + Stats child_stats = shape.mShape->GetStatsRecursive(ioVisitedShapes); + stats.mSizeBytes += child_stats.mSizeBytes; + stats.mNumTriangles += child_stats.mNumTriangles; + } + + return stats; +} + +float CompoundShape::GetVolume() const +{ + float volume = 0.0f; + for (const SubShape &shape : mSubShapes) + volume += shape.mShape->GetVolume(); + return volume; +} + +bool CompoundShape::IsValidScale(Vec3Arg inScale) const +{ + if (!Shape::IsValidScale(inScale)) + return false; + + for (const SubShape &shape : mSubShapes) + { + // Test if the scale is non-uniform and the shape is rotated + if (!shape.IsValidScale(inScale)) + return false; + + // Test the child shape + if (!shape.mShape->IsValidScale(shape.TransformScale(inScale))) + return false; + } + + return true; +} + +void CompoundShape::sRegister() +{ + for (EShapeSubType s1 : sCompoundSubShapeTypes) + for (EShapeSubType s2 : sAllSubShapeTypes) + CollisionDispatch::sRegisterCastShape(s1, s2, sCastCompoundVsShape); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.h new file mode 100644 index 000000000..7694bf4e0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShape.h @@ -0,0 +1,344 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; +class OrientedBox; + +/// Base class settings to construct a compound shape +class JPH_EXPORT CompoundShapeSettings : public ShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, CompoundShapeSettings) + + /// Constructor. Use AddShape to add the parts. + CompoundShapeSettings() = default; + + /// Add a shape to the compound. + void AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData = 0); + + /// Add a shape to the compound. Variant that uses a concrete shape, which means this object cannot be serialized. + void AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0); + + struct SubShapeSettings + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SubShapeSettings) + + RefConst mShape; ///< Sub shape (either this or mShapePtr needs to be filled up) + RefConst mShapePtr; ///< Sub shape (either this or mShape needs to be filled up) + Vec3 mPosition; ///< Position of the sub shape + Quat mRotation; ///< Rotation of the sub shape + uint32 mUserData = 0; ///< User data value (can be used by the application for any purpose) + }; + + using SubShapes = Array; + + SubShapes mSubShapes; +}; + +/// Base class for a compound shape +class JPH_EXPORT CompoundShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit CompoundShape(EShapeSubType inSubType) : Shape(EShapeType::Compound, inSubType) { } + CompoundShape(EShapeSubType inSubType, const ShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Compound, inSubType, inSettings, outResult) { } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override; + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetSubShapeUserData + virtual uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + /// Get which sub shape's bounding boxes overlap with an axis aligned box + /// @param inBox The axis aligned box to test against (relative to the center of mass of this shape) + /// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect + /// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices) + /// @return How many indices were placed in outSubShapeIndices + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0; + + /// Get which sub shape's bounding boxes overlap with an axis aligned box + /// @param inBox The axis aligned box to test against (relative to the center of mass of this shape) + /// @param outSubShapeIndices Buffer where to place the indices of the sub shapes that intersect + /// @param inMaxSubShapeIndices How many indices will fit in the buffer (normally you'd provide a buffer of GetNumSubShapes() indices) + /// @return How many indices were placed in outSubShapeIndices + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const = 0; + + struct SubShape + { + /// Initialize sub shape from sub shape settings + /// @param inSettings Settings object + /// @param outResult Result object, only used in case of error + /// @return True on success, false on failure + bool FromSettings(const CompoundShapeSettings::SubShapeSettings &inSettings, ShapeResult &outResult) + { + if (inSettings.mShapePtr != nullptr) + { + // Use provided shape + mShape = inSettings.mShapePtr; + } + else + { + // Create child shape + ShapeResult child_result = inSettings.mShape->Create(); + if (!child_result.IsValid()) + { + outResult = child_result; + return false; + } + mShape = child_result.Get(); + } + + // Copy user data + mUserData = inSettings.mUserData; + + SetTransform(inSettings.mPosition, inSettings.mRotation, Vec3::sZero() /* Center of mass not yet calculated */); + return true; + } + + /// Update the transform of this sub shape + /// @param inPosition New position + /// @param inRotation New orientation + /// @param inCenterOfMass The center of mass of the compound shape + JPH_INLINE void SetTransform(Vec3Arg inPosition, QuatArg inRotation, Vec3Arg inCenterOfMass) + { + SetPositionCOM(inPosition - inCenterOfMass + inRotation * mShape->GetCenterOfMass()); + + mIsRotationIdentity = inRotation.IsClose(Quat::sIdentity()) || inRotation.IsClose(-Quat::sIdentity()); + SetRotation(mIsRotationIdentity? Quat::sIdentity() : inRotation); + } + + /// Get the local transform for this shape given the scale of the child shape + /// The total transform of the child shape will be GetLocalTransformNoScale(inScale) * Mat44::sScaling(TransformScale(inScale)) + /// @param inScale The scale of the child shape (in local space of this shape) + JPH_INLINE Mat44 GetLocalTransformNoScale(Vec3Arg inScale) const + { + JPH_ASSERT(IsValidScale(inScale)); + return Mat44::sRotationTranslation(GetRotation(), inScale * GetPositionCOM()); + } + + /// Test if inScale is valid for this sub shape + inline bool IsValidScale(Vec3Arg inScale) const + { + // We can always handle uniform scale or identity rotations + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return true; + + return ScaleHelpers::CanScaleBeRotated(GetRotation(), inScale); + } + + /// Transform the scale to the local space of the child shape + inline Vec3 TransformScale(Vec3Arg inScale) const + { + // We don't need to transform uniform scale or if the rotation is identity + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return inScale; + + return ScaleHelpers::RotateScale(GetRotation(), inScale); + } + + /// Compress the center of mass position + JPH_INLINE void SetPositionCOM(Vec3Arg inPositionCOM) + { + inPositionCOM.StoreFloat3(&mPositionCOM); + } + + /// Uncompress the center of mass position + JPH_INLINE Vec3 GetPositionCOM() const + { + return Vec3::sLoadFloat3Unsafe(mPositionCOM); + } + + /// Compress the rotation + JPH_INLINE void SetRotation(QuatArg inRotation) + { + inRotation.StoreFloat3(&mRotation); + } + + /// Uncompress the rotation + JPH_INLINE Quat GetRotation() const + { + return mIsRotationIdentity? Quat::sIdentity() : Quat::sLoadFloat3Unsafe(mRotation); + } + + RefConst mShape; + Float3 mPositionCOM; ///< Note: Position of center of mass of sub shape! + Float3 mRotation; ///< Note: X, Y, Z of rotation quaternion - note we read 4 bytes beyond this so make sure there's something there + uint32 mUserData; ///< User data value (put here because it falls in padding bytes) + bool mIsRotationIdentity; ///< If mRotation is close to identity (put here because it falls in padding bytes) + // 3 padding bytes left + }; + + static_assert(sizeof(SubShape) == (JPH_CPU_ADDRESS_BITS == 64? 40 : 36), "Compiler added unexpected padding"); + + using SubShapes = Array; + + /// Access to the sub shapes of this compound + const SubShapes & GetSubShapes() const { return mSubShapes; } + + /// Get the total number of sub shapes + uint GetNumSubShapes() const { return uint(mSubShapes.size()); } + + /// Access to a particular sub shape + const SubShape & GetSubShape(uint inIdx) const { return mSubShapes[inIdx]; } + + /// Get the user data associated with a shape in this compound + uint32 GetCompoundUserData(uint inIdx) const { return mSubShapes[inIdx].mUserData; } + + /// Set the user data associated with a shape in this compound + void SetCompoundUserData(uint inIdx, uint32 inUserData) { mSubShapes[inIdx].mUserData = inUserData; } + + /// Check if a sub shape ID is still valid for this shape + /// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape + /// @return True if the ID is valid, false if not + inline bool IsSubShapeIDValid(SubShapeID inSubShapeID) const + { + SubShapeID remainder; + return inSubShapeID.PopID(GetSubShapeIDBits(), remainder) < mSubShapes.size(); + } + + /// Convert SubShapeID to sub shape index + /// @param inSubShapeID Sub shape id that indicates the leaf shape relative to this shape + /// @param outRemainder This is the sub shape ID for the sub shape of the compound after popping off the index + /// @return The index of the sub shape of this compound + inline uint32 GetSubShapeIndexFromID(SubShapeID inSubShapeID, SubShapeID &outRemainder) const + { + uint32 idx = inSubShapeID.PopID(GetSubShapeIDBits(), outRemainder); + JPH_ASSERT(idx < mSubShapes.size(), "Invalid SubShapeID"); + return idx; + } + + /// @brief Convert a sub shape index to a sub shape ID + /// @param inIdx Index of the sub shape of this compound + /// @param inParentSubShapeID Parent SubShapeID (describing the path to the compound shape) + /// @return A sub shape ID creator that contains the full path to the sub shape with index inIdx + inline SubShapeIDCreator GetSubShapeIDFromIndex(int inIdx, const SubShapeIDCreator &inParentSubShapeID) const + { + return inParentSubShapeID.PushID(inIdx, GetSubShapeIDBits()); + } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveSubShapeState(ShapeList &outSubShapes) const override; + virtual void RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override; + + // See Shape::GetStatsRecursive + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override; + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + + // Visitors for collision detection + struct CastRayVisitor; + struct CastRayVisitorCollector; + struct CollidePointVisitor; + struct CastShapeVisitor; + struct CollectTransformedShapesVisitor; + struct CollideCompoundVsShapeVisitor; + struct CollideShapeVsCompoundVisitor; + template struct GetIntersectingSubShapesVisitor; + + /// Determine amount of bits needed to encode sub shape id + inline uint GetSubShapeIDBits() const + { + // Ensure we have enough bits to encode our shape [0, n - 1] + uint32 n = uint32(mSubShapes.size()) - 1; + return 32 - CountLeadingZeros(n); + } + + /// Determine the inner radius of this shape + inline void CalculateInnerRadius() + { + mInnerRadius = FLT_MAX; + for (const SubShape &s : mSubShapes) + mInnerRadius = min(mInnerRadius, s.mShape->GetInnerRadius()); + } + + Vec3 mCenterOfMass { Vec3::sZero() }; ///< Center of mass of the compound + AABox mLocalBounds; + SubShapes mSubShapes; + float mInnerRadius = FLT_MAX; ///< Smallest radius of GetInnerRadius() of child shapes + +private: + // Helper functions called by CollisionDispatch + static void sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h b/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h new file mode 100644 index 000000000..ecccb658a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/CompoundShapeVisitors.h @@ -0,0 +1,460 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct CompoundShape::CastRayVisitor +{ + JPH_INLINE CastRayVisitor(const RayCast &inRay, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) : + mRay(inRay), + mHit(ioHit), + mSubShapeIDCreator(inSubShapeIDCreator), + mSubShapeBits(inShape->GetSubShapeIDBits()) + { + // Determine ray properties of cast + mInvDirection.Set(inRay.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + /// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the ray against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the ray + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + RayCast ray = mRay.Transformed(transform); + if (inSubShape.mShape->CastRay(ray, shape2_sub_shape_id, mHit)) + mReturnValue = true; + } + + RayInvDirection mInvDirection; + const RayCast & mRay; + RayCastResult & mHit; + SubShapeIDCreator mSubShapeIDCreator; + uint mSubShapeBits; + bool mReturnValue = false; +}; + +struct CompoundShape::CastRayVisitorCollector +{ + JPH_INLINE CastRayVisitorCollector(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mRay(inRay), + mCollector(ioCollector), + mSubShapeIDCreator(inSubShapeIDCreator), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mRayCastSettings(inRayCastSettings), + mShapeFilter(inShapeFilter) + { + // Determine ray properties of cast + mInvDirection.Set(inRay.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Test ray against 4 bounding boxes and returns the distance where the ray enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return RayAABox4(mRay.mOrigin, mInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the ray against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the ray + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + RayCast ray = mRay.Transformed(transform); + inSubShape.mShape->CastRay(ray, mRayCastSettings, shape2_sub_shape_id, mCollector, mShapeFilter); + } + + RayInvDirection mInvDirection; + const RayCast & mRay; + CastRayCollector & mCollector; + SubShapeIDCreator mSubShapeIDCreator; + uint mSubShapeBits; + RayCastSettings mRayCastSettings; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollidePointVisitor +{ + JPH_INLINE CollidePointVisitor(Vec3Arg inPoint, const CompoundShape *inShape, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mPoint(inPoint), + mSubShapeIDCreator(inSubShapeIDCreator), + mCollector(ioCollector), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Test if point overlaps with 4 boxes, returns true for the ones that do + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return AABox4VsPoint(mPoint, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Test the point against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Transform the point + Mat44 transform = Mat44::sInverseRotationTranslation(inSubShape.GetRotation(), inSubShape.GetPositionCOM()); + inSubShape.mShape->CollidePoint(transform * mPoint, shape2_sub_shape_id, mCollector, mShapeFilter); + } + + Vec3 mPoint; + SubShapeIDCreator mSubShapeIDCreator; + CollidePointCollector & mCollector; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CastShapeVisitor +{ + JPH_INLINE CastShapeVisitor(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const CompoundShape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) : + mBoxCenter(inShapeCast.mShapeWorldBounds.GetCenter()), + mBoxExtent(inShapeCast.mShapeWorldBounds.GetExtent()), + mScale(inScale), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mShapeFilter(inShapeFilter), + mCollector(ioCollector), + mCenterOfMassTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape->GetSubShapeIDBits()) + { + // Determine ray properties of cast + mInvDirection.Set(inShapeCast.mDirection); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the shape cast against 4 bounding boxes, returns the distance along the shape cast where the shape first enters the bounding box + JPH_INLINE Vec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test ray against the bounding boxes + return RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the cast shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(inSubShape.IsValidScale(mScale)); + + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits); + + // Calculate the local transform for this sub shape + Mat44 local_transform = Mat44::sRotationTranslation(inSubShape.GetRotation(), mScale * inSubShape.GetPositionCOM()); + + // Transform the center of mass of 2 + Mat44 center_of_mass_transform2 = mCenterOfMassTransform2 * local_transform; + + // Transform the shape cast + ShapeCast shape_cast = mShapeCast.PostTransformed(local_transform.InversedRotationTranslation()); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, mShapeCastSettings, inSubShape.mShape, inSubShape.TransformScale(mScale), mShapeFilter, center_of_mass_transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollector); + } + + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + Vec3 mScale; + const ShapeCast & mShapeCast; + const ShapeCastSettings & mShapeCastSettings; + const ShapeFilter & mShapeFilter; + CastShapeCollector & mCollector; + Mat44 mCenterOfMassTransform2; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; +}; + +struct CompoundShape::CollectTransformedShapesVisitor +{ + JPH_INLINE CollectTransformedShapesVisitor(const AABox &inBox, const CompoundShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mBox(inBox), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mPositionCOM(inPositionCOM), + mRotation(inRotation), + mScale(inScale), + mSubShapeIDCreator(inSubShapeIDCreator), + mCollector(ioCollector), + mSubShapeBits(inShape->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests 4 bounding boxes against the query box, returns true for the ones that collide + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + return AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Collect the transformed sub shapes for a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(inSubShape.IsValidScale(mScale)); + + // Create ID for sub shape + SubShapeIDCreator sub_shape_id = mSubShapeIDCreator.PushID(inSubShapeIndex, mSubShapeBits); + + // Calculate world transform for sub shape + Vec3 position = mPositionCOM + mRotation * (mScale * inSubShape.GetPositionCOM()); + Quat rotation = mRotation * inSubShape.GetRotation(); + + // Recurse to sub shape + inSubShape.mShape->CollectTransformedShapes(mBox, position, rotation, inSubShape.TransformScale(mScale), sub_shape_id, mCollector, mShapeFilter); + } + + AABox mBox; + OrientedBox mLocalBox; + Vec3 mPositionCOM; + Quat mRotation; + Vec3 mScale; + SubShapeIDCreator mSubShapeIDCreator; + TransformedShapeCollector & mCollector; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollideCompoundVsShapeVisitor +{ + JPH_INLINE CollideCompoundVsShapeVisitor(const CompoundShape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape2(inShape2), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape1->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + // Get transform from shape 2 to shape 1 + Mat44 transform2_to_1 = inCenterOfMassTransform1.InversedRotationTranslation() * inCenterOfMassTransform2; + + // Convert bounding box of 2 into space of 1 + mBoundsOf2InSpaceOf1 = inShape2->GetLocalBounds().Scaled(inScale2).Transformed(transform2_to_1); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the bounds of shape 2 vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale1, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which boxes collide + return AABox4VsBox(mBoundsOf2InSpaceOf1, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Get world transform of 1 + Mat44 transform1 = mTransform1 * inSubShape.GetLocalTransformNoScale(mScale1); + + // Create ID for sub shape + SubShapeIDCreator shape1_sub_shape_id = mSubShapeIDCreator1.PushID(inSubShapeIndex, mSubShapeBits); + + CollisionDispatch::sCollideShapeVsShape(inSubShape.mShape, mShape2, inSubShape.TransformScale(mScale1), mScale2, transform1, mTransform2, shape1_sub_shape_id, mSubShapeIDCreator2, mCollideShapeSettings, mCollector, mShapeFilter); + } + + const CollideShapeSettings & mCollideShapeSettings; + CollideShapeCollector & mCollector; + const Shape * mShape2; + Vec3 mScale1; + Vec3 mScale2; + Mat44 mTransform1; + Mat44 mTransform2; + AABox mBoundsOf2InSpaceOf1; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +struct CompoundShape::CollideShapeVsCompoundVisitor +{ + JPH_INLINE CollideShapeVsCompoundVisitor(const Shape *inShape1, const CompoundShape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) : + mCollideShapeSettings(inCollideShapeSettings), + mCollector(ioCollector), + mShape1(inShape1), + mScale1(inScale1), + mScale2(inScale2), + mTransform1(inCenterOfMassTransform1), + mTransform2(inCenterOfMassTransform2), + mSubShapeIDCreator1(inSubShapeIDCreator1), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mSubShapeBits(inShape2->GetSubShapeIDBits()), + mShapeFilter(inShapeFilter) + { + // Get transform from shape 1 to shape 2 + Mat44 transform1_to_2 = inCenterOfMassTransform2.InversedRotationTranslation() * inCenterOfMassTransform1; + + // Convert bounding box of 1 into space of 2 + mBoundsOf1InSpaceOf2 = inShape1->GetLocalBounds().Scaled(inScale1).Transformed(transform1_to_2); + mBoundsOf1InSpaceOf2.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + } + + /// Returns true when collision detection should abort because it's not possible to find a better hit + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + /// Tests the bounds of shape 1 vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Scale the bounding boxes + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which bounding boxes collide + return AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + } + + /// Test the shape against a single subshape + JPH_INLINE void VisitShape(const SubShape &inSubShape, uint32 inSubShapeIndex) + { + // Create ID for sub shape + SubShapeIDCreator shape2_sub_shape_id = mSubShapeIDCreator2.PushID(inSubShapeIndex, mSubShapeBits); + + // Get world transform of 2 + Mat44 transform2 = mTransform2 * inSubShape.GetLocalTransformNoScale(mScale2); + + CollisionDispatch::sCollideShapeVsShape(mShape1, inSubShape.mShape, mScale1, inSubShape.TransformScale(mScale2), mTransform1, transform2, mSubShapeIDCreator1, shape2_sub_shape_id, mCollideShapeSettings, mCollector, mShapeFilter); + } + + const CollideShapeSettings & mCollideShapeSettings; + CollideShapeCollector & mCollector; + const Shape * mShape1; + Vec3 mScale1; + Vec3 mScale2; + Mat44 mTransform1; + Mat44 mTransform2; + AABox mBoundsOf1InSpaceOf2; + SubShapeIDCreator mSubShapeIDCreator1; + SubShapeIDCreator mSubShapeIDCreator2; + uint mSubShapeBits; + const ShapeFilter & mShapeFilter; +}; + +template +struct CompoundShape::GetIntersectingSubShapesVisitor +{ + JPH_INLINE GetIntersectingSubShapesVisitor(const BoxType &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) : + mBox(inBox), + mSubShapeIndices(outSubShapeIndices), + mMaxSubShapeIndices(inMaxSubShapeIndices) + { + } + + /// Returns true when collision detection should abort because the buffer is full + JPH_INLINE bool ShouldAbort() const + { + return mNumResults >= mMaxSubShapeIndices; + } + + /// Tests the box vs 4 bounding boxes, returns true for the ones that intersect + JPH_INLINE UVec4 TestBounds(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + // Test which bounding boxes collide + return AABox4VsBox(mBox, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + /// Records a hit + JPH_INLINE void VisitShape([[maybe_unused]] const SubShape &inSubShape, uint32 inSubShapeIndex) + { + JPH_ASSERT(mNumResults < mMaxSubShapeIndices); + *mSubShapeIndices++ = inSubShapeIndex; + mNumResults++; + } + + /// Get the number of indices that were found + JPH_INLINE int GetNumResults() const + { + return mNumResults; + } + +private: + BoxType mBox; + uint * mSubShapeIndices; + int mMaxSubShapeIndices; + int mNumResults = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp new file mode 100644 index 000000000..d1de63717 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.cpp @@ -0,0 +1,1311 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConvexHullShapeSettings) +{ + JPH_ADD_BASE_CLASS(ConvexHullShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mPoints) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxConvexRadius) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mMaxErrorConvexRadius) + JPH_ADD_ATTRIBUTE(ConvexHullShapeSettings, mHullTolerance) +} + +ShapeSettings::ShapeResult ConvexHullShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new ConvexHullShape(*this, mCachedResult); + return mCachedResult; +} + +ConvexHullShape::ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::ConvexHull, inSettings, outResult), + mConvexRadius(inSettings.mMaxConvexRadius) +{ + using BuilderFace = ConvexHullBuilder::Face; + using Edge = ConvexHullBuilder::Edge; + using Faces = Array; + + // Check convex radius + if (mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + // Build convex hull + const char *error = nullptr; + ConvexHullBuilder builder(inSettings.mPoints); + ConvexHullBuilder::EResult result = builder.Initialize(cMaxPointsInHull, inSettings.mHullTolerance, error); + if (result != ConvexHullBuilder::EResult::Success && result != ConvexHullBuilder::EResult::MaxVerticesReached) + { + outResult.SetError(error); + return; + } + const Faces &builder_faces = builder.GetFaces(); + + // Check the consistency of the resulting hull if we fully built it + if (result == ConvexHullBuilder::EResult::Success) + { + ConvexHullBuilder::Face *max_error_face; + float max_error_distance, coplanar_distance; + int max_error_idx; + builder.DetermineMaxError(max_error_face, max_error_distance, max_error_idx, coplanar_distance); + if (max_error_distance > 4.0f * max(coplanar_distance, inSettings.mHullTolerance)) // Coplanar distance could be bigger than the allowed tolerance if the points are far apart + { + outResult.SetError(StringFormat("Hull building failed, point %d had an error of %g (relative to tolerance: %g)", max_error_idx, (double)max_error_distance, double(max_error_distance / inSettings.mHullTolerance))); + return; + } + } + + // Calculate center of mass and volume + builder.GetCenterOfMassAndVolume(mCenterOfMass, mVolume); + + // Calculate covariance matrix + // See: + // - Why the inertia tensor is the inertia tensor - Jonathan Blow (http://number-none.com/blow/inertia/deriving_i.html) + // - How to find the inertia tensor (or other mass properties) of a 3D solid body represented by a triangle mesh (Draft) - Jonathan Blow, Atman J Binstock (http://number-none.com/blow/inertia/bb_inertia.doc) + Mat44 covariance_canonical(Vec4(1.0f / 60.0f, 1.0f / 120.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 60.0f, 1.0f / 120.0f, 0), Vec4(1.0f / 120.0f, 1.0f / 120.0f, 1.0f / 60.0f, 0), Vec4(0, 0, 0, 1)); + Mat44 covariance_matrix = Mat44::sZero(); + for (BuilderFace *f : builder_faces) + { + // Fourth point of the tetrahedron is at the center of mass, we subtract it from the other points so we get a tetrahedron with one vertex at zero + // The first point on the face will be used to form a triangle fan + Edge *e = f->mFirstEdge; + Vec3 v1 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Get the 2nd point + e = e->mNextEdge; + Vec3 v2 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Loop over the triangle fan + for (e = e->mNextEdge; e != f->mFirstEdge; e = e->mNextEdge) + { + Vec3 v3 = inSettings.mPoints[e->mStartIdx] - mCenterOfMass; + + // Affine transform that transforms a unit tetrahedon (with vertices (0, 0, 0), (1, 0, 0), (0, 1, 0) and (0, 0, 1) to this tetrahedron + Mat44 a(Vec4(v1, 0), Vec4(v2, 0), Vec4(v3, 0), Vec4(0, 0, 0, 1)); + + // Calculate covariance matrix for this tetrahedron + float det_a = a.GetDeterminant3x3(); + Mat44 c = det_a * (a * covariance_canonical * a.Transposed()); + + // Add it + covariance_matrix += c; + + // Prepare for next triangle + v2 = v3; + } + } + + // Calculate inertia matrix assuming density is 1, note that element (3, 3) is garbage + mInertia = Mat44::sIdentity() * (covariance_matrix(0, 0) + covariance_matrix(1, 1) + covariance_matrix(2, 2)) - covariance_matrix; + + // Convert polygons from the builder to our internal representation + using VtxMap = UnorderedMap; + VtxMap vertex_map; + for (BuilderFace *builder_face : builder_faces) + { + // Determine where the vertices go + JPH_ASSERT(mVertexIdx.size() <= 0xFFFF); + uint16 first_vertex = (uint16)mVertexIdx.size(); + uint16 num_vertices = 0; + + // Loop over vertices in face + Edge *edge = builder_face->mFirstEdge; + do + { + // Remap to new index, not all points in the original input set are required to form the hull + uint8 new_idx; + int original_idx = edge->mStartIdx; + VtxMap::iterator m = vertex_map.find(original_idx); + if (m != vertex_map.end()) + { + // Found, reuse + new_idx = m->second; + } + else + { + // This is a new point + // Make relative to center of mass + Vec3 p = inSettings.mPoints[original_idx] - mCenterOfMass; + + // Update local bounds + mLocalBounds.Encapsulate(p); + + // Add to point list + JPH_ASSERT(mPoints.size() <= 0xff); + new_idx = (uint8)mPoints.size(); + mPoints.push_back({ p }); + vertex_map[original_idx] = new_idx; + } + + // Append to vertex list + JPH_ASSERT(mVertexIdx.size() < 0xffff); + mVertexIdx.push_back(new_idx); + num_vertices++; + + edge = edge->mNextEdge; + } while (edge != builder_face->mFirstEdge); + + // Add face + mFaces.push_back({ first_vertex, num_vertices }); + + // Add plane + Plane plane = Plane::sFromPointAndNormal(builder_face->mCentroid - mCenterOfMass, builder_face->mNormal.Normalized()); + mPlanes.push_back(plane); + } + + // Test if GetSupportFunction can support this many points + if (mPoints.size() > cMaxPointsInHull) + { + outResult.SetError(StringFormat("Internal error: Too many points in hull (%u), max allowed %d", (uint)mPoints.size(), cMaxPointsInHull)); + return; + } + + for (int p = 0; p < (int)mPoints.size(); ++p) + { + // For each point, find faces that use the point + Array faces; + for (int f = 0; f < (int)mFaces.size(); ++f) + { + const Face &face = mFaces[f]; + for (int v = 0; v < face.mNumVertices; ++v) + if (mVertexIdx[face.mFirstVertex + v] == p) + { + faces.push_back(f); + break; + } + } + + if (faces.size() < 2) + { + outResult.SetError("A point must be connected to 2 or more faces!"); + return; + } + + // Find the 3 normals that form the largest tetrahedron + // The largest tetrahedron we can get is ((1, 0, 0) x (0, 1, 0)) . (0, 0, 1) = 1, if the volume is only 5% of that, + // the three vectors are too coplanar and we fall back to using only 2 plane normals + float biggest_volume = 0.05f; + int best3[3] = { -1, -1, -1 }; + + // When using 2 normals, we get the two with the biggest angle between them with a minimal difference of 1 degree + // otherwise we fall back to just using 1 plane normal + float smallest_dot = Cos(DegreesToRadians(1.0f)); + int best2[2] = { -1, -1 }; + + for (int face1 = 0; face1 < (int)faces.size(); ++face1) + { + Vec3 normal1 = mPlanes[faces[face1]].GetNormal(); + for (int face2 = face1 + 1; face2 < (int)faces.size(); ++face2) + { + Vec3 normal2 = mPlanes[faces[face2]].GetNormal(); + Vec3 cross = normal1.Cross(normal2); + + // Determine the 2 face normals that are most apart + float dot = normal1.Dot(normal2); + if (dot < smallest_dot) + { + smallest_dot = dot; + best2[0] = faces[face1]; + best2[1] = faces[face2]; + } + + // Determine the 3 face normals that form the largest tetrahedron + for (int face3 = face2 + 1; face3 < (int)faces.size(); ++face3) + { + Vec3 normal3 = mPlanes[faces[face3]].GetNormal(); + float volume = abs(cross.Dot(normal3)); + if (volume > biggest_volume) + { + biggest_volume = volume; + best3[0] = faces[face1]; + best3[1] = faces[face2]; + best3[2] = faces[face3]; + } + } + } + } + + // If we didn't find 3 planes, use 2, if we didn't find 2 use 1 + if (best3[0] != -1) + faces = { best3[0], best3[1], best3[2] }; + else if (best2[0] != -1) + faces = { best2[0], best2[1] }; + else + faces = { faces[0] }; + + // Copy the faces to the points buffer + Point &point = mPoints[p]; + point.mNumFaces = (int)faces.size(); + for (int i = 0; i < (int)faces.size(); ++i) + point.mFaces[i] = faces[i]; + } + + // If the convex radius is already zero, there's no point in further reducing it + if (mConvexRadius > 0.0f) + { + // Find out how thin the hull is by walking over all planes and checking the thickness of the hull in that direction + float min_size = FLT_MAX; + for (const Plane &plane : mPlanes) + { + // Take the point that is furthest away from the plane as thickness of this hull + float max_dist = 0.0f; + for (const Point &point : mPoints) + { + float dist = -plane.SignedDistance(point.mPosition); // Point is always behind plane, so we need to negate + if (dist > max_dist) + max_dist = dist; + } + min_size = min(min_size, max_dist); + } + + // We need to fit in 2x the convex radius in min_size, so reduce the convex radius if it's bigger than that + mConvexRadius = min(mConvexRadius, 0.5f * min_size); + } + + // Now walk over all points and see if we have to further reduce the convex radius because of sharp edges + if (mConvexRadius > 0.0f) + { + for (const Point &point : mPoints) + if (point.mNumFaces != 1) // If we have a single face, shifting back is easy and we don't need to reduce the convex radius + { + // Get first two planes + Plane p1 = mPlanes[point.mFaces[0]]; + Plane p2 = mPlanes[point.mFaces[1]]; + Plane p3; + Vec3 offset_mask; + + if (point.mNumFaces == 3) + { + // Get third plane + p3 = mPlanes[point.mFaces[2]]; + + // All 3 planes will be offset by the convex radius + offset_mask = Vec3::sReplicate(1); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal())); + + // Only the first and 2nd plane will be offset, the 3rd plane is only there to guide the intersection point + offset_mask = Vec3(1, 1, 0); + } + + // Plane equation: point . normal + constant = 0 + // Offsetting the plane backwards with convex radius r: point . normal + constant + r = 0 + // To find the intersection 'point' of 3 planes we solve: + // |n1x n1y n1z| |x| | r + c1 | + // |n2x n2y n2z| |y| = - | r + c2 | <=> n point = -r (1, 1, 1) - (c1, c2, c3) + // |n3x n3y n3z| |z| | r + c3 | + // Where point = (x, y, z), n1x is the x component of the first plane, c1 = plane constant of plane 1, etc. + // The relation between how much the intersection point shifts as a function of r is: -r * n^-1 (1, 1, 1) = r * offset + // Where offset = -n^-1 (1, 1, 1) or -n^-1 (1, 1, 0) in case only the first 2 planes are offset + // The error that is introduced by a convex radius r is: error = r * |offset| - r + // So the max convex radius given error is: r = error / (|offset| - 1) + Mat44 n = Mat44(Vec4(p1.GetNormal(), 0), Vec4(p2.GetNormal(), 0), Vec4(p3.GetNormal(), 0), Vec4(0, 0, 0, 1)).Transposed(); + float det_n = n.GetDeterminant3x3(); + if (det_n == 0.0f) + { + // If the determinant is zero, the matrix is not invertible so no solution exists to move the point backwards and we have to choose a convex radius of zero + mConvexRadius = 0.0f; + break; + } + Mat44 adj_n = n.Adjointed3x3(); + float offset = ((adj_n * offset_mask) / det_n).Length(); + JPH_ASSERT(offset > 1.0f); + float max_convex_radius = inSettings.mMaxErrorConvexRadius / (offset - 1.0f); + mConvexRadius = min(mConvexRadius, max_convex_radius); + } + } + + // Calculate the inner radius by getting the minimum distance from the origin to the planes of the hull + mInnerRadius = FLT_MAX; + for (const Plane &p : mPlanes) + mInnerRadius = min(mInnerRadius, -p.GetConstant()); + mInnerRadius = max(0.0f, mInnerRadius); // Clamp against zero, this should do nothing as the shape is centered around the center of mass but for flat convex hulls there may be numerical round off issues + + outResult.Set(this); +} + +MassProperties ConvexHullShape::GetMassProperties() const +{ + MassProperties p; + + float density = GetDensity(); + + // Calculate mass + p.mMass = density * mVolume; + + // Calculate inertia matrix + p.mInertia = density * mInertia; + p.mInertia(3, 3) = 1.0f; + + return p; +} + +Vec3 ConvexHullShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + const Plane &first_plane = mPlanes[0]; + Vec3 best_normal = first_plane.GetNormal(); + float best_dist = abs(first_plane.SignedDistance(inLocalSurfacePosition)); + + // Find the face that has the shortest distance to the surface point + for (Array::size_type i = 1; i < mFaces.size(); ++i) + { + const Plane &plane = mPlanes[i]; + Vec3 plane_normal = plane.GetNormal(); + float dist = abs(plane.SignedDistance(inLocalSurfacePosition)); + if (dist < best_dist) + { + best_dist = dist; + best_normal = plane_normal; + } + } + + return best_normal; +} + +class ConvexHullShape::HullNoConvex final : public Support +{ +public: + explicit HullNoConvex(float inConvexRadius) : + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(HullNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (Vec3 point : mPoints) + { + // Check if its support is bigger than the current max + float dot = point.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = point; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + + using PointsArray = StaticArray; + + inline PointsArray & GetPoints() + { + return mPoints; + } + + const PointsArray & GetPoints() const + { + return mPoints; + } + +private: + float mConvexRadius; + PointsArray mPoints; +}; + +class ConvexHullShape::HullWithConvex final : public Support +{ +public: + explicit HullWithConvex(const ConvexHullShape *inShape) : + mShape(inShape) + { + static_assert(sizeof(HullWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (const Point &point : mShape->mPoints) + { + // Check if its support is bigger than the current max + float dot = point.mPosition.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = point.mPosition; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + const ConvexHullShape * mShape; +}; + +class ConvexHullShape::HullWithConvexScaled final : public Support +{ +public: + HullWithConvexScaled(const ConvexHullShape *inShape, Vec3Arg inScale) : + mShape(inShape), + mScale(inScale) + { + static_assert(sizeof(HullWithConvexScaled) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(HullWithConvexScaled))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Find the point with the highest projection on inDirection + float best_dot = -FLT_MAX; + Vec3 best_point = Vec3::sZero(); + + for (const Point &point : mShape->mPoints) + { + // Calculate scaled position + Vec3 pos = mScale * point.mPosition; + + // Check if its support is bigger than the current max + float dot = pos.Dot(inDirection); + if (dot > best_dot) + { + best_dot = dot; + best_point = pos; + } + } + + return best_point; + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + const ConvexHullShape * mShape; + Vec3 mScale; +}; + +const ConvexShape::Support *ConvexHullShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + // If there's no convex radius, we don't need to shrink the hull + if (mConvexRadius == 0.0f) + { + if (ScaleHelpers::IsNotScaled(inScale)) + return new (&inBuffer) HullWithConvex(this); + else + return new (&inBuffer) HullWithConvexScaled(this, inScale); + } + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + if (ScaleHelpers::IsNotScaled(inScale)) + return new (&inBuffer) HullWithConvex(this); + else + return new (&inBuffer) HullWithConvexScaled(this, inScale); + + case ESupportMode::ExcludeConvexRadius: + if (ScaleHelpers::IsNotScaled(inScale)) + { + // Create support function + HullNoConvex *hull = new (&inBuffer) HullNoConvex(mConvexRadius); + HullNoConvex::PointsArray &transformed_points = hull->GetPoints(); + JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!"); + + for (const Point &point : mPoints) + { + Vec3 new_point; + + if (point.mNumFaces == 1) + { + // Simply shift back by the convex radius using our 1 plane + new_point = point.mPosition - mPlanes[point.mFaces[0]].GetNormal() * mConvexRadius; + } + else + { + // Get first two planes and offset inwards by convex radius + Plane p1 = mPlanes[point.mFaces[0]].Offset(-mConvexRadius); + Plane p2 = mPlanes[point.mFaces[1]].Offset(-mConvexRadius); + Plane p3; + + if (point.mNumFaces == 3) + { + // Get third plane and offset inwards by convex radius + p3 = mPlanes[point.mFaces[2]].Offset(-mConvexRadius); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(point.mPosition, p1.GetNormal().Cross(p2.GetNormal())); + } + + // Find intersection point between the three planes + if (!Plane::sIntersectPlanes(p1, p2, p3, new_point)) + { + // Fallback: Just push point back using the first plane + new_point = point.mPosition - p1.GetNormal() * mConvexRadius; + } + } + + // Add point + transformed_points.push_back(new_point); + } + + return hull; + } + else + { + // Calculate scaled convex radius + float convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + + // Create new support function + HullNoConvex *hull = new (&inBuffer) HullNoConvex(convex_radius); + HullNoConvex::PointsArray &transformed_points = hull->GetPoints(); + JPH_ASSERT(mPoints.size() <= cMaxPointsInHull, "Not enough space, this should have been caught during shape creation!"); + + // Precalculate inverse scale + Vec3 inv_scale = inScale.Reciprocal(); + + for (const Point &point : mPoints) + { + // Calculate scaled position + Vec3 pos = inScale * point.mPosition; + + // Transform normals for plane 1 with scale + Vec3 n1 = (inv_scale * mPlanes[point.mFaces[0]].GetNormal()).Normalized(); + + Vec3 new_point; + + if (point.mNumFaces == 1) + { + // Simply shift back by the convex radius using our 1 plane + new_point = pos - n1 * convex_radius; + } + else + { + // Transform normals for plane 2 with scale + Vec3 n2 = (inv_scale * mPlanes[point.mFaces[1]].GetNormal()).Normalized(); + + // Get first two planes and offset inwards by convex radius + Plane p1 = Plane::sFromPointAndNormal(pos, n1).Offset(-convex_radius); + Plane p2 = Plane::sFromPointAndNormal(pos, n2).Offset(-convex_radius); + Plane p3; + + if (point.mNumFaces == 3) + { + // Transform last normal with scale + Vec3 n3 = (inv_scale * mPlanes[point.mFaces[2]].GetNormal()).Normalized(); + + // Get third plane and offset inwards by convex radius + p3 = Plane::sFromPointAndNormal(pos, n3).Offset(-convex_radius); + } + else + { + // Third plane has normal perpendicular to the other two planes and goes through the vertex position + JPH_ASSERT(point.mNumFaces == 2); + p3 = Plane::sFromPointAndNormal(pos, n1.Cross(n2)); + } + + // Find intersection point between the three planes + if (!Plane::sIntersectPlanes(p1, p2, p3, new_point)) + { + // Fallback: Just push point back using the first plane + new_point = pos - n1 * convex_radius; + } + } + + // Add point + transformed_points.push_back(new_point); + } + + return hull; + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void ConvexHullShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 inv_scale = inScale.Reciprocal(); + + // Need to transform the plane normals using inScale + // Transforming a direction with matrix M is done through multiplying by (M^-1)^T + // In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards + Vec3 plane0_normal = inv_scale * mPlanes[0].GetNormal(); + float best_dot = plane0_normal.Dot(inDirection) / plane0_normal.Length(); + int best_face_idx = 0; + + for (Array::size_type i = 1; i < mPlanes.size(); ++i) + { + Vec3 plane_normal = inv_scale * mPlanes[i].GetNormal(); + float dot = plane_normal.Dot(inDirection) / plane_normal.Length(); + if (dot < best_dot) + { + best_dot = dot; + best_face_idx = (int)i; + } + } + + // Get vertices + const Face &best_face = mFaces[best_face_idx]; + const uint8 *first_vtx = mVertexIdx.data() + best_face.mFirstVertex; + const uint8 *end_vtx = first_vtx + best_face.mNumVertices; + + // If we have more than 1/2 the capacity of outVertices worth of vertices, we start skipping vertices (note we can't fill the buffer completely since extra edges will be generated by clipping). + // TODO: This really needs a better algorithm to determine which vertices are important! + int max_vertices_to_return = outVertices.capacity() / 2; + int delta_vtx = (int(best_face.mNumVertices) + max_vertices_to_return) / max_vertices_to_return; + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + if (ScaleHelpers::IsInsideOut(inScale)) + { + // Flip winding of supporting face + for (const uint8 *v = end_vtx - 1; v >= first_vtx; v -= delta_vtx) + outVertices.push_back(transform * mPoints[*v].mPosition); + } + else + { + // Normal winding of supporting face + for (const uint8 *v = first_vtx; v < end_vtx; v += delta_vtx) + outVertices.push_back(transform * mPoints[*v].mPosition); + } +} + +void ConvexHullShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Trivially calculate total volume + Vec3 abs_scale = inScale.Abs(); + outTotalVolume = mVolume * abs_scale.GetX() * abs_scale.GetY() * abs_scale.GetZ(); + + // Check if shape has been scaled inside out + bool is_inside_out = ScaleHelpers::IsInsideOut(inScale); + + // Convert the points to world space and determine the distance to the surface + int num_points = int(mPoints.size()); + PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(num_points * sizeof(PolyhedronSubmergedVolumeCalculator::Point)); + PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(inScale), &mPoints[0].mPosition, sizeof(Point), num_points, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + if (submerged_vol_calc.AreAllAbove()) + { + // We're above the water + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (submerged_vol_calc.AreAllBelow()) + { + // We're fully submerged + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Calculate submerged volume + int reference_point_idx = submerged_vol_calc.GetReferencePointIdx(); + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // If any of the vertices of this face are the reference point, the volume will be zero so we can skip this face + bool degenerate = false; + for (const uint8 *v = first_vtx; v < end_vtx; ++v) + if (*v == reference_point_idx) + { + degenerate = true; + break; + } + if (degenerate) + continue; + + // Triangulate the face + int i1 = *first_vtx; + if (is_inside_out) + { + // Reverse winding + for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v) + { + int i2 = *(v - 1); + int i3 = *v; + submerged_vol_calc.AddFace(i1, i3, i2); + } + } + else + { + // Normal winding + for (const uint8 *v = first_vtx + 2; v < end_vtx; ++v) + { + int i2 = *(v - 1); + int i3 = *v; + submerged_vol_calc.AddFace(i1, i2, i3); + } + } + } + + // Get the results + submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy); + } + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void ConvexHullShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + if (mGeometry == nullptr) + { + Array triangles; + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Draw first triangle of polygon + Vec3 v0 = mPoints[first_vtx[0]].mPosition; + Vec3 v1 = mPoints[first_vtx[1]].mPosition; + Vec3 v2 = mPoints[first_vtx[2]].mPosition; + Vec3 uv_direction = (v1 - v0).Normalized(); + triangles.push_back({ v0, v1, v2, Color::sWhite, v0, uv_direction }); + + // Draw any other triangles in this polygon + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + triangles.push_back({ v0, mPoints[*(v - 1)].mPosition, mPoints[*v].mPosition, Color::sWhite, v0, uv_direction }); + } + mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds()); + } + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor; + RMat44 transform = inCenterOfMassTransform.PreScaled(inScale); + inRenderer->DrawGeometry(transform, color, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + // Draw the outline if requested + if (sDrawFaceOutlines) + for (const Face &f : mFaces) + { + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Draw edges of face + inRenderer->DrawLine(transform * mPoints[*(end_vtx - 1)].mPosition, transform * mPoints[*first_vtx].mPosition, Color::sGrey); + for (const uint8 *v = first_vtx + 1; v < end_vtx; ++v) + inRenderer->DrawLine(transform * mPoints[*(v - 1)].mPosition, transform * mPoints[*v].mPosition, Color::sGrey); + } +} + +void ConvexHullShape::DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + // Get the shrunk points + SupportBuffer buffer; + const HullNoConvex *support = mConvexRadius > 0.0f? static_cast(GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale)) : nullptr; + + RMat44 transform = inCenterOfMassTransform * Mat44::sScale(inScale); + + for (int p = 0; p < (int)mPoints.size(); ++p) + { + const Point &point = mPoints[p]; + RVec3 position = transform * point.mPosition; + RVec3 shrunk_point = support != nullptr? transform * support->GetPoints()[p] : position; + + // Draw difference between shrunk position and position + inRenderer->DrawLine(position, shrunk_point, Color::sGreen); + + // Draw face normals that are contributing + for (int i = 0; i < point.mNumFaces; ++i) + inRenderer->DrawLine(position, position + 0.1f * mPlanes[point.mFaces[i]].GetNormal(), Color::sYellow); + + // Draw point index + inRenderer->DrawText3D(position, ConvertToString(p), Color::sWhite, 0.1f); + } +} +#endif // JPH_DEBUG_RENDERER + +bool ConvexHullShape::CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const +{ + if (mFaces.size() == 2) + { + // If we have only 2 faces, we're a flat convex hull and we need to test edges instead of planes + + // Check if plane is parallel to ray + const Plane &p = mPlanes.front(); + Vec3 plane_normal = p.GetNormal(); + float direction_projection = inRay.mDirection.Dot(plane_normal); + if (abs(direction_projection) >= 1.0e-12f) + { + // Calculate intersection point + float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant(); + float fraction = -distance_to_plane / direction_projection; + if (fraction < 0.0f || fraction > 1.0f) + { + // Does not hit plane, no hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + Vec3 intersection_point = inRay.mOrigin + fraction * inRay.mDirection; + + // Test all edges to see if point is inside polygon + const Face &f = mFaces.front(); + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + Vec3 p1 = mPoints[*end_vtx].mPosition; + for (const uint8 *v = first_vtx; v < end_vtx; ++v) + { + Vec3 p2 = mPoints[*v].mPosition; + if ((p2 - p1).Cross(intersection_point - p1).Dot(plane_normal) < 0.0f) + { + // Outside polygon, no hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + p1 = p2; + } + + // Inside polygon, a hit + outMinFraction = fraction; + outMaxFraction = fraction; + return true; + } + else + { + // Parallel ray doesn't hit + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + return false; + } + } + else + { + // Clip ray against all planes + int fractions_set = 0; + bool all_inside = true; + float min_fraction = 0.0f, max_fraction = 1.0f + FLT_EPSILON; + for (const Plane &p : mPlanes) + { + // Check if the ray origin is behind this plane + Vec3 plane_normal = p.GetNormal(); + float distance_to_plane = inRay.mOrigin.Dot(plane_normal) + p.GetConstant(); + bool is_outside = distance_to_plane > 0.0f; + all_inside &= !is_outside; + + // Check if plane is parallel to ray + float direction_projection = inRay.mDirection.Dot(plane_normal); + if (abs(direction_projection) >= 1.0e-12f) + { + // Get intersection fraction between ray and plane + float fraction = -distance_to_plane / direction_projection; + + // Update interval of ray that is inside the hull + if (direction_projection < 0.0f) + { + min_fraction = max(fraction, min_fraction); + fractions_set |= 1; + } + else + { + max_fraction = min(fraction, max_fraction); + fractions_set |= 2; + } + } + else if (is_outside) + return false; // Outside the plane and parallel, no hit! + } + + // Test if both min and max have been set + if (fractions_set == 3) + { + // Output fractions + outMinFraction = min_fraction; + outMaxFraction = max_fraction; + + // Test if the infinite ray intersects with the hull (the length will be checked later) + return min_fraction <= max_fraction && max_fraction >= 0.0f; + } + else + { + // Degenerate case, either the ray is parallel to all planes or the ray has zero length + outMinFraction = 0.0f; + outMaxFraction = 1.0f + FLT_EPSILON; + + // Return if the origin is inside the hull + return all_inside; + } + } +} + +bool ConvexHullShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Determine if ray hits the shape + float min_fraction, max_fraction; + if (CastRayHelper(inRay, min_fraction, max_fraction) + && min_fraction < ioHit.mFraction) // Check if this is a closer hit + { + // Better hit than the current hit + ioHit.mFraction = min_fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void ConvexHullShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Determine if ray hits the shape + float min_fraction, max_fraction; + if (CastRayHelper(inRay, min_fraction, max_fraction) + && min_fraction < ioCollector.GetEarlyOutFraction()) // Check if this is closer than the early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side hit + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = min_fraction; + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces + && max_fraction < ioCollector.GetEarlyOutFraction()) + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void ConvexHullShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if point is behind all planes + for (const Plane &p : mPlanes) + if (p.SignedDistance(inPoint) > 0.0f) + return; + + // Point is inside + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void ConvexHullShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + Vec3 inv_scale = inScale.Reciprocal(); + bool is_not_scaled = ScaleHelpers::IsNotScaled(inScale); + float scale_flip = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f; + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + Vec3 local_pos = inverse_transform * v->mPosition; + + // Find most facing plane + float max_distance = -FLT_MAX; + Vec3 max_plane_normal = Vec3::sZero(); + uint max_plane_idx = 0; + if (is_not_scaled) + { + // Without scale, it is trivial to calculate the distance to the hull + for (const Plane &p : mPlanes) + { + float distance = p.SignedDistance(local_pos); + if (distance > max_distance) + { + max_distance = distance; + max_plane_normal = p.GetNormal(); + max_plane_idx = uint(&p - mPlanes.data()); + } + } + } + else + { + // When there's scale we need to calculate the planes first + for (uint i = 0; i < (uint)mPlanes.size(); ++i) + { + // Calculate plane normal and point by scaling the original plane + Vec3 plane_normal = (inv_scale * mPlanes[i].GetNormal()).Normalized(); + Vec3 plane_point = inScale * mPoints[mVertexIdx[mFaces[i].mFirstVertex]].mPosition; + + float distance = plane_normal.Dot(local_pos - plane_point); + if (distance > max_distance) + { + max_distance = distance; + max_plane_normal = plane_normal; + max_plane_idx = i; + } + } + } + bool is_outside = max_distance > 0.0f; + + // Project point onto that plane + Vec3 closest_point = local_pos - max_distance * max_plane_normal; + + // Check edges if we're outside the hull (when inside we know the closest face is also the closest point to the surface) + if (is_outside) + { + // Loop over edges + float closest_point_dist_sq = FLT_MAX; + const Face &face = mFaces[max_plane_idx]; + for (const uint8 *v_start = &mVertexIdx[face.mFirstVertex], *v1 = v_start, *v_end = v_start + face.mNumVertices; v1 < v_end; ++v1) + { + // Find second point + const uint8 *v2 = v1 + 1; + if (v2 == v_end) + v2 = v_start; + + // Get edge points + Vec3 p1 = inScale * mPoints[*v1].mPosition; + Vec3 p2 = inScale * mPoints[*v2].mPosition; + + // Check if the position is outside the edge (if not, the face will be closer) + Vec3 edge_normal = (p2 - p1).Cross(max_plane_normal); + if (scale_flip * edge_normal.Dot(local_pos - p1) > 0.0f) + { + // Get closest point on edge + uint32 set; + Vec3 closest = ClosestPoint::GetClosestPointOnLine(p1 - local_pos, p2 - local_pos, set); + float distance_sq = closest.LengthSq(); + if (distance_sq < closest_point_dist_sq) + closest_point = local_pos + closest; + } + } + } + + // Check if this is the largest penetration + Vec3 normal = local_pos - closest_point; + float normal_length = normal.Length(); + float penetration = normal_length; + if (is_outside) + penetration = -penetration; + else + normal = -normal; + if (penetration > v->mLargestPenetration) + { + v->mLargestPenetration = penetration; + + // Calculate contact plane + normal = normal_length > 0.0f? normal / normal_length : max_plane_normal; + Plane plane = Plane::sFromPointAndNormal(closest_point, normal); + + // Store collision + v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } +} + +class ConvexHullShape::CHSGetTrianglesContext +{ +public: + CHSGetTrianglesContext(Mat44Arg inTransform, bool inIsInsideOut) : mTransform(inTransform), mIsInsideOut(inIsInsideOut) { } + + Mat44 mTransform; + bool mIsInsideOut; + size_t mCurrentFace = 0; +}; + +void ConvexHullShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(CHSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(CHSGetTrianglesContext))); + + new (&ioContext) CHSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale), ScaleHelpers::IsInsideOut(inScale)); +} + +int ConvexHullShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 12, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + CHSGetTrianglesContext &context = (CHSGetTrianglesContext &)ioContext; + + int total_num_triangles = 0; + for (; context.mCurrentFace < mFaces.size(); ++context.mCurrentFace) + { + const Face &f = mFaces[context.mCurrentFace]; + + const uint8 *first_vtx = mVertexIdx.data() + f.mFirstVertex; + const uint8 *end_vtx = first_vtx + f.mNumVertices; + + // Check if there is still room in the output buffer for this face + int num_triangles = f.mNumVertices - 2; + inMaxTrianglesRequested -= num_triangles; + if (inMaxTrianglesRequested < 0) + break; + total_num_triangles += num_triangles; + + // Get first triangle of polygon + Vec3 v0 = context.mTransform * mPoints[first_vtx[0]].mPosition; + Vec3 v1 = context.mTransform * mPoints[first_vtx[1]].mPosition; + Vec3 v2 = context.mTransform * mPoints[first_vtx[2]].mPosition; + v0.StoreFloat3(outTriangleVertices++); + if (context.mIsInsideOut) + { + // Store first triangle in this polygon flipped + v2.StoreFloat3(outTriangleVertices++); + v1.StoreFloat3(outTriangleVertices++); + + // Store other triangles in this polygon flipped + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + { + v0.StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store first triangle in this polygon + v1.StoreFloat3(outTriangleVertices++); + v2.StoreFloat3(outTriangleVertices++); + + // Store other triangles in this polygon + for (const uint8 *v = first_vtx + 3; v < end_vtx; ++v) + { + v0.StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*(v - 1)].mPosition).StoreFloat3(outTriangleVertices++); + (context.mTransform * mPoints[*v].mPosition).StoreFloat3(outTriangleVertices++); + } + } + } + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +void ConvexHullShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mInertia); + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mPoints); + inStream.Write(mFaces); + inStream.Write(mPlanes); + inStream.Write(mVertexIdx); + inStream.Write(mConvexRadius); + inStream.Write(mVolume); + inStream.Write(mInnerRadius); +} + +void ConvexHullShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mInertia); + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mPoints); + inStream.Read(mFaces); + inStream.Read(mPlanes); + inStream.Read(mVertexIdx); + inStream.Read(mConvexRadius); + inStream.Read(mVolume); + inStream.Read(mInnerRadius); +} + +Shape::Stats ConvexHullShape::GetStats() const +{ + // Count number of triangles + uint triangle_count = 0; + for (const Face &f : mFaces) + triangle_count += f.mNumVertices - 2; + + return Stats( + sizeof(*this) + + mPoints.size() * sizeof(Point) + + mFaces.size() * sizeof(Face) + + mPlanes.size() * sizeof(Plane) + + mVertexIdx.size() * sizeof(uint8), + triangle_count); +} + +void ConvexHullShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::ConvexHull); + f.mConstruct = []() -> Shape * { return new ConvexHullShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.h new file mode 100644 index 000000000..8837a82d8 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexHullShape.h @@ -0,0 +1,202 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a ConvexHullShape +class JPH_EXPORT ConvexHullShapeSettings final : public ConvexShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConvexHullShapeSettings) + + /// Default constructor for deserialization + ConvexHullShapeSettings() = default; + + /// Create a convex hull from inPoints and maximum convex radius inMaxConvexRadius, the radius is automatically lowered if the hull requires it. + /// (internally this will be subtracted so the total size will not grow with the convex radius). + ConvexHullShapeSettings(const Vec3 *inPoints, int inNumPoints, float inMaxConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints, inPoints + inNumPoints), mMaxConvexRadius(inMaxConvexRadius) { } + ConvexHullShapeSettings(const Array &inPoints, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mPoints(inPoints), mMaxConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Array mPoints; ///< Points to create the hull from + float mMaxConvexRadius = 0.0f; ///< Convex radius as supplied by the constructor. Note that during hull creation the convex radius can be made smaller if the value is too big for the hull. + float mMaxErrorConvexRadius = 0.05f; ///< Maximum distance between the shrunk hull + convex radius and the actual hull. + float mHullTolerance = 1.0e-3f; ///< Points are allowed this far outside of the hull (increasing this yields a hull with less vertices). Note that the actual used value can be larger if the points of the hull are far apart. +}; + +/// A convex hull +class JPH_EXPORT ConvexHullShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Maximum amount of points supported in a convex hull. Note that while constructing a hull, interior points are discarded so you can provide more points. + /// The ConvexHullShapeSettings::Create function will return an error when too many points are provided. + static constexpr int cMaxPointsInHull = 256; + + /// Constructor + ConvexHullShape() : ConvexShape(EShapeSubType::ConvexHull) { } + ConvexHullShape(const ConvexHullShapeSettings &inSettings, ShapeResult &outResult); + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override { return mLocalBounds; } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + /// Debugging helper draw function that draws how all points are moved when a shape is shrunk by the convex radius + void DrawShrunkShape(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return mVolume; } + + /// Get the convex radius of this convex hull + float GetConvexRadius() const { return mConvexRadius; } + + /// Get the planes of this convex hull + const Array & GetPlanes() const { return mPlanes; } + + /// Get the number of vertices in this convex hull + inline uint GetNumPoints() const { return uint(mPoints.size()); } + + /// Get a vertex of this convex hull relative to the center of mass + inline Vec3 GetPoint(uint inIndex) const { return mPoints[inIndex].mPosition; } + + /// Get the number of faces in this convex hull + inline uint GetNumFaces() const { return uint(mFaces.size()); } + + /// Get the number of vertices in a face + inline uint GetNumVerticesInFace(uint inFaceIndex) const { return mFaces[inFaceIndex].mNumVertices; } + + /// Get the vertices indices of a face + /// @param inFaceIndex Index of the face. + /// @param inMaxVertices Maximum number of vertices to return. + /// @param outVertices Array of vertices indices, must be at least inMaxVertices in size, the vertices are returned in counter clockwise order and the positions can be obtained using GetPoint(index). + /// @return Number of vertices in face, if this is bigger than inMaxVertices, not all vertices were retrieved. + inline uint GetFaceVertices(uint inFaceIndex, uint inMaxVertices, uint *outVertices) const + { + const Face &face = mFaces[inFaceIndex]; + const uint8 *first_vertex = mVertexIdx.data() + face.mFirstVertex; + uint num_vertices = min(face.mNumVertices, inMaxVertices); + for (uint i = 0; i < num_vertices; ++i) + outVertices[i] = first_vertex[i]; + return face.mNumVertices; + } + + // Register shape functions with the registry + static void sRegister(); + +#ifdef JPH_DEBUG_RENDERER + /// Draw the outlines of the faces of the convex hull when drawing the shape + inline static bool sDrawFaceOutlines = false; +#endif // JPH_DEBUG_RENDERER + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + /// Helper function that returns the min and max fraction along the ray that hits the convex hull. Returns false if there is no hit. + bool CastRayHelper(const RayCast &inRay, float &outMinFraction, float &outMaxFraction) const; + + /// Class for GetTrianglesStart/Next + class CHSGetTrianglesContext; + + /// Classes for GetSupportFunction + class HullNoConvex; + class HullWithConvex; + class HullWithConvexScaled; + + struct Face + { + uint16 mFirstVertex; ///< First index in mVertexIdx to use + uint16 mNumVertices = 0; ///< Number of vertices in the mVertexIdx to use + }; + + static_assert(sizeof(Face) == 4, "Unexpected size"); + static_assert(alignof(Face) == 2, "Unexpected alignment"); + + struct Point + { + Vec3 mPosition; ///< Position of vertex + int mNumFaces = 0; ///< Number of faces in the face array below + int mFaces[3] = { -1, -1, -1 }; ///< Indices of 3 neighboring faces with the biggest difference in normal (used to shift vertices for convex radius) + }; + + static_assert(sizeof(Point) == 32, "Unexpected size"); + static_assert(alignof(Point) == JPH_VECTOR_ALIGNMENT, "Unexpected alignment"); + + Vec3 mCenterOfMass; ///< Center of mass of this convex hull + Mat44 mInertia; ///< Inertia matrix assuming density is 1 (needs to be multiplied by density) + AABox mLocalBounds; ///< Local bounding box for the convex hull + Array mPoints; ///< Points on the convex hull surface + Array mFaces; ///< Faces of the convex hull surface + Array mPlanes; ///< Planes for the faces (1-on-1 with mFaces array, separate because they need to be 16 byte aligned) + Array mVertexIdx; ///< A list of vertex indices (indexing in mPoints) for each of the faces + float mConvexRadius = 0.0f; ///< Convex radius + float mVolume; ///< Total volume of the convex hull + float mInnerRadius = FLT_MAX; ///< Radius of the biggest sphere that fits entirely in the convex hull + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.cpp new file mode 100644 index 000000000..017ec8dc3 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.cpp @@ -0,0 +1,559 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(ConvexShapeSettings) +{ + JPH_ADD_BASE_CLASS(ConvexShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mDensity) + JPH_ADD_ATTRIBUTE(ConvexShapeSettings, mMaterial) +} + +const StaticArray ConvexShape::sUnitSphereTriangles = []() { + const int level = 2; + + StaticArray verts; + GetTrianglesContextVertexList::sCreateHalfUnitSphereTop(verts, level); + GetTrianglesContextVertexList::sCreateHalfUnitSphereBottom(verts, level); + return verts; +}(); + +void ConvexShape::sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + const ConvexShape *shape2 = static_cast(inShape2); + + // Get transforms + Mat44 inverse_transform1 = inCenterOfMassTransform1.InversedRotationTranslation(); + Mat44 transform_2_to_1 = inverse_transform1 * inCenterOfMassTransform2; + + // Get bounding boxes + AABox shape1_bbox = shape1->GetLocalBounds().Scaled(inScale1); + shape1_bbox.ExpandBy(Vec3::sReplicate(inCollideShapeSettings.mMaxSeparationDistance)); + AABox shape2_bbox = shape2->GetLocalBounds().Scaled(inScale2); + + // Check if they overlap + if (!OrientedBox(transform_2_to_1, shape2_bbox).Overlaps(shape1_bbox)) + return; + + // Note: As we don't remember the penetration axis from the last iteration, and it is likely that shape2 is pushed out of + // collision relative to shape1 by comparing their COM's, we use that as an initial penetration axis: shape2.com - shape1.com + // This has been seen to improve performance by approx. 1% over using a fixed axis like (1, 0, 0). + Vec3 penetration_axis = transform_2_to_1.GetTranslation(); + + // Ensure that we do not pass in a near zero penetration axis + if (penetration_axis.IsNearZero()) + penetration_axis = Vec3::sAxisX(); + + Vec3 point1, point2; + EPAPenetrationDepth pen_depth; + EPAPenetrationDepth::EStatus status; + + // Scope to limit lifetime of SupportBuffer + { + // Create support function + SupportBuffer buffer1_excl_cvx_radius, buffer2_excl_cvx_radius; + const Support *shape1_excl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer1_excl_cvx_radius, inScale1); + const Support *shape2_excl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::ExcludeConvexRadius, buffer2_excl_cvx_radius, inScale2); + + // Transform shape 2 in the space of shape 1 + TransformedConvexObject transformed2_excl_cvx_radius(transform_2_to_1, *shape2_excl_cvx_radius); + + // Perform GJK step + status = pen_depth.GetPenetrationDepthStepGJK(*shape1_excl_cvx_radius, shape1_excl_cvx_radius->GetConvexRadius() + inCollideShapeSettings.mMaxSeparationDistance, transformed2_excl_cvx_radius, shape2_excl_cvx_radius->GetConvexRadius(), inCollideShapeSettings.mCollisionTolerance, penetration_axis, point1, point2); + } + + // Check result of collision detection + switch (status) + { + case EPAPenetrationDepth::EStatus::Colliding: + break; + + case EPAPenetrationDepth::EStatus::NotColliding: + return; + + case EPAPenetrationDepth::EStatus::Indeterminate: + { + // Need to run expensive EPA algorithm + + // Create support function + SupportBuffer buffer1_incl_cvx_radius, buffer2_incl_cvx_radius; + const Support *shape1_incl_cvx_radius = shape1->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer1_incl_cvx_radius, inScale1); + const Support *shape2_incl_cvx_radius = shape2->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer2_incl_cvx_radius, inScale2); + + // Add separation distance + AddConvexRadius shape1_add_max_separation_distance(*shape1_incl_cvx_radius, inCollideShapeSettings.mMaxSeparationDistance); + + // Transform shape 2 in the space of shape 1 + TransformedConvexObject transformed2_incl_cvx_radius(transform_2_to_1, *shape2_incl_cvx_radius); + + // Perform EPA step + if (!pen_depth.GetPenetrationDepthStepEPA(shape1_add_max_separation_distance, transformed2_incl_cvx_radius, inCollideShapeSettings.mPenetrationTolerance, penetration_axis, point1, point2)) + return; + break; + } + } + + // Check if the penetration is bigger than the early out fraction + float penetration_depth = (point2 - point1).Length() - inCollideShapeSettings.mMaxSeparationDistance; + if (-penetration_depth >= ioCollector.GetEarlyOutFraction()) + return; + + // Correct point1 for the added separation distance + float penetration_axis_len = penetration_axis.Length(); + if (penetration_axis_len > 0.0f) + point1 -= penetration_axis * (inCollideShapeSettings.mMaxSeparationDistance / penetration_axis_len); + + // Convert to world space + point1 = inCenterOfMassTransform1 * point1; + point2 = inCenterOfMassTransform1 * point2; + Vec3 penetration_axis_world = inCenterOfMassTransform1.Multiply3x3(penetration_axis); + + // Create collision result + CollideShapeResult result(point1, point2, penetration_axis_world, penetration_depth, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Gather faces + if (inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + shape1->GetSupportingFace(SubShapeID(), -penetration_axis, inScale1, inCenterOfMassTransform1, result.mShape1Face); + + // Get supporting face of shape 2 + shape2->GetSupportingFace(SubShapeID(), transform_2_to_1.Multiply3x3Transposed(penetration_axis), inScale2, inCenterOfMassTransform2, result.mShape2Face); + } + + // Notify the collector + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); +} + +bool ConvexShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Note: This is a fallback routine, most convex shapes should implement a more performant version! + + JPH_PROFILE_FUNCTION(); + + // Create support function + SupportBuffer buffer; + const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + + // Cast ray + GJKClosestPoint gjk; + if (gjk.CastRay(inRay.mOrigin, inRay.mDirection, cDefaultCollisionTolerance, *support, ioHit.mFraction)) + { + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + + return false; +} + +void ConvexShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Note: This is a fallback routine, most convex shapes should implement a more performant version! + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // First do a normal raycast, limited to the early out fraction + RayCastResult hit; + hit.mFraction = ioCollector.GetEarlyOutFraction(); + if (CastRay(inRay, inSubShapeIDCreator, hit)) + { + // Check front side + if (inRayCastSettings.mTreatConvexAsSolid || hit.mFraction > 0.0f) + { + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + ioCollector.AddHit(hit); + } + + // Check if we want back facing hits and the collector still accepts additional hits + if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces && !ioCollector.ShouldEarlyOut()) + { + // Invert the ray, going from the early out fraction back to the fraction where we found our forward hit + float start_fraction = min(1.0f, ioCollector.GetEarlyOutFraction()); + float delta_fraction = hit.mFraction - start_fraction; + if (delta_fraction < 0.0f) + { + RayCast inverted_ray { inRay.mOrigin + start_fraction * inRay.mDirection, delta_fraction * inRay.mDirection }; + + // Cast another ray + RayCastResult inverted_hit; + inverted_hit.mFraction = 1.0f; + if (CastRay(inverted_ray, inSubShapeIDCreator, inverted_hit) + && inverted_hit.mFraction > 0.0f) // Ignore hits with fraction 0, this means the ray ends inside the object and we don't want to report it as a back facing hit + { + // Invert fraction and rescale it to the fraction of the original ray + inverted_hit.mFraction = hit.mFraction + (inverted_hit.mFraction - 1.0f) * delta_fraction; + inverted_hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + ioCollector.AddHit(inverted_hit); + } + } + } + } +} + +void ConvexShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // First test bounding box + if (GetLocalBounds().Contains(inPoint)) + { + // Create support function + SupportBuffer buffer; + const Support *support = GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + + // Create support function for point + PointConvexSupport point { inPoint }; + + // Test intersection + GJKClosestPoint gjk; + Vec3 v = inPoint; + if (gjk.Intersects(*support, point, cDefaultCollisionTolerance, v)) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); + } +} + +void ConvexShape::sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + // Only supported for convex shapes + JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex); + const ConvexShape *cast_shape = static_cast(inShapeCast.mShape); + + JPH_ASSERT(inShape->GetType() == EShapeType::Convex); + const ConvexShape *shape = static_cast(inShape); + + // Determine if we want to use the actual shape or a shrunken shape with convex radius + ConvexShape::ESupportMode support_mode = inShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default; + + // Create support function for shape to cast + SupportBuffer cast_buffer; + const Support *cast_support = cast_shape->GetSupportFunction(support_mode, cast_buffer, inShapeCast.mScale); + + // Create support function for target shape + SupportBuffer target_buffer; + const Support *target_support = shape->GetSupportFunction(support_mode, target_buffer, inScale); + + // Do a raycast against the result + EPAPenetrationDepth epa; + float fraction = ioCollector.GetEarlyOutFraction(); + Vec3 contact_point_a, contact_point_b, contact_normal; + if (epa.CastShape(inShapeCast.mCenterOfMassStart, inShapeCast.mDirection, inShapeCastSettings.mCollisionTolerance, inShapeCastSettings.mPenetrationTolerance, *cast_support, *target_support, cast_support->GetConvexRadius(), target_support->GetConvexRadius(), inShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal) + && (inShapeCastSettings.mBackFaceModeConvex == EBackFaceMode::CollideWithBackFaces + || contact_normal.Dot(inShapeCast.mDirection) > 0.0f)) // Test if backfacing + { + // Convert to world space + contact_point_a = inCenterOfMassTransform2 * contact_point_a; + contact_point_b = inCenterOfMassTransform2 * contact_point_b; + Vec3 contact_normal_world = inCenterOfMassTransform2.Multiply3x3(contact_normal); + + ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, false, inSubShapeIDCreator1.GetID(), inSubShapeIDCreator2.GetID(), TransformedShape::sGetBodyID(ioCollector.GetContext())); + + // Early out if this hit is deeper than the collector's early out value + if (fraction == 0.0f && -result.mPenetrationDepth >= ioCollector.GetEarlyOutFraction()) + return; + + // Gather faces + if (inShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces) + { + // Get supporting face of shape 1 + Mat44 transform_1_to_2 = inShapeCast.mCenterOfMassStart; + transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * inShapeCast.mDirection); + cast_shape->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), inShapeCast.mScale, inCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face); + + // Get supporting face of shape 2 + shape->GetSupportingFace(SubShapeID(), contact_normal, inScale, inCenterOfMassTransform2, result.mShape2Face); + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) + ioCollector.AddHit(result); + } +} + +class ConvexShape::CSGetTrianglesContext +{ +public: + CSGetTrianglesContext(const ConvexShape *inShape, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + mSupport = inShape->GetSupportFunction(ESupportMode::IncludeConvexRadius, mSupportBuffer, Vec3::sReplicate(1.0f)); + } + + SupportBuffer mSupportBuffer; + const Support * mSupport; + Mat44 mLocalToWorld; + bool mIsInsideOut; + size_t mCurrentVertex = 0; +}; + +void ConvexShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(CSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(CSGetTrianglesContext))); + + new (&ioContext) CSGetTrianglesContext(this, inPositionCOM, inRotation, inScale); +} + +int ConvexShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + CSGetTrianglesContext &context = (CSGetTrianglesContext &)ioContext; + + int total_num_vertices = min(inMaxTrianglesRequested * 3, int(sUnitSphereTriangles.size() - context.mCurrentVertex)); + + if (context.mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = sUnitSphereTriangles.data() + context.mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (context.mLocalToWorld * context.mSupport->GetSupport(v[0])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[1])).StoreFloat3(outTriangleVertices++); + (context.mLocalToWorld * context.mSupport->GetSupport(v[2])).StoreFloat3(outTriangleVertices++); + } + } + + context.mCurrentVertex += total_num_vertices; + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + { + const PhysicsMaterial *material = GetMaterial(); + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = material; + } + + return total_num_triangles; +} + +void ConvexShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Calculate total volume + Vec3 abs_scale = inScale.Abs(); + Vec3 extent = GetLocalBounds().GetExtent() * abs_scale; + outTotalVolume = 8.0f * extent.GetX() * extent.GetY() * extent.GetZ(); + + // Points of the bounding box + Vec3 points[] = + { + Vec3(-1, -1, -1), + Vec3( 1, -1, -1), + Vec3(-1, 1, -1), + Vec3( 1, 1, -1), + Vec3(-1, -1, 1), + Vec3( 1, -1, 1), + Vec3(-1, 1, 1), + Vec3( 1, 1, 1), + }; + + // Faces of the bounding box + using Face = int[5]; + #define MAKE_FACE(a, b, c, d) { a, b, c, d, ((1 << a) | (1 << b) | (1 << c) | (1 << d)) } // Last int is a bit mask that indicates which indices are used + Face faces[] = + { + MAKE_FACE(0, 2, 3, 1), + MAKE_FACE(4, 6, 2, 0), + MAKE_FACE(4, 5, 7, 6), + MAKE_FACE(1, 3, 7, 5), + MAKE_FACE(2, 6, 7, 3), + MAKE_FACE(0, 1, 5, 4), + }; + + PolyhedronSubmergedVolumeCalculator::Point *buffer = (PolyhedronSubmergedVolumeCalculator::Point *)JPH_STACK_ALLOC(8 * sizeof(PolyhedronSubmergedVolumeCalculator::Point)); + PolyhedronSubmergedVolumeCalculator submerged_vol_calc(inCenterOfMassTransform * Mat44::sScale(extent), points, sizeof(Vec3), 8, inSurface, buffer JPH_IF_DEBUG_RENDERER(, inBaseOffset)); + + if (submerged_vol_calc.AreAllAbove()) + { + // We're above the water + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (submerged_vol_calc.AreAllBelow()) + { + // We're fully submerged + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Calculate submerged volume + int reference_point_bit = 1 << submerged_vol_calc.GetReferencePointIdx(); + for (const Face &f : faces) + { + // Test if this face includes the reference point + if ((f[4] & reference_point_bit) == 0) + { + // Triangulate the face (a quad) + submerged_vol_calc.AddFace(f[0], f[1], f[2]); + submerged_vol_calc.AddFace(f[0], f[2], f[3]); + } + } + + submerged_vol_calc.GetResult(outSubmergedVolume, outCenterOfBuoyancy); + } +} + +#ifdef JPH_DEBUG_RENDERER +void ConvexShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + // Get the support function with convex radius + SupportBuffer buffer; + const Support *support = GetSupportFunction(ESupportMode::ExcludeConvexRadius, buffer, inScale); + AddConvexRadius add_convex(*support, support->GetConvexRadius()); + + // Draw the shape + DebugRenderer::GeometryRef geometry = inRenderer->CreateTriangleGeometryForConvex([&add_convex](Vec3Arg inDirection) { return add_convex.GetSupport(inDirection); }); + AABox bounds = geometry->mBounds.Transformed(inCenterOfMassTransform); + float lod_scale_sq = geometry->mBounds.GetExtent().LengthSq(); + inRenderer->DrawGeometry(inCenterOfMassTransform, bounds, lod_scale_sq, inColor, geometry); + + if (inDrawSupportDirection) + { + // Iterate on all directions and draw the support point and an arrow in the direction that was sampled to test if the support points make sense + for (Vec3 v : Vec3::sUnitSphere) + { + Vec3 direction = 0.05f * v; + Vec3 pos = add_convex.GetSupport(direction); + RVec3 from = inCenterOfMassTransform * pos; + RVec3 to = inCenterOfMassTransform * (pos + direction); + inRenderer->DrawMarker(from, Color::sWhite, 0.001f); + inRenderer->DrawArrow(from, to, Color::sWhite, 0.001f); + } + } +} + +void ConvexShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + // Sample directions and map which faces belong to which directions + using FaceToDirection = UnorderedMap>; + FaceToDirection faces; + for (Vec3 v : Vec3::sUnitSphere) + { + Vec3 direction = 0.05f * v; + + SupportingFace face; + GetSupportingFace(SubShapeID(), direction, inScale, Mat44::sIdentity(), face); + + if (!face.empty()) + { + JPH_ASSERT(face.size() >= 2, "The GetSupportingFace function should either return nothing or at least an edge"); + faces[face].push_back(direction); + } + } + + // Draw each face in a unique color and draw corresponding directions + int color_it = 0; + for (FaceToDirection::value_type &ftd : faces) + { + Color color = Color::sGetDistinctColor(color_it++); + + // Create copy of face (key in map is read only) + SupportingFace face = ftd.first; + + // Displace the face a little bit forward so it is easier to see + Vec3 normal = face.size() >= 3? (face[2] - face[1]).Cross(face[0] - face[1]).Normalized() : Vec3::sZero(); + Vec3 displacement = 0.001f * normal; + + // Transform face to world space and calculate center of mass + Vec3 com_ls = Vec3::sZero(); + for (Vec3 &v : face) + { + v = inCenterOfMassTransform.Multiply3x3(v + displacement); + com_ls += v; + } + RVec3 com = inCenterOfMassTransform.GetTranslation() + com_ls / (float)face.size(); + + // Draw the polygon and directions + inRenderer->DrawWirePolygon(RMat44::sTranslation(inCenterOfMassTransform.GetTranslation()), face, color, face.size() >= 3? 0.001f : 0.0f); + if (face.size() >= 3) + inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(normal), color, 0.01f); + for (Vec3 &v : ftd.second) + inRenderer->DrawArrow(com, com + inCenterOfMassTransform.Multiply3x3(-v), color, 0.001f); + } +} +#endif // JPH_DEBUG_RENDERER + +void ConvexShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mDensity); +} + +void ConvexShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mDensity); +} + +void ConvexShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials.clear(); + outMaterials.push_back(mMaterial); +} + +void ConvexShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + JPH_ASSERT(inNumMaterials == 1); + mMaterial = inMaterials[0]; +} + +void ConvexShape::sRegister() +{ + for (EShapeSubType s1 : sConvexSubShapeTypes) + for (EShapeSubType s2 : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s1, s2, sCollideConvexVsConvex); + CollisionDispatch::sRegisterCastShape(s1, s2, sCastConvexVsConvex); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.h new file mode 100644 index 000000000..af87abdcf --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/ConvexShape.h @@ -0,0 +1,150 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a ConvexShape (abstract) +class JPH_EXPORT ConvexShapeSettings : public ShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ConvexShapeSettings) + + /// Constructor + ConvexShapeSettings() = default; + explicit ConvexShapeSettings(const PhysicsMaterial *inMaterial) : mMaterial(inMaterial) { } + + /// Set the density of the object in kg / m^3 + void SetDensity(float inDensity) { mDensity = inDensity; } + + // Properties + RefConst mMaterial; ///< Material assigned to this shape + float mDensity = 1000.0f; ///< Uniform density of the interior of the convex object (kg / m^3) +}; + +/// Base class for all convex shapes. Defines a virtual interface. +class JPH_EXPORT ConvexShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ConvexShape(EShapeSubType inSubType) : Shape(EShapeType::Convex, inSubType) { } + ConvexShape(EShapeSubType inSubType, const ConvexShapeSettings &inSettings, ShapeResult &outResult) : Shape(EShapeType::Convex, inSubType, inSettings, outResult), mMaterial(inSettings.mMaterial), mDensity(inSettings.mDensity) { } + ConvexShape(EShapeSubType inSubType, const PhysicsMaterial *inMaterial) : Shape(EShapeType::Convex, inSubType), mMaterial(inMaterial) { } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return 0; } // Convex shapes don't have sub shapes + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial([[maybe_unused]] const SubShapeID &inSubShapeID) const override { JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); return GetMaterial(); } + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + + /// Function that provides an interface for GJK + class Support + { + public: + /// Warning: Virtual destructor will not be called on this object! + virtual ~Support() = default; + + /// Calculate the support vector for this convex shape (includes / excludes the convex radius depending on how this was obtained). + /// Support vector is relative to the center of mass of the shape. + virtual Vec3 GetSupport(Vec3Arg inDirection) const = 0; + + /// Convex radius of shape. Collision detection on penetrating shapes is much more expensive, + /// so you can add a radius around objects to increase the shape. This makes it far less likely that they will actually penetrate. + virtual float GetConvexRadius() const = 0; + }; + + /// Buffer to hold a Support object, used to avoid dynamic memory allocations + class alignas(16) SupportBuffer + { + public: + uint8 mData[4160]; + }; + + /// How the GetSupport function should behave + enum class ESupportMode + { + ExcludeConvexRadius, ///< Return the shape excluding the convex radius, Support::GetConvexRadius will return the convex radius if there is one, but adding this radius may not result in the most accurate/efficient representation of shapes with sharp edges + IncludeConvexRadius, ///< Return the shape including the convex radius, Support::GetSupport includes the convex radius if there is one, Support::GetConvexRadius will return 0 + Default, ///< Use both Support::GetSupport add Support::GetConvexRadius to get a support point that matches the original shape as accurately/efficiently as possible + }; + + /// Returns an object that provides the GetSupport function for this shape. + /// inMode determines if this support function includes or excludes the convex radius. + /// of the values returned by the GetSupport function. This improves numerical accuracy of the results. + /// inScale scales this shape in local space. + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const = 0; + + /// Material of the shape + void SetMaterial(const PhysicsMaterial *inMaterial) { mMaterial = inMaterial; } + const PhysicsMaterial * GetMaterial() const { return mMaterial != nullptr? mMaterial : PhysicsMaterial::sDefault; } + + /// Set density of the shape (kg / m^3) + void SetDensity(float inDensity) { mDensity = inDensity; } + + /// Get density of the shape (kg / m^3) + float GetDensity() const { return mDensity; } + +#ifdef JPH_DEBUG_RENDERER + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + + /// Vertex list that forms a unit sphere + static const StaticArray sUnitSphereTriangles; + +private: + // Class for GetTrianglesStart/Next + class CSGetTrianglesContext; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsConvex(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsConvex(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Properties + RefConst mMaterial; ///< Material assigned to this shape + float mDensity = 1000.0f; ///< Uniform density of the interior of the convex object (kg / m^3) +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.cpp new file mode 100644 index 000000000..b08231cbb --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.cpp @@ -0,0 +1,417 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(CylinderShapeSettings) +{ + JPH_ADD_BASE_CLASS(CylinderShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mHalfHeight) + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mRadius) + JPH_ADD_ATTRIBUTE(CylinderShapeSettings, mConvexRadius) +} + +// Approximation of top face with 8 vertices +static const float cSin45 = 0.70710678118654752440084436210485f; +static const Vec3 cTopFace[] = +{ + Vec3(0.0f, 1.0f, 1.0f), + Vec3(cSin45, 1.0f, cSin45), + Vec3(1.0f, 1.0f, 0.0f), + Vec3(cSin45, 1.0f, -cSin45), + Vec3(-0.0f, 1.0f, -1.0f), + Vec3(-cSin45, 1.0f, -cSin45), + Vec3(-1.0f, 1.0f, 0.0f), + Vec3(-cSin45, 1.0f, cSin45) +}; + +static const StaticArray sUnitCylinderTriangles = []() { + StaticArray verts; + + const Vec3 bottom_offset(0.0f, -2.0f, 0.0f); + + int num_verts = sizeof(cTopFace) / sizeof(Vec3); + for (int i = 0; i < num_verts; ++i) + { + Vec3 t1 = cTopFace[i]; + Vec3 t2 = cTopFace[(i + 1) % num_verts]; + Vec3 b1 = cTopFace[i] + bottom_offset; + Vec3 b2 = cTopFace[(i + 1) % num_verts] + bottom_offset; + + // Top + verts.emplace_back(0.0f, 1.0f, 0.0f); + verts.push_back(t1); + verts.push_back(t2); + + // Bottom + verts.emplace_back(0.0f, -1.0f, 0.0f); + verts.push_back(b2); + verts.push_back(b1); + + // Side + verts.push_back(t1); + verts.push_back(b1); + verts.push_back(t2); + + verts.push_back(t2); + verts.push_back(b1); + verts.push_back(b2); + } + + return verts; +}(); + +ShapeSettings::ShapeResult CylinderShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new CylinderShape(*this, mCachedResult); + return mCachedResult; +} + +CylinderShape::CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Cylinder, inSettings, outResult), + mHalfHeight(inSettings.mHalfHeight), + mRadius(inSettings.mRadius), + mConvexRadius(inSettings.mConvexRadius) +{ + if (inSettings.mHalfHeight < inSettings.mConvexRadius) + { + outResult.SetError("Invalid height"); + return; + } + + if (inSettings.mRadius < inSettings.mConvexRadius) + { + outResult.SetError("Invalid radius"); + return; + } + + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + outResult.Set(this); +} + +CylinderShape::CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) : + ConvexShape(EShapeSubType::Cylinder, inMaterial), + mHalfHeight(inHalfHeight), + mRadius(inRadius), + mConvexRadius(inConvexRadius) +{ + JPH_ASSERT(inHalfHeight >= inConvexRadius); + JPH_ASSERT(inRadius >= inConvexRadius); + JPH_ASSERT(inConvexRadius >= 0.0f); +} + +class CylinderShape::Cylinder final : public Support +{ +public: + Cylinder(float inHalfHeight, float inRadius, float inConvexRadius) : + mHalfHeight(inHalfHeight), + mRadius(inRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(Cylinder) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(Cylinder))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Support mapping, taken from: + // A Fast and Robust GJK Implementation for Collision Detection of Convex Objects - Gino van den Bergen + // page 8 + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + if (o > 0.0f) + return Vec3((mRadius * x) / o, Sign(y) * mHalfHeight, (mRadius * z) / o); + else + return Vec3(0, Sign(y) * mHalfHeight, 0); + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mHalfHeight; + float mRadius; + float mConvexRadius; +}; + +const ConvexShape::Support *CylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = abs_scale.GetY(); + float scaled_half_height = scale_y * mHalfHeight; + float scaled_radius = scale_xz * mRadius; + float scaled_convex_radius = ScaleHelpers::ScaleConvexRadius(mConvexRadius, inScale); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) Cylinder(scaled_half_height, scaled_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) Cylinder(scaled_half_height - scaled_convex_radius, scaled_radius - scaled_convex_radius, scaled_convex_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +void CylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = abs_scale.GetY(); + float scaled_half_height = scale_y * mHalfHeight; + float scaled_radius = scale_xz * mRadius; + + float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ(); + float o = sqrt(Square(x) + Square(z)); + + // If o / |y| > scaled_radius / scaled_half_height, we're hitting the side + if (o * scaled_half_height > scaled_radius * abs(y)) + { + // Hitting side + float f = -scaled_radius / o; + float vx = x * f; + float vz = z * f; + outVertices.push_back(inCenterOfMassTransform * Vec3(vx, scaled_half_height, vz)); + outVertices.push_back(inCenterOfMassTransform * Vec3(vx, -scaled_half_height, vz)); + } + else + { + // Hitting top or bottom + Vec3 multiplier = y < 0.0f? Vec3(scaled_radius, scaled_half_height, scaled_radius) : Vec3(-scaled_radius, -scaled_half_height, scaled_radius); + Mat44 transform = inCenterOfMassTransform.PreScaled(multiplier); + for (const Vec3 &v : cTopFace) + outVertices.push_back(transform * v); + } +} + +MassProperties CylinderShape::GetMassProperties() const +{ + MassProperties p; + + // Mass is surface of circle * height + float radius_sq = Square(mRadius); + float height = 2.0f * mHalfHeight; + p.mMass = JPH_PI * radius_sq * height * GetDensity(); + + // Inertia according to https://en.wikipedia.org/wiki/List_of_moments_of_inertia: + float inertia_y = radius_sq * p.mMass * 0.5f; + float inertia_x = inertia_y * 0.5f + p.mMass * height * height / 12.0f; + float inertia_z = inertia_x; + + // Set inertia + p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z)); + + return p; +} + +Vec3 CylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Calculate distance to infinite cylinder surface + Vec3 local_surface_position_xz(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()); + float local_surface_position_xz_len = local_surface_position_xz.Length(); + float distance_to_curved_surface = abs(local_surface_position_xz_len - mRadius); + + // Calculate distance to top or bottom plane + float distance_to_top_or_bottom = abs(abs(inLocalSurfacePosition.GetY()) - mHalfHeight); + + // Return normal according to closest surface + if (distance_to_curved_surface < distance_to_top_or_bottom) + return local_surface_position_xz / local_surface_position_xz_len; + else + return inLocalSurfacePosition.GetY() > 0.0f? Vec3::sAxisY() : -Vec3::sAxisY(); +} + +AABox CylinderShape::GetLocalBounds() const +{ + Vec3 extent = Vec3(mRadius, mHalfHeight, mRadius); + return AABox(-extent, extent); +} + +#ifdef JPH_DEBUG_RENDERER +void CylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawCylinder(inCenterOfMassTransform * Mat44::sScale(inScale.Abs()), mHalfHeight, mRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool CylinderShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Test ray against capsule + float fraction = RayCylinder(inRay.mOrigin, inRay.mDirection, mHalfHeight, mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void CylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Check if the point is in the cylinder + if (abs(inPoint.GetY()) <= mHalfHeight // Within the height + && Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mRadius)) // Within the radius + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void CylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled cylinder + Vec3 abs_scale = inScale.Abs(); + float half_height = abs_scale.GetY() * mHalfHeight; + float radius = abs_scale.GetX() * mRadius; + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + Vec3 local_pos = inverse_transform * v->mPosition; + + // Calculate penetration into side surface + Vec3 side_normal = local_pos; + side_normal.SetY(0.0f); + float side_normal_length = side_normal.Length(); + float side_penetration = radius - side_normal_length; + + // Calculate penetration into top or bottom plane + float top_penetration = half_height - abs(local_pos.GetY()); + + Vec3 point, normal; + if (side_penetration < 0.0f && top_penetration < 0.0f) + { + // We're outside the cylinder height and radius + point = side_normal * (radius / side_normal_length) + Vec3(0, half_height * Sign(local_pos.GetY()), 0); + normal = (local_pos - point).NormalizedOr(Vec3::sAxisY()); + } + else if (side_penetration < top_penetration) + { + // Side surface is closest + normal = side_normal_length > 0.0f? side_normal / side_normal_length : Vec3::sAxisX(); + point = radius * normal; + } + else + { + // Top or bottom plane is closest + normal = Vec3(0, Sign(local_pos.GetY()), 0); + point = half_height * normal; + } + + // Calculate penetration + Plane plane = Plane::sFromPointAndNormal(point, normal); + float penetration = -plane.SignedDistance(local_pos); + if (penetration > v->mLargestPenetration) + { + v->mLargestPenetration = penetration; + + // Store collision + v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } +} + +void CylinderShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + Vec3 abs_scale = scale.Abs(); + float xz = 0.5f * (abs_scale.GetX() + abs_scale.GetZ()); + ts.SetShapeScale(Vec3(xz, abs_scale.GetY(), xz)); + ioCollector.AddHit(ts); +} + +void CylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + Mat44 unit_cylinder_transform(Vec4(mRadius, 0, 0, 0), Vec4(0, mHalfHeight, 0, 0), Vec4(0, 0, mRadius, 0), Vec4(0, 0, 0, 1)); + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, inScale, unit_cylinder_transform, sUnitCylinderTriangles.data(), sUnitCylinderTriangles.size(), GetMaterial()); +} + +int CylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void CylinderShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mHalfHeight); + inStream.Write(mRadius); + inStream.Write(mConvexRadius); +} + +void CylinderShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mHalfHeight); + inStream.Read(mRadius); + inStream.Read(mConvexRadius); +} + +bool CylinderShape::IsValidScale(Vec3Arg inScale) const +{ + // X and Z need same scale + Vec3 abs_scale = inScale.Abs(); + return ConvexShape::IsValidScale(inScale) && abs_scale.Swizzle().IsClose(abs_scale, ScaleHelpers::cScaleToleranceSq); +} + +void CylinderShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Cylinder); + f.mConstruct = []() -> Shape * { return new CylinderShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.h new file mode 100644 index 000000000..52108b13e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/CylinderShape.h @@ -0,0 +1,126 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a CylinderShape +class JPH_EXPORT CylinderShapeSettings final : public ConvexShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, CylinderShapeSettings) + + /// Default constructor for deserialization + CylinderShapeSettings() = default; + + /// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius. + /// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit). + CylinderShapeSettings(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mHalfHeight(inHalfHeight), mRadius(inRadius), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeight = 0.0f; + float mRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +/// A cylinder +class JPH_EXPORT CylinderShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + CylinderShape() : ConvexShape(EShapeSubType::Cylinder) { } + CylinderShape(const CylinderShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a shape centered around the origin with one top at (0, -inHalfHeight, 0) and the other at (0, inHalfHeight, 0) and radius inRadius. + /// (internally the convex radius will be subtracted from the cylinder the total cylinder will not grow with the convex radius, but the edges of the cylinder will be rounded a bit). + CylinderShape(float inHalfHeight, float inRadius, float inConvexRadius = cDefaultConvexRadius, const PhysicsMaterial *inMaterial = nullptr); + + /// Get half height of cylinder + float GetHalfHeight() const { return mHalfHeight; } + + /// Get radius of cylinder + float GetRadius() const { return mRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mHalfHeight, mRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + using ConvexShape::CastRay; + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 2.0f * JPH_PI * mHalfHeight * Square(mRadius); } + + /// Get the convex radius of this cylinder + float GetConvexRadius() const { return mConvexRadius; } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class Cylinder; + + float mHalfHeight = 0.0f; + float mRadius = 0.0f; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.cpp new file mode 100644 index 000000000..339f78363 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.cpp @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(DecoratedShapeSettings) +{ + JPH_ADD_BASE_CLASS(DecoratedShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(DecoratedShapeSettings, mInnerShape) +} + +DecoratedShape::DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Decorated, inSubType, inSettings, outResult) +{ + // Check that there's a shape + if (inSettings.mInnerShape == nullptr && inSettings.mInnerShapePtr == nullptr) + { + outResult.SetError("Inner shape is null!"); + return; + } + + if (inSettings.mInnerShapePtr != nullptr) + { + // Use provided shape + mInnerShape = inSettings.mInnerShapePtr; + } + else + { + // Create child shape + ShapeResult child_result = inSettings.mInnerShape->Create(); + if (!child_result.IsValid()) + { + outResult = child_result; + return; + } + mInnerShape = child_result.Get(); + } +} + +const PhysicsMaterial *DecoratedShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + return mInnerShape->GetMaterial(inSubShapeID); +} + +void DecoratedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform, outVertices); +} + +uint64 DecoratedShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const +{ + return mInnerShape->GetSubShapeUserData(inSubShapeID); +} + +void DecoratedShape::SaveSubShapeState(ShapeList &outSubShapes) const +{ + outSubShapes.clear(); + outSubShapes.push_back(mInnerShape); +} + +void DecoratedShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) +{ + JPH_ASSERT(inNumShapes == 1); + mInnerShape = inSubShapes[0]; +} + +Shape::Stats DecoratedShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + // Get own stats + Stats stats = Shape::GetStatsRecursive(ioVisitedShapes); + + // Add child stats + Stats child_stats = mInnerShape->GetStatsRecursive(ioVisitedShapes); + stats.mSizeBytes += child_stats.mSizeBytes; + stats.mNumTriangles += child_stats.mNumTriangles; + + return stats; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.h new file mode 100644 index 000000000..5c97dcebc --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/DecoratedShape.h @@ -0,0 +1,70 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a DecoratedShape +class JPH_EXPORT DecoratedShapeSettings : public ShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DecoratedShapeSettings) + + /// Default constructor for deserialization + DecoratedShapeSettings() = default; + + /// Constructor that decorates another shape + explicit DecoratedShapeSettings(const ShapeSettings *inShape) : mInnerShape(inShape) { } + explicit DecoratedShapeSettings(const Shape *inShape) : mInnerShapePtr(inShape) { } + + RefConst mInnerShape; ///< Sub shape (either this or mShapePtr needs to be filled up) + RefConst mInnerShapePtr; ///< Sub shape (either this or mShape needs to be filled up) +}; + +/// Base class for shapes that decorate another shape with extra functionality (e.g. scale, translation etc.) +class JPH_EXPORT DecoratedShape : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit DecoratedShape(EShapeSubType inSubType) : Shape(EShapeType::Decorated, inSubType) { } + DecoratedShape(EShapeSubType inSubType, const Shape *inInnerShape) : Shape(EShapeType::Decorated, inSubType), mInnerShape(inInnerShape) { } + DecoratedShape(EShapeSubType inSubType, const DecoratedShapeSettings &inSettings, ShapeResult &outResult); + + /// Access to the decorated inner shape + const Shape * GetInnerShape() const { return mInnerShape; } + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return mInnerShape->MustBeStatic(); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mInnerShape->GetCenterOfMass(); } + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return mInnerShape->GetSubShapeIDBitsRecursive(); } + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubShapeUserData + virtual uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const override; + + // See Shape + virtual void SaveSubShapeState(ShapeList &outSubShapes) const override; + virtual void RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes) override; + + // See Shape::GetStatsRecursive + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const override; + +protected: + RefConst mInnerShape; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/GetTrianglesContext.h b/WickedEngine/Jolt/Physics/Collision/Shape/GetTrianglesContext.h new file mode 100644 index 000000000..df68ae2f5 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/GetTrianglesContext.h @@ -0,0 +1,248 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsMaterial; + +/// Implementation of GetTrianglesStart/Next that uses a fixed list of vertices for the triangles. These are transformed into world space when getting the triangles. +class GetTrianglesContextVertexList +{ +public: + /// Constructor, to be called in GetTrianglesStart + GetTrianglesContextVertexList(Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, Mat44Arg inLocalTransform, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices, const PhysicsMaterial *inMaterial) : + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale) * inLocalTransform), + mTriangleVertices(inTriangleVertices), + mNumTriangleVertices(inNumTriangleVertices), + mMaterial(inMaterial), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + static_assert(sizeof(GetTrianglesContextVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextVertexList))); + JPH_ASSERT(inNumTriangleVertices % 3 == 0); + } + + /// @see Shape::GetTrianglesNext + int GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) + { + JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested); + + int total_num_vertices = min(inMaxTrianglesRequested * 3, int(mNumTriangleVertices - mCurrentVertex)); + + if (mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = mTriangleVertices + mCurrentVertex, *v_end = v + total_num_vertices; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + } + } + + // Update the current vertex to point to the next vertex to get + mCurrentVertex += total_num_vertices; + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = mMaterial; + + return total_num_triangles; + } + + /// Helper function that creates a vertex list of a half unit sphere (top part) + template + static void sCreateHalfUnitSphereTop(A &ioVertices, int inDetailLevel) + { + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel); + } + + /// Helper function that creates a vertex list of a half unit sphere (bottom part) + template + static void sCreateHalfUnitSphereBottom(A &ioVertices, int inDetailLevel) + { + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inDetailLevel); + sCreateUnitSphereHelper(ioVertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), inDetailLevel); + } + + /// Helper function that creates an open cylinder of half height 1 and radius 1 + template + static void sCreateUnitOpenCylinder(A &ioVertices, int inDetailLevel) + { + const Vec3 bottom_offset(0.0f, -2.0f, 0.0f); + int num_verts = 4 * (1 << inDetailLevel); + for (int i = 0; i < num_verts; ++i) + { + float angle1 = 2.0f * JPH_PI * (float(i) / num_verts); + float angle2 = 2.0f * JPH_PI * (float(i + 1) / num_verts); + + Vec3 t1(Sin(angle1), 1.0f, Cos(angle1)); + Vec3 t2(Sin(angle2), 1.0f, Cos(angle2)); + Vec3 b1 = t1 + bottom_offset; + Vec3 b2 = t2 + bottom_offset; + + ioVertices.push_back(t1); + ioVertices.push_back(b1); + ioVertices.push_back(t2); + + ioVertices.push_back(t2); + ioVertices.push_back(b1); + ioVertices.push_back(b2); + } + } + +private: + /// Recursive helper function for creating a sphere + template + static void sCreateUnitSphereHelper(A &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, int inLevel) + { + Vec3 center1 = (inV1 + inV2).Normalized(); + Vec3 center2 = (inV2 + inV3).Normalized(); + Vec3 center3 = (inV3 + inV1).Normalized(); + + if (inLevel > 0) + { + int new_level = inLevel - 1; + sCreateUnitSphereHelper(ioVertices, inV1, center1, center3, new_level); + sCreateUnitSphereHelper(ioVertices, center1, center2, center3, new_level); + sCreateUnitSphereHelper(ioVertices, center1, inV2, center2, new_level); + sCreateUnitSphereHelper(ioVertices, center3, center2, inV3, new_level); + } + else + { + ioVertices.push_back(inV1); + ioVertices.push_back(inV2); + ioVertices.push_back(inV3); + } + } + + Mat44 mLocalToWorld; + const Vec3 * mTriangleVertices; + size_t mNumTriangleVertices; + size_t mCurrentVertex = 0; + const PhysicsMaterial * mMaterial; + bool mIsInsideOut; +}; + +/// Implementation of GetTrianglesStart/Next that uses a multiple fixed lists of vertices for the triangles. These are transformed into world space when getting the triangles. +class GetTrianglesContextMultiVertexList +{ +public: + /// Constructor, to be called in GetTrianglesStart + GetTrianglesContextMultiVertexList(bool inIsInsideOut, const PhysicsMaterial *inMaterial) : + mMaterial(inMaterial), + mIsInsideOut(inIsInsideOut) + { + static_assert(sizeof(GetTrianglesContextMultiVertexList) <= sizeof(Shape::GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(this, alignof(GetTrianglesContextMultiVertexList))); + } + + /// Add a mesh part and its transform + void AddPart(Mat44Arg inLocalToWorld, const Vec3 *inTriangleVertices, size_t inNumTriangleVertices) + { + JPH_ASSERT(inNumTriangleVertices % 3 == 0); + + mParts.push_back({ inLocalToWorld, inTriangleVertices, inNumTriangleVertices }); + } + + /// @see Shape::GetTrianglesNext + int GetTrianglesNext(int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) + { + JPH_ASSERT(inMaxTrianglesRequested >= Shape::cGetTrianglesMinTrianglesRequested); + + int total_num_vertices = 0; + int max_vertices_requested = inMaxTrianglesRequested * 3; + + // Loop over parts + for (; mCurrentPart < mParts.size(); ++mCurrentPart) + { + const Part &part = mParts[mCurrentPart]; + + // Calculate how many vertices to take from this part + int part_num_vertices = min(max_vertices_requested, int(part.mNumTriangleVertices - mCurrentVertex)); + if (part_num_vertices == 0) + break; + + max_vertices_requested -= part_num_vertices; + total_num_vertices += part_num_vertices; + + if (mIsInsideOut) + { + // Store triangles flipped + for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3) + { + (part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + } + } + else + { + // Store triangles + for (const Vec3 *v = part.mTriangleVertices + mCurrentVertex, *v_end = v + part_num_vertices; v < v_end; v += 3) + { + (part.mLocalToWorld * v[0]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[1]).StoreFloat3(outTriangleVertices++); + (part.mLocalToWorld * v[2]).StoreFloat3(outTriangleVertices++); + } + } + + // Update the current vertex to point to the next vertex to get + mCurrentVertex += part_num_vertices; + + // Check if we completed this part + if (mCurrentVertex < part.mNumTriangleVertices) + break; + + // Reset current vertex for the next part + mCurrentVertex = 0; + } + + int total_num_triangles = total_num_vertices / 3; + + // Store materials + if (outMaterials != nullptr) + for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m) + *m = mMaterial; + + return total_num_triangles; + } + +private: + struct Part + { + Mat44 mLocalToWorld; + const Vec3 * mTriangleVertices; + size_t mNumTriangleVertices; + }; + + StaticArray mParts; + uint mCurrentPart = 0; + size_t mCurrentVertex = 0; + const PhysicsMaterial * mMaterial; + bool mIsInsideOut; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp new file mode 100644 index 000000000..bef7cc911 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.cpp @@ -0,0 +1,2700 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define JPH_DEBUG_HEIGHT_FIELD + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool HeightFieldShape::sDrawTriangleOutlines = false; +#endif // JPH_DEBUG_RENDERER + +using namespace HeightFieldShapeConstants; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HeightFieldShapeSettings) +{ + JPH_ADD_BASE_CLASS(HeightFieldShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mHeightSamples) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mOffset) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mScale) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMinHeightValue) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaxHeightValue) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialsCapacity) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mSampleCount) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBlockSize) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mBitsPerSample) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterialIndices) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mMaterials) + JPH_ADD_ATTRIBUTE(HeightFieldShapeSettings, mActiveEdgeCosThresholdAngle) +} + +const uint HeightFieldShape::sGridOffsets[] = +{ + 0, // level: 0, max x/y: 0, offset: 0 + 1, // level: 1, max x/y: 1, offset: 1 + 5, // level: 2, max x/y: 3, offset: 1 + 4 + 21, // level: 3, max x/y: 7, offset: 1 + 4 + 16 + 85, // level: 4, max x/y: 15, offset: 1 + 4 + 16 + 64 + 341, // level: 5, max x/y: 31, offset: 1 + 4 + 16 + 64 + 256 + 1365, // level: 6, max x/y: 63, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 5461, // level: 7, max x/y: 127, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + 21845, // level: 8, max x/y: 255, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 87381, // level: 9, max x/y: 511, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 349525, // level: 10, max x/y: 1023, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 1398101, // level: 11, max x/y: 2047, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 5592405, // level: 12, max x/y: 4095, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 22369621, // level: 13, max x/y: 8191, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... + 89478485, // level: 14, max x/y: 16383, offset: 1 + 4 + 16 + 64 + 256 + 1024 + 4096 + ... +}; + +HeightFieldShapeSettings::HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices, const PhysicsMaterialList &inMaterialList) : + mOffset(inOffset), + mScale(inScale), + mSampleCount(inSampleCount) +{ + mHeightSamples.assign(inSamples, inSamples + Square(inSampleCount)); + + if (!inMaterialList.empty() && inMaterialIndices != nullptr) + { + mMaterialIndices.assign(inMaterialIndices, inMaterialIndices + Square(inSampleCount - 1)); + mMaterials = inMaterialList; + } + else + { + JPH_ASSERT(inMaterialList.empty()); + JPH_ASSERT(inMaterialIndices == nullptr); + } +} + +ShapeSettings::ShapeResult HeightFieldShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new HeightFieldShape(*this, mCachedResult); + return mCachedResult; +} + +void HeightFieldShapeSettings::DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const +{ + // Determine min and max value + outMinValue = mMinHeightValue; + outMaxValue = mMaxHeightValue; + for (float h : mHeightSamples) + if (h != cNoCollisionValue) + { + outMinValue = min(outMinValue, h); + outMaxValue = max(outMaxValue, h); + } + + // Prevent dividing by zero by setting a minimal height difference + float height_diff = max(outMaxValue - outMinValue, 1.0e-6f); + + // Calculate the scale factor to quantize to 16 bits + outQuantizationScale = float(cMaxHeightValue16) / height_diff; +} + +uint32 HeightFieldShapeSettings::CalculateBitsPerSampleForError(float inMaxError) const +{ + // Start with 1 bit per sample + uint32 bits_per_sample = 1; + + // Determine total range + float min_value, max_value, scale; + DetermineMinAndMaxSample(min_value, max_value, scale); + if (min_value < max_value) + { + // Loop over all blocks + for (uint y = 0; y < mSampleCount; y += mBlockSize) + for (uint x = 0; x < mSampleCount; x += mBlockSize) + { + // Determine min and max block value + take 1 sample border just like we do while building the hierarchical grids + float block_min_value = FLT_MAX, block_max_value = -FLT_MAX; + for (uint bx = x; bx < min(x + mBlockSize + 1, mSampleCount); ++bx) + for (uint by = y; by < min(y + mBlockSize + 1, mSampleCount); ++by) + { + float h = mHeightSamples[by * mSampleCount + bx]; + if (h != cNoCollisionValue) + { + block_min_value = min(block_min_value, h); + block_max_value = max(block_max_value, h); + } + } + + if (block_min_value < block_max_value) + { + // Quantize then dequantize block min/max value + block_min_value = min_value + floor((block_min_value - min_value) * scale) / scale; + block_max_value = min_value + ceil((block_max_value - min_value) * scale) / scale; + float block_height = block_max_value - block_min_value; + + // Loop over the block again + for (uint bx = x; bx < x + mBlockSize; ++bx) + for (uint by = y; by < y + mBlockSize; ++by) + { + // Get the height + float height = mHeightSamples[by * mSampleCount + bx]; + if (height != cNoCollisionValue) + { + for (;;) + { + // Determine bitmask for sample + uint32 sample_mask = (1 << bits_per_sample) - 1; + + // Quantize + float quantized_height = floor((height - block_min_value) * float(sample_mask) / block_height); + quantized_height = Clamp(quantized_height, 0.0f, float(sample_mask - 1)); + + // Dequantize and check error + float dequantized_height = block_min_value + (quantized_height + 0.5f) * block_height / float(sample_mask); + if (abs(dequantized_height - height) <= inMaxError) + break; + + // Not accurate enough, increase bits per sample + bits_per_sample++; + + // Don't go above 8 bits per sample + if (bits_per_sample == 8) + return bits_per_sample; + } + } + } + } + } + + } + + return bits_per_sample; +} + +void HeightFieldShape::CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator) +{ + // Allocate temporary buffer for normals + uint normals_size = 2 * inSizeX * inSizeY * sizeof(Vec3); + Vec3 *normals = (Vec3 *)inAllocator.Allocate(normals_size); + JPH_SCOPE_EXIT([&inAllocator, normals, normals_size]{ inAllocator.Free(normals, normals_size); }); + + // Calculate triangle normals and make normals zero for triangles that are missing + Vec3 *out_normal = normals; + for (uint y = 0; y < inSizeY; ++y) + for (uint x = 0; x < inSizeX; ++x) + { + // Get height on diagonal + const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x); + float x1y1_h = height_samples[0]; + float x2y2_h = height_samples[inHeightsStride + 1]; + if (x1y1_h != cNoCollisionValue && x2y2_h != cNoCollisionValue) + { + // Calculate normal for lower left triangle (e.g. T1A) + float x1y2_h = height_samples[inHeightsStride]; + if (x1y2_h != cNoCollisionValue) + { + Vec3 x2y2_minus_x1y2(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0); + Vec3 x1y1_minus_x1y2(0, inHeightsScale * (x1y1_h - x1y2_h), -mScale.GetZ()); + out_normal[0] = x2y2_minus_x1y2.Cross(x1y1_minus_x1y2).Normalized(); + } + else + out_normal[0] = Vec3::sZero(); + + // Calculate normal for upper right triangle (e.g. T1B) + float x2y1_h = height_samples[1]; + if (x2y1_h != cNoCollisionValue) + { + Vec3 x1y1_minus_x2y1(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y1_h), 0); + Vec3 x2y2_minus_x2y1(0, inHeightsScale * (x2y2_h - x2y1_h), mScale.GetZ()); + out_normal[1] = x1y1_minus_x2y1.Cross(x2y2_minus_x2y1).Normalized(); + } + else + out_normal[1] = Vec3::sZero(); + } + else + { + out_normal[0] = Vec3::sZero(); + out_normal[1] = Vec3::sZero(); + } + + out_normal += 2; + } + + // Calculate active edges + const Vec3 *in_normal = normals; + uint global_bit_pos = 3 * (inY * (mSampleCount - 1) + inX); + for (uint y = 0; y < inSizeY; ++y) + { + for (uint x = 0; x < inSizeX; ++x) + { + // Get vertex heights + const float *height_samples = inHeights + (inY - inHeightsStartY + y) * inHeightsStride + (inX - inHeightsStartX + x); + float x1y1_h = height_samples[0]; + float x1y2_h = height_samples[inHeightsStride]; + float x2y2_h = height_samples[inHeightsStride + 1]; + bool x1y1_valid = x1y1_h != cNoCollisionValue; + bool x1y2_valid = x1y2_h != cNoCollisionValue; + bool x2y2_valid = x2y2_h != cNoCollisionValue; + + // Calculate the edge flags (3 bits) + // See diagram in the next function for the edge numbering + uint16 edge_mask = 0b111; + uint16 edge_flags = 0; + + // Edge 0 + if (x == 0) + edge_mask &= 0b110; // We need normal x - 1 which we didn't calculate, don't update this edge + else if (x1y1_valid && x1y2_valid) + { + Vec3 edge0_direction(0, inHeightsScale * (x1y2_h - x1y1_h), mScale.GetZ()); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[-1], edge0_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b001; + } + + // Edge 1 + if (y == inSizeY - 1) + edge_mask &= 0b101; // We need normal y + 1 which we didn't calculate, don't update this edge + else if (x1y2_valid && x2y2_valid) + { + Vec3 edge1_direction(mScale.GetX(), inHeightsScale * (x2y2_h - x1y2_h), 0); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[2 * inSizeX + 1], edge1_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b010; + } + + // Edge 2 + if (x1y1_valid && x2y2_valid) + { + Vec3 edge2_direction(-mScale.GetX(), inHeightsScale * (x1y1_h - x2y2_h), -mScale.GetZ()); + if (ActiveEdges::IsEdgeActive(in_normal[0], in_normal[1], edge2_direction, inActiveEdgeCosThresholdAngle)) + edge_flags |= 0b100; + } + + // Store the edge flags in the array + uint byte_pos = global_bit_pos >> 3; + uint bit_pos = global_bit_pos & 0b111; + JPH_ASSERT(byte_pos < mActiveEdgesSize); + uint8 *edge_flags_ptr = &mActiveEdges[byte_pos]; + uint16 combined_edge_flags = uint16(edge_flags_ptr[0]) | uint16(uint16(edge_flags_ptr[1]) << 8); + combined_edge_flags &= ~(edge_mask << bit_pos); + combined_edge_flags |= edge_flags << bit_pos; + edge_flags_ptr[0] = uint8(combined_edge_flags); + edge_flags_ptr[1] = uint8(combined_edge_flags >> 8); + + in_normal += 2; + global_bit_pos += 3; + } + + global_bit_pos += 3 * (mSampleCount - 1 - inSizeX); + } +} + +void HeightFieldShape::CalculateActiveEdges(const HeightFieldShapeSettings &inSettings) +{ + /* + Store active edges. The triangles are organized like this: + x ---> + + y + + + | \ T1B | \ T2B + | e0 e2 | \ + | | T1A \ | T2A \ + V +--e1---+-------+ + | \ T3B | \ T4B + | \ | \ + | T3A \ | T4A \ + +-------+-------+ + We store active edges e0 .. e2 as bits 0 .. 2. + We store triangles horizontally then vertically (order T1A, T2A, T3A and T4A). + The top edge and right edge of the heightfield are always active so we do not need to store them, + therefore we only need to store (mSampleCount - 1)^2 * 3-bit + The triangles T1B, T2B, T3B and T4B do not need to be stored, their active edges can be constructed from adjacent triangles. + Add 1 byte padding so we can always read 1 uint16 to get the bits that cross an 8 bit boundary + */ + + // Make all edges active (if mSampleCount is bigger than inSettings.mSampleCount we need to fill up the padding, + // also edges at x = 0 and y = inSettings.mSampleCount - 1 are not updated) + memset(mActiveEdges, 0xff, mActiveEdgesSize); + + // Now clear the edges that are not active + TempAllocatorMalloc allocator; + CalculateActiveEdges(0, 0, inSettings.mSampleCount - 1, inSettings.mSampleCount - 1, inSettings.mHeightSamples.data(), 0, 0, inSettings.mSampleCount, inSettings.mScale.GetY(), inSettings.mActiveEdgeCosThresholdAngle, allocator); +} + +void HeightFieldShape::StoreMaterialIndices(const HeightFieldShapeSettings &inSettings) +{ + // We need to account for any rounding of the sample count to the nearest block size + uint in_count_min_1 = inSettings.mSampleCount - 1; + uint out_count_min_1 = mSampleCount - 1; + + mNumBitsPerMaterialIndex = 32 - CountLeadingZeros(max((uint32)mMaterials.size(), inSettings.mMaterialsCapacity) - 1); + mMaterialIndices.resize(((Square(out_count_min_1) * mNumBitsPerMaterialIndex + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16 + + if (mMaterials.size() > 1) + for (uint y = 0; y < out_count_min_1; ++y) + for (uint x = 0; x < out_count_min_1; ++x) + { + // Read material + uint16 material_index = x < in_count_min_1 && y < in_count_min_1? uint16(inSettings.mMaterialIndices[x + y * in_count_min_1]) : 0; + + // Calculate byte and bit position where the material index needs to go + uint sample_pos = x + y * out_count_min_1; + uint bit_pos = sample_pos * mNumBitsPerMaterialIndex; + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + + // Write the material index + material_index <<= bit_pos; + JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size()); + mMaterialIndices[byte_pos] |= uint8(material_index); + mMaterialIndices[byte_pos + 1] |= uint8(material_index >> 8); + } +} + +void HeightFieldShape::CacheValues() +{ + mSampleMask = uint8((uint32(1) << mBitsPerSample) - 1); +} + +void HeightFieldShape::AllocateBuffers() +{ + uint num_blocks = GetNumBlocks(); + uint max_stride = (num_blocks + 1) >> 1; + mRangeBlocksSize = sGridOffsets[sGetMaxLevel(num_blocks) - 1] + Square(max_stride); + mHeightSamplesSize = (mSampleCount * mSampleCount * mBitsPerSample + 7) / 8 + 1; + mActiveEdgesSize = (Square(mSampleCount - 1) * 3 + 7) / 8 + 1; // See explanation at HeightFieldShape::CalculateActiveEdges + + JPH_ASSERT(mRangeBlocks == nullptr && mHeightSamples == nullptr && mActiveEdges == nullptr); + void *data = AlignedAllocate(mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize, alignof(RangeBlock)); + mRangeBlocks = reinterpret_cast(data); + mHeightSamples = reinterpret_cast(mRangeBlocks + mRangeBlocksSize); + mActiveEdges = mHeightSamples + mHeightSamplesSize; +} + +HeightFieldShape::HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::HeightField, EShapeSubType::HeightField, inSettings, outResult), + mOffset(inSettings.mOffset), + mScale(inSettings.mScale), + mSampleCount(((inSettings.mSampleCount + inSettings.mBlockSize - 1) / inSettings.mBlockSize) * inSettings.mBlockSize), // Round sample count to nearest block size + mBlockSize(inSettings.mBlockSize), + mBitsPerSample(uint8(inSettings.mBitsPerSample)) +{ + CacheValues(); + + // Reserve a bigger materials list if requested + if (inSettings.mMaterialsCapacity > 0) + mMaterials.reserve(inSettings.mMaterialsCapacity); + mMaterials = inSettings.mMaterials; + + // Check block size + if (mBlockSize < 2 || mBlockSize > 8) + { + outResult.SetError("HeightFieldShape: Block size must be in the range [2, 8]!"); + return; + } + + // Check bits per sample + if (inSettings.mBitsPerSample < 1 || inSettings.mBitsPerSample > 8) + { + outResult.SetError("HeightFieldShape: Bits per sample must be in the range [1, 8]!"); + return; + } + + // We stop at mBlockSize x mBlockSize height sample blocks + uint num_blocks = GetNumBlocks(); + + // We want at least 1 grid layer + if (num_blocks < 2) + { + outResult.SetError("HeightFieldShape: Sample count too low!"); + return; + } + + // Check that we don't overflow our 32 bit 'properties' + if (num_blocks > (1 << cNumBitsXY)) + { + outResult.SetError("HeightFieldShape: Sample count too high!"); + return; + } + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("HeightFieldShape: Size exceeds the amount of available sub shape ID bits!"); + return; + } + + if (!mMaterials.empty()) + { + // Validate materials + if (mMaterials.size() > 256) + { + outResult.SetError("Supporting max 256 materials per height field"); + return; + } + for (uint8 s : inSettings.mMaterialIndices) + if (s >= mMaterials.size()) + { + outResult.SetError(StringFormat("Material %u is beyond material list (size: %u)", s, (uint)mMaterials.size())); + return; + } + } + else + { + // No materials assigned, validate that no materials have been specified + if (!inSettings.mMaterialIndices.empty()) + { + outResult.SetError("No materials present, mMaterialIndices should be empty"); + return; + } + } + + // Determine range + float min_value, max_value, scale; + inSettings.DetermineMinAndMaxSample(min_value, max_value, scale); + if (min_value > max_value) + { + // If there is no collision with this heightmap, leave everything empty + mMaterials.clear(); + outResult.Set(this); + return; + } + + // Allocate space for this shape + AllocateBuffers(); + + // Quantize to uint16 + Array quantized_samples; + quantized_samples.reserve(mSampleCount * mSampleCount); + for (uint y = 0; y < inSettings.mSampleCount; ++y) + { + for (uint x = 0; x < inSettings.mSampleCount; ++x) + { + float h = inSettings.mHeightSamples[x + y * inSettings.mSampleCount]; + if (h == cNoCollisionValue) + { + quantized_samples.push_back(cNoCollisionValue16); + } + else + { + // Floor the quantized height to get a lower bound for the quantized value + int quantized_height = (int)floor(scale * (h - min_value)); + + // Ensure that the height says below the max height value so we can safely add 1 to get the upper bound for the quantized value + quantized_height = Clamp(quantized_height, 0, int(cMaxHeightValue16 - 1)); + + quantized_samples.push_back(uint16(quantized_height)); + } + } + // Pad remaining columns with no collision + for (uint x = inSettings.mSampleCount; x < mSampleCount; ++x) + quantized_samples.push_back(cNoCollisionValue16); + } + // Pad remaining rows with no collision + for (uint y = inSettings.mSampleCount; y < mSampleCount; ++y) + for (uint x = 0; x < mSampleCount; ++x) + quantized_samples.push_back(cNoCollisionValue16); + + // Update offset and scale to account for the compression to uint16 + if (min_value <= max_value) // Only when there was collision + { + // In GetPosition we always add 0.5 to the quantized sample in order to reduce the average error. + // We want to be able to exactly quantize min_value (this is important in case the heightfield is entirely flat) so we subtract that value from min_value. + min_value -= 0.5f / (scale * mSampleMask); + + mOffset.SetY(mOffset.GetY() + mScale.GetY() * min_value); + } + mScale.SetY(mScale.GetY() / scale); + + // Calculate amount of grids + uint max_level = sGetMaxLevel(num_blocks); + + // Temporary data structure used during creating of a hierarchy of grids + struct Range + { + uint16 mMin; + uint16 mMax; + }; + + // Reserve size for temporary range data + reserve 1 extra for a 1x1 grid that we won't store but use for calculating the bounding box + Array> ranges; + ranges.resize(max_level + 1); + + // Calculate highest detail grid by combining mBlockSize x mBlockSize height samples + Array *cur_range_vector = &ranges.back(); + uint num_blocks_pow2 = GetNextPowerOf2(num_blocks); // We calculate the range blocks as if the heightfield was a power of 2, when we save the range blocks we'll ignore the extra samples (this makes downsampling easier) + cur_range_vector->resize(num_blocks_pow2 * num_blocks_pow2); + Range *range_dst = &cur_range_vector->front(); + for (uint y = 0; y < num_blocks_pow2; ++y) + for (uint x = 0; x < num_blocks_pow2; ++x) + { + range_dst->mMin = 0xffff; + range_dst->mMax = 0; + uint max_bx = x == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; // for interior blocks take 1 more because the triangles connect to the next block so we must include their height too + uint max_by = y == num_blocks_pow2 - 1? mBlockSize : mBlockSize + 1; + for (uint by = 0; by < max_by; ++by) + for (uint bx = 0; bx < max_bx; ++bx) + { + uint sx = x * mBlockSize + bx; + uint sy = y * mBlockSize + by; + if (sx < mSampleCount && sy < mSampleCount) + { + uint16 h = quantized_samples[sy * mSampleCount + sx]; + if (h != cNoCollisionValue16) + { + range_dst->mMin = min(range_dst->mMin, h); + range_dst->mMax = max(range_dst->mMax, uint16(h + 1)); // Add 1 to the max so we know the real value is between mMin and mMax + } + } + } + ++range_dst; + } + + // Calculate remaining grids + for (uint n = num_blocks_pow2 >> 1; n >= 1; n >>= 1) + { + // Get source buffer + const Range *range_src = &cur_range_vector->front(); + + // Previous array element + --cur_range_vector; + + // Make space for this grid + cur_range_vector->resize(n * n); + + // Get target buffer + range_dst = &cur_range_vector->front(); + + // Combine the results of 2x2 ranges + for (uint y = 0; y < n; ++y) + for (uint x = 0; x < n; ++x) + { + range_dst->mMin = 0xffff; + range_dst->mMax = 0; + for (uint by = 0; by < 2; ++by) + for (uint bx = 0; bx < 2; ++bx) + { + const Range &r = range_src[(y * 2 + by) * n * 2 + x * 2 + bx]; + range_dst->mMin = min(range_dst->mMin, r.mMin); + range_dst->mMax = max(range_dst->mMax, r.mMax); + } + ++range_dst; + } + } + JPH_ASSERT(cur_range_vector == &ranges.front()); + + // Store global range for bounding box calculation + mMinSample = ranges[0][0].mMin; + mMaxSample = ranges[0][0].mMax; + +#ifdef JPH_ENABLE_ASSERTS + // Validate that we did not lose range along the way + uint16 minv = 0xffff, maxv = 0; + for (uint16 v : quantized_samples) + if (v != cNoCollisionValue16) + { + minv = min(minv, v); + maxv = max(maxv, uint16(v + 1)); + } + JPH_ASSERT(mMinSample == minv && mMaxSample == maxv); +#endif + + // Now erase the first element, we need a 2x2 grid to start with + ranges.erase(ranges.begin()); + + // Create blocks + uint max_stride = (num_blocks + 1) >> 1; + RangeBlock *current_block = mRangeBlocks; + for (uint level = 0; level < ranges.size(); ++level) + { + JPH_ASSERT(uint(current_block - mRangeBlocks) == sGridOffsets[level]); + + uint in_n = 1 << level; + uint out_n = min(in_n, max_stride); // At the most detailed level we store a non-power of 2 number of blocks + + for (uint y = 0; y < out_n; ++y) + for (uint x = 0; x < out_n; ++x) + { + // Convert from 2x2 Range structure to 1 RangeBlock structure + RangeBlock &rb = *current_block++; + for (uint by = 0; by < 2; ++by) + for (uint bx = 0; bx < 2; ++bx) + { + uint src_pos = (y * 2 + by) * 2 * in_n + (x * 2 + bx); + uint dst_pos = by * 2 + bx; + rb.mMin[dst_pos] = ranges[level][src_pos].mMin; + rb.mMax[dst_pos] = ranges[level][src_pos].mMax; + } + } + } + JPH_ASSERT(uint32(current_block - mRangeBlocks) == mRangeBlocksSize); + + // Quantize height samples + memset(mHeightSamples, 0, mHeightSamplesSize); + int sample = 0; + for (uint y = 0; y < mSampleCount; ++y) + for (uint x = 0; x < mSampleCount; ++x) + { + uint32 output_value; + + float h = x < inSettings.mSampleCount && y < inSettings.mSampleCount? inSettings.mHeightSamples[x + y * inSettings.mSampleCount] : cNoCollisionValue; + if (h == cNoCollisionValue) + { + // No collision + output_value = mSampleMask; + } + else + { + // Get range of block so we know what range to compress to + uint bx = x / mBlockSize; + uint by = y / mBlockSize; + const Range &range = ranges.back()[by * num_blocks_pow2 + bx]; + JPH_ASSERT(range.mMin < range.mMax); + + // Quantize to mBitsPerSample bits, note that mSampleMask is reserved for indicating that there's no collision. + // We divide the range into mSampleMask segments and use the mid points of these segments as the quantized values. + // This results in a lower error than if we had quantized our data using the lowest point of all these segments. + float h_min = min_value + range.mMin / scale; + float h_delta = float(range.mMax - range.mMin) / scale; + float quantized_height = floor((h - h_min) * float(mSampleMask) / h_delta); + output_value = uint32(Clamp((int)quantized_height, 0, int(mSampleMask) - 1)); // mSampleMask is reserved as 'no collision value' + } + + // Store the sample + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + output_value <<= bit_pos; + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + mHeightSamples[byte_pos] |= uint8(output_value); + mHeightSamples[byte_pos + 1] |= uint8(output_value >> 8); + sample += inSettings.mBitsPerSample; + } + + // Calculate the active edges + CalculateActiveEdges(inSettings); + + // Compress material indices + if (mMaterials.size() > 1 || inSettings.mMaterialsCapacity > 1) + StoreMaterialIndices(inSettings); + + outResult.Set(this); +} + +HeightFieldShape::~HeightFieldShape() +{ + if (mRangeBlocks != nullptr) + AlignedFree(mRangeBlocks); +} + +Ref HeightFieldShape::Clone() const +{ + Ref clone = new HeightFieldShape; + clone->SetUserData(GetUserData()); + + clone->mOffset = mOffset; + clone->mScale = mScale; + clone->mSampleCount = mSampleCount; + clone->mBlockSize = mBlockSize; + clone->mBitsPerSample = mBitsPerSample; + clone->mSampleMask = mSampleMask; + clone->mMinSample = mMinSample; + clone->mMaxSample = mMaxSample; + + clone->AllocateBuffers(); + memcpy(clone->mRangeBlocks, mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); // Copy the entire buffer in 1 go + + clone->mMaterials.reserve(mMaterials.capacity()); // Ensure we keep the capacity of the original + clone->mMaterials = mMaterials; + clone->mMaterialIndices = mMaterialIndices; + clone->mNumBitsPerMaterialIndex = mNumBitsPerMaterialIndex; + +#ifdef JPH_DEBUG_RENDERER + clone->mGeometry = mGeometry; + clone->mCachedUseMaterialColors = mCachedUseMaterialColors; +#endif // JPH_DEBUG_RENDERER + + return clone; +} + +inline void HeightFieldShape::sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride) +{ + outRangeBlockOffset = sGridOffsets[inMaxLevel - 1]; + outRangeBlockStride = (inNumBlocks + 1) >> 1; +} + +inline void HeightFieldShape::GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock) +{ + JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks()); + + // Convert to location of range block + uint rbx = inBlockX >> 1; + uint rby = inBlockY >> 1; + outIndexInBlock = ((inBlockY & 1) << 1) + (inBlockX & 1); + + uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx; + JPH_ASSERT(offset < mRangeBlocksSize); + outBlock = mRangeBlocks + offset; +} + +inline void HeightFieldShape::GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const +{ + JPH_ASSERT(inBlockX < GetNumBlocks() && inBlockY < GetNumBlocks()); + + // Convert to location of range block + uint rbx = inBlockX >> 1; + uint rby = inBlockY >> 1; + uint n = ((inBlockY & 1) << 1) + (inBlockX & 1); + + // Calculate offset and scale + uint offset = inRangeBlockOffset + rby * inRangeBlockStride + rbx; + JPH_ASSERT(offset < mRangeBlocksSize); + const RangeBlock &block = mRangeBlocks[offset]; + outBlockOffset = float(block.mMin[n]); + outBlockScale = float(block.mMax[n] - block.mMin[n]) / float(mSampleMask); +} + +inline uint8 HeightFieldShape::GetHeightSample(uint inX, uint inY) const +{ + JPH_ASSERT(inX < mSampleCount); + JPH_ASSERT(inY < mSampleCount); + + // Determine bit position of sample + uint sample = (inY * mSampleCount + inX) * uint(mBitsPerSample); + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + + // Fetch the height sample value + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + const uint8 *height_samples = mHeightSamples + byte_pos; + uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8); + return uint8(height_sample >> bit_pos) & mSampleMask; +} + +inline Vec3 HeightFieldShape::GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const +{ + // Get quantized value + uint8 height_sample = GetHeightSample(inX, inY); + outNoCollision = height_sample == mSampleMask; + + // Add 0.5 to the quantized value to minimize the error (see constructor) + return mOffset + mScale * Vec3(float(inX), inBlockOffset + (0.5f + height_sample) * inBlockScale, float(inY)); +} + +Vec3 HeightFieldShape::GetPosition(uint inX, uint inY) const +{ + // Test if there are any samples + if (mHeightSamplesSize == 0) + return mOffset + mScale * Vec3(float(inX), 0.0f, float(inY)); + + // Get block location + uint bx = inX / mBlockSize; + uint by = inY / mBlockSize; + + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride); + + float offset, scale; + GetBlockOffsetAndScale(bx, by, range_block_offset, range_block_stride, offset, scale); + + bool no_collision; + return GetPosition(inX, inY, offset, scale, no_collision); +} + +bool HeightFieldShape::IsNoCollision(uint inX, uint inY) const +{ + return mHeightSamplesSize == 0 || GetHeightSample(inX, inY) == mSampleMask; +} + +bool HeightFieldShape::ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const +{ + // Check if we have collision + if (mHeightSamplesSize == 0) + return false; + + // Convert coordinate to integer space + Vec3 integer_space = (inLocalPosition - mOffset) / mScale; + + // Get x coordinate and fraction + float x_frac = integer_space.GetX(); + if (x_frac < 0.0f || x_frac >= mSampleCount - 1) + return false; + uint x = (uint)floor(x_frac); + x_frac -= x; + + // Get y coordinate and fraction + float y_frac = integer_space.GetZ(); + if (y_frac < 0.0f || y_frac >= mSampleCount - 1) + return false; + uint y = (uint)floor(y_frac); + y_frac -= y; + + // If one of the diagonal points doesn't have collision, we don't have a height at this location + if (IsNoCollision(x, y) || IsNoCollision(x + 1, y + 1)) + return false; + + if (y_frac >= x_frac) + { + // Left bottom triangle, test the 3rd point + if (IsNoCollision(x, y + 1)) + return false; + + // Interpolate height value + Vec3 v1 = GetPosition(x, y); + Vec3 v2 = GetPosition(x, y + 1); + Vec3 v3 = GetPosition(x + 1, y + 1); + outSurfacePosition = v1 + y_frac * (v2 - v1) + x_frac * (v3 - v2); + SubShapeIDCreator creator; + outSubShapeID = EncodeSubShapeID(creator, x, y, 0); + return true; + } + else + { + // Right top triangle, test the third point + if (IsNoCollision(x + 1, y)) + return false; + + // Interpolate height value + Vec3 v1 = GetPosition(x, y); + Vec3 v2 = GetPosition(x + 1, y + 1); + Vec3 v3 = GetPosition(x + 1, y); + outSurfacePosition = v1 + y_frac * (v2 - v3) + x_frac * (v3 - v1); + SubShapeIDCreator creator; + outSubShapeID = EncodeSubShapeID(creator, x, y, 1); + return true; + } +} + +void HeightFieldShape::GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0); + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount); + + // Test if there are any samples + if (mHeightSamplesSize == 0) + { + // No samples, return the offset + float offset = mOffset.GetY(); + for (uint y = 0; y < inSizeY; ++y, outHeights += inHeightsStride) + for (uint x = 0; x < inSizeX; ++x) + outHeights[x] = offset; + } + else + { + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, sGetMaxLevel(num_blocks), range_block_offset, range_block_stride); + + // Loop over blocks + uint block_start_x = inX / mBlockSize; + uint block_start_y = inY / mBlockSize; + uint num_blocks_x = inSizeX / mBlockSize; + uint num_blocks_y = inSizeY / mBlockSize; + for (uint block_y = 0; block_y < num_blocks_y; ++block_y) + for (uint block_x = 0; block_x < num_blocks_x; ++block_x) + { + // Get offset and scale for block + float offset, scale; + GetBlockOffsetAndScale(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, offset, scale); + + // Adjust by global offset and scale + // Note: This is the math applied in GetPosition() written out to reduce calculations in the inner loop + scale *= mScale.GetY(); + offset = mOffset.GetY() + mScale.GetY() * offset + 0.5f * scale; + + // Loop over samples in block + for (uint sample_y = 0; sample_y < mBlockSize; ++sample_y) + for (uint sample_x = 0; sample_x < mBlockSize; ++sample_x) + { + // Calculate output coordinate + uint output_x = block_x * mBlockSize + sample_x; + uint output_y = block_y * mBlockSize + sample_y; + + // Get quantized value + uint8 height_sample = GetHeightSample(inX + output_x, inY + output_y); + + // Dequantize + float h = height_sample != mSampleMask? offset + height_sample * scale : cNoCollisionValue; + outHeights[output_y * inHeightsStride + output_x] = h; + } + } + } +} + +void HeightFieldShape::SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle) +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + JPH_ASSERT(mHeightSamplesSize > 0); + JPH_ASSERT(inX % mBlockSize == 0 && inY % mBlockSize == 0); + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX <= mSampleCount && inY + inSizeY <= mSampleCount); + + // If we have a block in negative x/y direction, we will affect its range so we need to take it into account + bool need_temp_heights = false; + uint affected_x = inX; + uint affected_y = inY; + uint affected_size_x = inSizeX; + uint affected_size_y = inSizeY; + if (inX > 0) { affected_x -= mBlockSize; affected_size_x += mBlockSize; need_temp_heights = true; } + if (inY > 0) { affected_y -= mBlockSize; affected_size_y += mBlockSize; need_temp_heights = true; } + + // If we have a block in positive x/y direction, our ranges are affected by it so we need to take it into account + uint heights_size_x = affected_size_x; + uint heights_size_y = affected_size_y; + if (inX + inSizeX < mSampleCount) { heights_size_x += mBlockSize; need_temp_heights = true; } + if (inY + inSizeY < mSampleCount) { heights_size_y += mBlockSize; need_temp_heights = true; } + + // Get heights for affected area + const float *heights; + intptr_t heights_stride; + float *temp_heights; + if (need_temp_heights) + { + // Fetch the surrounding height data (note we're forced to recompress this data with a potentially different range so there will be some precision loss here) + temp_heights = (float *)inAllocator.Allocate(heights_size_x * heights_size_y * sizeof(float)); + heights = temp_heights; + heights_stride = heights_size_x; + + // We need to fill in the following areas: + // + // +-----------------+ + // | 2 | + // |---+---------+---| + // | | | | + // | 3 | 1 | 4 | + // | | | | + // |---+---------+---| + // | 5 | + // +-----------------+ + // + // 1. The area that is affected by the new heights (we just copy these) + // 2-5. These areas are either needed to calculate the range of the affected blocks or they need to be recompressed with a different range + uint offset_x = inX - affected_x; + uint offset_y = inY - affected_y; + + // Area 2 + GetHeights(affected_x, affected_y, heights_size_x, offset_y, temp_heights, heights_size_x); + float *area3_start = temp_heights + offset_y * heights_size_x; + + // Area 3 + GetHeights(affected_x, inY, offset_x, inSizeY, area3_start, heights_size_x); + + // Area 1 + float *area1_start = area3_start + offset_x; + for (uint y = 0; y < inSizeY; ++y, area1_start += heights_size_x, inHeights += inHeightsStride) + memcpy(area1_start, inHeights, inSizeX * sizeof(float)); + + // Area 4 + uint area4_x = inX + inSizeX; + GetHeights(area4_x, inY, affected_x + heights_size_x - area4_x, inSizeY, area3_start + area4_x - affected_x, heights_size_x); + + // Area 5 + uint area5_y = inY + inSizeY; + float *area5_start = temp_heights + (area5_y - affected_y) * heights_size_x; + GetHeights(affected_x, area5_y, heights_size_x, affected_y + heights_size_y - area5_y, area5_start, heights_size_x); + } + else + { + // We can directly use the input buffer because there are no extra edges to take into account + heights = inHeights; + heights_stride = inHeightsStride; + temp_heights = nullptr; + } + + // Calculate offset and stride + uint num_blocks = GetNumBlocks(); + uint range_block_offset, range_block_stride; + uint max_level = sGetMaxLevel(num_blocks); + sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride); + + // Loop over blocks + uint block_start_x = affected_x / mBlockSize; + uint block_start_y = affected_y / mBlockSize; + uint num_blocks_x = affected_size_x / mBlockSize; + uint num_blocks_y = affected_size_y / mBlockSize; + for (uint block_y = 0, sample_start_y = 0; block_y < num_blocks_y; ++block_y, sample_start_y += mBlockSize) + for (uint block_x = 0, sample_start_x = 0; block_x < num_blocks_x; ++block_x, sample_start_x += mBlockSize) + { + // Determine quantized min and max value for block + // Note that we need to include 1 extra row in the positive x/y direction to account for connecting triangles + int min_value = 0xffff; + int max_value = 0; + uint sample_x_end = min(sample_start_x + mBlockSize + 1, mSampleCount - affected_x); + uint sample_y_end = min(sample_start_y + mBlockSize + 1, mSampleCount - affected_y); + for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y) + for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x) + { + float h = heights[sample_y * heights_stride + sample_x]; + if (h != cNoCollisionValue) + { + int quantized_height = Clamp((int)floor((h - mOffset.GetY()) / mScale.GetY()), 0, int(cMaxHeightValue16 - 1)); + min_value = min(min_value, quantized_height); + max_value = max(max_value, quantized_height + 1); + } + } + if (min_value > max_value) + min_value = max_value = cNoCollisionValue16; + + // Update range for block + RangeBlock *range_block; + uint index_in_block; + GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, range_block, index_in_block); + range_block->mMin[index_in_block] = uint16(min_value); + range_block->mMax[index_in_block] = uint16(max_value); + + // Get offset and scale for block + float offset_block = float(min_value); + float scale_block = float(max_value - min_value) / float(mSampleMask); + + // Calculate scale and offset using the formula used in GetPosition() solved for the quantized height (excluding 0.5 because we round down while quantizing) + float scale = scale_block * mScale.GetY(); + float offset = mOffset.GetY() + offset_block * mScale.GetY(); + + // Loop over samples in block + sample_x_end = sample_start_x + mBlockSize; + sample_y_end = sample_start_y + mBlockSize; + for (uint sample_y = sample_start_y; sample_y < sample_y_end; ++sample_y) + for (uint sample_x = sample_start_x; sample_x < sample_x_end; ++sample_x) + { + // Quantize height + float h = heights[sample_y * heights_stride + sample_x]; + uint8 quantized_height = h != cNoCollisionValue? uint8(Clamp((int)floor((h - offset) / scale), 0, int(mSampleMask) - 1)) : mSampleMask; + + // Determine bit position of sample + uint sample = ((affected_y + sample_y) * mSampleCount + affected_x + sample_x) * uint(mBitsPerSample); + uint byte_pos = sample >> 3; + uint bit_pos = sample & 0b111; + + // Update the height value sample + JPH_ASSERT(byte_pos + 1 < mHeightSamplesSize); + uint8 *height_samples = mHeightSamples + byte_pos; + uint16 height_sample = uint16(height_samples[0]) | uint16(uint16(height_samples[1]) << 8); + height_sample &= ~(uint16(mSampleMask) << bit_pos); + height_sample |= uint16(quantized_height) << bit_pos; + height_samples[0] = uint8(height_sample); + height_samples[1] = uint8(height_sample >> 8); + } + } + + // Update active edges + // Note that we must take an extra row on all sides to account for connecting triangles + uint ae_x = inX > 1? inX - 2 : 0; + uint ae_y = inY > 1? inY - 2 : 0; + uint ae_sx = min(inX + inSizeX + 1, mSampleCount - 1) - ae_x; + uint ae_sy = min(inY + inSizeY + 1, mSampleCount - 1) - ae_y; + CalculateActiveEdges(ae_x, ae_y, ae_sx, ae_sy, heights, affected_x, affected_y, heights_stride, 1.0f, inActiveEdgeCosThresholdAngle, inAllocator); + + // Free temporary buffer + if (temp_heights != nullptr) + inAllocator.Free(temp_heights, heights_size_x * heights_size_y * sizeof(float)); + + // Update hierarchy of range blocks + while (max_level > 1) + { + // Get offset and stride for destination blocks + uint dst_range_block_offset, dst_range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks >> 1, max_level - 1, dst_range_block_offset, dst_range_block_stride); + + // If we're starting halfway through a 2x2 block, we need to process one extra block since we take steps of 2 blocks below + uint block_x_end = (block_start_x & 1) && block_start_x + num_blocks_x < num_blocks? num_blocks_x + 1 : num_blocks_x; + uint block_y_end = (block_start_y & 1) && block_start_y + num_blocks_y < num_blocks? num_blocks_y + 1 : num_blocks_y; + + // Loop over all affected blocks + for (uint block_y = 0; block_y < block_y_end; block_y += 2) + for (uint block_x = 0; block_x < block_x_end; block_x += 2) + { + // Get source range block + RangeBlock *src_range_block; + uint index_in_src_block; + GetRangeBlock(block_start_x + block_x, block_start_y + block_y, range_block_offset, range_block_stride, src_range_block, index_in_src_block); + + // Determine quantized min and max value for the entire 2x2 block + uint16 min_value = 0xffff; + uint16 max_value = 0; + for (uint i = 0; i < 4; ++i) + if (src_range_block->mMin[i] != cNoCollisionValue16) + { + min_value = min(min_value, src_range_block->mMin[i]); + max_value = max(max_value, src_range_block->mMax[i]); + } + + // Write to destination block + RangeBlock *dst_range_block; + uint index_in_dst_block; + GetRangeBlock((block_start_x + block_x) >> 1, (block_start_y + block_y) >> 1, dst_range_block_offset, dst_range_block_stride, dst_range_block, index_in_dst_block); + dst_range_block->mMin[index_in_dst_block] = uint16(min_value); + dst_range_block->mMax[index_in_dst_block] = uint16(max_value); + } + + // Go up one level + --max_level; + num_blocks >>= 1; + block_start_x >>= 1; + block_start_y >>= 1; + num_blocks_x = min((num_blocks_x + 1) >> 1, num_blocks); + num_blocks_y = min((num_blocks_y + 1) >> 1, num_blocks); + + // Update stride and offset for source to old destination + range_block_offset = dst_range_block_offset; + range_block_stride = dst_range_block_stride; + } + + // Calculate new min and max sample for the entire height field + mMinSample = 0xffff; + mMaxSample = 0; + for (uint i = 0; i < 4; ++i) + if (mRangeBlocks[0].mMin[i] != cNoCollisionValue16) + { + mMinSample = min(mMinSample, mRangeBlocks[0].mMin[i]); + mMaxSample = max(mMaxSample, mRangeBlocks[0].mMax[i]); + } + +#ifdef JPH_DEBUG_RENDERER + // Invalidate temporary rendering data + mGeometry.clear(); +#endif +} + +void HeightFieldShape::GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const +{ + if (inSizeX == 0 || inSizeY == 0) + return; + + if (mMaterialIndices.empty()) + { + // Return all 0's + for (uint y = 0; y < inSizeY; ++y) + { + uint8 *out_indices = outMaterials + y * inMaterialsStride; + for (uint x = 0; x < inSizeX; ++x) + *out_indices++ = 0; + } + return; + } + + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount); + + uint count_min_1 = mSampleCount - 1; + uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + + for (uint y = 0; y < inSizeY; ++y) + { + // Calculate input position + uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex; + const uint8 *in_indices = mMaterialIndices.data() + (bit_pos >> 3); + bit_pos &= 0b111; + + // Calculate output position + uint8 *out_indices = outMaterials + y * inMaterialsStride; + + for (uint x = 0; x < inSizeX; ++x) + { + // Get material index + uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8); + material_index >>= bit_pos; + material_index &= material_index_mask; + *out_indices = uint8(material_index); + + // Go to the next index + bit_pos += mNumBitsPerMaterialIndex; + in_indices += bit_pos >> 3; + bit_pos &= 0b111; + ++out_indices; + } + } +} + +bool HeightFieldShape::SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator) +{ + if (inSizeX == 0 || inSizeY == 0) + return true; + + JPH_ASSERT(inX < mSampleCount && inY < mSampleCount); + JPH_ASSERT(inX + inSizeX < mSampleCount && inY + inSizeY < mSampleCount); + + // Remap materials + uint material_remap_table_size = uint(inMaterialList != nullptr? inMaterialList->size() : mMaterials.size()); + uint8 *material_remap_table = (uint8 *)inAllocator.Allocate(material_remap_table_size); + JPH_SCOPE_EXIT([&inAllocator, material_remap_table, material_remap_table_size]{ inAllocator.Free(material_remap_table, material_remap_table_size); }); + if (inMaterialList != nullptr) + { + // Conservatively reserve more space if the incoming material list is bigger + if (inMaterialList->size() > mMaterials.size()) + mMaterials.reserve(inMaterialList->size()); + + // Create a remap table + uint8 *remap_entry = material_remap_table; + for (const PhysicsMaterial *material : *inMaterialList) + { + // Try to find it in the existing list + PhysicsMaterialList::const_iterator it = std::find(mMaterials.begin(), mMaterials.end(), material); + if (it != mMaterials.end()) + { + // Found it, calculate index + *remap_entry = uint8(it - mMaterials.begin()); + } + else + { + // Not found, add it + if (mMaterials.size() >= 256) + { + // We can't have more than 256 materials since we use uint8 as indices + return false; + } + *remap_entry = uint8(mMaterials.size()); + mMaterials.push_back(material); + } + ++remap_entry; + } + } + else + { + // No remapping + for (uint i = 0; i < material_remap_table_size; ++i) + material_remap_table[i] = uint8(i); + } + + if (mMaterials.size() == 1) + { + // Only 1 material, we don't need to store the material indices + return true; + } + + // Check if we need to resize the material indices array + uint count_min_1 = mSampleCount - 1; + uint32 new_bits_per_material_index = 32 - CountLeadingZeros((uint32)mMaterials.size() - 1); + JPH_ASSERT(mNumBitsPerMaterialIndex <= 8 && new_bits_per_material_index <= 8); + if (new_bits_per_material_index > mNumBitsPerMaterialIndex) + { + // Resize the material indices array + mMaterialIndices.resize(((Square(count_min_1) * new_bits_per_material_index + 7) >> 3) + 1, 0); // Add 1 byte so we don't read out of bounds when reading an uint16 + + // Calculate old and new mask + uint16 old_material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + uint16 new_material_index_mask = uint16((1 << new_bits_per_material_index) - 1); + + // Loop through the array backwards to avoid overwriting data + int in_bit_pos = (count_min_1 * count_min_1 - 1) * mNumBitsPerMaterialIndex; + const uint8 *in_indices = mMaterialIndices.data() + (in_bit_pos >> 3); + in_bit_pos &= 0b111; + int out_bit_pos = (count_min_1 * count_min_1 - 1) * new_bits_per_material_index; + uint8 *out_indices = mMaterialIndices.data() + (out_bit_pos >> 3); + out_bit_pos &= 0b111; + + while (out_indices >= mMaterialIndices.data()) + { + // Read the material index + uint16 material_index = uint16(in_indices[0]) + uint16(uint16(in_indices[1]) << 8); + material_index >>= in_bit_pos; + material_index &= old_material_index_mask; + + // Write the material index + uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8); + output_data &= ~(new_material_index_mask << out_bit_pos); + output_data |= material_index << out_bit_pos; + out_indices[0] = uint8(output_data); + out_indices[1] = uint8(output_data >> 8); + + // Go to the previous index + in_bit_pos -= int(mNumBitsPerMaterialIndex); + in_indices += in_bit_pos >> 3; + in_bit_pos &= 0b111; + out_bit_pos -= int(new_bits_per_material_index); + out_indices += out_bit_pos >> 3; + out_bit_pos &= 0b111; + } + + // Accept the new bits per material index + mNumBitsPerMaterialIndex = new_bits_per_material_index; + } + + uint16 material_index_mask = uint16((1 << mNumBitsPerMaterialIndex) - 1); + for (uint y = 0; y < inSizeY; ++y) + { + // Calculate input position + const uint8 *in_indices = inMaterials + y * inMaterialsStride; + + // Calculate output position + uint bit_pos = (inX + (inY + y) * count_min_1) * mNumBitsPerMaterialIndex; + uint8 *out_indices = mMaterialIndices.data() + (bit_pos >> 3); + bit_pos &= 0b111; + + for (uint x = 0; x < inSizeX; ++x) + { + // Update material + uint16 output_data = uint16(out_indices[0]) + uint16(uint16(out_indices[1]) << 8); + output_data &= ~(material_index_mask << bit_pos); + output_data |= material_remap_table[*in_indices] << bit_pos; + out_indices[0] = uint8(output_data); + out_indices[1] = uint8(output_data >> 8); + + // Go to the next index + in_indices++; + bit_pos += mNumBitsPerMaterialIndex; + out_indices += bit_pos >> 3; + bit_pos &= 0b111; + } + } + + return true; +} + +MassProperties HeightFieldShape::GetMassProperties() const +{ + // Object should always be static, return default mass properties + return MassProperties(); +} + +const PhysicsMaterial *HeightFieldShape::GetMaterial(uint inX, uint inY) const +{ + if (mMaterials.empty()) + return PhysicsMaterial::sDefault; + if (mMaterials.size() == 1) + return mMaterials[0]; + + uint count_min_1 = mSampleCount - 1; + JPH_ASSERT(inX < count_min_1); + JPH_ASSERT(inY < count_min_1); + + // Calculate at which bit the material index starts + uint bit_pos = (inX + inY * count_min_1) * mNumBitsPerMaterialIndex; + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + + // Read the material index + JPH_ASSERT(byte_pos + 1 < mMaterialIndices.size()); + const uint8 *material_indices = mMaterialIndices.data() + byte_pos; + uint16 material_index = uint16(material_indices[0]) + uint16(uint16(material_indices[1]) << 8); + material_index >>= bit_pos; + material_index &= (1 << mNumBitsPerMaterialIndex) - 1; + + // Return the material + return mMaterials[material_index]; +} + +uint HeightFieldShape::GetSubShapeIDBits() const +{ + // Need to store X, Y and 1 extra bit to specify the triangle number in the quad + return 2 * (32 - CountLeadingZeros(mSampleCount - 1)) + 1; +} + +SubShapeID HeightFieldShape::EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const +{ + return inCreator.PushID((inX + inY * mSampleCount) * 2 + inTriangle, GetSubShapeIDBits()).GetID(); +} + +void HeightFieldShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const +{ + // Decode sub shape id + SubShapeID remainder; + uint32 id = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID"); + + // Get triangle index + outTriangle = id & 1; + id >>= 1; + + // Fetch the x and y coordinate + outX = id % mSampleCount; + outY = id / mSampleCount; +} + +const PhysicsMaterial *HeightFieldShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch the material + return GetMaterial(x, y); +} + +Vec3 HeightFieldShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch vertices that both triangles share + Vec3 x1y1 = GetPosition(x, y); + Vec3 x2y2 = GetPosition(x + 1, y + 1); + + // Get normal depending on which triangle was selected + Vec3 normal; + if (triangle == 0) + { + Vec3 x1y2 = GetPosition(x, y + 1); + normal = (x2y2 - x1y2).Cross(x1y1 - x1y2); + } + else + { + Vec3 x2y1 = GetPosition(x + 1, y); + normal = (x1y1 - x2y1).Cross(x2y2 - x2y1); + } + + return normal.Normalized(); +} + +void HeightFieldShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode ID + uint x, y, triangle; + DecodeSubShapeID(inSubShapeID, x, y, triangle); + + // Fetch the triangle + outVertices.resize(3); + outVertices[0] = GetPosition(x, y); + Vec3 v2 = GetPosition(x + 1, y + 1); + if (triangle == 0) + { + outVertices[1] = GetPosition(x, y + 1); + outVertices[2] = v2; + } + else + { + outVertices[1] = v2; + outVertices[2] = GetPosition(x + 1, y); + } + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + swap(outVertices[1], outVertices[2]); + + // Transform to world space + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + for (Vec3 &v : outVertices) + v = transform * v; +} + +inline uint8 HeightFieldShape::GetEdgeFlags(uint inX, uint inY, uint inTriangle) const +{ + JPH_ASSERT(inX < mSampleCount - 1 && inY < mSampleCount - 1); + + if (inTriangle == 0) + { + // The edge flags for this triangle are directly stored, find the right 3 bits + uint bit_pos = 3 * (inX + inY * (mSampleCount - 1)); + uint byte_pos = bit_pos >> 3; + bit_pos &= 0b111; + JPH_ASSERT(byte_pos + 1 < mActiveEdgesSize); + const uint8 *active_edges = mActiveEdges + byte_pos; + uint16 edge_flags = uint16(active_edges[0]) + uint16(uint16(active_edges[1]) << 8); + return uint8(edge_flags >> bit_pos) & 0b111; + } + else + { + // We don't store this triangle directly, we need to look at our three neighbours to construct the edge flags + uint8 edge0 = (GetEdgeFlags(inX, inY, 0) & 0b100) != 0? 0b001 : 0; // Diagonal edge + uint8 edge1 = inX == mSampleCount - 2 || (GetEdgeFlags(inX + 1, inY, 0) & 0b001) != 0? 0b010 : 0; // Vertical edge + uint8 edge2 = inY == 0 || (GetEdgeFlags(inX, inY - 1, 0) & 0b010) != 0? 0b100 : 0; // Horizontal edge + return edge0 | edge1 | edge2; + } +} + +AABox HeightFieldShape::GetLocalBounds() const +{ + if (mMinSample == cNoCollisionValue16) + { + // This whole height field shape doesn't have any collision, return the center point + Vec3 center = mOffset + 0.5f * mScale * Vec3(float(mSampleCount - 1), 0.0f, float(mSampleCount - 1)); + return AABox(center, center); + } + else + { + // Bounding box based on min and max sample height + Vec3 bmin = mOffset + mScale * Vec3(0.0f, float(mMinSample), 0.0f); + Vec3 bmax = mOffset + mScale * Vec3(float(mSampleCount - 1), float(mMaxSample), float(mSampleCount - 1)); + return AABox(bmin, bmax); + } +} + +#ifdef JPH_DEBUG_RENDERER +void HeightFieldShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Don't draw anything if we don't have any collision + if (mHeightSamplesSize == 0) + return; + + // Reset the batch if we switch coloring mode + if (mCachedUseMaterialColors != inUseMaterialColors) + { + mGeometry.clear(); + mCachedUseMaterialColors = inUseMaterialColors; + } + + if (mGeometry.empty()) + { + // Divide terrain in triangle batches of max 64x64x2 triangles to allow better culling of the terrain + uint32 block_size = min(mSampleCount, 64); + for (uint32 by = 0; by < mSampleCount; by += block_size) + for (uint32 bx = 0; bx < mSampleCount; bx += block_size) + { + // Create vertices for a block + Array triangles; + triangles.resize(block_size * block_size * 2); + DebugRenderer::Triangle *out_tri = &triangles[0]; + for (uint32 y = by, max_y = min(by + block_size, mSampleCount - 1); y < max_y; ++y) + for (uint32 x = bx, max_x = min(bx + block_size, mSampleCount - 1); x < max_x; ++x) + if (!IsNoCollision(x, y) && !IsNoCollision(x + 1, y + 1)) + { + Vec3 x1y1 = GetPosition(x, y); + Vec3 x2y2 = GetPosition(x + 1, y + 1); + Color color = inUseMaterialColors? GetMaterial(x, y)->GetDebugColor() : Color::sWhite; + + if (!IsNoCollision(x, y + 1)) + { + Vec3 x1y2 = GetPosition(x, y + 1); + + x1y1.StoreFloat3(&out_tri->mV[0].mPosition); + x1y2.StoreFloat3(&out_tri->mV[1].mPosition); + x2y2.StoreFloat3(&out_tri->mV[2].mPosition); + + Vec3 normal = (x2y2 - x1y2).Cross(x1y1 - x1y2).Normalized(); + for (DebugRenderer::Vertex &v : out_tri->mV) + { + v.mColor = color; + v.mUV = Float2(0, 0); + normal.StoreFloat3(&v.mNormal); + } + + ++out_tri; + } + + if (!IsNoCollision(x + 1, y)) + { + Vec3 x2y1 = GetPosition(x + 1, y); + + x1y1.StoreFloat3(&out_tri->mV[0].mPosition); + x2y2.StoreFloat3(&out_tri->mV[1].mPosition); + x2y1.StoreFloat3(&out_tri->mV[2].mPosition); + + Vec3 normal = (x1y1 - x2y1).Cross(x2y2 - x2y1).Normalized(); + for (DebugRenderer::Vertex &v : out_tri->mV) + { + v.mColor = color; + v.mUV = Float2(0, 0); + normal.StoreFloat3(&v.mNormal); + } + + ++out_tri; + } + } + + // Resize triangles array to actual amount of triangles written + size_t num_triangles = out_tri - &triangles[0]; + triangles.resize(num_triangles); + + // Create batch + if (num_triangles > 0) + mGeometry.push_back(new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), DebugRenderer::sCalculateBounds(&triangles[0].mV[0], int(3 * num_triangles)))); + } + } + + // Get transform including scale + RMat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + for (const DebugRenderer::GeometryRef &b : mGeometry) + inRenderer->DrawGeometry(transform, inColor, b, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + if (sDrawTriangleOutlines) + { + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, DebugRenderer *inRenderer, RMat44Arg inTransform) : + mShape(inShape), + mRenderer(inRenderer), + mTransform(inTransform) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + UVec4 valid = Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const + { + // Determine active edges + uint8 active_edges = mShape->GetEdgeFlags(inX, inY, inTriangle); + + // Loop through edges + Vec3 v[] = { inV0, inV1, inV2 }; + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + RVec3 v1 = mTransform * v[edge_idx]; + RVec3 v2 = mTransform * v[(edge_idx + 1) % 3]; + + // Draw active edge as a green arrow, other edges as grey + if (active_edges & (1 << edge_idx)) + mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f); + else + mRenderer->DrawLine(v1, v2, Color::sGrey); + } + } + + const HeightFieldShape *mShape; + DebugRenderer * mRenderer; + RMat44 mTransform; + }; + + Visitor visitor(this, inRenderer, inCenterOfMassTransform.PreScaled(inScale)); + WalkHeightField(visitor); + } +} +#endif // JPH_DEBUG_RENDERER + +class HeightFieldShape::DecodingContext +{ +public: + JPH_INLINE explicit DecodingContext(const HeightFieldShape *inShape) : + mShape(inShape) + { + static_assert(sizeof(sGridOffsets) / sizeof(uint) == cNumBitsXY + 1, "Offsets array is not long enough"); + + // Construct root stack entry + mPropertiesStack[0] = 0; // level: 0, x: 0, y: 0 + } + + template + JPH_INLINE void WalkHeightField(Visitor &ioVisitor) + { + // Early out if there's no collision + if (mShape->mHeightSamplesSize == 0) + return; + + // Precalculate values relating to sample count + uint32 sample_count = mShape->mSampleCount; + UVec4 sample_count_min_1 = UVec4::sReplicate(sample_count - 1); + + // Precalculate values relating to block size + uint32 block_size = mShape->mBlockSize; + uint32 block_size_plus_1 = block_size + 1; + uint num_blocks = mShape->GetNumBlocks(); + uint num_blocks_min_1 = num_blocks - 1; + uint max_level = HeightFieldShape::sGetMaxLevel(num_blocks); + uint32 max_stride = (num_blocks + 1) >> 1; + + // Precalculate range block offset and stride for GetBlockOffsetAndScale + uint range_block_offset, range_block_stride; + sGetRangeBlockOffsetAndStride(num_blocks, max_level, range_block_offset, range_block_stride); + + // Allocate space for vertices and 'no collision' flags + int array_size = Square(block_size_plus_1); + Vec3 *vertices = reinterpret_cast(JPH_STACK_ALLOC(array_size * sizeof(Vec3))); + bool *no_collision = reinterpret_cast(JPH_STACK_ALLOC(array_size * sizeof(bool))); + + // Splat offsets + Vec4 ox = mShape->mOffset.SplatX(); + Vec4 oy = mShape->mOffset.SplatY(); + Vec4 oz = mShape->mOffset.SplatZ(); + + // Splat scales + Vec4 sx = mShape->mScale.SplatX(); + Vec4 sy = mShape->mScale.SplatY(); + Vec4 sz = mShape->mScale.SplatZ(); + + do + { + // Decode properties + uint32 properties_top = mPropertiesStack[mTop]; + uint32 x = properties_top & cMaskBitsXY; + uint32 y = (properties_top >> cNumBitsXY) & cMaskBitsXY; + uint32 level = properties_top >> cLevelShift; + + if (level >= max_level) + { + // Determine actual range of samples (minus one because we eventually want to iterate over the triangles, not the samples) + uint32 min_x = x * block_size; + uint32 max_x = min_x + block_size; + uint32 min_y = y * block_size; + uint32 max_y = min_y + block_size; + + // Decompress vertices of block at (x, y) + Vec3 *dst_vertex = vertices; + bool *dst_no_collision = no_collision; + float block_offset, block_scale; + mShape->GetBlockOffsetAndScale(x, y, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_y = min_y; v_y < max_y; ++v_y) + { + for (uint32 v_x = min_x; v_x < max_x; ++v_x) + { + *dst_vertex = mShape->GetPosition(v_x, v_y, block_offset, block_scale, *dst_no_collision); + ++dst_vertex; + ++dst_no_collision; + } + + // Skip last column, these values come from a different block + ++dst_vertex; + ++dst_no_collision; + } + + // Decompress block (x + 1, y) + uint32 max_x_decrement = 0; + if (x < num_blocks_min_1) + { + dst_vertex = vertices + block_size; + dst_no_collision = no_collision + block_size; + mShape->GetBlockOffsetAndScale(x + 1, y, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_y = min_y; v_y < max_y; ++v_y) + { + *dst_vertex = mShape->GetPosition(max_x, v_y, block_offset, block_scale, *dst_no_collision); + dst_vertex += block_size_plus_1; + dst_no_collision += block_size_plus_1; + } + } + else + max_x_decrement = 1; // We don't have a next block, one less triangle to test + + // Decompress block (x, y + 1) + if (y < num_blocks_min_1) + { + uint start = block_size * block_size_plus_1; + dst_vertex = vertices + start; + dst_no_collision = no_collision + start; + mShape->GetBlockOffsetAndScale(x, y + 1, range_block_offset, range_block_stride, block_offset, block_scale); + for (uint32 v_x = min_x; v_x < max_x; ++v_x) + { + *dst_vertex = mShape->GetPosition(v_x, max_y, block_offset, block_scale, *dst_no_collision); + ++dst_vertex; + ++dst_no_collision; + } + + // Decompress single sample of block at (x + 1, y + 1) + if (x < num_blocks_min_1) + { + mShape->GetBlockOffsetAndScale(x + 1, y + 1, range_block_offset, range_block_stride, block_offset, block_scale); + *dst_vertex = mShape->GetPosition(max_x, max_y, block_offset, block_scale, *dst_no_collision); + } + } + else + --max_y; // We don't have a next block, one less triangle to test + + // Update max_x (we've been using it so we couldn't update it earlier) + max_x -= max_x_decrement; + + // We're going to divide the vertices in 4 blocks to do one more runtime sub-division, calculate the ranges of those blocks + struct Range + { + uint32 mMinX, mMinY, mNumTrianglesX, mNumTrianglesY; + }; + uint32 half_block_size = block_size >> 1; + uint32 block_size_x = max_x - min_x - half_block_size; + uint32 block_size_y = max_y - min_y - half_block_size; + Range ranges[] = + { + { 0, 0, half_block_size, half_block_size }, + { half_block_size, 0, block_size_x, half_block_size }, + { 0, half_block_size, half_block_size, block_size_y }, + { half_block_size, half_block_size, block_size_x, block_size_y }, + }; + + // Calculate the min and max of each of the blocks + Mat44 block_min, block_max; + for (int block = 0; block < 4; ++block) + { + // Get the range for this block + const Range &range = ranges[block]; + uint32 start = range.mMinX + range.mMinY * block_size_plus_1; + uint32 size_x_plus_1 = range.mNumTrianglesX + 1; + uint32 size_y_plus_1 = range.mNumTrianglesY + 1; + + // Calculate where to start reading + const Vec3 *src_vertex = vertices + start; + const bool *src_no_collision = no_collision + start; + uint32 stride = block_size_plus_1 - size_x_plus_1; + + // Start range with a very large inside-out box + Vec3 value_min = Vec3::sReplicate(1.0e30f); + Vec3 value_max = Vec3::sReplicate(-1.0e30f); + + // Loop over the samples to determine the min and max of this block + for (uint32 block_y = 0; block_y < size_y_plus_1; ++block_y) + { + for (uint32 block_x = 0; block_x < size_x_plus_1; ++block_x) + { + if (!*src_no_collision) + { + value_min = Vec3::sMin(value_min, *src_vertex); + value_max = Vec3::sMax(value_max, *src_vertex); + } + ++src_vertex; + ++src_no_collision; + } + src_vertex += stride; + src_no_collision += stride; + } + block_min.SetColumn4(block, Vec4(value_min)); + block_max.SetColumn4(block, Vec4(value_max)); + } + + #ifdef JPH_DEBUG_HEIGHT_FIELD + // Draw the bounding boxes of the sub-nodes + for (int block = 0; block < 4; ++block) + { + AABox bounds(block_min.GetColumn3(block), block_max.GetColumn3(block)); + if (bounds.IsValid()) + DebugRenderer::sInstance->DrawWireBox(bounds, Color::sYellow); + } + #endif // JPH_DEBUG_HEIGHT_FIELD + + // Transpose so we have the mins and maxes of each of the blocks in rows instead of columns + Mat44 transposed_min = block_min.Transposed(); + Mat44 transposed_max = block_max.Transposed(); + + // Check which blocks collide + // Note: At this point we don't use our own stack but we do allow the visitor to use its own stack + // to store collision distances so that we can still early out when no closer hits have been found. + UVec4 colliding_blocks(0, 1, 2, 3); + int num_results = ioVisitor.VisitRangeBlock(transposed_min.GetColumn4(0), transposed_min.GetColumn4(1), transposed_min.GetColumn4(2), transposed_max.GetColumn4(0), transposed_max.GetColumn4(1), transposed_max.GetColumn4(2), colliding_blocks, mTop); + + // Loop through the results backwards (closest first) + int result = num_results - 1; + while (result >= 0) + { + // Calculate the min and max of this block + uint32 block = colliding_blocks[result]; + const Range &range = ranges[block]; + uint32 block_min_x = min_x + range.mMinX; + uint32 block_max_x = block_min_x + range.mNumTrianglesX; + uint32 block_min_y = min_y + range.mMinY; + uint32 block_max_y = block_min_y + range.mNumTrianglesY; + + // Loop triangles + for (uint32 v_y = block_min_y; v_y < block_max_y; ++v_y) + for (uint32 v_x = block_min_x; v_x < block_max_x; ++v_x) + { + // Get first vertex + const int offset = (v_y - min_y) * block_size_plus_1 + (v_x - min_x); + const Vec3 *start_vertex = vertices + offset; + const bool *start_no_collision = no_collision + offset; + + // Check if vertices shared by both triangles have collision + if (!start_no_collision[0] && !start_no_collision[block_size_plus_1 + 1]) + { + // Loop 2 triangles + for (uint t = 0; t < 2; ++t) + { + // Determine triangle vertices + Vec3 v0, v1, v2; + if (t == 0) + { + // Check third vertex + if (start_no_collision[block_size_plus_1]) + continue; + + // Get vertices for triangle + v0 = start_vertex[0]; + v1 = start_vertex[block_size_plus_1]; + v2 = start_vertex[block_size_plus_1 + 1]; + } + else + { + // Check third vertex + if (start_no_collision[1]) + continue; + + // Get vertices for triangle + v0 = start_vertex[0]; + v1 = start_vertex[block_size_plus_1 + 1]; + v2 = start_vertex[1]; + } + + #ifdef JPH_DEBUG_HEIGHT_FIELD + DebugRenderer::sInstance->DrawWireTriangle(RVec3(v0), RVec3(v1), RVec3(v2), Color::sWhite); + #endif + + // Call visitor + ioVisitor.VisitTriangle(v_x, v_y, t, v0, v1, v2); + + // Check if we're done + if (ioVisitor.ShouldAbort()) + return; + } + } + } + + // Fetch next block until we find one that the visitor wants to see + do + --result; + while (result >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop + result)); + } + } + else + { + // Visit child grid + uint32 stride = min(1U << level, max_stride); // At the most detailed level we store a non-power of 2 number of blocks + uint32 offset = sGridOffsets[level] + stride * y + x; + + // Decode min/max height + JPH_ASSERT(offset < mShape->mRangeBlocksSize); + UVec4 block = UVec4::sLoadInt4Aligned(reinterpret_cast(&mShape->mRangeBlocks[offset])); + Vec4 bounds_miny = oy + sy * block.Expand4Uint16Lo().ToFloat(); + Vec4 bounds_maxy = oy + sy * block.Expand4Uint16Hi().ToFloat(); + + // Calculate size of one cell at this grid level + UVec4 internal_cell_size = UVec4::sReplicate(block_size << (max_level - level - 1)); // subtract 1 from level because we have an internal grid of 2x2 + + // Calculate min/max x and z + UVec4 two_x = UVec4::sReplicate(2 * x); // multiply by two because we have an internal grid of 2x2 + Vec4 bounds_minx = ox + sx * (internal_cell_size * (two_x + UVec4(0, 1, 0, 1))).ToFloat(); + Vec4 bounds_maxx = ox + sx * UVec4::sMin(internal_cell_size * (two_x + UVec4(1, 2, 1, 2)), sample_count_min_1).ToFloat(); + + UVec4 two_y = UVec4::sReplicate(2 * y); + Vec4 bounds_minz = oz + sz * (internal_cell_size * (two_y + UVec4(0, 0, 1, 1))).ToFloat(); + Vec4 bounds_maxz = oz + sz * UVec4::sMin(internal_cell_size * (two_y + UVec4(1, 1, 2, 2)), sample_count_min_1).ToFloat(); + + // Calculate properties of child blocks + UVec4 properties = UVec4::sReplicate(((level + 1) << cLevelShift) + (y << (cNumBitsXY + 1)) + (x << 1)) + UVec4(0, 1, 1 << cNumBitsXY, (1 << cNumBitsXY) + 1); + + #ifdef JPH_DEBUG_HEIGHT_FIELD + // Draw boxes + for (int i = 0; i < 4; ++i) + { + AABox b(Vec3(bounds_minx[i], bounds_miny[i], bounds_minz[i]), Vec3(bounds_maxx[i], bounds_maxy[i], bounds_maxz[i])); + if (b.IsValid()) + DebugRenderer::sInstance->DrawWireBox(b, Color::sGreen); + } + #endif + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitRangeBlock(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, mTop); + + // Push them onto the stack + JPH_ASSERT(mTop + 4 < cStackSize); + properties.StoreInt4(&mPropertiesStack[mTop]); + mTop += num_results; + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + return; + + // Fetch next node until we find one that the visitor wants to see + do + --mTop; + while (mTop >= 0 && !ioVisitor.ShouldVisitRangeBlock(mTop)); + } + while (mTop >= 0); + } + + // This can be used to have the visitor early out (ioVisitor.ShouldAbort() returns true) and later continue again (call WalkHeightField() again) + JPH_INLINE bool IsDoneWalking() const + { + return mTop < 0; + } + +private: + const HeightFieldShape * mShape; + int mTop = 0; + uint32 mPropertiesStack[cStackSize]; +}; + +template +void HeightFieldShape::WalkHeightField(Visitor &ioVisitor) const +{ + DecodingContext ctx(this); + ctx.WalkHeightField(ioVisitor); +} + +bool HeightFieldShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) : + mHit(ioHit), + mRayOrigin(inRay.mOrigin), + mRayDirection(inRay.mDirection), + mRayInvDirection(inRay.mDirection), + mShape(inShape), + mSubShapeIDCreator(inSubShapeIDCreator) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mHit.mFraction) + { + // It's a closer hit + mHit.mFraction = fraction; + mHit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle); + mReturnValue = true; + } + } + + RayCastResult & mHit; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + const HeightFieldShape *mShape; + SubShapeIDCreator mSubShapeIDCreator; + bool mReturnValue = false; + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(this, inRay, inSubShapeIDCreator, ioHit); + WalkHeightField(visitor); + + return visitor.mReturnValue; +} + +void HeightFieldShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor + { + JPH_INLINE explicit Visitor(const HeightFieldShape *inShape, const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector) : + mCollector(ioCollector), + mRayOrigin(inRay.mOrigin), + mRayDirection(inRay.mDirection), + mRayInvDirection(inRay.mDirection), + mBackFaceMode(inRayCastSettings.mBackFaceMode), + mShape(inShape), + mSubShapeIDCreator(inSubShapeIDCreator) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) const + { + // Back facing check + if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0) + return; + + // Check the triangle + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = mShape->EncodeSubShapeID(mSubShapeIDCreator, inX, inY, inTriangle); + mCollector.AddHit(hit); + } + } + + CastRayCollector & mCollector; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + EBackFaceMode mBackFaceMode; + const HeightFieldShape *mShape; + SubShapeIDCreator mSubShapeIDCreator; + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(this, inRay, inRayCastSettings, inSubShapeIDCreator, ioCollector); + WalkHeightField(visitor); +} + +void HeightFieldShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // A height field doesn't have volume, so we can't test insideness +} + +void HeightFieldShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideSoftBodyVerticesVsTriangles + { + using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return mDistanceStack[inStackTop] < mClosestDistanceSq; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Get distance to vertex + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle([[maybe_unused]] uint inX, [[maybe_unused]] uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + ProcessTriangle(inV0, inV1, inV2); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inCenterOfMassTransform, inScale); + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + visitor.StartVertex(*v); + WalkHeightField(visitor); + visitor.FinishVertex(*v, inCollidingShapeIndex); + } +} + +void HeightFieldShape::sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastConvexVsTriangles + { + using CastConvexVsTriangles::CastConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Clear distance for invalid bounds + distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create sub shape id for this part + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + SubShapeIDCreator mSubShapeIDCreator2; + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField); + const HeightFieldShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mShape2 = shape; + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter(); + visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent(); + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape->WalkHeightField(visitor); +} + +void HeightFieldShape::sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastSphereVsTriangles + { + using CastSphereVsTriangles::CastSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the radius of the sphere + AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Clear distance for invalid bounds + distance = Vec4::sSelect(Vec4::sReplicate(FLT_MAX), distance, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create sub shape id for this part + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Cast(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + RayInvDirection mInvDirection; + SubShapeIDCreator mSubShapeIDCreator2; + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::HeightField); + const HeightFieldShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mShape2 = shape; + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape->WalkHeightField(visitor); +} + +struct HeightFieldShape::HSGetTrianglesContext +{ + HSGetTrianglesContext(const HeightFieldShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mDecodeCtx(inShape), + mShape(inShape), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mHeightFieldScale(inScale), + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + } + + bool ShouldAbort() const + { + return mShouldAbort; + } + + bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mHeightFieldScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + void VisitTriangle(uint inX, uint inY, [[maybe_unused]] uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // When the buffer is full and we cannot process the triangles, abort the height field walk. The next time GetTrianglesNext is called we will continue here. + if (mNumTrianglesFound + 1 > mMaxTrianglesRequested) + { + mShouldAbort = true; + return; + } + + // Store vertices as Float3 + if (mIsInsideOut) + { + // Reverse vertices + (mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++); + } + else + { + // Normal scale + (mLocalToWorld * inV0).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV1).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * inV2).StoreFloat3(mTriangleVertices++); + } + + // Decode material + if (mMaterials != nullptr) + *mMaterials++ = mShape->GetMaterial(inX, inY); + + // Accumulate triangles found + mNumTrianglesFound++; + } + + DecodingContext mDecodeCtx; + const HeightFieldShape * mShape; + OrientedBox mLocalBox; + Vec3 mHeightFieldScale; + Mat44 mLocalToWorld; + int mMaxTrianglesRequested; + Float3 * mTriangleVertices; + int mNumTrianglesFound; + const PhysicsMaterial ** mMaterials; + bool mShouldAbort; + bool mIsInsideOut; +}; + +void HeightFieldShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(HSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(HSGetTrianglesContext))); + + new (&ioContext) HSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale); +} + +int HeightFieldShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 1, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + HSGetTrianglesContext &context = (HSGetTrianglesContext &)ioContext; + if (context.mDecodeCtx.IsDoneWalking()) + return 0; + + // Store parameters on context + context.mMaxTrianglesRequested = inMaxTrianglesRequested; + context.mTriangleVertices = outTriangleVertices; + context.mMaterials = outMaterials; + context.mShouldAbort = false; // Reset the abort flag + context.mNumTrianglesFound = 0; + + // Continue (or start) walking the height field + context.mDecodeCtx.WalkHeightField(context); + return context.mNumTrianglesFound; +} + +void HeightFieldShape::sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField); + const ConvexShape *shape1 = static_cast(inShape1); + const HeightFieldShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideConvexVsTriangles + { + using CollideConvexVsTriangles::CollideConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create ID for triangle + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + SubShapeIDCreator mSubShapeIDCreator2; + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + visitor.mShape2 = shape2; + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape2->WalkHeightField(visitor); +} + +void HeightFieldShape::sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + JPH_ASSERT(inShape2->GetType() == EShapeType::HeightField); + const SphereShape *shape1 = static_cast(inShape1); + const HeightFieldShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideSphereVsTriangles + { + using CollideSphereVsTriangles::CollideSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitRangeBlock([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitRangeBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Filter out invalid bounding boxes + collides = UVec4::sAnd(collides, Vec4::sLessOrEqual(inBoundsMinY, inBoundsMaxY)); + + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(uint inX, uint inY, uint inTriangle, Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2) + { + // Create ID for triangle + SubShapeID triangle_sub_shape_id = mShape2->EncodeSubShapeID(mSubShapeIDCreator2, inX, inY, inTriangle); + + // Determine active edges + uint8 active_edges = mShape2->GetEdgeFlags(inX, inY, inTriangle); + + Collide(inV0, inV1, inV2, active_edges, triangle_sub_shape_id); + } + + const HeightFieldShape * mShape2; + SubShapeIDCreator mSubShapeIDCreator2; + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + visitor.mShape2 = shape2; + visitor.mSubShapeIDCreator2 = inSubShapeIDCreator2; + shape2->WalkHeightField(visitor); +} + +void HeightFieldShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(mOffset); + inStream.Write(mScale); + inStream.Write(mSampleCount); + inStream.Write(mBlockSize); + inStream.Write(mBitsPerSample); + inStream.Write(mMinSample); + inStream.Write(mMaxSample); + inStream.Write(mMaterialIndices); + inStream.Write(mNumBitsPerMaterialIndex); + + if (mRangeBlocks != nullptr) + { + inStream.Write(true); + inStream.WriteBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); + } + else + { + inStream.Write(false); + } +} + +void HeightFieldShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(mOffset); + inStream.Read(mScale); + inStream.Read(mSampleCount); + inStream.Read(mBlockSize); + inStream.Read(mBitsPerSample); + inStream.Read(mMinSample); + inStream.Read(mMaxSample); + inStream.Read(mMaterialIndices); + inStream.Read(mNumBitsPerMaterialIndex); + + // We don't have the exact number of reserved materials anymore, but ensure that our array is big enough + // TODO: Next time when we bump the binary serialization format of this class we should store the capacity and allocate the right amount, for now we accept a little bit of waste + mMaterials.reserve(PhysicsMaterialList::size_type(1) << mNumBitsPerMaterialIndex); + + CacheValues(); + + bool has_heights = false; + inStream.Read(has_heights); + if (has_heights) + { + AllocateBuffers(); + inStream.ReadBytes(mRangeBlocks, mRangeBlocksSize * sizeof(RangeBlock) + mHeightSamplesSize + mActiveEdgesSize); + } +} + +void HeightFieldShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = mMaterials; +} + +void HeightFieldShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + mMaterials.assign(inMaterials, inMaterials + inNumMaterials); +} + +Shape::Stats HeightFieldShape::GetStats() const +{ + return Stats( + sizeof(*this) + + mMaterials.size() * sizeof(Ref) + + mRangeBlocksSize * sizeof(RangeBlock) + + mHeightSamplesSize * sizeof(uint8) + + mActiveEdgesSize * sizeof(uint8) + + mMaterialIndices.size() * sizeof(uint8), + mHeightSamplesSize == 0? 0 : Square(mSampleCount - 1) * 2); +} + +void HeightFieldShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::HeightField); + f.mConstruct = []() -> Shape * { return new HeightFieldShape; }; + f.mColor = Color::sPurple; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::HeightField, sCollideConvexVsHeightField); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::HeightField, sCastConvexVsHeightField); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::HeightField, s, CollisionDispatch::sReversedCollideShape); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCollideSphereVsHeightField); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::HeightField, sCastSphereVsHeightField); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.h new file mode 100644 index 000000000..350b63f8e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/HeightFieldShape.h @@ -0,0 +1,373 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class ConvexShape; +class CollideShapeSettings; +class TempAllocator; + +/// Constants for HeightFieldShape, this was moved out of the HeightFieldShape because of a linker bug +namespace HeightFieldShapeConstants +{ + /// Value used to create gaps in the height field + constexpr float cNoCollisionValue = FLT_MAX; + + /// Stack size to use during WalkHeightField + constexpr int cStackSize = 128; + + /// A position in the hierarchical grid is defined by a level (which grid), x and y position. We encode this in a single uint32 as: level << 28 | y << 14 | x + constexpr uint cNumBitsXY = 14; + constexpr uint cMaskBitsXY = (1 << cNumBitsXY) - 1; + constexpr uint cLevelShift = 2 * cNumBitsXY; + + /// When height samples are converted to 16 bit: + constexpr uint16 cNoCollisionValue16 = 0xffff; ///< This is the magic value for 'no collision' + constexpr uint16 cMaxHeightValue16 = 0xfffe; ///< This is the maximum allowed height value +}; + +/// Class that constructs a HeightFieldShape +class JPH_EXPORT HeightFieldShapeSettings final : public ShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HeightFieldShapeSettings) + + /// Default constructor for deserialization + HeightFieldShapeSettings() = default; + + /// Create a height field shape of inSampleCount * inSampleCount vertices. + /// The height field is a surface defined by: inOffset + inScale * (x, inSamples[y * inSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, inSampleCount - 1]. + /// inSampleCount: inSampleCount / mBlockSize must be minimally 2 and a power of 2 is the most efficient in terms of performance and storage. + /// inSamples: inSampleCount^2 vertices. + /// inMaterialIndices: (inSampleCount - 1)^2 indices that index into inMaterialList. + HeightFieldShapeSettings(const float *inSamples, Vec3Arg inOffset, Vec3Arg inScale, uint32 inSampleCount, const uint8 *inMaterialIndices = nullptr, const PhysicsMaterialList &inMaterialList = PhysicsMaterialList()); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Determine the minimal and maximal value of mHeightSamples (will ignore cNoCollisionValue) + /// @param outMinValue The minimal value of mHeightSamples or FLT_MAX if no samples have collision + /// @param outMaxValue The maximal value of mHeightSamples or -FLT_MAX if no samples have collision + /// @param outQuantizationScale (value - outMinValue) * outQuantizationScale quantizes a height sample to 16 bits + void DetermineMinAndMaxSample(float &outMinValue, float &outMaxValue, float &outQuantizationScale) const; + + /// Given mBlockSize, mSampleCount and mHeightSamples, calculate the amount of bits needed to stay below absolute error inMaxError + /// @param inMaxError Maximum allowed error in mHeightSamples after compression (note that this does not take mScale.Y into account) + /// @return Needed bits per sample in the range [1, 8]. + uint32 CalculateBitsPerSampleForError(float inMaxError) const; + + /// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, mSampleCount - 1]. + Vec3 mOffset = Vec3::sZero(); + Vec3 mScale = Vec3::sReplicate(1.0f); + uint32 mSampleCount = 0; + + /// Artificial minimal value of mHeightSamples, used for compression and can be used to update the terrain after creating with lower height values. If there are any lower values in mHeightSamples, this value will be ignored. + float mMinHeightValue = FLT_MAX; + + /// Artificial maximum value of mHeightSamples, used for compression and can be used to update the terrain after creating with higher height values. If there are any higher values in mHeightSamples, this value will be ignored. + float mMaxHeightValue = -FLT_MAX; + + /// When bigger than mMaterials.size() the internal material list will be preallocated to support this number of materials. + /// This avoids reallocations when calling HeightFieldShape::SetMaterials with new materials later. + uint32 mMaterialsCapacity = 0; + + /// The heightfield is divided in blocks of mBlockSize * mBlockSize * 2 triangles and the acceleration structure culls blocks only, + /// bigger block sizes reduce memory consumption but also reduce query performance. Sensible values are [2, 8], does not need to be + /// a power of 2. Note that at run-time we'll perform one more grid subdivision, so the effective block size is half of what is provided here. + uint32 mBlockSize = 2; + + /// How many bits per sample to use to compress the height field. Can be in the range [1, 8]. + /// Note that each sample is compressed relative to the min/max value of its block of mBlockSize * mBlockSize pixels so the effective precision is higher. + /// Also note that increasing mBlockSize saves more memory than reducing the amount of bits per sample. + uint32 mBitsPerSample = 8; + + /// An array of mSampleCount^2 height samples. Samples are stored in row major order, so the sample at (x, y) is at index y * mSampleCount + x. + Array mHeightSamples; + + /// An array of (mSampleCount - 1)^2 material indices. + Array mMaterialIndices; + + /// The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]] + PhysicsMaterialList mMaterials; + + /// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + /// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly). + /// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees). + float mActiveEdgeCosThresholdAngle = 0.996195f; // cos(5 degrees) +}; + +/// A height field shape. Cannot be used as a dynamic object. +/// +/// Note: If you're using HeightFieldShape and are querying data while modifying the shape you'll have a race condition. +/// In this case it is best to create a new HeightFieldShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape. +/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes. +class JPH_EXPORT HeightFieldShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + HeightFieldShape() : Shape(EShapeType::HeightField, EShapeSubType::HeightField) { } + HeightFieldShape(const HeightFieldShapeSettings &inSettings, ShapeResult &outResult); + virtual ~HeightFieldShape() override; + + /// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information. + Ref Clone() const; + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + /// Get the size of the height field. Note that this will always be rounded up to the nearest multiple of GetBlockSize(). + inline uint GetSampleCount() const { return mSampleCount; } + + /// Get the size of a block + inline uint GetBlockSize() const { return mBlockSize; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override { return GetSubShapeIDBits(); } + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + /// Overload to get the material at a particular location + const PhysicsMaterial * GetMaterial(uint inX, uint inY) const; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + /// Get height field position at sampled location (inX, inY). + /// where inX and inY are integers in the range inX e [0, mSampleCount - 1] and inY e [0, mSampleCount - 1]. + Vec3 GetPosition(uint inX, uint inY) const; + + /// Check if height field at sampled location (inX, inY) has collision (has a hole or not) + bool IsNoCollision(uint inX, uint inY) const; + + /// Projects inLocalPosition (a point in the space of the shape) along the Y axis onto the surface and returns it in outSurfacePosition. + /// When there is no surface position (because of a hole or because the point is outside the heightfield) the function will return false. + bool ProjectOntoSurface(Vec3Arg inLocalPosition, Vec3 &outSurfacePosition, SubShapeID &outSubShapeID) const; + + /// Get the range of height values that this height field can encode. Can be used to determine the allowed range when setting the height values with SetHeights. + float GetMinHeightValue() const { return mOffset.GetY(); } + float GetMaxHeightValue() const { return mOffset.GetY() + mScale.GetY() * HeightFieldShapeConstants::cMaxHeightValue16; } + + /// Get the height values of a block of data. + /// Note that the height values are decompressed so will be slightly different from what the shape was originally created with. + /// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param outHeights Returned height values, must be at least inSizeX * inSizeY floats. Values are returned in x-major order and can be cNoCollisionValue. + /// @param inHeightsStride Stride in floats between two consecutive rows of outHeights (can be negative if the data is upside down). + void GetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, float *outHeights, intptr_t inHeightsStride) const; + + /// Set the height values of a block of data. + /// Note that this requires decompressing and recompressing a border of size mBlockSize in the negative x/y direction so will cause some precision loss. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inX Start X position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must be a multiple of mBlockSize and in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param inSizeY Number of samples in Y direction, must be a multiple of mBlockSize and in the range [0, mSampleCount - inX] + /// @param inHeights The new height values to set, must be an array of inSizeX * inSizeY floats, can be cNoCollisionValue. Values outside of the range [GetMinHeightValue(), GetMaxHeightValue()] will be clamped. + /// @param inHeightsStride Stride in floats between two consecutive rows of outHeights (can be negative if the data is upside down). + /// @param inAllocator Allocator to use for temporary memory + /// @param inActiveEdgeCosThresholdAngle Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + void SetHeights(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, intptr_t inHeightsStride, TempAllocator &inAllocator, float inActiveEdgeCosThresholdAngle = 0.996195f); + + /// Get the current list of materials, the indices returned by GetMaterials() will index into this list. + const PhysicsMaterialList & GetMaterialList() const { return mMaterials; } + + /// Get the material indices of a block of data. + /// @param inX Start X position, must in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction + /// @param inSizeY Number of samples in Y direction + /// @param outMaterials Returned material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order. + /// @param inMaterialsStride Stride in uint8s between two consecutive rows of outMaterials (can be negative if the data is upside down). + void GetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, uint8 *outMaterials, intptr_t inMaterialsStride) const; + + /// Set the material indices of a block of data. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inX Start X position, must in the range [0, mSampleCount - 1] + /// @param inY Start Y position, must in the range [0, mSampleCount - 1] + /// @param inSizeX Number of samples in X direction + /// @param inSizeY Number of samples in Y direction + /// @param inMaterials The new material indices, must be at least inSizeX * inSizeY uint8s. Values are returned in x-major order. + /// @param inMaterialsStride Stride in uint8s between two consecutive rows of inMaterials (can be negative if the data is upside down). + /// @param inMaterialList The material list to use for the new material indices or nullptr if the material list should not be updated + /// @param inAllocator Allocator to use for temporary memory + /// @return True if the material indices were set, false if the total number of materials exceeded 256 + bool SetMaterials(uint inX, uint inY, uint inSizeX, uint inSizeY, const uint8 *inMaterials, intptr_t inMaterialsStride, const PhysicsMaterialList *inMaterialList, TempAllocator &inAllocator); + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + +#ifdef JPH_DEBUG_RENDERER + // Settings + static bool sDrawTriangleOutlines; +#endif // JPH_DEBUG_RENDERER + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + class DecodingContext; ///< Context class for walking through all nodes of a heightfield + struct HSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + /// Calculate commonly used values and store them in the shape + void CacheValues(); + + /// Allocate the mRangeBlocks, mHeightSamples and mActiveEdges buffers as a single data block + void AllocateBuffers(); + + /// Calculate bit mask for all active edges in the heightfield for a specific region + void CalculateActiveEdges(uint inX, uint inY, uint inSizeX, uint inSizeY, const float *inHeights, uint inHeightsStartX, uint inHeightsStartY, intptr_t inHeightsStride, float inHeightsScale, float inActiveEdgeCosThresholdAngle, TempAllocator &inAllocator); + + /// Calculate bit mask for all active edges in the heightfield + void CalculateActiveEdges(const HeightFieldShapeSettings &inSettings); + + /// Store material indices in the least amount of bits per index possible + void StoreMaterialIndices(const HeightFieldShapeSettings &inSettings); + + /// Get the amount of horizontal/vertical blocks + inline uint GetNumBlocks() const { return mSampleCount / mBlockSize; } + + /// Get the maximum level (amount of grids) of the tree + static inline uint sGetMaxLevel(uint inNumBlocks) { return 32 - CountLeadingZeros(inNumBlocks - 1); } + + /// Get the range block offset and stride for GetBlockOffsetAndScale + static inline void sGetRangeBlockOffsetAndStride(uint inNumBlocks, uint inMaxLevel, uint &outRangeBlockOffset, uint &outRangeBlockStride); + + /// For block (inBlockX, inBlockY) get the offset and scale needed to decode a uint8 height sample to a uint16 + inline void GetBlockOffsetAndScale(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, float &outBlockOffset, float &outBlockScale) const; + + /// Get the height sample at position (inX, inY) + inline uint8 GetHeightSample(uint inX, uint inY) const; + + /// Faster version of GetPosition when block offset and scale are already known + inline Vec3 GetPosition(uint inX, uint inY, float inBlockOffset, float inBlockScale, bool &outNoCollision) const; + + /// Determine amount of bits needed to encode sub shape id + uint GetSubShapeIDBits() const; + + /// En/decode a sub shape ID. inX and inY specify the coordinate of the triangle. inTriangle == 0 is the lower triangle, inTriangle == 1 is the upper triangle. + inline SubShapeID EncodeSubShapeID(const SubShapeIDCreator &inCreator, uint inX, uint inY, uint inTriangle) const; + inline void DecodeSubShapeID(const SubShapeID &inSubShapeID, uint &outX, uint &outY, uint &outTriangle) const; + + /// Get the edge flags for a triangle + inline uint8 GetEdgeFlags(uint inX, uint inY, uint inTriangle) const; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsHeightField(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsHeightField(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Visit the entire height field using a visitor pattern + /// Note: Used to be inlined but this triggers a bug in MSVC where it will not free the memory allocated by alloca which causes a stack overflow when WalkHeightField is called in a loop (clang does it correct) + template + void WalkHeightField(Visitor &ioVisitor) const; + + /// A block of 2x2 ranges used to form a hierarchical grid, ordered left top, right top, left bottom, right bottom + struct alignas(16) RangeBlock + { + uint16 mMin[4]; + uint16 mMax[4]; + }; + + /// For block (inBlockX, inBlockY) get the range block and the entry in the range block + inline void GetRangeBlock(uint inBlockX, uint inBlockY, uint inRangeBlockOffset, uint inRangeBlockStride, RangeBlock *&outBlock, uint &outIndexInBlock); + + /// Offset of first RangedBlock in grid per level + static const uint sGridOffsets[]; + + /// The height field is a surface defined by: mOffset + mScale * (x, mHeightSamples[y * mSampleCount + x], y). + /// where x and y are integers in the range x and y e [0, mSampleCount - 1]. + Vec3 mOffset = Vec3::sZero(); + Vec3 mScale = Vec3::sReplicate(1.0f); + + /// Height data + uint32 mSampleCount = 0; ///< See HeightFieldShapeSettings::mSampleCount + uint32 mBlockSize = 2; ///< See HeightFieldShapeSettings::mBlockSize + uint32 mHeightSamplesSize = 0; ///< Size of mHeightSamples in bytes + uint32 mRangeBlocksSize = 0; ///< Size of mRangeBlocks in elements + uint32 mActiveEdgesSize = 0; ///< Size of mActiveEdges in bytes + uint8 mBitsPerSample = 8; ///< See HeightFieldShapeSettings::mBitsPerSample + uint8 mSampleMask = 0xff; ///< All bits set for a sample: (1 << mBitsPerSample) - 1, used to indicate that there's no collision + uint16 mMinSample = HeightFieldShapeConstants::cNoCollisionValue16; ///< Min and max value in mHeightSamples quantized to 16 bit, for calculating bounding box + uint16 mMaxSample = HeightFieldShapeConstants::cNoCollisionValue16; + RangeBlock * mRangeBlocks = nullptr; ///< Hierarchical grid of range data describing the height variations within 1 block. The grid for level starts at offset sGridOffsets[] + uint8 * mHeightSamples = nullptr; ///< mBitsPerSample-bit height samples. Value [0, mMaxHeightValue] maps to highest detail grid in mRangeBlocks [mMin, mMax]. mNoCollisionValue is reserved to indicate no collision. + uint8 * mActiveEdges = nullptr; ///< (mSampleCount - 1)^2 * 3-bit active edge flags. + + /// Materials + PhysicsMaterialList mMaterials; ///< The materials of square at (x, y) is: mMaterials[mMaterialIndices[x + y * (mSampleCount - 1)]] + Array mMaterialIndices; ///< Compressed to the minimum amount of bits per material index (mSampleCount - 1) * (mSampleCount - 1) * mNumBitsPerMaterialIndex bits of data + uint32 mNumBitsPerMaterialIndex = 0; ///< Number of bits per material index + +#ifdef JPH_DEBUG_RENDERER + /// Temporary rendering data + mutable Array mGeometry; + mutable bool mCachedUseMaterialColors = false; ///< This is used to regenerate the triangle batch if the drawing settings change +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.cpp new file mode 100644 index 000000000..cf5e86403 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.cpp @@ -0,0 +1,1244 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool MeshShape::sDrawTriangleGroups = false; +bool MeshShape::sDrawTriangleOutlines = false; +#endif // JPH_DEBUG_RENDERER + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MeshShapeSettings) +{ + JPH_ADD_BASE_CLASS(MeshShapeSettings, ShapeSettings) + + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mTriangleVertices) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mIndexedTriangles) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaterials) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mMaxTrianglesPerLeaf) + JPH_ADD_ATTRIBUTE(MeshShapeSettings, mActiveEdgeCosThresholdAngle) +} + +// Codecs this mesh shape is using +using TriangleCodec = TriangleCodecIndexed8BitPackSOA4Flags; +using NodeCodec = NodeCodecQuadTreeHalfFloat<1>; + +// Get header for tree +static JPH_INLINE const NodeCodec::Header *sGetNodeHeader(const ByteBuffer &inTree) +{ + return inTree.Get(0); +} + +// Get header for triangles +static JPH_INLINE const TriangleCodec::TriangleHeader *sGetTriangleHeader(const ByteBuffer &inTree) +{ + return inTree.Get(NodeCodec::HeaderSize); +} + +MeshShapeSettings::MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials) : + mMaterials(std::move(inMaterials)) +{ + Indexify(inTriangles, mTriangleVertices, mIndexedTriangles); + + Sanitize(); +} + +MeshShapeSettings::MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials) : + mTriangleVertices(std::move(inVertices)), + mIndexedTriangles(std::move(inTriangles)), + mMaterials(std::move(inMaterials)) +{ + Sanitize(); +} + +void MeshShapeSettings::Sanitize() +{ + // Remove degenerate and duplicate triangles + UnorderedSet triangles; + triangles.reserve(mIndexedTriangles.size()); + TriangleCodec::ValidationContext validation_ctx(mIndexedTriangles, mTriangleVertices); + for (int t = (int)mIndexedTriangles.size() - 1; t >= 0; --t) + { + const IndexedTriangle &tri = mIndexedTriangles[t]; + + if (tri.IsDegenerate(mTriangleVertices) // Degenerate triangle + || validation_ctx.IsDegenerate(tri) // Triangle is degenerate in the quantized space + || !triangles.insert(tri.GetLowestIndexFirst()).second) // Duplicate triangle + { + // The order of triangles doesn't matter (gets reordered while building the tree), so we can just swap the last triangle into this slot + mIndexedTriangles[t] = mIndexedTriangles.back(); + mIndexedTriangles.pop_back(); + } + } +} + +ShapeSettings::ShapeResult MeshShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new MeshShape(*this, mCachedResult); + return mCachedResult; +} + +MeshShape::MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult) : + Shape(EShapeType::Mesh, EShapeSubType::Mesh, inSettings, outResult) +{ + // Check if there are any triangles + if (inSettings.mIndexedTriangles.empty()) + { + outResult.SetError("Need triangles to create a mesh shape!"); + return; + } + + // Check triangles + TriangleCodec::ValidationContext validation_ctx(inSettings.mIndexedTriangles, inSettings.mTriangleVertices); + for (int t = (int)inSettings.mIndexedTriangles.size() - 1; t >= 0; --t) + { + const IndexedTriangle &triangle = inSettings.mIndexedTriangles[t]; + if (triangle.IsDegenerate(inSettings.mTriangleVertices) + || validation_ctx.IsDegenerate(triangle)) + { + outResult.SetError(StringFormat("Triangle %d is degenerate!", t)); + return; + } + else + { + // Check vertex indices + for (uint32 idx : triangle.mIdx) + if (idx >= inSettings.mTriangleVertices.size()) + { + outResult.SetError(StringFormat("Vertex index %u is beyond vertex list (size: %u)", idx, (uint)inSettings.mTriangleVertices.size())); + return; + } + } + } + + // Copy materials + mMaterials = inSettings.mMaterials; + if (!mMaterials.empty()) + { + // Validate materials + if (mMaterials.size() > (1 << FLAGS_MATERIAL_BITS)) + { + outResult.SetError(StringFormat("Supporting max %d materials per mesh", 1 << FLAGS_MATERIAL_BITS)); + return; + } + for (const IndexedTriangle &t : inSettings.mIndexedTriangles) + if (t.mMaterialIndex >= mMaterials.size()) + { + outResult.SetError(StringFormat("Triangle material %u is beyond material list (size: %u)", t.mMaterialIndex, (uint)mMaterials.size())); + return; + } + } + else + { + // No materials assigned, validate that all triangles use material index 0 + for (const IndexedTriangle &t : inSettings.mIndexedTriangles) + if (t.mMaterialIndex != 0) + { + outResult.SetError("No materials present, all triangles should have material index 0"); + return; + } + } + + // Check max triangles + if (inSettings.mMaxTrianglesPerLeaf < 1 || inSettings.mMaxTrianglesPerLeaf > MaxTrianglesPerLeaf) + { + outResult.SetError("Invalid max triangles per leaf"); + return; + } + + // Fill in active edge bits + IndexedTriangleList indexed_triangles = inSettings.mIndexedTriangles; // Copy indices since we're adding the 'active edge' flag + sFindActiveEdges(inSettings, indexed_triangles); + + // Create triangle splitter + TriangleSplitterBinning splitter(inSettings.mTriangleVertices, indexed_triangles); + + // Build tree + AABBTreeBuilder builder(splitter, inSettings.mMaxTrianglesPerLeaf); + AABBTreeBuilderStats builder_stats; + AABBTreeBuilder::Node *root = builder.Build(builder_stats); + + // Convert to buffer + AABBTreeToBuffer buffer; + const char *error = nullptr; + if (!buffer.Convert(inSettings.mTriangleVertices, root, error)) + { + outResult.SetError(error); + delete root; + return; + } + + // Kill tree + delete root; + + // Move data to this class + mTree.swap(buffer.GetBuffer()); + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Mesh is too big and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +void MeshShape::sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices) +{ + // A struct to hold the two vertex indices of an edge + struct Edge + { + Edge(int inIdx1, int inIdx2) : mIdx1(min(inIdx1, inIdx2)), mIdx2(max(inIdx1, inIdx2)) { } + + uint GetIndexInTriangle(const IndexedTriangle &inTriangle) const + { + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + Edge edge(inTriangle.mIdx[edge_idx], inTriangle.mIdx[(edge_idx + 1) % 3]); + if (*this == edge) + return edge_idx; + } + + JPH_ASSERT(false); + return ~uint(0); + } + + bool operator == (const Edge &inRHS) const + { + return mIdx1 == inRHS.mIdx1 && mIdx2 == inRHS.mIdx2; + } + + int mIdx1; + int mIdx2; + }; + + JPH_MAKE_HASH_STRUCT(Edge, EdgeHash, t.mIdx1, t.mIdx2) + + // A struct to hold the triangles that are connected to an edge + struct TriangleIndices + { + uint mNumTriangles = 0; + uint mTriangleIndices[2]; + }; + + // Build a list of edge to triangles + using EdgeToTriangle = UnorderedMap; + EdgeToTriangle edge_to_triangle; + edge_to_triangle.reserve(ioIndices.size() * 3); + for (uint triangle_idx = 0; triangle_idx < ioIndices.size(); ++triangle_idx) + { + IndexedTriangle &triangle = ioIndices[triangle_idx]; + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + Edge edge(triangle.mIdx[edge_idx], triangle.mIdx[(edge_idx + 1) % 3]); + TriangleIndices &indices = edge_to_triangle[edge]; + if (indices.mNumTriangles < 2) + { + // Store index of triangle that connects to this edge + indices.mTriangleIndices[indices.mNumTriangles] = triangle_idx; + indices.mNumTriangles++; + } + else + { + // 3 or more triangles share an edge, mark this edge as active + uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT); + JPH_ASSERT((triangle.mMaterialIndex & mask) == 0); + triangle.mMaterialIndex |= mask; + } + } + } + + // Walk over all edges and determine which ones are active + for (const EdgeToTriangle::value_type &edge : edge_to_triangle) + { + uint num_active = 0; + if (edge.second.mNumTriangles == 1) + { + // Edge is not shared, it is an active edge + num_active = 1; + } + else if (edge.second.mNumTriangles == 2) + { + // Simple shared edge, determine if edge is active based on the two adjacent triangles + const IndexedTriangle &triangle1 = ioIndices[edge.second.mTriangleIndices[0]]; + const IndexedTriangle &triangle2 = ioIndices[edge.second.mTriangleIndices[1]]; + + // Find which edge this is for both triangles + uint edge_idx1 = edge.first.GetIndexInTriangle(triangle1); + uint edge_idx2 = edge.first.GetIndexInTriangle(triangle2); + + // Construct a plane for triangle 1 (e1 = edge vertex 1, e2 = edge vertex 2, op = opposing vertex) + Vec3 triangle1_e1 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[edge_idx1]]); + Vec3 triangle1_e2 = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 1) % 3]]); + Vec3 triangle1_op = Vec3(inSettings.mTriangleVertices[triangle1.mIdx[(edge_idx1 + 2) % 3]]); + Plane triangle1_plane = Plane::sFromPointsCCW(triangle1_e1, triangle1_e2, triangle1_op); + + // Construct a plane for triangle 2 + Vec3 triangle2_e1 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[edge_idx2]]); + Vec3 triangle2_e2 = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 1) % 3]]); + Vec3 triangle2_op = Vec3(inSettings.mTriangleVertices[triangle2.mIdx[(edge_idx2 + 2) % 3]]); + Plane triangle2_plane = Plane::sFromPointsCCW(triangle2_e1, triangle2_e2, triangle2_op); + + // Determine if the edge is active + num_active = ActiveEdges::IsEdgeActive(triangle1_plane.GetNormal(), triangle2_plane.GetNormal(), triangle1_e2 - triangle1_e1, inSettings.mActiveEdgeCosThresholdAngle)? 2 : 0; + } + else + { + // More edges incoming, we've already marked all edges beyond the 2nd as active + num_active = 2; + } + + // Mark edges of all original triangles active + for (uint i = 0; i < num_active; ++i) + { + uint triangle_idx = edge.second.mTriangleIndices[i]; + IndexedTriangle &triangle = ioIndices[triangle_idx]; + uint edge_idx = edge.first.GetIndexInTriangle(triangle); + uint32 mask = 1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT); + JPH_ASSERT((triangle.mMaterialIndex & mask) == 0); + triangle.mMaterialIndex |= mask; + } + } +} + +MassProperties MeshShape::GetMassProperties() const +{ + // Object should always be static, return default mass properties + return MassProperties(); +} + +void MeshShape::DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const +{ + // Get block + SubShapeID triangle_idx_subshape_id; + uint32 block_id = inSubShapeID.PopID(NodeCodec::DecodingContext::sTriangleBlockIDBits(mTree), triangle_idx_subshape_id); + outTriangleBlock = NodeCodec::DecodingContext::sGetTriangleBlockStart(&mTree[0], block_id); + + // Fetch the triangle index + SubShapeID remainder; + outTriangleIndex = triangle_idx_subshape_id.PopID(NumTriangleBits, remainder); + JPH_ASSERT(remainder.IsEmpty(), "Invalid subshape ID"); +} + +uint MeshShape::GetMaterialIndex(const SubShapeID &inSubShapeID) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Fetch the flags + uint8 flags = TriangleCodec::DecodingContext::sGetFlags(block_start, triangle_idx); + return flags & FLAGS_MATERIAL_MASK; +} + +const PhysicsMaterial *MeshShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + // Return the default material if there are no materials on this shape + if (mMaterials.empty()) + return PhysicsMaterial::sDefault; + + return mMaterials[GetMaterialIndex(inSubShapeID)]; +} + +Vec3 MeshShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + Vec3 v1, v2, v3; + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + triangle_ctx.GetTriangle(block_start, triangle_idx, v1, v2, v3); + + // Calculate normal + return (v3 - v2).Cross(v1 - v2).Normalized(); +} + +void MeshShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + // Decode ID + const void *block_start; + uint32 triangle_idx; + DecodeSubShapeID(inSubShapeID, block_start, triangle_idx); + + // Decode triangle + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + outVertices.resize(3); + triangle_ctx.GetTriangle(block_start, triangle_idx, outVertices[0], outVertices[1], outVertices[2]); + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + swap(outVertices[1], outVertices[2]); + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Transform to world space + for (Vec3 &v : outVertices) + v = transform * v; +} + +AABox MeshShape::GetLocalBounds() const +{ + const NodeCodec::Header *header = sGetNodeHeader(mTree); + return AABox(Vec3::sLoadFloat3Unsafe(header->mRootBoundsMin), Vec3::sLoadFloat3Unsafe(header->mRootBoundsMax)); +} + +uint MeshShape::GetSubShapeIDBitsRecursive() const +{ + return NodeCodec::DecodingContext::sTriangleBlockIDBits(mTree) + NumTriangleBits; +} + +template +JPH_INLINE void MeshShape::WalkTree(Visitor &ioVisitor) const +{ + const NodeCodec::Header *header = sGetNodeHeader(mTree); + NodeCodec::DecodingContext node_ctx(header); + + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + const uint8 *buffer_start = &mTree[0]; + node_ctx.WalkTree(buffer_start, triangle_ctx, ioVisitor); +} + +template +JPH_INLINE void MeshShape::WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const +{ + struct ChainedVisitor + { + JPH_INLINE ChainedVisitor(Visitor &ioVisitor, const SubShapeIDCreator &inSubShapeIDCreator2, uint inTriangleBlockIDBits) : + mVisitor(ioVisitor), + mSubShapeIDCreator2(inSubShapeIDCreator2), + mTriangleBlockIDBits(inTriangleBlockIDBits) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mVisitor.ShouldAbort(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mVisitor.ShouldVisitNode(inStackTop); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + return mVisitor.VisitNodes(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, ioProperties, inStackTop); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Create ID for triangle block + SubShapeIDCreator block_sub_shape_id = mSubShapeIDCreator2.PushID(inTriangleBlockID, mTriangleBlockIDBits); + + // Decode vertices and flags + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + uint8 flags[MaxTrianglesPerLeaf]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags); + + int triangle_idx = 0; + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, triangle_idx++) + { + // Determine active edges + uint8 active_edges = (flags[triangle_idx] >> FLAGS_ACTIVE_EGDE_SHIFT) & FLAGS_ACTIVE_EDGE_MASK; + + // Create ID for triangle + SubShapeIDCreator triangle_sub_shape_id = block_sub_shape_id.PushID(triangle_idx, NumTriangleBits); + + mVisitor.VisitTriangle(v[0], v[1], v[2], active_edges, triangle_sub_shape_id.GetID()); + + // Check if we should early out now + if (mVisitor.ShouldAbort()) + break; + } + } + + Visitor & mVisitor; + SubShapeIDCreator mSubShapeIDCreator2; + uint mTriangleBlockIDBits; + }; + + ChainedVisitor visitor(ioVisitor, inSubShapeIDCreator2, NodeCodec::DecodingContext::sTriangleBlockIDBits(mTree)); + WalkTree(visitor); +} + +#ifdef JPH_DEBUG_RENDERER +void MeshShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + // Reset the batch if we switch coloring mode + if (mCachedTrianglesColoredPerGroup != sDrawTriangleGroups || mCachedUseMaterialColors != inUseMaterialColors) + { + mGeometry = nullptr; + mCachedTrianglesColoredPerGroup = sDrawTriangleGroups; + mCachedUseMaterialColors = inUseMaterialColors; + } + + if (mGeometry == nullptr) + { + struct Visitor + { + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices); + + if (mDrawTriangleGroups || !mUseMaterialColors || mMaterials.empty()) + { + // Single color for mesh + Color color = mDrawTriangleGroups? Color::sGetDistinctColor(mColorIdx++) : (mUseMaterialColors? PhysicsMaterial::sDefault->GetDebugColor() : Color::sWhite); + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3) + mTriangles.push_back({ v[0], v[1], v[2], color }); + } + else + { + // Per triangle color + uint8 flags[MaxTrianglesPerLeaf]; + TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags); + + const uint8 *f = flags; + for (const Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, f++) + mTriangles.push_back({ v[0], v[1], v[2], mMaterials[*f & FLAGS_MATERIAL_MASK]->GetDebugColor() }); + } + } + + Array & mTriangles; + const PhysicsMaterialList & mMaterials; + bool mUseMaterialColors; + bool mDrawTriangleGroups; + int mColorIdx = 0; + }; + + Array triangles; + Visitor visitor { triangles, mMaterials, mCachedUseMaterialColors, mCachedTrianglesColoredPerGroup }; + WalkTree(visitor); + mGeometry = new DebugRenderer::Geometry(inRenderer->CreateTriangleBatch(triangles), GetLocalBounds()); + } + + // Test if the shape is scaled inside out + DebugRenderer::ECullMode cull_mode = ScaleHelpers::IsInsideOut(inScale)? DebugRenderer::ECullMode::CullFrontFace : DebugRenderer::ECullMode::CullBackFace; + + // Determine the draw mode + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + // Draw the geometry + inRenderer->DrawGeometry(inCenterOfMassTransform * Mat44::sScale(inScale), inColor, mGeometry, cull_mode, DebugRenderer::ECastShadow::On, draw_mode); + + if (sDrawTriangleOutlines) + { + struct Visitor + { + JPH_INLINE Visitor(DebugRenderer *inRenderer, RMat44Arg inTransform) : + mRenderer(inRenderer), + mTransform(inTransform) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Decode vertices and flags + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + uint8 flags[MaxTrianglesPerLeaf]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices, flags); + + // Loop through triangles + const uint8 *f = flags; + for (Vec3 *v = vertices, *v_end = vertices + inNumTriangles * 3; v < v_end; v += 3, ++f) + { + // Loop through edges + for (uint edge_idx = 0; edge_idx < 3; ++edge_idx) + { + RVec3 v1 = mTransform * v[edge_idx]; + RVec3 v2 = mTransform * v[(edge_idx + 1) % 3]; + + // Draw active edge as a green arrow, other edges as grey + if (*f & (1 << (edge_idx + FLAGS_ACTIVE_EGDE_SHIFT))) + mRenderer->DrawArrow(v1, v2, Color::sGreen, 0.01f); + else + mRenderer->DrawLine(v1, v2, Color::sGrey); + } + } + } + + DebugRenderer * mRenderer; + RMat44 mTransform; + }; + + Visitor visitor { inRenderer, inCenterOfMassTransform.PreScaled(inScale) }; + WalkTree(visitor); + } +} +#endif // JPH_DEBUG_RENDERER + +bool MeshShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor + { + JPH_INLINE explicit Visitor(RayCastResult &ioHit) : + mHit(ioHit) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mHit.mFraction <= 0.0f; + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, uint32 inTriangleBlockID) + { + // Test against triangles + uint32 triangle_idx; + float fraction = ioContext.TestRay(mRayOrigin, mRayDirection, inTriangles, inNumTriangles, mHit.mFraction, triangle_idx); + if (fraction < mHit.mFraction) + { + mHit.mFraction = fraction; + mHit.mSubShapeID2 = mSubShapeIDCreator.PushID(inTriangleBlockID, mTriangleBlockIDBits).PushID(triangle_idx, NumTriangleBits).GetID(); + mReturnValue = true; + } + } + + RayCastResult & mHit; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + uint mTriangleBlockIDBits; + SubShapeIDCreator mSubShapeIDCreator; + bool mReturnValue = false; + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(ioHit); + visitor.mRayOrigin = inRay.mOrigin; + visitor.mRayDirection = inRay.mDirection; + visitor.mRayInvDirection.Set(inRay.mDirection); + visitor.mTriangleBlockIDBits = NodeCodec::DecodingContext::sTriangleBlockIDBits(mTree); + visitor.mSubShapeIDCreator = inSubShapeIDCreator; + WalkTree(visitor); + + return visitor.mReturnValue; +} + +void MeshShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor + { + JPH_INLINE explicit Visitor(CastRayCollector &ioCollector) : + mCollector(ioCollector) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = RayAABox4(mRayOrigin, mRayInvDirection, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + // Back facing check + if (mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (inV2 - inV0).Cross(inV1 - inV0).Dot(mRayDirection) < 0) + return; + + // Check the triangle + float fraction = RayTriangle(mRayOrigin, mRayDirection, inV0, inV1, inV2); + if (fraction < mCollector.GetEarlyOutFraction()) + { + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(mCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeID2; + mCollector.AddHit(hit); + } + } + + CastRayCollector & mCollector; + Vec3 mRayOrigin; + Vec3 mRayDirection; + RayInvDirection mRayInvDirection; + EBackFaceMode mBackFaceMode; + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(ioCollector); + visitor.mBackFaceMode = inRayCastSettings.mBackFaceMode; + visitor.mRayOrigin = inRay.mOrigin; + visitor.mRayDirection = inRay.mDirection; + visitor.mRayInvDirection.Set(inRay.mDirection); + WalkTreePerTriangle(inSubShapeIDCreator, visitor); +} + +void MeshShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void MeshShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideSoftBodyVerticesVsTriangles + { + using CollideSoftBodyVerticesVsTriangles::CollideSoftBodyVerticesVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return mDistanceStack[inStackTop] < mClosestDistanceSq; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Get distance to vertex + Vec4 dist_sq = AABox4DistanceSqToPoint(mLocalPosition, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(dist_sq, mClosestDistanceSq, ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, [[maybe_unused]] uint8 inActiveEdges, [[maybe_unused]] SubShapeID inSubShapeID2) + { + ProcessTriangle(inV0, inV1, inV2); + } + + float mDistanceStack[NodeCodec::StackSize]; + }; + + Visitor visitor(inCenterOfMassTransform, inScale); + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + visitor.StartVertex(*v); + WalkTreePerTriangle(SubShapeIDCreator(), visitor); + visitor.FinishVertex(*v, inCollidingShapeIndex); + } +} + +void MeshShape::sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastConvexVsTriangles + { + using CastConvexVsTriangles::CastConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the casted shape's box extents + AABox4EnlargeWithExtent(mBoxExtent, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mBoxCenter, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + + RayInvDirection mInvDirection; + Vec3 mBoxCenter; + Vec3 mBoxExtent; + float mDistanceStack[NodeCodec::StackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh); + const MeshShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mInvDirection.Set(inShapeCast.mDirection); + visitor.mBoxCenter = inShapeCast.mShapeWorldBounds.GetCenter(); + visitor.mBoxExtent = inShapeCast.mShapeWorldBounds.GetExtent(); + shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastSphereVsTriangles + { + using CastSphereVsTriangles::CastSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Enlarge them by the radius of the sphere + AABox4EnlargeWithExtent(Vec3::sReplicate(mRadius), bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test bounds of 4 children + Vec4 distance = RayAABox4(mStart, mInvDirection, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Cast(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + + RayInvDirection mInvDirection; + float mDistanceStack[NodeCodec::StackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Mesh); + const MeshShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + visitor.mInvDirection.Set(inShapeCast.mDirection); + shape->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +struct MeshShape::MSGetTrianglesContext +{ + JPH_INLINE MSGetTrianglesContext(const MeshShape *inShape, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) : + mDecodeCtx(sGetNodeHeader(inShape->mTree)), + mShape(inShape), + mLocalBox(Mat44::sInverseRotationTranslation(inRotation, inPositionCOM), inBox), + mMeshScale(inScale), + mLocalToWorld(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale)), + mIsInsideOut(ScaleHelpers::IsInsideOut(inScale)) + { + } + + JPH_INLINE bool ShouldAbort() const + { + return mShouldAbort; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mMeshScale, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mLocalBox, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangles(const TriangleCodec::DecodingContext &ioContext, const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + // When the buffer is full and we cannot process the triangles, abort the tree walk. The next time GetTrianglesNext is called we will continue here. + if (mNumTrianglesFound + inNumTriangles > mMaxTrianglesRequested) + { + mShouldAbort = true; + return; + } + + // Decode vertices + JPH_ASSERT(inNumTriangles <= MaxTrianglesPerLeaf); + Vec3 vertices[MaxTrianglesPerLeaf * 3]; + ioContext.Unpack(inTriangles, inNumTriangles, vertices); + + // Store vertices as Float3 + if (mIsInsideOut) + { + // Scaled inside out, flip the triangles + for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; v += 3) + { + (mLocalToWorld * v[0]).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * v[2]).StoreFloat3(mTriangleVertices++); + (mLocalToWorld * v[1]).StoreFloat3(mTriangleVertices++); + } + } + else + { + // Normal scale + for (const Vec3 *v = vertices, *v_end = v + 3 * inNumTriangles; v < v_end; ++v) + (mLocalToWorld * *v).StoreFloat3(mTriangleVertices++); + } + + if (mMaterials != nullptr) + { + if (mShape->mMaterials.empty()) + { + // No materials, output default + const PhysicsMaterial *default_material = PhysicsMaterial::sDefault; + for (int m = 0; m < inNumTriangles; ++m) + *mMaterials++ = default_material; + } + else + { + // Decode triangle flags + uint8 flags[MaxTrianglesPerLeaf]; + TriangleCodec::DecodingContext::sGetFlags(inTriangles, inNumTriangles, flags); + + // Store materials + for (const uint8 *f = flags, *f_end = f + inNumTriangles; f < f_end; ++f) + *mMaterials++ = mShape->mMaterials[*f & FLAGS_MATERIAL_MASK].GetPtr(); + } + } + + // Accumulate triangles found + mNumTrianglesFound += inNumTriangles; + } + + NodeCodec::DecodingContext mDecodeCtx; + const MeshShape * mShape; + OrientedBox mLocalBox; + Vec3 mMeshScale; + Mat44 mLocalToWorld; + int mMaxTrianglesRequested; + Float3 * mTriangleVertices; + int mNumTrianglesFound; + const PhysicsMaterial ** mMaterials; + bool mShouldAbort; + bool mIsInsideOut; +}; + +void MeshShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(MSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(MSGetTrianglesContext))); + + new (&ioContext) MSGetTrianglesContext(this, inBox, inPositionCOM, inRotation, inScale); +} + +int MeshShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= MaxTrianglesPerLeaf, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + // Check if we're done + MSGetTrianglesContext &context = (MSGetTrianglesContext &)ioContext; + if (context.mDecodeCtx.IsDoneWalking()) + return 0; + + // Store parameters on context + context.mMaxTrianglesRequested = inMaxTrianglesRequested; + context.mTriangleVertices = outTriangleVertices; + context.mMaterials = outMaterials; + context.mShouldAbort = false; // Reset the abort flag + context.mNumTrianglesFound = 0; + + // Continue (or start) walking the tree + const TriangleCodec::DecodingContext triangle_ctx(sGetTriangleHeader(mTree)); + const uint8 *buffer_start = &mTree[0]; + context.mDecodeCtx.WalkTree(buffer_start, triangle_ctx, context); + return context.mNumTrianglesFound; +} + +void MeshShape::sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh); + const ConvexShape *shape1 = static_cast(inShape1); + const MeshShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideConvexVsTriangles + { + using CollideConvexVsTriangles::CollideConvexVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsBox(mBoundsOf1InSpaceOf2, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + // Get the shapes + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + JPH_ASSERT(inShape2->GetType() == EShapeType::Mesh); + const SphereShape *shape1 = static_cast(inShape1); + const MeshShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideSphereVsTriangles + { + using CollideSphereVsTriangles::CollideSphereVsTriangles; + + JPH_INLINE bool ShouldAbort() const + { + return mCollector.ShouldEarlyOut(); + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Scale the bounding boxes of this node + Vec4 bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z; + AABox4Scale(mScale2, inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + + // Test which nodes collide + UVec4 collides = AABox4VsSphere(mSphereCenterIn2, mRadiusPlusMaxSeparationSq, bounds_min_x, bounds_min_y, bounds_min_z, bounds_max_x, bounds_max_y, bounds_max_z); + return CountAndSortTrues(collides, ioProperties); + } + + JPH_INLINE void VisitTriangle(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, SubShapeID inSubShapeID2) + { + Collide(inV0, inV1, inV2, inActiveEdges, inSubShapeID2); + } + }; + + Visitor visitor(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + shape2->WalkTreePerTriangle(inSubShapeIDCreator2, visitor); +} + +void MeshShape::SaveBinaryState(StreamOut &inStream) const +{ + Shape::SaveBinaryState(inStream); + + inStream.Write(static_cast(mTree)); // Make sure we use the Array<> overload +} + +void MeshShape::RestoreBinaryState(StreamIn &inStream) +{ + Shape::RestoreBinaryState(inStream); + + inStream.Read(static_cast(mTree)); // Make sure we use the Array<> overload +} + +void MeshShape::SaveMaterialState(PhysicsMaterialList &outMaterials) const +{ + outMaterials = mMaterials; +} + +void MeshShape::RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) +{ + mMaterials.assign(inMaterials, inMaterials + inNumMaterials); +} + +Shape::Stats MeshShape::GetStats() const +{ + // Walk the tree to count the triangles + struct Visitor + { + JPH_INLINE bool ShouldAbort() const + { + return false; + } + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Visit all valid children + UVec4 valid = UVec4::sOr(UVec4::sOr(Vec4::sLess(inBoundsMinX, inBoundsMaxX), Vec4::sLess(inBoundsMinY, inBoundsMaxY)), Vec4::sLess(inBoundsMinZ, inBoundsMaxZ)); + return CountAndSortTrues(valid, ioProperties); + } + + JPH_INLINE void VisitTriangles([[maybe_unused]] const TriangleCodec::DecodingContext &ioContext, [[maybe_unused]] const void *inTriangles, int inNumTriangles, [[maybe_unused]] uint32 inTriangleBlockID) + { + mNumTriangles += inNumTriangles; + } + + uint mNumTriangles = 0; + }; + + Visitor visitor; + WalkTree(visitor); + + return Stats(sizeof(*this) + mMaterials.size() * sizeof(Ref) + mTree.size() * sizeof(uint8), visitor.mNumTriangles); +} + +void MeshShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Mesh); + f.mConstruct = []() -> Shape * { return new MeshShape; }; + f.mColor = Color::sRed; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Mesh, sCollideConvexVsMesh); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Mesh, sCastConvexVsMesh); + + CollisionDispatch::sRegisterCastShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCastShape); + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Mesh, s, CollisionDispatch::sReversedCollideShape); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCollideSphereVsMesh); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Mesh, sCastSphereVsMesh); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.h new file mode 100644 index 000000000..218d1e100 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/MeshShape.h @@ -0,0 +1,208 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class ConvexShape; +class CollideShapeSettings; + +/// Class that constructs a MeshShape +class JPH_EXPORT MeshShapeSettings final : public ShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MeshShapeSettings) + + /// Default constructor for deserialization + MeshShapeSettings() = default; + + /// Create a mesh shape. + MeshShapeSettings(const TriangleList &inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList()); + MeshShapeSettings(VertexList inVertices, IndexedTriangleList inTriangles, PhysicsMaterialList inMaterials = PhysicsMaterialList()); + + /// Sanitize the mesh data. Remove duplicate and degenerate triangles. This is called automatically when constructing the MeshShapeSettings with a list of (indexed-) triangles. + void Sanitize(); + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Vertices belonging to mIndexedTriangles + VertexList mTriangleVertices; + + /// Original list of indexed triangles (triangles will be reordered internally in the mesh shape). + /// Triangles must be provided in counter clockwise order. + /// Degenerate triangles will automatically be removed during mesh creation but no other mesh simplifications are performed, use an external library if this is desired. + /// For simulation, the triangles are considered to be single sided. + /// For ray casts you can choose to make triangles double sided by setting RayCastSettings::mBackFaceMode to EBackFaceMode::CollideWithBackFaces. + /// For collide shape tests you can use CollideShapeSettings::mBackFaceMode and for shape casts you can use ShapeCastSettings::mBackFaceModeTriangles. + IndexedTriangleList mIndexedTriangles; + + /// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex + PhysicsMaterialList mMaterials; + + /// Maximum number of triangles in each leaf of the axis aligned box tree. This is a balance between memory and performance. Can be in the range [1, MeshShape::MaxTrianglesPerLeaf]. + /// Sensible values are between 4 (for better performance) and 8 (for less memory usage). + uint mMaxTrianglesPerLeaf = 8; + + /// Cosine of the threshold angle (if the angle between the two triangles is bigger than this, the edge is active, note that a concave edge is always inactive). + /// Setting this value too small can cause ghost collisions with edges, setting it too big can cause depenetration artifacts (objects not depenetrating quickly). + /// Valid ranges are between cos(0 degrees) and cos(90 degrees). The default value is cos(5 degrees). + float mActiveEdgeCosThresholdAngle = 0.996195f; // cos(5 degrees) +}; + +/// A mesh shape, consisting of triangles. Mesh shapes are mostly used for static geometry. +/// They can be used by dynamic or kinematic objects but only if they don't collide with other mesh or heightfield shapes as those collisions are currently not supported. +/// Note that if you make a mesh shape a dynamic or kinematic object, you need to provide a mass yourself as mesh shapes don't need to form a closed hull so don't have a well defined volume from which the mass can be calculated. +class JPH_EXPORT MeshShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MeshShape() : Shape(EShapeType::Mesh, EShapeSubType::Mesh) { } + MeshShape(const MeshShapeSettings &inSettings, ShapeResult &outResult); + + // See Shape::MustBeStatic + virtual bool MustBeStatic() const override { return true; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetSubShapeIDBitsRecursive + virtual uint GetSubShapeIDBitsRecursive() const override; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return 0.0f; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetMaterial + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + + /// Get the list of all materials + const PhysicsMaterialList & GetMaterialList() const { return mMaterials; } + + /// Determine which material index a particular sub shape uses (note that if there are no materials this function will return 0 so check the array size) + /// Note: This could for example be used to create a decorator shape around a mesh shape that overrides the GetMaterial call to replace a material with another material. + uint GetMaterialIndex(const SubShapeID &inSubShapeID) const; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + /// See: Shape::CollidePoint + /// Note that for CollidePoint to work for a mesh shape, the mesh needs to be closed (a manifold) or multiple non-intersecting manifolds. Triangles may be facing the interior of the manifold. + /// Insideness is tested by counting the amount of triangles encountered when casting an infinite ray from inPoint. If the number of hits is odd we're inside, if it's even we're outside. + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override { JPH_ASSERT(false, "Not supported"); } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void SaveMaterialState(PhysicsMaterialList &outMaterials) const override; + virtual void RestoreMaterialState(const PhysicsMaterialRefC *inMaterials, uint inNumMaterials) override; + + // See Shape::GetStats + virtual Stats GetStats() const override; + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + +#ifdef JPH_DEBUG_RENDERER + // Settings + static bool sDrawTriangleGroups; + static bool sDrawTriangleOutlines; +#endif // JPH_DEBUG_RENDERER + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + struct MSGetTrianglesContext; ///< Context class for GetTrianglesStart/Next + + static constexpr int NumTriangleBits = 3; ///< How many bits to reserve to encode the triangle index + static constexpr int MaxTrianglesPerLeaf = 1 << NumTriangleBits; ///< Number of triangles that are stored max per leaf aabb node + + /// Find and flag active edges + static void sFindActiveEdges(const MeshShapeSettings &inSettings, IndexedTriangleList &ioIndices); + + /// Visit the entire tree using a visitor pattern + template + void WalkTree(Visitor &ioVisitor) const; + + /// Same as above but with a callback per triangle instead of per block of triangles + template + void WalkTreePerTriangle(const SubShapeIDCreator &inSubShapeIDCreator2, Visitor &ioVisitor) const; + + /// Decode a sub shape ID + inline void DecodeSubShapeID(const SubShapeID &inSubShapeID, const void *&outTriangleBlock, uint32 &outTriangleIndex) const; + + // Helper functions called by CollisionDispatch + static void sCollideConvexVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsMesh(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsMesh(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Materials assigned to the triangles. Each triangle specifies which material it uses through its mMaterialIndex + PhysicsMaterialList mMaterials; + + ByteBuffer mTree; ///< Resulting packed data structure + + /// 8 bit flags stored per triangle + enum ETriangleFlags + { + /// Material index + FLAGS_MATERIAL_BITS = 5, + FLAGS_MATERIAL_MASK = (1 << FLAGS_MATERIAL_BITS) - 1, + + /// Active edge bits + FLAGS_ACTIVE_EGDE_SHIFT = FLAGS_MATERIAL_BITS, + FLAGS_ACTIVE_EDGE_BITS = 3, + FLAGS_ACTIVE_EDGE_MASK = (1 << FLAGS_ACTIVE_EDGE_BITS) - 1 + }; + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; ///< Debug rendering data + mutable bool mCachedTrianglesColoredPerGroup = false; ///< This is used to regenerate the triangle batch if the drawing settings change + mutable bool mCachedUseMaterialColors = false; ///< This is used to regenerate the triangle batch if the drawing settings change +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp new file mode 100644 index 000000000..755773de7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.cpp @@ -0,0 +1,574 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MutableCompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(MutableCompoundShapeSettings, CompoundShapeSettings) +} + +ShapeSettings::ShapeResult MutableCompoundShapeSettings::Create() const +{ + // Build a mutable compound shape + if (mCachedResult.IsEmpty()) + Ref shape = new MutableCompoundShape(*this, mCachedResult); + + return mCachedResult; +} + +MutableCompoundShape::MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult) : + CompoundShape(EShapeSubType::MutableCompound, inSettings, outResult) +{ + mSubShapes.reserve(inSettings.mSubShapes.size()); + for (const CompoundShapeSettings::SubShapeSettings &shape : inSettings.mSubShapes) + { + // Start constructing the runtime sub shape + SubShape out_shape; + if (!out_shape.FromSettings(shape, outResult)) + return; + + mSubShapes.push_back(out_shape); + } + + AdjustCenterOfMass(); + + CalculateSubShapeBounds(0, (uint)mSubShapes.size()); + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +Ref MutableCompoundShape::Clone() const +{ + Ref clone = new MutableCompoundShape(); + clone->SetUserData(GetUserData()); + + clone->mCenterOfMass = mCenterOfMass; + clone->mLocalBounds = mLocalBounds; + clone->mSubShapes = mSubShapes; + clone->mInnerRadius = mInnerRadius; + clone->mSubShapeBounds = mSubShapeBounds; + + return clone; +} + +void MutableCompoundShape::AdjustCenterOfMass() +{ + // First calculate the delta of the center of mass + float mass = 0.0f; + Vec3 center_of_mass = Vec3::sZero(); + for (const CompoundShape::SubShape &sub_shape : mSubShapes) + { + MassProperties child = sub_shape.mShape->GetMassProperties(); + mass += child.mMass; + center_of_mass += sub_shape.GetPositionCOM() * child.mMass; + } + if (mass > 0.0f) + center_of_mass /= mass; + + // Now adjust all shapes to recenter around center of mass + for (CompoundShape::SubShape &sub_shape : mSubShapes) + sub_shape.SetPositionCOM(sub_shape.GetPositionCOM() - center_of_mass); + + // And adjust the center of mass for this shape in the opposite direction + mCenterOfMass += center_of_mass; +} + +void MutableCompoundShape::CalculateLocalBounds() +{ + uint num_blocks = GetNumBlocks(); + if (num_blocks > 0) + { + // Initialize min/max for first block + const Bounds *bounds = mSubShapeBounds.data(); + Vec4 min_x = bounds->mMinX; + Vec4 min_y = bounds->mMinY; + Vec4 min_z = bounds->mMinZ; + Vec4 max_x = bounds->mMaxX; + Vec4 max_y = bounds->mMaxY; + Vec4 max_z = bounds->mMaxZ; + + // Accumulate other blocks + const Bounds *bounds_end = bounds + num_blocks; + for (++bounds; bounds < bounds_end; ++bounds) + { + min_x = Vec4::sMin(min_x, bounds->mMinX); + min_y = Vec4::sMin(min_y, bounds->mMinY); + min_z = Vec4::sMin(min_z, bounds->mMinZ); + max_x = Vec4::sMax(max_x, bounds->mMaxX); + max_y = Vec4::sMax(max_y, bounds->mMaxY); + max_z = Vec4::sMax(max_z, bounds->mMaxZ); + } + + // Calculate resulting bounding box + mLocalBounds.mMin.SetX(min_x.ReduceMin()); + mLocalBounds.mMin.SetY(min_y.ReduceMin()); + mLocalBounds.mMin.SetZ(min_z.ReduceMin()); + mLocalBounds.mMax.SetX(max_x.ReduceMax()); + mLocalBounds.mMax.SetY(max_y.ReduceMax()); + mLocalBounds.mMax.SetZ(max_z.ReduceMax()); + } + else + { + // There are no subshapes, set the bounding box to invalid + mLocalBounds.SetEmpty(); + } + + // Cache the inner radius as it can take a while to recursively iterate over all sub shapes + CalculateInnerRadius(); +} + +void MutableCompoundShape::EnsureSubShapeBoundsCapacity() +{ + // Check if we have enough space + uint new_capacity = ((uint)mSubShapes.size() + 3) >> 2; + if (mSubShapeBounds.size() < new_capacity) + mSubShapeBounds.resize(new_capacity); +} + +void MutableCompoundShape::CalculateSubShapeBounds(uint inStartIdx, uint inNumber) +{ + // Ensure that we have allocated the required space for mSubShapeBounds + EnsureSubShapeBoundsCapacity(); + + // Loop over blocks of 4 sub shapes + for (uint sub_shape_idx_start = inStartIdx & ~uint(3), sub_shape_idx_end = inStartIdx + inNumber; sub_shape_idx_start < sub_shape_idx_end; sub_shape_idx_start += 4) + { + Mat44 bounds_min; + Mat44 bounds_max; + + AABox sub_shape_bounds; + for (uint col = 0; col < 4; ++col) + { + uint sub_shape_idx = sub_shape_idx_start + col; + if (sub_shape_idx < mSubShapes.size()) // else reuse sub_shape_bounds from previous iteration + { + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + + // Tranform the shape's bounds into our local space + Mat44 transform = Mat44::sRotationTranslation(sub_shape.GetRotation(), sub_shape.GetPositionCOM()); + + // Get the bounding box + sub_shape_bounds = sub_shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f)); + } + + // Put the bounds as columns in a matrix + bounds_min.SetColumn3(col, sub_shape_bounds.mMin); + bounds_max.SetColumn3(col, sub_shape_bounds.mMax); + } + + // Transpose to go to strucucture of arrays format + Mat44 bounds_min_t = bounds_min.Transposed(); + Mat44 bounds_max_t = bounds_max.Transposed(); + + // Store in our bounds array + Bounds &bounds = mSubShapeBounds[sub_shape_idx_start >> 2]; + bounds.mMinX = bounds_min_t.GetColumn4(0); + bounds.mMinY = bounds_min_t.GetColumn4(1); + bounds.mMinZ = bounds_min_t.GetColumn4(2); + bounds.mMaxX = bounds_max_t.GetColumn4(0); + bounds.mMaxY = bounds_max_t.GetColumn4(1); + bounds.mMaxZ = bounds_max_t.GetColumn4(2); + } + + CalculateLocalBounds(); +} + +uint MutableCompoundShape::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData) +{ + SubShape sub_shape; + sub_shape.mShape = inShape; + sub_shape.mUserData = inUserData; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + mSubShapes.push_back(sub_shape); + uint shape_idx = (uint)mSubShapes.size() - 1; + + CalculateSubShapeBounds(shape_idx, 1); + + return shape_idx; +} + +void MutableCompoundShape::RemoveShape(uint inIndex) +{ + mSubShapes.erase(mSubShapes.begin() + inIndex); + + uint num_bounds = (uint)mSubShapes.size() - inIndex; + if (num_bounds > 0) + CalculateSubShapeBounds(inIndex, num_bounds); + else + CalculateLocalBounds(); +} + +void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation) +{ + SubShape &sub_shape = mSubShapes[inIndex]; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + + CalculateSubShapeBounds(inIndex, 1); +} + +void MutableCompoundShape::ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) +{ + SubShape &sub_shape = mSubShapes[inIndex]; + sub_shape.mShape = inShape; + sub_shape.SetTransform(inPosition, inRotation, mCenterOfMass); + + CalculateSubShapeBounds(inIndex, 1); +} + +void MutableCompoundShape::ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride, uint inRotationStride) +{ + JPH_ASSERT(inStartIndex + inNumber <= mSubShapes.size()); + + const Vec3 *pos = inPositions; + const Quat *rot = inRotations; + for (SubShape *dest = &mSubShapes[inStartIndex], *dest_end = dest + inNumber; dest < dest_end; ++dest) + { + // Update transform + dest->SetTransform(*pos, *rot, mCenterOfMass); + + // Advance pointer in position / rotation buffer + pos = reinterpret_cast(reinterpret_cast(pos) + inPositionStride); + rot = reinterpret_cast(reinterpret_cast(rot) + inRotationStride); + } + + CalculateSubShapeBounds(inStartIndex, inNumber); +} + +template +inline void MutableCompoundShape::WalkSubShapes(Visitor &ioVisitor) const +{ + // Loop over all blocks of 4 bounding boxes + for (uint block = 0, num_blocks = GetNumBlocks(); block < num_blocks; ++block) + { + // Test the bounding boxes + const Bounds &bounds = mSubShapeBounds[block]; + typename Visitor::Result result = ioVisitor.TestBlock(bounds.mMinX, bounds.mMinY, bounds.mMinZ, bounds.mMaxX, bounds.mMaxY, bounds.mMaxZ); + + // Check if any of the bounding boxes collided + if (ioVisitor.ShouldVisitBlock(result)) + { + // Go through the individual boxes + uint sub_shape_start_idx = block << 2; + for (uint col = 0, max_col = min(4, (uint)mSubShapes.size() - sub_shape_start_idx); col < max_col; ++col) // Don't read beyond the end of the subshapes array + if (ioVisitor.ShouldVisitSubShape(result, col)) // Because the early out fraction can change, we need to retest every shape + { + // Test sub shape + uint sub_shape_idx = sub_shape_start_idx + col; + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + ioVisitor.VisitShape(sub_shape, sub_shape_idx); + + // If no better collision is available abort + if (ioVisitor.ShouldAbort()) + break; + } + } + } +} + +bool MutableCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitor + { + using CastRayVisitor::CastRayVisitor; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mHit.mFraction)); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mHit.mFraction; + } + }; + + Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit); + WalkSubShapes(visitor); + return visitor.mReturnValue; +} + +void MutableCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CastRayVisitorCollector + { + using CastRayVisitorCollector::CastRayVisitorCollector; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetEarlyOutFraction())); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mCollector.GetEarlyOutFraction(); + } + }; + + Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +void MutableCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollidePointVisitor + { + using CollidePointVisitor::CollidePointVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +void MutableCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastShapeVisitor + { + using CastShapeVisitor::CastShapeVisitor; + + using Result = Vec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(Vec4Arg inResult) const + { + UVec4 closer = Vec4::sLess(inResult, Vec4::sReplicate(mCollector.GetPositiveEarlyOutFraction())); + return closer.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(Vec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] < mCollector.GetPositiveEarlyOutFraction(); + } + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + shape->WalkSubShapes(visitor); +} + +void MutableCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollectTransformedShapesVisitor + { + using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkSubShapes(visitor); +} + +int MutableCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorMC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkSubShapes(visitor); + return visitor.GetNumResults(); +} + +int MutableCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorMC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkSubShapes(visitor); + return visitor.GetNumResults(); +} + +void MutableCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape1 = static_cast(inShape1); + + struct Visitor : public CollideCompoundVsShapeVisitor + { + using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape1->WalkSubShapes(visitor); +} + +void MutableCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::MutableCompound); + const MutableCompoundShape *shape2 = static_cast(inShape2); + + struct Visitor : public CollideShapeVsCompoundVisitor + { + using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape2->WalkSubShapes(visitor); +} + +void MutableCompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + CompoundShape::SaveBinaryState(inStream); + + // Write bounds + uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds); + inStream.WriteBytes(mSubShapeBounds.data(), bounds_size); +} + +void MutableCompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + CompoundShape::RestoreBinaryState(inStream); + + // Ensure that we have allocated the required space for mSubShapeBounds + EnsureSubShapeBoundsCapacity(); + + // Read bounds + uint bounds_size = (((uint)mSubShapes.size() + 3) >> 2) * sizeof(Bounds); + inStream.ReadBytes(mSubShapeBounds.data(), bounds_size); +} + +void MutableCompoundShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::MutableCompound); + f.mConstruct = []() -> Shape * { return new MutableCompoundShape; }; + f.mColor = Color::sDarkOrange; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::MutableCompound, s, sCollideCompoundVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::MutableCompound, sCollideShapeVsCompound); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::MutableCompound, sCastShapeVsCompound); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.h new file mode 100644 index 000000000..427b14898 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/MutableCompoundShape.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a MutableCompoundShape. +class JPH_EXPORT MutableCompoundShapeSettings final : public CompoundShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MutableCompoundShapeSettings) + + // See: ShapeSettings + virtual ShapeResult Create() const override; +}; + +/// A compound shape, sub shapes can be rotated and translated. +/// This shape is optimized for adding / removing and changing the rotation / translation of sub shapes but is less efficient in querying. +/// Shifts all child objects so that they're centered around the center of mass (which needs to be kept up to date by calling AdjustCenterOfMass). +/// +/// Note: If you're using MutableCompoundShape and are querying data while modifying the shape you'll have a race condition. +/// In this case it is best to create a new MutableCompoundShape using the Clone function. You replace the shape on a body using BodyInterface::SetShape. +/// If a query is still working on the old shape, it will have taken a reference and keep the old shape alive until the query finishes. +class JPH_EXPORT MutableCompoundShape final : public CompoundShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MutableCompoundShape() : CompoundShape(EShapeSubType::MutableCompound) { } + MutableCompoundShape(const MutableCompoundShapeSettings &inSettings, ShapeResult &outResult); + + /// Clone this shape. Can be used to avoid race conditions. See the documentation of this class for more information. + Ref Clone() const; + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mSubShapeBounds.size() * sizeof(Bounds), 0); } + + ///@{ + /// @name Mutating shapes. Note that this is not thread safe, so you need to ensure that any bodies that use this shape are locked at the time of modification using BodyLockWrite. After modification you need to call BodyInterface::NotifyShapeChanged to update the broadphase and collision caches. + + /// Adding a new shape. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @return The index of the newly added shape + uint AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData = 0); + + /// Remove a shape by index. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void RemoveShape(uint inIndex); + + /// Modify the position / orientation of a shape. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation); + + /// Modify the position / orientation and shape at the same time. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void ModifyShape(uint inIndex, Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape); + + /// @brief Batch set positions / orientations, this avoids duplicate work due to bounding box calculation. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + /// @param inStartIndex Index of first shape to update + /// @param inNumber Number of shapes to update + /// @param inPositions A list of positions with arbitrary stride + /// @param inRotations A list of orientations with arbitrary stride + /// @param inPositionStride The position stride (the number of bytes between the first and second element) + /// @param inRotationStride The orientation stride (the number of bytes between the first and second element) + void ModifyShapes(uint inStartIndex, uint inNumber, const Vec3 *inPositions, const Quat *inRotations, uint inPositionStride = sizeof(Vec3), uint inRotationStride = sizeof(Quat)); + + /// Recalculate the center of mass and shift all objects so they're centered around it + /// (this needs to be done of dynamic bodies and if the center of mass changes significantly due to adding / removing / repositioning sub shapes or else the simulation will look unnatural) + /// Note that after adjusting the center of mass of an object you need to call BodyInterface::NotifyShapeChanged and Constraint::NotifyShapeChanged on the relevant bodies / constraints. + /// Beware this can create a race condition if you're running collision queries in parallel. See class documentation for more information. + void AdjustCenterOfMass(); + + ///@} + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Visitor for GetIntersectingSubShapes + template + struct GetIntersectingSubShapesVisitorMC : public GetIntersectingSubShapesVisitor + { + using GetIntersectingSubShapesVisitor::GetIntersectingSubShapesVisitor; + + using Result = UVec4; + + JPH_INLINE Result TestBlock(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ) const + { + return GetIntersectingSubShapesVisitor::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + } + + JPH_INLINE bool ShouldVisitBlock(UVec4Arg inResult) const + { + return inResult.TestAnyTrue(); + } + + JPH_INLINE bool ShouldVisitSubShape(UVec4Arg inResult, uint inIndexInBlock) const + { + return inResult[inIndexInBlock] != 0; + } + }; + + /// Get the number of blocks of 4 bounding boxes + inline uint GetNumBlocks() const { return ((uint)mSubShapes.size() + 3) >> 2; } + + /// Ensure that the mSubShapeBounds has enough space to store bounding boxes equivalent to the number of shapes in mSubShapes + void EnsureSubShapeBoundsCapacity(); + + /// Update mSubShapeBounds + /// @param inStartIdx First sub shape to update + /// @param inNumber Number of shapes to update + void CalculateSubShapeBounds(uint inStartIdx, uint inNumber); + + /// Calculate mLocalBounds from mSubShapeBounds + void CalculateLocalBounds(); + + template + JPH_INLINE void WalkSubShapes(Visitor &ioVisitor) const; ///< Walk the sub shapes and call Visitor::VisitShape for each sub shape encountered + + // Helper functions called by CollisionDispatch + static void sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + struct Bounds + { + Vec4 mMinX; + Vec4 mMinY; + Vec4 mMinZ; + Vec4 mMaxX; + Vec4 mMaxY; + Vec4 mMaxZ; + }; + + Array mSubShapeBounds; ///< Bounding boxes of all sub shapes in SOA format (in blocks of 4 boxes), MinX 0..3, MinY 0..3, MinZ 0..3, MaxX 0..3, MaxY 0..3, MaxZ 0..3, MinX 4..7, MinY 4..7, ... +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp new file mode 100644 index 000000000..942eed0d9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.cpp @@ -0,0 +1,217 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(OffsetCenterOfMassShapeSettings) +{ + JPH_ADD_BASE_CLASS(OffsetCenterOfMassShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(OffsetCenterOfMassShapeSettings, mOffset) +} + +ShapeSettings::ShapeResult OffsetCenterOfMassShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new OffsetCenterOfMassShape(*this, mCachedResult); + return mCachedResult; +} + +OffsetCenterOfMassShape::OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::OffsetCenterOfMass, inSettings, outResult), + mOffset(inSettings.mOffset) +{ + if (outResult.HasError()) + return; + + outResult.Set(this); +} + +AABox OffsetCenterOfMassShape::GetLocalBounds() const +{ + AABox bounds = mInnerShape->GetLocalBounds(); + bounds.mMin -= mOffset; + bounds.mMax -= mOffset; + return bounds; +} + +AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale); +} + +TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM - inRotation * (inScale * mOffset)), inRotation, mInnerShape, BodyID()); + ts.SetShapeScale(inScale); + return ts; +} + +Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform surface position to local space and pass call on + return mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition + mOffset); +} + +void OffsetCenterOfMassShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), outVertices); +} + +void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe); +} + +void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inDrawSupportDirection); +} + +void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale); +} +#endif // JPH_DEBUG_RENDERER + +bool OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Transform the ray to local space + RayCast ray = inRay; + ray.mOrigin += mOffset; + + return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit); +} + +void OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the ray to local space + RayCast ray = inRay; + ray.mOrigin += mOffset; + + return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Pass the point on to the inner shape in local space + mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); +} + +void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM - inRotation * (inScale * mOffset), inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform.PreTranslated(-mOffset), ioCollector); +} + +void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape1 = static_cast(inShape1); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1.PreTranslated(-inScale1 * shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape2 = static_cast(inShape2); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2.PreTranslated(-inScale2 * shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // Fetch offset center of mass shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape1 = static_cast(inShapeCast.mShape); + + // Transform the shape cast and update the shape + ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart.PreTranslated(-inShapeCast.mScale * shape1->mOffset), inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::OffsetCenterOfMass); + const OffsetCenterOfMassShape *shape = static_cast(inShape); + + // Transform the shape cast + ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(inScale * shape->mOffset)); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2.PreTranslated(-inScale * shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mOffset); +} + +void OffsetCenterOfMassShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mOffset); +} + +void OffsetCenterOfMassShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::OffsetCenterOfMass); + f.mConstruct = []() -> Shape * { return new OffsetCenterOfMassShape; }; + f.mColor = Color::sCyan; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::OffsetCenterOfMass, s, sCollideOffsetCenterOfMassVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::OffsetCenterOfMass, sCollideShapeVsOffsetCenterOfMass); + CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, s, sCastOffsetCenterOfMassVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::OffsetCenterOfMass, sCastShapeVsOffsetCenterOfMass); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h new file mode 100644 index 000000000..3e828181c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/OffsetCenterOfMassShape.h @@ -0,0 +1,143 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs an OffsetCenterOfMassShape +class JPH_EXPORT OffsetCenterOfMassShapeSettings final : public DecoratedShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, OffsetCenterOfMassShapeSettings) + + /// Constructor + OffsetCenterOfMassShapeSettings() = default; + + /// Construct with shape settings, can be serialized. + OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mOffset(inOffset) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + OffsetCenterOfMassShapeSettings(Vec3Arg inOffset, const Shape *inShape): DecoratedShapeSettings(inShape), mOffset(inOffset) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mOffset; ///< Offset to be applied to the center of mass of the child shape +}; + +/// This shape will shift the center of mass of a child shape, it can e.g. be used to lower the center of mass of an unstable object like a boat to make it stable +class JPH_EXPORT OffsetCenterOfMassShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + OffsetCenterOfMassShape() : DecoratedShape(EShapeSubType::OffsetCenterOfMass) { } + OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult); + OffsetCenterOfMassShape(const Shape *inShape, Vec3Arg inOffset) : DecoratedShape(EShapeSubType::OffsetCenterOfMass, inShape), mOffset(inOffset) { } + + /// Access the offset that is applied to the center of mass + Vec3 GetOffset() const { return mOffset; } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mInnerShape->GetCenterOfMass() + mOffset; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override + { + MassProperties mp = mInnerShape->GetMassProperties(); + mp.Translate(mOffset); + return mp; + } + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return mInnerShape->GetVolume(); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override { return mInnerShape->IsValidScale(inScale); } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Vec3 mOffset; ///< Offset of the center of mass +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h b/WickedEngine/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h new file mode 100644 index 000000000..96e69f0b1 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/PolyhedronSubmergedVolumeCalculator.h @@ -0,0 +1,319 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// This class calculates the intersection between a fluid surface and a polyhedron and returns the submerged volume and its center of buoyancy +/// Construct this class and then one by one add all faces of the polyhedron using the AddFace function. After all faces have been added the result +/// can be gotten through GetResult. +class PolyhedronSubmergedVolumeCalculator +{ +private: + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 4 vertices submerged + // inV1 .. inV4 are submerged + inline static void sTetrahedronVolume4(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // Calculate center of mass and mass of this tetrahedron, + // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume + outVolumeTimes6 = max((inV1 - inV4).Dot((inV2 - inV4).Cross(inV3 - inV4)), 0.0f); // All contributions should be positive because we use a reference point that is on the surface of the hull + outCenterTimes4 = inV1 + inV2 + inV3 + inV4; + } + + // Get the intersection point with a plane. + // inV1 is inD1 distance away from the plane, inV2 is inD2 distance away from the plane + inline static Vec3 sGetPlaneIntersection(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2) + { + JPH_ASSERT(Sign(inD1) != Sign(inD2), "Assuming both points are on opposite ends of the plane"); + float delta = inD1 - inD2; + if (abs(delta) < 1.0e-6f) + return inV1; // Parallel to plane, just pick a point + else + return inV1 + inD1 * (inV2 - inV1) / delta; + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged + // inV1 is submerged, inV2 .. inV4 are not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron + Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2); + Vec3 v3 = sGetPlaneIntersection(inV1, inD1, inV3, inD3); + Vec3 v4 = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 v2w = mBaseOffset + v2; + RVec3 v3w = mBaseOffset + v3; + RVec3 v4w = mBaseOffset + v4; + + DebugRenderer::sInstance->DrawTriangle(v4w, v3w, v2w, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(v4w, v3w, v2w, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + sTetrahedronVolume4(inV1, v2, v3, v4, outVolumeTimes6, outCenterTimes4); + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged + // inV1, inV2 are submerged, inV3, inV4 are not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 2 points submerged is cut along 4 edges forming a quad + Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3); + Vec3 d = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + Vec3 e = sGetPlaneIntersection(inV2, inD2, inV4, inD4); + Vec3 f = sGetPlaneIntersection(inV2, inD2, inV3, inD3); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 cw = mBaseOffset + c; + RVec3 dw = mBaseOffset + d; + RVec3 ew = mBaseOffset + e; + RVec3 fw = mBaseOffset + f; + + DebugRenderer::sInstance->DrawTriangle(cw, ew, dw, Color::sGreen); + DebugRenderer::sInstance->DrawTriangle(cw, fw, ew, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(cw, ew, dw, Color::sWhite); + DebugRenderer::sInstance->DrawWireTriangle(cw, fw, ew, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + // We pick point c as reference (which is on the cut off surface) + // This leaves us with three tetrahedrons to sum up (any faces that are in the same plane as c will have zero volume) + Vec3 center1, center2, center3; + float volume1, volume2, volume3; + sTetrahedronVolume4(e, f, inV2, c, volume1, center1); + sTetrahedronVolume4(e, inV1, d, c, volume2, center2); + sTetrahedronVolume4(e, inV2, inV1, c, volume3, center3); + + // Tally up the totals + outVolumeTimes6 = volume1 + volume2 + volume3; + outCenterTimes4 = outVolumeTimes6 > 0.0f? (volume1 * center1 + volume2 * center2 + volume3 * center3) / outVolumeTimes6 : Vec3::sZero(); + } + + // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged + // inV1, inV2, inV3 are submerged, inV4 is not + // inD1 .. inD4 are the distances from the points to the plane + inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4) + { + // A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron + Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4); + Vec3 v2 = sGetPlaneIntersection(inV2, inD2, inV4, inD4); + Vec3 v3 = sGetPlaneIntersection(inV3, inD3, inV4, inD4); + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between tetrahedron and surface + if (Shape::sDrawSubmergedVolumes) + { + RVec3 v1w = mBaseOffset + v1; + RVec3 v2w = mBaseOffset + v2; + RVec3 v3w = mBaseOffset + v3; + + DebugRenderer::sInstance->DrawTriangle(v3w, v2w, v1w, Color::sGreen); + DebugRenderer::sInstance->DrawWireTriangle(v3w, v2w, v1w, Color::sWhite); + } + #endif // JPH_DEBUG_RENDERER + + Vec3 dry_center, total_center; + float dry_volume, total_volume; + + // We first calculate the part that is above the surface + sTetrahedronVolume4(v1, v2, v3, inV4, dry_volume, dry_center); + + // Calculate the total volume + sTetrahedronVolume4(inV1, inV2, inV3, inV4, total_volume, total_center); + + // From this we can calculate the center and volume of the submerged part + outVolumeTimes6 = max(total_volume - dry_volume, 0.0f); + outCenterTimes4 = outVolumeTimes6 > 0.0f? (total_center * total_volume - dry_center * dry_volume) / outVolumeTimes6 : Vec3::sZero(); + } + +public: + /// A helper class that contains cached information about a polyhedron vertex + class Point + { + public: + Vec3 mPosition; ///< World space position of vertex + float mDistanceToSurface; ///< Signed distance to the surface (> 0 is above, < 0 is below) + bool mAboveSurface; ///< If the point is above the surface (mDistanceToSurface > 0) + }; + + /// Constructor + /// @param inTransform Transform to transform all incoming points with + /// @param inPoints Array of points that are part of the polyhedron + /// @param inPointStride Amount of bytes between each point (should usually be sizeof(Vec3)) + /// @param inNumPoints The amount of points + /// @param inSurface The plane that forms the fluid surface (normal should point up) + /// @param ioBuffer A temporary buffer of Point's that should have inNumPoints entries and should stay alive while this class is alive +#ifdef JPH_DEBUG_RENDERER + /// @param inBaseOffset The offset to transform inTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing. +#endif // JPH_DEBUG_RENDERER + PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3 inBaseOffset +#endif // JPH_DEBUG_RENDERER + ) : + mPoints(ioBuffer) +#ifdef JPH_DEBUG_RENDERER + , mBaseOffset(inBaseOffset) +#endif // JPH_DEBUG_RENDERER + { + // Convert the points to world space and determine the distance to the surface + float reference_dist = FLT_MAX; + for (int p = 0; p < inNumPoints; ++p) + { + // Calculate values + Vec3 transformed_point = inTransform * *reinterpret_cast(reinterpret_cast(inPoints) + p * inPointStride); + float dist = inSurface.SignedDistance(transformed_point); + bool above = dist >= 0.0f; + + // Keep track if all are above or below + mAllAbove &= above; + mAllBelow &= !above; + + // Calculate lowest point, we use this to create tetrahedrons out of all faces + if (reference_dist > dist) + { + mReferencePointIdx = p; + reference_dist = dist; + } + + // Store values + ioBuffer->mPosition = transformed_point; + ioBuffer->mDistanceToSurface = dist; + ioBuffer->mAboveSurface = above; + ++ioBuffer; + } + } + + /// Check if all points are above the surface. Should be used as early out. + inline bool AreAllAbove() const + { + return mAllAbove; + } + + /// Check if all points are below the surface. Should be used as early out. + inline bool AreAllBelow() const + { + return mAllBelow; + } + + /// Get the lowest point of the polyhedron. Used to form the 4th vertex to make a tetrahedron out of a polyhedron face. + inline int GetReferencePointIdx() const + { + return mReferencePointIdx; + } + + /// Add a polyhedron face. Supply the indices of the points that form the face (in counter clockwise order). + void AddFace(int inIdx1, int inIdx2, int inIdx3) + { + JPH_ASSERT(inIdx1 != mReferencePointIdx && inIdx2 != mReferencePointIdx && inIdx3 != mReferencePointIdx, "A face using the reference point will not contribute to the volume"); + + // Find the points + const Point &ref = mPoints[mReferencePointIdx]; + const Point &p1 = mPoints[inIdx1]; + const Point &p2 = mPoints[inIdx2]; + const Point &p3 = mPoints[inIdx3]; + + // Determine which vertices are submerged + uint code = (p1.mAboveSurface? 0 : 0b001) | (p2.mAboveSurface? 0 : 0b010) | (p3.mAboveSurface? 0 : 0b100); + + float volume; + Vec3 center; + switch (code) + { + case 0b000: + // One point submerged + sTetrahedronVolume1(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b001: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center); + break; + + case 0b010: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center); + break; + + case 0b100: + // Two points submerged + sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b011: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center); + break; + + case 0b101: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center); + break; + + case 0b110: + // Three points submerged + sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center); + break; + + case 0b111: + // Four points submerged + sTetrahedronVolume4(ref.mPosition, p3.mPosition, p2.mPosition, p1.mPosition, volume, center); + break; + + default: + // Should not be possible + JPH_ASSERT(false); + volume = 0.0f; + center = Vec3::sZero(); + break; + } + + mSubmergedVolume += volume; + mCenterOfBuoyancy += volume * center; + } + + /// Call after all faces have been added. Returns the submerged volume and the center of buoyancy for the submerged volume. + void GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const + { + outCenterOfBuoyancy = mSubmergedVolume > 0.0f? mCenterOfBuoyancy / (4.0f * mSubmergedVolume) : Vec3::sZero(); // Do this before dividing submerged volume by 6 to get correct weight factor + outSubmergedVolume = mSubmergedVolume / 6.0f; + } + +private: + // The precalculated points for this polyhedron + const Point * mPoints; + + // If all points are above/below the surface + bool mAllBelow = true; + bool mAllAbove = true; + + // The lowest point + int mReferencePointIdx = 0; + + // Aggregator for submerged volume and center of buoyancy + float mSubmergedVolume = 0.0f; + Vec3 mCenterOfBuoyancy = Vec3::sZero(); + +#ifdef JPH_DEBUG_RENDERER + // Base offset used for drawing + RVec3 mBaseOffset; +#endif +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp new file mode 100644 index 000000000..7d6e06a01 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.cpp @@ -0,0 +1,315 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RotatedTranslatedShapeSettings) +{ + JPH_ADD_BASE_CLASS(RotatedTranslatedShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mPosition) + JPH_ADD_ATTRIBUTE(RotatedTranslatedShapeSettings, mRotation) +} + +ShapeSettings::ShapeResult RotatedTranslatedShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new RotatedTranslatedShape(*this, mCachedResult); + return mCachedResult; +} + +RotatedTranslatedShape::RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::RotatedTranslated, inSettings, outResult) +{ + if (outResult.HasError()) + return; + + // Calculate center of mass position + mCenterOfMass = inSettings.mPosition + inSettings.mRotation * mInnerShape->GetCenterOfMass(); + + // Store rotation (position is always zero because we center around the center of mass) + mRotation = inSettings.mRotation; + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); + + outResult.Set(this); +} + +RotatedTranslatedShape::RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape) : + DecoratedShape(EShapeSubType::RotatedTranslated, inShape) +{ + // Calculate center of mass position + mCenterOfMass = inPosition + inRotation * mInnerShape->GetCenterOfMass(); + + // Store rotation (position is always zero because we center around the center of mass) + mRotation = inRotation; + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); +} + +MassProperties RotatedTranslatedShape::GetMassProperties() const +{ + // Rotate inertia of child into place + MassProperties p = mInnerShape->GetMassProperties(); + p.Rotate(Mat44::sRotation(mRotation)); + return p; +} + +AABox RotatedTranslatedShape::GetLocalBounds() const +{ + return mInnerShape->GetLocalBounds().Transformed(Mat44::sRotation(mRotation)); +} + +AABox RotatedTranslatedShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation); + return mInnerShape->GetWorldSpaceBounds(transform, TransformScale(inScale)); +} + +TransformedShape RotatedTranslatedShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM), inRotation * mRotation, mInnerShape, BodyID()); + ts.SetShapeScale(TransformScale(inScale)); + return ts; +} + +Vec3 RotatedTranslatedShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform surface position to local space and pass call on + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, transform * inLocalSurfacePosition); + + // Transform normal to this shape's space + return transform.Multiply3x3Transposed(normal); +} + +void RotatedTranslatedShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + Mat44 transform = Mat44::sRotation(mRotation); + mInnerShape->GetSupportingFace(inSubShapeID, transform.Multiply3x3Transposed(inDirection), TransformScale(inScale), inCenterOfMassTransform * transform, outVertices); +} + +void RotatedTranslatedShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // Get center of mass transform of child + Mat44 transform = inCenterOfMassTransform * Mat44::sRotation(mRotation); + + // Recurse to child + mInnerShape->GetSubmergedVolume(transform, TransformScale(inScale), inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void RotatedTranslatedShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe); +} + +void RotatedTranslatedShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale), inColor, inDrawSupportDirection); +} + +void RotatedTranslatedShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * Mat44::sRotation(mRotation), TransformScale(inScale)); +} +#endif // JPH_DEBUG_RENDERER + +bool RotatedTranslatedShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + // Transform the ray + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + RayCast ray = inRay.Transformed(transform); + + return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit); +} + +void RotatedTranslatedShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the ray + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + RayCast ray = inRay.Transformed(transform); + + return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Transform the point + Mat44 transform = Mat44::sRotation(mRotation.Conjugated()); + mInnerShape->CollidePoint(transform * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform * Mat44::sRotation(mRotation), inScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); +} + +void RotatedTranslatedShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation * mRotation, TransformScale(inScale), inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sRotation(mRotation), ioCollector); +} + +void RotatedTranslatedShape::sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShape1); + + // Get world transform of 1 + Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, shape1->TransformScale(inScale1), inScale2, transform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape2); + + // Get world transform of 2 + Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, shape2->TransformScale(inScale2), inCenterOfMassTransform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape2); + + // Get world transform of 1 and 2 + Mat44 transform1 = inCenterOfMassTransform1 * Mat44::sRotation(shape1->mRotation); + Mat44 transform2 = inCenterOfMassTransform2 * Mat44::sRotation(shape2->mRotation); + + CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, shape2->mInnerShape, shape1->TransformScale(inScale1), shape2->TransformScale(inScale2), transform1, transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void RotatedTranslatedShape::sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + // Fetch rotated translated shape from cast shape + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShapeCast.mShape); + + // Transform the shape cast and update the shape + Mat44 transform = inShapeCast.mCenterOfMassStart * Mat44::sRotation(shape1->mRotation); + Vec3 scale = shape1->TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape1->mInnerShape, scale, transform, inShapeCast.mDirection); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape = static_cast(inShape); + + // Determine the local transform + Mat44 local_transform = Mat44::sRotation(shape->mRotation); + + // Transform the shape cast + ShapeCast shape_cast = inShapeCast.PostTransformed(local_transform.Transposed3x3()); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, shape->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape1 = static_cast(inShapeCast.mShape); + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::RotatedTranslated); + const RotatedTranslatedShape *shape2 = static_cast(inShape); + + // Determine the local transform of shape 2 + Mat44 local_transform2 = Mat44::sRotation(shape2->mRotation); + Mat44 local_transform2_transposed = local_transform2.Transposed3x3(); + + // Transform the shape cast and update the shape + Mat44 transform = (local_transform2_transposed * inShapeCast.mCenterOfMassStart) * Mat44::sRotation(shape1->mRotation); + Vec3 scale = shape1->TransformScale(inShapeCast.mScale); + ShapeCast shape_cast(shape1->mInnerShape, scale, transform, local_transform2_transposed.Multiply3x3(inShapeCast.mDirection)); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape2->mInnerShape, shape2->TransformScale(inScale), inShapeFilter, inCenterOfMassTransform2 * local_transform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void RotatedTranslatedShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mRotation); +} + +void RotatedTranslatedShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mRotation); + mIsRotationIdentity = mRotation.IsClose(Quat::sIdentity()); +} + +bool RotatedTranslatedShape::IsValidScale(Vec3Arg inScale) const +{ + if (!DecoratedShape::IsValidScale(inScale)) + return false; + + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return mInnerShape->IsValidScale(inScale); + + if (!ScaleHelpers::CanScaleBeRotated(mRotation, inScale)) + return false; + + return mInnerShape->IsValidScale(ScaleHelpers::RotateScale(mRotation, inScale)); +} + +void RotatedTranslatedShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::RotatedTranslated); + f.mConstruct = []() -> Shape * { return new RotatedTranslatedShape; }; + f.mColor = Color::sBlue; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, s, sCollideRotatedTranslatedVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::RotatedTranslated, sCollideShapeVsRotatedTranslated); + CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, s, sCastRotatedTranslatedVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::RotatedTranslated, sCastShapeVsRotatedTranslated); + } + + CollisionDispatch::sRegisterCollideShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCollideRotatedTranslatedVsRotatedTranslated); + CollisionDispatch::sRegisterCastShape(EShapeSubType::RotatedTranslated, EShapeSubType::RotatedTranslated, sCastRotatedTranslatedVsRotatedTranslated); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h new file mode 100644 index 000000000..c37dbca82 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; + +/// Class that constructs a RotatedTranslatedShape +class JPH_EXPORT RotatedTranslatedShapeSettings final : public DecoratedShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RotatedTranslatedShapeSettings) + + /// Constructor + RotatedTranslatedShapeSettings() = default; + + /// Construct with shape settings, can be serialized. + RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape) : DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + RotatedTranslatedShapeSettings(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape): DecoratedShapeSettings(inShape), mPosition(inPosition), mRotation(inRotation) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mPosition; ///< Position of the sub shape + Quat mRotation; ///< Rotation of the sub shape +}; + +/// A rotated translated shape will rotate and translate a child shape. +/// Shifts the child object so that it is centered around the center of mass. +class JPH_EXPORT RotatedTranslatedShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + RotatedTranslatedShape() : DecoratedShape(EShapeSubType::RotatedTranslated) { } + RotatedTranslatedShape(const RotatedTranslatedShapeSettings &inSettings, ShapeResult &outResult); + RotatedTranslatedShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape); + + /// Access the rotation that is applied to the inner shape + Quat GetRotation() const { return mRotation; } + + /// Access the translation that has been applied to the inner shape + Vec3 GetPosition() const { return mCenterOfMass - mRotation * mInnerShape->GetCenterOfMass(); } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return mInnerShape->GetVolume(); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideRotatedTranslatedVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideRotatedTranslatedVsRotatedTranslated(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastRotatedTranslatedVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastRotatedTranslatedVsRotatedTranslated(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + /// Transform the scale to the local space of the child shape + inline Vec3 TransformScale(Vec3Arg inScale) const + { + // We don't need to transform uniform scale or if the rotation is identity + if (mIsRotationIdentity || ScaleHelpers::IsUniformScale(inScale)) + return inScale; + + return ScaleHelpers::RotateScale(mRotation, inScale); + } + + bool mIsRotationIdentity; ///< If mRotation is close to identity (put here because it falls in padding bytes) + Vec3 mCenterOfMass; ///< Position of the center of mass + Quat mRotation; ///< Rotation of the child shape +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/ScaleHelpers.h b/WickedEngine/Jolt/Physics/Collision/Shape/ScaleHelpers.h new file mode 100644 index 000000000..ba20c2519 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/ScaleHelpers.h @@ -0,0 +1,68 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Helper functions to get properties of a scaling vector +namespace ScaleHelpers +{ + /// The tolerance used to check if components of the scale vector are the same + static constexpr float cScaleToleranceSq = 1.0e-8f; + + /// Test if a scale is identity + inline bool IsNotScaled(Vec3Arg inScale) { return inScale.IsClose(Vec3::sReplicate(1.0f), cScaleToleranceSq); } + + /// Test if a scale is uniform + inline bool IsUniformScale(Vec3Arg inScale) { return inScale.Swizzle().IsClose(inScale, cScaleToleranceSq); } + + /// Scale the convex radius of an object + inline float ScaleConvexRadius(float inConvexRadius, Vec3Arg inScale) { return min(inConvexRadius * inScale.Abs().ReduceMin(), cDefaultConvexRadius); } + + /// Test if a scale flips an object inside out (which requires flipping all normals and polygon windings) + inline bool IsInsideOut(Vec3Arg inScale) { return (CountBits(Vec3::sLess(inScale, Vec3::sZero()).GetTrues() & 0x7) & 1) != 0; } + + /// Get the average scale if inScale, used to make the scale uniform when a shape doesn't support non-uniform scale + inline Vec3 MakeUniformScale(Vec3Arg inScale) { return Vec3::sReplicate((inScale.GetX() + inScale.GetY() + inScale.GetZ()) / 3.0f); } + + /// Checks in scale can be rotated to child shape + /// @param inRotation Rotation of child shape + /// @param inScale Scale in local space of parent shape + /// @return True if the scale is valid (no shearing introduced) + inline bool CanScaleBeRotated(QuatArg inRotation, Vec3Arg inScale) + { + // inScale is a scale in local space of the shape, so the transform for the shape (ignoring translation) is: T = Mat44::sScale(inScale) * mRotation. + // when we pass the scale to the child it needs to be local to the child, so we want T = mRotation * Mat44::sScale(ChildScale). + // Solving for ChildScale: ChildScale = mRotation^-1 * Mat44::sScale(inScale) * mRotation = mRotation^T * Mat44::sScale(inScale) * mRotation + // If any of the off diagonal elements are non-zero, it means the scale / rotation is not compatible. + Mat44 r = Mat44::sRotation(inRotation); + Mat44 child_scale = r.Multiply3x3LeftTransposed(r.PostScaled(inScale)); + + // Get the columns, but zero the diagonal + Vec4 zero = Vec4::sZero(); + Vec4 c0 = Vec4::sSelect(child_scale.GetColumn4(0), zero, UVec4(0xffffffff, 0, 0, 0)).Abs(); + Vec4 c1 = Vec4::sSelect(child_scale.GetColumn4(1), zero, UVec4(0, 0xffffffff, 0, 0)).Abs(); + Vec4 c2 = Vec4::sSelect(child_scale.GetColumn4(2), zero, UVec4(0, 0, 0xffffffff, 0)).Abs(); + + // Check if all elements are less than epsilon + Vec4 epsilon = Vec4::sReplicate(1.0e-6f); + return UVec4::sAnd(UVec4::sAnd(Vec4::sLess(c0, epsilon), Vec4::sLess(c1, epsilon)), Vec4::sLess(c2, epsilon)).TestAllTrue(); + } + + /// Adjust scale for rotated child shape + /// @param inRotation Rotation of child shape + /// @param inScale Scale in local space of parent shape + /// @return Rotated scale + inline Vec3 RotateScale(QuatArg inRotation, Vec3Arg inScale) + { + // Get the diagonal of mRotation^T * Mat44::sScale(inScale) * mRotation (see comment at CanScaleBeRotated) + Mat44 r = Mat44::sRotation(inRotation); + return r.Multiply3x3LeftTransposed(r.PostScaled(inScale)).GetDiagonal3(); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.cpp new file mode 100644 index 000000000..10b1c83f3 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.cpp @@ -0,0 +1,226 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ScaledShapeSettings) +{ + JPH_ADD_BASE_CLASS(ScaledShapeSettings, DecoratedShapeSettings) + + JPH_ADD_ATTRIBUTE(ScaledShapeSettings, mScale) +} + +ShapeSettings::ShapeResult ScaledShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new ScaledShape(*this, mCachedResult); + return mCachedResult; +} + +ScaledShape::ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult) : + DecoratedShape(EShapeSubType::Scaled, inSettings, outResult), + mScale(inSettings.mScale) +{ + if (outResult.HasError()) + return; + + outResult.Set(this); +} + +MassProperties ScaledShape::GetMassProperties() const +{ + MassProperties p = mInnerShape->GetMassProperties(); + p.Scale(mScale); + return p; +} + +AABox ScaledShape::GetLocalBounds() const +{ + return mInnerShape->GetLocalBounds().Scaled(mScale); +} + +AABox ScaledShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform, inScale * mScale); +} + +TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We don't use any bits in the sub shape ID + outRemainder = inSubShapeID; + + TransformedShape ts(RVec3(inPositionCOM), inRotation, mInnerShape, BodyID()); + ts.SetShapeScale(inScale * mScale); + return ts; +} + +Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + // Transform the surface point to local space and pass the query on + Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition / mScale); + + // Need to transform the plane normals using inScale + // Transforming a direction with matrix M is done through multiplying by (M^-1)^T + // In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards + return (normal / mScale).Normalized(); +} + +void ScaledShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale * mScale, inCenterOfMassTransform, outVertices); +} + +void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset)); +} + +#ifdef JPH_DEBUG_RENDERER +void ScaledShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + mInnerShape->Draw(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inUseMaterialColors, inDrawWireframe); +} + +void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const +{ + mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection); +} + +void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale); +} +#endif // JPH_DEBUG_RENDERER + +bool ScaledShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + Vec3 inv_scale = mScale.Reciprocal(); + RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection }; + return mInnerShape->CastRay(scaled_ray, inSubShapeIDCreator, ioHit); +} + +void ScaledShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + Vec3 inv_scale = mScale.Reciprocal(); + RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection }; + return mInnerShape->CastRay(scaled_ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + Vec3 inv_scale = mScale.Reciprocal(); + mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, ioVertices, inNumVertices, inDeltaTime, inDisplacementDueToGravity, inCollidingShapeIndex); +} + +void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation, inScale * mScale, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void ScaledShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sScale(mScale), ioCollector); +} + +void ScaledShape::SaveBinaryState(StreamOut &inStream) const +{ + DecoratedShape::SaveBinaryState(inStream); + + inStream.Write(mScale); +} + +void ScaledShape::RestoreBinaryState(StreamIn &inStream) +{ + DecoratedShape::RestoreBinaryState(inStream); + + inStream.Read(mScale); +} + +float ScaledShape::GetVolume() const +{ + return abs(mScale.GetX() * mScale.GetY() * mScale.GetZ()) * mInnerShape->GetVolume(); +} + +bool ScaledShape::IsValidScale(Vec3Arg inScale) const +{ + return mInnerShape->IsValidScale(inScale * mScale); +} + +void ScaledShape::sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape1 = static_cast(inShape1); + + CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), inShape2, inScale1 * shape1->GetScale(), inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void ScaledShape::sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape2 = static_cast(inShape2); + + CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->GetInnerShape(), inScale1, inScale2 * shape2->GetScale(), inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); +} + +void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape = static_cast(inShapeCast.mShape); + + ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection); + CollisionDispatch::sCastShapeVsShapeLocalSpace(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled); + const ScaledShape *shape = static_cast(inShape); + + CollisionDispatch::sCastShapeVsShapeLocalSpace(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); +} + +void ScaledShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Scaled); + f.mConstruct = []() -> Shape * { return new ScaledShape; }; + f.mColor = Color::sYellow; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Scaled, s, sCollideScaledVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Scaled, sCollideShapeVsScaled); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, s, sCastScaledVsShape); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Scaled, sCastShapeVsScaled); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.h new file mode 100644 index 000000000..ea9b34351 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/ScaledShape.h @@ -0,0 +1,140 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class SubShapeIDCreator; +class CollideShapeSettings; + +/// Class that constructs a ScaledShape +class JPH_EXPORT ScaledShapeSettings final : public DecoratedShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ScaledShapeSettings) + + /// Default constructor for deserialization + ScaledShapeSettings() = default; + + /// Constructor that decorates another shape with a scale + ScaledShapeSettings(const ShapeSettings *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { } + + /// Variant that uses a concrete shape, which means this object cannot be serialized. + ScaledShapeSettings(const Shape *inShape, Vec3Arg inScale) : DecoratedShapeSettings(inShape), mScale(inScale) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mScale = Vec3(1, 1, 1); +}; + +/// A shape that scales a child shape in local space of that shape. The scale can be non-uniform and can even turn it inside out when one or three components of the scale are negative. +class JPH_EXPORT ScaledShape final : public DecoratedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + ScaledShape() : DecoratedShape(EShapeSubType::Scaled) { } + ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult); + + /// Constructor that decorates another shape with a scale + ScaledShape(const Shape *inShape, Vec3Arg inScale) : DecoratedShape(EShapeSubType::Scaled, inShape), mScale(inScale) { } + + /// Get the scale + Vec3 GetScale() const { return mScale; } + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mScale * mInnerShape->GetCenterOfMass(); } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mScale.ReduceMin() * mInnerShape->GetInnerRadius(); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSubShapeTransformedShape + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; + + // See Shape::DrawGetSupportFunction + virtual void DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const override; + + // See Shape::DrawGetSupportingFace + virtual void DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); } + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override { JPH_ASSERT(false, "Cannot call on non-leaf shapes, use CollectTransformedShapes to collect the leaves first!"); return 0; } + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override; + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideScaledVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsScaled(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + Vec3 mScale = Vec3(1, 1, 1); +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/Shape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/Shape.cpp new file mode 100644 index 000000000..8412156f7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/Shape.cpp @@ -0,0 +1,309 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT_BASE(ShapeSettings) +{ + JPH_ADD_BASE_CLASS(ShapeSettings, SerializableObject) + + JPH_ADD_ATTRIBUTE(ShapeSettings, mUserData) +} + +#ifdef JPH_DEBUG_RENDERER +bool Shape::sDrawSubmergedVolumes = false; +#endif // JPH_DEBUG_RENDERER + +ShapeFunctions ShapeFunctions::sRegistry[NumSubShapeTypes]; + +TransformedShape Shape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const +{ + // We have reached the leaf shape so there is no remainder + outRemainder = SubShapeID(); + + // Just return the transformed shape for this shape + TransformedShape ts(RVec3(inPositionCOM), inRotation, this, BodyID()); + ts.SetShapeScale(inScale); + return ts; +} + +void Shape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + TransformedShape ts(RVec3(inPositionCOM), inRotation, this, TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator); + ts.SetShapeScale(inScale); + ioCollector.AddHit(ts); +} + +void Shape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + ts.SetShapeScale(scale); + ioCollector.AddHit(ts); +} + +void Shape::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mShapeSubType); + inStream.Write(mUserData); +} + +void Shape::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mUserData); +} + +Shape::ShapeResult Shape::sRestoreFromBinaryState(StreamIn &inStream) +{ + ShapeResult result; + + // Read the type of the shape + EShapeSubType shape_sub_type; + inStream.Read(shape_sub_type); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read type id"); + return result; + } + + // Construct and read the data of the shape + Ref shape = ShapeFunctions::sGet(shape_sub_type).mConstruct(); + shape->RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to restore shape"); + return result; + } + + result.Set(shape); + return result; +} + +void Shape::SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const +{ + ShapeToIDMap::const_iterator shape_id_iter = ioShapeMap.find(this); + if (shape_id_iter == ioShapeMap.end()) + { + // Write shape ID of this shape + uint32 shape_id = (uint32)ioShapeMap.size(); + ioShapeMap[this] = shape_id; + inStream.Write(shape_id); + + // Write the shape itself + SaveBinaryState(inStream); + + // Write the ID's of all sub shapes + ShapeList sub_shapes; + SaveSubShapeState(sub_shapes); + inStream.Write(sub_shapes.size()); + for (const Shape *shape : sub_shapes) + { + if (shape == nullptr) + inStream.Write(~uint32(0)); + else + shape->SaveWithChildren(inStream, ioShapeMap, ioMaterialMap); + } + + // Write the materials + PhysicsMaterialList materials; + SaveMaterialState(materials); + StreamUtils::SaveObjectArray(inStream, materials, &ioMaterialMap); + } + else + { + // Known shape, just write the ID + inStream.Write(shape_id_iter->second); + } +} + +Shape::ShapeResult Shape::sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap) +{ + ShapeResult result; + + // Read ID of this shape + uint32 shape_id; + inStream.Read(shape_id); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read shape id"); + return result; + } + + // Check nullptr shape + if (shape_id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if we already read this shape + if (shape_id < ioShapeMap.size()) + { + result.Set(ioShapeMap[shape_id]); + return result; + } + + // Read the shape + result = sRestoreFromBinaryState(inStream); + if (result.HasError()) + return result; + JPH_ASSERT(ioShapeMap.size() == shape_id); // Assert that this is the next ID in the map + ioShapeMap.push_back(result.Get()); + + // Read the sub shapes + size_t len; + inStream.Read(len); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read stream"); + return result; + } + ShapeList sub_shapes; + sub_shapes.reserve(len); + for (size_t i = 0; i < len; ++i) + { + ShapeResult sub_shape_result = sRestoreWithChildren(inStream, ioShapeMap, ioMaterialMap); + if (sub_shape_result.HasError()) + return sub_shape_result; + sub_shapes.push_back(sub_shape_result.Get()); + } + result.Get()->RestoreSubShapeState(sub_shapes.data(), (uint)sub_shapes.size()); + + // Read the materials + Result mlresult = StreamUtils::RestoreObjectArray(inStream, ioMaterialMap); + if (mlresult.HasError()) + { + result.SetError(mlresult.GetError()); + return result; + } + const PhysicsMaterialList &materials = mlresult.Get(); + result.Get()->RestoreMaterialState(materials.data(), (uint)materials.size()); + + return result; +} + +Shape::Stats Shape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const +{ + Stats stats = GetStats(); + + // If shape is already visited, don't count its size again + if (!ioVisitedShapes.insert(this).second) + stats.mSizeBytes = 0; + + return stats; +} + +Shape::ShapeResult Shape::ScaleShape(Vec3Arg inScale) const +{ + const Vec3 unit_scale = Vec3::sReplicate(1.0f); + + if (inScale.IsNearZero()) + { + ShapeResult result; + result.SetError("Can't use zero scale!"); + return result; + } + + // First test if we can just wrap this shape in a scaled shape + if (IsValidScale(inScale)) + { + // Test if the scale is near unit + ShapeResult result; + if (inScale.IsClose(unit_scale)) + result.Set(const_cast(this)); + else + result.Set(new ScaledShape(this, inScale)); + return result; + } + + // Collect the leaf shapes and their transforms + struct Collector : TransformedShapeCollector + { + virtual void AddHit(const ResultType &inResult) override + { + mShapes.push_back(inResult); + } + + Array mShapes; + }; + Collector collector; + TransformShape(Mat44::sScale(inScale) * Mat44::sTranslation(GetCenterOfMass()), collector); + + // Construct a compound shape + StaticCompoundShapeSettings compound; + compound.mSubShapes.reserve(collector.mShapes.size()); + for (const TransformedShape &ts : collector.mShapes) + { + const Shape *shape = ts.mShape; + + // Construct a scaled shape if scale is not unit + Vec3 scale = ts.GetShapeScale(); + if (!scale.IsClose(unit_scale)) + shape = new ScaledShape(shape, scale); + + // Add the shape + compound.AddShape(Vec3(ts.mShapePositionCOM) - ts.mShapeRotation * shape->GetCenterOfMass(), ts.mShapeRotation, shape); + } + + return compound.Create(); +} + +void Shape::sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + // First test if we're inside our bounding box + AABox bounds = inShape.GetLocalBounds(); + if (bounds.Contains(inPoint)) + { + // A collector that just counts the number of hits + class HitCountCollector : public CastRayCollector + { + public: + virtual void AddHit(const RayCastResult &inResult) override + { + // Store the last sub shape ID so that we can provide something to our outer hit collector + mSubShapeID = inResult.mSubShapeID2; + + ++mHitCount; + } + + int mHitCount = 0; + SubShapeID mSubShapeID; + }; + HitCountCollector collector; + + // Configure the raycast + RayCastSettings settings; + settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces; + + // Cast a ray that's 10% longer than the height of our bounding box + inShape.CastRay(RayCast { inPoint, 1.1f * bounds.GetSize().GetY() * Vec3::sAxisY() }, settings, inSubShapeIDCreator, collector, inShapeFilter); + + // Odd amount of hits means inside + if ((collector.mHitCount & 1) == 1) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), collector.mSubShapeID }); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/Shape.h b/WickedEngine/Jolt/Physics/Collision/Shape/Shape.h new file mode 100644 index 000000000..a61314483 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/Shape.h @@ -0,0 +1,447 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RayCast; +class RayCastSettings; +struct ShapeCast; +class ShapeCastSettings; +class RayCastResult; +class ShapeCastResult; +class CollidePointResult; +class CollideShapeResult; +class SubShapeIDCreator; +class SubShapeID; +class PhysicsMaterial; +class TransformedShape; +class Plane; +class SoftBodyVertex; +class Shape; +class StreamOut; +class StreamIn; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +using CastRayCollector = CollisionCollector; +using CastShapeCollector = CollisionCollector; +using CollidePointCollector = CollisionCollector; +using CollideShapeCollector = CollisionCollector; +using TransformedShapeCollector = CollisionCollector; + +using ShapeRefC = RefConst; +using ShapeList = Array; +using PhysicsMaterialRefC = RefConst; +using PhysicsMaterialList = Array; + +/// Shapes are categorized in groups, each shape can return which group it belongs to through its Shape::GetType function. +enum class EShapeType : uint8 +{ + Convex, ///< Used by ConvexShape, all shapes that use the generic convex vs convex collision detection system (box, sphere, capsule, tapered capsule, cylinder, triangle) + Compound, ///< Used by CompoundShape + Decorated, ///< Used by DecoratedShape + Mesh, ///< Used by MeshShape + HeightField, ///< Used by HeightFieldShape + SoftBody, ///< Used by SoftBodyShape + + // User defined shapes + User1, + User2, + User3, + User4, +}; + +/// This enumerates all shape types, each shape can return its type through Shape::GetSubType +enum class EShapeSubType : uint8 +{ + // Convex shapes + Sphere, + Box, + Triangle, + Capsule, + TaperedCapsule, + Cylinder, + ConvexHull, + + // Compound shapes + StaticCompound, + MutableCompound, + + // Decorated shapes + RotatedTranslated, + Scaled, + OffsetCenterOfMass, + + // Other shapes + Mesh, + HeightField, + SoftBody, + + // User defined shapes + User1, + User2, + User3, + User4, + User5, + User6, + User7, + User8, + + // User defined convex shapes + UserConvex1, + UserConvex2, + UserConvex3, + UserConvex4, + UserConvex5, + UserConvex6, + UserConvex7, + UserConvex8, +}; + +// Sets of shape sub types +static constexpr EShapeSubType sAllSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::StaticCompound, EShapeSubType::MutableCompound, EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass, EShapeSubType::Mesh, EShapeSubType::HeightField, EShapeSubType::SoftBody, EShapeSubType::User1, EShapeSubType::User2, EShapeSubType::User3, EShapeSubType::User4, EShapeSubType::User5, EShapeSubType::User6, EShapeSubType::User7, EShapeSubType::User8, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 }; +static constexpr EShapeSubType sConvexSubShapeTypes[] = { EShapeSubType::Sphere, EShapeSubType::Box, EShapeSubType::Triangle, EShapeSubType::Capsule, EShapeSubType::TaperedCapsule, EShapeSubType::Cylinder, EShapeSubType::ConvexHull, EShapeSubType::UserConvex1, EShapeSubType::UserConvex2, EShapeSubType::UserConvex3, EShapeSubType::UserConvex4, EShapeSubType::UserConvex5, EShapeSubType::UserConvex6, EShapeSubType::UserConvex7, EShapeSubType::UserConvex8 }; +static constexpr EShapeSubType sCompoundSubShapeTypes[] = { EShapeSubType::StaticCompound, EShapeSubType::MutableCompound }; +static constexpr EShapeSubType sDecoratorSubShapeTypes[] = { EShapeSubType::RotatedTranslated, EShapeSubType::Scaled, EShapeSubType::OffsetCenterOfMass }; + +/// How many shape types we support +static constexpr uint NumSubShapeTypes = uint(size(sAllSubShapeTypes)); + +/// Names of sub shape types +static constexpr const char *sSubShapeTypeNames[] = { "Sphere", "Box", "Triangle", "Capsule", "TaperedCapsule", "Cylinder", "ConvexHull", "StaticCompound", "MutableCompound", "RotatedTranslated", "Scaled", "OffsetCenterOfMass", "Mesh", "HeightField", "SoftBody", "User1", "User2", "User3", "User4", "User5", "User6", "User7", "User8", "UserConvex1", "UserConvex2", "UserConvex3", "UserConvex4", "UserConvex5", "UserConvex6", "UserConvex7", "UserConvex8" }; +static_assert(size(sSubShapeTypeNames) == NumSubShapeTypes); + +/// Class that can construct shapes and that is serializable using the ObjectStream system. +/// Can be used to store shape data in 'uncooked' form (i.e. in a form that is still human readable and authorable). +/// Once the shape has been created using the Create() function, the data will be moved into the Shape class +/// in a form that is optimized for collision detection. After this, the ShapeSettings object is no longer needed +/// and can be destroyed. Each shape class has a derived class of the ShapeSettings object to store shape specific +/// data. +class JPH_EXPORT ShapeSettings : public SerializableObject, public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, ShapeSettings) + + using ShapeResult = Result>; + + /// Create a shape according to the settings specified by this object. + virtual ShapeResult Create() const = 0; + + /// When creating a shape, the result is cached so that calling Create() again will return the same shape. + /// If you make changes to the ShapeSettings you need to call this function to clear the cached result to allow Create() to build a new shape. + void ClearCachedResult() { mCachedResult.Clear(); } + + /// User data (to be used freely by the application) + uint64 mUserData = 0; + +protected: + mutable ShapeResult mCachedResult; +}; + +/// Function table for functions on shapes +class JPH_EXPORT ShapeFunctions +{ +public: + /// Construct a shape + Shape * (*mConstruct)() = nullptr; + + /// Color of the shape when drawing + Color mColor = Color::sBlack; + + /// Get an entry in the registry for a particular sub type + static inline ShapeFunctions & sGet(EShapeSubType inSubType) { return sRegistry[int(inSubType)]; } + +private: + static ShapeFunctions sRegistry[NumSubShapeTypes]; +}; + +/// Base class for all shapes (collision volume of a body). Defines a virtual interface for collision detection. +class JPH_EXPORT Shape : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + using ShapeResult = ShapeSettings::ShapeResult; + + /// Constructor + Shape(EShapeType inType, EShapeSubType inSubType) : mShapeType(inType), mShapeSubType(inSubType) { } + Shape(EShapeType inType, EShapeSubType inSubType, const ShapeSettings &inSettings, [[maybe_unused]] ShapeResult &outResult) : mUserData(inSettings.mUserData), mShapeType(inType), mShapeSubType(inSubType) { } + + /// Destructor + virtual ~Shape() = default; + + /// Get type + inline EShapeType GetType() const { return mShapeType; } + inline EShapeSubType GetSubType() const { return mShapeSubType; } + + /// User data (to be used freely by the application) + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Check if this shape can only be used to create a static body or if it can also be dynamic/kinematic + virtual bool MustBeStatic() const { return false; } + + /// All shapes are centered around their center of mass. This function returns the center of mass position that needs to be applied to transform the shape to where it was created. + virtual Vec3 GetCenterOfMass() const { return Vec3::sZero(); } + + /// Get local bounding box including convex radius, this box is centered around the center of mass rather than the world transform + virtual AABox GetLocalBounds() const = 0; + + /// Get the max number of sub shape ID bits that are needed to be able to address any leaf shape in this shape. Used mainly for checking that it is smaller or equal than SubShapeID::MaxBits. + virtual uint GetSubShapeIDBitsRecursive() const = 0; + + /// Get world space bounds including convex radius. + /// This shape is scaled by inScale in local space first. + /// This function can be overridden to return a closer fitting world space bounding box, by default it will just transform what GetLocalBounds() returns. + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const { return GetLocalBounds().Scaled(inScale).Transformed(inCenterOfMassTransform); } + + /// Get world space bounds including convex radius. + AABox GetWorldSpaceBounds(DMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const + { + // Use single precision version using the rotation only + AABox bounds = GetWorldSpaceBounds(inCenterOfMassTransform.GetRotation(), inScale); + + // Apply translation + bounds.Translate(inCenterOfMassTransform.GetTranslation()); + + return bounds; + } + + /// Returns the radius of the biggest sphere that fits entirely in the shape. In case this shape consists of multiple sub shapes, it returns the smallest sphere of the parts. + /// This can be used as a measure of how far the shape can be moved without risking going through geometry. + virtual float GetInnerRadius() const = 0; + + /// Calculate the mass and inertia of this shape + virtual MassProperties GetMassProperties() const = 0; + + /// Get the material assigned to a particular sub shape ID + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const = 0; + + /// Get the surface normal of a particular sub shape ID and point on surface (all vectors are relative to center of mass for this shape). + /// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetSurfaceNormal will only return face normals (and not vertex or edge normals). + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const = 0; + + /// Type definition for a supporting face + using SupportingFace = StaticArray; + + /// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of + /// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided. + /// @param inSubShapeID Sub shape ID of target shape + /// @param inDirection Direction that the face should be facing (in local space to this shape) + /// @param inCenterOfMassTransform Transform to transform outVertices with + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param outVertices Resulting face. The returned face can be empty if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space. + virtual void GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const { /* Nothing */ } + + /// Get the user data of a particular sub shape ID + virtual uint64 GetSubShapeUserData([[maybe_unused]] const SubShapeID &inSubShapeID) const { return mUserData; } + + /// Get the direct child sub shape and its transform for a sub shape ID. + /// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape + /// @param inPositionCOM The position of the center of mass of this shape + /// @param inRotation The orientation of this shape + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param outRemainder The remainder of the sub shape ID after removing the sub shape + /// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid + virtual TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const; + + /// Gets the properties needed to do buoyancy calculations for a body using this shape + /// @param inCenterOfMassTransform Transform that takes this shape (centered around center of mass) to world space (or a desired other space) + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param inSurface The surface plane of the liquid relative to inCenterOfMassTransform + /// @param outTotalVolume On return this contains the total volume of the shape + /// @param outSubmergedVolume On return this contains the submerged volume of the shape + /// @param outCenterOfBuoyancy On return this contains the world space center of mass of the submerged volume +#ifdef JPH_DEBUG_RENDERER + /// @param inBaseOffset The offset to transform inCenterOfMassTransform to world space (in double precision mode this can be used to shift the whole operation closer to the origin). Only used for debug drawing. +#endif + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const = 0; + +#ifdef JPH_DEBUG_RENDERER + /// Draw the shape at a particular location with a particular color (debugging purposes) + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const = 0; + + /// Draw the results of the GetSupportFunction with the convex radius added back on to show any errors introduced by this process (only relevant for convex shapes) + virtual void DrawGetSupportFunction([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] ColorArg inColor, [[maybe_unused]] bool inDrawSupportDirection) const { /* Only implemented for convex shapes */ } + + /// Draw the results of the GetSupportingFace function to show any errors introduced by this process (only relevant for convex shapes) + virtual void DrawGetSupportingFace([[maybe_unused]] DebugRenderer *inRenderer, [[maybe_unused]] RMat44Arg inCenterOfMassTransform, [[maybe_unused]] Vec3Arg inScale) const { /* Only implemented for convex shapes */ } +#endif // JPH_DEBUG_RENDERER + + /// Cast a ray against this shape, returns true if it finds a hit closer than ioHit.mFraction and updates that fraction. Otherwise ioHit is left untouched and the function returns false. + /// Note that the ray should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from RayCast::mOrigin if you want to cast against the shape in the space it was created). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits against triangles are returned. + /// If you want the surface normal of the hit use GetSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)). + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const = 0; + + /// Cast a ray against this shape. Allows returning multiple hits through ioCollector. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use GetSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected faction)). + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0; + + /// Check if inPoint is inside this shape. For this tests all shapes are treated as if they were solid. + /// Note that inPoint should be relative to the center of mass of this shape (i.e. subtract Shape::GetCenterOfMass() from inPoint if you want to test against the shape in the space it was created). + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit. + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const = 0; + + /// Collides all vertices of a soft body with this shape and updates SoftBodyVertex::mCollisionPlane, SoftBodyVertex::mCollidingShapeIndex and SoftBodyVertex::mLargestPenetration if a collision with more penetration was found. + /// @param inCenterOfMassTransform Center of mass transform for this shape relative to the vertices. + /// @param inScale Scale in local space of the shape (scales relative to its center of mass) + /// @param ioVertices The vertices of the soft body + /// @param inNumVertices The number of vertices in ioVertices + /// @param inDeltaTime Delta time of this time step (can be used to extrapolate the position using the velocity of the particle) + /// @param inDisplacementDueToGravity Displacement due to gravity during this time step + /// @param inCollidingShapeIndex Value to store in SoftBodyVertex::mCollidingShapeIndex when a collision was found + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const = 0; + + /// Collect the leaf transformed shapes of all leaf shapes of this shape. + /// inBox is the world space axis aligned box which leaf shapes should collide with. + /// inPositionCOM/inRotation/inScale describes the transform of this shape. + /// inSubShapeIDCeator represents the current sub shape ID of this shape. + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const; + + /// Transforms this shape and all of its children with inTransform, resulting shape(s) are passed to ioCollector. + /// Note that not all shapes support all transforms (especially true for scaling), the resulting shape will try to match the transform as accurately as possible. + /// @param inCenterOfMassTransform The transform (rotation, translation, scale) that the center of mass of the shape should get + /// @param ioCollector The transformed shapes will be passed to this collector + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const; + + /// Scale this shape. Note that not all shapes support all scales, this will return a shape that matches the scale as accurately as possible. See Shape::IsValidScale for more information. + /// @param inScale The scale to use for this shape (note: this scale is applied to the entire shape in the space it was created, most other functions apply the scale in the space of the leaf shapes and from the center of mass!) + ShapeResult ScaleShape(Vec3Arg inScale) const; + + /// An opaque buffer that holds shape specific information during GetTrianglesStart/Next. + struct alignas(16) GetTrianglesContext { uint8 mData[4288]; }; + + /// This is the minimum amount of triangles that should be requested through GetTrianglesNext. + static constexpr int cGetTrianglesMinTrianglesRequested = 32; + + /// To start iterating over triangles, call this function first. + /// ioContext is a temporary buffer and should remain untouched until the last call to GetTrianglesNext. + /// inBox is the world space bounding in which you want to get the triangles. + /// inPositionCOM/inRotation/inScale describes the transform of this shape. + /// To get the actual triangles call GetTrianglesNext. + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const = 0; + + /// Call this repeatedly to get all triangles in the box. + /// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries. + /// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries. + /// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles. + /// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks). + /// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles. + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const = 0; + + ///@name Binary serialization of the shape. Note that this saves the 'cooked' shape in a format which will not be backwards compatible for newer library versions. + /// In this case you need to recreate the shape from the ShapeSettings object and save it again. The user is expected to call SaveBinaryState followed by SaveMaterialState and SaveSubShapeState. + /// The stream should be stored as is and the material and shape list should be saved using the applications own serialization system (e.g. by assigning an ID to each pointer). + /// When restoring data, call sRestoreFromBinaryState to get the shape and then call RestoreMaterialState and RestoreSubShapeState to restore the pointers to the external objects. + /// Alternatively you can use SaveWithChildren and sRestoreWithChildren to save and restore the shape and all its child shapes and materials in a single stream. + ///@{ + + /// Saves the contents of the shape in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a Shape of the correct type and restores its contents from the binary stream inStream. + static ShapeResult sRestoreFromBinaryState(StreamIn &inStream); + + /// Outputs the material references that this shape has to outMaterials. + virtual void SaveMaterialState([[maybe_unused]] PhysicsMaterialList &outMaterials) const { /* By default do nothing */ } + + /// Restore the material references after calling sRestoreFromBinaryState. Note that the exact same materials need to be provided in the same order as returned by SaveMaterialState. + virtual void RestoreMaterialState([[maybe_unused]] const PhysicsMaterialRefC *inMaterials, [[maybe_unused]] uint inNumMaterials) { JPH_ASSERT(inNumMaterials == 0); } + + /// Outputs the shape references that this shape has to outSubShapes. + virtual void SaveSubShapeState([[maybe_unused]] ShapeList &outSubShapes) const { /* By default do nothing */ } + + /// Restore the shape references after calling sRestoreFromBinaryState. Note that the exact same shapes need to be provided in the same order as returned by SaveSubShapeState. + virtual void RestoreSubShapeState([[maybe_unused]] const ShapeRefC *inSubShapes, [[maybe_unused]] uint inNumShapes) { JPH_ASSERT(inNumShapes == 0); } + + using ShapeToIDMap = StreamUtils::ObjectToIDMap; + using IDToShapeMap = StreamUtils::IDToObjectMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this shape, all its children and its materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + void SaveWithChildren(StreamOut &inStream, ShapeToIDMap &ioShapeMap, MaterialToIDMap &ioMaterialMap) const; + + /// Restore a shape, all its children and materials. Pass in an empty map in ioShapeMap / ioMaterialMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static ShapeResult sRestoreWithChildren(StreamIn &inStream, IDToShapeMap &ioShapeMap, IDToMaterialMap &ioMaterialMap); + + ///@} + + /// Class that holds information about the shape that can be used for logging / data collection purposes + struct Stats + { + Stats(size_t inSizeBytes, uint inNumTriangles) : mSizeBytes(inSizeBytes), mNumTriangles(inNumTriangles) { } + + size_t mSizeBytes; ///< Amount of memory used by this shape (size in bytes) + uint mNumTriangles; ///< Number of triangles in this shape (when applicable) + }; + + /// Get stats of this shape. Use for logging / data collection purposes only. Does not add values from child shapes, use GetStatsRecursive for this. + virtual Stats GetStats() const = 0; + + using VisitedShapes = UnorderedSet; + + /// Get the combined stats of this shape and its children. + /// @param ioVisitedShapes is used to track which shapes have already been visited, to avoid calculating the wrong memory size. + virtual Stats GetStatsRecursive(VisitedShapes &ioVisitedShapes) const; + + ///< Volume of this shape (m^3). Note that for compound shapes the volume may be incorrect since child shapes can overlap which is not accounted for. + virtual float GetVolume() const = 0; + + /// Test if inScale is a valid scale for this shape. Some shapes can only be scaled uniformly, compound shapes cannot handle shapes + /// being rotated and scaled (this would cause shearing), scale can never be zero. When the scale is invalid, the function will return false. + /// + /// Here's a list of supported scales: + /// * SphereShape: Scale must be uniform (signs of scale are ignored). + /// * BoxShape: Any scale supported (signs of scale are ignored). + /// * TriangleShape: Any scale supported when convex radius is zero, otherwise only uniform scale supported. + /// * CapsuleShape: Scale must be uniform (signs of scale are ignored). + /// * TaperedCapsuleShape: Scale must be uniform (sign of Y scale can be used to flip the capsule). + /// * CylinderShape: Scale must be uniform in XZ plane, Y can scale independently (signs of scale are ignored). + /// * RotatedTranslatedShape: Scale must not cause shear in the child shape. + /// * CompoundShape: Scale must not cause shear in any of the child shapes. + virtual bool IsValidScale(Vec3Arg inScale) const { return !inScale.IsNearZero(); } + +#ifdef JPH_DEBUG_RENDERER + /// Debug helper which draws the intersection between water and the shapes, the center of buoyancy and the submerged volume + static bool sDrawSubmergedVolumes; +#endif // JPH_DEBUG_RENDERER + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); + + /// A fallback version of CollidePoint that uses a ray cast and counts the number of hits to determine if the point is inside the shape. Odd number of hits means inside, even number of hits means outside. + static void sCollidePointUsingRayCast(const Shape &inShape, Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter); + +private: + uint64 mUserData = 0; + EShapeType mShapeType; + EShapeSubType mShapeSubType; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.cpp new file mode 100644 index 000000000..28577f7ec --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.cpp @@ -0,0 +1,352 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SphereShapeSettings) +{ + JPH_ADD_BASE_CLASS(SphereShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(SphereShapeSettings, mRadius) +} + +ShapeSettings::ShapeResult SphereShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new SphereShape(*this, mCachedResult); + return mCachedResult; +} + +SphereShape::SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Sphere, inSettings, outResult), + mRadius(inSettings.mRadius) +{ + if (inSettings.mRadius <= 0.0f) + { + outResult.SetError("Invalid radius"); + return; + } + + outResult.Set(this); +} + +float SphereShape::GetScaledRadius(Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + return abs_scale.GetX() * mRadius; +} + +AABox SphereShape::GetLocalBounds() const +{ + Vec3 half_extent = Vec3::sReplicate(mRadius); + return AABox(-half_extent, half_extent); +} + +AABox SphereShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + Vec3 half_extent = Vec3::sReplicate(scaled_radius); + AABox bounds(-half_extent, half_extent); + bounds.Translate(inCenterOfMassTransform.GetTranslation()); + return bounds; +} + +class SphereShape::SphereNoConvex final : public Support +{ +public: + explicit SphereNoConvex(float inRadius) : + mRadius(inRadius) + { + static_assert(sizeof(SphereNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(SphereNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return Vec3::sZero(); + } + + virtual float GetConvexRadius() const override + { + return mRadius; + } + +private: + float mRadius; +}; + +class SphereShape::SphereWithConvex final : public Support +{ +public: + explicit SphereWithConvex(float inRadius) : + mRadius(inRadius) + { + static_assert(sizeof(SphereWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(SphereWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + float len = inDirection.Length(); + return len > 0.0f? (mRadius / len) * inDirection : Vec3::sZero(); + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + float mRadius; +}; + +const ConvexShape::Support *SphereShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) SphereWithConvex(scaled_radius); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + return new (&inBuffer) SphereNoConvex(scaled_radius); + } + + JPH_ASSERT(false); + return nullptr; +} + +MassProperties SphereShape::GetMassProperties() const +{ + MassProperties p; + + // Calculate mass + float r2 = mRadius * mRadius; + p.mMass = (4.0f / 3.0f * JPH_PI) * mRadius * r2 * GetDensity(); + + // Calculate inertia + float inertia = (2.0f / 5.0f) * p.mMass * r2; + p.mInertia = Mat44::sScale(inertia); + + return p; +} + +Vec3 SphereShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + float len = inLocalSurfacePosition.Length(); + return len != 0.0f? inLocalSurfacePosition / len : Vec3::sAxisY(); +} + +void SphereShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + float scaled_radius = GetScaledRadius(inScale); + outTotalVolume = (4.0f / 3.0f * JPH_PI) * Cubed(scaled_radius); + + float distance_to_surface = inSurface.SignedDistance(inCenterOfMassTransform.GetTranslation()); + if (distance_to_surface >= scaled_radius) + { + // Above surface + outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); + } + else if (distance_to_surface <= -scaled_radius) + { + // Under surface + outSubmergedVolume = outTotalVolume; + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation(); + } + else + { + // Intersecting surface + + // Calculate submerged volume, see: https://en.wikipedia.org/wiki/Spherical_cap + float h = scaled_radius - distance_to_surface; + outSubmergedVolume = (JPH_PI / 3.0f) * Square(h) * (3.0f * scaled_radius - h); + + // Calculate center of buoyancy, see: http://mathworld.wolfram.com/SphericalCap.html (eq 10) + float z = (3.0f / 4.0f) * Square(2.0f * scaled_radius - h) / (3.0f * scaled_radius - h); + outCenterOfBuoyancy = inCenterOfMassTransform.GetTranslation() - z * inSurface.GetNormal(); // Negative normal since we want the portion under the water + + #ifdef JPH_DEBUG_RENDERER + // Draw intersection between sphere and water plane + if (sDrawSubmergedVolumes) + { + Vec3 circle_center = inCenterOfMassTransform.GetTranslation() - distance_to_surface * inSurface.GetNormal(); + float circle_radius = sqrt(Square(scaled_radius) - Square(distance_to_surface)); + DebugRenderer::sInstance->DrawPie(inBaseOffset + circle_center, circle_radius, inSurface.GetNormal(), inSurface.GetNormal().GetNormalizedPerpendicular(), -JPH_PI, JPH_PI, Color::sGreen, DebugRenderer::ECastShadow::Off); + } + #endif // JPH_DEBUG_RENDERER + } + +#ifdef JPH_DEBUG_RENDERER + // Draw center of buoyancy + if (sDrawSubmergedVolumes) + DebugRenderer::sInstance->DrawWireSphere(inBaseOffset + outCenterOfBuoyancy, 0.05f, Color::sRed, 1); +#endif // JPH_DEBUG_RENDERER +} + +#ifdef JPH_DEBUG_RENDERER +void SphereShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + inRenderer->DrawUnitSphere(inCenterOfMassTransform * Mat44::sScale(mRadius * inScale.Abs().GetX()), inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +bool SphereShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + float fraction = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void SphereShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + float min_fraction, max_fraction; + int num_results = RaySphere(inRay.mOrigin, inRay.mDirection, Vec3::sZero(), mRadius, min_fraction, max_fraction); + if (num_results > 0 // Ray should intersect + && max_fraction >= 0.0f // End of ray should be inside sphere + && min_fraction < ioCollector.GetEarlyOutFraction()) // Start of ray should be before early out fraction + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + + // Check front side hit + if (inRayCastSettings.mTreatConvexAsSolid || min_fraction > 0.0f) + { + hit.mFraction = max(0.0f, min_fraction); + ioCollector.AddHit(hit); + } + + // Check back side hit + if (inRayCastSettings.mBackFaceMode == EBackFaceMode::CollideWithBackFaces + && num_results > 1 // Ray should have 2 intersections + && max_fraction < ioCollector.GetEarlyOutFraction()) // End of ray should be before early out fraction + { + hit.mFraction = max_fraction; + ioCollector.AddHit(hit); + } + } +} + +void SphereShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + if (inPoint.LengthSq() <= Square(mRadius)) + ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() }); +} + +void SphereShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + Vec3 center = inCenterOfMassTransform.GetTranslation(); + float radius = GetScaledRadius(inScale); + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + // Calculate penetration + Vec3 delta = v->mPosition - center; + float distance = delta.Length(); + float penetration = radius - distance; + if (penetration > v->mLargestPenetration) + { + v->mLargestPenetration = penetration; + + // Calculate contact point and normal + Vec3 normal = distance > 0.0f? delta / distance : Vec3::sAxisY(); + Vec3 point = center + radius * normal; + + // Store collision + v->mCollisionPlane = Plane::sFromPointAndNormal(point, normal); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } +} + +void SphereShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + ts.SetShapeScale(ScaleHelpers::MakeUniformScale(scale.Abs())); + ioCollector.AddHit(ts); +} + +void SphereShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + float scaled_radius = GetScaledRadius(inScale); + new (&ioContext) GetTrianglesContextVertexList(inPositionCOM, inRotation, Vec3::sReplicate(1.0f), Mat44::sScale(scaled_radius), sUnitSphereTriangles.data(), sUnitSphereTriangles.size(), GetMaterial()); +} + +int SphereShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + return ((GetTrianglesContextVertexList &)ioContext).GetTrianglesNext(inMaxTrianglesRequested, outTriangleVertices, outMaterials); +} + +void SphereShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mRadius); +} + +void SphereShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mRadius); +} + +bool SphereShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +void SphereShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Sphere); + f.mConstruct = []() -> Shape * { return new SphereShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.h new file mode 100644 index 000000000..9d976948a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/SphereShape.h @@ -0,0 +1,125 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a SphereShape +class JPH_EXPORT SphereShapeSettings final : public ConvexShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SphereShapeSettings) + + /// Default constructor for deserialization + SphereShapeSettings() = default; + + /// Create a sphere with radius inRadius + SphereShapeSettings(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mRadius(inRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mRadius = 0.0f; +}; + +/// A sphere, centered around the origin. +/// Note that it is implemented as a point with convex radius. +class JPH_EXPORT SphereShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SphereShape() : ConvexShape(EShapeSubType::Sphere) { } + SphereShape(const SphereShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a sphere with radius inRadius + SphereShape(float inRadius, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Sphere, inMaterial), mRadius(inRadius) { JPH_ASSERT(inRadius > 0.0f); } + + /// Radius of the sphere + float GetRadius() const { return mRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace([[maybe_unused]] const SubShapeID &inSubShapeID, [[maybe_unused]] Vec3Arg inDirection, [[maybe_unused]] Vec3Arg inScale, [[maybe_unused]] Mat44Arg inCenterOfMassTransform, [[maybe_unused]] SupportingFace &outVertices) const override { /* Hit is always a single point, no point in returning anything */ } + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 4.0f / 3.0f * JPH_PI * Cubed(mRadius); } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Get the radius of this sphere scaled by inScale + inline float GetScaledRadius(Vec3Arg inScale) const; + + // Classes for GetSupportFunction + class SphereNoConvex; + class SphereWithConvex; + + float mRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp new file mode 100644 index 000000000..745581750 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.cpp @@ -0,0 +1,674 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(StaticCompoundShapeSettings) +{ + JPH_ADD_BASE_CLASS(StaticCompoundShapeSettings, CompoundShapeSettings) +} + +ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create(TempAllocator &inTempAllocator) const +{ + if (mCachedResult.IsEmpty()) + { + if (mSubShapes.size() == 0) + { + // It's an error to create a compound with no subshapes (the compound cannot encode this) + mCachedResult.SetError("Compound needs a sub shape!"); + } + else if (mSubShapes.size() == 1) + { + // If there's only 1 part we don't need a StaticCompoundShape + const SubShapeSettings &s = mSubShapes[0]; + if (s.mPosition == Vec3::sZero() + && s.mRotation == Quat::sIdentity()) + { + // No rotation or translation, we can use the shape directly + if (s.mShapePtr != nullptr) + mCachedResult.Set(const_cast(s.mShapePtr.GetPtr())); + else if (s.mShape != nullptr) + mCachedResult = s.mShape->Create(); + else + mCachedResult.SetError("Sub shape is null!"); + } + else + { + // We can use a RotatedTranslatedShape instead + RotatedTranslatedShapeSettings settings; + settings.mPosition = s.mPosition; + settings.mRotation = s.mRotation; + settings.mInnerShape = s.mShape; + settings.mInnerShapePtr = s.mShapePtr; + Ref shape = new RotatedTranslatedShape(settings, mCachedResult); + } + } + else + { + // Build a regular compound shape + Ref shape = new StaticCompoundShape(*this, inTempAllocator, mCachedResult); + } + } + return mCachedResult; +} + +ShapeSettings::ShapeResult StaticCompoundShapeSettings::Create() const +{ + TempAllocatorMalloc allocator; + return Create(allocator); +} + +void StaticCompoundShape::Node::SetChildInvalid(uint inIndex) +{ + // Make this an invalid node + mNodeProperties[inIndex] = INVALID_NODE; + + // Make bounding box invalid + mBoundsMinX[inIndex] = HALF_FLT_MAX; + mBoundsMinY[inIndex] = HALF_FLT_MAX; + mBoundsMinZ[inIndex] = HALF_FLT_MAX; + mBoundsMaxX[inIndex] = HALF_FLT_MAX; + mBoundsMaxY[inIndex] = HALF_FLT_MAX; + mBoundsMaxZ[inIndex] = HALF_FLT_MAX; +} + +void StaticCompoundShape::Node::SetChildBounds(uint inIndex, const AABox &inBounds) +{ + mBoundsMinX[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetX()); + mBoundsMinY[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetY()); + mBoundsMinZ[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMin.GetZ()); + mBoundsMaxX[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetX()); + mBoundsMaxY[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetY()); + mBoundsMaxZ[inIndex] = HalfFloatConversion::FromFloat(inBounds.mMax.GetZ()); +} + +void StaticCompoundShape::sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint) +{ + // Handle trivial case + if (inNumber <= 4) + { + outMidPoint = inNumber / 2; + return; + } + + // Calculate bounding box of box centers + Vec3 center_min = Vec3::sReplicate(FLT_MAX); + Vec3 center_max = Vec3::sReplicate(-FLT_MAX); + for (const AABox *b = ioBounds, *b_end = ioBounds + inNumber; b < b_end; ++b) + { + Vec3 center = b->GetCenter(); + center_min = Vec3::sMin(center_min, center); + center_max = Vec3::sMax(center_max, center); + } + + // Calculate split plane + int dimension = (center_max - center_min).GetHighestComponentIndex(); + float split = 0.5f * (center_min + center_max)[dimension]; + + // Divide bodies + int start = 0, end = inNumber; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && ioBounds[start].GetCenter()[dimension] < split) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && ioBounds[end - 1].GetCenter()[dimension] >= split) + --end; + + if (start < end) + { + // Swap the two elements + swap(ioBodyIdx[start], ioBodyIdx[end - 1]); + swap(ioBounds[start], ioBounds[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + + if (start > 0 && start < inNumber) + { + // Success! + outMidPoint = start; + } + else + { + // Failed to divide bodies + outMidPoint = inNumber / 2; + } +} + +void StaticCompoundShape::sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit) +{ + uint *body_idx = ioBodyIdx + inBegin; + AABox *node_bounds = ioBounds + inBegin; + int number = inEnd - inBegin; + + // Partition entire range + sPartition(body_idx, node_bounds, number, outSplit[2]); + + // Partition lower half + sPartition(body_idx, node_bounds, outSplit[2], outSplit[1]); + + // Partition upper half + sPartition(body_idx + outSplit[2], node_bounds + outSplit[2], number - outSplit[2], outSplit[3]); + + // Convert to proper range + outSplit[0] = inBegin; + outSplit[1] += inBegin; + outSplit[2] += inBegin; + outSplit[3] += outSplit[2]; + outSplit[4] = inEnd; +} + +StaticCompoundShape::StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult) : + CompoundShape(EShapeSubType::StaticCompound, inSettings, outResult) +{ + // Check that there's at least 1 shape + uint num_subshapes = (uint)inSettings.mSubShapes.size(); + if (num_subshapes < 2) + { + outResult.SetError("Compound needs at least 2 sub shapes, otherwise you should use a RotatedTranslatedShape!"); + return; + } + + // Keep track of total mass to calculate center of mass + float mass = 0.0f; + + mSubShapes.resize(num_subshapes); + for (uint i = 0; i < num_subshapes; ++i) + { + const CompoundShapeSettings::SubShapeSettings &shape = inSettings.mSubShapes[i]; + + // Start constructing the runtime sub shape + SubShape &out_shape = mSubShapes[i]; + if (!out_shape.FromSettings(shape, outResult)) + return; + + // Calculate mass properties of child + MassProperties child = out_shape.mShape->GetMassProperties(); + + // Accumulate center of mass + mass += child.mMass; + mCenterOfMass += out_shape.GetPositionCOM() * child.mMass; + } + + if (mass > 0.0f) + mCenterOfMass /= mass; + + // Cache the inner radius as it can take a while to recursively iterate over all sub shapes + CalculateInnerRadius(); + + // Temporary storage for the bounding boxes of all shapes + uint bounds_size = num_subshapes * sizeof(AABox); + AABox *bounds = (AABox *)inTempAllocator.Allocate(bounds_size); + JPH_SCOPE_EXIT([&inTempAllocator, bounds, bounds_size]{ inTempAllocator.Free(bounds, bounds_size); }); + + // Temporary storage for body indexes (we're shuffling them) + uint body_idx_size = num_subshapes * sizeof(uint); + uint *body_idx = (uint *)inTempAllocator.Allocate(body_idx_size); + JPH_SCOPE_EXIT([&inTempAllocator, body_idx, body_idx_size]{ inTempAllocator.Free(body_idx, body_idx_size); }); + + // Shift all shapes so that the center of mass is now at the origin and calculate bounds + for (uint i = 0; i < num_subshapes; ++i) + { + SubShape &shape = mSubShapes[i]; + + // Shift the shape so it's centered around our center of mass + shape.SetPositionCOM(shape.GetPositionCOM() - mCenterOfMass); + + // Transform the shape's bounds into our local space + Mat44 transform = Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()); + AABox shape_bounds = shape.mShape->GetWorldSpaceBounds(transform, Vec3::sReplicate(1.0f)); + + // Store bounds and body index for tree construction + bounds[i] = shape_bounds; + body_idx[i] = i; + + // Update our local bounds + mLocalBounds.Encapsulate(shape_bounds); + } + + // The algorithm is a recursive tree build, but to avoid the call overhead we keep track of a stack here + struct StackEntry + { + uint32 mNodeIdx; // Node index of node that is generated + int mChildIdx; // Index of child that we're currently processing + int mSplit[5]; // Indices where the node ID's have been split to form 4 partitions + AABox mBounds; // Bounding box of this node + }; + uint stack_size = num_subshapes * sizeof(StackEntry); + StackEntry *stack = (StackEntry *)inTempAllocator.Allocate(stack_size); + JPH_SCOPE_EXIT([&inTempAllocator, stack, stack_size]{ inTempAllocator.Free(stack, stack_size); }); + int top = 0; + + // Reserve enough space so that every sub shape gets its own leaf node + uint next_node_idx = 0; + mNodes.resize(num_subshapes + (num_subshapes + 2) / 3); // = Sum(num_subshapes * 4^-i) with i = [0, Inf]. + + // Create root node + stack[0].mNodeIdx = next_node_idx++; + stack[0].mChildIdx = -1; + stack[0].mBounds = AABox(); + sPartition4(body_idx, bounds, 0, num_subshapes, stack[0].mSplit); + + for (;;) + { + StackEntry &cur_stack = stack[top]; + + // Next child + cur_stack.mChildIdx++; + + // Check if all children processed + if (cur_stack.mChildIdx >= 4) + { + // Terminate if there's nothing left to pop + if (top <= 0) + break; + + // Add our bounds to our parents bounds + StackEntry &prev_stack = stack[top - 1]; + prev_stack.mBounds.Encapsulate(cur_stack.mBounds); + + // Store this node's properties in the parent node + Node &parent_node = mNodes[prev_stack.mNodeIdx]; + parent_node.mNodeProperties[prev_stack.mChildIdx] = cur_stack.mNodeIdx; + parent_node.SetChildBounds(prev_stack.mChildIdx, cur_stack.mBounds); + + // Pop entry from stack + --top; + } + else + { + // Get low and high index to bodies to process + int low = cur_stack.mSplit[cur_stack.mChildIdx]; + int high = cur_stack.mSplit[cur_stack.mChildIdx + 1]; + int num_bodies = high - low; + + if (num_bodies == 0) + { + // Mark invalid + Node &node = mNodes[cur_stack.mNodeIdx]; + node.SetChildInvalid(cur_stack.mChildIdx); + } + else if (num_bodies == 1) + { + // Get body info + uint child_node_idx = body_idx[low]; + const AABox &child_bounds = bounds[low]; + + // Update node + Node &node = mNodes[cur_stack.mNodeIdx]; + node.mNodeProperties[cur_stack.mChildIdx] = child_node_idx | IS_SUBSHAPE; + node.SetChildBounds(cur_stack.mChildIdx, child_bounds); + + // Encapsulate bounding box in parent + cur_stack.mBounds.Encapsulate(child_bounds); + } + else + { + // Allocate new node + StackEntry &new_stack = stack[++top]; + JPH_ASSERT(top < (int)num_subshapes); + new_stack.mNodeIdx = next_node_idx++; + new_stack.mChildIdx = -1; + new_stack.mBounds = AABox(); + sPartition4(body_idx, bounds, low, high, new_stack.mSplit); + } + } + } + + // Resize nodes to actual size + JPH_ASSERT(next_node_idx <= mNodes.size()); + mNodes.resize(next_node_idx); + mNodes.shrink_to_fit(); + + // Check if we ran out of bits for addressing a node + if (next_node_idx > IS_SUBSHAPE) + { + outResult.SetError("Compound hierarchy has too many nodes"); + return; + } + + // Check if we're not exceeding the amount of sub shape id bits + if (GetSubShapeIDBitsRecursive() > SubShapeID::MaxBits) + { + outResult.SetError("Compound hierarchy is too deep and exceeds the amount of available sub shape ID bits"); + return; + } + + outResult.Set(this); +} + +template +inline void StaticCompoundShape::WalkTree(Visitor &ioVisitor) const +{ + uint32 node_stack[cStackSize]; + node_stack[0] = 0; + int top = 0; + do + { + // Test if the node is valid, the node should rarely be invalid but it is possible when testing + // a really large box against the tree that the invalid nodes will intersect with the box + uint32 node_properties = node_stack[top]; + if (node_properties != INVALID_NODE) + { + // Test if node contains triangles + bool is_node = (node_properties & IS_SUBSHAPE) == 0; + if (is_node) + { + const Node &node = mNodes[node_properties]; + + // Unpack bounds + UVec4 bounds_minxy = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMinX[0])); + Vec4 bounds_minx = HalfFloatConversion::ToFloat(bounds_minxy); + Vec4 bounds_miny = HalfFloatConversion::ToFloat(bounds_minxy.Swizzle()); + + UVec4 bounds_minzmaxx = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMinZ[0])); + Vec4 bounds_minz = HalfFloatConversion::ToFloat(bounds_minzmaxx); + Vec4 bounds_maxx = HalfFloatConversion::ToFloat(bounds_minzmaxx.Swizzle()); + + UVec4 bounds_maxyz = UVec4::sLoadInt4(reinterpret_cast(&node.mBoundsMaxY[0])); + Vec4 bounds_maxy = HalfFloatConversion::ToFloat(bounds_maxyz); + Vec4 bounds_maxz = HalfFloatConversion::ToFloat(bounds_maxyz.Swizzle()); + + // Load properties for 4 children + UVec4 properties = UVec4::sLoadInt4(&node.mNodeProperties[0]); + + // Check which sub nodes to visit + int num_results = ioVisitor.VisitNodes(bounds_minx, bounds_miny, bounds_minz, bounds_maxx, bounds_maxy, bounds_maxz, properties, top); + + // Push them onto the stack + JPH_ASSERT(top + 4 < cStackSize); + properties.StoreInt4(&node_stack[top]); + top += num_results; + } + else + { + // Points to a sub shape + uint32 sub_shape_idx = node_properties ^ IS_SUBSHAPE; + const SubShape &sub_shape = mSubShapes[sub_shape_idx]; + + ioVisitor.VisitShape(sub_shape, sub_shape_idx); + } + + // Check if we're done + if (ioVisitor.ShouldAbort()) + break; + } + + // Fetch next node until we find one that the visitor wants to see + do + --top; + while (top >= 0 && !ioVisitor.ShouldVisitNode(top)); + } + while (top >= 0); +} + +bool StaticCompoundShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitor + { + using CastRayVisitor::CastRayVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mHit.mFraction; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mHit.mFraction, ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inRay, this, inSubShapeIDCreator, ioHit); + WalkTree(visitor); + return visitor.mReturnValue; +} + +void StaticCompoundShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastRayVisitorCollector + { + using CastRayVisitorCollector::CastRayVisitorCollector; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + Visitor visitor(inRay, inRayCastSettings, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +void StaticCompoundShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollidePointVisitor + { + using CollidePointVisitor::CollidePointVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test if point overlaps with box + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(inPoint, this, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +void StaticCompoundShape::sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CastShapeVisitor + { + using CastShapeVisitor::CastShapeVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return mDistanceStack[inStackTop] < mCollector.GetPositiveEarlyOutFraction(); + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test bounds of 4 children + Vec4 distance = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + return SortReverseAndStore(distance, mCollector.GetPositiveEarlyOutFraction(), ioProperties, &mDistanceStack[inStackTop]); + } + + float mDistanceStack[cStackSize]; + }; + + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape = static_cast(inShape); + + Visitor visitor(inShapeCast, inShapeCastSettings, shape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector); + shape->WalkTree(visitor); +} + +void StaticCompoundShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + struct Visitor : public CollectTransformedShapesVisitor + { + using CollectTransformedShapesVisitor::CollectTransformedShapesVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(inBox, this, inPositionCOM, inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter); + WalkTree(visitor); +} + +int StaticCompoundShape::GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorSC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkTree(visitor); + return visitor.GetNumResults(); +} + +int StaticCompoundShape::GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const +{ + JPH_PROFILE_FUNCTION(); + + GetIntersectingSubShapesVisitorSC visitor(inBox, outSubShapeIndices, inMaxSubShapeIndices); + WalkTree(visitor); + return visitor.GetNumResults(); +} + +void StaticCompoundShape::sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape1 = static_cast(inShape1); + + struct Visitor : public CollideCompoundVsShapeVisitor + { + using CollideCompoundVsShapeVisitor::CollideCompoundVsShapeVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + Visitor visitor(shape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape1->WalkTree(visitor); +} + +void StaticCompoundShape::sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) +{ + JPH_PROFILE_FUNCTION(); + + struct Visitor : public CollideShapeVsCompoundVisitor + { + using CollideShapeVsCompoundVisitor::CollideShapeVsCompoundVisitor; + + JPH_INLINE bool ShouldVisitNode([[maybe_unused]] int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, [[maybe_unused]] int inStackTop) const + { + // Test which nodes collide + UVec4 collides = TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::StaticCompound); + const StaticCompoundShape *shape2 = static_cast(inShape2); + + Visitor visitor(inShape1, shape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter); + shape2->WalkTree(visitor); +} + +void StaticCompoundShape::SaveBinaryState(StreamOut &inStream) const +{ + CompoundShape::SaveBinaryState(inStream); + + inStream.Write(mNodes); +} + +void StaticCompoundShape::RestoreBinaryState(StreamIn &inStream) +{ + CompoundShape::RestoreBinaryState(inStream); + + inStream.Read(mNodes); +} + +void StaticCompoundShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::StaticCompound); + f.mConstruct = []() -> Shape * { return new StaticCompoundShape; }; + f.mColor = Color::sOrange; + + for (EShapeSubType s : sAllSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(EShapeSubType::StaticCompound, s, sCollideCompoundVsShape); + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::StaticCompound, sCollideShapeVsCompound); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::StaticCompound, sCastShapeVsCompound); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.h new file mode 100644 index 000000000..c3b7b7e76 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/StaticCompoundShape.h @@ -0,0 +1,139 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class CollideShapeSettings; +class TempAllocator; + +/// Class that constructs a StaticCompoundShape. Note that if you only want a compound of 1 shape, use a RotatedTranslatedShape instead. +class JPH_EXPORT StaticCompoundShapeSettings final : public CompoundShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, StaticCompoundShapeSettings) + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + /// Specialization of Create() function that allows specifying a temp allocator to avoid temporary memory allocations on the heap + ShapeResult Create(TempAllocator &inTempAllocator) const; +}; + +/// A compound shape, sub shapes can be rotated and translated. +/// Sub shapes cannot be modified once the shape is constructed. +/// Shifts all child objects so that they're centered around the center of mass. +class JPH_EXPORT StaticCompoundShape final : public CompoundShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + StaticCompoundShape() : CompoundShape(EShapeSubType::StaticCompound) { } + StaticCompoundShape(const StaticCompoundShapeSettings &inSettings, TempAllocator &inTempAllocator, ShapeResult &outResult); + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See Shape::CollectTransformedShapes + virtual void CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const AABox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See: CompoundShape::GetIntersectingSubShapes + virtual int GetIntersectingSubShapes(const OrientedBox &inBox, uint *outSubShapeIndices, int inMaxSubShapeIndices) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this) + mSubShapes.size() * sizeof(SubShape) + mNodes.size() * sizeof(Node), 0); } + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Visitor for GetIntersectingSubShapes + template + struct GetIntersectingSubShapesVisitorSC : public GetIntersectingSubShapesVisitor + { + using GetIntersectingSubShapesVisitor::GetIntersectingSubShapesVisitor; + + JPH_INLINE bool ShouldVisitNode(int inStackTop) const + { + return true; + } + + JPH_INLINE int VisitNodes(Vec4Arg inBoundsMinX, Vec4Arg inBoundsMinY, Vec4Arg inBoundsMinZ, Vec4Arg inBoundsMaxX, Vec4Arg inBoundsMaxY, Vec4Arg inBoundsMaxZ, UVec4 &ioProperties, int inStackTop) + { + // Test if point overlaps with box + UVec4 collides = GetIntersectingSubShapesVisitor::TestBounds(inBoundsMinX, inBoundsMinY, inBoundsMinZ, inBoundsMaxX, inBoundsMaxY, inBoundsMaxZ); + return CountAndSortTrues(collides, ioProperties); + } + }; + + /// Sorts ioBodyIdx spatially into 2 groups. Second groups starts at ioBodyIdx + outMidPoint. + /// After the function returns ioBodyIdx and ioBounds will be shuffled + static void sPartition(uint *ioBodyIdx, AABox *ioBounds, int inNumber, int &outMidPoint); + + /// Sorts ioBodyIdx from inBegin to (but excluding) inEnd spatially into 4 groups. + /// outSplit needs to be 5 ints long, when the function returns each group runs from outSplit[i] to (but excluding) outSplit[i + 1] + /// After the function returns ioBodyIdx and ioBounds will be shuffled + static void sPartition4(uint *ioBodyIdx, AABox *ioBounds, int inBegin, int inEnd, int *outSplit); + + // Helper functions called by CollisionDispatch + static void sCollideCompoundVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideShapeVsCompound(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastShapeVsCompound(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Maximum size of the stack during tree walk + static constexpr int cStackSize = 128; + + template + JPH_INLINE void WalkTree(Visitor &ioVisitor) const; ///< Walk the node tree calling the Visitor::VisitNodes for each node encountered and Visitor::VisitShape for each sub shape encountered + + /// Bits used in Node::mNodeProperties + enum : uint32 + { + IS_SUBSHAPE = 0x80000000, ///< If this bit is set, the other bits index in mSubShape, otherwise in mNodes + INVALID_NODE = 0x7fffffff, ///< Signifies an invalid node + }; + + /// Node structure + struct Node + { + void SetChildBounds(uint inIndex, const AABox &inBounds); ///< Set bounding box for child inIndex to inBounds + void SetChildInvalid(uint inIndex); ///< Mark the child inIndex as invalid and set its bounding box to invalid + + HalfFloat mBoundsMinX[4]; ///< 4 child bounding boxes + HalfFloat mBoundsMinY[4]; + HalfFloat mBoundsMinZ[4]; + HalfFloat mBoundsMaxX[4]; + HalfFloat mBoundsMaxY[4]; + HalfFloat mBoundsMaxZ[4]; + uint32 mNodeProperties[4]; ///< 4 child node properties + }; + + static_assert(sizeof(Node) == 64, "Node should be 64 bytes"); + + using Nodes = Array; + + Nodes mNodes; ///< Quad tree node structure +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/SubShapeID.h b/WickedEngine/Jolt/Physics/Collision/Shape/SubShapeID.h new file mode 100644 index 000000000..f1e8d3713 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/SubShapeID.h @@ -0,0 +1,138 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// @brief A sub shape id contains a path to an element (usually a triangle or other primitive type) of a compound shape +/// +/// Each sub shape knows how many bits it needs to encode its ID, so knows how many bits to take from the sub shape ID. +/// +/// For example: +/// * We have a CompoundShape A with 5 child shapes (identify sub shape using 3 bits AAA) +/// * One of its child shapes is CompoundShape B which has 3 child shapes (identify sub shape using 2 bits BB) +/// * One of its child shapes is MeshShape C which contains enough triangles to need 7 bits to identify a triangle (identify sub shape using 7 bits CCCCCCC, note that MeshShape is block based and sorts triangles spatially, you can't assume that the first triangle will have bit pattern 0000000). +/// +/// The bit pattern of the sub shape ID to identify a triangle in MeshShape C will then be CCCCCCCBBAAA. +/// +/// A sub shape ID will become invalid when the structure of the shape changes. For example, if a child shape is removed from a compound shape, the sub shape ID will no longer be valid. +/// This can be a problem when caching sub shape IDs from one frame to the next. See comments at ContactListener::OnContactPersisted / OnContactRemoved. +class SubShapeID +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Underlying storage type + using Type = uint32; + + /// Type that is bigger than the underlying storage type for operations that would otherwise overflow + using BiggerType = uint64; + + static_assert(sizeof(BiggerType) > sizeof(Type), "The calculation below assumes BiggerType is a bigger type than Type"); + + /// How many bits we can store in this ID + static constexpr uint MaxBits = 8 * sizeof(Type); + + /// Constructor + SubShapeID() = default; + + /// Get the next id in the chain of ids (pops parents before children) + Type PopID(uint inBits, SubShapeID &outRemainder) const + { + Type mask_bits = Type((BiggerType(1) << inBits) - 1); + Type fill_bits = Type(BiggerType(cEmpty) << (MaxBits - inBits)); // Fill left side bits with 1 so that if there's no remainder all bits will be set, note that we do this using a BiggerType since on intel 0xffffffff << 32 == 0xffffffff + Type v = mValue & mask_bits; + outRemainder = SubShapeID(Type(BiggerType(mValue) >> inBits) | fill_bits); + return v; + } + + /// Get the value of the path to the sub shape ID + inline Type GetValue() const + { + return mValue; + } + + /// Set the value of the sub shape ID (use with care!) + inline void SetValue(Type inValue) + { + mValue = inValue; + } + + /// Check if there is any bits of subshape ID left. + /// Note that this is not a 100% guarantee as the subshape ID could consist of all 1 bits. Use for asserts only. + inline bool IsEmpty() const + { + return mValue == cEmpty; + } + + /// Check equal + inline bool operator == (const SubShapeID &inRHS) const + { + return mValue == inRHS.mValue; + } + + /// Check not-equal + inline bool operator != (const SubShapeID &inRHS) const + { + return mValue != inRHS.mValue; + } + +private: + friend class SubShapeIDCreator; + + /// An empty SubShapeID has all bits set + static constexpr Type cEmpty = ~Type(0); + + /// Constructor + explicit SubShapeID(const Type &inValue) : mValue(inValue) { } + + /// Adds an id at a particular position in the chain + /// (this should really only be called by the SubShapeIDCreator) + void PushID(Type inValue, uint inFirstBit, uint inBits) + { + // First clear the bits + mValue &= ~(Type((BiggerType(1) << inBits) - 1) << inFirstBit); + + // Then set them to the new value + mValue |= inValue << inFirstBit; + } + + Type mValue = cEmpty; +}; + +/// A sub shape id creator can be used to create a new sub shape id by recursing through the shape +/// hierarchy and pushing new ID's onto the chain +class SubShapeIDCreator +{ +public: + /// Add a new id to the chain of id's and return it + SubShapeIDCreator PushID(uint inValue, uint inBits) const + { + JPH_ASSERT(inValue < (SubShapeID::BiggerType(1) << inBits)); + SubShapeIDCreator copy = *this; + copy.mID.PushID(inValue, mCurrentBit, inBits); + copy.mCurrentBit += inBits; + JPH_ASSERT(copy.mCurrentBit <= SubShapeID::MaxBits); + return copy; + } + + // Get the resulting sub shape ID + const SubShapeID & GetID() const + { + return mID; + } + + /// Get the number of bits that have been written to the sub shape ID so far + inline uint GetNumBitsWritten() const + { + return mCurrentBit; + } + +private: + SubShapeID mID; + uint mCurrentBit = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/SubShapeIDPair.h b/WickedEngine/Jolt/Physics/Collision/Shape/SubShapeIDPair.h new file mode 100644 index 000000000..ccad840b6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/SubShapeIDPair.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A pair of bodies and their sub shape ID's. Can be used as a key in a map to find a contact point. +class SubShapeIDPair +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SubShapeIDPair() = default; + SubShapeIDPair(const BodyID &inBody1ID, const SubShapeID &inSubShapeID1, const BodyID &inBody2ID, const SubShapeID &inSubShapeID2) : mBody1ID(inBody1ID), mSubShapeID1(inSubShapeID1), mBody2ID(inBody2ID), mSubShapeID2(inSubShapeID2) { } + SubShapeIDPair & operator = (const SubShapeIDPair &) = default; + SubShapeIDPair(const SubShapeIDPair &) = default; + + /// Equality operator + inline bool operator == (const SubShapeIDPair &inRHS) const + { + return UVec4::sLoadInt4(reinterpret_cast(this)) == UVec4::sLoadInt4(reinterpret_cast(&inRHS)); + } + + /// Less than operator, used to consistently order contact points for a deterministic simulation + inline bool operator < (const SubShapeIDPair &inRHS) const + { + if (mBody1ID != inRHS.mBody1ID) + return mBody1ID < inRHS.mBody1ID; + + if (mSubShapeID1.GetValue() != inRHS.mSubShapeID1.GetValue()) + return mSubShapeID1.GetValue() < inRHS.mSubShapeID1.GetValue(); + + if (mBody2ID != inRHS.mBody2ID) + return mBody2ID < inRHS.mBody2ID; + + return mSubShapeID2.GetValue() < inRHS.mSubShapeID2.GetValue(); + } + + const BodyID & GetBody1ID() const { return mBody1ID; } + const SubShapeID & GetSubShapeID1() const { return mSubShapeID1; } + const BodyID & GetBody2ID() const { return mBody2ID; } + const SubShapeID & GetSubShapeID2() const { return mSubShapeID2; } + + uint64 GetHash() const { return HashBytes(this, sizeof(SubShapeIDPair)); } + +private: + BodyID mBody1ID; + SubShapeID mSubShapeID1; + BodyID mBody2ID; + SubShapeID mSubShapeID2; +}; + +static_assert(sizeof(SubShapeIDPair) == 16, "Unexpected size"); +static_assert(alignof(SubShapeIDPair) == 4, "Assuming 4 byte aligned"); + +JPH_NAMESPACE_END + +JPH_SUPPRESS_WARNINGS_STD_BEGIN + +namespace std +{ + /// Declare std::hash for SubShapeIDPair, note that std::hash is platform dependent and we need this one to be consistent because we sort on it in the ContactConstraintManager + template <> + struct hash + { + inline size_t operator () (const JPH::SubShapeIDPair &inRHS) const + { + return static_cast(inRHS.GetHash()); + } + }; +} + +JPH_SUPPRESS_WARNINGS_STD_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp new file mode 100644 index 000000000..983864185 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.cpp @@ -0,0 +1,458 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCapsuleShapeSettings) +{ + JPH_ADD_BASE_CLASS(TaperedCapsuleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mHalfHeightOfTaperedCylinder) + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mTopRadius) + JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mBottomRadius) +} + +bool TaperedCapsuleShapeSettings::IsSphere() const +{ + return max(mTopRadius, mBottomRadius) >= 2.0f * mHalfHeightOfTaperedCylinder + min(mTopRadius, mBottomRadius); +} + +ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + { + Ref shape; + if (IsValid() && IsSphere()) + { + // Determine sphere center and radius + float radius, center; + if (mTopRadius > mBottomRadius) + { + radius = mTopRadius; + center = mHalfHeightOfTaperedCylinder; + } + else + { + radius = mBottomRadius; + center = -mHalfHeightOfTaperedCylinder; + } + + // Create sphere + shape = new SphereShape(radius, mMaterial); + + // Offset sphere if needed + if (abs(center) > 1.0e-6f) + { + RotatedTranslatedShapeSettings rot_trans(Vec3(0, center, 0), Quat::sIdentity(), shape); + mCachedResult = rot_trans.Create(); + } + else + mCachedResult.Set(shape); + } + else + { + // Normal tapered capsule shape + shape = new TaperedCapsuleShape(*this, mCachedResult); + } + } + return mCachedResult; +} + +TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) : + ConvexShapeSettings(inMaterial), + mHalfHeightOfTaperedCylinder(inHalfHeightOfTaperedCylinder), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius) +{ +} + +TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::TaperedCapsule, inSettings, outResult), + mTopRadius(inSettings.mTopRadius), + mBottomRadius(inSettings.mBottomRadius) +{ + if (mTopRadius <= 0.0f) + { + outResult.SetError("Invalid top radius"); + return; + } + + if (mBottomRadius <= 0.0f) + { + outResult.SetError("Invalid bottom radius"); + return; + } + + if (inSettings.mHalfHeightOfTaperedCylinder <= 0.0f) + { + outResult.SetError("Invalid height"); + return; + } + + // If this goes off one of the sphere ends falls totally inside the other and you should use a sphere instead + if (inSettings.IsSphere()) + { + outResult.SetError("One sphere embedded in other sphere, please use sphere shape instead"); + return; + } + + // Approximation: The center of mass is exactly half way between the top and bottom cap of the tapered capsule + mTopCenter = inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius); + mBottomCenter = -inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius); + + // Calculate center of mass + mCenterOfMass = Vec3(0, inSettings.mHalfHeightOfTaperedCylinder - mTopCenter, 0); + + // Calculate convex radius + mConvexRadius = min(mTopRadius, mBottomRadius); + JPH_ASSERT(mConvexRadius > 0.0f); + + // Calculate the sin and tan of the angle that the cone surface makes with the Y axis + // See: TaperedCapsuleShape.gliffy + mSinAlpha = (mBottomRadius - mTopRadius) / (mTopCenter - mBottomCenter); + JPH_ASSERT(mSinAlpha >= -1.0f && mSinAlpha <= 1.0f); + mTanAlpha = Tan(ASin(mSinAlpha)); + + outResult.Set(this); +} + +class TaperedCapsuleShape::TaperedCapsule final : public Support +{ +public: + TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) : + mTopCenter(inTopCenter), + mBottomCenter(inBottomCenter), + mTopRadius(inTopRadius), + mBottomRadius(inBottomRadius), + mConvexRadius(inConvexRadius) + { + static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TaperedCapsule))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + // Check zero vector + float len = inDirection.Length(); + if (len == 0.0f) + return mTopCenter + Vec3(0, mTopRadius, 0); // Return top + + // Check if the support of the top sphere or bottom sphere is bigger + Vec3 support_top = mTopCenter + (mTopRadius / len) * inDirection; + Vec3 support_bottom = mBottomCenter + (mBottomRadius / len) * inDirection; + if (support_top.Dot(inDirection) > support_bottom.Dot(inDirection)) + return support_top; + else + return support_bottom; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + Vec3 mTopCenter; + Vec3 mBottomCenter; + float mTopRadius; + float mBottomRadius; + float mConvexRadius; +}; + +const ConvexShape::Support *TaperedCapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + float scaled_convex_radius = scale_xz * mConvexRadius; + + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, scaled_top_radius, scaled_bottom_radius, 0.0f); + + case ESupportMode::ExcludeConvexRadius: + case ESupportMode::Default: + { + // Get radii reduced by convex radius + float tr = scaled_top_radius - scaled_convex_radius; + float br = scaled_bottom_radius - scaled_convex_radius; + JPH_ASSERT(tr >= 0.0f && br >= 0.0f); + JPH_ASSERT(tr == 0.0f || br == 0.0f, "Convex radius should be that of the smallest sphere"); + return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, tr, br, scaled_convex_radius); + } + } + + JPH_ASSERT(false); + return nullptr; +} + +void TaperedCapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + JPH_ASSERT(IsValidScale(inScale)); + + // Check zero vector + float len = inDirection.Length(); + if (len == 0.0f) + return; + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + + // Get support point for top and bottom sphere in the opposite of inDirection (including convex radius) + Vec3 support_top = scaled_top_center - (scaled_top_radius / len) * inDirection; + Vec3 support_bottom = scaled_bottom_center - (scaled_bottom_radius / len) * inDirection; + + // Get projection on inDirection + float proj_top = support_top.Dot(inDirection); + float proj_bottom = support_bottom.Dot(inDirection); + + // If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point + if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * len) + { + outVertices.push_back(inCenterOfMassTransform * support_top); + outVertices.push_back(inCenterOfMassTransform * support_bottom); + } +} + +MassProperties TaperedCapsuleShape::GetMassProperties() const +{ + AABox box = GetInertiaApproximation(); + + MassProperties p; + p.SetMassAndInertiaOfSolidBox(box.GetSize(), GetDensity()); + return p; +} + +Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // See: TaperedCapsuleShape.gliffy + // We need to calculate ty and by in order to see if the position is on the top or bottom sphere + // sin(alpha) = by / br = ty / tr + // => by = sin(alpha) * br, ty = sin(alpha) * tr + + if (inLocalSurfacePosition.GetY() > mTopCenter + mSinAlpha * mTopRadius) + return (inLocalSurfacePosition - Vec3(0, mTopCenter, 0)).Normalized(); + else if (inLocalSurfacePosition.GetY() < mBottomCenter + mSinAlpha * mBottomRadius) + return (inLocalSurfacePosition - Vec3(0, mBottomCenter, 0)).Normalized(); + else + { + // Get perpendicular vector to the surface in the xz plane + Vec3 perpendicular = Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX()); + + // We know that the perpendicular has length 1 and that it needs a y component where tan(alpha) = y / 1 in order to align it to the surface + perpendicular.SetY(mTanAlpha); + return perpendicular.Normalized(); + } +} + +AABox TaperedCapsuleShape::GetLocalBounds() const +{ + float max_radius = max(mTopRadius, mBottomRadius); + return AABox(Vec3(-max_radius, mBottomCenter - mBottomRadius, -max_radius), Vec3(max_radius, mTopCenter + mTopRadius, max_radius)); +} + +AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 abs_scale = inScale.Abs(); + float scale_xz = abs_scale.GetX(); + float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule + Vec3 bottom_extent = Vec3::sReplicate(scale_xz * mBottomRadius); + Vec3 bottom_center = inCenterOfMassTransform * Vec3(0, scale_y * mBottomCenter, 0); + Vec3 top_extent = Vec3::sReplicate(scale_xz * mTopRadius); + Vec3 top_center = inCenterOfMassTransform * Vec3(0, scale_y * mTopCenter, 0); + Vec3 p1 = Vec3::sMin(top_center - top_extent, bottom_center - bottom_extent); + Vec3 p2 = Vec3::sMax(top_center + top_extent, bottom_center + bottom_extent); + return AABox(p1, p2); +} + +void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation(); + + // Get scaled tapered capsule + Vec3 abs_scale = inScale.Abs(); + float scale_y = abs_scale.GetY(); + float scale_xz = abs_scale.GetX(); + Vec3 scale_y_flip(1, Sign(inScale.GetY()), 1); + Vec3 scaled_top_center(0, scale_y * mTopCenter, 0); + Vec3 scaled_bottom_center(0, scale_y * mBottomCenter, 0); + float scaled_top_radius = scale_xz * mTopRadius; + float scaled_bottom_radius = scale_xz * mBottomRadius; + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + Vec3 local_pos = scale_y_flip * (inverse_transform * v->mPosition); + + Vec3 position, normal; + + // If the vertex is inside the cone starting at the top center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the top sphere + // This corresponds to: Dot(y-axis, (local_pos - top_center) / |local_pos - top_center|) >= cos(PI/2 - alpha) + // <=> (local_pos - top_center).y >= sin(alpha) * |local_pos - top_center| + Vec3 top_center_to_local_pos = local_pos - scaled_top_center; + float top_center_to_local_pos_len = top_center_to_local_pos.Length(); + if (top_center_to_local_pos.GetY() >= mSinAlpha * top_center_to_local_pos_len) + { + // Top sphere + normal = top_center_to_local_pos_len != 0.0f? top_center_to_local_pos / top_center_to_local_pos_len : Vec3::sAxisY(); + position = scaled_top_center + scaled_top_radius * normal; + } + else + { + // If the vertex is outside the cone starting at the bottom center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the bottom sphere + // This corresponds to: Dot(y-axis, (local_pos - bottom_center) / |local_pos - bottom_center|) <= cos(PI/2 - alpha) + // <=> (local_pos - bottom_center).y <= sin(alpha) * |local_pos - bottom_center| + Vec3 bottom_center_to_local_pos = local_pos - scaled_bottom_center; + float bottom_center_to_local_pos_len = bottom_center_to_local_pos.Length(); + if (bottom_center_to_local_pos.GetY() <= mSinAlpha * bottom_center_to_local_pos_len) + { + // Bottom sphere + normal = bottom_center_to_local_pos_len != 0.0f? bottom_center_to_local_pos / bottom_center_to_local_pos_len : -Vec3::sAxisY(); + } + else + { + // Tapered cylinder + normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX()); + normal.SetY(mTanAlpha); + normal = normal.NormalizedOr(Vec3::sAxisX()); + } + position = scaled_bottom_center + scaled_bottom_radius * normal; + } + + Plane plane = Plane::sFromPointAndNormal(position, normal); + float penetration = -plane.SignedDistance(local_pos); + if (penetration > v->mLargestPenetration) + { + v->mLargestPenetration = penetration; + + // Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y) + plane.SetNormal(scale_y_flip * plane.GetNormal()); + + // Store collision + v->mCollisionPlane = plane.GetTransformed(inCenterOfMassTransform); + v->mCollidingShapeIndex = inCollidingShapeIndex; + } + } +} + +#ifdef JPH_DEBUG_RENDERER +void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + if (mGeometry == nullptr) + { + SupportBuffer buffer; + const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f)); + mGeometry = inRenderer->CreateTriangleGeometryForConvex([support](Vec3Arg inDirection) { return support->GetSupport(inDirection); }); + } + + // Preserve flip along y axis but make sure we're not inside out + Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? Vec3(-1, 1, 1) * inScale : inScale; + RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale); + + AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale); + + float lod_scale_sq = Square(max(mTopRadius, mBottomRadius)); + + Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor; + + DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid; + + inRenderer->DrawGeometry(world_transform, bounds, lod_scale_sq, color, mGeometry, DebugRenderer::ECullMode::CullBackFace, DebugRenderer::ECastShadow::On, draw_mode); +} +#endif // JPH_DEBUG_RENDERER + +AABox TaperedCapsuleShape::GetInertiaApproximation() const +{ + // TODO: For now the mass and inertia is that of a box + float avg_radius = 0.5f * (mTopRadius + mBottomRadius); + return AABox(Vec3(-avg_radius, mBottomCenter - mBottomRadius, -avg_radius), Vec3(avg_radius, mTopCenter + mTopRadius, avg_radius)); +} + +void TaperedCapsuleShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + ts.SetShapeScale(scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs())); + ioCollector.AddHit(ts); +} + +void TaperedCapsuleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mCenterOfMass); + inStream.Write(mTopRadius); + inStream.Write(mBottomRadius); + inStream.Write(mTopCenter); + inStream.Write(mBottomCenter); + inStream.Write(mConvexRadius); + inStream.Write(mSinAlpha); + inStream.Write(mTanAlpha); +} + +void TaperedCapsuleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mCenterOfMass); + inStream.Read(mTopRadius); + inStream.Read(mBottomRadius); + inStream.Read(mTopCenter); + inStream.Read(mBottomCenter); + inStream.Read(mConvexRadius); + inStream.Read(mSinAlpha); + inStream.Read(mTanAlpha); +} + +bool TaperedCapsuleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs()); +} + +void TaperedCapsuleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCapsule); + f.mConstruct = []() -> Shape * { return new TaperedCapsuleShape; }; + f.mColor = Color::sGreen; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy b/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy new file mode 100644 index 000000000..3e5221b86 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.gliffy @@ -0,0 +1 @@ +{"contentType":"application/gliffy+json","version":"1.1","metadata":{"title":"untitled","revision":0,"exportBorder":false},"embeddedResources":{"index":0,"resources":[]},"stage":{"objects":[{"x":870,"y":406,"rotation":0,"id":62,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":62,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[1.5,5.5],[1.5,-46.196228102251325]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":410,"y":406,"rotation":0,"id":60,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":60,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,5.5],[0,-49.03668490108288]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":614,"y":385,"rotation":0,"id":58,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":58,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-204.06126531020038,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":626,"y":385,"rotation":0,"id":57,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":57,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[248.0020161208372,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":818,"y":520,"rotation":0,"id":55,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":55,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48,76]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":830,"y":502,"rotation":0,"id":54,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":54,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[48,-82]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":373,"y":410,"rotation":0,"id":50,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":28,"height":23,"lockAspectRatio":false,"lockShape":false,"order":19,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    tx

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":387,"y":392,"rotation":0,"id":49,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":26,"height":23,"lockAspectRatio":false,"lockShape":false,"order":18,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    ty

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":722,"y":488,"rotation":0,"id":45,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":20,"lockAspectRatio":false,"lockShape":false,"order":16,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    bx

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":440,"y":411.5,"rotation":0,"id":40,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":13,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    α

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":411,"y":414,"rotation":0,"id":34,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":12,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-1,-2.5],[349,180]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":744,"y":613,"rotation":0,"id":32,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":11,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-1.1368683772161603e-13,-201.00995000248122]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":394,"y":436,"rotation":0,"id":30,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":10,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":"1.0,1.0","startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[0,-26.076809620810593]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":607,"y":368.5,"rotation":0,"id":26,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":9,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    h

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":401,"y":412.5,"rotation":0,"id":25,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":8,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    tr

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":808.5,"y":500,"rotation":0,"id":23,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":49,"height":27,"lockAspectRatio":false,"lockShape":false,"order":7,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    br - tr

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":402,"y":416,"rotation":0,"id":21,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":6,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[8,-4.5],[-7,21]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":0,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":347,"y":412,"rotation":0,"id":16,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":5,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[523.5,2.5]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":854,"y":433,"rotation":0,"id":14,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":4,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[17.5,-21.5],[-106,184]],"lockSegments":{}}},"children":null,"constraints":{"constraints":[],"startConstraint":{"type":"StartPositionConstraint","StartPositionConstraint":{"nodeId":4,"px":0.5,"py":0.5}}},"linkMap":[]},{"x":395,"y":437,"rotation":0,"id":9,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":3,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-52,-25],[695,360]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":395,"y":384,"rotation":0,"id":7,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":2,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":0,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[-49,26],[703,-359]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":630,"y":169.99999999999997,"rotation":0,"id":4,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":483.00000000000006,"height":483.00000000000006,"lockAspectRatio":true,"lockShape":false,"order":1,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":380,"y":381.5,"rotation":0,"id":0,"uid":"com.gliffy.shape.basic.basic_v1.default.circle","width":60,"height":60,"lockAspectRatio":true,"lockShape":false,"order":0,"graphic":{"type":"Shape","Shape":{"tid":"com.gliffy.stencil.ellipse.basic_v1","strokeWidth":2,"strokeColor":"#333333","fillColor":"#FFFFFF","gradient":false,"dropShadow":false,"state":0,"shadowX":0,"shadowY":0,"opacity":1}},"children":[],"linkMap":[]},{"x":728,"y":612,"rotation":0,"id":41,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":14,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    α

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":375,"y":432.5,"rotation":0,"id":42,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":28.500000000000004,"lockAspectRatio":false,"lockShape":false,"order":15,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    α

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":753,"y":595.5,"rotation":0,"id":53,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":20,"height":30,"lockAspectRatio":false,"lockShape":false,"order":20,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    tr

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":794,"y":400,"rotation":0,"id":65,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":65,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[-48.01041553663117,0]],"lockSegments":{}}},"children":null,"linkMap":[]},{"x":790,"y":393.25,"rotation":0,"id":46,"uid":"com.gliffy.shape.basic.basic_v1.default.text","width":29,"height":14,"lockAspectRatio":false,"lockShape":false,"order":17,"graphic":{"type":"Text","Text":{"tid":null,"valign":"middle","overflow":"none","vposition":"none","hposition":"none","html":"

    by

    ","paddingLeft":2,"paddingRight":2,"paddingBottom":2,"paddingTop":2}},"children":null,"linkMap":[]},{"x":818,"y":400,"rotation":0,"id":66,"uid":"com.gliffy.shape.basic.basic_v1.default.line","width":100,"height":100,"lockAspectRatio":false,"lockShape":false,"order":66,"graphic":{"type":"Line","Line":{"strokeWidth":2,"strokeColor":"#000000","fillColor":"none","dashStyle":null,"startArrow":0,"endArrow":2,"startArrowRotation":"auto","endArrowRotation":"auto","ortho":false,"interpolationType":"linear","cornerRadius":null,"controlPath":[[0,0],[54.230987451824944,0]],"lockSegments":{}}},"children":null,"linkMap":[]}],"background":"#FFFFFF","width":1113,"height":802,"maxWidth":5000,"maxHeight":5000,"nodeIndex":69,"autoFit":true,"exportBorder":false,"gridOn":true,"snapToGrid":false,"drawingGuidesOn":false,"pageBreaksOn":false,"printGridOn":false,"printPaper":"LETTER","printShrinkToFit":false,"printPortrait":true,"shapeStyles":{"com.gliffy.shape.basic.basic_v1.default":{"fill":"#FFFFFF","stroke":"#333333","strokeWidth":2}},"lineStyles":{"global":{"dashStyle":null,"endArrow":2,"startArrow":0}},"textStyles":{},"themeData":null}} \ No newline at end of file diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h new file mode 100644 index 000000000..6dad3c2e9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h @@ -0,0 +1,125 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TaperedCapsuleShape +class JPH_EXPORT TaperedCapsuleShapeSettings final : public ConvexShapeSettings +{ + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TaperedCapsuleShapeSettings) + + /// Default constructor for deserialization + TaperedCapsuleShapeSettings() = default; + + /// Create a tapered capsule centered around the origin with one sphere cap at (0, -inHalfHeightOfTaperedCylinder, 0) with radius inBottomRadius and the other at (0, inHalfHeightOfTaperedCylinder, 0) with radius inTopRadius + TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial = nullptr); + + /// Check if the settings are valid + bool IsValid() const { return mTopRadius > 0.0f && mBottomRadius > 0.0f && mHalfHeightOfTaperedCylinder >= 0.0f; } + + /// Checks if the settings of this tapered capsule make this shape a sphere + bool IsSphere() const; + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + float mHalfHeightOfTaperedCylinder = 0.0f; + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; +}; + +/// A capsule with different top and bottom radii +class JPH_EXPORT TaperedCapsuleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TaperedCapsuleShape() : ConvexShape(EShapeSubType::TaperedCapsule) { } + TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult); + + // See Shape::GetCenterOfMass + virtual Vec3 GetCenterOfMass() const override { return mCenterOfMass; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return min(mTopRadius, mBottomRadius); } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 0); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return GetLocalBounds().GetVolume(); } // Volume is approximate! + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Class for GetSupportFunction + class TaperedCapsule; + + /// Returns box that approximates the inertia + AABox GetInertiaApproximation() const; + + Vec3 mCenterOfMass = Vec3::sZero(); + float mTopRadius = 0.0f; + float mBottomRadius = 0.0f; + float mTopCenter = 0.0f; + float mBottomCenter = 0.0f; + float mConvexRadius = 0.0f; + float mSinAlpha = 0.0f; + float mTanAlpha = 0.0f; + +#ifdef JPH_DEBUG_RENDERER + mutable DebugRenderer::GeometryRef mGeometry; +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.cpp b/WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.cpp new file mode 100644 index 000000000..b70179647 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.cpp @@ -0,0 +1,413 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TriangleShapeSettings) +{ + JPH_ADD_BASE_CLASS(TriangleShapeSettings, ConvexShapeSettings) + + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV1) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV2) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mV3) + JPH_ADD_ATTRIBUTE(TriangleShapeSettings, mConvexRadius) +} + +ShapeSettings::ShapeResult TriangleShapeSettings::Create() const +{ + if (mCachedResult.IsEmpty()) + Ref shape = new TriangleShape(*this, mCachedResult); + return mCachedResult; +} + +TriangleShape::TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult) : + ConvexShape(EShapeSubType::Triangle, inSettings, outResult), + mV1(inSettings.mV1), + mV2(inSettings.mV2), + mV3(inSettings.mV3), + mConvexRadius(inSettings.mConvexRadius) +{ + if (inSettings.mConvexRadius < 0.0f) + { + outResult.SetError("Invalid convex radius"); + return; + } + + outResult.Set(this); +} + +AABox TriangleShape::GetLocalBounds() const +{ + AABox bounds(mV1, mV1); + bounds.Encapsulate(mV2); + bounds.Encapsulate(mV3); + bounds.ExpandBy(Vec3::sReplicate(mConvexRadius)); + return bounds; +} + +AABox TriangleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const +{ + JPH_ASSERT(IsValidScale(inScale)); + + Vec3 v1 = inCenterOfMassTransform * (inScale * mV1); + Vec3 v2 = inCenterOfMassTransform * (inScale * mV2); + Vec3 v3 = inCenterOfMassTransform * (inScale * mV3); + + AABox bounds(v1, v1); + bounds.Encapsulate(v2); + bounds.Encapsulate(v3); + bounds.ExpandBy(inScale * mConvexRadius); + return bounds; +} + +class TriangleShape::TriangleNoConvex final : public Support +{ +public: + TriangleNoConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : + mTriangleSuport(inV1, inV2, inV3) + { + static_assert(sizeof(TriangleNoConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TriangleNoConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + return mTriangleSuport.GetSupport(inDirection); + } + + virtual float GetConvexRadius() const override + { + return 0.0f; + } + +private: + TriangleConvexSupport mTriangleSuport; +}; + +class TriangleShape::TriangleWithConvex final : public Support +{ +public: + TriangleWithConvex(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius) : + mConvexRadius(inConvexRadius), + mTriangleSuport(inV1, inV2, inV3) + { + static_assert(sizeof(TriangleWithConvex) <= sizeof(SupportBuffer), "Buffer size too small"); + JPH_ASSERT(IsAligned(this, alignof(TriangleWithConvex))); + } + + virtual Vec3 GetSupport(Vec3Arg inDirection) const override + { + Vec3 support = mTriangleSuport.GetSupport(inDirection); + float len = inDirection.Length(); + if (len > 0.0f) + support += (mConvexRadius / len) * inDirection; + return support; + } + + virtual float GetConvexRadius() const override + { + return mConvexRadius; + } + +private: + float mConvexRadius; + TriangleConvexSupport mTriangleSuport; +}; + +const ConvexShape::Support *TriangleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const +{ + switch (inMode) + { + case ESupportMode::IncludeConvexRadius: + case ESupportMode::Default: + if (mConvexRadius > 0.0f) + return new (&inBuffer) TriangleWithConvex(inScale * mV1, inScale * mV2, inScale * mV3, mConvexRadius); + [[fallthrough]]; + + case ESupportMode::ExcludeConvexRadius: + return new (&inBuffer) TriangleNoConvex(inScale * mV1, inScale * mV2, inScale * mV3); + } + + JPH_ASSERT(false); + return nullptr; +} + +void TriangleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + // Calculate transform with scale + Mat44 transform = inCenterOfMassTransform.PreScaled(inScale); + + // Flip triangle if scaled inside out + if (ScaleHelpers::IsInsideOut(inScale)) + { + outVertices.push_back(transform * mV1); + outVertices.push_back(transform * mV3); + outVertices.push_back(transform * mV2); + } + else + { + outVertices.push_back(transform * mV1); + outVertices.push_back(transform * mV2); + outVertices.push_back(transform * mV3); + } +} + +MassProperties TriangleShape::GetMassProperties() const +{ + // Object should always be static, return default mass properties + return MassProperties(); +} + +Vec3 TriangleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID"); + + Vec3 cross = (mV2 - mV1).Cross(mV3 - mV1); + float len = cross.Length(); + return len != 0.0f? cross / len : Vec3::sAxisY(); +} + +void TriangleShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + // A triangle has no volume + outTotalVolume = outSubmergedVolume = 0.0f; + outCenterOfBuoyancy = Vec3::sZero(); +} + +#ifdef JPH_DEBUG_RENDERER +void TriangleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + RVec3 v1 = inCenterOfMassTransform * (inScale * mV1); + RVec3 v2 = inCenterOfMassTransform * (inScale * mV2); + RVec3 v3 = inCenterOfMassTransform * (inScale * mV3); + + if (ScaleHelpers::IsInsideOut(inScale)) + swap(v1, v2); + + if (inDrawWireframe) + inRenderer->DrawWireTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor); + else + inRenderer->DrawTriangle(v1, v2, v3, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor); +} +#endif // JPH_DEBUG_RENDERER + +bool TriangleShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3); + if (fraction < ioHit.mFraction) + { + ioHit.mFraction = fraction; + ioHit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + return true; + } + return false; +} + +void TriangleShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + // Back facing check + if (inRayCastSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (mV2 - mV1).Cross(mV3 - mV1).Dot(inRay.mDirection) > 0.0f) + return; + + // Test ray against triangle + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, mV1, mV2, mV3); + if (fraction < ioCollector.GetEarlyOutFraction()) + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.GetID(); + ioCollector.AddHit(hit); + } +} + +void TriangleShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + // Can't be inside a triangle +} + +void TriangleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, [[maybe_unused]] float inDeltaTime, [[maybe_unused]] Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + CollideSoftBodyVerticesVsTriangles collider(inCenterOfMassTransform, inScale); + + for (SoftBodyVertex *v = ioVertices, *sbv_end = ioVertices + inNumVertices; v < sbv_end; ++v) + if (v->mInvMass > 0.0f) + { + collider.StartVertex(*v); + collider.ProcessTriangle(mV1, mV2, mV3); + collider.FinishVertex(*v, inCollidingShapeIndex); + } +} + +void TriangleShape::sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape2 = static_cast(inShape2); + + CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCollideSphereVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + const SphereShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape2 = static_cast(inShape2); + + CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + collider.Collide(shape2->mV1, shape2->mV2, shape2->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape = static_cast(inShape); + + CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Triangle); + const TriangleShape *shape = static_cast(inShape); + + CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + caster.Cast(shape->mV1, shape->mV2, shape->mV3, 0b111, inSubShapeIDCreator2.GetID()); +} + +void TriangleShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const +{ + Vec3 scale; + Mat44 transform = inCenterOfMassTransform.Decompose(scale); + TransformedShape ts(RVec3(transform.GetTranslation()), transform.GetQuaternion(), this, BodyID(), SubShapeIDCreator()); + ts.SetShapeScale(mConvexRadius == 0.0f? scale : scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs())); + ioCollector.AddHit(ts); +} + +class TriangleShape::TSGetTrianglesContext +{ +public: + TSGetTrianglesContext(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3) : mV1(inV1), mV2(inV2), mV3(inV3) { } + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + + bool mIsDone = false; +}; + +void TriangleShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + static_assert(sizeof(TSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small"); + JPH_ASSERT(IsAligned(&ioContext, alignof(TSGetTrianglesContext))); + + Mat44 m = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale); + + new (&ioContext) TSGetTrianglesContext(m * mV1, m * mV2, m * mV3); +} + +int TriangleShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + static_assert(cGetTrianglesMinTrianglesRequested >= 3, "cGetTrianglesMinTrianglesRequested is too small"); + JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested); + + TSGetTrianglesContext &context = (TSGetTrianglesContext &)ioContext; + + // Only return the triangle the 1st time + if (context.mIsDone) + return 0; + context.mIsDone = true; + + // Store triangle + context.mV1.StoreFloat3(outTriangleVertices); + context.mV2.StoreFloat3(outTriangleVertices + 1); + context.mV3.StoreFloat3(outTriangleVertices + 2); + + // Store material + if (outMaterials != nullptr) + *outMaterials = GetMaterial(); + + return 1; +} + +void TriangleShape::SaveBinaryState(StreamOut &inStream) const +{ + ConvexShape::SaveBinaryState(inStream); + + inStream.Write(mV1); + inStream.Write(mV2); + inStream.Write(mV3); + inStream.Write(mConvexRadius); +} + +void TriangleShape::RestoreBinaryState(StreamIn &inStream) +{ + ConvexShape::RestoreBinaryState(inStream); + + inStream.Read(mV1); + inStream.Read(mV2); + inStream.Read(mV3); + inStream.Read(mConvexRadius); +} + +bool TriangleShape::IsValidScale(Vec3Arg inScale) const +{ + return ConvexShape::IsValidScale(inScale) && (mConvexRadius == 0.0f || ScaleHelpers::IsUniformScale(inScale.Abs())); +} + +void TriangleShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Triangle); + f.mConstruct = []() -> Shape * { return new TriangleShape; }; + f.mColor = Color::sGreen; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Triangle, sCollideConvexVsTriangle); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Triangle, sCastConvexVsTriangle); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCollideSphereVsTriangle); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::Triangle, sCastSphereVsTriangle); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.h b/WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.h new file mode 100644 index 000000000..990e6e051 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/Shape/TriangleShape.h @@ -0,0 +1,138 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class that constructs a TriangleShape +class JPH_EXPORT TriangleShapeSettings final : public ConvexShapeSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TriangleShapeSettings) + + /// Default constructor for deserialization + TriangleShapeSettings() = default; + + /// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius. + /// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin. + TriangleShapeSettings(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShapeSettings(inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { } + + // See: ShapeSettings + virtual ShapeResult Create() const override; + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + float mConvexRadius = 0.0f; +}; + +/// A single triangle, not the most efficient way of creating a world filled with triangles but can be used as a query shape for example. +class JPH_EXPORT TriangleShape final : public ConvexShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TriangleShape() : ConvexShape(EShapeSubType::Triangle) { } + TriangleShape(const TriangleShapeSettings &inSettings, ShapeResult &outResult); + + /// Create a triangle with points (inV1, inV2, inV3) (counter clockwise) and convex radius inConvexRadius. + /// Note that the convex radius is currently only used for shape vs shape collision, for all other purposes the triangle is infinitely thin. + TriangleShape(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, float inConvexRadius = 0.0f, const PhysicsMaterial *inMaterial = nullptr) : ConvexShape(EShapeSubType::Triangle, inMaterial), mV1(inV1), mV2(inV2), mV3(inV3), mConvexRadius(inConvexRadius) { JPH_ASSERT(inConvexRadius >= 0.0f); } + + /// Convex radius + float GetConvexRadius() const { return mConvexRadius; } + + // See Shape::GetLocalBounds + virtual AABox GetLocalBounds() const override; + + // See Shape::GetWorldSpaceBounds + virtual AABox GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const override; + using Shape::GetWorldSpaceBounds; + + // See Shape::GetInnerRadius + virtual float GetInnerRadius() const override { return mConvexRadius; } + + // See Shape::GetMassProperties + virtual MassProperties GetMassProperties() const override; + + // See Shape::GetSurfaceNormal + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + + // See Shape::GetSupportingFace + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + + // See ConvexShape::GetSupportFunction + virtual const Support * GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const override; + + // See Shape::GetSubmergedVolume + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const override; + +#ifdef JPH_DEBUG_RENDERER + // See Shape::Draw + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + + // See Shape::CastRay + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollidePoint + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + + // See: Shape::CollideSoftBodyVertices + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + + // See Shape::TransformShape + virtual void TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const override; + + // See Shape::GetTrianglesStart + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + + // See Shape::GetTrianglesNext + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + + // See Shape + virtual void SaveBinaryState(StreamOut &inStream) const override; + + // See Shape::GetStats + virtual Stats GetStats() const override { return Stats(sizeof(*this), 1); } + + // See Shape::GetVolume + virtual float GetVolume() const override { return 0; } + + // See Shape::IsValidScale + virtual bool IsValidScale(Vec3Arg inScale) const override; + + // Register shape functions with the registry + static void sRegister(); + +protected: + // See: Shape::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + // Helper functions called by CollisionDispatch + static void sCollideConvexVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsTriangle(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsTriangle(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + // Context for GetTrianglesStart/Next + class TSGetTrianglesContext; + + // Classes for GetSupportFunction + class TriangleNoConvex; + class TriangleWithConvex; + + Vec3 mV1; + Vec3 mV2; + Vec3 mV3; + float mConvexRadius = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ShapeCast.h b/WickedEngine/Jolt/Physics/Collision/ShapeCast.h new file mode 100644 index 000000000..43cf6f3da --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ShapeCast.h @@ -0,0 +1,170 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Structure that holds a single shape cast (a shape moving along a linear path in 3d space with no rotation) +template +struct ShapeCastT +{ + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection, const AABox &inWorldSpaceBounds) : + mShape(inShape), + mScale(inScale), + mCenterOfMassStart(inCenterOfMassStart), + mDirection(inDirection), + mShapeWorldBounds(inWorldSpaceBounds) + { + } + + /// Constructor + ShapeCastT(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inCenterOfMassStart, Vec3Arg inDirection) : + ShapeCastT(inShape, inScale, inCenterOfMassStart, inDirection, inShape->GetWorldSpaceBounds(inCenterOfMassStart, inScale)) + { + } + + /// Construct a shape cast using a world transform for a shape instead of a center of mass transform + static inline ShapeCastType sFromWorldTransform(const Shape *inShape, Vec3Arg inScale, typename Mat::ArgType inWorldTransform, Vec3Arg inDirection) + { + return ShapeCastType(inShape, inScale, inWorldTransform.PreTranslated(inShape->GetCenterOfMass()), inDirection); + } + + /// Transform this shape cast using inTransform. Multiply transform on the left left hand side. + ShapeCastType PostTransformed(typename Mat::ArgType inTransform) const + { + Mat44 start = inTransform * mCenterOfMassStart; + Vec3 direction = inTransform.Multiply3x3(mDirection); + return { mShape, mScale, start, direction }; + } + + /// Translate this shape cast by inTranslation. + ShapeCastType PostTranslated(typename Vec::ArgType inTranslation) const + { + return { mShape, mScale, mCenterOfMassStart.PostTranslated(inTranslation), mDirection }; + } + + /// Get point with fraction inFraction on ray from mCenterOfMassStart to mCenterOfMassStart + mDirection (0 = start of ray, 1 = end of ray) + inline Vec GetPointOnRay(float inFraction) const + { + return mCenterOfMassStart.GetTranslation() + inFraction * mDirection; + } + + const Shape * mShape; ///< Shape that's being cast (cannot be mesh shape). Note that this structure does not assume ownership over the shape for performance reasons. + const Vec3 mScale; ///< Scale in local space of the shape being cast (scales relative to its center of mass) + const Mat mCenterOfMassStart; ///< Start position and orientation of the center of mass of the shape (construct using sFromWorldTransform if you have a world transform for your shape) + const Vec3 mDirection; ///< Direction and length of the cast (anything beyond this length will not be reported as a hit) + const AABox mShapeWorldBounds; ///< Cached shape's world bounds, calculated in constructor +}; + +struct ShapeCast : public ShapeCastT +{ + using ShapeCastT::ShapeCastT; +}; + +struct RShapeCast : public ShapeCastT +{ + using ShapeCastT::ShapeCastT; + + /// Convert from ShapeCast, converts single to double precision + explicit RShapeCast(const ShapeCast &inCast) : + RShapeCast(inCast.mShape, inCast.mScale, RMat44(inCast.mCenterOfMassStart), inCast.mDirection, inCast.mShapeWorldBounds) + { + } + + /// Convert to ShapeCast, which implies casting from double precision to single precision + explicit operator ShapeCast() const + { + return ShapeCast(mShape, mScale, mCenterOfMassStart.ToMat44(), mDirection, mShapeWorldBounds); + } +}; + +/// Settings to be passed with a shape cast +class ShapeCastSettings : public CollideSettingsBase +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// How backfacing triangles should be treated (should we report moving out of a triangle?) + EBackFaceMode mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + + /// How backfacing convex objects should be treated (should we report starting inside an object and moving out?) + EBackFaceMode mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + + /// Indicates if we want to shrink the shape by the convex radius and then expand it again. This speeds up collision detection and gives a more accurate normal at the cost of a more 'rounded' shape. + bool mUseShrunkenShapeAndConvexRadius = false; + + /// When true, and the shape is intersecting at the beginning of the cast (fraction = 0) then this will calculate the deepest penetration point (costing additional CPU time) + bool mReturnDeepestPoint = false; +}; + +/// Result of a shape cast test +class ShapeCastResult : public CollideShapeResult +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Default constructor + ShapeCastResult() = default; + + /// Constructor + /// @param inFraction Fraction at which the cast hit + /// @param inContactPoint1 Contact point on shape 1 + /// @param inContactPoint2 Contact point on shape 2 + /// @param inContactNormalOrPenetrationDepth Contact normal pointing from shape 1 to 2 or penetration depth vector when the objects are penetrating (also from 1 to 2) + /// @param inBackFaceHit If this hit was a back face hit + /// @param inSubShapeID1 Sub shape id for shape 1 + /// @param inSubShapeID2 Sub shape id for shape 2 + /// @param inBodyID2 BodyID that was hit + ShapeCastResult(float inFraction, Vec3Arg inContactPoint1, Vec3Arg inContactPoint2, Vec3Arg inContactNormalOrPenetrationDepth, bool inBackFaceHit, const SubShapeID &inSubShapeID1, const SubShapeID &inSubShapeID2, const BodyID &inBodyID2) : + CollideShapeResult(inContactPoint1, inContactPoint2, inContactNormalOrPenetrationDepth, (inContactPoint2 - inContactPoint1).Length(), inSubShapeID1, inSubShapeID2, inBodyID2), + mFraction(inFraction), + mIsBackFaceHit(inBackFaceHit) + { + } + + /// Function required by the CollisionCollector. A smaller fraction is considered to be a 'better hit'. For rays/cast shapes we can just use the collision fraction. The fraction and penetration depth are combined in such a way that deeper hits at fraction 0 go first. + inline float GetEarlyOutFraction() const { return mFraction > 0.0f? mFraction : -mPenetrationDepth; } + + /// Reverses the hit result, swapping contact point 1 with contact point 2 etc. + /// @param inWorldSpaceCastDirection Direction of the shape cast in world space + ShapeCastResult Reversed(Vec3Arg inWorldSpaceCastDirection) const + { + // Calculate by how much to shift the contact points + Vec3 delta = mFraction * inWorldSpaceCastDirection; + + ShapeCastResult result; + result.mContactPointOn2 = mContactPointOn1 - delta; + result.mContactPointOn1 = mContactPointOn2 - delta; + result.mPenetrationAxis = -mPenetrationAxis; + result.mPenetrationDepth = mPenetrationDepth; + result.mSubShapeID2 = mSubShapeID1; + result.mSubShapeID1 = mSubShapeID2; + result.mBodyID2 = mBodyID2; + result.mFraction = mFraction; + result.mIsBackFaceHit = mIsBackFaceHit; + + result.mShape2Face.resize(mShape1Face.size()); + for (Face::size_type i = 0; i < mShape1Face.size(); ++i) + result.mShape2Face[i] = mShape1Face[i] - delta; + + result.mShape1Face.resize(mShape2Face.size()); + for (Face::size_type i = 0; i < mShape2Face.size(); ++i) + result.mShape1Face[i] = mShape2Face[i] - delta; + + return result; + } + + float mFraction; ///< This is the fraction where the shape hit the other shape: CenterOfMassOnHit = Start + value * (End - Start) + bool mIsBackFaceHit; ///< True if the shape was hit from the back side +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/ShapeFilter.h b/WickedEngine/Jolt/Physics/Collision/ShapeFilter.h new file mode 100644 index 000000000..0b5ebbefd --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/ShapeFilter.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Shape; +class SubShapeID; + +/// Filter class +class ShapeFilter : public NonCopyable +{ +public: + /// Destructor + virtual ~ShapeFilter() = default; + + /// Filter function to determine if we should collide with a shape. Returns true if the filter passes. + /// This overload is called when the query doesn't have a source shape (e.g. ray cast / collide point) + /// @param inShape2 Shape we're colliding against + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2) + virtual bool ShouldCollide([[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } + + /// Filter function to determine if two shapes should collide. Returns true if the filter passes. + /// This overload is called when querying a shape vs a shape (e.g. collide object / cast object). + /// It is called at each level of the shape hierarchy, so if you have a compound shape with a box, this function will be called twice. + /// It will not be called on triangles that are part of another shape, i.e a mesh shape will not trigger a callback per triangle. You can filter out individual triangles in the CollisionCollector::AddHit function by their sub shape ID. + /// @param inShape1 1st shape that is colliding + /// @param inSubShapeIDOfShape1 The sub shape ID that will lead from the root shape to inShape1 (i.e. the shape that is used to collide or cast against shape 2) + /// @param inShape2 2nd shape that is colliding + /// @param inSubShapeIDOfShape2 The sub shape ID that will lead from the root shape to inShape2 (i.e. the shape of mBodyID2) + virtual bool ShouldCollide([[maybe_unused]] const Shape *inShape1, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape1, [[maybe_unused]] const Shape *inShape2, [[maybe_unused]] const SubShapeID &inSubShapeIDOfShape2) const + { + return true; + } + + /// Set by the collision detection functions to the body ID of the body that we're colliding against before calling the ShouldCollide function + mutable BodyID mBodyID2; +}; + +/// Helper class to reverse the order of the shapes in the ShouldCollide function +class ReversedShapeFilter : public ShapeFilter +{ +public: + /// Constructor + explicit ReversedShapeFilter(const ShapeFilter &inFilter) : mFilter(inFilter) + { + mBodyID2 = inFilter.mBodyID2; + } + + virtual bool ShouldCollide(const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2); + } + + virtual bool ShouldCollide(const Shape *inShape1, const SubShapeID &inSubShapeIDOfShape1, const Shape *inShape2, const SubShapeID &inSubShapeIDOfShape2) const override + { + return mFilter.ShouldCollide(inShape2, inSubShapeIDOfShape2, inShape1, inSubShapeIDOfShape1); + } + +private: + const ShapeFilter & mFilter; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/SortReverseAndStore.h b/WickedEngine/Jolt/Physics/Collision/SortReverseAndStore.h new file mode 100644 index 000000000..4a073096c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/SortReverseAndStore.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// This function will sort values from high to low and only keep the ones that are less than inMaxValue +/// @param inValues Values to be sorted +/// @param inMaxValue Values need to be less than this to keep them +/// @param ioIdentifiers 4 identifiers that will be sorted in the same way as the values +/// @param outValues The values are stored here from high to low +/// @return The number of values that were kept +JPH_INLINE int SortReverseAndStore(Vec4Arg inValues, float inMaxValue, UVec4 &ioIdentifiers, float *outValues) +{ + // Sort so that highest values are first (we want to first process closer hits and we process stack top to bottom) + Vec4 values = inValues; + Vec4::sSort4Reverse(values, ioIdentifiers); + + // Count how many results are less than the max value + UVec4 closer = Vec4::sLess(values, Vec4::sReplicate(inMaxValue)); + int num_results = closer.CountTrues(); + + // Shift the values so that only the ones that are less than max are kept + values = values.ReinterpretAsInt().ShiftComponents4Minus(num_results).ReinterpretAsFloat(); + ioIdentifiers = ioIdentifiers.ShiftComponents4Minus(num_results); + + // Store the values + values.StoreFloat4(reinterpret_cast(outValues)); + + return num_results; +} + +/// Shift the elements so that the identifiers that correspond with the trues in inValue come first +/// @param inValue Values to test for true or false +/// @param ioIdentifiers the identifiers that are shifted, on return they are shifted +/// @return The number of trues +JPH_INLINE int CountAndSortTrues(UVec4Arg inValue, UVec4 &ioIdentifiers) +{ + // Sort the hits + ioIdentifiers = UVec4::sSort4True(inValue, ioIdentifiers); + + // Return the amount of hits + return inValue.CountTrues(); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/TransformedShape.cpp b/WickedEngine/Jolt/Physics/Collision/TransformedShape.cpp new file mode 100644 index 000000000..470387b52 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/TransformedShape.cpp @@ -0,0 +1,180 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool TransformedShape::CastRay(const RRayCast &inRay, RayCastResult &ioHit) const +{ + if (mShape != nullptr) + { + // Transform the ray to local space, note that this drops precision which is possible because we're in local space now + RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform())); + + // Scale the ray + Vec3 inv_scale = GetShapeScale().Reciprocal(); + ray.mOrigin *= inv_scale; + ray.mDirection *= inv_scale; + + // Cast the ray on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + if (mShape->CastRay(ray, sub_shape_id, ioHit)) + { + // Set body ID on the hit result + ioHit.mBodyID = mBodyID; + + return true; + } + } + + return false; +} + +void TransformedShape::CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Transform the ray to local space, note that this drops precision which is possible because we're in local space now + RayCast ray(inRay.Transformed(GetInverseCenterOfMassTransform())); + + // Scale the ray + Vec3 inv_scale = GetShapeScale().Reciprocal(); + ray.mOrigin *= inv_scale; + ray.mDirection *= inv_scale; + + // Cast the ray on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + mShape->CastRay(ray, inRayCastSettings, sub_shape_id, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Transform and scale the point to local space + Vec3 point = Vec3(GetInverseCenterOfMassTransform() * inPoint) / GetShapeScale(); + + // Do point collide on the shape + SubShapeIDCreator sub_shape_id(mSubShapeIDCreator); + mShape->CollidePoint(point, sub_shape_id, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator); + Mat44 transform1 = inCenterOfMassTransform.PostTranslated(-inBaseOffset).ToMat44(); + Mat44 transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + CollisionDispatch::sCollideShapeVsShape(inShape, mShape, inShapeScale, GetShapeScale(), transform1, transform2, sub_shape_id1, sub_shape_id2, inCollideShapeSettings, ioCollector, inShapeFilter); + } +} + +void TransformedShape::CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + // Set the context on the collector and filter + ioCollector.SetContext(this); + inShapeFilter.mBodyID2 = mBodyID; + + // Get the shape cast relative to the base offset and convert it to floats + ShapeCast shape_cast(inShapeCast.PostTranslated(-inBaseOffset)); + + // Get center of mass of object we're casting against relative to the base offset and convert it to floats + Mat44 center_of_mass_transform2 = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + + SubShapeIDCreator sub_shape_id1, sub_shape_id2(mSubShapeIDCreator); + CollisionDispatch::sCastShapeVsShapeWorldSpace(shape_cast, inShapeCastSettings, mShape, GetShapeScale(), inShapeFilter, center_of_mass_transform2, sub_shape_id1, sub_shape_id2, ioCollector); + } +} + +void TransformedShape::CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + if (mShape != nullptr) + { + struct MyCollector : public TransformedShapeCollector + { + MyCollector(TransformedShapeCollector &ioCollector, RVec3 inShapePositionCOM) : + TransformedShapeCollector(ioCollector), + mCollector(ioCollector), + mShapePositionCOM(inShapePositionCOM) + { + } + + virtual void AddHit(const TransformedShape &inResult) override + { + // Apply the center of mass offset + TransformedShape ts = inResult; + ts.mShapePositionCOM += mShapePositionCOM; + + // Pass hit on to child collector + mCollector.AddHit(ts); + + // Update early out fraction based on child collector + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + TransformedShapeCollector & mCollector; + RVec3 mShapePositionCOM; + }; + + // Set the context on the collector + ioCollector.SetContext(this); + + // Wrap the collector so we can add the center of mass precision, we do this to avoid losing precision because CollectTransformedShapes uses single precision floats + MyCollector collector(ioCollector, mShapePositionCOM); + + // Take box to local space for the shape + AABox box = inBox; + box.Translate(-mShapePositionCOM); + + mShape->CollectTransformedShapes(box, Vec3::sZero(), mShapeRotation, GetShapeScale(), mSubShapeIDCreator, collector, inShapeFilter); + } +} + +void TransformedShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const +{ + if (mShape != nullptr) + { + // Take box to local space for the shape + AABox box = inBox; + box.Translate(-inBaseOffset); + + mShape->GetTrianglesStart(ioContext, box, Vec3(mShapePositionCOM - inBaseOffset), mShapeRotation, GetShapeScale()); + } +} + +int TransformedShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + if (mShape != nullptr) + return mShape->GetTrianglesNext(ioContext, inMaxTrianglesRequested, outTriangleVertices, outMaterials); + else + return 0; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Collision/TransformedShape.h b/WickedEngine/Jolt/Physics/Collision/TransformedShape.h new file mode 100644 index 000000000..887a8ee44 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Collision/TransformedShape.h @@ -0,0 +1,194 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +struct RRayCast; +struct RShapeCast; +class CollideShapeSettings; +class RayCastResult; + +/// Temporary data structure that contains a shape and a transform. +/// This structure can be obtained from a body (e.g. after a broad phase query) under lock protection. +/// The lock can then be released and collision detection operations can be safely performed since +/// the class takes a reference on the shape and does not use anything from the body anymore. +class JPH_EXPORT TransformedShape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TransformedShape() = default; + TransformedShape(RVec3Arg inPositionCOM, QuatArg inRotation, const Shape *inShape, const BodyID &inBodyID, const SubShapeIDCreator &inSubShapeIDCreator = SubShapeIDCreator()) : mShapePositionCOM(inPositionCOM), mShapeRotation(inRotation), mShape(inShape), mBodyID(inBodyID), mSubShapeIDCreator(inSubShapeIDCreator) { } + + /// Cast a ray and find the closest hit. Returns true if it finds a hit. Hits further than ioHit.mFraction will not be considered and in this case ioHit will remain unmodified (and the function will return false). + /// Convex objects will be treated as solid (meaning if the ray starts inside, you'll get a hit fraction of 0) and back face hits are returned. + /// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(ioHit.mSubShapeID2, inRay.GetPointOnRay(ioHit.mFraction)) on this object. + bool CastRay(const RRayCast &inRay, RayCastResult &ioHit) const; + + /// Cast a ray, allows collecting multiple hits. Note that this version is more flexible but also slightly slower than the CastRay function that returns only a single hit. + /// If you want the surface normal of the hit use GetWorldSpaceSurfaceNormal(collected sub shape ID, inRay.GetPointOnRay(collected fraction)) on this object. + void CastRay(const RRayCast &inRay, const RayCastSettings &inRayCastSettings, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Check if inPoint is inside any shapes. For this tests all shapes are treated as if they were solid. + /// For a mesh shape, this test will only provide sensible information if the mesh is a closed manifold. + /// For each shape that collides, ioCollector will receive a hit + void CollidePoint(RVec3Arg inPoint, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Collide a shape and report any hits to ioCollector + /// @param inShape Shape to test + /// @param inShapeScale Scale in local space of shape + /// @param inCenterOfMassTransform Center of mass transform for the shape + /// @param inCollideShapeSettings Settings + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inShapeFilter Filter that allows you to reject collisions + void CollideShape(const Shape *inShape, Vec3Arg inShapeScale, RMat44Arg inCenterOfMassTransform, const CollideShapeSettings &inCollideShapeSettings, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Cast a shape and report any hits to ioCollector + /// @param inShapeCast The shape cast and its position and direction + /// @param inShapeCastSettings Settings for the shape cast + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM or inShapeCast.mCenterOfMassStart.GetTranslation() since floats are most accurate near the origin + /// @param ioCollector Collector that receives the hits + /// @param inShapeFilter Filter that allows you to reject collisions + void CastShape(const RShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, RVec3Arg inBaseOffset, CastShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Collect the leaf transformed shapes of all leaf shapes of this shape + /// inBox is the world space axis aligned box which leaf shapes should collide with + void CollectTransformedShapes(const AABox &inBox, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const; + + /// Use the context from Shape + using GetTrianglesContext = Shape::GetTrianglesContext; + + /// To start iterating over triangles, call this function first. + /// To get the actual triangles call GetTrianglesNext. + /// @param ioContext A temporary buffer and should remain untouched until the last call to GetTrianglesNext. + /// @param inBox The world space bounding in which you want to get the triangles. + /// @param inBaseOffset All hit results will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. inBox.GetCenter() since floats are most accurate near the origin + void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, RVec3Arg inBaseOffset) const; + + /// Call this repeatedly to get all triangles in the box. + /// outTriangleVertices should be large enough to hold 3 * inMaxTriangleRequested entries + /// outMaterials (if it is not null) should contain inMaxTrianglesRequested entries + /// The function returns the amount of triangles that it found (which will be <= inMaxTrianglesRequested), or 0 if there are no more triangles. + /// Note that the function can return a value < inMaxTrianglesRequested and still have more triangles to process (triangles can be returned in blocks) + /// Note that the function may return triangles outside of the requested box, only coarse culling is performed on the returned triangles + int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const; + + /// Get/set the scale of the shape as a Vec3 + inline Vec3 GetShapeScale() const { return Vec3::sLoadFloat3Unsafe(mShapeScale); } + inline void SetShapeScale(Vec3Arg inScale) { inScale.StoreFloat3(&mShapeScale); } + + /// Calculates the transform for this shapes's center of mass (excluding scale) + inline RMat44 GetCenterOfMassTransform() const { return RMat44::sRotationTranslation(mShapeRotation, mShapePositionCOM); } + + /// Calculates the inverse of the transform for this shape's center of mass (excluding scale) + inline RMat44 GetInverseCenterOfMassTransform() const { return RMat44::sInverseRotationTranslation(mShapeRotation, mShapePositionCOM); } + + /// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created) + inline void SetWorldTransform(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inScale) + { + mShapePositionCOM = inPosition + inRotation * (inScale * mShape->GetCenterOfMass()); + mShapeRotation = inRotation; + SetShapeScale(inScale); + } + + /// Sets the world transform (including scale) of this transformed shape (not from the center of mass but in the space the shape was created) + inline void SetWorldTransform(RMat44Arg inTransform) + { + Vec3 scale; + RMat44 rot_trans = inTransform.Decompose(scale); + SetWorldTransform(rot_trans.GetTranslation(), rot_trans.GetQuaternion(), scale); + } + + /// Calculates the world transform including scale of this shape (not from the center of mass but in the space the shape was created) + inline RMat44 GetWorldTransform() const + { + RMat44 transform = RMat44::sRotation(mShapeRotation).PreScaled(GetShapeScale()); + transform.SetTranslation(mShapePositionCOM - transform.Multiply3x3(mShape->GetCenterOfMass())); + return transform; + } + + /// Get the world space bounding box for this transformed shape + AABox GetWorldSpaceBounds() const { return mShape != nullptr? mShape->GetWorldSpaceBounds(GetCenterOfMassTransform(), GetShapeScale()) : AABox(); } + + /// Make inSubShapeID relative to mShape. When mSubShapeIDCreator is not empty, this is needed in order to get the correct path to the sub shape. + inline SubShapeID MakeSubShapeIDRelativeToShape(const SubShapeID &inSubShapeID) const + { + // Take off the sub shape ID part that comes from mSubShapeIDCreator and validate that it is the same + SubShapeID sub_shape_id; + uint num_bits_written = mSubShapeIDCreator.GetNumBitsWritten(); + JPH_IF_ENABLE_ASSERTS(uint32 root_id =) inSubShapeID.PopID(num_bits_written, sub_shape_id); + JPH_ASSERT(root_id == (mSubShapeIDCreator.GetID().GetValue() & ((1 << num_bits_written) - 1))); + return sub_shape_id; + } + + /// Get surface normal of a particular sub shape and its world space surface position on this body. + /// Note: When you have a CollideShapeResult or ShapeCastResult you should use -mPenetrationAxis.Normalized() as contact normal as GetWorldSpaceSurfaceNormal will only return face normals (and not vertex or edge normals). + inline Vec3 GetWorldSpaceSurfaceNormal(const SubShapeID &inSubShapeID, RVec3Arg inPosition) const + { + RMat44 inv_com = GetInverseCenterOfMassTransform(); + Vec3 scale = GetShapeScale(); // See comment at ScaledShape::GetSurfaceNormal for the math behind the scaling of the normal + return inv_com.Multiply3x3Transposed(mShape->GetSurfaceNormal(MakeSubShapeIDRelativeToShape(inSubShapeID), Vec3(inv_com * inPosition) / scale) / scale).Normalized(); + } + + /// Get the vertices of the face that faces inDirection the most (includes any convex radius). Note that this function can only return faces of + /// convex shapes or triangles, which is why a sub shape ID to get to that leaf must be provided. + /// @param inSubShapeID Sub shape ID of target shape + /// @param inDirection Direction that the face should be facing (in world space) + /// @param inBaseOffset The vertices will be returned relative to this offset, can be zero to get results in world position, but when you're testing far from the origin you get better precision by picking a position that's closer e.g. mShapePositionCOM since floats are most accurate near the origin + /// @param outVertices Resulting face. Note the returned face can have a single point if the shape doesn't have polygons to return (e.g. because it's a sphere). The face will be returned in world space. + void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, RVec3Arg inBaseOffset, Shape::SupportingFace &outVertices) const + { + Mat44 com = GetCenterOfMassTransform().PostTranslated(-inBaseOffset).ToMat44(); + mShape->GetSupportingFace(MakeSubShapeIDRelativeToShape(inSubShapeID), com.Multiply3x3Transposed(inDirection), GetShapeScale(), com, outVertices); + } + + /// Get material of a particular sub shape + inline const PhysicsMaterial *GetMaterial(const SubShapeID &inSubShapeID) const + { + return mShape->GetMaterial(MakeSubShapeIDRelativeToShape(inSubShapeID)); + } + + /// Get the user data of a particular sub shape + inline uint64 GetSubShapeUserData(const SubShapeID &inSubShapeID) const + { + return mShape->GetSubShapeUserData(MakeSubShapeIDRelativeToShape(inSubShapeID)); + } + + /// Get the direct child sub shape and its transform for a sub shape ID. + /// @param inSubShapeID Sub shape ID that indicates the path to the leaf shape + /// @param outRemainder The remainder of the sub shape ID after removing the sub shape + /// @return Direct child sub shape and its transform, note that the body ID and sub shape ID will be invalid + TransformedShape GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, SubShapeID &outRemainder) const + { + TransformedShape ts = mShape->GetSubShapeTransformedShape(inSubShapeID, Vec3::sZero(), mShapeRotation, GetShapeScale(), outRemainder); + ts.mShapePositionCOM += mShapePositionCOM; + return ts; + } + + /// Helper function to return the body id from a transformed shape. If the transformed shape is null an invalid body ID will be returned. + inline static BodyID sGetBodyID(const TransformedShape *inTS) { return inTS != nullptr? inTS->mBodyID : BodyID(); } + + RVec3 mShapePositionCOM; ///< Center of mass world position of the shape + Quat mShapeRotation; ///< Rotation of the shape + RefConst mShape; ///< The shape itself + Float3 mShapeScale { 1, 1, 1 }; ///< Not stored as Vec3 to get a nicely packed structure + BodyID mBodyID; ///< Optional body ID from which this shape comes + SubShapeIDCreator mSubShapeIDCreator; ///< Optional sub shape ID creator for the shape (can be used when expanding compound shapes into multiple transformed shapes) +}; + +static_assert(JPH_CPU_ADDRESS_BITS != 64 || sizeof(TransformedShape) == JPH_IF_SINGLE_PRECISION_ELSE(64, 96), "Not properly packed"); +static_assert(alignof(TransformedShape) == JPH_RVECTOR_ALIGNMENT, "Not properly aligned"); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/CalculateSolverSteps.h b/WickedEngine/Jolt/Physics/Constraints/CalculateSolverSteps.h new file mode 100644 index 000000000..857eddfb8 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/CalculateSolverSteps.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Class used to calculate the total number of velocity and position steps +class CalculateSolverSteps +{ +public: + /// Constructor + JPH_INLINE explicit CalculateSolverSteps(const PhysicsSettings &inSettings) : mSettings(inSettings) { } + + /// Combine the number of velocity and position steps for this body/constraint with the current values + template + JPH_INLINE void operator () (const Type *inObject) + { + uint num_velocity_steps = inObject->GetNumVelocityStepsOverride(); + mNumVelocitySteps = max(mNumVelocitySteps, num_velocity_steps); + mApplyDefaultVelocity |= num_velocity_steps == 0; + + uint num_position_steps = inObject->GetNumPositionStepsOverride(); + mNumPositionSteps = max(mNumPositionSteps, num_position_steps); + mApplyDefaultPosition |= num_position_steps == 0; + } + + /// Must be called after all bodies/constraints have been processed + JPH_INLINE void Finalize() + { + // If we have a default velocity/position step count, take the max of the default and the overrides + if (mApplyDefaultVelocity) + mNumVelocitySteps = max(mNumVelocitySteps, mSettings.mNumVelocitySteps); + if (mApplyDefaultPosition) + mNumPositionSteps = max(mNumPositionSteps, mSettings.mNumPositionSteps); + } + + /// Get the results of the calculation + JPH_INLINE uint GetNumPositionSteps() const { return mNumPositionSteps; } + JPH_INLINE uint GetNumVelocitySteps() const { return mNumVelocitySteps; } + +private: + const PhysicsSettings & mSettings; + + uint mNumVelocitySteps = 0; + uint mNumPositionSteps = 0; + + bool mApplyDefaultVelocity = false; + bool mApplyDefaultPosition = false; +}; + +/// Dummy class to replace the steps calculator when we don't need the result +class DummyCalculateSolverSteps +{ +public: + template + JPH_INLINE void operator () (const Type *) const + { + /* Nothing to do */ + } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConeConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/ConeConstraint.cpp new file mode 100644 index 000000000..9889fa643 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConeConstraint.cpp @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConeConstraintSettings) +{ + JPH_ADD_BASE_CLASS(ConeConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(ConeConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis1) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mTwistAxis2) + JPH_ADD_ATTRIBUTE(ConeConstraintSettings, mHalfConeAngle) +} + +void ConeConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mTwistAxis1); + inStream.Write(mPoint2); + inStream.Write(mTwistAxis2); + inStream.Write(mHalfConeAngle); +} + +void ConeConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mTwistAxis1); + inStream.Read(mPoint2); + inStream.Read(mTwistAxis2); + inStream.Read(mHalfConeAngle); +} + +TwoBodyConstraint *ConeConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new ConeConstraint(inBody1, inBody2, *this); +} + +ConeConstraint::ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Store limits + SetHalfConeAngle(inSettings.mHalfConeAngle); + + // Initialize rotation axis to perpendicular of twist axis in case the angle between the twist axis is 0 in the first frame + mWorldSpaceRotationAxis = inSettings.mTwistAxis1.GetNormalizedPerpendicular(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpaceTwistAxis1 = inv_transform1.Multiply3x3(inSettings.mTwistAxis1); + + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + mLocalSpaceTwistAxis2 = inv_transform2.Multiply3x3(inSettings.mTwistAxis2); + } + else + { + // Properties already in local space + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mLocalSpaceTwistAxis1 = inSettings.mTwistAxis1; + mLocalSpaceTwistAxis2 = inSettings.mTwistAxis2; + + // If they were in local space, we need to take the initial rotation axis to world space + mWorldSpaceRotationAxis = inBody1.GetRotation() * mWorldSpaceRotationAxis; + } +} + +void ConeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void ConeConstraint::CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Rotation is along the cross product of both twist axis + Vec3 twist1 = inRotation1.Multiply3x3(mLocalSpaceTwistAxis1); + Vec3 twist2 = inRotation2.Multiply3x3(mLocalSpaceTwistAxis2); + + // Calculate dot product between twist axis, if it's smaller than the cone angle we need to correct + mCosTheta = twist1.Dot(twist2); + if (mCosTheta < mCosHalfConeAngle) + { + // Rotation axis is defined by the two twist axis + Vec3 rot_axis = twist2.Cross(twist1); + + // If we can't find a rotation axis because the twist is too small, we'll use last frame's rotation axis + float len = rot_axis.Length(); + if (len > 0.0f) + mWorldSpaceRotationAxis = rot_axis / len; + + mAngleConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceRotationAxis); + } + else + mAngleConstraintPart.Deactivate(); +} + +void ConeConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + CalculateRotationConstraintProperties(rotation1, rotation2); +} + +void ConeConstraint::ResetWarmStart() +{ + mPointConstraintPart.Deactivate(); + mAngleConstraintPart.Deactivate(); +} + +void ConeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mAngleConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool ConeConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + bool rot = false; + if (mAngleConstraintPart.IsActive()) + rot = mAngleConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceRotationAxis, 0, FLT_MAX); + + return pos || rot; +} + +bool ConeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + bool rot = false; + CalculateRotationConstraintProperties(Mat44::sRotation(mBody1->GetRotation()), Mat44::sRotation(mBody2->GetRotation())); + if (mAngleConstraintPart.IsActive()) + rot = mAngleConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mCosTheta - mCosHalfConeAngle, inBaumgarte); + + return pos || rot; +} + +#ifdef JPH_DEBUG_RENDERER +void ConeConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + RVec3 p1 = transform1 * mLocalSpacePosition1; + RVec3 p2 = transform2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawMarker(p1, Color::sRed, 0.1f); + inRenderer->DrawMarker(p2, Color::sGreen, 0.1f); + + // Draw twist axis + inRenderer->DrawLine(p1, p1 + mDrawConstraintSize * transform1.Multiply3x3(mLocalSpaceTwistAxis1), Color::sRed); + inRenderer->DrawLine(p2, p2 + mDrawConstraintSize * transform2.Multiply3x3(mLocalSpaceTwistAxis2), Color::sGreen); +} + +void ConeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Vec3 twist_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1); + Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceTwistAxis1.GetNormalizedPerpendicular()); + + inRenderer->DrawOpenCone(position1, twist_axis1, normal_axis1, ACos(mCosHalfConeAngle), mDrawConstraintSize * mCosHalfConeAngle, Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void ConeConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); + mAngleConstraintPart.SaveState(inStream); + inStream.Write(mWorldSpaceRotationAxis); // When twist is too small, the rotation is used from last frame so we need to store it +} + +void ConeConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); + mAngleConstraintPart.RestoreState(inStream); + inStream.Read(mWorldSpaceRotationAxis); +} + +Ref ConeConstraint::GetConstraintSettings() const +{ + ConeConstraintSettings *settings = new ConeConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mTwistAxis1 = mLocalSpaceTwistAxis1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mTwistAxis2 = mLocalSpaceTwistAxis2; + settings->mHalfConeAngle = ACos(mCosHalfConeAngle); + return settings; +} + +Mat44 ConeConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceTwistAxis1.GetNormalizedPerpendicular(); + Vec3 perp2 = mLocalSpaceTwistAxis1.Cross(perp); + return Mat44(Vec4(mLocalSpaceTwistAxis1, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 ConeConstraint::GetConstraintToBody2Matrix() const +{ + // Note: Incorrect in rotation around the twist axis (the perpendicular does not match that of body 1), + // this should not matter as we're not limiting rotation around the twist axis. + Vec3 perp = mLocalSpaceTwistAxis2.GetNormalizedPerpendicular(); + Vec3 perp2 = mLocalSpaceTwistAxis2.Cross(perp); + return Mat44(Vec4(mLocalSpaceTwistAxis2, 0), Vec4(perp, 0), Vec4(perp2, 0), Vec4(mLocalSpacePosition2, 1)); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConeConstraint.h b/WickedEngine/Jolt/Physics/Constraints/ConeConstraint.h new file mode 100644 index 000000000..630d089dd --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConeConstraint.h @@ -0,0 +1,133 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Cone constraint settings, used to create a cone constraint +class JPH_EXPORT ConeConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConeConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mTwistAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mTwistAxis2 = Vec3::sAxisX(); + + /// Half of maximum angle between twist axis of body 1 and 2 + float mHalfConeAngle = 0.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A cone constraint constraints 2 bodies to a single point and limits the swing between the twist axis within a cone: +/// +/// t1 . t2 <= cos(theta) +/// +/// Where: +/// +/// t1 = twist axis of body 1. +/// t2 = twist axis of body 2. +/// theta = half cone angle (angle from the principal axis of the cone to the edge). +/// +/// Calculating the Jacobian: +/// +/// Constraint equation: +/// +/// C = t1 . t2 - cos(theta) +/// +/// Derivative: +/// +/// d/dt C = d/dt (t1 . t2) = (d/dt t1) . t2 + t1 . (d/dt t2) = (w1 x t1) . t2 + t1 . (w2 x t2) = (t1 x t2) . w1 + (t2 x t1) . w2 +/// +/// d/dt C = J v = [0, -t2 x t1, 0, t2 x t1] [v1, w1, v2, w2] +/// +/// Where J is the Jacobian. +/// +/// Note that this is the exact same equation as used in AngleConstraintPart if we use t2 x t1 as the world space axis +class JPH_EXPORT ConeConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct cone constraint + ConeConstraint(Body &inBody1, Body &inBody2, const ConeConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Cone; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Update maximum angle between body 1 and 2 (see ConeConstraintSettings) + void SetHalfConeAngle(float inHalfConeAngle) { JPH_ASSERT(inHalfConeAngle >= 0.0f && inHalfConeAngle <= JPH_PI); mCosHalfConeAngle = Cos(inHalfConeAngle); } + float GetCosHalfConeAngle() const { return mCosHalfConeAngle; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaRotation() const { return mAngleConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateRotationConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space constraint axis + Vec3 mLocalSpaceTwistAxis1; + Vec3 mLocalSpaceTwistAxis2; + + // Angular limits + float mCosHalfConeAngle; + + // RUN TIME PROPERTIES FOLLOW + + // Axis and angle of rotation between the two bodies + Vec3 mWorldSpaceRotationAxis; + float mCosTheta; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + AngleConstraintPart mAngleConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/Constraint.cpp b/WickedEngine/Jolt/Physics/Constraints/Constraint.cpp new file mode 100644 index 000000000..b81a8d81a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/Constraint.cpp @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ConstraintSettings) +{ + JPH_ADD_BASE_CLASS(ConstraintSettings, SerializableObject) + + JPH_ADD_ATTRIBUTE(ConstraintSettings, mEnabled) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mDrawConstraintSize) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mConstraintPriority) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumVelocityStepsOverride) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mNumPositionStepsOverride) + JPH_ADD_ATTRIBUTE(ConstraintSettings, mUserData) +} + +void ConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); + inStream.Write(mEnabled); + inStream.Write(mDrawConstraintSize); + inStream.Write(mConstraintPriority); + inStream.Write(mNumVelocityStepsOverride); + inStream.Write(mNumPositionStepsOverride); +} + +void ConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mEnabled); + inStream.Read(mDrawConstraintSize); + inStream.Read(mConstraintPriority); + inStream.Read(mNumVelocityStepsOverride); + inStream.Read(mNumPositionStepsOverride); +} + +ConstraintSettings::ConstraintResult ConstraintSettings::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &ConstraintSettings::RestoreBinaryState); +} + +void Constraint::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mEnabled); +} + +void Constraint::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mEnabled); +} + +void Constraint::ToConstraintSettings(ConstraintSettings &outSettings) const +{ + outSettings.mEnabled = mEnabled; + outSettings.mConstraintPriority = mConstraintPriority; + outSettings.mNumVelocityStepsOverride = mNumVelocityStepsOverride; + outSettings.mNumPositionStepsOverride = mNumPositionStepsOverride; + outSettings.mUserData = mUserData; +#ifdef JPH_DEBUG_RENDERER + outSettings.mDrawConstraintSize = mDrawConstraintSize; +#endif // JPH_DEBUG_RENDERER +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/Constraint.h b/WickedEngine/Jolt/Physics/Constraints/Constraint.h new file mode 100644 index 000000000..f8827cf68 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/Constraint.h @@ -0,0 +1,238 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class BodyID; +class IslandBuilder; +class LargeIslandSplitter; +class BodyManager; +class StateRecorder; +class StreamIn; +class StreamOut; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Enum to identify constraint type +enum class EConstraintType +{ + Constraint, + TwoBodyConstraint, +}; + +/// Enum to identify constraint sub type +enum class EConstraintSubType +{ + Fixed, + Point, + Hinge, + Slider, + Distance, + Cone, + SwingTwist, + SixDOF, + Path, + Vehicle, + RackAndPinion, + Gear, + Pulley, + + /// User defined constraint types start here + User1, + User2, + User3, + User4 +}; + +/// Certain constraints support setting them up in local or world space. This governs what is used. +enum class EConstraintSpace +{ + LocalToBodyCOM, ///< All constraint properties are specified in local space to center of mass of the bodies that are being constrained (so e.g. 'constraint position 1' will be local to body 1 COM, 'constraint position 2' will be local to body 2 COM). Note that this means you need to subtract Shape::GetCenterOfMass() from positions! + WorldSpace, ///< All constraint properties are specified in world space +}; + +/// Class used to store the configuration of a constraint. Allows run-time creation of constraints. +class JPH_EXPORT ConstraintSettings : public SerializableObject, public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, ConstraintSettings) + + using ConstraintResult = Result>; + + /// Saves the contents of the constraint settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a constraint of the correct type and restores its contents from the binary stream inStream. + static ConstraintResult sRestoreFromBinaryState(StreamIn &inStream); + + /// If this constraint is enabled initially. Use Constraint::SetEnabled to toggle after creation. + bool mEnabled = true; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + /// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering. + uint32 mConstraintPriority = 0; + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumVelocityStepsOverride = 0; + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint mNumPositionStepsOverride = 0; + + /// Size of constraint when drawing it through the debug renderer + float mDrawConstraintSize = 1.0f; + + /// User data value (can be used by application) + uint64 mUserData = 0; + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); +}; + +/// Base class for all physics constraints. A constraint removes one or more degrees of freedom for a rigid body. +class JPH_EXPORT Constraint : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Constraint(const ConstraintSettings &inSettings) : +#ifdef JPH_DEBUG_RENDERER + mDrawConstraintSize(inSettings.mDrawConstraintSize), +#endif // JPH_DEBUG_RENDERER + mConstraintPriority(inSettings.mConstraintPriority), + mNumVelocityStepsOverride(uint8(inSettings.mNumVelocityStepsOverride)), + mNumPositionStepsOverride(uint8(inSettings.mNumPositionStepsOverride)), + mEnabled(inSettings.mEnabled), + mUserData(inSettings.mUserData) + { + JPH_ASSERT(inSettings.mNumVelocityStepsOverride < 256); + JPH_ASSERT(inSettings.mNumPositionStepsOverride < 256); + } + + /// Virtual destructor + virtual ~Constraint() = default; + + /// Get the type of a constraint + virtual EConstraintType GetType() const { return EConstraintType::Constraint; } + + /// Get the sub type of a constraint + virtual EConstraintSubType GetSubType() const = 0; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + /// Note that if you want a deterministic simulation and you cannot guarantee the order in which constraints are added/removed, you can make the priority for all constraints unique to get a deterministic ordering. + uint32 GetConstraintPriority() const { return mConstraintPriority; } + void SetConstraintPriority(uint32 inPriority) { mConstraintPriority = inPriority; } + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumVelocityStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumVelocityStepsOverride = uint8(inN); } + uint GetNumVelocityStepsOverride() const { return mNumVelocityStepsOverride; } + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + void SetNumPositionStepsOverride(uint inN) { JPH_ASSERT(inN < 256); mNumPositionStepsOverride = uint8(inN); } + uint GetNumPositionStepsOverride() const { return mNumPositionStepsOverride; } + + /// Enable / disable this constraint. This can e.g. be used to implement a breakable constraint by detecting that the constraint impulse + /// (see e.g. PointConstraint::GetTotalLambdaPosition) went over a certain limit and then disabling the constraint. + /// Note that although a disabled constraint will not affect the simulation in any way anymore, it does incur some processing overhead. + /// Alternatively you can remove a constraint from the constraint manager (which may be more costly if you want to disable the constraint for a short while). + void SetEnabled(bool inEnabled) { mEnabled = inEnabled; } + + /// Test if a constraint is enabled. + bool GetEnabled() const { return mEnabled; } + + /// Access to the user data, can be used for anything by the application + uint64 GetUserData() const { return mUserData; } + void SetUserData(uint64 inUserData) { mUserData = inUserData; } + + /// Notify the constraint that the shape of a body has changed and that its center of mass has moved by inDeltaCOM. + /// Bodies don't know which constraints are connected to them so the user is responsible for notifying the relevant constraints when a body changes. + /// @param inBodyID ID of the body that has changed + /// @param inDeltaCOM The delta of the center of mass of the body (shape->GetCenterOfMass() - shape_before_change->GetCenterOfMass()) + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) = 0; + + /// Notify the system that the configuration of the bodies and/or constraint has changed enough so that the warm start impulses should not be applied the next frame. + /// You can use this function for example when repositioning a ragdoll through Ragdoll::SetPose in such a way that the orientation of the bodies completely changes so that + /// the previous frame impulses are no longer a good approximation of what the impulses will be in the next frame. Calling this function when there are no big changes + /// will result in the constraints being much 'softer' than usual so they are more easily violated (e.g. a long chain of bodies might sag a bit if you call this every frame). + virtual void ResetWarmStart() = 0; + + ///@name Solver interface + ///@{ + virtual bool IsActive() const { return mEnabled; } + virtual void SetupVelocityConstraint(float inDeltaTime) = 0; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) = 0; + virtual bool SolveVelocityConstraint(float inDeltaTime) = 0; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) = 0; + ///@} + + /// Link bodies that are connected by this constraint in the island builder + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) = 0; + + /// Link bodies that are connected by this constraint in the same split. Returns the split index. + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const = 0; + +#ifdef JPH_DEBUG_RENDERER + // Drawing interface + virtual void DrawConstraint(DebugRenderer *inRenderer) const = 0; + virtual void DrawConstraintLimits([[maybe_unused]] DebugRenderer *inRenderer) const { } + virtual void DrawConstraintReferenceFrame([[maybe_unused]] DebugRenderer *inRenderer) const { } + + /// Size of constraint when drawing it through the debug renderer + float GetDrawConstraintSize() const { return mDrawConstraintSize; } + void SetDrawConstraintSize(float inSize) { mDrawConstraintSize = inSize; } +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + virtual void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + virtual void RestoreState(StateRecorder &inStream); + + /// Debug function to convert a constraint to its settings, note that this will not save to which bodies the constraint is connected to + virtual Ref GetConstraintSettings() const = 0; + +protected: + /// Helper function to copy settings back to constraint settings for this base class + void ToConstraintSettings(ConstraintSettings &outSettings) const; + +#ifdef JPH_DEBUG_RENDERER + /// Size of constraint when drawing it through the debug renderer + float mDrawConstraintSize; +#endif // JPH_DEBUG_RENDERER + +private: + friend class ConstraintManager; + + /// Index that indicates this constraint is not in the constraint manager + static constexpr uint32 cInvalidConstraintIndex = 0xffffffff; + + /// Index in the mConstraints list of the ConstraintManager for easy finding + uint32 mConstraintIndex = cInvalidConstraintIndex; + + /// Priority of the constraint when solving. Higher numbers have are more likely to be solved correctly. + uint32 mConstraintPriority = 0; + + /// Used only when the constraint is active. Override for the number of solver velocity iterations to run, 0 means use the default in PhysicsSettings::mNumVelocitySteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumVelocityStepsOverride = 0; + + /// Used only when the constraint is active. Override for the number of solver position iterations to run, 0 means use the default in PhysicsSettings::mNumPositionSteps. The number of iterations to use is the max of all contacts and constraints in the island. + uint8 mNumPositionStepsOverride = 0; + + /// If this constraint is currently enabled + bool mEnabled = true; + + /// User data value (can be used by application) + uint64 mUserData; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintManager.cpp b/WickedEngine/Jolt/Physics/Constraints/ConstraintManager.cpp new file mode 100644 index 000000000..509bddbd9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintManager.cpp @@ -0,0 +1,289 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void ConstraintManager::Add(Constraint **inConstraints, int inNumber) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + mConstraints.reserve(mConstraints.size() + inNumber); + + for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c) + { + Constraint *constraint = *c; + + // Assume this constraint has not been added yet + JPH_ASSERT(constraint->mConstraintIndex == Constraint::cInvalidConstraintIndex); + + // Add to the list + constraint->mConstraintIndex = uint32(mConstraints.size()); + mConstraints.push_back(constraint); + } +} + +void ConstraintManager::Remove(Constraint **inConstraints, int inNumber) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (Constraint **c = inConstraints, **c_end = inConstraints + inNumber; c < c_end; ++c) + { + Constraint *constraint = *c; + + // Reset constraint index for this constraint + uint32 this_constraint_idx = constraint->mConstraintIndex; + constraint->mConstraintIndex = Constraint::cInvalidConstraintIndex; + JPH_ASSERT(this_constraint_idx != Constraint::cInvalidConstraintIndex); + + // Check if this constraint is somewhere in the middle of the constraints, in this case we need to move the last constraint to this position + uint32 last_constraint_idx = uint32(mConstraints.size() - 1); + if (this_constraint_idx < last_constraint_idx) + { + Constraint *last_constraint = mConstraints[last_constraint_idx]; + last_constraint->mConstraintIndex = this_constraint_idx; + mConstraints[this_constraint_idx] = last_constraint; + } + + // Pop last constraint + mConstraints.pop_back(); + } +} + +Constraints ConstraintManager::GetConstraints() const +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + Constraints copy = mConstraints; + return copy; +} + +void ConstraintManager::GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inEndConstraintIdx <= mConstraints.size()); + + uint32 num_active_constraints = 0; + for (uint32 constraint_idx = inStartConstraintIdx; constraint_idx < inEndConstraintIdx; ++constraint_idx) + { + Constraint *c = mConstraints[constraint_idx]; + JPH_ASSERT(c->mConstraintIndex == constraint_idx); + if (c->IsActive()) + { + *(outActiveConstraints++) = c; + num_active_constraints++; + } + } + + outNumActiveConstraints = num_active_constraints; +} + +void ConstraintManager::sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + JPH_PROFILE_FUNCTION(); + + for (uint32 constraint_idx = 0; constraint_idx < inNumActiveConstraints; ++constraint_idx) + { + Constraint *c = inActiveConstraints[constraint_idx]; + c->BuildIslands(constraint_idx, ioBuilder, inBodyManager); + } +} + +void ConstraintManager::sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [inActiveConstraints](uint32 inLHS, uint32 inRHS) { + const Constraint *lhs = inActiveConstraints[inLHS]; + const Constraint *rhs = inActiveConstraints[inRHS]; + + if (lhs->GetConstraintPriority() != rhs->GetConstraintPriority()) + return lhs->GetConstraintPriority() < rhs->GetConstraintPriority(); + + return lhs->mConstraintIndex < rhs->mConstraintIndex; + }); +} + +void ConstraintManager::sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + for (Constraint **c = inActiveConstraints, **c_end = inActiveConstraints + inNumActiveConstraints; c < c_end; ++c) + (*c)->SetupVelocityConstraint(inDeltaTime); +} + +template +void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback) +{ + JPH_PROFILE_FUNCTION(); + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + ioCallback(c); + c->WarmStartVelocityConstraint(inWarmStartImpulseRatio); + } +} + +// Specialize for the two constraint callback types +template void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback); +template void ConstraintManager::sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback); + +bool ConstraintManager::sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + any_impulse_applied |= c->SolveVelocityConstraint(inDeltaTime); + } + + return any_impulse_applied; +} + +bool ConstraintManager::sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + Constraint *c = inActiveConstraints[*constraint_idx]; + any_impulse_applied |= c->SolvePositionConstraint(inDeltaTime, inBaumgarte); + } + + return any_impulse_applied; +} + +#ifdef JPH_DEBUG_RENDERER +void ConstraintManager::DrawConstraints(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraint(inRenderer); +} + +void ConstraintManager::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraintLimits(inRenderer); +} + +void ConstraintManager::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const +{ + JPH_PROFILE_FUNCTION(); + + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + for (const Ref &c : mConstraints) + c->DrawConstraintReferenceFrame(inRenderer); +} +#endif // JPH_DEBUG_RENDERER + +void ConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + // Write state of constraints + if (inFilter != nullptr) + { + // Determine which constraints to save + Array constraints; + constraints.reserve(mConstraints.size()); + for (const Ref &c : mConstraints) + if (inFilter->ShouldSaveConstraint(*c)) + constraints.push_back(c); + + // Save them + uint32 num_constraints = (uint32)constraints.size(); + inStream.Write(num_constraints); + for (const Constraint *c : constraints) + { + inStream.Write(c->mConstraintIndex); + c->SaveState(inStream); + } + } + else + { + // Save all constraints + uint32 num_constraints = (uint32)mConstraints.size(); + inStream.Write(num_constraints); + for (const Ref &c : mConstraints) + { + inStream.Write(c->mConstraintIndex); + c->SaveState(inStream); + } + } +} + +bool ConstraintManager::RestoreState(StateRecorder &inStream) +{ + UniqueLock lock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); + + if (inStream.IsValidating()) + { + // Read state of constraints + uint32 num_constraints = (uint32)mConstraints.size(); // Initialize to current value for validation + inStream.Read(num_constraints); + if (num_constraints != mConstraints.size()) + { + JPH_ASSERT(false, "Cannot handle adding/removing constraints"); + return false; + } + for (const Ref &c : mConstraints) + { + uint32 constraint_index = c->mConstraintIndex; + inStream.Read(constraint_index); + if (constraint_index != c->mConstraintIndex) + { + JPH_ASSERT(false, "Unexpected constraint index"); + return false; + } + c->RestoreState(inStream); + } + } + else + { + // Not validating, use more flexible reading, read number of constraints + uint32 num_constraints = 0; + inStream.Read(num_constraints); + + for (uint32 idx = 0; idx < num_constraints; ++idx) + { + uint32 constraint_index; + inStream.Read(constraint_index); + if (mConstraints.size() <= constraint_index) + { + JPH_ASSERT(false, "Restoring state for non-existing constraint"); + return false; + } + mConstraints[constraint_index]->RestoreState(inStream); + } + } + + return true; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintManager.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintManager.h new file mode 100644 index 000000000..7be8b0692 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintManager.h @@ -0,0 +1,99 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class IslandBuilder; +class BodyManager; +class StateRecorderFilter; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// A list of constraints +using Constraints = Array>; + +/// A constraint manager manages all constraints of the same type +class JPH_EXPORT ConstraintManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + +#ifdef JPH_ENABLE_ASSERTS + /// Constructor + ConstraintManager(PhysicsLockContext inContext) : mLockContext(inContext) { } +#endif // JPH_ENABLE_ASSERTS + + /// Add a new constraint. This is thread safe. + /// Note that the inConstraints array is allowed to have nullptrs, these will be ignored. + void Add(Constraint **inConstraints, int inNumber); + + /// Remove a constraint. This is thread safe. + /// Note that the inConstraints array is allowed to have nullptrs, these will be ignored. + void Remove(Constraint **inConstraint, int inNumber); + + /// Get a list of all constraints + Constraints GetConstraints() const; + + /// Get total number of constraints + inline uint32 GetNumConstraints() const { return uint32(mConstraints.size()); } + + /// Determine the active constraints of a subset of the constraints + void GetActiveConstraints(uint32 inStartConstraintIdx, uint32 inEndConstraintIdx, Constraint **outActiveConstraints, uint32 &outNumActiveConstraints) const; + + /// Link bodies to form islands + static void sBuildIslands(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, IslandBuilder &ioBuilder, BodyManager &inBodyManager); + + /// In order to have a deterministic simulation, we need to sort the constraints of an island before solving them + static void sSortConstraints(Constraint **inActiveConstraints, uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd); + + /// Prior to solving the velocity constraints, you must call SetupVelocityConstraints once to precalculate values that are independent of velocity + static void sSetupVelocityConstraints(Constraint **inActiveConstraints, uint32 inNumActiveConstraints, float inDeltaTime); + + /// Apply last frame's impulses, must be called prior to SolveVelocityConstraints + template + static void sWarmStartVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, ConstraintCallback &ioCallback); + + /// This function is called multiple times to iteratively come to a solution that meets all velocity constraints + static bool sSolveVelocityConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime); + + /// This function is called multiple times to iteratively come to a solution that meets all position constraints + static bool sSolvePositionConstraints(Constraint **inActiveConstraints, const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inDeltaTime, float inBaumgarte); + +#ifdef JPH_DEBUG_RENDERER + /// Draw all constraints + void DrawConstraints(DebugRenderer *inRenderer) const; + + /// Draw all constraint limits + void DrawConstraintLimits(DebugRenderer *inRenderer) const; + + /// Draw all constraint reference frames + void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const; +#endif // JPH_DEBUG_RENDERER + + /// Save state of constraints + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restore the state of constraints. Returns false if failed. + bool RestoreState(StateRecorder &inStream); + + /// Lock all constraints. This should only be done during PhysicsSystem::Update(). + void LockAllConstraints() { PhysicsLock::sLock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); } + void UnlockAllConstraints() { PhysicsLock::sUnlock(mConstraintsMutex JPH_IF_ENABLE_ASSERTS(, mLockContext, EPhysicsLockTypes::ConstraintsList)); } + +private: +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mLockContext; +#endif // JPH_ENABLE_ASSERTS + Constraints mConstraints; + mutable Mutex mConstraintsMutex; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h new file mode 100644 index 000000000..f7493102e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AngleConstraintPart.h @@ -0,0 +1,257 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains rotation along 1 axis +/// +/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, see section 2.4.5 +/// +/// Constraint equation (eq 108): +/// +/// \f[C = \theta(t) - \theta_{min}\f] +/// +/// Jacobian (eq 109): +/// +/// \f[J = \begin{bmatrix}0 & -a^T & 0 & a^T\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which rotation is constrained (normalized).\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class AngleConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(inLambda * mInvI1_Axis); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_Axis); + return true; + } + + return false; + } + + /// Internal helper function to calculate the inverse effective mass + JPH_INLINE float CalculateInverseEffectiveMass(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis) + { + JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-4f)); + + // Calculate properties used below + mInvI1_Axis = inBody1.IsDynamic()? inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceAxis) : Vec3::sZero(); + mInvI2_Axis = inBody2.IsDynamic()? inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceAxis) : Vec3::sZero(); + + // Calculate inverse effective mass: K = J M^-1 J^T + return inWorldSpaceAxis.Dot(mInvI1_Axis + mInvI2_Axis); + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C) + /// @param inFrequency Oscillation frequency (Hz) + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping) + inline void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass); + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// Set the following terms to zero if you don't want to drive the constraint to zero with a spring: + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C) + /// @param inStiffness Spring stiffness k. + /// @param inDamping Spring damping coefficient c. + inline void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass); + } + + /// Selects one of the above functions based on the spring settings + inline void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, const Body &inBody2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inBody2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping) + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis The axis of rotation along which the constraint acts (normalized) + /// @param inMinLambda Minimum angular impulse to apply (N m s) + /// @param inMaxLambda Maximum angular impulse to apply (N m s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (inWorldSpaceAxis.Dot(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()) - mSpringPart.GetBias(mTotalLambda)); + float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse + lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply + mTotalLambda = new_lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(lambda * mInvI1_Axis); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(lambda * mInvI2_Axis); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_Axis; + Vec3 mInvI2_Axis; + float mEffectiveMass = 0.0f; + SpringPart mSpringPart; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h new file mode 100644 index 000000000..a41ee3af3 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/AxisConstraintPart.h @@ -0,0 +1,682 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains motion along 1 axis +/// +/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.1.1 +/// (we're not using the approximation of eq 27 but instead add the U term as in eq 55) +/// +/// Constraint equation (eq 25): +/// +/// \f[C = (p_2 - p_1) \cdot n\f] +/// +/// Jacobian (eq 28): +/// +/// \f[J = \begin{bmatrix} -n^T & (-(r_1 + u) \times n)^T & n^T & (r_2 \times n)^T \end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// n = constraint axis (normalized).\n +/// p1, p2 = constraint points.\n +/// r1 = p1 - x1.\n +/// r2 = p2 - x2.\n +/// u = x2 + r2 - x1 - r1 = p2 - p1.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class AxisConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + template + JPH_INLINE bool ApplyVelocityStep(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if constexpr (Type1 == EMotionType::Dynamic) + { + ioMotionProperties1->SubLinearVelocityStep((inLambda * inInvMass1) * inWorldSpaceAxis); + ioMotionProperties1->SubAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if constexpr (Type2 == EMotionType::Dynamic) + { + ioMotionProperties2->AddLinearVelocityStep((inLambda * inInvMass2) * inWorldSpaceAxis); + ioMotionProperties2->AddAngularVelocityStep(inLambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Internal helper function to calculate the inverse effective mass + template + JPH_INLINE float TemplatedCalculateInverseEffectiveMass(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + JPH_ASSERT(inWorldSpaceAxis.IsNormalized(1.0e-5f)); + + // Calculate properties used below + Vec3 r1_plus_u_x_axis; + if constexpr (Type1 != EMotionType::Static) + { + r1_plus_u_x_axis = inR1PlusU.Cross(inWorldSpaceAxis); + r1_plus_u_x_axis.StoreFloat3(&mR1PlusUxAxis); + } + else + { + #ifdef JPH_DEBUG + Vec3::sNaN().StoreFloat3(&mR1PlusUxAxis); + #endif + } + + Vec3 r2_x_axis; + if constexpr (Type2 != EMotionType::Static) + { + r2_x_axis = inR2.Cross(inWorldSpaceAxis); + r2_x_axis.StoreFloat3(&mR2xAxis); + } + else + { + #ifdef JPH_DEBUG + Vec3::sNaN().StoreFloat3(&mR2xAxis); + #endif + } + + // Calculate inverse effective mass: K = J M^-1 J^T + float inv_effective_mass; + + if constexpr (Type1 == EMotionType::Dynamic) + { + Vec3 invi1_r1_plus_u_x_axis = inInvI1.Multiply3x3(r1_plus_u_x_axis); + invi1_r1_plus_u_x_axis.StoreFloat3(&mInvI1_R1PlusUxAxis); + inv_effective_mass = inInvMass1 + invi1_r1_plus_u_x_axis.Dot(r1_plus_u_x_axis); + } + else + { + (void)r1_plus_u_x_axis; // Fix compiler warning: Not using this (it's not calculated either) + JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI1_R1PlusUxAxis);) + inv_effective_mass = 0.0f; + } + + if constexpr (Type2 == EMotionType::Dynamic) + { + Vec3 invi2_r2_x_axis = inInvI2.Multiply3x3(r2_x_axis); + invi2_r2_x_axis.StoreFloat3(&mInvI2_R2xAxis); + inv_effective_mass += inInvMass2 + invi2_r2_x_axis.Dot(r2_x_axis); + } + else + { + (void)r2_x_axis; // Fix compiler warning: Not using this (it's not calculated either) + JPH_IF_DEBUG(Vec3::sNaN().StoreFloat3(&mInvI2_R2xAxis);) + } + + return inv_effective_mass; + } + + /// Internal helper function to calculate the inverse effective mass + JPH_INLINE float CalculateInverseEffectiveMass(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + float inv_m1 = mp1->GetInverseMass(); + Mat44 inv_i1 = inBody1.GetInverseInertia(); + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Kinematic: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + case EMotionType::Static: + return TemplatedCalculateInverseEffectiveMass(inv_m1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + default: + break; + } + break; + } + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inBody2.GetMotionPropertiesUnchecked()->GetInverseMass(), inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + default: + break; + } + + JPH_ASSERT(false); + return 0.0f; + } + + /// Internal helper function to calculate the inverse effective mass, version that supports mass scaling + JPH_INLINE float CalculateInverseEffectiveMassWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis) + { + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + { + Mat44 inv_i1 = inInvInertiaScale1 * inBody1.GetInverseInertia(); + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Kinematic: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + case EMotionType::Static: + return TemplatedCalculateInverseEffectiveMass(inInvMass1, inv_i1, inR1PlusU, 0 /* Will not be used */, Mat44() /* Will not be used */, inR2, inWorldSpaceAxis); + + default: + break; + } + break; + } + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + return TemplatedCalculateInverseEffectiveMass(0 /* Will not be used */, Mat44() /* Will not be used */, inR1PlusU, inInvMass2, inInvInertiaScale2 * inBody2.GetInverseInertia(), inR2, inWorldSpaceAxis); + + default: + break; + } + + JPH_ASSERT(false); + return 0.0f; + } + +public: + /// Templated form of CalculateConstraintProperties with the motion types baked in + template + JPH_INLINE void TemplatedCalculateConstraintProperties(float inInvMass1, Mat44Arg inInvI1, Vec3Arg inR1PlusU, float inInvMass2, Mat44Arg inInvI2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = TemplatedCalculateInverseEffectiveMass(inInvMass1, inInvI1, inR1PlusU, inInvMass2, inInvI2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + + JPH_DET_LOG("TemplatedCalculateConstraintProperties: invM1: " << inInvMass1 << " invI1: " << inInvI1 << " r1PlusU: " << inR1PlusU << " invM2: " << inInvMass2 << " invI2: " << inInvI2 << " r2: " << inR2 << " bias: " << inBias << " r1PlusUxAxis: " << mR1PlusUxAxis << " r2xAxis: " << mR2xAxis << " invI1_R1PlusUxAxis: " << mInvI1_R1PlusUxAxis << " invI2_R2xAxis: " << mInvI2_R2xAxis << " effectiveMass: " << mEffectiveMass << " totalLambda: " << mTotalLambda); + } + + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below, version that supports mass scaling + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inInvInertiaScale1 Scale factor for the inverse inertia of body 1 + /// @param inInvInertiaScale2 Scale factor for the inverse inertia of body 2 + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateConstraintPropertiesWithMassOverride(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, Vec3Arg inR1PlusU, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias = 0.0f) + { + float inv_effective_mass = CalculateInverseEffectiveMassWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, inR1PlusU, inBody2, inInvMass2, inInvInertiaScale2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + { + mEffectiveMass = 1.0f / inv_effective_mass; + mSpringPart.CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). + /// @param inFrequency Oscillation frequency (Hz). + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping). + inline void CalculateConstraintPropertiesWithFrequencyAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inFrequency, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inFrequency, inDamping, mEffectiveMass); + } + + /// Calculate properties used during the functions below + /// @param inDeltaTime Time step + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1PlusU See equations above (r1 + u) + /// @param inR2 See equations above (r2) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized, pointing from body 1 to 2) + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). + /// @param inStiffness Spring stiffness k. + /// @param inDamping Spring damping coefficient c. + inline void CalculateConstraintPropertiesWithStiffnessAndDamping(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, float inStiffness, float inDamping) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inStiffness, inDamping, mEffectiveMass); + } + + /// Selects one of the above functions based on the spring settings + inline void CalculateConstraintPropertiesWithSettings(float inDeltaTime, const Body &inBody1, Vec3Arg inR1PlusU, const Body &inBody2, Vec3Arg inR2, Vec3Arg inWorldSpaceAxis, float inBias, float inC, const SpringSettings &inSpringSettings) + { + float inv_effective_mass = CalculateInverseEffectiveMass(inBody1, inR1PlusU, inBody2, inR2, inWorldSpaceAxis); + + if (inv_effective_mass == 0.0f) + Deactivate(); + else if (inSpringSettings.mMode == ESpringMode::FrequencyAndDamping) + mSpringPart.CalculateSpringPropertiesWithFrequencyAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mFrequency, inSpringSettings.mDamping, mEffectiveMass); + else + mSpringPart.CalculateSpringPropertiesWithStiffnessAndDamping(inDeltaTime, inv_effective_mass, inBias, inC, inSpringSettings.mStiffness, inSpringSettings.mDamping, mEffectiveMass); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Templated form of WarmStart with the motion types baked in + template + inline void TemplatedWarmStart(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + + ApplyVelocityStep(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, mTotalLambda); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inWarmStartImpulseRatio) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + // Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies + if (motion_type1 == EMotionType::Dynamic) + { + if (motion_type2 == EMotionType::Dynamic) + TemplatedWarmStart(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio); + else + TemplatedWarmStart(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inWarmStartImpulseRatio); + } + else + { + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + TemplatedWarmStart(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inWarmStartImpulseRatio); + } + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in, part 1: get the total lambda + template + JPH_INLINE float TemplatedSolveVelocityConstraintGetTotalLambda(const MotionProperties *ioMotionProperties1, const MotionProperties *ioMotionProperties2, Vec3Arg inWorldSpaceAxis) const + { + // Calculate jacobian multiplied by linear velocity + float jv; + if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity() - ioMotionProperties2->GetLinearVelocity()); + else if constexpr (Type1 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(ioMotionProperties1->GetLinearVelocity()); + else if constexpr (Type2 != EMotionType::Static) + jv = inWorldSpaceAxis.Dot(-ioMotionProperties2->GetLinearVelocity()); + else + JPH_ASSERT(false); // Static vs static is nonsensical! + + // Calculate jacobian multiplied by angular velocity + if constexpr (Type1 != EMotionType::Static) + jv += Vec3::sLoadFloat3Unsafe(mR1PlusUxAxis).Dot(ioMotionProperties1->GetAngularVelocity()); + if constexpr (Type2 != EMotionType::Static) + jv -= Vec3::sLoadFloat3Unsafe(mR2xAxis).Dot(ioMotionProperties2->GetAngularVelocity()); + + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (jv - mSpringPart.GetBias(mTotalLambda)); + + // Return the total accumulated lambda + return mTotalLambda + lambda; + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in, part 2: apply new lambda + template + JPH_INLINE bool TemplatedSolveVelocityConstraintApplyLambda(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inTotalLambda) + { + float delta_lambda = inTotalLambda - mTotalLambda; // Calculate change in lambda + mTotalLambda = inTotalLambda; // Store accumulated impulse + + return ApplyVelocityStep(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, delta_lambda); + } + + /// Templated form of SolveVelocityConstraint with the motion types baked in + template + inline bool TemplatedSolveVelocityConstraint(MotionProperties *ioMotionProperties1, float inInvMass1, MotionProperties *ioMotionProperties2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + float total_lambda = TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, inWorldSpaceAxis); + + // Clamp impulse to specified range + total_lambda = Clamp(total_lambda, inMinLambda, inMaxLambda); + + return TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, inInvMass1, ioMotionProperties2, inInvMass2, inWorldSpaceAxis, total_lambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inMinLambda Minimum value of constraint impulse to apply (N s) + /// @param inMaxLambda Maximum value of constraint impulse to apply (N s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Kinematic: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + return TemplatedSolveVelocityConstraint(motion_properties1, motion_properties1->GetInverseMass(), motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, motion_properties2->GetInverseMass(), inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + + return false; + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inMinLambda Minimum value of constraint impulse to apply (N s) + /// @param inMaxLambda Maximum value of constraint impulse to apply (N s) + inline bool SolveVelocityConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inMinLambda, float inMaxLambda) + { + EMotionType motion_type1 = ioBody1.GetMotionType(); + MotionProperties *motion_properties1 = ioBody1.GetMotionPropertiesUnchecked(); + + EMotionType motion_type2 = ioBody2.GetMotionType(); + MotionProperties *motion_properties2 = ioBody2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Kinematic: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + return TemplatedSolveVelocityConstraint(motion_properties1, inInvMass1, motion_properties2, 0.0f /* Unused */, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + return TemplatedSolveVelocityConstraint(motion_properties1, 0.0f /* Unused */, motion_properties2, inInvMass2, inWorldSpaceAxis, inMinLambda, inMaxLambda); + + default: + JPH_ASSERT(false); + break; + } + + return false; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep((lambda * ioBody1.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis); + ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * ioBody2.GetMotionProperties()->GetInverseMass()) * inWorldSpaceAxis); + ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inInvMass1 The inverse mass of body 1 (only used when body 1 is dynamic) + /// @param inInvMass2 The inverse mass of body 2 (only used when body 2 is dynamic) + /// @param inWorldSpaceAxis Axis along which the constraint acts (normalized) + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraintWithMassOverride(Body &ioBody1, float inInvMass1, Body &ioBody2, float inInvMass2, Vec3Arg inWorldSpaceAxis, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f && !mSpringPart.IsActive()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep((lambda * inInvMass1) * inWorldSpaceAxis); + ioBody1.SubRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI1_R1PlusUxAxis)); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * inInvMass2) * inWorldSpaceAxis); + ioBody2.AddRotationStep(lambda * Vec3::sLoadFloat3Unsafe(mInvI2_R2xAxis)); + } + return true; + } + + return false; + } + + /// Override total lagrange multiplier, can be used to set the initial value for warm starting + inline void SetTotalLambda(float inLambda) + { + mTotalLambda = inLambda; + } + + /// Return lagrange multiplier + inline float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Float3 mR1PlusUxAxis; + Float3 mR2xAxis; + Float3 mInvI1_R1PlusUxAxis; + Float3 mInvI2_R2xAxis; + float mEffectiveMass = 0.0f; + SpringPart mSpringPart; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h new file mode 100644 index 000000000..c0ebe5ac4 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/DualAxisConstraintPart.h @@ -0,0 +1,276 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/** + Constrains movement on 2 axis + + @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.3.1 + + Constraint equation (eq 51): + + \f[C = \begin{bmatrix} (p_2 - p_1) \cdot n_1 \\ (p_2 - p_1) \cdot n_2\end{bmatrix}\f] + + Jacobian (transposed) (eq 55): + + \f[J^T = \begin{bmatrix} + -n_1 & -n_2 \\ + -(r_1 + u) \times n_1 & -(r_1 + u) \times n_2 \\ + n_1 & n_2 \\ + r_2 \times n_1 & r_2 \times n_2 + \end{bmatrix}\f] + + Used terms (here and below, everything in world space):\n + n1, n2 = constraint axis (normalized).\n + p1, p2 = constraint points.\n + r1 = p1 - x1.\n + r2 = p2 - x2.\n + u = x2 + r2 - x1 - r1 = p2 - p1.\n + x1, x2 = center of mass for the bodies.\n + v = [v1, w1, v2, w2].\n + v1, v2 = linear velocity of body 1 and 2.\n + w1, w2 = angular velocity of body 1 and 2.\n + M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n + \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n + b = velocity bias.\n + \f$\beta\f$ = baumgarte constant. +**/ +class DualAxisConstraintPart +{ +public: + using Vec2 = Vector<2>; + using Mat22 = Matrix<2, 2>; + +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, const Vec2 &inLambda) const + { + // Apply impulse if delta is not zero + if (!inLambda.IsZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + Vec3 impulse = inN1 * inLambda[0] + inN2 * inLambda[1]; + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->SubLinearVelocityStep(mp1->GetInverseMass() * impulse); + mp1->SubAngularVelocityStep(mInvI1_R1PlusUxN1 * inLambda[0] + mInvI1_R1PlusUxN2 * inLambda[1]); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep(mp2->GetInverseMass() * impulse); + mp2->AddAngularVelocityStep(mInvI2_R2xN1 * inLambda[0] + mInvI2_R2xN2 * inLambda[1]); + } + return true; + } + + return false; + } + + /// Internal helper function to calculate the lagrange multiplier + inline void CalculateLagrangeMultiplier(const Body &inBody1, const Body &inBody2, Vec3Arg inN1, Vec3Arg inN2, Vec2 &outLambda) const + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 delta_lin = inBody1.GetLinearVelocity() - inBody2.GetLinearVelocity(); + Vec2 jv; + jv[0] = inN1.Dot(delta_lin) + mR1PlusUxN1.Dot(inBody1.GetAngularVelocity()) - mR2xN1.Dot(inBody2.GetAngularVelocity()); + jv[1] = inN2.Dot(delta_lin) + mR1PlusUxN2.Dot(inBody1.GetAngularVelocity()) - mR2xN2.Dot(inBody2.GetAngularVelocity()); + outLambda = mEffectiveMass * jv; + } + +public: + /// Calculate properties used during the functions below + /// All input vectors are in world space + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1PlusU, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2, Vec3Arg inN1, Vec3Arg inN2) + { + JPH_ASSERT(inN1.IsNormalized(1.0e-5f)); + JPH_ASSERT(inN2.IsNormalized(1.0e-5f)); + + // Calculate properties used during constraint solving + mR1PlusUxN1 = inR1PlusU.Cross(inN1); + mR1PlusUxN2 = inR1PlusU.Cross(inN2); + mR2xN1 = inR2.Cross(inN1); + mR2xN2 = inR2.Cross(inN2); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1, eq 59 + Mat22 inv_effective_mass; + if (inBody1.IsDynamic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1); + mInvI1_R1PlusUxN1 = inv_i1.Multiply3x3(mR1PlusUxN1); + mInvI1_R1PlusUxN2 = inv_i1.Multiply3x3(mR1PlusUxN2); + + inv_effective_mass(0, 0) = mp1->GetInverseMass() + mR1PlusUxN1.Dot(mInvI1_R1PlusUxN1); + inv_effective_mass(0, 1) = mR1PlusUxN1.Dot(mInvI1_R1PlusUxN2); + inv_effective_mass(1, 0) = mR1PlusUxN2.Dot(mInvI1_R1PlusUxN1); + inv_effective_mass(1, 1) = mp1->GetInverseMass() + mR1PlusUxN2.Dot(mInvI1_R1PlusUxN2); + } + else + { + JPH_IF_DEBUG(mInvI1_R1PlusUxN1 = Vec3::sNaN();) + JPH_IF_DEBUG(mInvI1_R1PlusUxN2 = Vec3::sNaN();) + + inv_effective_mass = Mat22::sZero(); + } + + if (inBody2.IsDynamic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2); + mInvI2_R2xN1 = inv_i2.Multiply3x3(mR2xN1); + mInvI2_R2xN2 = inv_i2.Multiply3x3(mR2xN2); + + inv_effective_mass(0, 0) += mp2->GetInverseMass() + mR2xN1.Dot(mInvI2_R2xN1); + inv_effective_mass(0, 1) += mR2xN1.Dot(mInvI2_R2xN2); + inv_effective_mass(1, 0) += mR2xN2.Dot(mInvI2_R2xN1); + inv_effective_mass(1, 1) += mp2->GetInverseMass() + mR2xN2.Dot(mInvI2_R2xN2); + } + else + { + JPH_IF_DEBUG(mInvI2_R2xN1 = Vec3::sNaN();) + JPH_IF_DEBUG(mInvI2_R2xN2 = Vec3::sNaN();) + } + + if (!mEffectiveMass.SetInversed(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass.SetZero(); + mTotalLambda.SetZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return !mEffectiveMass.IsZero(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// All input vectors are in world space + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// All input vectors are in world space + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2) + { + Vec2 lambda; + CalculateLagrangeMultiplier(ioBody1, ioBody2, inN1, inN2, lambda); + + // Store accumulated lambda + mTotalLambda += lambda; + + return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// All input vectors are in world space + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inU, Vec3Arg inN1, Vec3Arg inN2, float inBaumgarte) const + { + Vec2 c; + c[0] = inU.Dot(inN1); + c[1] = inU.Dot(inN2); + if (!c.IsZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec2 lambda = -inBaumgarte * (mEffectiveMass * c); + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + Vec3 impulse = inN1 * lambda[0] + inN2 * lambda[1]; + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * impulse); + ioBody1.SubRotationStep(mInvI1_R1PlusUxN1 * lambda[0] + mInvI1_R1PlusUxN2 * lambda[1]); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * impulse); + ioBody2.AddRotationStep(mInvI2_R2xN1 * lambda[0] + mInvI2_R2xN2 * lambda[1]); + } + return true; + } + + return false; + } + + /// Override total lagrange multiplier, can be used to set the initial value for warm starting + inline void SetTotalLambda(const Vec2 &inLambda) + { + mTotalLambda = inLambda; + } + + /// Return lagrange multiplier + inline const Vec2 & GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1PlusUxN1; + Vec3 mR1PlusUxN2; + Vec3 mR2xN1; + Vec3 mR2xN2; + Vec3 mInvI1_R1PlusUxN1; + Vec3 mInvI1_R1PlusUxN2; + Vec3 mInvI2_R2xN1; + Vec3 mInvI2_R2xN2; + Mat22 mEffectiveMass; + Vec2 mTotalLambda { Vec2::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h new file mode 100644 index 000000000..6e277a64b --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/GearConstraintPart.h @@ -0,0 +1,195 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains two rotations using a gear (rotating in opposite direction) +/// +/// Constraint equation: +/// +/// C = Rotation1(t) + r Rotation2(t) +/// +/// Derivative: +/// +/// d/dt C = 0 +/// <=> w1 . a + r w2 . b = 0 +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}0 & a^T & 0 & r b^T\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which body 1 rotates (normalized).\n +/// b = axis along which body 2 slides (normalized).\n +/// Rotation1(t) = rotation around a of body 1.\n +/// Rotation2(t) = rotation around b of body 2.\n +/// r = ratio between rotation for body 1 and 2.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// \f$\beta\f$ = baumgarte constant. +class GearConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A); + ioBody2.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI2_B); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates + /// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates + /// @param inRatio The ratio between rotation and translation + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio) + { + JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-4f)); + JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-4f)); + + // Calculate: I1^-1 a + mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis1); + + // Calculate: I2^-1 b + mInvI2_B = inBody2.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), inWorldSpaceHingeAxis2); + + // K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + r^2 * b^T I2^-1 b) + float inv_effective_mass = (inWorldSpaceHingeAxis1.Dot(mInvI1_A) + inWorldSpaceHingeAxis2.Dot(mInvI2_B) * Square(inRatio)); + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis1 The axis around which body 1 rotates + /// @param inWorldSpaceHingeAxis2 The axis around which body 2 rotates + /// @param inRatio The ratio between rotation and translation + inline bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis1, Body &ioBody2, Vec3Arg inWorldSpaceHingeAxis2, float inRatio) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = -mEffectiveMass * (inWorldSpaceHingeAxis1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inWorldSpaceHingeAxis2.Dot(ioBody2.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.AddRotationStep(lambda * mInvI1_A); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(lambda * mInvI2_B); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_A; + Vec3 mInvI2_B; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h new file mode 100644 index 000000000..5f566f701 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/HingeRotationConstraintPart.h @@ -0,0 +1,222 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/** + Constrains rotation around 2 axis so that it only allows rotation around 1 axis + + Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.4.1 + + Constraint equation (eq 87): + + \f[C = \begin{bmatrix}a_1 \cdot b_2 \\ a_1 \cdot c_2\end{bmatrix}\f] + + Jacobian (eq 90): + + \f[J = \begin{bmatrix} + 0 & -b_2 \times a_1 & 0 & b_2 \times a_1 \\ + 0 & -c_2 \times a_1 & 0 & c2 \times a_1 + \end{bmatrix}\f] + + Used terms (here and below, everything in world space):\n + a1 = hinge axis on body 1.\n + b2, c2 = axis perpendicular to hinge axis on body 2.\n + x1, x2 = center of mass for the bodies.\n + v = [v1, w1, v2, w2].\n + v1, v2 = linear velocity of body 1 and 2.\n + w1, w2 = angular velocity of body 1 and 2.\n + M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n + \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n + b = velocity bias.\n + \f$\beta\f$ = baumgarte constant.\n + E = identity matrix. +**/ +class HingeRotationConstraintPart +{ +public: + using Vec2 = Vector<2>; + using Mat22 = Matrix<2, 2>; + +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, const Vec2 &inLambda) const + { + // Apply impulse if delta is not zero + if (!inLambda.IsZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + Vec3 impulse = mB2xA1 * inLambda[0] + mC2xA1 * inLambda[1]; + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(impulse)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(impulse)); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inWorldSpaceHingeAxis1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inWorldSpaceHingeAxis2) + { + JPH_ASSERT(inWorldSpaceHingeAxis1.IsNormalized(1.0e-5f)); + JPH_ASSERT(inWorldSpaceHingeAxis2.IsNormalized(1.0e-5f)); + + // Calculate hinge axis in world space + mA1 = inWorldSpaceHingeAxis1; + Vec3 a2 = inWorldSpaceHingeAxis2; + float dot = mA1.Dot(a2); + if (dot <= 1.0e-3f) + { + // World space axes are more than 90 degrees apart, get a perpendicular vector in the plane formed by mA1 and a2 as hinge axis until the rotation is less than 90 degrees + Vec3 perp = a2 - dot * mA1; + if (perp.LengthSq() < 1.0e-6f) + { + // mA1 ~ -a2, take random perpendicular + perp = mA1.GetNormalizedPerpendicular(); + } + + // Blend in a little bit from mA1 so we're less than 90 degrees apart + a2 = (0.99f * perp.Normalized() + 0.01f * mA1).Normalized(); + } + mB2 = a2.GetNormalizedPerpendicular(); + mC2 = a2.Cross(mB2); + + // Calculate properties used during constraint solving + mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + mB2xA1 = mB2.Cross(mA1); + mC2xA1 = mC2.Cross(mA1); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + Mat44 summed_inv_inertia = mInvI1 + mInvI2; + Mat22 inv_effective_mass; + inv_effective_mass(0, 0) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1)); + inv_effective_mass(0, 1) = mB2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1)); + inv_effective_mass(1, 0) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mB2xA1)); + inv_effective_mass(1, 1) = mC2xA1.Dot(summed_inv_inertia.Multiply3x3(mC2xA1)); + if (!mEffectiveMass.SetInversed(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass.SetZero(); + mTotalLambda.SetZero(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 delta_ang = ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity(); + Vec2 jv; + jv[0] = mB2xA1.Dot(delta_ang); + jv[1] = mC2xA1.Dot(delta_ang); + Vec2 lambda = mEffectiveMass * jv; + + // Store accumulated lambda + mTotalLambda += lambda; + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const + { + // Constraint needs Axis of body 1 perpendicular to both B and C from body 2 (which are both perpendicular to the Axis of body 2) + Vec2 c; + c[0] = mA1.Dot(mB2); + c[1] = mA1.Dot(mC2); + if (!c.IsZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec2 lambda = -inBaumgarte * (mEffectiveMass * c); + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + Vec3 impulse = mB2xA1 * lambda[0] + mC2xA1 * lambda[1]; + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1.Multiply3x3(impulse)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2.Multiply3x3(impulse)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + const Vec2 & GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mA1; ///< World space hinge axis for body 1 + Vec3 mB2; ///< World space perpendiculars of hinge axis for body 2 + Vec3 mC2; + Mat44 mInvI1; + Mat44 mInvI2; + Vec3 mB2xA1; + Vec3 mC2xA1; + Mat22 mEffectiveMass; + Vec2 mTotalLambda { Vec2::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h new file mode 100644 index 000000000..5b752c6c6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/IndependentAxisConstraintPart.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint part to an AxisConstraintPart but both bodies have an independent axis on which the force is applied. +/// +/// Constraint equation: +/// +/// \f[C = (x_1 + r_1 - f_1) . n_1 + r (x_2 + r_2 - f_2) \cdot n_2\f] +/// +/// Calculating the Jacobian: +/// +/// \f[dC/dt = (v_1 + w_1 \times r_1) \cdot n_1 + (x_1 + r_1 - f_1) \cdot d n_1/dt + r (v_2 + w_2 \times r_2) \cdot n_2 + r (x_2 + r_2 - f_2) \cdot d n_2/dt\f] +/// +/// Assuming that d n1/dt and d n2/dt are small this becomes: +/// +/// \f[(v_1 + w_1 \times r_1) \cdot n_1 + r (v_2 + w_2 \times r_2) \cdot n_2\f] +/// \f[= v_1 \cdot n_1 + r_1 \times n_1 \cdot w_1 + r v_2 \cdot n_2 + r r_2 \times n_2 \cdot w_2\f] +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}n_1 & r_1 \times n_1 & r n_2 & r r_2 \times n_2\end{bmatrix}\f] +/// +/// Effective mass: +/// +/// \f[K = m_1^{-1} + r_1 \times n_1 I_1^{-1} r_1 \times n_1 + r^2 m_2^{-1} + r^2 r_2 \times n_2 I_2^{-1} r_2 \times n_2\f] +/// +/// Used terms (here and below, everything in world space):\n +/// n1 = (x1 + r1 - f1) / |x1 + r1 - f1|, axis along which the force is applied for body 1\n +/// n2 = (x2 + r2 - f2) / |x2 + r2 - f2|, axis along which the force is applied for body 2\n +/// r = ratio how forces are applied between bodies.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant. +class IndependentAxisConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->AddLinearVelocityStep((mp1->GetInverseMass() * inLambda) * inN1); + mp1->AddAngularVelocityStep(mInvI1_R1xN1 * inLambda); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep((inRatio * mp2->GetInverseMass() * inLambda) * inN2); + mp2->AddAngularVelocityStep(mInvI2_RatioR2xN2 * inLambda); + } + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inR1 The position on which the constraint operates on body 1 relative to COM + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inR2 The position on which the constraint operates on body 1 relative to COM + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, Vec3Arg inR1, Vec3Arg inN1, Vec3Arg inR2, Vec3Arg inN2, float inRatio) + { + JPH_ASSERT(inN1.IsNormalized(1.0e-4f) && inN2.IsNormalized(1.0e-4f)); + + float inv_effective_mass = 0.0f; + + if (!inBody1.IsStatic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + + mR1xN1 = inR1.Cross(inN1); + mInvI1_R1xN1 = mp1->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), mR1xN1); + + inv_effective_mass += mp1->GetInverseMass() + mInvI1_R1xN1.Dot(mR1xN1); + } + + if (!inBody2.IsStatic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + + mRatioR2xN2 = inRatio * inR2.Cross(inN2); + mInvI2_RatioR2xN2 = mp2->MultiplyWorldSpaceInverseInertiaByVector(inBody2.GetRotation(), mRatioR2xN2); + + inv_effective_mass += Square(inRatio) * mp2->GetInverseMass() + mInvI2_RatioR2xN2.Dot(mRatioR2xN2); + } + + // Calculate inverse effective mass: K = J M^-1 J^T + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inMinLambda Minimum angular impulse to apply (N m s) + /// @param inMaxLambda Maximum angular impulse to apply (N m s) + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inMinLambda, float inMaxLambda) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = -mEffectiveMass * (inN1.Dot(ioBody1.GetLinearVelocity()) + mR1xN1.Dot(ioBody1.GetAngularVelocity()) + inRatio * inN2.Dot(ioBody2.GetLinearVelocity()) + mRatioR2xN2.Dot(ioBody2.GetAngularVelocity())); + float new_lambda = Clamp(mTotalLambda + lambda, inMinLambda, inMaxLambda); // Clamp impulse + lambda = new_lambda - mTotalLambda; // Lambda potentially got clamped, calculate the new impulse to apply + mTotalLambda = new_lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, inN1, inN2, inRatio, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inN1 The world space normal in which the constraint operates for body 1 + /// @param inN2 The world space normal in which the constraint operates for body 2 + /// @param inRatio The ratio how forces are applied between bodies + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, Vec3Arg inN1, Vec3Arg inN2, float inRatio, float inC, float inBaumgarte) const + { + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.AddPositionStep((lambda * ioBody1.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN1); + ioBody1.AddRotationStep(lambda * mInvI1_R1xN1); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep((lambda * inRatio * ioBody2.GetMotionPropertiesUnchecked()->GetInverseMass()) * inN2); + ioBody2.AddRotationStep(lambda * mInvI2_RatioR2xN2); + } + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1xN1; + Vec3 mInvI1_R1xN1; + Vec3 mRatioR2xN2; + Vec3 mInvI2_RatioR2xN2; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h new file mode 100644 index 000000000..62f663f9e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/PointConstraintPart.h @@ -0,0 +1,239 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constrains movement along 3 axis +/// +/// @see "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.2.1 +/// +/// Constraint equation (eq 45): +/// +/// \f[C = p_2 - p_1\f] +/// +/// Jacobian (transposed) (eq 47): +/// +/// \f[J^T = \begin{bmatrix}-E & r1x & E & -r2x^T\end{bmatrix} +/// = \begin{bmatrix}-E^T \\ r1x^T \\ E^T \\ -r2x^T\end{bmatrix} +/// = \begin{bmatrix}-E \\ -r1x \\ E \\ r2x\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// p1, p2 = constraint points.\n +/// r1 = p1 - x1.\n +/// r2 = p2 - x2.\n +/// r1x = 3x3 matrix for which r1x v = r1 x v (cross product).\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant.\n +/// E = identity matrix. +class PointConstraintPart +{ + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + { + MotionProperties *mp1 = ioBody1.GetMotionProperties(); + mp1->SubLinearVelocityStep(mp1->GetInverseMass() * inLambda); + mp1->SubAngularVelocityStep(mInvI1_R1X * inLambda); + } + if (ioBody2.IsDynamic()) + { + MotionProperties *mp2 = ioBody2.GetMotionProperties(); + mp2->AddLinearVelocityStep(mp2->GetInverseMass() * inLambda); + mp2->AddAngularVelocityStep(mInvI2_R2X * inLambda); + } + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inRotation1 The 3x3 rotation matrix for body 1 (translation part is ignored) + /// @param inRotation2 The 3x3 rotation matrix for body 2 (translation part is ignored) + /// @param inR1 Local space vector from center of mass to constraint point for body 1 + /// @param inR2 Local space vector from center of mass to constraint point for body 2 + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, Vec3Arg inR1, const Body &inBody2, Mat44Arg inRotation2, Vec3Arg inR2) + { + // Positions where the point constraint acts on (middle point between center of masses) in world space + mR1 = inRotation1.Multiply3x3(inR1); + mR2 = inRotation2.Multiply3x3(inR2); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + // Using: I^-1 = R * Ibody^-1 * R^T + float summed_inv_mass; + Mat44 inv_effective_mass; + if (inBody1.IsDynamic()) + { + const MotionProperties *mp1 = inBody1.GetMotionProperties(); + Mat44 inv_i1 = mp1->GetInverseInertiaForRotation(inRotation1); + summed_inv_mass = mp1->GetInverseMass(); + + Mat44 r1x = Mat44::sCrossProduct(mR1); + mInvI1_R1X = inv_i1.Multiply3x3(r1x); + inv_effective_mass = r1x.Multiply3x3(inv_i1).Multiply3x3RightTransposed(r1x); + } + else + { + JPH_IF_DEBUG(mInvI1_R1X = Mat44::sNaN();) + + summed_inv_mass = 0.0f; + inv_effective_mass = Mat44::sZero(); + } + + if (inBody2.IsDynamic()) + { + const MotionProperties *mp2 = inBody2.GetMotionProperties(); + Mat44 inv_i2 = mp2->GetInverseInertiaForRotation(inRotation2); + summed_inv_mass += mp2->GetInverseMass(); + + Mat44 r2x = Mat44::sCrossProduct(mR2); + mInvI2_R2X = inv_i2.Multiply3x3(r2x); + inv_effective_mass += r2x.Multiply3x3(inv_i2).Multiply3x3RightTransposed(r2x); + } + else + { + JPH_IF_DEBUG(mInvI2_R2X = Mat44::sNaN();) + } + + inv_effective_mass += Mat44::sScale(summed_inv_mass); + if (!mEffectiveMass.SetInversed3x3(inv_effective_mass)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass * (ioBody1.GetLinearVelocity() - mR1.Cross(ioBody1.GetAngularVelocity()) - ioBody2.GetLinearVelocity() + mR2.Cross(ioBody2.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated lambda + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inBaumgarte) const + { + Vec3 separation = (Vec3(ioBody2.GetCenterOfMassPosition() - ioBody1.GetCenterOfMassPosition()) + mR2 - mR1); + if (separation != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = mEffectiveMass * -inBaumgarte * separation; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + { + ioBody1.SubPositionStep(ioBody1.GetMotionProperties()->GetInverseMass() * lambda); + ioBody1.SubRotationStep(mInvI1_R1X * lambda); + } + if (ioBody2.IsDynamic()) + { + ioBody2.AddPositionStep(ioBody2.GetMotionProperties()->GetInverseMass() * lambda); + ioBody2.AddRotationStep(mInvI2_R2X * lambda); + } + + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mR1; + Vec3 mR2; + Mat44 mInvI1_R1X; + Mat44 mInvI2_R2X; + Mat44 mEffectiveMass; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h new file mode 100644 index 000000000..641f4867a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RackAndPinionConstraintPart.h @@ -0,0 +1,196 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constraint that constrains a rotation to a translation +/// +/// Constraint equation: +/// +/// C = Theta(t) - r d(t) +/// +/// Derivative: +/// +/// d/dt C = 0 +/// <=> w1 . a - r v2 . b = 0 +/// +/// Jacobian: +/// +/// \f[J = \begin{bmatrix}0 & a^T & -r b^T & 0\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// a = axis around which body 1 rotates (normalized).\n +/// b = axis along which body 2 slides (normalized).\n +/// Theta(t) = rotation around a of body 1.\n +/// d(t) = distance body 2 slides.\n +/// r = ratio between rotation and translation.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// \f$\beta\f$ = baumgarte constant. +class RackAndPinionConstraintPart +{ + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, float inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != 0.0f) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + ioBody1.GetMotionProperties()->AddAngularVelocityStep(inLambda * mInvI1_A); + ioBody2.GetMotionProperties()->SubLinearVelocityStep(inLambda * mRatio_InvM2_B); + return true; + } + + return false; + } + +public: + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis The axis around which body 1 rotates + /// @param inWorldSpaceSliderAxis The axis along which body 2 slides + /// @param inRatio The ratio between rotation and translation + inline void CalculateConstraintProperties(const Body &inBody1, Vec3Arg inWorldSpaceHingeAxis, const Body &inBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio) + { + JPH_ASSERT(inWorldSpaceHingeAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inWorldSpaceSliderAxis.IsNormalized(1.0e-4f)); + + // Calculate: I1^-1 a + mInvI1_A = inBody1.GetMotionProperties()->MultiplyWorldSpaceInverseInertiaByVector(inBody1.GetRotation(), inWorldSpaceHingeAxis); + + // Calculate: r/m2 b + float inv_m2 = inBody2.GetMotionProperties()->GetInverseMass(); + mRatio_InvM2_B = inRatio * inv_m2 * inWorldSpaceSliderAxis; + + // K^-1 = 1 / (J M^-1 J^T) = 1 / (a^T I1^-1 a + 1/m2 * r^2 * b . b) + float inv_effective_mass = (inWorldSpaceHingeAxis.Dot(mInvI1_A) + inv_m2 * Square(inRatio)); + if (inv_effective_mass == 0.0f) + Deactivate(); + else + mEffectiveMass = 1.0f / inv_effective_mass; + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = 0.0f; + mTotalLambda = 0.0f; + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWarmStartImpulseRatio Ratio of new step to old time step (dt_new / dt_old) for scaling the lagrange multiplier of the previous frame + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inWorldSpaceHingeAxis The axis around which body 1 rotates + /// @param inWorldSpaceSliderAxis The axis along which body 2 slides + /// @param inRatio The ratio between rotation and translation + inline bool SolveVelocityConstraint(Body &ioBody1, Vec3Arg inWorldSpaceHingeAxis, Body &ioBody2, Vec3Arg inWorldSpaceSliderAxis, float inRatio) + { + // Lagrange multiplier is: + // + // lambda = -K^-1 (J v + b) + float lambda = mEffectiveMass * (inRatio * inWorldSpaceSliderAxis.Dot(ioBody2.GetLinearVelocity()) - inWorldSpaceHingeAxis.Dot(ioBody1.GetAngularVelocity())); + mTotalLambda += lambda; // Store accumulated impulse + + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Return lagrange multiplier + float GetTotalLambda() const + { + return mTotalLambda; + } + + /// Iteratively update the position constraint. Makes sure C(...) == 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inC Value of the constraint equation (C) + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, float inC, float inBaumgarte) const + { + // Only apply position constraint when the constraint is hard, otherwise the velocity bias will fix the constraint + if (inC != 0.0f) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + float lambda = -mEffectiveMass * inBaumgarte * inC; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.AddRotationStep(lambda * mInvI1_A); + if (ioBody2.IsDynamic()) + ioBody2.SubPositionStep(lambda * mRatio_InvM2_B); + return true; + } + + return false; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Vec3 mInvI1_A; + Vec3 mRatio_InvM2_B; + float mEffectiveMass = 0.0f; + float mTotalLambda = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h new file mode 100644 index 000000000..34319e980 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h @@ -0,0 +1,270 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Constrains rotation around all axis so that only translation is allowed +/// +/// Based on: "Constraints Derivation for Rigid Body Simulation in 3D" - Daniel Chappuis, section 2.5.1 +/// +/// Constraint equation (eq 129): +/// +/// \f[C = \begin{bmatrix}\Delta\theta_x, \Delta\theta_y, \Delta\theta_z\end{bmatrix}\f] +/// +/// Jacobian (eq 131): +/// +/// \f[J = \begin{bmatrix}0 & -E & 0 & E\end{bmatrix}\f] +/// +/// Used terms (here and below, everything in world space):\n +/// delta_theta_* = difference in rotation between initial rotation of bodies 1 and 2.\n +/// x1, x2 = center of mass for the bodies.\n +/// v = [v1, w1, v2, w2].\n +/// v1, v2 = linear velocity of body 1 and 2.\n +/// w1, w2 = angular velocity of body 1 and 2.\n +/// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2].\n +/// \f$K^{-1} = \left( J M^{-1} J^T \right)^{-1}\f$ = effective mass.\n +/// b = velocity bias.\n +/// \f$\beta\f$ = baumgarte constant.\n +/// E = identity matrix.\n +class RotationEulerConstraintPart +{ +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1.Multiply3x3(inLambda)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2.Multiply3x3(inLambda)); + return true; + } + + return false; + } + +public: + /// Return inverse of initial rotation from body 1 to body 2 in body 1 space + static Quat sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2) + { + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation from body 1 to body 2 + return inBody2.GetRotation().Conjugated() * inBody1.GetRotation(); + } + + /// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space + /// @param inAxisX1 Reference axis X for body 1 + /// @param inAxisY1 Reference axis Y for body 1 + /// @param inAxisX2 Reference axis X for body 2 + /// @param inAxisY2 Reference axis Y for body 2 + static Quat sGetInvInitialOrientationXY(Vec3Arg inAxisX1, Vec3Arg inAxisY1, Vec3Arg inAxisX2, Vec3Arg inAxisY2) + { + // Store inverse of initial rotation from body 1 to body 2 in body 1 space: + // + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q10, q20 = world space initial orientation of body 1 and 2 + // r0 = initial rotation from body 1 to body 2 in local space of body 1 + // + // We can also write this in terms of the constraint matrices: + // + // q20 c2 = q10 c1 + // <=> q20 = q10 c1 c2^-1 + // => r0 = c1 c2^-1 + // <=> r0^-1 = c2 c1^-1 + // + // where: + // + // c1, c2 = matrix that takes us from body 1 and 2 COM to constraint space 1 and 2 + if (inAxisX1 == inAxisX2 && inAxisY1 == inAxisY2) + { + // Axis are the same -> identity transform + return Quat::sIdentity(); + } + else + { + Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisY1, 0), Vec4(inAxisX1.Cross(inAxisY1), 0), Vec4(0, 0, 0, 1)); + Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisY2, 0), Vec4(inAxisX2.Cross(inAxisY2), 0), Vec4(0, 0, 0, 1)); + return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated(); + } + } + + /// @brief Return inverse of initial rotation from body 1 to body 2 in body 1 space + /// @param inAxisX1 Reference axis X for body 1 + /// @param inAxisZ1 Reference axis Z for body 1 + /// @param inAxisX2 Reference axis X for body 2 + /// @param inAxisZ2 Reference axis Z for body 2 + static Quat sGetInvInitialOrientationXZ(Vec3Arg inAxisX1, Vec3Arg inAxisZ1, Vec3Arg inAxisX2, Vec3Arg inAxisZ2) + { + // See comment at sGetInvInitialOrientationXY + if (inAxisX1 == inAxisX2 && inAxisZ1 == inAxisZ2) + { + return Quat::sIdentity(); + } + else + { + Mat44 constraint1(Vec4(inAxisX1, 0), Vec4(inAxisZ1.Cross(inAxisX1), 0), Vec4(inAxisZ1, 0), Vec4(0, 0, 0, 1)); + Mat44 constraint2(Vec4(inAxisX2, 0), Vec4(inAxisZ2.Cross(inAxisX2), 0), Vec4(inAxisZ2, 0), Vec4(0, 0, 0, 1)); + return constraint2.GetQuaternion() * constraint1.GetQuaternion().Conjugated(); + } + } + + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2) + { + // Calculate properties used during constraint solving + mInvI1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2)) + Deactivate(); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()); + mTotalLambda += lambda; + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const + { + // Calculate difference in rotation + // + // The rotation should be: + // + // q2 = q1 r0 + // + // But because of drift the actual rotation is + // + // q2 = diff q1 r0 + // <=> diff = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // diff = error that needs to be reduced to zero + Quat diff = ioBody2.GetRotation() * inInvInitialOrientation * ioBody1.GetRotation().Conjugated(); + + // A quaternion can be seen as: + // + // q = [sin(theta / 2) * v, cos(theta/2)] + // + // Where: + // v = rotation vector + // theta = rotation angle + // + // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: + Vec3 error = 2.0f * diff.EnsureWPositive().GetXYZ(); + if (error != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = -inBaumgarte * mEffectiveMass * error; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1.Multiply3x3(lambda)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2.Multiply3x3(lambda)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Mat44 mInvI1; + Mat44 mInvI2; + Mat44 mEffectiveMass; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h new file mode 100644 index 000000000..e7fb9672d --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/RotationQuatConstraintPart.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Quaternion based constraint that constrains rotation around all axis so that only translation is allowed. +/// +/// NOTE: This constraint part is more expensive than the RotationEulerConstraintPart and slightly more correct since +/// RotationEulerConstraintPart::SolvePositionConstraint contains an approximation. In practice the difference +/// is small, so the RotationEulerConstraintPart is probably the better choice. +/// +/// Rotation is fixed between bodies like this: +/// +/// q2 = q1 r0 +/// +/// Where: +/// q1, q2 = world space quaternions representing rotation of body 1 and 2. +/// r0 = initial rotation between bodies in local space of body 1, this can be calculated by: +/// +/// q20 = q10 r0 +/// <=> r0 = q10^* q20 +/// +/// Where: +/// q10, q20 = initial world space rotations of body 1 and 2. +/// q10^* = conjugate of quaternion q10 (which is the same as the inverse for a unit quaternion) +/// +/// We exclusively use the conjugate below: +/// +/// r0^* = q20^* q10 +/// +/// The error in the rotation is (in local space of body 1): +/// +/// q2 = q1 error r0 +/// <=> error = q1^* q2 r0^* +/// +/// The imaginary part of the quaternion represents the rotation axis * sin(angle / 2). The real part of the quaternion +/// does not add any additional information (we know the quaternion in normalized) and we're removing 3 degrees of freedom +/// so we want 3 parameters. Therefore we define the constraint equation like: +/// +/// C = A q1^* q2 r0^* = 0 +/// +/// Where (if you write a quaternion as [real-part, i-part, j-part, k-part]): +/// +/// [0, 1, 0, 0] +/// A = [0, 0, 1, 0] +/// [0, 0, 0, 1] +/// +/// or in our case since we store a quaternion like [i-part, j-part, k-part, real-part]: +/// +/// [1, 0, 0, 0] +/// A = [0, 1, 0, 0] +/// [0, 0, 1, 0] +/// +/// Time derivative: +/// +/// d/dt C = A (q1^* d/dt(q2) + d/dt(q1^*) q2) r0^* +/// = A (q1^* (1/2 W2 q2) + (1/2 W1 q1)^* q2) r0^* +/// = 1/2 A (q1^* W2 q2 + q1^* W1^* q2) r0^* +/// = 1/2 A (q1^* W2 q2 - q1^* W1 * q2) r0^* +/// = 1/2 A ML(q1^*) MR(q2 r0^*) (W2 - W1) +/// = 1/2 A ML(q1^*) MR(q2 r0^*) A^T (w2 - w1) +/// +/// Where: +/// W1 = [0, w1], W2 = [0, w2] (converting angular velocity to imaginary part of quaternion). +/// w1, w2 = angular velocity of body 1 and 2. +/// d/dt(q) = 1/2 W q (time derivative of a quaternion). +/// W^* = -W (conjugate negates angular velocity as quaternion). +/// ML(q): 4x4 matrix so that q * p = ML(q) * p, where q and p are quaternions. +/// MR(p): 4x4 matrix so that q * p = MR(p) * q, where q and p are quaternions. +/// A^T: Transpose of A. +/// +/// Jacobian: +/// +/// J = [0, -1/2 A ML(q1^*) MR(q2 r0^*) A^T, 0, 1/2 A ML(q1^*) MR(q2 r0^*) A^T] +/// = [0, -JP, 0, JP] +/// +/// Suggested reading: +/// - 3D Constraint Derivations for Impulse Solvers - Marijn Tamis +/// - Game Physics Pearls - Section 9 - Quaternion Based Constraints - Claude Lacoursiere +class RotationQuatConstraintPart +{ +private: + /// Internal helper function to update velocities of bodies after Lagrange multiplier is calculated + JPH_INLINE bool ApplyVelocityStep(Body &ioBody1, Body &ioBody2, Vec3Arg inLambda) const + { + // Apply impulse if delta is not zero + if (inLambda != Vec3::sZero()) + { + // Calculate velocity change due to constraint + // + // Impulse: + // P = J^T lambda + // + // Euler velocity integration: + // v' = v + M^-1 P + if (ioBody1.IsDynamic()) + ioBody1.GetMotionProperties()->SubAngularVelocityStep(mInvI1_JPT.Multiply3x3(inLambda)); + if (ioBody2.IsDynamic()) + ioBody2.GetMotionProperties()->AddAngularVelocityStep(mInvI2_JPT.Multiply3x3(inLambda)); + return true; + } + + return false; + } + +public: + /// Return inverse of initial rotation from body 1 to body 2 in body 1 space + static Quat sGetInvInitialOrientation(const Body &inBody1, const Body &inBody2) + { + // q20 = q10 r0 + // <=> r0 = q10^-1 q20 + // <=> r0^-1 = q20^-1 q10 + // + // where: + // + // q20 = initial orientation of body 2 + // q10 = initial orientation of body 1 + // r0 = initial rotation from body 1 to body 2 + return inBody2.GetRotation().Conjugated() * inBody1.GetRotation(); + } + + /// Calculate properties used during the functions below + inline void CalculateConstraintProperties(const Body &inBody1, Mat44Arg inRotation1, const Body &inBody2, Mat44Arg inRotation2, QuatArg inInvInitialOrientation) + { + // Calculate: JP = 1/2 A ML(q1^*) MR(q2 r0^*) A^T + Mat44 jp = (Mat44::sQuatLeftMultiply(0.5f * inBody1.GetRotation().Conjugated()) * Mat44::sQuatRightMultiply(inBody2.GetRotation() * inInvInitialOrientation)).GetRotationSafe(); + + // Calculate properties used during constraint solving + Mat44 inv_i1 = inBody1.IsDynamic()? inBody1.GetMotionProperties()->GetInverseInertiaForRotation(inRotation1) : Mat44::sZero(); + Mat44 inv_i2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + mInvI1_JPT = inv_i1.Multiply3x3RightTransposed(jp); + mInvI2_JPT = inv_i2.Multiply3x3RightTransposed(jp); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 + // = (JP * I1^-1 * JP^T + JP * I2^-1 * JP^T)^-1 + // = (JP * (I1^-1 + I2^-1) * JP^T)^-1 + if (!mEffectiveMass.SetInversed3x3(jp.Multiply3x3(inv_i1 + inv_i2).Multiply3x3RightTransposed(jp))) + Deactivate(); + else + mEffectiveMass_JP = mEffectiveMass.Multiply3x3(jp); + } + + /// Deactivate this constraint + inline void Deactivate() + { + mEffectiveMass = Mat44::sZero(); + mEffectiveMass_JP = Mat44::sZero(); + mTotalLambda = Vec3::sZero(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mEffectiveMass(3, 3) != 0.0f; + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mTotalLambda *= inWarmStartImpulseRatio; + ApplyVelocityStep(ioBody1, ioBody2, mTotalLambda); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + // Calculate lagrange multiplier: + // + // lambda = -K^-1 (J v + b) + Vec3 lambda = mEffectiveMass_JP.Multiply3x3(ioBody1.GetAngularVelocity() - ioBody2.GetAngularVelocity()); + mTotalLambda += lambda; + return ApplyVelocityStep(ioBody1, ioBody2, lambda); + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inInvInitialOrientation, float inBaumgarte) const + { + // Calculate constraint equation + Vec3 c = (ioBody1.GetRotation().Conjugated() * ioBody2.GetRotation() * inInvInitialOrientation).GetXYZ(); + if (c != Vec3::sZero()) + { + // Calculate lagrange multiplier (lambda) for Baumgarte stabilization: + // + // lambda = -K^-1 * beta / dt * C + // + // We should divide by inDeltaTime, but we should multiply by inDeltaTime in the Euler step below so they're cancelled out + Vec3 lambda = -inBaumgarte * mEffectiveMass * c; + + // Directly integrate velocity change for one time step + // + // Euler velocity integration: + // dv = M^-1 P + // + // Impulse: + // P = J^T lambda + // + // Euler position integration: + // x' = x + dv * dt + // + // Note we don't accumulate velocities for the stabilization. This is using the approach described in 'Modeling and + // Solving Constraints' by Erin Catto presented at GDC 2007. On slide 78 it is suggested to split up the Baumgarte + // stabilization for positional drift so that it does not actually add to the momentum. We combine an Euler velocity + // integrate + a position integrate and then discard the velocity change. + if (ioBody1.IsDynamic()) + ioBody1.SubRotationStep(mInvI1_JPT.Multiply3x3(lambda)); + if (ioBody2.IsDynamic()) + ioBody2.AddRotationStep(mInvI2_JPT.Multiply3x3(lambda)); + return true; + } + + return false; + } + + /// Return lagrange multiplier + Vec3 GetTotalLambda() const + { + return mTotalLambda; + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + inStream.Write(mTotalLambda); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + inStream.Read(mTotalLambda); + } + +private: + Mat44 mInvI1_JPT; + Mat44 mInvI2_JPT; + Mat44 mEffectiveMass; + Mat44 mEffectiveMass_JP; + Vec3 mTotalLambda { Vec3::sZero() }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h new file mode 100644 index 000000000..0a8a4a973 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SpringPart.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN +#ifndef JPH_PLATFORM_DOXYGEN // Somehow Doxygen gets confused and thinks the parameters to CalculateSpringProperties belong to this macro +JPH_MSVC_SUPPRESS_WARNING(4723) // potential divide by 0 - caused by line: outEffectiveMass = 1.0f / inInvEffectiveMass, note that JPH_NAMESPACE_BEGIN already pushes the warning state +#endif // !JPH_PLATFORM_DOXYGEN + +/// Class used in other constraint parts to calculate the required bias factor in the lagrange multiplier for creating springs +class SpringPart +{ +private: + JPH_INLINE void CalculateSpringPropertiesHelper(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass) + { + // Soft constraints as per: Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 + + // Note that the calculation of beta and gamma below are based on the solution of an implicit Euler integration scheme + // This scheme is unconditionally stable but has built in damping, so even when you set the damping ratio to 0 there will still + // be damping. See page 16 and 32. + + // Calculate softness (gamma in the slides) + // See page 34 and note that the gamma needs to be divided by delta time since we're working with impulses rather than forces: + // softness = 1 / (dt * (c + dt * k)) + // Note that the spring stiffness is k and the spring damping is c + mSoftness = 1.0f / (inDeltaTime * (inDamping + inDeltaTime * inStiffness)); + + // Calculate bias factor (baumgarte stabilization): + // beta = dt * k / (c + dt * k) = dt * k^2 * softness + // b = beta / dt * C = dt * k * softness * C + mBias = inBias + inDeltaTime * inStiffness * mSoftness * inC; + + // Update the effective mass, see post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354 + // + // Newton's Law: + // M * (v2 - v1) = J^T * lambda + // + // Velocity constraint with softness and Baumgarte: + // J * v2 + softness * lambda + b = 0 + // + // where b = beta * C / dt + // + // We know everything except v2 and lambda. + // + // First solve Newton's law for v2 in terms of lambda: + // + // v2 = v1 + M^-1 * J^T * lambda + // + // Substitute this expression into the velocity constraint: + // + // J * (v1 + M^-1 * J^T * lambda) + softness * lambda + b = 0 + // + // Now collect coefficients of lambda: + // + // (J * M^-1 * J^T + softness) * lambda = - J * v1 - b + // + // Now we define: + // + // K = J * M^-1 * J^T + softness + // + // So our new effective mass is K^-1 + outEffectiveMass = 1.0f / (inInvEffectiveMass + mSoftness); + } + +public: + /// Turn off the spring and set a bias only + /// + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + inline void CalculateSpringPropertiesWithBias(float inBias) + { + mSoftness = 0.0f; + mBias = inBias; + } + + /// Calculate spring properties based on frequency and damping ratio + /// + /// @param inDeltaTime Time step + /// @param inInvEffectiveMass Inverse effective mass K + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inFrequency Oscillation frequency (Hz). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inDamping Damping factor (0 = no damping, 1 = critical damping). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param outEffectiveMass On return, this contains the new effective mass K^-1 + inline void CalculateSpringPropertiesWithFrequencyAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inFrequency, float inDamping, float &outEffectiveMass) + { + outEffectiveMass = 1.0f / inInvEffectiveMass; + + if (inFrequency > 0.0f) + { + // Calculate angular frequency + float omega = 2.0f * JPH_PI * inFrequency; + + // Calculate spring stiffness k and damping constant c (page 45) + float k = outEffectiveMass * Square(omega); + float c = 2.0f * outEffectiveMass * inDamping * omega; + + CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, k, c, outEffectiveMass); + } + else + { + CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Calculate spring properties with spring Stiffness (k) and damping (c), this is based on the spring equation: F = -k * x - c * v + /// + /// @param inDeltaTime Time step + /// @param inInvEffectiveMass Inverse effective mass K + /// @param inBias Bias term (b) for the constraint impulse: lambda = J v + b + /// @param inC Value of the constraint equation (C). Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inStiffness Spring stiffness k. Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param inDamping Spring damping coefficient c. Set to zero if you don't want to drive the constraint to zero with a spring. + /// @param outEffectiveMass On return, this contains the new effective mass K^-1 + inline void CalculateSpringPropertiesWithStiffnessAndDamping(float inDeltaTime, float inInvEffectiveMass, float inBias, float inC, float inStiffness, float inDamping, float &outEffectiveMass) + { + if (inStiffness > 0.0f) + { + CalculateSpringPropertiesHelper(inDeltaTime, inInvEffectiveMass, inBias, inC, inStiffness, inDamping, outEffectiveMass); + } + else + { + outEffectiveMass = 1.0f / inInvEffectiveMass; + + CalculateSpringPropertiesWithBias(inBias); + } + } + + /// Returns if this spring is active + inline bool IsActive() const + { + return mSoftness != 0.0f; + } + + /// Get total bias b, including supplied bias and bias for spring: lambda = J v + b + inline float GetBias(float inTotalLambda) const + { + // Remainder of post by Erin Catto: http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=1354 + // + // Each iteration we are not computing the whole impulse, we are computing an increment to the impulse and we are updating the velocity. + // Also, as we solve each constraint we get a perfect v2, but then some other constraint will come along and mess it up. + // So we want to patch up the constraint while acknowledging the accumulated impulse and the damaged velocity. + // To help with that we use P for the accumulated impulse and lambda as the update. Mathematically we have: + // + // M * (v2new - v2damaged) = J^T * lambda + // J * v2new + softness * (total_lambda + lambda) + b = 0 + // + // If we solve this we get: + // + // v2new = v2damaged + M^-1 * J^T * lambda + // J * (v2damaged + M^-1 * J^T * lambda) + softness * total_lambda + softness * lambda + b = 0 + // + // (J * M^-1 * J^T + softness) * lambda = -(J * v2damaged + softness * total_lambda + b) + // + // So our lagrange multiplier becomes: + // + // lambda = -K^-1 (J v + softness * total_lambda + b) + // + // So we return the bias: softness * total_lambda + b + return mSoftness * inTotalLambda + mBias; + } + +private: + float mBias = 0.0f; + float mSoftness = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h new file mode 100644 index 000000000..c2a47547f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ConstraintPart/SwingTwistConstraintPart.h @@ -0,0 +1,597 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// How the swing limit behaves +enum class ESwingType : uint8 +{ + Cone, ///< Swing is limited by a cone shape, note that this cone starts to deform for larger swing angles. Cone limits only support limits that are symmetric around 0. + Pyramid, ///< Swing is limited by a pyramid shape, note that this pyramid starts to deform for larger swing angles. +}; + +/// Quaternion based constraint that decomposes the rotation in constraint space in swing and twist: q = q_swing * q_twist +/// where q_swing.x = 0 and where q_twist.y = q_twist.z = 0 +/// +/// - Rotation around the twist (x-axis) is within [inTwistMinAngle, inTwistMaxAngle]. +/// - Rotation around the swing axis (y and z axis) are limited to an ellipsoid in quaternion space formed by the equation: +/// +/// (q_swing.y / sin(inSwingYHalfAngle / 2))^2 + (q_swing.z / sin(inSwingZHalfAngle / 2))^2 <= 1 +/// +/// Which roughly corresponds to an elliptic cone shape with major axis (inSwingYHalfAngle, inSwingZHalfAngle). +/// +/// In case inSwingYHalfAngle = 0, the rotation around Y will be constrained to 0 and the rotation around Z +/// will be constrained between [-inSwingZHalfAngle, inSwingZHalfAngle]. Vice versa if inSwingZHalfAngle = 0. +class SwingTwistConstraintPart +{ +public: + /// Override the swing type + void SetSwingType(ESwingType inSwingType) + { + mSwingType = inSwingType; + } + + /// Get the swing type for this part + ESwingType GetSwingType() const + { + return mSwingType; + } + + /// Set limits for this constraint (see description above for parameters) + void SetLimits(float inTwistMinAngle, float inTwistMaxAngle, float inSwingYMinAngle, float inSwingYMaxAngle, float inSwingZMinAngle, float inSwingZMaxAngle) + { + constexpr float cLockedAngle = DegreesToRadians(0.5f); + constexpr float cFreeAngle = DegreesToRadians(179.5f); + + // Assume sane input + JPH_ASSERT(inTwistMinAngle <= inTwistMaxAngle); + JPH_ASSERT(inSwingYMinAngle <= inSwingYMaxAngle); + JPH_ASSERT(inSwingZMinAngle <= inSwingZMaxAngle); + JPH_ASSERT(inSwingYMinAngle >= -JPH_PI && inSwingYMaxAngle <= JPH_PI); + JPH_ASSERT(inSwingZMinAngle >= -JPH_PI && inSwingZMaxAngle <= JPH_PI); + + // Calculate the sine and cosine of the half angles + Vec4 half_twist = 0.5f * Vec4(inTwistMinAngle, inTwistMaxAngle, 0, 0); + Vec4 twist_s, twist_c; + half_twist.SinCos(twist_s, twist_c); + Vec4 half_swing = 0.5f * Vec4(inSwingYMinAngle, inSwingYMaxAngle, inSwingZMinAngle, inSwingZMaxAngle); + Vec4 swing_s, swing_c; + half_swing.SinCos(swing_s, swing_c); + + // Store half angles for pyramid limit + mSwingYHalfMinAngle = half_swing.GetX(); + mSwingYHalfMaxAngle = half_swing.GetY(); + mSwingZHalfMinAngle = half_swing.GetZ(); + mSwingZHalfMaxAngle = half_swing.GetW(); + + // Store axis flags which are used at runtime to quickly decided which constraints to apply + mRotationFlags = 0; + if (inTwistMinAngle > -cLockedAngle && inTwistMaxAngle < cLockedAngle) + { + mRotationFlags |= TwistXLocked; + mSinTwistHalfMinAngle = 0.0f; + mSinTwistHalfMaxAngle = 0.0f; + mCosTwistHalfMinAngle = 1.0f; + mCosTwistHalfMaxAngle = 1.0f; + } + else if (inTwistMinAngle < -cFreeAngle && inTwistMaxAngle > cFreeAngle) + { + mRotationFlags |= TwistXFree; + mSinTwistHalfMinAngle = -1.0f; + mSinTwistHalfMaxAngle = 1.0f; + mCosTwistHalfMinAngle = 0.0f; + mCosTwistHalfMaxAngle = 0.0f; + } + else + { + mSinTwistHalfMinAngle = twist_s.GetX(); + mSinTwistHalfMaxAngle = twist_s.GetY(); + mCosTwistHalfMinAngle = twist_c.GetX(); + mCosTwistHalfMaxAngle = twist_c.GetY(); + } + + if (inSwingYMinAngle > -cLockedAngle && inSwingYMaxAngle < cLockedAngle) + { + mRotationFlags |= SwingYLocked; + mSinSwingYHalfMinAngle = 0.0f; + mSinSwingYHalfMaxAngle = 0.0f; + mCosSwingYHalfMinAngle = 1.0f; + mCosSwingYHalfMaxAngle = 1.0f; + } + else if (inSwingYMinAngle < -cFreeAngle && inSwingYMaxAngle > cFreeAngle) + { + mRotationFlags |= SwingYFree; + mSinSwingYHalfMinAngle = -1.0f; + mSinSwingYHalfMaxAngle = 1.0f; + mCosSwingYHalfMinAngle = 0.0f; + mCosSwingYHalfMaxAngle = 0.0f; + } + else + { + mSinSwingYHalfMinAngle = swing_s.GetX(); + mSinSwingYHalfMaxAngle = swing_s.GetY(); + mCosSwingYHalfMinAngle = swing_c.GetX(); + mCosSwingYHalfMaxAngle = swing_c.GetY(); + JPH_ASSERT(mSinSwingYHalfMinAngle <= mSinSwingYHalfMaxAngle); + } + + if (inSwingZMinAngle > -cLockedAngle && inSwingZMaxAngle < cLockedAngle) + { + mRotationFlags |= SwingZLocked; + mSinSwingZHalfMinAngle = 0.0f; + mSinSwingZHalfMaxAngle = 0.0f; + mCosSwingZHalfMinAngle = 1.0f; + mCosSwingZHalfMaxAngle = 1.0f; + } + else if (inSwingZMinAngle < -cFreeAngle && inSwingZMaxAngle > cFreeAngle) + { + mRotationFlags |= SwingZFree; + mSinSwingZHalfMinAngle = -1.0f; + mSinSwingZHalfMaxAngle = 1.0f; + mCosSwingZHalfMinAngle = 0.0f; + mCosSwingZHalfMaxAngle = 0.0f; + } + else + { + mSinSwingZHalfMinAngle = swing_s.GetZ(); + mSinSwingZHalfMaxAngle = swing_s.GetW(); + mCosSwingZHalfMinAngle = swing_c.GetZ(); + mCosSwingZHalfMaxAngle = swing_c.GetW(); + JPH_ASSERT(mSinSwingZHalfMinAngle <= mSinSwingZHalfMaxAngle); + } + } + + /// Flags to indicate which axis got clamped by ClampSwingTwist + static constexpr uint cClampedTwistMin = 1 << 0; + static constexpr uint cClampedTwistMax = 1 << 1; + static constexpr uint cClampedSwingYMin = 1 << 2; + static constexpr uint cClampedSwingYMax = 1 << 3; + static constexpr uint cClampedSwingZMin = 1 << 4; + static constexpr uint cClampedSwingZMax = 1 << 5; + + /// Helper function to determine if we're clamped against the min or max limit + static JPH_INLINE bool sDistanceToMinShorter(float inDeltaMin, float inDeltaMax) + { + // We're outside of the limits, get actual delta to min/max range + // Note that a swing/twist of -1 and 1 represent the same angle, so if the difference is bigger than 1, the shortest angle is the other way around (2 - difference) + // We should actually be working with angles rather than sin(angle / 2). When the difference is small the approximation is accurate, but + // when working with extreme values the calculation is off and e.g. when the limit is between 0 and 180 a value of approx -60 will clamp + // to 180 rather than 0 (you'd expect anything > -90 to go to 0). + inDeltaMin = abs(inDeltaMin); + if (inDeltaMin > 1.0f) inDeltaMin = 2.0f - inDeltaMin; + inDeltaMax = abs(inDeltaMax); + if (inDeltaMax > 1.0f) inDeltaMax = 2.0f - inDeltaMax; + return inDeltaMin < inDeltaMax; + } + + /// Clamp twist and swing against the constraint limits, returns which parts were clamped (everything assumed in constraint space) + inline void ClampSwingTwist(Quat &ioSwing, Quat &ioTwist, uint &outClampedAxis) const + { + // Start with not clamped + outClampedAxis = 0; + + // Check that swing and twist quaternions don't contain rotations around the wrong axis + JPH_ASSERT(ioSwing.GetX() == 0.0f); + JPH_ASSERT(ioTwist.GetY() == 0.0f); + JPH_ASSERT(ioTwist.GetZ() == 0.0f); + + // Ensure quaternions have w > 0 + bool negate_swing = ioSwing.GetW() < 0.0f; + if (negate_swing) + ioSwing = -ioSwing; + bool negate_twist = ioTwist.GetW() < 0.0f; + if (negate_twist) + ioTwist = -ioTwist; + + if (mRotationFlags & TwistXLocked) + { + // Twist axis is locked, clamp whenever twist is not identity + outClampedAxis |= ioTwist.GetX() != 0.0f? (cClampedTwistMin | cClampedTwistMax) : 0; + ioTwist = Quat::sIdentity(); + } + else if ((mRotationFlags & TwistXFree) == 0) + { + // Twist axis has limit, clamp whenever out of range + float delta_min = mSinTwistHalfMinAngle - ioTwist.GetX(); + float delta_max = ioTwist.GetX() - mSinTwistHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the twist that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioTwist = Quat(mSinTwistHalfMinAngle, 0, 0, mCosTwistHalfMinAngle); + outClampedAxis |= cClampedTwistMin; + } + else + { + ioTwist = Quat(mSinTwistHalfMaxAngle, 0, 0, mCosTwistHalfMaxAngle); + outClampedAxis |= cClampedTwistMax; + } + } + } + + // Clamp swing + if (mRotationFlags & SwingYLocked) + { + if (mRotationFlags & SwingZLocked) + { + // Both swing Y and Z are disabled, no degrees of freedom in swing + outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0; + outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0; + ioSwing = Quat::sIdentity(); + } + else + { + // Swing Y angle disabled, only 1 degree of freedom in swing + outClampedAxis |= ioSwing.GetY() != 0.0f? (cClampedSwingYMin | cClampedSwingYMax) : 0; + float delta_min = mSinSwingZHalfMinAngle - ioSwing.GetZ(); + float delta_max = ioSwing.GetZ() - mSinSwingZHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the swing that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioSwing = Quat(0, 0, mSinSwingZHalfMinAngle, mCosSwingZHalfMinAngle); + outClampedAxis |= cClampedSwingZMin; + } + else + { + ioSwing = Quat(0, 0, mSinSwingZHalfMaxAngle, mCosSwingZHalfMaxAngle); + outClampedAxis |= cClampedSwingZMax; + } + } + else if ((outClampedAxis & cClampedSwingYMin) != 0) + { + float z = ioSwing.GetZ(); + ioSwing = Quat(0, 0, z, sqrt(1.0f - Square(z))); + } + } + } + else if (mRotationFlags & SwingZLocked) + { + // Swing Z angle disabled, only 1 degree of freedom in swing + outClampedAxis |= ioSwing.GetZ() != 0.0f? (cClampedSwingZMin | cClampedSwingZMax) : 0; + float delta_min = mSinSwingYHalfMinAngle - ioSwing.GetY(); + float delta_max = ioSwing.GetY() - mSinSwingYHalfMaxAngle; + if (delta_min > 0.0f || delta_max > 0.0f) + { + // Pick the swing that corresponds to the smallest delta + if (sDistanceToMinShorter(delta_min, delta_max)) + { + ioSwing = Quat(0, mSinSwingYHalfMinAngle, 0, mCosSwingYHalfMinAngle); + outClampedAxis |= cClampedSwingYMin; + } + else + { + ioSwing = Quat(0, mSinSwingYHalfMaxAngle, 0, mCosSwingYHalfMaxAngle); + outClampedAxis |= cClampedSwingYMax; + } + } + else if ((outClampedAxis & cClampedSwingZMin) != 0) + { + float y = ioSwing.GetY(); + ioSwing = Quat(0, y, 0, sqrt(1.0f - Square(y))); + } + } + else + { + // Two degrees of freedom + if (mSwingType == ESwingType::Cone) + { + // Use ellipse to solve limits + Ellipse ellipse(mSinSwingYHalfMaxAngle, mSinSwingZHalfMaxAngle); + Float2 point(ioSwing.GetY(), ioSwing.GetZ()); + if (!ellipse.IsInside(point)) + { + Float2 closest = ellipse.GetClosestPoint(point); + ioSwing = Quat(0, closest.x, closest.y, sqrt(max(0.0f, 1.0f - Square(closest.x) - Square(closest.y)))); + outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here + } + } + else + { + // Use pyramid to solve limits + // The quaternion rotating by angle y around the Y axis then rotating by angle z around the Z axis is: + // q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) + // [q.x, q.y, q.z, q.w] = [-sin(y / 2) * sin(z / 2), sin(y / 2) * cos(z / 2), cos(y / 2) * sin(z / 2), cos(y / 2) * cos(z / 2)] + // So we can calculate y / 2 = atan2(q.y, q.w) and z / 2 = atan2(q.z, q.w) + Vec4 half_angle = Vec4::sATan2(ioSwing.GetXYZW().Swizzle(), ioSwing.GetXYZW().SplatW()); + Vec4 min_half_angle(mSwingYHalfMinAngle, mSwingYHalfMinAngle, mSwingZHalfMinAngle, mSwingZHalfMinAngle); + Vec4 max_half_angle(mSwingYHalfMaxAngle, mSwingYHalfMaxAngle, mSwingZHalfMaxAngle, mSwingZHalfMaxAngle); + Vec4 clamped_half_angle = Vec4::sMin(Vec4::sMax(half_angle, min_half_angle), max_half_angle); + UVec4 unclamped = Vec4::sEquals(half_angle, clamped_half_angle); + if (!unclamped.TestAllTrue()) + { + // We now calculate the quaternion again using the formula for q above, + // but we leave out the x component in order to not introduce twist + Vec4 s, c; + clamped_half_angle.SinCos(s, c); + ioSwing = Quat(0, s.GetY() * c.GetZ(), c.GetY() * s.GetZ(), c.GetY() * c.GetZ()).Normalized(); + outClampedAxis |= cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax; // We're not using the flags on which side we got clamped here + } + } + } + + // Flip sign back + if (negate_swing) + ioSwing = -ioSwing; + if (negate_twist) + ioTwist = -ioTwist; + + JPH_ASSERT(ioSwing.IsNormalized()); + JPH_ASSERT(ioTwist.IsNormalized()); + } + + /// Calculate properties used during the functions below + /// @param inBody1 The first body that this constraint is attached to + /// @param inBody2 The second body that this constraint is attached to + /// @param inConstraintRotation The current rotation of the constraint in constraint space + /// @param inConstraintToWorld Rotates from constraint space into world space + inline void CalculateConstraintProperties(const Body &inBody1, const Body &inBody2, QuatArg inConstraintRotation, QuatArg inConstraintToWorld) + { + // Decompose into swing and twist + Quat q_swing, q_twist; + inConstraintRotation.GetSwingTwist(q_swing, q_twist); + + // Clamp against joint limits + Quat q_clamped_swing = q_swing, q_clamped_twist = q_twist; + uint clamped_axis; + ClampSwingTwist(q_clamped_swing, q_clamped_twist, clamped_axis); + + if (mRotationFlags & SwingYLocked) + { + Quat twist_to_world = inConstraintToWorld * q_swing; + mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY(); + mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ(); + + if (mRotationFlags & SwingZLocked) + { + // Swing fully locked + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else + { + // Swing only locked around Y + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + if ((clamped_axis & (cClampedSwingZMin | cClampedSwingZMax)) != 0) + { + if ((clamped_axis & cClampedSwingZMin) != 0) + mWorldSpaceSwingLimitZRotationAxis = -mWorldSpaceSwingLimitZRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else + mSwingLimitZConstraintPart.Deactivate(); + } + } + else if (mRotationFlags & SwingZLocked) + { + // Swing only locked around Z + Quat twist_to_world = inConstraintToWorld * q_swing; + mWorldSpaceSwingLimitYRotationAxis = twist_to_world.RotateAxisY(); + mWorldSpaceSwingLimitZRotationAxis = twist_to_world.RotateAxisZ(); + + if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax)) != 0) + { + if ((clamped_axis & cClampedSwingYMin) != 0) + mWorldSpaceSwingLimitYRotationAxis = -mWorldSpaceSwingLimitYRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + } + else + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitZRotationAxis); + } + else if ((mRotationFlags & SwingYZFree) != SwingYZFree) + { + // Swing has limits around Y and Z + if ((clamped_axis & (cClampedSwingYMin | cClampedSwingYMax | cClampedSwingZMin | cClampedSwingZMax)) != 0) + { + // Calculate axis of rotation from clamped swing to swing + Vec3 current = (inConstraintToWorld * q_swing).RotateAxisX(); + Vec3 desired = (inConstraintToWorld * q_clamped_swing).RotateAxisX(); + mWorldSpaceSwingLimitYRotationAxis = desired.Cross(current); + float len = mWorldSpaceSwingLimitYRotationAxis.Length(); + if (len != 0.0f) + { + mWorldSpaceSwingLimitYRotationAxis /= len; + mSwingLimitYConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceSwingLimitYRotationAxis); + } + else + mSwingLimitYConstraintPart.Deactivate(); + } + else + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + } + else + { + // No swing limits + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + } + + if (mRotationFlags & TwistXLocked) + { + // Twist locked, always activate constraint + mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX(); + mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis); + } + else if ((mRotationFlags & TwistXFree) == 0) + { + // Twist has limits + if ((clamped_axis & (cClampedTwistMin | cClampedTwistMax)) != 0) + { + mWorldSpaceTwistLimitRotationAxis = (inConstraintToWorld * q_swing).RotateAxisX(); + if ((clamped_axis & cClampedTwistMin) != 0) + mWorldSpaceTwistLimitRotationAxis = -mWorldSpaceTwistLimitRotationAxis; // Flip axis if hitting min limit because the impulse limit is going to be between [-FLT_MAX, 0] + mTwistLimitConstraintPart.CalculateConstraintProperties(inBody1, inBody2, mWorldSpaceTwistLimitRotationAxis); + } + else + mTwistLimitConstraintPart.Deactivate(); + } + else + { + // No twist limits + mTwistLimitConstraintPart.Deactivate(); + } + } + + /// Deactivate this constraint + void Deactivate() + { + mSwingLimitYConstraintPart.Deactivate(); + mSwingLimitZConstraintPart.Deactivate(); + mTwistLimitConstraintPart.Deactivate(); + } + + /// Check if constraint is active + inline bool IsActive() const + { + return mSwingLimitYConstraintPart.IsActive() || mSwingLimitZConstraintPart.IsActive() || mTwistLimitConstraintPart.IsActive(); + } + + /// Must be called from the WarmStartVelocityConstraint call to apply the previous frame's impulses + inline void WarmStart(Body &ioBody1, Body &ioBody2, float inWarmStartImpulseRatio) + { + mSwingLimitYConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + mSwingLimitZConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + mTwistLimitConstraintPart.WarmStart(ioBody1, ioBody2, inWarmStartImpulseRatio); + } + + /// Iteratively update the velocity constraint. Makes sure d/dt C(...) = 0, where C is the constraint equation. + inline bool SolveVelocityConstraint(Body &ioBody1, Body &ioBody2) + { + bool impulse = false; + + // Solve swing constraint + if (mSwingLimitYConstraintPart.IsActive()) + impulse |= mSwingLimitYConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitYRotationAxis, -FLT_MAX, mSinSwingYHalfMinAngle == mSinSwingYHalfMaxAngle? FLT_MAX : 0.0f); + + if (mSwingLimitZConstraintPart.IsActive()) + impulse |= mSwingLimitZConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceSwingLimitZRotationAxis, -FLT_MAX, mSinSwingZHalfMinAngle == mSinSwingZHalfMaxAngle? FLT_MAX : 0.0f); + + // Solve twist constraint + if (mTwistLimitConstraintPart.IsActive()) + impulse |= mTwistLimitConstraintPart.SolveVelocityConstraint(ioBody1, ioBody2, mWorldSpaceTwistLimitRotationAxis, -FLT_MAX, mSinTwistHalfMinAngle == mSinTwistHalfMaxAngle? FLT_MAX : 0.0f); + + return impulse; + } + + /// Iteratively update the position constraint. Makes sure C(...) = 0. + /// @param ioBody1 The first body that this constraint is attached to + /// @param ioBody2 The second body that this constraint is attached to + /// @param inConstraintRotation The current rotation of the constraint in constraint space + /// @param inConstraintToBody1 , inConstraintToBody2 Rotates from constraint space to body 1/2 space + /// @param inBaumgarte Baumgarte constant (fraction of the error to correct) + inline bool SolvePositionConstraint(Body &ioBody1, Body &ioBody2, QuatArg inConstraintRotation, QuatArg inConstraintToBody1, QuatArg inConstraintToBody2, float inBaumgarte) const + { + Quat q_swing, q_twist; + inConstraintRotation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + ClampSwingTwist(q_swing, q_twist, clamped_axis); + + // Solve rotation violations + if (clamped_axis != 0) + { + RotationEulerConstraintPart part; + Quat inv_initial_orientation = inConstraintToBody2 * (inConstraintToBody1 * q_swing * q_twist).Conjugated(); + part.CalculateConstraintProperties(ioBody1, Mat44::sRotation(ioBody1.GetRotation()), ioBody2, Mat44::sRotation(ioBody2.GetRotation())); + return part.SolvePositionConstraint(ioBody1, ioBody2, inv_initial_orientation, inBaumgarte); + } + + return false; + } + + /// Return lagrange multiplier for swing + inline float GetTotalSwingYLambda() const + { + return mSwingLimitYConstraintPart.GetTotalLambda(); + } + + inline float GetTotalSwingZLambda() const + { + return mSwingLimitZConstraintPart.GetTotalLambda(); + } + + /// Return lagrange multiplier for twist + inline float GetTotalTwistLambda() const + { + return mTwistLimitConstraintPart.GetTotalLambda(); + } + + /// Save state of this constraint part + void SaveState(StateRecorder &inStream) const + { + mSwingLimitYConstraintPart.SaveState(inStream); + mSwingLimitZConstraintPart.SaveState(inStream); + mTwistLimitConstraintPart.SaveState(inStream); + } + + /// Restore state of this constraint part + void RestoreState(StateRecorder &inStream) + { + mSwingLimitYConstraintPart.RestoreState(inStream); + mSwingLimitZConstraintPart.RestoreState(inStream); + mTwistLimitConstraintPart.RestoreState(inStream); + } + +private: + // CONFIGURATION PROPERTIES FOLLOW + + enum ERotationFlags + { + /// Indicates that axis is completely locked (cannot rotate around this axis) + TwistXLocked = 1 << 0, + SwingYLocked = 1 << 1, + SwingZLocked = 1 << 2, + + /// Indicates that axis is completely free (can rotate around without limits) + TwistXFree = 1 << 3, + SwingYFree = 1 << 4, + SwingZFree = 1 << 5, + SwingYZFree = SwingYFree | SwingZFree + }; + + uint8 mRotationFlags; + + // Constants + ESwingType mSwingType = ESwingType::Cone; + float mSinTwistHalfMinAngle; + float mSinTwistHalfMaxAngle; + float mCosTwistHalfMinAngle; + float mCosTwistHalfMaxAngle; + float mSwingYHalfMinAngle; + float mSwingYHalfMaxAngle; + float mSwingZHalfMinAngle; + float mSwingZHalfMaxAngle; + float mSinSwingYHalfMinAngle; + float mSinSwingYHalfMaxAngle; + float mSinSwingZHalfMinAngle; + float mSinSwingZHalfMaxAngle; + float mCosSwingYHalfMinAngle; + float mCosSwingYHalfMaxAngle; + float mCosSwingZHalfMinAngle; + float mCosSwingZHalfMaxAngle; + + // RUN TIME PROPERTIES FOLLOW + + /// Rotation axis for the angle constraint parts + Vec3 mWorldSpaceSwingLimitYRotationAxis; + Vec3 mWorldSpaceSwingLimitZRotationAxis; + Vec3 mWorldSpaceTwistLimitRotationAxis; + + /// The constraint parts + AngleConstraintPart mSwingLimitYConstraintPart; + AngleConstraintPart mSwingLimitZConstraintPart; + AngleConstraintPart mTwistLimitConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.cpp new file mode 100644 index 000000000..f3f968883 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.cpp @@ -0,0 +1,1718 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +#ifdef JPH_DEBUG_RENDERER +bool ContactConstraintManager::sDrawContactPoint = false; +bool ContactConstraintManager::sDrawSupportingFaces = false; +bool ContactConstraintManager::sDrawContactPointReduction = false; +bool ContactConstraintManager::sDrawContactManifolds = false; +#endif // JPH_DEBUG_RENDERER + +//#define JPH_MANIFOLD_CACHE_DEBUG + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::WorldContactPoint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal) +{ + // Calculate collision points relative to body + RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2); + Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition()); + Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition()); + + mNonPenetrationConstraint.CalculateConstraintPropertiesWithMassOverride(inBody1, inInvMass1, inInvInertiaScale1, r1, inBody2, inInvMass2, inInvInertiaScale2, r2, inWorldSpaceNormal); +} + +template +JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution) +{ + JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2 + << " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2 + << " restitution: " << inSettings.mCombinedRestitution << " friction: " << inSettings.mCombinedFriction << " minv: " << inMinVelocityForRestitution + << " surface_vel: " << inSettings.mRelativeLinearSurfaceVelocity << " surface_ang: " << inSettings.mRelativeAngularSurfaceVelocity); + + // Calculate collision points relative to body + RVec3 p = 0.5_r * (inWorldSpacePosition1 + inWorldSpacePosition2); + Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition()); + Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition()); + + // Calculate velocity of collision points + Vec3 relative_velocity; + if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static) + relative_velocity = inBody2.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r2) - inBody1.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r1); + else if constexpr (Type1 != EMotionType::Static) + relative_velocity = -inBody1.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r1); + else if constexpr (Type2 != EMotionType::Static) + relative_velocity = inBody2.GetMotionPropertiesUnchecked()->GetPointVelocityCOM(r2); + else + { + JPH_ASSERT(false); // Static vs static makes no sense + relative_velocity = Vec3::sZero(); + } + float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal); + + // How much the shapes are penetrating (> 0 if penetrating, < 0 if separated) + float penetration = Vec3(inWorldSpacePosition1 - inWorldSpacePosition2).Dot(inWorldSpaceNormal); + + // If there is no penetration, this is a speculative contact and we will apply a bias to the contact constraint + // so that the constraint becomes relative_velocity . contact normal > -penetration / delta_time + // instead of relative_velocity . contact normal > 0 + // See: GDC 2013: "Physics for Game Programmers; Continuous Collision" - Erin Catto + float speculative_contact_velocity_bias = max(0.0f, -penetration / inDeltaTime); + + // Determine if the velocity is big enough for restitution + float normal_velocity_bias; + if (inSettings.mCombinedRestitution > 0.0f && normal_velocity < -inMinVelocityForRestitution) + { + // We have a velocity that is big enough for restitution. This is where speculative contacts don't work + // great as we have to decide now if we're going to apply the restitution or not. If the relative + // velocity is big enough for a hit, we apply the restitution (in the end, due to other constraints, + // the objects may actually not collide and we will have applied restitution incorrectly). Another + // artifact that occurs because of this approximation is that the object will bounce from its current + // position rather than from a position where it is touching the other object. This causes the object + // to appear to move faster for 1 frame (the opposite of time stealing). + if (normal_velocity < -speculative_contact_velocity_bias) + normal_velocity_bias = inSettings.mCombinedRestitution * normal_velocity; + else + // In this case we have predicted that we don't hit the other object, but if we do (due to other constraints changing velocities) + // the speculative contact will prevent penetration but will not apply restitution leading to another artifact. + normal_velocity_bias = speculative_contact_velocity_bias; + } + else + { + // No restitution. We can safely apply our contact velocity bias. + normal_velocity_bias = speculative_contact_velocity_bias; + } + + mNonPenetrationConstraint.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceNormal, normal_velocity_bias); + + // Calculate friction part + if (inSettings.mCombinedFriction > 0.0f) + { + // Get surface velocity relative to tangents + Vec3 ws_surface_velocity = inSettings.mRelativeLinearSurfaceVelocity + inSettings.mRelativeAngularSurfaceVelocity.Cross(r1); + float surface_velocity1 = inWorldSpaceTangent1.Dot(ws_surface_velocity); + float surface_velocity2 = inWorldSpaceTangent2.Dot(ws_surface_velocity); + + // Implement friction as 2 AxisContraintParts + mFrictionConstraint1.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent1, surface_velocity1); + mFrictionConstraint2.TemplatedCalculateConstraintProperties(inInvM1, inInvI1, r1, inInvM2, inInvI2, r2, inWorldSpaceTangent2, surface_velocity2); + } + else + { + // Turn off friction constraint + mFrictionConstraint1.Deactivate(); + mFrictionConstraint2.Deactivate(); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::ContactConstraint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +#ifdef JPH_DEBUG_RENDERER +void ContactConstraintManager::ContactConstraint::Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const +{ + if (mContactPoints.empty()) + return; + + // Get body transforms + RMat44 transform_body1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform_body2 = mBody2->GetCenterOfMassTransform(); + + RVec3 prev_point = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints.back().mContactPoint->mPosition1); + for (const WorldContactPoint &wcp : mContactPoints) + { + // Test if any lambda from the previous frame was transferred + float radius = wcp.mNonPenetrationConstraint.GetTotalLambda() == 0.0f + && wcp.mFrictionConstraint1.GetTotalLambda() == 0.0f + && wcp.mFrictionConstraint2.GetTotalLambda() == 0.0f? 0.1f : 0.2f; + + RVec3 next_point = transform_body1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + inRenderer->DrawMarker(next_point, Color::sCyan, radius); + inRenderer->DrawMarker(transform_body2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2), Color::sPurple, radius); + + // Draw edge + inRenderer->DrawArrow(prev_point, next_point, inManifoldColor, 0.05f); + prev_point = next_point; + } + + // Draw normal + RVec3 wp = transform_body1 * Vec3::sLoadFloat3Unsafe(mContactPoints[0].mContactPoint->mPosition1); + inRenderer->DrawArrow(wp, wp + GetWorldSpaceNormal(), Color::sRed, 0.05f); + + // Get tangents + Vec3 t1, t2; + GetTangents(t1, t2); + + // Draw tangents + inRenderer->DrawLine(wp, wp + t1, Color::sGreen); + inRenderer->DrawLine(wp, wp + t2, Color::sBlue); +} +#endif // JPH_DEBUG_RENDERER + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedContactPoint +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedContactPoint::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mPosition1); + inStream.Write(mPosition2); + inStream.Write(mNonPenetrationLambda); + inStream.Write(mFrictionLambda); +} + +void ContactConstraintManager::CachedContactPoint::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mPosition1); + inStream.Read(mPosition2); + inStream.Read(mNonPenetrationLambda); + inStream.Read(mFrictionLambda); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedManifold +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedManifold::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mContactNormal); +} + +void ContactConstraintManager::CachedManifold::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mContactNormal); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::CachedBodyPair +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::CachedBodyPair::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mDeltaPosition); + inStream.Write(mDeltaRotation); +} + +void ContactConstraintManager::CachedBodyPair::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mDeltaPosition); + inStream.Read(mDeltaRotation); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager::ManifoldCache +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +void ContactConstraintManager::ManifoldCache::Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize) +{ + mAllocator.Init(inMaxBodyPairs * sizeof(BodyPairMap::KeyValue) + inCachedManifoldsSize); + mCachedManifolds.Init(GetNextPowerOf2(inMaxContactConstraints)); + mCachedBodyPairs.Init(GetNextPowerOf2(inMaxBodyPairs)); +} + +void ContactConstraintManager::ManifoldCache::Clear() +{ + JPH_PROFILE_FUNCTION(); + + mCachedManifolds.Clear(); + mCachedBodyPairs.Clear(); + mAllocator.Clear(); + +#ifdef JPH_ENABLE_ASSERTS + // Mark as incomplete + mIsFinalized = false; +#endif +} + +void ContactConstraintManager::ManifoldCache::Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds) +{ + // Minimum amount of buckets to use in the hash map + constexpr uint32 cMinBuckets = 1024; + + // Use the next higher power of 2 of amount of objects in the cache from last frame to determine the amount of buckets in this frame + mCachedManifolds.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumManifolds)), mCachedManifolds.GetMaxBuckets())); + mCachedBodyPairs.SetNumBuckets(min(max(cMinBuckets, GetNextPowerOf2(inExpectedNumBodyPairs)), mCachedBodyPairs.GetMaxBuckets())); +} + +const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedManifolds.Find(inKey, inKeyHash); +} + +ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints) +{ + JPH_ASSERT(!mIsFinalized); + MKeyValue *kv = mCachedManifolds.Create(ioContactAllocator, inKey, inKeyHash, CachedManifold::sGetRequiredExtraSize(inNumContactPoints)); + if (kv == nullptr) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ManifoldCacheFull; + return nullptr; + } + kv->GetValue().mNumContactPoints = uint16(inNumContactPoints); + ++ioContactAllocator.mNumManifolds; + return kv; +} + +ContactConstraintManager::MKVAndCreated ContactConstraintManager::ManifoldCache::FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints) +{ + MKeyValue *kv = const_cast(mCachedManifolds.Find(inKey, inKeyHash)); + if (kv != nullptr) + return { kv, false }; + + return { Create(ioContactAllocator, inKey, inKeyHash, inNumContactPoints), true }; +} + +uint32 ContactConstraintManager::ManifoldCache::ToHandle(const MKeyValue *inKeyValue) const +{ + JPH_ASSERT(!mIsFinalized); + return mCachedManifolds.ToHandle(inKeyValue); +} + +const ContactConstraintManager::MKeyValue *ContactConstraintManager::ManifoldCache::FromHandle(uint32 inHandle) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedManifolds.FromHandle(inHandle); +} + +const ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Find(const BodyPair &inKey, uint64 inKeyHash) const +{ + JPH_ASSERT(mIsFinalized); + return mCachedBodyPairs.Find(inKey, inKeyHash); +} + +ContactConstraintManager::BPKeyValue *ContactConstraintManager::ManifoldCache::Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash) +{ + JPH_ASSERT(!mIsFinalized); + BPKeyValue *kv = mCachedBodyPairs.Create(ioContactAllocator, inKey, inKeyHash, 0); + if (kv == nullptr) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::BodyPairCacheFull; + return nullptr; + } + ++ioContactAllocator.mNumBodyPairs; + return kv; +} + +void ContactConstraintManager::ManifoldCache::GetAllBodyPairsSorted(Array &outAll) const +{ + JPH_ASSERT(mIsFinalized); + mCachedBodyPairs.GetAllKeyValues(outAll); + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const BPKeyValue *inLHS, const BPKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array &outAll) const +{ + JPH_ASSERT(mIsFinalized); + + // Iterate through the attached manifolds + for (uint32 handle = inBodyPair.mFirstCachedManifold; handle != ManifoldMap::cInvalidHandle; handle = FromHandle(handle)->GetValue().mNextWithSameBodyPair) + { + const MKeyValue *kv = mCachedManifolds.FromHandle(handle); + outAll.push_back(kv); + } + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::GetAllCCDManifoldsSorted(Array &outAll) const +{ + mCachedManifolds.GetAllKeyValues(outAll); + + for (int i = (int)outAll.size() - 1; i >= 0; --i) + if ((outAll[i]->GetValue().mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0) + { + outAll[i] = outAll.back(); + outAll.pop_back(); + } + + // Sort by key + QuickSort(outAll.begin(), outAll.end(), [](const MKeyValue *inLHS, const MKeyValue *inRHS) { + return inLHS->GetKey() < inRHS->GetKey(); + }); +} + +void ContactConstraintManager::ManifoldCache::ContactPointRemovedCallbacks(ContactListener *inListener) +{ + JPH_PROFILE_FUNCTION(); + + for (MKeyValue &kv : mCachedManifolds) + if ((kv.GetValue().mFlags & uint16(CachedManifold::EFlags::ContactPersisted)) == 0) + inListener->OnContactRemoved(kv.GetKey()); +} + +#ifdef JPH_ENABLE_ASSERTS + +void ContactConstraintManager::ManifoldCache::Finalize() +{ + mIsFinalized = true; + +#ifdef JPH_MANIFOLD_CACHE_DEBUG + Trace("ManifoldMap:"); + mCachedManifolds.TraceStats(); + Trace("BodyPairMap:"); + mCachedBodyPairs.TraceStats(); +#endif // JPH_MANIFOLD_CACHE_DEBUG +} + +#endif + +void ContactConstraintManager::ManifoldCache::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + JPH_ASSERT(mIsFinalized); + + // Get contents of cache + Array all_bp; + GetAllBodyPairsSorted(all_bp); + + // Determine which ones to save + Array selected_bp; + if (inFilter == nullptr) + selected_bp = std::move(all_bp); + else + { + selected_bp.reserve(all_bp.size()); + for (const BPKeyValue *bp_kv : all_bp) + if (inFilter->ShouldSaveContact(bp_kv->GetKey().mBodyA, bp_kv->GetKey().mBodyB)) + selected_bp.push_back(bp_kv); + } + + // Write body pairs + size_t num_body_pairs = selected_bp.size(); + inStream.Write(num_body_pairs); + for (const BPKeyValue *bp_kv : selected_bp) + { + // Write body pair key + inStream.Write(bp_kv->GetKey()); + + // Write body pair + const CachedBodyPair &bp = bp_kv->GetValue(); + bp.SaveState(inStream); + + // Get attached manifolds + Array all_m; + GetAllManifoldsSorted(bp, all_m); + + // Write num manifolds + size_t num_manifolds = all_m.size(); + inStream.Write(num_manifolds); + + // Write all manifolds + for (const MKeyValue *m_kv : all_m) + { + // Write key + inStream.Write(m_kv->GetKey()); + const CachedManifold &cm = m_kv->GetValue(); + JPH_ASSERT((cm.mFlags & (uint16)CachedManifold::EFlags::CCDContact) == 0); + + // Write amount of contacts + inStream.Write(cm.mNumContactPoints); + + // Write manifold + cm.SaveState(inStream); + + // Write contact points + for (uint32 i = 0; i < cm.mNumContactPoints; ++i) + cm.mContactPoints[i].SaveState(inStream); + } + } + + // Get CCD manifolds + Array all_m; + GetAllCCDManifoldsSorted(all_m); + + // Determine which ones to save + Array selected_m; + if (inFilter == nullptr) + selected_m = std::move(all_m); + else + { + selected_m.reserve(all_m.size()); + for (const MKeyValue *m_kv : all_m) + if (inFilter->ShouldSaveContact(m_kv->GetKey().GetBody1ID(), m_kv->GetKey().GetBody2ID())) + selected_m.push_back(m_kv); + } + + // Write all CCD manifold keys + size_t num_manifolds = selected_m.size(); + inStream.Write(num_manifolds); + for (const MKeyValue *m_kv : selected_m) + inStream.Write(m_kv->GetKey()); +} + +bool ContactConstraintManager::ManifoldCache::RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream) +{ + JPH_ASSERT(!mIsFinalized); + + bool success = true; + + // Create a contact allocator for restoring the contact cache + ContactAllocator contact_allocator(GetContactAllocator()); + + // When validating, get all existing body pairs + Array all_bp; + if (inStream.IsValidating()) + inReadCache.GetAllBodyPairsSorted(all_bp); + + // Read amount of body pairs + size_t num_body_pairs; + if (inStream.IsValidating()) + num_body_pairs = all_bp.size(); + inStream.Read(num_body_pairs); + + // Read entire cache + for (size_t i = 0; i < num_body_pairs; ++i) + { + // Read key + BodyPair body_pair_key; + if (inStream.IsValidating() && i < all_bp.size()) + body_pair_key = all_bp[i]->GetKey(); + inStream.Read(body_pair_key); + + // Create new entry for this body pair + uint64 body_pair_hash = body_pair_key.GetHash(); + BPKeyValue *bp_kv = Create(contact_allocator, body_pair_key, body_pair_hash); + if (bp_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedBodyPair &bp = bp_kv->GetValue(); + + // Read body pair + if (inStream.IsValidating() && i < all_bp.size()) + memcpy(&bp, &all_bp[i]->GetValue(), sizeof(CachedBodyPair)); + bp.RestoreState(inStream); + + // When validating, get all existing manifolds + Array all_m; + if (inStream.IsValidating()) + inReadCache.GetAllManifoldsSorted(all_bp[i]->GetValue(), all_m); + + // Read amount of manifolds + size_t num_manifolds; + if (inStream.IsValidating()) + num_manifolds = all_m.size(); + inStream.Read(num_manifolds); + + uint32 handle = ManifoldMap::cInvalidHandle; + for (size_t j = 0; j < num_manifolds; ++j) + { + // Read key + SubShapeIDPair sub_shape_key; + if (inStream.IsValidating() && j < all_m.size()) + sub_shape_key = all_m[j]->GetKey(); + inStream.Read(sub_shape_key); + uint64 sub_shape_key_hash = sub_shape_key.GetHash(); + + // Read amount of contact points + uint16 num_contact_points; + if (inStream.IsValidating() && j < all_m.size()) + num_contact_points = all_m[j]->GetValue().mNumContactPoints; + inStream.Read(num_contact_points); + + // Read manifold + MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, num_contact_points); + if (m_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedManifold &cm = m_kv->GetValue(); + if (inStream.IsValidating() && j < all_m.size()) + { + memcpy(&cm, &all_m[j]->GetValue(), CachedManifold::sGetRequiredTotalSize(num_contact_points)); + cm.mNumContactPoints = uint16(num_contact_points); // Restore num contact points + } + cm.RestoreState(inStream); + cm.mNextWithSameBodyPair = handle; + handle = ToHandle(m_kv); + + // Read contact points + for (uint32 k = 0; k < num_contact_points; ++k) + cm.mContactPoints[k].RestoreState(inStream); + } + bp.mFirstCachedManifold = handle; + } + + // When validating, get all existing CCD manifolds + Array all_m; + if (inStream.IsValidating()) + inReadCache.GetAllCCDManifoldsSorted(all_m); + + // Read amount of CCD manifolds + size_t num_manifolds; + if (inStream.IsValidating()) + num_manifolds = all_m.size(); + inStream.Read(num_manifolds); + + for (size_t j = 0; j < num_manifolds; ++j) + { + // Read key + SubShapeIDPair sub_shape_key; + if (inStream.IsValidating() && j < all_m.size()) + sub_shape_key = all_m[j]->GetKey(); + inStream.Read(sub_shape_key); + uint64 sub_shape_key_hash = sub_shape_key.GetHash(); + + // Create CCD manifold + MKeyValue *m_kv = Create(contact_allocator, sub_shape_key, sub_shape_key_hash, 0); + if (m_kv == nullptr) + { + // Out of cache space + success = false; + break; + } + CachedManifold &cm = m_kv->GetValue(); + cm.mFlags |= (uint16)CachedManifold::EFlags::CCDContact; + } + +#ifdef JPH_ENABLE_ASSERTS + mIsFinalized = true; +#endif + + return success; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +// ContactConstraintManager +//////////////////////////////////////////////////////////////////////////////////////////////////////// + +ContactConstraintManager::ContactConstraintManager(const PhysicsSettings &inPhysicsSettings) : + mPhysicsSettings(inPhysicsSettings) +{ +#ifdef JPH_ENABLE_ASSERTS + // For the first frame mark this empty buffer as finalized + mCache[mCacheWriteIdx ^ 1].Finalize(); +#endif +} + +ContactConstraintManager::~ContactConstraintManager() +{ + JPH_ASSERT(mConstraints == nullptr); +} + +void ContactConstraintManager::Init(uint inMaxBodyPairs, uint inMaxContactConstraints) +{ + mMaxConstraints = inMaxContactConstraints; + + // Calculate worst case cache usage + uint cached_manifolds_size = inMaxContactConstraints * (sizeof(CachedManifold) + (MaxContactPoints - 1) * sizeof(CachedContactPoint)); + + // Init the caches + mCache[0].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size); + mCache[1].Init(inMaxBodyPairs, inMaxContactConstraints, cached_manifolds_size); +} + +void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inContext) +{ + // Store context + mUpdateContext = inContext; + + // Allocate temporary constraint buffer + JPH_ASSERT(mConstraints == nullptr); + mConstraints = (ContactConstraint *)inContext->mTempAllocator->Allocate(mMaxConstraints * sizeof(ContactConstraint)); +} + +template +JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) +{ + // Calculate scaled mass and inertia + Mat44 inv_i1; + if constexpr (Type1 == EMotionType::Dynamic) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + inv_i1 = inSettings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inTransformBody1.GetRotation()); + } + else + { + inv_i1 = Mat44::sZero(); + } + + Mat44 inv_i2; + if constexpr (Type2 == EMotionType::Dynamic) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + inv_i2 = inSettings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inTransformBody2.GetRotation()); + } + else + { + inv_i2 = Mat44::sZero(); + } + + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + // Setup velocity constraint properties + float min_velocity_for_restitution = mPhysicsSettings.mMinVelocityForRestitution; + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2); + wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(inDeltaTime, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution); + } +} + +inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2) +{ + // Dispatch to the correct templated form + switch (inBody1.GetMotionType()) + { + case EMotionType::Dynamic: + switch (inBody2.GetMotionType()) + { + case EMotionType::Dynamic: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Kinematic: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(inBody2.IsDynamic()); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + case EMotionType::Static: + JPH_ASSERT(inBody2.IsDynamic()); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + break; + + default: + JPH_ASSERT(false); + break; + } +} + +void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated) +{ + JPH_PROFILE_FUNCTION(); + + // Start with nothing found and not handled + outConstraintCreated = false; + outPairHandled = false; + + // Swap bodies so that body 1 id < body 2 id + Body *body1, *body2; + if (inBody1.GetID() < inBody2.GetID()) + { + body1 = &inBody1; + body2 = &inBody2; + } + else + { + body1 = &inBody2; + body2 = &inBody1; + } + + // Find the cached body pair + BodyPair body_pair_key(body1->GetID(), body2->GetID()); + uint64 body_pair_hash = body_pair_key.GetHash(); + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const BPKeyValue *kv = read_cache.Find(body_pair_key, body_pair_hash); + if (kv == nullptr) + return; + const CachedBodyPair &input_cbp = kv->GetValue(); + + // Get relative translation + Quat inv_r1 = body1->GetRotation().Conjugated(); + Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition()); + + // Get old position delta + Vec3 old_delta_position = Vec3::sLoadFloat3Unsafe(input_cbp.mDeltaPosition); + + // Check if bodies are still roughly in the same relative position + if ((delta_position - old_delta_position).LengthSq() > mPhysicsSettings.mBodyPairCacheMaxDeltaPositionSq) + return; + + // Determine relative orientation + Quat delta_rotation = inv_r1 * body2->GetRotation(); + + // Reconstruct old quaternion delta + Quat old_delta_rotation = Quat::sLoadFloat3Unsafe(input_cbp.mDeltaRotation); + + // Check if bodies are still roughly in the same relative orientation + // The delta between 2 quaternions p and q is: p q^* = [rotation_axis * sin(angle / 2), cos(angle / 2)] + // From the W component we can extract the angle: cos(angle / 2) = px * qx + py * qy + pz * qz + pw * qw = p . q + // Since we want to abort if the rotation is smaller than -angle or bigger than angle, we can write the comparison as |p . q| < cos(angle / 2) + if (abs(delta_rotation.Dot(old_delta_rotation)) < mPhysicsSettings.mBodyPairCacheCosMaxDeltaRotationDiv2) + return; + + // The cache is valid, return that we've handled this body pair + outPairHandled = true; + + // Copy the cached body pair to this frame + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + BPKeyValue *output_bp_kv = write_cache.Create(ioContactAllocator, body_pair_key, body_pair_hash); + if (output_bp_kv == nullptr) + return; // Out of cache space + CachedBodyPair *output_cbp = &output_bp_kv->GetValue(); + memcpy(output_cbp, &input_cbp, sizeof(CachedBodyPair)); + + // If there were no contacts, we have handled the contact + if (input_cbp.mFirstCachedManifold == ManifoldMap::cInvalidHandle) + return; + + // Get body transforms + RMat44 transform_body1 = body1->GetCenterOfMassTransform(); + RMat44 transform_body2 = body2->GetCenterOfMassTransform(); + + // Get time step + float delta_time = mUpdateContext->mStepDeltaTime; + + // Copy manifolds + uint32 output_handle = ManifoldMap::cInvalidHandle; + uint32 input_handle = input_cbp.mFirstCachedManifold; + do + { + JPH_PROFILE("Add Constraint From Cached Manifold"); + + // Find the existing manifold + const MKeyValue *input_kv = read_cache.FromHandle(input_handle); + const SubShapeIDPair &input_key = input_kv->GetKey(); + const CachedManifold &input_cm = input_kv->GetValue(); + JPH_ASSERT(input_cm.mNumContactPoints > 0); // There should be contact points in this manifold! + + // Create room for manifold in write buffer and copy data + uint64 input_hash = input_key.GetHash(); + MKeyValue *output_kv = write_cache.Create(ioContactAllocator, input_key, input_hash, input_cm.mNumContactPoints); + if (output_kv == nullptr) + break; // Out of cache space + CachedManifold *output_cm = &output_kv->GetValue(); + memcpy(output_cm, &input_cm, CachedManifold::sGetRequiredTotalSize(input_cm.mNumContactPoints)); + + // Link the object under the body pairs + output_cm->mNextWithSameBodyPair = output_handle; + output_handle = write_cache.ToHandle(output_kv); + + // Calculate default contact settings + ContactSettings settings; + settings.mCombinedFriction = mCombineFriction(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2()); + settings.mCombinedRestitution = mCombineRestitution(*body1, input_key.GetSubShapeID1(), *body2, input_key.GetSubShapeID2()); + settings.mIsSensor = body1->IsSensor() || body2->IsSensor(); + + // Calculate world space contact normal + Vec3 world_space_normal = transform_body2.Multiply3x3(Vec3::sLoadFloat3Unsafe(output_cm->mContactNormal)).Normalized(); + + // Call contact listener to update settings + if (mContactListener != nullptr) + { + // Convert constraint to manifold structure for callback + ContactManifold manifold; + manifold.mWorldSpaceNormal = world_space_normal; + manifold.mSubShapeID1 = input_key.GetSubShapeID1(); + manifold.mSubShapeID2 = input_key.GetSubShapeID2(); + manifold.mBaseOffset = transform_body1.GetTranslation(); + manifold.mRelativeContactPointsOn1.resize(output_cm->mNumContactPoints); + manifold.mRelativeContactPointsOn2.resize(output_cm->mNumContactPoints); + Mat44 local_transform_body2 = transform_body2.PostTranslated(-manifold.mBaseOffset).ToMat44(); + float penetration_depth = -FLT_MAX; + for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i) + { + const CachedContactPoint &ccp = output_cm->mContactPoints[i]; + manifold.mRelativeContactPointsOn1[i] = transform_body1.Multiply3x3(Vec3::sLoadFloat3Unsafe(ccp.mPosition1)); + manifold.mRelativeContactPointsOn2[i] = local_transform_body2 * Vec3::sLoadFloat3Unsafe(ccp.mPosition2); + penetration_depth = max(penetration_depth, (manifold.mRelativeContactPointsOn1[0] - manifold.mRelativeContactPointsOn2[0]).Dot(world_space_normal)); + } + manifold.mPenetrationDepth = penetration_depth; // We don't have the penetration depth anymore, estimate it + + // Notify callback + mContactListener->OnContactPersisted(*body1, *body2, manifold, settings); + } + + JPH_ASSERT(settings.mIsSensor || !(body1->IsSensor() || body2->IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); + if (!settings.mIsSensor // If one of the bodies is a sensor, don't actually create the constraint + && ((body1->IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint + || (body2->IsDynamic() && settings.mInvMassScale2 != 0.0f))) + { + // Add contact constraint in world space for the solver + uint32 constraint_idx = mNumConstraints++; + if (constraint_idx >= mMaxConstraints) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull; + break; + } + + // A constraint will be created + outConstraintCreated = true; + + ContactConstraint &constraint = mConstraints[constraint_idx]; + new (&constraint) ContactConstraint(); + constraint.mBody1 = body1; + constraint.mBody2 = body2; + constraint.mSortKey = input_hash; + world_space_normal.StoreFloat3(&constraint.mWorldSpaceNormal); + constraint.mCombinedFriction = settings.mCombinedFriction; + constraint.mInvMass1 = body1->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * body1->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale1 = settings.mInvInertiaScale1; + constraint.mInvMass2 = body2->GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * body2->GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale2 = settings.mInvInertiaScale2; + constraint.mContactPoints.resize(output_cm->mNumContactPoints); + for (uint32 i = 0; i < output_cm->mNumContactPoints; ++i) + { + CachedContactPoint &ccp = output_cm->mContactPoints[i]; + WorldContactPoint &wcp = constraint.mContactPoints[i]; + wcp.mNonPenetrationConstraint.SetTotalLambda(ccp.mNonPenetrationLambda); + wcp.mFrictionConstraint1.SetTotalLambda(ccp.mFrictionLambda[0]); + wcp.mFrictionConstraint2.SetTotalLambda(ccp.mFrictionLambda[1]); + wcp.mContactPoint = &ccp; + } + + JPH_DET_LOG("GetContactsFromCache: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey); + + // Calculate friction and non-penetration constraint properties for all contact points + CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, transform_body1, transform_body2, *body1, *body2); + + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal()); + + #ifdef JPH_DEBUG_RENDERER + // Draw the manifold + if (sDrawContactManifolds) + constraint.Draw(DebugRenderer::sInstance, Color::sYellow); + #endif // JPH_DEBUG_RENDERER + } + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + input_cm.mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + + // Fetch the next manifold + input_handle = input_cm.mNextWithSameBodyPair; + } + while (input_handle != ManifoldMap::cInvalidHandle); + output_cbp->mFirstCachedManifold = output_handle; +} + +ContactConstraintManager::BodyPairHandle ContactConstraintManager::AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2) +{ + JPH_PROFILE_FUNCTION(); + + // Swap bodies so that body 1 id < body 2 id + const Body *body1, *body2; + if (inBody1.GetID() < inBody2.GetID()) + { + body1 = &inBody1; + body2 = &inBody2; + } + else + { + body1 = &inBody2; + body2 = &inBody1; + } + + // Add an entry + BodyPair body_pair_key(body1->GetID(), body2->GetID()); + uint64 body_pair_hash = body_pair_key.GetHash(); + BPKeyValue *body_pair_kv = mCache[mCacheWriteIdx].Create(ioContactAllocator, body_pair_key, body_pair_hash); + if (body_pair_kv == nullptr) + return nullptr; // Out of cache space + CachedBodyPair *cbp = &body_pair_kv->GetValue(); + cbp->mFirstCachedManifold = ManifoldMap::cInvalidHandle; + + // Get relative translation + Quat inv_r1 = body1->GetRotation().Conjugated(); + Vec3 delta_position = inv_r1 * Vec3(body2->GetCenterOfMassPosition() - body1->GetCenterOfMassPosition()); + + // Store it + delta_position.StoreFloat3(&cbp->mDeltaPosition); + + // Determine relative orientation + Quat delta_rotation = inv_r1 * body2->GetRotation(); + + // Store it + delta_rotation.StoreFloat3(&cbp->mDeltaRotation); + + return cbp; +} + +template +bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold) +{ + // Calculate hash + SubShapeIDPair key { inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2 }; + uint64 key_hash = key.GetHash(); + + // Determine number of contact points + int num_contact_points = (int)inManifold.mRelativeContactPointsOn1.size(); + JPH_ASSERT(num_contact_points <= MaxContactPoints); + JPH_ASSERT(num_contact_points == (int)inManifold.mRelativeContactPointsOn2.size()); + + // Reserve space for new contact cache entry + // Note that for dynamic vs dynamic we always require the first body to have a lower body id to get a consistent key + // under which to look up the contact + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + MKeyValue *new_manifold_kv = write_cache.Create(ioContactAllocator, key, key_hash, num_contact_points); + if (new_manifold_kv == nullptr) + return false; // Out of cache space + CachedManifold *new_manifold = &new_manifold_kv->GetValue(); + + // Transform the world space normal to the space of body 2 (this is usually the static body) + RMat44 inverse_transform_body2 = inBody2.GetInverseCenterOfMassTransform(); + inverse_transform_body2.Multiply3x3(inManifold.mWorldSpaceNormal).Normalized().StoreFloat3(&new_manifold->mContactNormal); + + // Settings object that gets passed to the callback + ContactSettings settings; + settings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + settings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + settings.mIsSensor = inBody1.IsSensor() || inBody2.IsSensor(); + + // Get the contact points for the old cache entry + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash); + const CachedContactPoint *ccp_start; + const CachedContactPoint *ccp_end; + if (old_manifold_kv != nullptr) + { + // Call point persisted listener + if (mContactListener != nullptr) + mContactListener->OnContactPersisted(inBody1, inBody2, inManifold, settings); + + // Fetch the contact points from the old manifold + const CachedManifold *old_manifold = &old_manifold_kv->GetValue(); + ccp_start = old_manifold->mContactPoints; + ccp_end = ccp_start + old_manifold->mNumContactPoints; + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + old_manifold->mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + } + else + { + // Call point added listener + if (mContactListener != nullptr) + mContactListener->OnContactAdded(inBody1, inBody2, inManifold, settings); + + // No contact points available from old manifold + ccp_start = nullptr; + ccp_end = nullptr; + } + + // Get inverse transform for body 1 + RMat44 inverse_transform_body1 = inBody1.GetInverseCenterOfMassTransform(); + + bool contact_constraint_created = false; + + // If one of the bodies is a sensor, don't actually create the constraint + JPH_ASSERT(settings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); + if (!settings.mIsSensor + && ((inBody1.IsDynamic() && settings.mInvMassScale1 != 0.0f) // One of the bodies must have mass to be able to create a contact constraint + || (inBody2.IsDynamic() && settings.mInvMassScale2 != 0.0f))) + { + // Add contact constraint + uint32 constraint_idx = mNumConstraints++; + if (constraint_idx >= mMaxConstraints) + { + ioContactAllocator.mErrors |= EPhysicsUpdateError::ContactConstraintsFull; + + // Manifold has been created already, we're not filling it in, so we need to reset the contact number of points. + // Note that we don't hook it up to the body pair cache so that it won't be used as a cache during the next simulation. + new_manifold->mNumContactPoints = 0; + return false; + } + + // We will create a contact constraint + contact_constraint_created = true; + + ContactConstraint &constraint = mConstraints[constraint_idx]; + new (&constraint) ContactConstraint(); + constraint.mBody1 = &inBody1; + constraint.mBody2 = &inBody2; + constraint.mSortKey = key_hash; + inManifold.mWorldSpaceNormal.StoreFloat3(&constraint.mWorldSpaceNormal); + constraint.mCombinedFriction = settings.mCombinedFriction; + constraint.mInvMass1 = inBody1.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale1 * inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale1 = settings.mInvInertiaScale1; + constraint.mInvMass2 = inBody2.GetMotionPropertiesUnchecked() != nullptr? settings.mInvMassScale2 * inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + constraint.mInvInertiaScale2 = settings.mInvInertiaScale2; + + JPH_DET_LOG("TemplatedAddContactConstraint: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey); + + // Notify island builder + mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal()); + + // Get time step + float delta_time = mUpdateContext->mStepDeltaTime; + + // Calculate scaled mass and inertia + float inv_m1; + Mat44 inv_i1; + if constexpr (Type1 == EMotionType::Dynamic) + { + const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked(); + inv_m1 = settings.mInvMassScale1 * mp1->GetInverseMass(); + inv_i1 = settings.mInvInertiaScale1 * mp1->GetInverseInertiaForRotation(inverse_transform_body1.Transposed3x3()); + } + else + { + inv_m1 = 0.0f; + inv_i1 = Mat44::sZero(); + } + + float inv_m2; + Mat44 inv_i2; + if constexpr (Type2 == EMotionType::Dynamic) + { + const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked(); + inv_m2 = settings.mInvMassScale2 * mp2->GetInverseMass(); + inv_i2 = settings.mInvInertiaScale2 * mp2->GetInverseInertiaForRotation(inverse_transform_body2.Transposed3x3()); + } + else + { + inv_m2 = 0.0f; + inv_i2 = Mat44::sZero(); + } + + // Calculate tangents + Vec3 t1, t2; + constraint.GetTangents(t1, t2); + + constraint.mContactPoints.resize(num_contact_points); + for (int i = 0; i < num_contact_points; ++i) + { + // Convert to world space and set positions + WorldContactPoint &wcp = constraint.mContactPoints[i]; + RVec3 p1_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i]; + RVec3 p2_ws = inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i]; + + // Convert to local space to the body + Vec3 p1_ls = Vec3(inverse_transform_body1 * p1_ws); + Vec3 p2_ls = Vec3(inverse_transform_body2 * p2_ws); + + // Check if we have a close contact point from last update + bool lambda_set = false; + for (const CachedContactPoint *ccp = ccp_start; ccp < ccp_end; ccp++) + if (Vec3::sLoadFloat3Unsafe(ccp->mPosition1).IsClose(p1_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq) + && Vec3::sLoadFloat3Unsafe(ccp->mPosition2).IsClose(p2_ls, mPhysicsSettings.mContactPointPreserveLambdaMaxDistSq)) + { + // Get lambdas from previous frame + wcp.mNonPenetrationConstraint.SetTotalLambda(ccp->mNonPenetrationLambda); + wcp.mFrictionConstraint1.SetTotalLambda(ccp->mFrictionLambda[0]); + wcp.mFrictionConstraint2.SetTotalLambda(ccp->mFrictionLambda[1]); + lambda_set = true; + break; + } + if (!lambda_set) + { + wcp.mNonPenetrationConstraint.SetTotalLambda(0.0f); + wcp.mFrictionConstraint1.SetTotalLambda(0.0f); + wcp.mFrictionConstraint2.SetTotalLambda(0.0f); + } + + // Create new contact point + CachedContactPoint &cp = new_manifold->mContactPoints[i]; + p1_ls.StoreFloat3(&cp.mPosition1); + p2_ls.StoreFloat3(&cp.mPosition2); + wcp.mContactPoint = &cp; + + // Setup velocity constraint + wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(delta_time, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution); + } + + #ifdef JPH_DEBUG_RENDERER + // Draw the manifold + if (sDrawContactManifolds) + constraint.Draw(DebugRenderer::sInstance, Color::sOrange); + #endif // JPH_DEBUG_RENDERER + } + else + { + // Store the contact manifold in the cache + for (int i = 0; i < num_contact_points; ++i) + { + // Convert to local space to the body + Vec3 p1 = Vec3(inverse_transform_body1 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn1[i])); + Vec3 p2 = Vec3(inverse_transform_body2 * (inManifold.mBaseOffset + inManifold.mRelativeContactPointsOn2[i])); + + // Create new contact point + CachedContactPoint &cp = new_manifold->mContactPoints[i]; + p1.StoreFloat3(&cp.mPosition1); + p2.StoreFloat3(&cp.mPosition2); + + // Reset contact impulses, we haven't applied any + cp.mNonPenetrationLambda = 0.0f; + cp.mFrictionLambda[0] = 0.0f; + cp.mFrictionLambda[1] = 0.0f; + } + } + + // Store cached contact point in body pair cache + CachedBodyPair *cbp = reinterpret_cast(inBodyPairHandle); + new_manifold->mNextWithSameBodyPair = cbp->mFirstCachedManifold; + cbp->mFirstCachedManifold = write_cache.ToHandle(new_manifold_kv); + + // A contact constraint was added + return contact_constraint_created; +} + +bool ContactConstraintManager::AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold) +{ + JPH_PROFILE_FUNCTION(); + + JPH_DET_LOG("AddContactConstraint: id1: " << inBody1.GetID() << " id2: " << inBody2.GetID() + << " subshape1: " << inManifold.mSubShapeID1 << " subshape2: " << inManifold.mSubShapeID2 + << " normal: " << inManifold.mWorldSpaceNormal << " pendepth: " << inManifold.mPenetrationDepth); + + JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized()); + + // Swap bodies so that body 1 id < body 2 id + const ContactManifold *manifold; + Body *body1, *body2; + ContactManifold temp; + if (inBody2.GetID() < inBody1.GetID()) + { + body1 = &inBody2; + body2 = &inBody1; + temp = inManifold.SwapShapes(); + manifold = &temp; + } + else + { + body1 = &inBody1; + body2 = &inBody2; + manifold = &inManifold; + } + + // Dispatch to the correct templated form + // Note: Non-dynamic vs non-dynamic can happen in this case due to one body being a sensor, so we need to have an extended switch case here + switch (body1->GetMotionType()) + { + case EMotionType::Dynamic: + { + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + default: + JPH_ASSERT(false); + break; + } + break; + } + + case EMotionType::Kinematic: + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Static: + switch (body2->GetMotionType()) + { + case EMotionType::Dynamic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Kinematic: + return TemplatedAddContactConstraint(ioContactAllocator, inBodyPairHandle, *body1, *body2, *manifold); + + case EMotionType::Static: // Static vs static not possible + default: + JPH_ASSERT(false); + break; + } + break; + + default: + JPH_ASSERT(false); + break; + } + + return false; +} + +void ContactConstraintManager::OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings) +{ + JPH_ASSERT(inManifold.mWorldSpaceNormal.IsNormalized()); + + // Calculate contact settings + outSettings.mCombinedFriction = mCombineFriction(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + outSettings.mCombinedRestitution = mCombineRestitution(inBody1, inManifold.mSubShapeID1, inBody2, inManifold.mSubShapeID2); + outSettings.mIsSensor = false; // For now, no sensors are supported during CCD + + // The remainder of this function only deals with calling contact callbacks, if there's no contact callback we also don't need to do this work + if (mContactListener != nullptr) + { + // Swap bodies so that body 1 id < body 2 id + const ContactManifold *manifold; + const Body *body1, *body2; + ContactManifold temp; + if (inBody2.GetID() < inBody1.GetID()) + { + body1 = &inBody2; + body2 = &inBody1; + temp = inManifold.SwapShapes(); + manifold = &temp; + } + else + { + body1 = &inBody1; + body2 = &inBody2; + manifold = &inManifold; + } + + // Calculate hash + SubShapeIDPair key { body1->GetID(), manifold->mSubShapeID1, body2->GetID(), manifold->mSubShapeID2 }; + uint64 key_hash = key.GetHash(); + + // Check if we already created this contact this physics update + ManifoldCache &write_cache = mCache[mCacheWriteIdx]; + MKVAndCreated new_manifold_kv = write_cache.FindOrCreate(ioContactAllocator, key, key_hash, 0); + if (new_manifold_kv.second) + { + // This contact is new for this physics update, check if previous update we already had this contact. + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + const MKeyValue *old_manifold_kv = read_cache.Find(key, key_hash); + if (old_manifold_kv == nullptr) + { + // New contact + mContactListener->OnContactAdded(*body1, *body2, *manifold, outSettings); + } + else + { + // Existing contact + mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings); + + // Mark contact as persisted so that we won't fire OnContactRemoved callbacks + old_manifold_kv->GetValue().mFlags |= (uint16)CachedManifold::EFlags::ContactPersisted; + } + + // Check if the cache is full + if (new_manifold_kv.first != nullptr) + { + // We don't store any contact points in this manifold as it is not for caching impulses, we only need to know that the contact was created + CachedManifold &new_manifold = new_manifold_kv.first->GetValue(); + new_manifold.mContactNormal = { 0, 0, 0 }; + new_manifold.mFlags |= (uint16)CachedManifold::EFlags::CCDContact; + } + } + else + { + // Already found this contact this physics update. + // Note that we can trigger OnContactPersisted multiple times per physics update, but otherwise we have no way of obtaining the settings + mContactListener->OnContactPersisted(*body1, *body2, *manifold, outSettings); + } + + // If we swapped body1 and body2 we need to swap the mass scales back + if (manifold == &temp) + { + swap(outSettings.mInvMassScale1, outSettings.mInvMassScale2); + swap(outSettings.mInvInertiaScale1, outSettings.mInvInertiaScale2); + // Note we do not need to negate the relative surface velocity as it is not applied by the CCD collision constraint + } + } + + JPH_ASSERT(outSettings.mIsSensor || !(inBody1.IsSensor() || inBody2.IsSensor()), "Sensors cannot be converted into regular bodies by a contact callback!"); +} + +void ContactConstraintManager::SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const +{ + JPH_PROFILE_FUNCTION(); + + QuickSort(inConstraintIdxBegin, inConstraintIdxEnd, [this](uint32 inLHS, uint32 inRHS) { + const ContactConstraint &lhs = mConstraints[inLHS]; + const ContactConstraint &rhs = mConstraints[inRHS]; + + // Most of the time the sort key will be different so we sort on that + if (lhs.mSortKey != rhs.mSortKey) + return lhs.mSortKey < rhs.mSortKey; + + // If they're equal we use the IDs of body 1 to order + if (lhs.mBody1 != rhs.mBody1) + return lhs.mBody1->GetID() < rhs.mBody1->GetID(); + + // If they're still equal we use the IDs of body 2 to order + if (lhs.mBody2 != rhs.mBody2) + return lhs.mBody2->GetID() < rhs.mBody2->GetID(); + + JPH_ASSERT(inLHS == inRHS, "Hash collision, ordering will be inconsistent"); + return false; + }); +} + +void ContactConstraintManager::FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds) +{ + JPH_PROFILE_FUNCTION(); + +#ifdef JPH_ENABLE_ASSERTS + // Mark cache as finalized + ManifoldCache &old_write_cache = mCache[mCacheWriteIdx]; + old_write_cache.Finalize(); + + // Check that the count of body pairs and manifolds that we tracked outside of the cache (to avoid contention on an atomic) is correct + JPH_ASSERT(old_write_cache.GetNumBodyPairs() == inExpectedNumBodyPairs); + JPH_ASSERT(old_write_cache.GetNumManifolds() == inExpectedNumManifolds); +#endif + + // Buffers are now complete, make write buffer the read buffer + mCacheWriteIdx ^= 1; + + // Get the old read cache / new write cache + ManifoldCache &old_read_cache = mCache[mCacheWriteIdx]; + + // Call the contact point removal callbacks + if (mContactListener != nullptr) + old_read_cache.ContactPointRemovedCallbacks(mContactListener); + + // We're done with the old read cache now + old_read_cache.Clear(); + + // Use the amount of contacts from the last iteration to determine the amount of buckets to use in the hash map for the next iteration + old_read_cache.Prepare(inExpectedNumBodyPairs, inExpectedNumManifolds); +} + +bool ContactConstraintManager::WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const +{ + // The body pair needs to be in the cache and it needs to have a manifold (otherwise it's just a record indicating that there are no collisions) + const ManifoldCache &read_cache = mCache[mCacheWriteIdx ^ 1]; + BodyPair key; + if (inBody1ID < inBody2ID) + key = BodyPair(inBody1ID, inBody2ID); + else + key = BodyPair(inBody2ID, inBody1ID); + uint64 key_hash = key.GetHash(); + const BPKeyValue *kv = read_cache.Find(key, key_hash); + return kv != nullptr && kv->GetValue().mFirstCachedManifold != ManifoldMap::cInvalidHandle; +} + +template +JPH_INLINE void ContactConstraintManager::sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio) +{ + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Warm starting: Apply impulse from last frame + if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive()) + { + wcp.mFrictionConstraint1.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, inWarmStartImpulseRatio); + wcp.mFrictionConstraint2.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, inWarmStartImpulseRatio); + } + wcp.mNonPenetrationConstraint.TemplatedWarmStart(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, inWarmStartImpulseRatio); + } +} + +template +void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback) +{ + JPH_PROFILE_FUNCTION(); + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + EMotionType motion_type1 = body1.GetMotionType(); + MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked(); + + Body &body2 = *constraint.mBody2; + EMotionType motion_type2 = body2.GetMotionType(); + MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + // Note: Warm starting doesn't differentiate between kinematic/static bodies so we handle both as static bodies + if (motion_type1 == EMotionType::Dynamic) + { + if (motion_type2 == EMotionType::Dynamic) + { + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties2); + } + else + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties1); + } + else + { + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + + sWarmStartConstraint(constraint, motion_properties1, motion_properties2, inWarmStartImpulseRatio); + + ioCallback(motion_properties2); + } + } +} + +// Specialize for the two body callback types +template void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, CalculateSolverSteps &ioCallback); +template void ContactConstraintManager::WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, DummyCalculateSolverSteps &ioCallback); + +template +JPH_INLINE bool ContactConstraintManager::sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2) +{ + bool any_impulse_applied = false; + + // Calculate tangents + Vec3 t1, t2; + ioConstraint.GetTangents(t1, t2); + + // First apply all friction constraints (non-penetration is more important than friction) + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Check if friction is enabled + if (wcp.mFrictionConstraint1.IsActive() || wcp.mFrictionConstraint2.IsActive()) + { + // Calculate impulse to stop motion in tangential direction + float lambda1 = wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, t1); + float lambda2 = wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintGetTotalLambda(ioMotionProperties1, ioMotionProperties2, t2); + float total_lambda_sq = Square(lambda1) + Square(lambda2); + + // Calculate max impulse that can be applied. Note that we're using the non-penetration impulse from the previous iteration here. + // We do this because non-penetration is more important so is solved last (the last things that are solved in an iterative solver + // contribute the most). + float max_lambda_f = ioConstraint.mCombinedFriction * wcp.mNonPenetrationConstraint.GetTotalLambda(); + + // If the total lambda that we will apply is too large, scale it back + if (total_lambda_sq > Square(max_lambda_f)) + { + float scale = max_lambda_f / sqrt(total_lambda_sq); + lambda1 *= scale; + lambda2 *= scale; + } + + // Apply the friction impulse + if (wcp.mFrictionConstraint1.TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t1, lambda1)) + any_impulse_applied = true; + if (wcp.mFrictionConstraint2.TemplatedSolveVelocityConstraintApplyLambda(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, t2, lambda2)) + any_impulse_applied = true; + } + } + + Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal(); + + // Then apply all non-penetration constraints + for (WorldContactPoint &wcp : ioConstraint.mContactPoints) + { + // Solve non penetration velocities + if (wcp.mNonPenetrationConstraint.TemplatedSolveVelocityConstraint(ioMotionProperties1, ioConstraint.mInvMass1, ioMotionProperties2, ioConstraint.mInvMass2, ws_normal, 0.0f, FLT_MAX)) + any_impulse_applied = true; + } + + return any_impulse_applied; +} + +bool ContactConstraintManager::SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + EMotionType motion_type1 = body1.GetMotionType(); + MotionProperties *motion_properties1 = body1.GetMotionPropertiesUnchecked(); + + Body &body2 = *constraint.mBody2; + EMotionType motion_type2 = body2.GetMotionType(); + MotionProperties *motion_properties2 = body2.GetMotionPropertiesUnchecked(); + + // Dispatch to the correct templated form + switch (motion_type1) + { + case EMotionType::Dynamic: + switch (motion_type2) + { + case EMotionType::Dynamic: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Kinematic: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Static: + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + default: + JPH_ASSERT(false); + break; + } + break; + + case EMotionType::Kinematic: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + case EMotionType::Static: + JPH_ASSERT(motion_type2 == EMotionType::Dynamic); + any_impulse_applied |= sSolveVelocityConstraint(constraint, motion_properties1, motion_properties2); + break; + + default: + JPH_ASSERT(false); + break; + } + } + + return any_impulse_applied; +} + +void ContactConstraintManager::StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const +{ + // Copy back total applied impulse to cache for the next frame + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + const ContactConstraint &constraint = mConstraints[*constraint_idx]; + + for (const WorldContactPoint &wcp : constraint.mContactPoints) + { + wcp.mContactPoint->mNonPenetrationLambda = wcp.mNonPenetrationConstraint.GetTotalLambda(); + wcp.mContactPoint->mFrictionLambda[0] = wcp.mFrictionConstraint1.GetTotalLambda(); + wcp.mContactPoint->mFrictionLambda[1] = wcp.mFrictionConstraint2.GetTotalLambda(); + } + } +} + +bool ContactConstraintManager::SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) +{ + JPH_PROFILE_FUNCTION(); + + bool any_impulse_applied = false; + + for (const uint32 *constraint_idx = inConstraintIdxBegin; constraint_idx < inConstraintIdxEnd; ++constraint_idx) + { + ContactConstraint &constraint = mConstraints[*constraint_idx]; + + // Fetch bodies + Body &body1 = *constraint.mBody1; + Body &body2 = *constraint.mBody2; + + // Get transforms + RMat44 transform1 = body1.GetCenterOfMassTransform(); + RMat44 transform2 = body2.GetCenterOfMassTransform(); + + Vec3 ws_normal = constraint.GetWorldSpaceNormal(); + + for (WorldContactPoint &wcp : constraint.mContactPoints) + { + // Calculate new contact point positions in world space (the bodies may have moved) + RVec3 p1 = transform1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1); + RVec3 p2 = transform2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2); + + // Calculate separation along the normal (negative if interpenetrating) + // Allow a little penetration by default (PhysicsSettings::mPenetrationSlop) to avoid jittering between contact/no-contact which wipes out the contact cache and warm start impulses + // Clamp penetration to a max PhysicsSettings::mMaxPenetrationDistance so that we don't apply a huge impulse if we're penetrating a lot + float separation = max(Vec3(p2 - p1).Dot(ws_normal) + mPhysicsSettings.mPenetrationSlop, -mPhysicsSettings.mMaxPenetrationDistance); + + // Only enforce constraint when separation < 0 (otherwise we're apart) + if (separation < 0.0f) + { + // Update constraint properties (bodies may have moved) + wcp.CalculateNonPenetrationConstraintProperties(body1, constraint.mInvMass1, constraint.mInvInertiaScale1, body2, constraint.mInvMass2, constraint.mInvInertiaScale2, p1, p2, ws_normal); + + // Solve position errors + if (wcp.mNonPenetrationConstraint.SolvePositionConstraintWithMassOverride(body1, constraint.mInvMass1, body2, constraint.mInvMass2, ws_normal, separation, mPhysicsSettings.mBaumgarte)) + any_impulse_applied = true; + } + } + } + + return any_impulse_applied; +} + +void ContactConstraintManager::RecycleConstraintBuffer() +{ + // Reset constraint array + mNumConstraints = 0; +} + +void ContactConstraintManager::FinishConstraintBuffer() +{ + // Free constraints buffer + mUpdateContext->mTempAllocator->Free(mConstraints, mMaxConstraints * sizeof(ContactConstraint)); + mConstraints = nullptr; + mNumConstraints = 0; + + // Reset update context + mUpdateContext = nullptr; +} + +void ContactConstraintManager::SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const +{ + mCache[mCacheWriteIdx ^ 1].SaveState(inStream, inFilter); +} + +bool ContactConstraintManager::RestoreState(StateRecorder &inStream) +{ + bool success = mCache[mCacheWriteIdx].RestoreState(mCache[mCacheWriteIdx ^ 1], inStream); + mCacheWriteIdx ^= 1; + mCache[mCacheWriteIdx].Clear(); + return success; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.h b/WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.h new file mode 100644 index 000000000..cb9d7d80f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/ContactConstraintManager.h @@ -0,0 +1,513 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +struct PhysicsSettings; +class PhysicsUpdateContext; + +class JPH_EXPORT ContactConstraintManager : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit ContactConstraintManager(const PhysicsSettings &inPhysicsSettings); + ~ContactConstraintManager(); + + /// Initialize the system. + /// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching + /// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world) + void Init(uint inMaxBodyPairs, uint inMaxContactConstraints); + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed + void SetContactListener(ContactListener *inListener) { mContactListener = inListener; } + ContactListener * GetContactListener() const { return mContactListener; } + + /// Callback function to combine the restitution or friction of two bodies + /// Note that when merging manifolds (when PhysicsSettings::mUseManifoldReduction is true) you will only get a callback for the merged manifold. + /// It is not possible in that case to get all sub shape ID pairs that were colliding, you'll get the first encountered pair. + using CombineFunction = float (*)(const Body &inBody1, const SubShapeID &inSubShapeID1, const Body &inBody2, const SubShapeID &inSubShapeID2); + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(CombineFunction inCombineFriction) { mCombineFriction = inCombineFriction; } + CombineFunction GetCombineFriction() const { return mCombineFriction; } + + /// Set the function that combines the restitution of two bodies and returns it + /// Default method is max(restitution1, restitution1) + void SetCombineRestitution(CombineFunction inCombineRestitution) { mCombineRestitution = inCombineRestitution; } + CombineFunction GetCombineRestitution() const { return mCombineRestitution; } + + /// Get the max number of contact constraints that are allowed + uint32 GetMaxConstraints() const { return mMaxConstraints; } + + /// Check with the listener if inBody1 and inBody2 could collide, returns false if not + inline ValidateResult ValidateContactPoint(const Body &inBody1, const Body &inBody2, RVec3Arg inBaseOffset, const CollideShapeResult &inCollisionResult) const + { + if (mContactListener == nullptr) + return ValidateResult::AcceptAllContactsForThisBodyPair; + + return mContactListener->OnContactValidate(inBody1, inBody2, inBaseOffset, inCollisionResult); + } + + /// Sets up the constraint buffer. Should be called before starting collision detection. + void PrepareConstraintBuffer(PhysicsUpdateContext *inContext); + + /// Max 4 contact points are needed for a stable manifold + static const int MaxContactPoints = 4; + + /// Contacts are allocated in a lock free hash map + class ContactAllocator : public LFHMAllocatorContext + { + public: + using LFHMAllocatorContext::LFHMAllocatorContext; + + uint mNumBodyPairs = 0; ///< Total number of body pairs added using this allocator + uint mNumManifolds = 0; ///< Total number of manifolds added using this allocator + EPhysicsUpdateError mErrors = EPhysicsUpdateError::None; ///< Errors reported on this allocator + }; + + /// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context. + ContactAllocator GetContactAllocator() { return mCache[mCacheWriteIdx].GetContactAllocator(); } + + /// Check if the contact points from the previous frame are reusable and if so copy them. + /// When the cache was usable and the pair has been handled: outPairHandled = true. + /// When a contact constraint was produced: outConstraintCreated = true. + void GetContactsFromCache(ContactAllocator &ioContactAllocator, Body &inBody1, Body &inBody2, bool &outPairHandled, bool &outConstraintCreated); + + /// Handle used to keep track of the current body pair + using BodyPairHandle = void *; + + /// Create a handle for a colliding body pair so that contact constraints can be added between them. + /// Needs to be called once per body pair per frame before calling AddContactConstraint. + BodyPairHandle AddBodyPair(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2); + + /// Add a contact constraint for this frame. + /// + /// @param ioContactAllocator The allocator that reserves memory for the contacts + /// @param inBodyPair The handle for the contact cache for this body pair + /// @param inBody1 The first body that is colliding + /// @param inBody2 The second body that is colliding + /// @param inManifold The manifold that describes the collision + /// @return true if a contact constraint was created (can be false in the case of a sensor) + /// + /// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2009 (and later years with slight modifications). + /// We're using the formulas from slide 50 - 53 combined. + /// + /// Euler velocity integration: + /// + /// v1' = v1 + M^-1 P + /// + /// Impulse: + /// + /// P = J^T lambda + /// + /// Constraint force: + /// + /// lambda = -K^-1 J v1 + /// + /// Inverse effective mass: + /// + /// K = J M^-1 J^T + /// + /// Constraint equation (limits movement in 1 axis): + /// + /// C = (p2 - p1) . n + /// + /// Jacobian (for position constraint) + /// + /// J = [-n, -r1 x n, n, r2 x n] + /// + /// n = contact normal (pointing away from body 1). + /// p1, p2 = positions of collision on body 1 and 2. + /// r1, r2 = contact point relative to center of mass of body 1 and body 2 (r1 = p1 - x1, r2 = p2 - x2). + /// v1, v2 = (linear velocity, angular velocity): 6 vectors containing linear and angular velocity for body 1 and 2. + /// M = mass matrix, a diagonal matrix of the mass and inertia with diagonal [m1, I1, m2, I2]. + bool AddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPair, Body &inBody1, Body &inBody2, const ContactManifold &inManifold); + + /// Finalizes the contact cache, the contact cache that was generated during the calls to AddContactConstraint in this update + /// will be used from now on to read from. After finalizing the contact cache, the contact removed callbacks will be called. + /// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is + /// used to determine the amount of buckets the contact cache hash map will use in the next update. + void FinalizeContactCacheAndCallContactPointRemovedCallbacks(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds); + + /// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, at least one of the bodies must be active. + /// Uses the read collision cache to determine if 2 bodies are in contact. + bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const; + + /// Get the number of contact constraints that were found + uint32 GetNumConstraints() const { return min(mNumConstraints, mMaxConstraints); } + + /// Sort contact constraints deterministically + void SortContacts(uint32 *inConstraintIdxBegin, uint32 *inConstraintIdxEnd) const; + + /// Get the affected bodies for a given constraint + inline void GetAffectedBodies(uint32 inConstraintIdx, const Body *&outBody1, const Body *&outBody2) const + { + const ContactConstraint &constraint = mConstraints[inConstraintIdx]; + outBody1 = constraint.mBody1; + outBody2 = constraint.mBody2; + } + + /// Apply last frame's impulses as an initial guess for this frame's impulses + template + void WarmStartVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd, float inWarmStartImpulseRatio, MotionPropertiesCallback &ioCallback); + + /// Solve velocity constraints, when almost nothing changes this should only apply very small impulses + /// since we're warm starting with the total impulse applied in the last frame above. + /// + /// Friction wise we're using the Coulomb friction model which says that: + /// + /// |F_T| <= mu |F_N| + /// + /// Where F_T is the tangential force, F_N is the normal force and mu is the friction coefficient + /// + /// In impulse terms this becomes: + /// + /// |lambda_T| <= mu |lambda_N| + /// + /// And the constraint that needs to be applied is exactly the same as a non penetration constraint + /// except that we use a tangent instead of a normal. The tangent should point in the direction of the + /// tangential velocity of the point: + /// + /// J = [-T, -r1 x T, T, r2 x T] + /// + /// Where T is the tangent. + /// + /// See slide 42 and 43. + /// + /// Restitution is implemented as a velocity bias (see slide 41): + /// + /// b = e v_n^- + /// + /// e = the restitution coefficient, v_n^- is the normal velocity prior to the collision + /// + /// Restitution is only applied when v_n^- is large enough and the points are moving towards collision + bool SolveVelocityConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd); + + /// Save back the lambdas to the contact cache for the next warm start + void StoreAppliedImpulses(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd) const; + + /// Solve position constraints. + /// This is using the approach described in 'Modeling and Solving Constraints' by Erin Catto presented at GDC 2007. + /// On slide 78 it is suggested to split up the Baumgarte stabilization for positional drift so that it does not + /// actually add to the momentum. We combine an Euler velocity integrate + a position integrate and then discard the velocity + /// change. + /// + /// Constraint force: + /// + /// lambda = -K^-1 b + /// + /// Baumgarte stabilization: + /// + /// b = beta / dt C + /// + /// beta = baumgarte stabilization factor. + /// dt = delta time. + bool SolvePositionConstraints(const uint32 *inConstraintIdxBegin, const uint32 *inConstraintIdxEnd); + + /// Recycle the constraint buffer. Should be called between collision simulation steps. + void RecycleConstraintBuffer(); + + /// Terminate the constraint buffer. Should be called after simulation ends. + void FinishConstraintBuffer(); + + /// Called by continuous collision detection to notify the contact listener that a contact was added + /// @param ioContactAllocator The allocator that reserves memory for the contacts + /// @param inBody1 The first body that is colliding + /// @param inBody2 The second body that is colliding + /// @param inManifold The manifold that describes the collision + /// @param outSettings The calculated contact settings (may be overridden by the contact listener) + void OnCCDContactAdded(ContactAllocator &ioContactAllocator, const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &outSettings); + +#ifdef JPH_DEBUG_RENDERER + // Drawing properties + static bool sDrawContactPoint; + static bool sDrawSupportingFaces; + static bool sDrawContactPointReduction; + static bool sDrawContactManifolds; +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + + /// Restoring state for replay. Returns false when failed. + bool RestoreState(StateRecorder &inStream); + +private: + /// Local space contact point, used for caching impulses + class CachedContactPoint + { + public: + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Local space positions on body 1 and 2. + /// Note: these values are read through sLoadFloat3Unsafe. + Float3 mPosition1; + Float3 mPosition2; + + /// Total applied impulse during the last update that it was used + float mNonPenetrationLambda; + Vector<2> mFrictionLambda; + }; + + static_assert(sizeof(CachedContactPoint) == 36, "Unexpected size"); + static_assert(alignof(CachedContactPoint) == 4, "Assuming 4 byte aligned"); + + /// A single cached manifold + class CachedManifold + { + public: + /// Calculate size in bytes needed beyond the size of the class to store inNumContactPoints + static int sGetRequiredExtraSize(int inNumContactPoints) { return max(0, inNumContactPoints - 1) * sizeof(CachedContactPoint); } + + /// Calculate total class size needed for storing inNumContactPoints + static int sGetRequiredTotalSize(int inNumContactPoints) { return sizeof(CachedManifold) + sGetRequiredExtraSize(inNumContactPoints); } + + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Handle to next cached contact points in ManifoldCache::mCachedManifolds for the same body pair + uint32 mNextWithSameBodyPair; + + /// Contact normal in the space of 2. + /// Note: this value is read through sLoadFloat3Unsafe. + Float3 mContactNormal; + + /// Flags for this cached manifold + enum class EFlags : uint16 + { + ContactPersisted = 1, ///< If this cache entry was reused in the next simulation update + CCDContact = 2 ///< This is a cached manifold reported by continuous collision detection and was only used to create a contact callback + }; + + /// @see EFlags + mutable atomic mFlags { 0 }; + + /// Number of contact points in the array below + uint16 mNumContactPoints; + + /// Contact points that this manifold consists of + CachedContactPoint mContactPoints[1]; + }; + + static_assert(sizeof(CachedManifold) == 56, "This structure is expect to not contain any waste due to alignment"); + static_assert(alignof(CachedManifold) == 4, "Assuming 4 byte aligned"); + + /// Define a map that maps SubShapeIDPair -> manifold + using ManifoldMap = LockFreeHashMap; + using MKeyValue = ManifoldMap::KeyValue; + using MKVAndCreated = pair; + + /// Start of list of contact points for a particular pair of bodies + class CachedBodyPair + { + public: + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + /// Local space position difference from Body A to Body B. + /// Note: this value is read through sLoadFloat3Unsafe + Float3 mDeltaPosition; + + /// Local space rotation difference from Body A to Body B, fourth component of quaternion is not stored but is guaranteed >= 0. + /// Note: this value is read through sLoadFloat3Unsafe + Float3 mDeltaRotation; + + /// Handle to first manifold in ManifoldCache::mCachedManifolds + uint32 mFirstCachedManifold; + }; + + static_assert(sizeof(CachedBodyPair) == 28, "Unexpected size"); + static_assert(alignof(CachedBodyPair) == 4, "Assuming 4 byte aligned"); + + /// Define a map that maps BodyPair -> CachedBodyPair + using BodyPairMap = LockFreeHashMap; + using BPKeyValue = BodyPairMap::KeyValue; + + /// Holds all caches that are needed to quickly find cached body pairs / manifolds + class ManifoldCache + { + public: + /// Initialize the cache + void Init(uint inMaxBodyPairs, uint inMaxContactConstraints, uint inCachedManifoldsSize); + + /// Reset all entries from the cache + void Clear(); + + /// Prepare cache before creating new contacts. + /// inExpectedNumBodyPairs / inExpectedNumManifolds are the amount of body pairs / manifolds found in the previous step and is used to determine the amount of buckets the contact cache hash map will use. + void Prepare(uint inExpectedNumBodyPairs, uint inExpectedNumManifolds); + + /// Get a new allocator context for storing contacts. Note that you should call this once and then add multiple contacts using the context. + ContactAllocator GetContactAllocator() { return ContactAllocator(mAllocator, cAllocatorBlockSize); } + + /// Find / create cached entry for SubShapeIDPair -> CachedManifold + const MKeyValue * Find(const SubShapeIDPair &inKey, uint64 inKeyHash) const; + MKeyValue * Create(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints); + MKVAndCreated FindOrCreate(ContactAllocator &ioContactAllocator, const SubShapeIDPair &inKey, uint64 inKeyHash, int inNumContactPoints); + uint32 ToHandle(const MKeyValue *inKeyValue) const; + const MKeyValue * FromHandle(uint32 inHandle) const; + + /// Find / create entry for BodyPair -> CachedBodyPair + const BPKeyValue * Find(const BodyPair &inKey, uint64 inKeyHash) const; + BPKeyValue * Create(ContactAllocator &ioContactAllocator, const BodyPair &inKey, uint64 inKeyHash); + void GetAllBodyPairsSorted(Array &outAll) const; + void GetAllManifoldsSorted(const CachedBodyPair &inBodyPair, Array &outAll) const; + void GetAllCCDManifoldsSorted(Array &outAll) const; + void ContactPointRemovedCallbacks(ContactListener *inListener); + +#ifdef JPH_ENABLE_ASSERTS + /// Get the amount of manifolds in the cache + uint GetNumManifolds() const { return mCachedManifolds.GetNumKeyValues(); } + + /// Get the amount of body pairs in the cache + uint GetNumBodyPairs() const { return mCachedBodyPairs.GetNumKeyValues(); } + + /// Before a cache is finalized you can only do Create(), after only Find() or Clear() + void Finalize(); +#endif + + /// Saving / restoring state for replay + void SaveState(StateRecorder &inStream, const StateRecorderFilter *inFilter) const; + bool RestoreState(const ManifoldCache &inReadCache, StateRecorder &inStream); + + private: + /// Block size used when allocating new blocks in the contact cache + static constexpr uint32 cAllocatorBlockSize = 4096; + + /// Allocator used by both mCachedManifolds and mCachedBodyPairs, this makes it more likely that a body pair and its manifolds are close in memory + LFHMAllocator mAllocator; + + /// Simple hash map for SubShapeIDPair -> CachedManifold + ManifoldMap mCachedManifolds { mAllocator }; + + /// Simple hash map for BodyPair -> CachedBodyPair + BodyPairMap mCachedBodyPairs { mAllocator }; + +#ifdef JPH_ENABLE_ASSERTS + bool mIsFinalized = false; ///< Marks if this buffer is complete +#endif + }; + + ManifoldCache mCache[2]; ///< We have one cache to read from and one to write to + int mCacheWriteIdx = 0; ///< Which cache we're currently writing to + + /// World space contact point, used for solving penetrations + class WorldContactPoint + { + public: + /// Calculate constraint properties below + void CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal); + + template + JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution); + + /// The constraint parts + AxisConstraintPart mNonPenetrationConstraint; + AxisConstraintPart mFrictionConstraint1; + AxisConstraintPart mFrictionConstraint2; + + /// Contact cache + CachedContactPoint * mContactPoint; + }; + + using WorldContactPoints = StaticArray; + + /// Contact constraint class, used for solving penetrations + class ContactConstraint + { + public: + #ifdef JPH_DEBUG_RENDERER + /// Draw the state of the contact constraint + void Draw(DebugRenderer *inRenderer, ColorArg inManifoldColor) const; + #endif // JPH_DEBUG_RENDERER + + /// Convert the world space normal to a Vec3 + JPH_INLINE Vec3 GetWorldSpaceNormal() const + { + return Vec3::sLoadFloat3Unsafe(mWorldSpaceNormal); + } + + /// Get the tangents for this contact constraint + JPH_INLINE void GetTangents(Vec3 &outTangent1, Vec3 &outTangent2) const + { + Vec3 ws_normal = GetWorldSpaceNormal(); + outTangent1 = ws_normal.GetNormalizedPerpendicular(); + outTangent2 = ws_normal.Cross(outTangent1); + } + + Body * mBody1; + Body * mBody2; + uint64 mSortKey; + Float3 mWorldSpaceNormal; + float mCombinedFriction; + float mInvMass1; + float mInvInertiaScale1; + float mInvMass2; + float mInvInertiaScale2; + WorldContactPoints mContactPoints; + }; + + /// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2); + + /// Internal helper function to calculate the friction and non-penetration constraint properties. + inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2); + + /// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + bool TemplatedAddContactConstraint(ContactAllocator &ioContactAllocator, BodyPairHandle inBodyPairHandle, Body &inBody1, Body &inBody2, const ContactManifold &inManifold); + + /// Internal helper function to warm start contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE static void sWarmStartConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2, float inWarmStartImpulseRatio); + + /// Internal helper function to solve a single contact constraint. Templated to the motion type to reduce the amount of branches and calculations. + template + JPH_INLINE static bool sSolveVelocityConstraint(ContactConstraint &ioConstraint, MotionProperties *ioMotionProperties1, MotionProperties *ioMotionProperties2); + + /// The main physics settings instance + const PhysicsSettings & mPhysicsSettings; + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed + ContactListener * mContactListener = nullptr; + + /// Functions that are used to combine friction and restitution of 2 bodies + CombineFunction mCombineFriction = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return sqrt(inBody1.GetFriction() * inBody2.GetFriction()); }; + CombineFunction mCombineRestitution = [](const Body &inBody1, const SubShapeID &, const Body &inBody2, const SubShapeID &) { return max(inBody1.GetRestitution(), inBody2.GetRestitution()); }; + + /// The constraints that were added this frame + ContactConstraint * mConstraints = nullptr; + uint32 mMaxConstraints = 0; + atomic mNumConstraints { 0 }; + + /// Context used for this physics update + PhysicsUpdateContext * mUpdateContext; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.cpp new file mode 100644 index 000000000..e70bfb24c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.cpp @@ -0,0 +1,266 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(DistanceConstraintSettings) +{ + JPH_ADD_BASE_CLASS(DistanceConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(DistanceConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMinDistance) + JPH_ADD_ATTRIBUTE(DistanceConstraintSettings, mMaxDistance) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(DistanceConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping") +} + +void DistanceConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mPoint2); + inStream.Write(mMinDistance); + inStream.Write(mMaxDistance); + mLimitsSpringSettings.SaveBinaryState(inStream); +} + +void DistanceConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mPoint2); + inStream.Read(mMinDistance); + inStream.Read(mMaxDistance); + mLimitsSpringSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *DistanceConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new DistanceConstraint(inBody1, inBody2, *this); +} + +DistanceConstraint::DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMinDistance(inSettings.mMinDistance), + mMaxDistance(inSettings.mMaxDistance) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + mWorldSpacePosition1 = inSettings.mPoint1; + mWorldSpacePosition2 = inSettings.mPoint2; + } + else + { + // If properties were specified in local space, we need to calculate world space positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mPoint1; + mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mPoint2; + } + + // Store distance we want to keep between the world space points + float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Length(); + float min_distance, max_distance; + if (mMinDistance < 0.0f && mMaxDistance < 0.0f) + { + min_distance = max_distance = distance; + } + else + { + min_distance = mMinDistance < 0.0f? min(distance, mMaxDistance) : mMinDistance; + max_distance = mMaxDistance < 0.0f? max(distance, mMinDistance) : mMaxDistance; + } + SetDistance(min_distance, max_distance); + + // Most likely gravity is going to tear us apart (this is only used when the distance between the points = 0) + mWorldSpaceNormal = Vec3::sAxisY(); + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void DistanceConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void DistanceConstraint::CalculateConstraintProperties(float inDeltaTime) +{ + // Update world space positions (the bodies may have moved) + mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + + // Calculate world space normal + Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1); + float delta_len = delta.Length(); + if (delta_len > 0.0f) + mWorldSpaceNormal = delta / delta_len; + + // Calculate points relative to body + // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1 + Vec3 r1_plus_u = Vec3(mWorldSpacePosition2 - mBody1->GetCenterOfMassPosition()); + Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition()); + + if (mMinDistance == mMaxDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings); + + // Single distance, allow constraint forces in both directions + mMinLambda = -FLT_MAX; + mMaxLambda = FLT_MAX; + } + else if (delta_len <= mMinDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMinDistance, mLimitsSpringSettings); + + // Allow constraint forces to make distance bigger only + mMinLambda = 0; + mMaxLambda = FLT_MAX; + } + else if (delta_len >= mMaxDistance) + { + mAxisConstraint.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, mWorldSpaceNormal, 0.0f, delta_len - mMaxDistance, mLimitsSpringSettings); + + // Allow constraint forces to make distance smaller only + mMinLambda = -FLT_MAX; + mMaxLambda = 0; + } + else + mAxisConstraint.Deactivate(); +} + +void DistanceConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(inDeltaTime); +} + +void DistanceConstraint::ResetWarmStart() +{ + mAxisConstraint.Deactivate(); +} + +void DistanceConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + mAxisConstraint.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal, inWarmStartImpulseRatio); +} + +bool DistanceConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + if (mAxisConstraint.IsActive()) + return mAxisConstraint.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal, mMinLambda, mMaxLambda); + else + return false; +} + +bool DistanceConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mLimitsSpringSettings.mFrequency <= 0.0f) // When the spring is active, we don't need to solve the position constraint + { + float distance = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1).Dot(mWorldSpaceNormal); + + // Calculate position error + float position_error = 0.0f; + if (distance < mMinDistance) + position_error = distance - mMinDistance; + else if (distance > mMaxDistance) + position_error = distance - mMaxDistance; + + if (position_error != 0.0f) + { + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(inDeltaTime); + + return mAxisConstraint.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal, position_error, inBaumgarte); + } + } + + return false; +} + +#ifdef JPH_DEBUG_RENDERER +void DistanceConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Draw constraint + Vec3 delta = Vec3(mWorldSpacePosition2 - mWorldSpacePosition1); + float len = delta.Length(); + if (len < mMinDistance) + { + RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMinDistance / len : Vec3(0, len, 0)); + inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen); + inRenderer->DrawLine(mWorldSpacePosition2, real_end_pos, Color::sYellow); + } + else if (len > mMaxDistance) + { + RVec3 real_end_pos = mWorldSpacePosition1 + (len > 0.0f? delta * mMaxDistance / len : Vec3(0, len, 0)); + inRenderer->DrawLine(mWorldSpacePosition1, real_end_pos, Color::sGreen); + inRenderer->DrawLine(real_end_pos, mWorldSpacePosition2, Color::sRed); + } + else + inRenderer->DrawLine(mWorldSpacePosition1, mWorldSpacePosition2, Color::sGreen); + + // Draw constraint end points + inRenderer->DrawMarker(mWorldSpacePosition1, Color::sWhite, 0.1f); + inRenderer->DrawMarker(mWorldSpacePosition2, Color::sWhite, 0.1f); + + // Draw current length + inRenderer->DrawText3D(0.5_r * (mWorldSpacePosition1 + mWorldSpacePosition2), StringFormat("%.2f", (double)len)); +} +#endif // JPH_DEBUG_RENDERER + +void DistanceConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mAxisConstraint.SaveState(inStream); + inStream.Write(mWorldSpaceNormal); // When distance = 0, the normal is used from last frame so we need to store it +} + +void DistanceConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mAxisConstraint.RestoreState(inStream); + inStream.Read(mWorldSpaceNormal); +} + +Ref DistanceConstraint::GetConstraintSettings() const +{ + DistanceConstraintSettings *settings = new DistanceConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mMinDistance = mMinDistance; + settings->mMaxDistance = mMaxDistance; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + return settings; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.h b/WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.h new file mode 100644 index 000000000..fc9d35267 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/DistanceConstraint.h @@ -0,0 +1,120 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Distance constraint settings, used to create a distance constraint +class JPH_EXPORT DistanceConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, DistanceConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Constraint will keep mPoint1 (a point on body 1) and mPoint2 (a point on body 2) at the same distance. + /// Note that this constraint can be used as a cheap PointConstraint by setting mPoint1 = mPoint2 (but this removes only 1 degree of freedom instead of 3). + RVec3 mPoint1 = RVec3::sZero(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + + /// Ability to override the distance range at which the two points are kept apart. If the value is negative, it will be replaced by the distance between mPoint1 and mPoint2 (works only if mSpace is world space). + float mMinDistance = -1.0f; + float mMaxDistance = -1.0f; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// This constraint is a stiff spring that holds 2 points at a fixed distance from each other +class JPH_EXPORT DistanceConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct distance constraint + DistanceConstraint(Body &inBody1, Body &inBody2, const DistanceConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Distance; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + /// Update the minimum and maximum distance for the constraint + void SetDistance(float inMinDistance, float inMaxDistance) { JPH_ASSERT(inMinDistance <= inMaxDistance); mMinDistance = inMinDistance; mMaxDistance = inMaxDistance; } + float GetMinDistance() const { return mMinDistance; } + float GetMaxDistance() const { return mMaxDistance; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline float GetTotalLambdaPosition() const { return mAxisConstraint.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Min/max distance that must be kept between the world space points + float mMinDistance; + float mMaxDistance; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // RUN TIME PROPERTIES FOLLOW + + // World space positions and normal + RVec3 mWorldSpacePosition1; + RVec3 mWorldSpacePosition2; + Vec3 mWorldSpaceNormal; + + // Depending on if the distance < min or distance > max we can apply forces to prevent further violations + float mMinLambda; + float mMaxLambda; + + // The constraint part + AxisConstraintPart mAxisConstraint; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/FixedConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/FixedConstraint.cpp new file mode 100644 index 000000000..b06398516 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/FixedConstraint.cpp @@ -0,0 +1,215 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(FixedConstraintSettings) +{ + JPH_ADD_BASE_CLASS(FixedConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(FixedConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAutoDetectPoint) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY1) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisX2) + JPH_ADD_ATTRIBUTE(FixedConstraintSettings, mAxisY2) +} + +void FixedConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mAutoDetectPoint); + inStream.Write(mPoint1); + inStream.Write(mAxisX1); + inStream.Write(mAxisY1); + inStream.Write(mPoint2); + inStream.Write(mAxisX2); + inStream.Write(mAxisY2); +} + +void FixedConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mAutoDetectPoint); + inStream.Read(mPoint1); + inStream.Read(mAxisX1); + inStream.Read(mAxisY1); + inStream.Read(mPoint2); + inStream.Read(mAxisX2); + inStream.Read(mAxisY2); +} + +TwoBodyConstraint *FixedConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new FixedConstraint(inBody1, inBody2, *this); +} + +FixedConstraint::FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mAxisX1, inSettings.mAxisY1, inSettings.mAxisX2, inSettings.mAxisY2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + if (inSettings.mAutoDetectPoint) + { + // Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point + RVec3 anchor; + if (!inBody1.CanBeKinematicOrDynamic()) + anchor = inBody2.GetCenterOfMassPosition(); + else if (!inBody2.CanBeKinematicOrDynamic()) + anchor = inBody1.GetCenterOfMassPosition(); + else + { + // Otherwise use weighted anchor point towards the lightest body + Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real total_inv_mass = inv_m1 + inv_m2; + if (total_inv_mass != 0.0_r) + anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / (inv_m1 + inv_m2); + else + anchor = inBody1.GetCenterOfMassPosition(); + } + + // Store local positions + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * anchor); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * anchor); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + } + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + } +} + +void FixedConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void FixedConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint values that don't change when the bodies don't change position + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); +} + +void FixedConstraint::ResetWarmStart() +{ + mRotationConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); +} + +void FixedConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool FixedConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + return rot || pos; +} + +bool FixedConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Solve rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + + // Solve position constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + return rot || pos; +} + +#ifdef JPH_DEBUG_RENDERER +void FixedConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 com1 = mBody1->GetCenterOfMassTransform(); + RMat44 com2 = mBody2->GetCenterOfMassTransform(); + + RVec3 anchor1 = com1 * mLocalSpacePosition1; + RVec3 anchor2 = com2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawLine(com1.GetTranslation(), anchor1, Color::sGreen); + inRenderer->DrawLine(com2.GetTranslation(), anchor2, Color::sBlue); +} +#endif // JPH_DEBUG_RENDERER + +void FixedConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mRotationConstraintPart.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); +} + +void FixedConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mRotationConstraintPart.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); +} + +Ref FixedConstraint::GetConstraintSettings() const +{ + FixedConstraintSettings *settings = new FixedConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mAxisX1 = Vec3::sAxisX(); + settings->mAxisY1 = Vec3::sAxisY(); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mAxisX2 = mInvInitialOrientation.RotateAxisX(); + settings->mAxisY2 = mInvInitialOrientation.RotateAxisY(); + return settings; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/FixedConstraint.h b/WickedEngine/Jolt/Physics/Constraints/FixedConstraint.h new file mode 100644 index 000000000..16d54d3c6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/FixedConstraint.h @@ -0,0 +1,96 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Fixed constraint settings, used to create a fixed constraint +class JPH_EXPORT FixedConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, FixedConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (they will be fixated in their current relative position/orientation). Set this to false if you want to supply the attachment points yourself. + bool mAutoDetectPoint = false; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mAxisX1 = Vec3::sAxisX(); + Vec3 mAxisY1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mAxisX2 = Vec3::sAxisX(); + Vec3 mAxisY2 = Vec3::sAxisY(); + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A fixed constraint welds two bodies together removing all degrees of freedom between them. +/// This variant uses Euler angles for the rotation constraint. +class JPH_EXPORT FixedConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + FixedConstraint(Body &inBody1, Body &inBody2, const FixedConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Fixed; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mInvInitialOrientation, mLocalSpacePosition2); } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + +private: + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + Quat mInvInitialOrientation; + + // RUN TIME PROPERTIES FOLLOW + + // The constraint parts + RotationEulerConstraintPart mRotationConstraintPart; + PointConstraintPart mPointConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/GearConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/GearConstraint.cpp new file mode 100644 index 000000000..b2a7284c9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/GearConstraint.cpp @@ -0,0 +1,188 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(GearConstraintSettings) +{ + JPH_ADD_BASE_CLASS(GearConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(GearConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis1) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mHingeAxis2) + JPH_ADD_ATTRIBUTE(GearConstraintSettings, mRatio) +} + +void GearConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mHingeAxis1); + inStream.Write(mHingeAxis2); + inStream.Write(mRatio); +} + +void GearConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mHingeAxis1); + inStream.Read(mHingeAxis2); + inStream.Read(mRatio); +} + +TwoBodyConstraint *GearConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new GearConstraint(inBody1, inBody2, *this); +} + +GearConstraint::GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mLocalSpaceHingeAxis1(inSettings.mHingeAxis1), + mLocalSpaceHingeAxis2(inSettings.mHingeAxis2), + mRatio(inSettings.mRatio) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpaceHingeAxis1 = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis1).Normalized(); + mLocalSpaceHingeAxis2 = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis2).Normalized(); + } +} + +void GearConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mWorldSpaceHingeAxis1 = inRotation1 * mLocalSpaceHingeAxis1; + mWorldSpaceHingeAxis2 = inRotation2 * mLocalSpaceHingeAxis2; + + mGearConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio); +} + +void GearConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); +} + +void GearConstraint::ResetWarmStart() +{ + mGearConstraintPart.Deactivate(); +} + +void GearConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mGearConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool GearConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mGearConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis1, *mBody2, mWorldSpaceHingeAxis2, mRatio); +} + +bool GearConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mGear1Constraint == nullptr || mGear2Constraint == nullptr) + return false; + + float gear1rot; + if (mGear1Constraint->GetSubType() == EConstraintSubType::Hinge) + { + gear1rot = StaticCast(mGear1Constraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float gear2rot; + if (mGear2Constraint->GetSubType() == EConstraintSubType::Hinge) + { + gear2rot = StaticCast(mGear2Constraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float error = CenterAngleAroundZero(fmod(gear1rot + mRatio * gear2rot, 2.0f * JPH_PI)); + if (error == 0.0f) + return false; + + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); + return mGearConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void GearConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint axis + inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis1, Color::sGreen, 0.01f); + inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceHingeAxis2, Color::sBlue, 0.01f); +} + +#endif // JPH_DEBUG_RENDERER + +void GearConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mGearConstraintPart.SaveState(inStream); +} + +void GearConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mGearConstraintPart.RestoreState(inStream); +} + +Ref GearConstraint::GetConstraintSettings() const +{ + GearConstraintSettings *settings = new GearConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mHingeAxis1 = mLocalSpaceHingeAxis1; + settings->mHingeAxis2 = mLocalSpaceHingeAxis2; + settings->mRatio = mRatio; + return settings; +} + +Mat44 GearConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis1.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis1.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 GearConstraint::GetConstraintToBody2Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis2.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis2.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/GearConstraint.h b/WickedEngine/Jolt/Physics/Constraints/GearConstraint.h new file mode 100644 index 000000000..8b6233b1a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/GearConstraint.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Gear constraint settings +class JPH_EXPORT GearConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, GearConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Defines the ratio between the rotation of both gears + /// The ratio is defined as: Gear1Rotation(t) = -ratio * Gear2Rotation(t) + /// @param inNumTeethGear1 Number of teeth that body 1 has + /// @param inNumTeethGear2 Number of teeth that body 2 has + void SetRatio(int inNumTeethGear1, int inNumTeethGear2) + { + mRatio = float(inNumTeethGear2) / float(inNumTeethGear1); + } + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + Vec3 mHingeAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + Vec3 mHingeAxis2 = Vec3::sAxisX(); + + /// Ratio between both gears, see SetRatio. + float mRatio = 1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A gear constraint constrains the rotation of body1 to the rotation of body 2 using a gear. +/// Note that this constraint needs to be used in conjunction with a two hinge constraints. +class JPH_EXPORT GearConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct gear constraint + GearConstraint(Body &inBody1, Body &inBody2, const GearConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Gear; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift. + void SetConstraints(const Constraint *inGear1, const Constraint *inGear2) { mGear1Constraint = inGear1; mGear2Constraint = inGear2; } + + ///@name Get Lagrange multiplier from last physics update (the angular impulse applied to satisfy the constraint) + inline float GetTotalLambda() const { return mGearConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space hinge axis for body 1 + Vec3 mLocalSpaceHingeAxis1; + + // Local space hinge axis for body 2 + Vec3 mLocalSpaceHingeAxis2; + + // Ratio between gear 1 and 2 + float mRatio; + + // The constraints that constrain both gears (2 hinges), optional and used to calculate the rotation error and fix numerical drift. + RefConst mGear1Constraint; + RefConst mGear2Constraint; + + // RUN TIME PROPERTIES FOLLOW + + // World space hinge axis for body 1 + Vec3 mWorldSpaceHingeAxis1; + + // World space hinge axis for body 2 + Vec3 mWorldSpaceHingeAxis2; + + // The constraint parts + GearConstraintPart mGearConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/HingeConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/HingeConstraint.cpp new file mode 100644 index 000000000..824657362 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/HingeConstraint.cpp @@ -0,0 +1,424 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(HingeConstraintSettings) +{ + JPH_ADD_BASE_CLASS(HingeConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(HingeConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis1) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mHingeAxis2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mNormalAxis2) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMin) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsMax) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mLimitsSpringSettings) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMaxFrictionTorque) + JPH_ADD_ATTRIBUTE(HingeConstraintSettings, mMotorSettings) +} + +void HingeConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mHingeAxis1); + inStream.Write(mNormalAxis1); + inStream.Write(mPoint2); + inStream.Write(mHingeAxis2); + inStream.Write(mNormalAxis2); + inStream.Write(mLimitsMin); + inStream.Write(mLimitsMax); + inStream.Write(mMaxFrictionTorque); + mLimitsSpringSettings.SaveBinaryState(inStream); + mMotorSettings.SaveBinaryState(inStream); +} + +void HingeConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mHingeAxis1); + inStream.Read(mNormalAxis1); + inStream.Read(mPoint2); + inStream.Read(mHingeAxis2); + inStream.Read(mNormalAxis2); + inStream.Read(mLimitsMin); + inStream.Read(mLimitsMax); + inStream.Read(mMaxFrictionTorque); + mLimitsSpringSettings.RestoreBinaryState(inStream); + mMotorSettings.RestoreBinaryState(inStream);} + +TwoBodyConstraint *HingeConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new HingeConstraint(inBody1, inBody2, *this); +} + +HingeConstraint::HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMaxFrictionTorque(inSettings.mMaxFrictionTorque), + mMotorSettings(inSettings.mMotorSettings) +{ + // Store limits + JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint in this case"); + SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax); + + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXZ(inSettings.mNormalAxis1, inSettings.mHingeAxis1, inSettings.mNormalAxis2, inSettings.mHingeAxis2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpaceHingeAxis1 = inv_transform1.Multiply3x3(inSettings.mHingeAxis1).Normalized(); + mLocalSpaceNormalAxis1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized(); + + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + mLocalSpaceHingeAxis2 = inv_transform2.Multiply3x3(inSettings.mHingeAxis2).Normalized(); + mLocalSpaceNormalAxis2 = inv_transform2.Multiply3x3(inSettings.mNormalAxis2).Normalized(); + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpaceHingeAxis1 = inSettings.mHingeAxis1; + mLocalSpaceNormalAxis1 = inSettings.mNormalAxis1; + + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + mLocalSpaceHingeAxis2 = inSettings.mHingeAxis2; + mLocalSpaceNormalAxis2 = inSettings.mNormalAxis2; + } + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void HingeConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float HingeConstraint::GetCurrentAngle() const +{ + // See: CalculateA1AndTheta + Quat rotation1 = mBody1->GetRotation(); + Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated(); + return diff.GetRotationAngle(rotation1 * mLocalSpaceHingeAxis1); +} + +void HingeConstraint::SetLimits(float inLimitsMin, float inLimitsMax) +{ + JPH_ASSERT(inLimitsMin <= 0.0f && inLimitsMin >= -JPH_PI); + JPH_ASSERT(inLimitsMax >= 0.0f && inLimitsMax <= JPH_PI); + mLimitsMin = inLimitsMin; + mLimitsMax = inLimitsMax; + mHasLimits = mLimitsMin > -JPH_PI && mLimitsMax < JPH_PI; +} + +void HingeConstraint::CalculateA1AndTheta() +{ + if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f) + { + Quat rotation1 = mBody1->GetRotation(); + + // Calculate relative rotation in world space + // + // The rest rotation is: + // + // q2 = q1 r0 + // + // But the actual rotation is + // + // q2 = diff q1 r0 + // <=> diff = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // diff = relative rotation in world space + Quat diff = mBody2->GetRotation() * mInvInitialOrientation * rotation1.Conjugated(); + + // Calculate hinge axis in world space + mA1 = rotation1 * mLocalSpaceHingeAxis1; + + // Get rotation angle around the hinge axis + mTheta = diff.GetRotationAngle(mA1); + } +} + +void HingeConstraint::CalculateRotationLimitsConstraintProperties(float inDeltaTime) +{ + // Apply constraint if outside of limits + if (mHasLimits && (mTheta <= mLimitsMin || mTheta >= mLimitsMax)) + mRotationLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, GetSmallestAngleToLimit(), mLimitsSpringSettings); + else + mRotationLimitsConstraintPart.Deactivate(); +} + +void HingeConstraint::CalculateMotorConstraintProperties(float inDeltaTime) +{ + switch (mMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1); + else + mMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, mA1, -mTargetAngularVelocity); + break; + + case EMotorState::Position: + if (mMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mA1, 0.0f, CenterAngleAroundZero(mTheta - mTargetAngle), mMotorSettings.mSpringSettings); + else + mMotorConstraintPart.Deactivate(); + break; + } +} + +void HingeConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Cache constraint values that are valid until the bodies move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2)); + CalculateA1AndTheta(); + CalculateRotationLimitsConstraintProperties(inDeltaTime); + CalculateMotorConstraintProperties(inDeltaTime); +} + +void HingeConstraint::ResetWarmStart() +{ + mMotorConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + mRotationLimitsConstraintPart.Deactivate(); +} + +void HingeConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mMotorConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mRotationLimitsConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +float HingeConstraint::GetSmallestAngleToLimit() const +{ + float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin); + float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax); + return abs(dist_to_min) < abs(dist_to_max)? dist_to_min : dist_to_max; +} + +bool HingeConstraint::IsMinLimitClosest() const +{ + float dist_to_min = CenterAngleAroundZero(mTheta - mLimitsMin); + float dist_to_max = CenterAngleAroundZero(mTheta - mLimitsMax); + return abs(dist_to_min) < abs(dist_to_max); +} + +bool HingeConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mMotorConstraintPart.IsActive()) + { + switch (mMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionTorque * inDeltaTime; + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, inDeltaTime * mMotorSettings.mMinTorqueLimit, inDeltaTime * mMotorSettings.mMaxTorqueLimit); + break; + } + } + + // Solve point constraint + bool pos = mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve rotation limits + bool limit = false; + if (mRotationLimitsConstraintPart.IsActive()) + { + float min_lambda, max_lambda; + if (mLimitsMin == mLimitsMax) + { + min_lambda = -FLT_MAX; + max_lambda = FLT_MAX; + } + else if (IsMinLimitClosest()) + { + min_lambda = 0.0f; + max_lambda = FLT_MAX; + } + else + { + min_lambda = -FLT_MAX; + max_lambda = 0.0f; + } + limit = mRotationLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mA1, min_lambda, max_lambda); + } + + return motor || pos || rot || limit; +} + +bool HingeConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Motor operates on velocities only, don't call SolvePositionConstraint + + // Solve point constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + bool pos = mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + // Solve rotation constraint + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); // Note that previous call to GetRotation() is out of date since the rotation has changed + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, rotation1.Multiply3x3(mLocalSpaceHingeAxis1), *mBody2, rotation2, rotation2.Multiply3x3(mLocalSpaceHingeAxis2)); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + // Solve rotation limits + bool limit = false; + if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f) + { + CalculateA1AndTheta(); + CalculateRotationLimitsConstraintProperties(inDeltaTime); + if (mRotationLimitsConstraintPart.IsActive()) + limit = mRotationLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, GetSmallestAngleToLimit(), inBaumgarte); + } + + return pos || rot || limit; +} + +#ifdef JPH_DEBUG_RENDERER +void HingeConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint + RVec3 constraint_pos1 = transform1 * mLocalSpacePosition1; + inRenderer->DrawMarker(constraint_pos1, Color::sRed, 0.1f); + inRenderer->DrawLine(constraint_pos1, transform1 * (mLocalSpacePosition1 + mDrawConstraintSize * mLocalSpaceHingeAxis1), Color::sRed); + + RVec3 constraint_pos2 = transform2 * mLocalSpacePosition2; + inRenderer->DrawMarker(constraint_pos2, Color::sGreen, 0.1f); + inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceHingeAxis2), Color::sGreen); + inRenderer->DrawLine(constraint_pos2, transform2 * (mLocalSpacePosition2 + mDrawConstraintSize * mLocalSpaceNormalAxis2), Color::sWhite); +} + +void HingeConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + if (mHasLimits && mLimitsMax > mLimitsMin) + { + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Vec3 hinge_axis1 = transform1.Multiply3x3(mLocalSpaceHingeAxis1); + Vec3 normal_axis1 = transform1.Multiply3x3(mLocalSpaceNormalAxis1); + + inRenderer->DrawPie(position1, mDrawConstraintSize, hinge_axis1, normal_axis1, mLimitsMin, mLimitsMax, Color::sPurple, DebugRenderer::ECastShadow::Off); + } +} +#endif // JPH_DEBUG_RENDERER + +void HingeConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mMotorConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); + mRotationLimitsConstraintPart.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetAngle); +} + +void HingeConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mMotorConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); + mRotationLimitsConstraintPart.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetAngle); +} + + +Ref HingeConstraint::GetConstraintSettings() const +{ + HingeConstraintSettings *settings = new HingeConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mHingeAxis1 = mLocalSpaceHingeAxis1; + settings->mNormalAxis1 = mLocalSpaceNormalAxis1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + settings->mHingeAxis2 = mLocalSpaceHingeAxis2; + settings->mNormalAxis2 = mLocalSpaceNormalAxis2; + settings->mLimitsMin = mLimitsMin; + settings->mLimitsMax = mLimitsMax; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + settings->mMaxFrictionTorque = mMaxFrictionTorque; + settings->mMotorSettings = mMotorSettings; + return settings; +} + +Mat44 HingeConstraint::GetConstraintToBody1Matrix() const +{ + return Mat44(Vec4(mLocalSpaceHingeAxis1, 0), Vec4(mLocalSpaceNormalAxis1, 0), Vec4(mLocalSpaceHingeAxis1.Cross(mLocalSpaceNormalAxis1), 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 HingeConstraint::GetConstraintToBody2Matrix() const +{ + return Mat44(Vec4(mLocalSpaceHingeAxis2, 0), Vec4(mLocalSpaceNormalAxis2, 0), Vec4(mLocalSpaceHingeAxis2.Cross(mLocalSpaceNormalAxis2), 0), Vec4(mLocalSpacePosition2, 1)); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/HingeConstraint.h b/WickedEngine/Jolt/Physics/Constraints/HingeConstraint.h new file mode 100644 index 000000000..edf48d0e0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/HingeConstraint.h @@ -0,0 +1,182 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Hinge constraint settings, used to create a hinge constraint +class JPH_EXPORT HingeConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, HingeConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Hinge axis is the axis where rotation is allowed, normal axis defines the 0 angle of the hinge. + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mHingeAxis1 = Vec3::sAxisY(); + Vec3 mNormalAxis1 = Vec3::sAxisX(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mHingeAxis2 = Vec3::sAxisY(); + Vec3 mNormalAxis2 = Vec3::sAxisX(); + + /// Bodies are assumed to be placed so that the hinge angle = 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-pi, 0] and mLimitsMax e [0, pi]. + /// Both angles are in radians. + float mLimitsMin = -JPH_PI; + float mLimitsMax = JPH_PI; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + + /// Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor + float mMaxFrictionTorque = 0.0f; + + /// In case the constraint is powered, this determines the motor settings around the hinge axis + MotorSettings mMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A hinge constraint constrains 2 bodies on a single point and allows only a single axis of rotation +class JPH_EXPORT HingeConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct hinge constraint + HingeConstraint(Body &inBody1, Body &inBody2, const HingeConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Hinge; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Get the current rotation angle from the rest position + float GetCurrentAngle() const; + + // Friction control + void SetMaxFrictionTorque(float inFrictionTorque) { mMaxFrictionTorque = inFrictionTorque; } + float GetMaxFrictionTorque() const { return mMaxFrictionTorque; } + + // Motor settings + MotorSettings & GetMotorSettings() { return mMotorSettings; } + const MotorSettings & GetMotorSettings() const { return mMotorSettings; } + + // Motor controls + void SetMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; } + EMotorState GetMotorState() const { return mMotorState; } + void SetTargetAngularVelocity(float inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } ///< rad/s + float GetTargetAngularVelocity() const { return mTargetAngularVelocity; } + void SetTargetAngle(float inAngle) { mTargetAngle = mHasLimits? Clamp(inAngle, mLimitsMin, mLimitsMax) : inAngle; } ///< rad + float GetTargetAngle() const { return mTargetAngle; } + + /// Update the rotation limits of the hinge, value in radians (see HingeConstraintSettings) + void SetLimits(float inLimitsMin, float inLimitsMax); + float GetLimitsMin() const { return mLimitsMin; } + float GetLimitsMax() const { return mLimitsMax; } + bool HasLimits() const { return mHasLimits; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline Vector<2> GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaRotationLimits() const { return mRotationLimitsConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mMotorConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateA1AndTheta(); + void CalculateRotationLimitsConstraintProperties(float inDeltaTime); + void CalculateMotorConstraintProperties(float inDeltaTime); + inline float GetSmallestAngleToLimit() const; + inline bool IsMinLimitClosest() const; + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space hinge directions + Vec3 mLocalSpaceHingeAxis1; + Vec3 mLocalSpaceHingeAxis2; + + // Local space normal direction (direction relative to which to draw constraint limits) + Vec3 mLocalSpaceNormalAxis1; + Vec3 mLocalSpaceNormalAxis2; + + // Inverse of initial relative orientation between bodies (which defines hinge angle = 0) + Quat mInvInitialOrientation; + + // Hinge limits + bool mHasLimits; + float mLimitsMin; + float mLimitsMax; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // Friction + float mMaxFrictionTorque; + + // Motor controls + MotorSettings mMotorSettings; + EMotorState mMotorState = EMotorState::Off; + float mTargetAngularVelocity = 0.0f; + float mTargetAngle = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Current rotation around the hinge axis + float mTheta = 0.0f; + + // World space hinge axis for body 1 + Vec3 mA1; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + HingeRotationConstraintPart mRotationConstraintPart; + AngleConstraintPart mRotationLimitsConstraintPart; + AngleConstraintPart mMotorConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/MotorSettings.cpp b/WickedEngine/Jolt/Physics/Constraints/MotorSettings.cpp new file mode 100644 index 000000000..e4daecdd7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/MotorSettings.cpp @@ -0,0 +1,43 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(MotorSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(MotorSettings, mSpringSettings.mDamping, "mDamping") + JPH_ADD_ATTRIBUTE(MotorSettings, mMinForceLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMaxForceLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMinTorqueLimit) + JPH_ADD_ATTRIBUTE(MotorSettings, mMaxTorqueLimit) +} + +void MotorSettings::SaveBinaryState(StreamOut &inStream) const +{ + mSpringSettings.SaveBinaryState(inStream); + inStream.Write(mMinForceLimit); + inStream.Write(mMaxForceLimit); + inStream.Write(mMinTorqueLimit); + inStream.Write(mMaxTorqueLimit); +} + +void MotorSettings::RestoreBinaryState(StreamIn &inStream) +{ + mSpringSettings.RestoreBinaryState(inStream); + inStream.Read(mMinForceLimit); + inStream.Read(mMaxForceLimit); + inStream.Read(mMinTorqueLimit); + inStream.Read(mMaxTorqueLimit); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/MotorSettings.h b/WickedEngine/Jolt/Physics/Constraints/MotorSettings.h new file mode 100644 index 000000000..99484b7d0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/MotorSettings.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +enum class EMotorState +{ + Off, ///< Motor is off + Velocity, ///< Motor will drive to target velocity + Position ///< Motor will drive to target position +}; + +/// Class that contains the settings for a constraint motor. +/// See the main page of the API documentation for more information on how to configure a motor. +class JPH_EXPORT MotorSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, MotorSettings) + + /// Constructor + MotorSettings() = default; + MotorSettings(const MotorSettings &) = default; + MotorSettings & operator = (const MotorSettings &) = default; + MotorSettings(float inFrequency, float inDamping) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping) { JPH_ASSERT(IsValid()); } + MotorSettings(float inFrequency, float inDamping, float inForceLimit, float inTorqueLimit) : mSpringSettings(ESpringMode::FrequencyAndDamping, inFrequency, inDamping), mMinForceLimit(-inForceLimit), mMaxForceLimit(inForceLimit), mMinTorqueLimit(-inTorqueLimit), mMaxTorqueLimit(inTorqueLimit) { JPH_ASSERT(IsValid()); } + + /// Set asymmetric force limits + void SetForceLimits(float inMin, float inMax) { JPH_ASSERT(inMin <= inMax); mMinForceLimit = inMin; mMaxForceLimit = inMax; } + + /// Set asymmetric torque limits + void SetTorqueLimits(float inMin, float inMax) { JPH_ASSERT(inMin <= inMax); mMinTorqueLimit = inMin; mMaxTorqueLimit = inMax; } + + /// Set symmetric force limits + void SetForceLimit(float inLimit) { mMinForceLimit = -inLimit; mMaxForceLimit = inLimit; } + + /// Set symmetric torque limits + void SetTorqueLimit(float inLimit) { mMinTorqueLimit = -inLimit; mMaxTorqueLimit = inLimit; } + + /// Check if settings are valid + bool IsValid() const { return mSpringSettings.mFrequency >= 0.0f && mSpringSettings.mDamping >= 0.0f && mMinForceLimit <= mMaxForceLimit && mMinTorqueLimit <= mMaxTorqueLimit; } + + /// Saves the contents of the motor settings in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores contents from the binary stream inStream. + void RestoreBinaryState(StreamIn &inStream); + + // Settings + SpringSettings mSpringSettings { ESpringMode::FrequencyAndDamping, 2.0f, 1.0f }; ///< Settings for the spring that is used to drive to the position target (not used when motor is a velocity motor). + float mMinForceLimit = -FLT_MAX; ///< Minimum force to apply in case of a linear constraint (N). Usually this is -mMaxForceLimit unless you want a motor that can e.g. push but not pull. Not used when motor is an angular motor. + float mMaxForceLimit = FLT_MAX; ///< Maximum force to apply in case of a linear constraint (N). Not used when motor is an angular motor. + float mMinTorqueLimit = -FLT_MAX; ///< Minimum torque to apply in case of a angular constraint (N m). Usually this is -mMaxTorqueLimit unless you want a motor that can e.g. push but not pull. Not used when motor is a position motor. + float mMaxTorqueLimit = FLT_MAX; ///< Maximum torque to apply in case of a angular constraint (N m). Not used when motor is a position motor. +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PathConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/PathConstraint.cpp new file mode 100644 index 000000000..e5b6eb7da --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PathConstraint.cpp @@ -0,0 +1,458 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PathConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPath) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathPosition) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathRotation) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPathFraction) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mMaxFrictionForce) + JPH_ADD_ATTRIBUTE(PathConstraintSettings, mPositionMotorSettings) + JPH_ADD_ENUM_ATTRIBUTE(PathConstraintSettings, mRotationConstraintType) +} + +void PathConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + mPath->SaveBinaryState(inStream); + inStream.Write(mPathPosition); + inStream.Write(mPathRotation); + inStream.Write(mPathFraction); + inStream.Write(mMaxFrictionForce); + inStream.Write(mRotationConstraintType); + mPositionMotorSettings.SaveBinaryState(inStream); +} + +void PathConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + PathConstraintPath::PathResult result = PathConstraintPath::sRestoreFromBinaryState(inStream); + if (!result.HasError()) + mPath = result.Get(); + inStream.Read(mPathPosition); + inStream.Read(mPathRotation); + inStream.Read(mPathFraction); + inStream.Read(mMaxFrictionForce); + inStream.Read(mRotationConstraintType); + mPositionMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *PathConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PathConstraint(inBody1, inBody2, *this); +} + +PathConstraint::PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mRotationConstraintType(inSettings.mRotationConstraintType), + mMaxFrictionForce(inSettings.mMaxFrictionForce), + mPositionMotorSettings(inSettings.mPositionMotorSettings) +{ + // Calculate transform that takes us from the path start to center of mass space of body 1 + mPathToBody1 = Mat44::sRotationTranslation(inSettings.mPathRotation, inSettings.mPathPosition - inBody1.GetShape()->GetCenterOfMass()); + + SetPath(inSettings.mPath, inSettings.mPathFraction); +} + +void PathConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mPathToBody1.SetTranslation(mPathToBody1.GetTranslation() - inDeltaCOM); + else if (mBody2->GetID() == inBodyID) + mPathToBody2.SetTranslation(mPathToBody2.GetTranslation() - inDeltaCOM); +} + +void PathConstraint::SetPath(const PathConstraintPath *inPath, float inPathFraction) +{ + mPath = inPath; + mPathFraction = inPathFraction; + + if (mPath != nullptr) + { + // Get the point on the path for this fraction + Vec3 path_point, path_tangent, path_normal, path_binormal; + mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal); + + // Construct the matrix that takes us from the closest point on the path to body 2 center of mass space + Mat44 closest_point_to_path(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4(path_point, 1)); + Mat44 cp_to_body1 = mPathToBody1 * closest_point_to_path; + mPathToBody2 = (mBody2->GetInverseCenterOfMassTransform() * mBody1->GetCenterOfMassTransform()).ToMat44() * cp_to_body1; + + // Calculate initial orientation + if (mRotationConstraintType == EPathRotationConstraintType::FullyConstrained) + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientation(*mBody1, *mBody2); + } +} + +void PathConstraint::CalculateConstraintProperties(float inDeltaTime) +{ + // Get transforms of body 1 and 2 + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Get the transform of the path transform as seen from body 1 in world space + RMat44 path_to_world_1 = transform1 * mPathToBody1; + + // Get the transform of from the point on path that body 2 is attached to in world space + RMat44 path_to_world_2 = transform2 * mPathToBody2; + + // Calculate new closest point on path + RVec3 position2 = path_to_world_2.GetTranslation(); + Vec3 position2_local_to_path = Vec3(path_to_world_1.InversedRotationTranslation() * position2); + mPathFraction = mPath->GetClosestPoint(position2_local_to_path, mPathFraction); + + // Get the point on the path for this fraction + Vec3 path_point, path_tangent, path_normal, path_binormal; + mPath->GetPointOnPath(mPathFraction, path_point, path_tangent, path_normal, path_binormal); + + // Calculate R1 and R2 + RVec3 path_point_ws = path_to_world_1 * path_point; + mR1 = Vec3(path_point_ws - mBody1->GetCenterOfMassPosition()); + mR2 = Vec3(position2 - mBody2->GetCenterOfMassPosition()); + + // Calculate U = X2 + R2 - X1 - R1 + mU = Vec3(position2 - path_point_ws); + + // Calculate world space normals + mPathNormal = path_to_world_1.Multiply3x3(path_normal); + mPathBinormal = path_to_world_1.Multiply3x3(path_binormal); + + // Calculate slide axis + mPathTangent = path_to_world_1.Multiply3x3(path_tangent); + + // Prepare constraint part for position constraint to slide along the path + mPositionConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mR1 + mU, *mBody2, transform2.GetRotation(), mR2, mPathNormal, mPathBinormal); + + // Check if closest point is on the boundary of the path and if so apply limit + if (!mPath->IsLooping() && (mPathFraction <= 0.0f || mPathFraction >= mPath->GetPathMaxFraction())) + mPositionLimitsConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent); + else + mPositionLimitsConstraintPart.Deactivate(); + + // Prepare rotation constraint part + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathTangent, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisX()); + break; + + case EPathRotationConstraintType::ConstrainAroundNormal: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathNormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisZ()); + break; + + case EPathRotationConstraintType::ConstrainAroundBinormal: + mHingeConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), mPathBinormal, *mBody2, transform2.GetRotation(), path_to_world_2.GetAxisY()); + break; + + case EPathRotationConstraintType::ConstrainToPath: + // We need to calculate the inverse of the rotation from body 1 to body 2 for the current path position (see: RotationEulerConstraintPart::sGetInvInitialOrientation) + // RotationBody2 = RotationBody1 * InitialOrientation <=> InitialOrientation^-1 = RotationBody2^-1 * RotationBody1 + // We can express RotationBody2 in terms of RotationBody1: RotationBody2 = RotationBody1 * PathToBody1 * RotationClosestPointOnPath * PathToBody2^-1 + // Combining these two: InitialOrientation^-1 = PathToBody2 * (PathToBody1 * RotationClosestPointOnPath)^-1 + mInvInitialOrientation = mPathToBody2.Multiply3x3RightTransposed(mPathToBody1.Multiply3x3(Mat44(Vec4(path_tangent, 0), Vec4(path_binormal, 0), Vec4(path_normal, 0), Vec4::sZero()))).GetQuaternion(); + [[fallthrough]]; + + case EPathRotationConstraintType::FullyConstrained: + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, transform1.GetRotation(), *mBody2, transform2.GetRotation()); + break; + } + + // Motor properties + switch (mPositionMotorState) + { + case EMotorState::Off: + if (mMaxFrictionForce > 0.0f) + mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent); + else + mPositionMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mPositionMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, -mTargetVelocity); + break; + + case EMotorState::Position: + if (mPositionMotorSettings.mSpringSettings.HasStiffness()) + { + // Calculate constraint value to drive to + float c; + if (mPath->IsLooping()) + { + float max_fraction = mPath->GetPathMaxFraction(); + c = fmod(mPathFraction - mTargetPathFraction, max_fraction); + float half_max_fraction = 0.5f * max_fraction; + if (c > half_max_fraction) + c -= max_fraction; + else if (c < -half_max_fraction) + c += max_fraction; + } + else + c = mPathFraction - mTargetPathFraction; + mPositionMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mPathTangent, 0.0f, c, mPositionMotorSettings.mSpringSettings); + } + else + mPositionMotorConstraintPart.Deactivate(); + break; + } +} + +void PathConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(inDeltaTime); +} + +void PathConstraint::ResetWarmStart() +{ + mPositionMotorConstraintPart.Deactivate(); + mPositionConstraintPart.Deactivate(); + mPositionLimitsConstraintPart.Deactivate(); + mHingeConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); +} + +void PathConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPositionMotorConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio); + mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mPathNormal, mPathBinormal, inWarmStartImpulseRatio); + mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mPathTangent, inWarmStartImpulseRatio); + + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + mHingeConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + break; + } +} + +bool PathConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mPositionMotorConstraintPart.IsActive()) + { + switch (mPositionMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionForce * inDeltaTime; + motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mPositionMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, inDeltaTime * mPositionMotorSettings.mMinForceLimit, inDeltaTime * mPositionMotorSettings.mMaxForceLimit); + break; + } + } + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathNormal, mPathBinormal); + + // Solve limits along path axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mPathFraction <= 0.0f) + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, 0, FLT_MAX); + else + { + JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction()); + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mPathTangent, -FLT_MAX, 0); + } + } + + // Solve rotational constraint + // Note, this is not entirely correct, we should apply a velocity constraint so that the body will actually follow the path + // by looking at the derivative of the tangent, normal or binormal but we don't. This means the position constraint solver + // will need to correct the orientation error that builds up, which in turn means that the simulation is not physically correct. + bool rot = false; + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + rot = mHingeConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + break; + } + + return motor || pos || limit || rot; +} + +bool PathConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(inDeltaTime); + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mPathNormal, mPathBinormal, inBaumgarte); + + // Solve limits along path axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mPathFraction <= 0.0f) + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte); + else + { + JPH_ASSERT(mPathFraction >= mPath->GetPathMaxFraction()); + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mPathTangent, mU.Dot(mPathTangent), inBaumgarte); + } + } + + // Solve rotational constraint + bool rot = false; + switch (mRotationConstraintType) + { + case EPathRotationConstraintType::Free: + // No rotational limits + break; + + case EPathRotationConstraintType::ConstrainAroundTangent: + case EPathRotationConstraintType::ConstrainAroundNormal: + case EPathRotationConstraintType::ConstrainAroundBinormal: + rot = mHingeConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + break; + + case EPathRotationConstraintType::ConstrainToPath: + case EPathRotationConstraintType::FullyConstrained: + rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + break; + } + + return pos || limit || rot; +} + +#ifdef JPH_DEBUG_RENDERER +void PathConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + if (mPath != nullptr) + { + // Draw the path in world space + RMat44 path_to_world = mBody1->GetCenterOfMassTransform() * mPathToBody1; + mPath->DrawPath(inRenderer, path_to_world); + + // Draw anchor point of both bodies in world space + RVec3 x1 = mBody1->GetCenterOfMassPosition() + mR1; + RVec3 x2 = mBody2->GetCenterOfMassPosition() + mR2; + inRenderer->DrawMarker(x1, Color::sYellow, 0.1f); + inRenderer->DrawMarker(x2, Color::sYellow, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathTangent, Color::sBlue, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathNormal, Color::sRed, 0.1f); + inRenderer->DrawArrow(x1, x1 + mPathBinormal, Color::sGreen, 0.1f); + inRenderer->DrawText3D(x1, StringFormat("%.1f", (double)mPathFraction)); + + // Draw motor + switch (mPositionMotorState) + { + case EMotorState::Position: + { + // Draw target marker + Vec3 position, tangent, normal, binormal; + mPath->GetPointOnPath(mTargetPathFraction, position, tangent, normal, binormal); + inRenderer->DrawMarker(path_to_world * position, Color::sYellow, 1.0f); + break; + } + + case EMotorState::Velocity: + { + RVec3 position = mBody2->GetCenterOfMassPosition() + mR2; + inRenderer->DrawArrow(position, position + mPathTangent * mTargetVelocity, Color::sRed, 0.1f); + break; + } + + case EMotorState::Off: + break; + } + } +} +#endif // JPH_DEBUG_RENDERER + +void PathConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPositionConstraintPart.SaveState(inStream); + mPositionLimitsConstraintPart.SaveState(inStream); + mPositionMotorConstraintPart.SaveState(inStream); + mHingeConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + + inStream.Write(mMaxFrictionForce); + inStream.Write(mPositionMotorSettings); + inStream.Write(mPositionMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetPathFraction); + inStream.Write(mPathFraction); +} + +void PathConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPositionConstraintPart.RestoreState(inStream); + mPositionLimitsConstraintPart.RestoreState(inStream); + mPositionMotorConstraintPart.RestoreState(inStream); + mHingeConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + + inStream.Read(mMaxFrictionForce); + inStream.Read(mPositionMotorSettings); + inStream.Read(mPositionMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetPathFraction); + inStream.Read(mPathFraction); +} + +Ref PathConstraint::GetConstraintSettings() const +{ + JPH_ASSERT(false); // Not implemented yet + return nullptr; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PathConstraint.h b/WickedEngine/Jolt/Physics/Constraints/PathConstraint.h new file mode 100644 index 000000000..6301b8523 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PathConstraint.h @@ -0,0 +1,186 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// How to constrain the rotation of the body to a PathConstraint +enum class EPathRotationConstraintType +{ + Free, ///< Do not constrain the rotation of the body at all + ConstrainAroundTangent, ///< Only allow rotation around the tangent vector (following the path) + ConstrainAroundNormal, ///< Only allow rotation around the normal vector (perpendicular to the path) + ConstrainAroundBinormal, ///< Only allow rotation around the binormal vector (perpendicular to the path) + ConstrainToPath, ///< Fully constrain the rotation of body 2 to the path (following the tangent and normal of the path) + FullyConstrained, ///< Fully constrain the rotation of the body 2 to the rotation of body 1 +}; + +/// Path constraint settings, used to constrain the degrees of freedom between two bodies to a path +/// +/// The requirements of the path are that: +/// * Tangent, normal and bi-normal form an orthonormal basis with: tangent cross bi-normal = normal +/// * The path points along the tangent vector +/// * The path is continuous so doesn't contain any sharp corners +/// +/// The reason for all this is that the constraint acts like a slider constraint with the sliding axis being the tangent vector (the assumption here is that delta time will be small enough so that the path is linear for that delta time). +class JPH_EXPORT PathConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// The path that constrains the two bodies + RefConst mPath; + + /// The position of the path start relative to world transform of body 1 + Vec3 mPathPosition = Vec3::sZero(); + + /// The rotation of the path start relative to world transform of body 1 + Quat mPathRotation = Quat::sIdentity(); + + /// The fraction along the path that corresponds to the initial position of body 2. Usually this is 0, the beginning of the path. But if you want to start an object halfway the path you can calculate this with mPath->GetClosestPoint(point on path to attach body to). + float mPathFraction = 0.0f; + + /// Maximum amount of friction force to apply (N) when not driven by a motor. + float mMaxFrictionForce = 0.0f; + + /// In case the constraint is powered, this determines the motor settings along the path + MotorSettings mPositionMotorSettings; + + /// How to constrain the rotation of the body to the path + EPathRotationConstraintType mRotationConstraintType = EPathRotationConstraintType::Free; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// Path constraint, used to constrain the degrees of freedom between two bodies to a path +class JPH_EXPORT PathConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct point constraint + PathConstraint(Body &inBody1, Body &inBody2, const PathConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Path; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual bool IsActive() const override { return TwoBodyConstraint::IsActive() && mPath != nullptr; } + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return mPathToBody1; } + virtual Mat44 GetConstraintToBody2Matrix() const override { return mPathToBody2; } + + /// Update the path for this constraint + void SetPath(const PathConstraintPath *inPath, float inPathFraction); + + /// Access to the current path + const PathConstraintPath * GetPath() const { return mPath; } + + /// Access to the current fraction along the path e [0, GetPath()->GetMaxPathFraction()] + float GetPathFraction() const { return mPathFraction; } + + /// Friction control + void SetMaxFrictionForce(float inFrictionForce) { mMaxFrictionForce = inFrictionForce; } + float GetMaxFrictionForce() const { return mMaxFrictionForce; } + + /// Position motor settings + MotorSettings & GetPositionMotorSettings() { return mPositionMotorSettings; } + const MotorSettings & GetPositionMotorSettings() const { return mPositionMotorSettings; } + + // Position motor controls (drives body 2 along the path) + void SetPositionMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mPositionMotorSettings.IsValid()); mPositionMotorState = inState; } + EMotorState GetPositionMotorState() const { return mPositionMotorState; } + void SetTargetVelocity(float inVelocity) { mTargetVelocity = inVelocity; } + float GetTargetVelocity() const { return mTargetVelocity; } + void SetTargetPathFraction(float inFraction) { JPH_ASSERT(mPath->IsLooping() || (inFraction >= 0.0f && inFraction <= mPath->GetPathMaxFraction())); mTargetPathFraction = inFraction; } + float GetTargetPathFraction() const { return mTargetPathFraction; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vector<2> GetTotalLambdaPosition() const { return mPositionConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaPositionLimits() const { return mPositionLimitsConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mPositionMotorConstraintPart.GetTotalLambda(); } + inline Vector<2> GetTotalLambdaRotationHinge() const { return mHingeConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + RefConst mPath; ///< The path that attaches the two bodies + Mat44 mPathToBody1; ///< Transform that takes a quantity from path space to body 1 center of mass space + Mat44 mPathToBody2; ///< Transform that takes a quantity from path space to body 2 center of mass space + EPathRotationConstraintType mRotationConstraintType; ///< How to constrain the rotation of the path + + // Friction + float mMaxFrictionForce; + + // Motor controls + MotorSettings mPositionMotorSettings; + EMotorState mPositionMotorState = EMotorState::Off; + float mTargetVelocity = 0.0f; + float mTargetPathFraction = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Positions where the point constraint acts on in world space + Vec3 mR1; + Vec3 mR2; + + // X2 + R2 - X1 - R1 + Vec3 mU; + + // World space path tangent + Vec3 mPathTangent; + + // Normals to the path tangent + Vec3 mPathNormal; + Vec3 mPathBinormal; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space (only used when rotation constraint type is FullyConstrained) + Quat mInvInitialOrientation; + + // Current fraction along the path where body 2 is attached + float mPathFraction = 0.0f; + + // Translation constraint parts + DualAxisConstraintPart mPositionConstraintPart; ///< Constraint part that keeps the movement along the tangent of the path + AxisConstraintPart mPositionLimitsConstraintPart; ///< Constraint part that prevents movement beyond the beginning and end of the path + AxisConstraintPart mPositionMotorConstraintPart; ///< Constraint to drive the object along the path or to apply friction + + // Rotation constraint parts + HingeRotationConstraintPart mHingeConstraintPart; ///< Constraint part that removes 2 degrees of rotation freedom + RotationEulerConstraintPart mRotationConstraintPart; ///< Constraint part that removes all rotational freedom +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.cpp b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.cpp new file mode 100644 index 000000000..69c0a82f3 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.cpp @@ -0,0 +1,85 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(PathConstraintPath) +{ + JPH_ADD_BASE_CLASS(PathConstraintPath, SerializableObject) +} + +#ifdef JPH_DEBUG_RENDERER +// Helper function to transform the results of GetPointOnPath to world space +static inline void sTransformPathPoint(RMat44Arg inTransform, Vec3Arg inPosition, RVec3 &outPosition, Vec3 &ioNormal, Vec3 &ioBinormal) +{ + outPosition = inTransform * inPosition; + ioNormal = inTransform.Multiply3x3(ioNormal); + ioBinormal = inTransform.Multiply3x3(ioBinormal); +} + +// Helper function to draw a path segment +static inline void sDrawPathSegment(DebugRenderer *inRenderer, RVec3Arg inPrevPosition, RVec3Arg inPosition, Vec3Arg inNormal, Vec3Arg inBinormal) +{ + inRenderer->DrawLine(inPrevPosition, inPosition, Color::sWhite); + inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inNormal, Color::sRed, 0.02f); + inRenderer->DrawArrow(inPosition, inPosition + 0.1f * inBinormal, Color::sGreen, 0.02f); +} + +void PathConstraintPath::DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const +{ + // Calculate first point + Vec3 lfirst_pos, first_tangent, first_normal, first_binormal; + GetPointOnPath(0.0f, lfirst_pos, first_tangent, first_normal, first_binormal); + RVec3 first_pos; + sTransformPathPoint(inBaseTransform, lfirst_pos, first_pos, first_normal, first_binormal); + + float t_max = GetPathMaxFraction(); + + // Draw the segments + RVec3 prev_pos = first_pos; + for (float t = 0.1f; t < t_max; t += 0.1f) + { + Vec3 lpos, tangent, normal, binormal; + GetPointOnPath(t, lpos, tangent, normal, binormal); + RVec3 pos; + sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal); + sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal); + prev_pos = pos; + } + + // Draw last point + Vec3 lpos, tangent, normal, binormal; + GetPointOnPath(t_max, lpos, tangent, normal, binormal); + RVec3 pos; + sTransformPathPoint(inBaseTransform, lpos, pos, normal, binormal); + sDrawPathSegment(inRenderer, prev_pos, pos, normal, binormal); +} +#endif // JPH_DEBUG_RENDERER + +void PathConstraintPath::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(GetRTTI()->GetHash()); + inStream.Write(mIsLooping); +} + +void PathConstraintPath::RestoreBinaryState(StreamIn &inStream) +{ + // Type hash read by sRestoreFromBinaryState + inStream.Read(mIsLooping); +} + +PathConstraintPath::PathResult PathConstraintPath::sRestoreFromBinaryState(StreamIn &inStream) +{ + return StreamUtils::RestoreObject(inStream, &PathConstraintPath::RestoreBinaryState); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.h b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.h new file mode 100644 index 000000000..06f10408a --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPath.h @@ -0,0 +1,71 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// The path for a path constraint. It allows attaching two bodies to each other while giving the second body the freedom to move along a path relative to the first. +class JPH_EXPORT PathConstraintPath : public SerializableObject, public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, PathConstraintPath) + + using PathResult = Result>; + + /// Virtual destructor to ensure that derived types get their destructors called + virtual ~PathConstraintPath() override = default; + + /// Gets the max fraction along the path. I.e. sort of the length of the path. + virtual float GetPathMaxFraction() const = 0; + + /// Get the globally closest point on the curve (Could be slow!) + /// @param inPosition Position to find closest point for + /// @param inFractionHint Last known fraction along the path (can be used to speed up the search) + /// @return Fraction of closest point along the path + virtual float GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const = 0; + + /// Given the fraction along the path, get the point, tangent and normal. + /// @param inFraction Fraction along the path [0, GetPathMaxFraction()]. + /// @param outPathPosition Returns the closest position to inSearchPosition on the path. + /// @param outPathTangent Returns the tangent to the path at outPathPosition (the vector that follows the direction of the path) + /// @param outPathNormal Return the normal to the path at outPathPosition (a vector that's perpendicular to outPathTangent) + /// @param outPathBinormal Returns the binormal to the path at outPathPosition (a vector so that normal cross tangent = binormal) + virtual void GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const = 0; + + /// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points. + void SetIsLooping(bool inIsLooping) { mIsLooping = inIsLooping; } + bool IsLooping() const { return mIsLooping; } + +#ifdef JPH_DEBUG_RENDERER + /// Draw the path relative to inBaseTransform. Used for debug purposes. + void DrawPath(DebugRenderer *inRenderer, RMat44Arg inBaseTransform) const; +#endif // JPH_DEBUG_RENDERER + + /// Saves the contents of the path in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Creates a Shape of the correct type and restores its contents from the binary stream inStream. + static PathResult sRestoreFromBinaryState(StreamIn &inStream); + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream); + +private: + /// If the path is looping or not. If a path is looping, the first and last point are automatically connected to each other. They should not be the same points. + bool mIsLooping = false; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp new file mode 100644 index 000000000..55c1f33ad --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.cpp @@ -0,0 +1,308 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PathConstraintPathHermite::Point) +{ + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mPosition) + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mTangent) + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite::Point, mNormal) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PathConstraintPathHermite) +{ + JPH_ADD_BASE_CLASS(PathConstraintPathHermite, PathConstraintPath) + + JPH_ADD_ATTRIBUTE(PathConstraintPathHermite, mPoints) +} + +// Calculate position and tangent for a Cubic Hermite Spline segment +static inline void sCalculatePositionAndTangent(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inT, Vec3 &outPosition, Vec3 &outTangent) +{ + // Calculate factors for Cubic Hermite Spline + // See: https://en.wikipedia.org/wiki/Cubic_Hermite_spline + float t2 = inT * inT; + float t3 = inT * t2; + float h00 = 2.0f * t3 - 3.0f * t2 + 1.0f; + float h10 = t3 - 2.0f * t2 + inT; + float h01 = -2.0f * t3 + 3.0f * t2; + float h11 = t3 - t2; + + // Calculate d/dt for factors to calculate the tangent + float ddt_h00 = 6.0f * (t2 - inT); + float ddt_h10 = 3.0f * t2 - 4.0f * inT + 1.0f; + float ddt_h01 = -ddt_h00; + float ddt_h11 = 3.0f * t2 - 2.0f * inT; + + outPosition = h00 * inP1 + h10 * inM1 + h01 * inP2 + h11 * inM2; + outTangent = ddt_h00 * inP1 + ddt_h10 * inM1 + ddt_h01 * inP2 + ddt_h11 * inM2; +} + +// Calculate the closest point to the origin for a Cubic Hermite Spline segment +// This is used to get an estimate for the interval in which the closest point can be found, +// the interval [0, 1] is too big for Newton Raphson to work on because it is solving a 5th degree polynomial which may +// have multiple local minima that are not the root. This happens especially when the path is straight (tangents aligned with inP2 - inP1). +// Based on the bisection method: https://en.wikipedia.org/wiki/Bisection_method +static inline void sCalculateClosestPointThroughBisection(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float &outTMin, float &outTMax) +{ + outTMin = 0.0f; + outTMax = 1.0f; + + // To get the closest point of the curve to the origin we need to solve: + // d/dt P(t) . P(t) = 0 for t, where P(t) is the point on the curve segment + // Using d/dt (a(t) . b(t)) = d/dt a(t) . b(t) + a(t) . d/dt b(t) + // See: https://proofwiki.org/wiki/Derivative_of_Dot_Product_of_Vector-Valued_Functions + // d/dt P(t) . P(t) = 2 P(t) d/dt P(t) = 2 P(t) . Tangent(t) + + // Calculate the derivative at t = 0, we know P(0) = inP1 and Tangent(0) = inM1 + float ddt_min = inP1.Dot(inM1); // Leaving out factor 2, we're only interested in the root + if (abs(ddt_min) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMax = 0.0f; + return; + } + bool ddt_min_negative = ddt_min < 0.0f; + + // Calculate derivative at t = 1, we know P(1) = inP2 and Tangent(1) = inM2 + float ddt_max = inP2.Dot(inM2); + if (abs(ddt_max) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMin = 1.0f; + return; + } + bool ddt_max_negative = ddt_max < 0.0f; + + // If the signs of the derivative are not different, this algorithm can't find the root + if (ddt_min_negative == ddt_max_negative) + return; + + // With 4 iterations we'll get a result accurate to 1 / 2^4 = 0.0625 + for (int iteration = 0; iteration < 4; ++iteration) + { + float t_mid = 0.5f * (outTMin + outTMax); + Vec3 position, tangent; + sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t_mid, position, tangent); + float ddt_mid = position.Dot(tangent); + if (abs(ddt_mid) < 1.0e-6f) + { + // Derivative is near zero, we found our root + outTMin = outTMax = t_mid; + return; + } + bool ddt_mid_negative = ddt_mid < 0.0f; + + // Update the search interval so that the signs of the derivative at both ends of the interval are still different + if (ddt_mid_negative == ddt_min_negative) + outTMin = t_mid; + else + outTMax = t_mid; + } +} + +// Calculate the closest point to the origin for a Cubic Hermite Spline segment +// Only considers the range t e [inTMin, inTMax] and will stop as soon as the closest point falls outside of that range +static inline float sCalculateClosestPointThroughNewtonRaphson(Vec3Arg inP1, Vec3Arg inM1, Vec3Arg inP2, Vec3Arg inM2, float inTMin, float inTMax, float &outDistanceSq) +{ + // This is the closest position on the curve to the origin that we found + Vec3 position; + + // Calculate the size of the interval + float interval = inTMax - inTMin; + + // Start in the middle of the interval + float t = 0.5f * (inTMin + inTMax); + + // Do max 10 iterations to prevent taking too much CPU time + for (int iteration = 0; iteration < 10; ++iteration) + { + // Calculate derivative at t, see comment at sCalculateClosestPointThroughBisection for derivation of the equations + Vec3 tangent; + sCalculatePositionAndTangent(inP1, inM1, inP2, inM2, t, position, tangent); + float ddt = position.Dot(tangent); // Leaving out factor 2, we're only interested in the root + + // Calculate derivative of ddt: d^2/dt P(t) . P(t) = d/dt (2 P(t) . Tangent(t)) + // = 2 (d/dt P(t)) . Tangent(t) + P(t) . d/dt Tangent(t)) = 2 (Tangent(t) . Tangent(t) + P(t) . d/dt Tangent(t)) + float d2dt_h00 = 12.0f * t - 6.0f; + float d2dt_h10 = 6.0f * t - 4.0f; + float d2dt_h01 = -d2dt_h00; + float d2dt_h11 = 6.0f * t - 2.0f; + Vec3 ddt_tangent = d2dt_h00 * inP1 + d2dt_h10 * inM1 + d2dt_h01 * inP2 + d2dt_h11 * inM2; + float d2dt = tangent.Dot(tangent) + position.Dot(ddt_tangent); // Leaving out factor 2, because we left it out above too + + // If d2dt is zero, the curve is flat and there are multiple t's for which we are closest to the origin, stop now + if (d2dt == 0.0f) + break; + + // Do a Newton Raphson step + // See: https://en.wikipedia.org/wiki/Newton%27s_method + // Clamp against [-interval, interval] to avoid overshooting too much, we're not interested outside the interval + float delta = Clamp(-ddt / d2dt, -interval, interval); + + // If we're stepping away further from t e [inTMin, inTMax] stop now + if ((t > inTMax && delta > 0.0f) || (t < inTMin && delta < 0.0f)) + break; + + // If we've converged, stop now + t += delta; + if (abs(delta) < 1.0e-4f) + break; + } + + // Calculate the distance squared for the origin to the curve + outDistanceSq = position.LengthSq(); + return t; +} + +void PathConstraintPathHermite::GetIndexAndT(float inFraction, int &outIndex, float &outT) const +{ + int num_points = int(mPoints.size()); + + // Start by truncating the fraction to get the index and storing the remainder in t + int index = int(trunc(inFraction)); + float t = inFraction - float(index); + + if (IsLooping()) + { + JPH_ASSERT(!mPoints.front().mPosition.IsClose(mPoints.back().mPosition), "A looping path should have a different first and last point!"); + + // Make sure index is positive by adding a multiple of num_points + if (index < 0) + index += (-index / num_points + 1) * num_points; + + // Index needs to be modulo num_points + index = index % num_points; + } + else + { + // Clamp against range of points + if (index < 0) + { + index = 0; + t = 0.0f; + } + else if (index >= num_points - 1) + { + index = num_points - 2; + t = 1.0f; + } + } + + outIndex = index; + outT = t; +} + +float PathConstraintPathHermite::GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const +{ + JPH_PROFILE_FUNCTION(); + + int num_points = int(mPoints.size()); + + // Start with last point on the path, in the non-looping case we won't be visiting this point + float best_dist_sq = (mPoints[num_points - 1].mPosition - inPosition).LengthSq(); + float best_t = float(num_points - 1); + + // Loop over all points + for (int i = 0, max_i = IsLooping()? num_points : num_points - 1; i < max_i; ++i) + { + const Point &p1 = mPoints[i]; + const Point &p2 = mPoints[(i + 1) % num_points]; + + // Make the curve relative to inPosition + Vec3 p1_pos = p1.mPosition - inPosition; + Vec3 p2_pos = p2.mPosition - inPosition; + + // Get distance to p1 + float dist_sq = p1_pos.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_t = float(i); + best_dist_sq = dist_sq; + } + + // First find an interval for the closest point so that we can start doing Newton Raphson steps + float t_min, t_max; + sCalculateClosestPointThroughBisection(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max); + + if (t_min == t_max) + { + // If the function above returned no interval then it found the root already and we can just calculate the distance + Vec3 position, tangent; + sCalculatePositionAndTangent(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, position, tangent); + dist_sq = position.LengthSq(); + if (dist_sq < best_dist_sq) + { + best_t = float(i) + t_min; + best_dist_sq = dist_sq; + } + } + else + { + // Get closest distance along curve segment + float t = sCalculateClosestPointThroughNewtonRaphson(p1_pos, p1.mTangent, p2_pos, p2.mTangent, t_min, t_max, dist_sq); + if (t >= 0.0f && t <= 1.0f && dist_sq < best_dist_sq) + { + best_t = float(i) + t; + best_dist_sq = dist_sq; + } + } + } + + return best_t; +} + +void PathConstraintPathHermite::GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const +{ + JPH_PROFILE_FUNCTION(); + + // Determine which hermite spline segment we need + int index; + float t; + GetIndexAndT(inFraction, index, t); + + // Get the points on the segment + const Point &p1 = mPoints[index]; + const Point &p2 = mPoints[(index + 1) % int(mPoints.size())]; + + // Calculate the position and tangent on the path + Vec3 tangent; + sCalculatePositionAndTangent(p1.mPosition, p1.mTangent, p2.mPosition, p2.mTangent, t, outPathPosition, tangent); + outPathTangent = tangent.Normalized(); + + // Just linearly interpolate the normal + Vec3 normal = (1.0f - t) * p1.mNormal + t * p2.mNormal; + + // Calculate binormal + outPathBinormal = normal.Cross(outPathTangent).Normalized(); + + // Recalculate normal so it is perpendicular to both (linear interpolation will cause it not to be) + outPathNormal = outPathTangent.Cross(outPathBinormal); + JPH_ASSERT(outPathNormal.IsNormalized()); +} + +void PathConstraintPathHermite::SaveBinaryState(StreamOut &inStream) const +{ + PathConstraintPath::SaveBinaryState(inStream); + + inStream.Write(mPoints); +} + +void PathConstraintPathHermite::RestoreBinaryState(StreamIn &inStream) +{ + PathConstraintPath::RestoreBinaryState(inStream); + + inStream.Read(mPoints); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.h b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.h new file mode 100644 index 000000000..aaf2ed8f7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PathConstraintPathHermite.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A path that follows a Hermite spline +class JPH_EXPORT PathConstraintPathHermite final : public PathConstraintPath +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PathConstraintPathHermite) + + // See PathConstraintPath::GetPathMaxFraction + virtual float GetPathMaxFraction() const override { return float(IsLooping()? mPoints.size() : mPoints.size() - 1); } + + // See PathConstraintPath::GetClosestPoint + virtual float GetClosestPoint(Vec3Arg inPosition, float inFractionHint) const override; + + // See PathConstraintPath::GetPointOnPath + virtual void GetPointOnPath(float inFraction, Vec3 &outPathPosition, Vec3 &outPathTangent, Vec3 &outPathNormal, Vec3 &outPathBinormal) const override; + + /// Adds a point to the path + void AddPoint(Vec3Arg inPosition, Vec3Arg inTangent, Vec3Arg inNormal) { mPoints.push_back({ inPosition, inTangent, inNormal}); } + + // See: PathConstraintPath::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + struct Point + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Point) + + Vec3 mPosition; ///< Position on the path + Vec3 mTangent; ///< Tangent of the path, does not need to be normalized (in the direction of the path) + Vec3 mNormal; ///< Normal of the path (together with the tangent along the curve this forms a basis for the constraint) + }; + +protected: + // See: PathConstraintPath::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; + +private: + /// Helper function that returns the index of the path segment and the fraction t on the path segment based on the full path fraction + inline void GetIndexAndT(float inFraction, int &outIndex, float &outT) const; + + using Points = Array; + + Points mPoints; ///< Points on the Hermite spline +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PointConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/PointConstraint.cpp new file mode 100644 index 000000000..74d0ecd7c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PointConstraint.cpp @@ -0,0 +1,157 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PointConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PointConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(PointConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(PointConstraintSettings, mPoint2) +} + +void PointConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPoint1); + inStream.Write(mPoint2); +} + +void PointConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPoint1); + inStream.Read(mPoint2); +} + +TwoBodyConstraint *PointConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PointConstraint(inBody1, inBody2, *this); +} + +PointConstraint::PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPoint2); + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + } +} + +void PointConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void PointConstraint::SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1) +{ + if (inSpace == EConstraintSpace::WorldSpace) + mLocalSpacePosition1 = Vec3(mBody1->GetInverseCenterOfMassTransform() * inPoint1); + else + mLocalSpacePosition1 = Vec3(inPoint1); +} + +void PointConstraint::SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2) +{ + if (inSpace == EConstraintSpace::WorldSpace) + mLocalSpacePosition2 = Vec3(mBody2->GetInverseCenterOfMassTransform() * inPoint2); + else + mLocalSpacePosition2 = Vec3(inPoint2); +} + +void PointConstraint::CalculateConstraintProperties() +{ + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); +} + +void PointConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + CalculateConstraintProperties(); +} + +void PointConstraint::ResetWarmStart() +{ + mPointConstraintPart.Deactivate(); +} + +void PointConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool PointConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); +} + +bool PointConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(); + + return mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void PointConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Draw constraint + inRenderer->DrawMarker(mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1, Color::sRed, 0.1f); + inRenderer->DrawMarker(mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2, Color::sGreen, 0.1f); +} +#endif // JPH_DEBUG_RENDERER + +void PointConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); +} + +void PointConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); +} + +Ref PointConstraint::GetConstraintSettings() const +{ + PointConstraintSettings *settings = new PointConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mPoint2 = RVec3(mLocalSpacePosition2); + return settings; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PointConstraint.h b/WickedEngine/Jolt/Physics/Constraints/PointConstraint.h new file mode 100644 index 000000000..f88756931 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PointConstraint.h @@ -0,0 +1,94 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Point constraint settings, used to create a point constraint +class JPH_EXPORT PointConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PointConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint position (space determined by mSpace). + RVec3 mPoint1 = RVec3::sZero(); + + /// Body 2 constraint position (space determined by mSpace). + /// Note: Normally you would set mPoint1 = mPoint2 if the bodies are already placed how you want to constrain them (if mSpace = world space). + RVec3 mPoint2 = RVec3::sZero(); + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A point constraint constrains 2 bodies on a single point (removing 3 degrees of freedom) +class JPH_EXPORT PointConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct point constraint + PointConstraint(Body &inBody1, Body &inBody2, const PointConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Point; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + /// Update the attachment point for body 1 + void SetPoint1(EConstraintSpace inSpace, RVec3Arg inPoint1); + + /// Update the attachment point for body 2 + void SetPoint2(EConstraintSpace inSpace, RVec3Arg inPoint2); + + /// Get the attachment point for body 1 relative to body 1 COM + inline Vec3 GetLocalSpacePoint1() const { return mLocalSpacePosition1; } + + /// Get the attachment point for body 2 relative to body 2 COM + inline Vec3 GetLocalSpacePoint2() const { return mLocalSpacePosition2; } + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(); + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // The constraint part + PointConstraintPart mPointConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.cpp new file mode 100644 index 000000000..9d15c1d4d --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.cpp @@ -0,0 +1,253 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(PulleyConstraintSettings) +{ + JPH_ADD_BASE_CLASS(PulleyConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(PulleyConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint1) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint1) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mBodyPoint2) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mFixedPoint2) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mRatio) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMinLength) + JPH_ADD_ATTRIBUTE(PulleyConstraintSettings, mMaxLength) +} + +void PulleyConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mBodyPoint1); + inStream.Write(mFixedPoint1); + inStream.Write(mBodyPoint2); + inStream.Write(mFixedPoint2); + inStream.Write(mRatio); + inStream.Write(mMinLength); + inStream.Write(mMaxLength); +} + +void PulleyConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mBodyPoint1); + inStream.Read(mFixedPoint1); + inStream.Read(mBodyPoint2); + inStream.Read(mFixedPoint2); + inStream.Read(mRatio); + inStream.Read(mMinLength); + inStream.Read(mMaxLength); +} + +TwoBodyConstraint *PulleyConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new PulleyConstraint(inBody1, inBody2, *this); +} + +PulleyConstraint::PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mFixedPosition1(inSettings.mFixedPoint1), + mFixedPosition2(inSettings.mFixedPoint2), + mRatio(inSettings.mRatio), + mMinLength(inSettings.mMinLength), + mMaxLength(inSettings.mMaxLength) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint1); + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mBodyPoint2); + mWorldSpacePosition1 = inSettings.mBodyPoint1; + mWorldSpacePosition2 = inSettings.mBodyPoint2; + } + else + { + // If properties were specified in local space, we need to calculate world space positions + mLocalSpacePosition1 = Vec3(inSettings.mBodyPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mBodyPoint2); + mWorldSpacePosition1 = inBody1.GetCenterOfMassTransform() * inSettings.mBodyPoint1; + mWorldSpacePosition2 = inBody2.GetCenterOfMassTransform() * inSettings.mBodyPoint2; + } + + // Calculate min/max length if it was not provided + float current_length = GetCurrentLength(); + if (mMinLength < 0.0f) + mMinLength = current_length; + if (mMaxLength < 0.0f) + mMaxLength = current_length; + + // Initialize the normals to a likely valid axis in case the fixed points overlap with the attachment points (most likely the fixed points are above both bodies) + mWorldSpaceNormal1 = mWorldSpaceNormal2 = -Vec3::sAxisY(); +} + +void PulleyConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float PulleyConstraint::CalculatePositionsNormalsAndLength() +{ + // Update world space positions (the bodies may have moved) + mWorldSpacePosition1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + mWorldSpacePosition2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + + // Calculate world space normals + Vec3 delta1 = Vec3(mWorldSpacePosition1 - mFixedPosition1); + float delta1_len = delta1.Length(); + if (delta1_len > 0.0f) + mWorldSpaceNormal1 = delta1 / delta1_len; + + Vec3 delta2 = Vec3(mWorldSpacePosition2 - mFixedPosition2); + float delta2_len = delta2.Length(); + if (delta2_len > 0.0f) + mWorldSpaceNormal2 = delta2 / delta2_len; + + // Calculate length + return delta1_len + mRatio * delta2_len; +} + +void PulleyConstraint::CalculateConstraintProperties() +{ + // Calculate attachment points relative to COM + Vec3 r1 = Vec3(mWorldSpacePosition1 - mBody1->GetCenterOfMassPosition()); + Vec3 r2 = Vec3(mWorldSpacePosition2 - mBody2->GetCenterOfMassPosition()); + + mIndependentAxisConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, r1, mWorldSpaceNormal1, r2, mWorldSpaceNormal2, mRatio); +} + +void PulleyConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Determine if the constraint is active + float current_length = CalculatePositionsNormalsAndLength(); + bool min_length_violation = current_length <= mMinLength; + bool max_length_violation = current_length >= mMaxLength; + if (min_length_violation || max_length_violation) + { + // Determine max lambda based on if the length is too big or small + mMinLambda = max_length_violation? -FLT_MAX : 0.0f; + mMaxLambda = min_length_violation? FLT_MAX : 0.0f; + + CalculateConstraintProperties(); + } + else + mIndependentAxisConstraintPart.Deactivate(); +} + +void PulleyConstraint::ResetWarmStart() +{ + mIndependentAxisConstraintPart.Deactivate(); +} + +void PulleyConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + mIndependentAxisConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, inWarmStartImpulseRatio); +} + +bool PulleyConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + if (mIndependentAxisConstraintPart.IsActive()) + return mIndependentAxisConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, mMinLambda, mMaxLambda); + else + return false; +} + +bool PulleyConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Calculate new length (bodies may have changed) + float current_length = CalculatePositionsNormalsAndLength(); + + float position_error = 0.0f; + if (current_length < mMinLength) + position_error = current_length - mMinLength; + else if (current_length > mMaxLength) + position_error = current_length - mMaxLength; + + if (position_error != 0.0f) + { + // Update constraint properties (bodies may have moved) + CalculateConstraintProperties(); + + return mIndependentAxisConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceNormal1, mWorldSpaceNormal2, mRatio, position_error, inBaumgarte); + } + + return false; +} + +#ifdef JPH_DEBUG_RENDERER +void PulleyConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Color according to length vs min/max length + float current_length = GetCurrentLength(); + Color color = Color::sGreen; + if (current_length < mMinLength) + color = Color::sYellow; + else if (current_length > mMaxLength) + color = Color::sRed; + + // Draw constraint + inRenderer->DrawLine(mWorldSpacePosition1, mFixedPosition1, color); + inRenderer->DrawLine(mFixedPosition1, mFixedPosition2, color); + inRenderer->DrawLine(mFixedPosition2, mWorldSpacePosition2, color); + + // Draw current length + inRenderer->DrawText3D(0.5_r * (mFixedPosition1 + mFixedPosition2), StringFormat("%.2f", (double)current_length)); +} +#endif // JPH_DEBUG_RENDERER + +void PulleyConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mIndependentAxisConstraintPart.SaveState(inStream); + inStream.Write(mWorldSpaceNormal1); // When distance to fixed point = 0, the normal is used from last frame so we need to store it + inStream.Write(mWorldSpaceNormal2); +} + +void PulleyConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mIndependentAxisConstraintPart.RestoreState(inStream); + inStream.Read(mWorldSpaceNormal1); + inStream.Read(mWorldSpaceNormal2); +} + +Ref PulleyConstraint::GetConstraintSettings() const +{ + PulleyConstraintSettings *settings = new PulleyConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mBodyPoint1 = RVec3(mLocalSpacePosition1); + settings->mFixedPoint1 = mFixedPosition1; + settings->mBodyPoint2 = RVec3(mLocalSpacePosition2); + settings->mFixedPoint2 = mFixedPosition2; + settings->mRatio = mRatio; + settings->mMinLength = mMinLength; + settings->mMaxLength = mMaxLength; + return settings; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.h b/WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.h new file mode 100644 index 000000000..5f2523dae --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/PulleyConstraint.h @@ -0,0 +1,137 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Pulley constraint settings, used to create a pulley constraint. +/// A pulley connects two bodies via two fixed world points to each other similar to a distance constraint. +/// We define Length1 = |BodyPoint1 - FixedPoint1| where Body1 is a point on body 1 in world space and FixedPoint1 a fixed point in world space +/// Length2 = |BodyPoint2 - FixedPoint2| +/// The constraint keeps the two line segments constrained so that +/// MinDistance <= Length1 + Ratio * Length2 <= MaxDistance +class JPH_EXPORT PulleyConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, PulleyConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, specified properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint attachment point (space determined by mSpace). + RVec3 mBodyPoint1 = RVec3::sZero(); + + /// Fixed world point to which body 1 is connected (always world space) + RVec3 mFixedPoint1 = RVec3::sZero(); + + /// Body 2 constraint attachment point (space determined by mSpace) + RVec3 mBodyPoint2 = RVec3::sZero(); + + /// Fixed world point to which body 2 is connected (always world space) + RVec3 mFixedPoint2 = RVec3::sZero(); + + /// Ratio between the two line segments (see formula above), can be used to create a block and tackle + float mRatio = 1.0f; + + /// The minimum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created. + float mMinLength = 0.0f; + + /// The maximum length of the line segments (see formula above), use -1 to calculate the length based on the positions of the objects when the constraint is created. + float mMaxLength = -1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A pulley constraint. +class JPH_EXPORT PulleyConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct distance constraint + PulleyConstraint(Body &inBody1, Body &inBody2, const PulleyConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Pulley; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sTranslation(mLocalSpacePosition2); } // Note: Incorrect rotation as we don't track the original rotation difference, should not matter though as the constraint is not limiting rotation. + + /// Update the minimum and maximum length for the constraint + void SetLength(float inMinLength, float inMaxLength) { JPH_ASSERT(inMinLength >= 0.0f && inMinLength <= inMaxLength); mMinLength = inMinLength; mMaxLength = inMaxLength; } + float GetMinLength() const { return mMinLength; } + float GetMaxLength() const { return mMaxLength; } + + /// Get the current length of both segments (multiplied by the ratio for segment 2) + float GetCurrentLength() const { return Vec3(mWorldSpacePosition1 - mFixedPosition1).Length() + mRatio * Vec3(mWorldSpacePosition2 - mFixedPosition2).Length(); } + + ///@name Get Lagrange multiplier from last physics update (the linear impulse applied to satisfy the constraint) + inline float GetTotalLambdaPosition() const { return mIndependentAxisConstraintPart.GetTotalLambda(); } + +private: + // Calculates world positions and normals and returns current length + float CalculatePositionsNormalsAndLength(); + + // Internal helper function to calculate the values below + void CalculateConstraintProperties(); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions on the bodies + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // World space fixed positions + RVec3 mFixedPosition1; + RVec3 mFixedPosition2; + + /// Ratio between the two line segments + float mRatio; + + // The minimum/maximum length of the line segments + float mMinLength; + float mMaxLength; + + // RUN TIME PROPERTIES FOLLOW + + // World space positions and normal + RVec3 mWorldSpacePosition1; + RVec3 mWorldSpacePosition2; + Vec3 mWorldSpaceNormal1; + Vec3 mWorldSpaceNormal2; + + // Depending on if the length < min or length > max we can apply forces to prevent further violations + float mMinLambda; + float mMaxLambda; + + // The constraint part + IndependentAxisConstraintPart mIndependentAxisConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp new file mode 100644 index 000000000..d0bcb62fe --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.cpp @@ -0,0 +1,189 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(RackAndPinionConstraintSettings) +{ + JPH_ADD_BASE_CLASS(RackAndPinionConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(RackAndPinionConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mHingeAxis) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mSliderAxis) + JPH_ADD_ATTRIBUTE(RackAndPinionConstraintSettings, mRatio) +} + +void RackAndPinionConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mHingeAxis); + inStream.Write(mSliderAxis); + inStream.Write(mRatio); +} + +void RackAndPinionConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mHingeAxis); + inStream.Read(mSliderAxis); + inStream.Read(mRatio); +} + +TwoBodyConstraint *RackAndPinionConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new RackAndPinionConstraint(inBody1, inBody2, *this); +} + +RackAndPinionConstraint::RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mLocalSpaceHingeAxis(inSettings.mHingeAxis), + mLocalSpaceSliderAxis(inSettings.mSliderAxis), + mRatio(inSettings.mRatio) +{ + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpaceHingeAxis = inBody1.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceHingeAxis).Normalized(); + mLocalSpaceSliderAxis = inBody2.GetInverseCenterOfMassTransform().Multiply3x3(mLocalSpaceSliderAxis).Normalized(); + } +} + +void RackAndPinionConstraint::CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mWorldSpaceHingeAxis = inRotation1 * mLocalSpaceHingeAxis; + mWorldSpaceSliderAxis = inRotation2 * mLocalSpaceSliderAxis; + + mRackAndPinionConstraintPart.CalculateConstraintProperties(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio); +} + +void RackAndPinionConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); +} + +void RackAndPinionConstraint::ResetWarmStart() +{ + mRackAndPinionConstraintPart.Deactivate(); +} + +void RackAndPinionConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mRackAndPinionConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool RackAndPinionConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + return mRackAndPinionConstraintPart.SolveVelocityConstraint(*mBody1, mWorldSpaceHingeAxis, *mBody2, mWorldSpaceSliderAxis, mRatio); +} + +bool RackAndPinionConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + if (mRackConstraint == nullptr || mPinionConstraint == nullptr) + return false; + + float rotation; + if (mPinionConstraint->GetSubType() == EConstraintSubType::Hinge) + { + rotation = StaticCast(mPinionConstraint)->GetCurrentAngle(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float translation; + if (mRackConstraint->GetSubType() == EConstraintSubType::Slider) + { + translation = StaticCast(mRackConstraint)->GetCurrentPosition(); + } + else + { + JPH_ASSERT(false, "Unsupported"); + return false; + } + + float error = CenterAngleAroundZero(fmod(rotation - mRatio * translation, 2.0f * JPH_PI)); + if (error == 0.0f) + return false; + + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateConstraintProperties(rotation1, rotation2); + return mRackAndPinionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, error, inBaumgarte); +} + +#ifdef JPH_DEBUG_RENDERER +void RackAndPinionConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Draw constraint axis + inRenderer->DrawArrow(transform1.GetTranslation(), transform1 * mLocalSpaceHingeAxis, Color::sGreen, 0.01f); + inRenderer->DrawArrow(transform2.GetTranslation(), transform2 * mLocalSpaceSliderAxis, Color::sBlue, 0.01f); +} + +#endif // JPH_DEBUG_RENDERER + +void RackAndPinionConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mRackAndPinionConstraintPart.SaveState(inStream); +} + +void RackAndPinionConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mRackAndPinionConstraintPart.RestoreState(inStream); +} + +Ref RackAndPinionConstraint::GetConstraintSettings() const +{ + RackAndPinionConstraintSettings *settings = new RackAndPinionConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mHingeAxis = mLocalSpaceHingeAxis; + settings->mSliderAxis = mLocalSpaceSliderAxis; + settings->mRatio = mRatio; + return settings; +} + +Mat44 RackAndPinionConstraint::GetConstraintToBody1Matrix() const +{ + Vec3 perp = mLocalSpaceHingeAxis.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceHingeAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceHingeAxis.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +Mat44 RackAndPinionConstraint::GetConstraintToBody2Matrix() const +{ + Vec3 perp = mLocalSpaceSliderAxis.GetNormalizedPerpendicular(); + return Mat44(Vec4(mLocalSpaceSliderAxis, 0), Vec4(perp, 0), Vec4(mLocalSpaceSliderAxis.Cross(perp), 0), Vec4(0, 0, 0, 1)); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.h b/WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.h new file mode 100644 index 000000000..1a0e43efb --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/RackAndPinionConstraint.h @@ -0,0 +1,118 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Rack and pinion constraint (slider & gear) settings +class JPH_EXPORT RackAndPinionConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, RackAndPinionConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + /// Body1 should be the pinion (gear) and body 2 the rack (slider). + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Defines the ratio between the rotation of the pinion and the translation of the rack. + /// The ratio is defined as: PinionRotation(t) = ratio * RackTranslation(t) + /// @param inNumTeethRack Number of teeth that the rack has + /// @param inRackLength Length of the rack + /// @param inNumTeethPinion Number of teeth the pinion has + void SetRatio(int inNumTeethRack, float inRackLength, int inNumTeethPinion) + { + mRatio = 2.0f * JPH_PI * inNumTeethRack / (inRackLength * inNumTeethPinion); + } + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 (pinion) constraint reference frame (space determined by mSpace). + Vec3 mHingeAxis = Vec3::sAxisX(); + + /// Body 2 (rack) constraint reference frame (space determined by mSpace) + Vec3 mSliderAxis = Vec3::sAxisX(); + + /// Ratio between the rack and pinion, see SetRatio. + float mRatio = 1.0f; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A rack and pinion constraint constrains the rotation of body1 to the translation of body 2. +/// Note that this constraint needs to be used in conjunction with a hinge constraint for body 1 and a slider constraint for body 2. +class JPH_EXPORT RackAndPinionConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct gear constraint + RackAndPinionConstraint(Body &inBody1, Body &inBody2, const RackAndPinionConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::RackAndPinion; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift. + void SetConstraints(const Constraint *inPinion, const Constraint *inRack) { mPinionConstraint = inPinion; mRackConstraint = inRack; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline float GetTotalLambda() const { return mRackAndPinionConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space hinge axis + Vec3 mLocalSpaceHingeAxis; + + // Local space sliding direction + Vec3 mLocalSpaceSliderAxis; + + // Ratio between rack and pinion + float mRatio; + + // The constraints that constrain the rack and pinion (a slider and a hinge), optional and used to calculate the position error and fix numerical drift. + RefConst mPinionConstraint; + RefConst mRackConstraint; + + // RUN TIME PROPERTIES FOLLOW + + // World space hinge axis + Vec3 mWorldSpaceHingeAxis; + + // World space sliding direction + Vec3 mWorldSpaceSliderAxis; + + // The constraint parts + RackAndPinionConstraintPart mRackAndPinionConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.cpp new file mode 100644 index 000000000..070a45e21 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.cpp @@ -0,0 +1,900 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SixDOFConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SixDOFConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY1) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mPosition2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisX2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mAxisY2) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMaxFriction) + JPH_ADD_ENUM_ATTRIBUTE(SixDOFConstraintSettings, mSwingType) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMin) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitMax) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mLimitsSpringSettings) + JPH_ADD_ATTRIBUTE(SixDOFConstraintSettings, mMotorSettings) +} + +void SixDOFConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPosition1); + inStream.Write(mAxisX1); + inStream.Write(mAxisY1); + inStream.Write(mPosition2); + inStream.Write(mAxisX2); + inStream.Write(mAxisY2); + inStream.Write(mMaxFriction); + inStream.Write(mSwingType); + inStream.Write(mLimitMin); + inStream.Write(mLimitMax); + for (const SpringSettings &s : mLimitsSpringSettings) + s.SaveBinaryState(inStream); + for (const MotorSettings &m : mMotorSettings) + m.SaveBinaryState(inStream); +} + +void SixDOFConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPosition1); + inStream.Read(mAxisX1); + inStream.Read(mAxisY1); + inStream.Read(mPosition2); + inStream.Read(mAxisX2); + inStream.Read(mAxisY2); + inStream.Read(mMaxFriction); + inStream.Read(mSwingType); + inStream.Read(mLimitMin); + inStream.Read(mLimitMax); + for (SpringSettings &s : mLimitsSpringSettings) + s.RestoreBinaryState(inStream); + for (MotorSettings &m : mMotorSettings) + m.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SixDOFConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SixDOFConstraint(inBody1, inBody2, *this); +} + +void SixDOFConstraint::UpdateTranslationLimits() +{ + // Set to zero if the limits are inversed + for (int i = EAxis::TranslationX; i <= EAxis::TranslationZ; ++i) + if (mLimitMin[i] > mLimitMax[i]) + mLimitMin[i] = mLimitMax[i] = 0.0f; +} + +void SixDOFConstraint::UpdateRotationLimits() +{ + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Cone) + { + // Cone swing upper limit needs to be positive + mLimitMax[EAxis::RotationY] = max(0.0f, mLimitMax[EAxis::RotationY]); + mLimitMax[EAxis::RotationZ] = max(0.0f, mLimitMax[EAxis::RotationZ]); + + // Cone swing limits only support symmetric ranges + mLimitMin[EAxis::RotationY] = -mLimitMax[EAxis::RotationY]; + mLimitMin[EAxis::RotationZ] = -mLimitMax[EAxis::RotationZ]; + } + + for (int i = EAxis::RotationX; i <= EAxis::RotationZ; ++i) + { + // Clamp to [-PI, PI] range + mLimitMin[i] = Clamp(mLimitMin[i], -JPH_PI, JPH_PI); + mLimitMax[i] = Clamp(mLimitMax[i], -JPH_PI, JPH_PI); + + // Set to zero if the limits are inversed + if (mLimitMin[i] > mLimitMax[i]) + mLimitMin[i] = mLimitMax[i] = 0.0f; + } + + // Pass limits on to constraint part + mSwingTwistConstraintPart.SetLimits(mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ]); +} + +void SixDOFConstraint::UpdateFixedFreeAxis() +{ + uint8 old_free_axis = mFreeAxis; + uint8 old_fixed_axis = mFixedAxis; + + // Cache which axis are fixed and which ones are free + mFreeAxis = 0; + mFixedAxis = 0; + for (int a = 0; a < EAxis::Num; ++a) + { + float limit = a >= EAxis::RotationX? JPH_PI : FLT_MAX; + + if (mLimitMin[a] >= mLimitMax[a]) + mFixedAxis |= 1 << a; + else if (mLimitMin[a] <= -limit && mLimitMax[a] >= limit) + mFreeAxis |= 1 << a; + } + + // On change we deactivate all constraints to reset warm starting + if (old_free_axis != mFreeAxis || old_fixed_axis != mFixedAxis) + { + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.Deactivate(); + mPointConstraintPart.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.Deactivate(); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.Deactivate(); + } +} + +SixDOFConstraint::SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings) +{ + // Override swing type + mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType); + + // Calculate rotation needed to go from constraint space to body1 local space + Vec3 axis_z1 = inSettings.mAxisX1.Cross(inSettings.mAxisY1); + Mat44 c_to_b1(Vec4(inSettings.mAxisX1, 0), Vec4(inSettings.mAxisY1, 0), Vec4(axis_z1, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody1 = c_to_b1.GetQuaternion(); + + // Calculate rotation needed to go from constraint space to body2 local space + Vec3 axis_z2 = inSettings.mAxisX2.Cross(inSettings.mAxisY2); + Mat44 c_to_b2(Vec4(inSettings.mAxisX2, 0), Vec4(inSettings.mAxisY2, 0), Vec4(axis_z2, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody2 = c_to_b2.GetQuaternion(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1); + mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1; + + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2); + mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2; + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPosition1); + mLocalSpacePosition2 = Vec3(inSettings.mPosition2); + } + + // Copy translation and rotation limits + memcpy(mLimitMin, inSettings.mLimitMin, sizeof(mLimitMin)); + memcpy(mLimitMax, inSettings.mLimitMax, sizeof(mLimitMax)); + memcpy(mLimitsSpringSettings, inSettings.mLimitsSpringSettings, sizeof(mLimitsSpringSettings)); + UpdateTranslationLimits(); + UpdateRotationLimits(); + UpdateFixedFreeAxis(); + CacheHasSpringLimits(); + + // Store friction settings + memcpy(mMaxFriction, inSettings.mMaxFriction, sizeof(mMaxFriction)); + + // Store motor settings + for (int i = 0; i < EAxis::Num; ++i) + mMotorSettings[i] = inSettings.mMotorSettings[i]; + + // Cache if motors are active (motors are off initially, but we may have friction) + CacheTranslationMotorActive(); + CacheRotationMotorActive(); +} + +void SixDOFConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +void SixDOFConstraint::SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax) +{ + mLimitMin[EAxis::TranslationX] = inLimitMin.GetX(); + mLimitMin[EAxis::TranslationY] = inLimitMin.GetY(); + mLimitMin[EAxis::TranslationZ] = inLimitMin.GetZ(); + mLimitMax[EAxis::TranslationX] = inLimitMax.GetX(); + mLimitMax[EAxis::TranslationY] = inLimitMax.GetY(); + mLimitMax[EAxis::TranslationZ] = inLimitMax.GetZ(); + + UpdateTranslationLimits(); + UpdateFixedFreeAxis(); +} + +void SixDOFConstraint::SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax) +{ + mLimitMin[EAxis::RotationX] = inLimitMin.GetX(); + mLimitMin[EAxis::RotationY] = inLimitMin.GetY(); + mLimitMin[EAxis::RotationZ] = inLimitMin.GetZ(); + mLimitMax[EAxis::RotationX] = inLimitMax.GetX(); + mLimitMax[EAxis::RotationY] = inLimitMax.GetY(); + mLimitMax[EAxis::RotationZ] = inLimitMax.GetZ(); + + UpdateRotationLimits(); + UpdateFixedFreeAxis(); +} + +void SixDOFConstraint::SetMaxFriction(EAxis inAxis, float inFriction) +{ + mMaxFriction[inAxis] = inFriction; + + if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ) + CacheTranslationMotorActive(); + else + CacheRotationMotorActive(); +} + +void SixDOFConstraint::GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const +{ + RVec3 p1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + RVec3 p2 = mBody2->GetCenterOfMassTransform() * mLocalSpacePosition2; + outR1PlusU = Vec3(p2 - mBody1->GetCenterOfMassPosition()); // r1 + u = (p1 - x1) + (p2 - p1) = p2 - x1 + outR2 = Vec3(p2 - mBody2->GetCenterOfMassPosition()); + outU = Vec3(p2 - p1); +} + +Quat SixDOFConstraint::GetRotationInConstraintSpace() const +{ + // Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform()) + // Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1)) + // Let q be the rotation of the constraint in constraint space + // b2 takes a vector from the local space of body2 to world space + // To express this in terms of b1: b2 = b1 * c1 * q * c2^-1 + // c2^-1 goes from local body 2 space to constraint space + // q rotates the constraint + // c1 goes from constraint space to body 1 local space + // b1 goes from body 1 local space to world space + // So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2 + // Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations + return (mBody1->GetRotation() * mConstraintToBody1).Conjugated() * mBody2->GetRotation() * mConstraintToBody2; +} + +void SixDOFConstraint::CacheTranslationMotorActive() +{ + mTranslationMotorActive = mMotorState[EAxis::TranslationX] != EMotorState::Off + || mMotorState[EAxis::TranslationY] != EMotorState::Off + || mMotorState[EAxis::TranslationZ] != EMotorState::Off + || HasFriction(EAxis::TranslationX) + || HasFriction(EAxis::TranslationY) + || HasFriction(EAxis::TranslationZ); +} + +void SixDOFConstraint::CacheRotationMotorActive() +{ + mRotationMotorActive = mMotorState[EAxis::RotationX] != EMotorState::Off + || mMotorState[EAxis::RotationY] != EMotorState::Off + || mMotorState[EAxis::RotationZ] != EMotorState::Off + || HasFriction(EAxis::RotationX) + || HasFriction(EAxis::RotationY) + || HasFriction(EAxis::RotationZ); +} + +void SixDOFConstraint::CacheRotationPositionMotorActive() +{ + mRotationPositionMotorActive = 0; + for (int i = 0; i < 3; ++i) + if (mMotorState[EAxis::RotationX + i] == EMotorState::Position) + mRotationPositionMotorActive |= 1 << i; +} + +void SixDOFConstraint::CacheHasSpringLimits() +{ + mHasSpringLimits = mLimitsSpringSettings[EAxis::TranslationX].mFrequency > 0.0f + || mLimitsSpringSettings[EAxis::TranslationY].mFrequency > 0.0f + || mLimitsSpringSettings[EAxis::TranslationZ].mFrequency > 0.0f; +} + +void SixDOFConstraint::SetMotorState(EAxis inAxis, EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mMotorSettings[inAxis].IsValid()); + + if (mMotorState[inAxis] != inState) + { + mMotorState[inAxis] = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + if (inAxis >= EAxis::TranslationX && inAxis <= EAxis::TranslationZ) + { + mMotorTranslationConstraintPart[inAxis - EAxis::TranslationX].Deactivate(); + + CacheTranslationMotorActive(); + } + else + { + JPH_ASSERT(inAxis >= EAxis::RotationX && inAxis <= EAxis::RotationZ); + + mMotorRotationConstraintPart[inAxis - EAxis::RotationX].Deactivate(); + + CacheRotationMotorActive(); + CacheRotationPositionMotorActive(); + } + } +} + +void SixDOFConstraint::SetTargetOrientationCS(QuatArg inOrientation) +{ + Quat q_swing, q_twist; + inOrientation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis); + + if (clamped_axis != 0) + mTargetOrientation = q_swing * q_twist; + else + mTargetOrientation = inOrientation; +} + +void SixDOFConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Get body rotations + Quat rotation1 = mBody1->GetRotation(); + Quat rotation2 = mBody2->GetRotation(); + + // Quaternion that rotates from body1's constraint space to world space + Quat constraint_body1_to_world = rotation1 * mConstraintToBody1; + + // Store world space axis of constraint space + Mat44 translation_axis_mat = Mat44::sRotation(constraint_body1_to_world); + for (int i = 0; i < 3; ++i) + mTranslationAxis[i] = translation_axis_mat.GetColumn3(i); + + if (IsTranslationFullyConstrained()) + { + // All translation locked: Setup point constraint + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(rotation1), mLocalSpacePosition1, *mBody2, Mat44::sRotation(rotation2), mLocalSpacePosition2); + } + else if (IsTranslationConstrained() || mTranslationMotorActive) + { + // Update world space positions (the bodies may have moved) + Vec3 r1_plus_u, r2, u; + GetPositionConstraintProperties(r1_plus_u, r2, u); + + // Setup axis constraint parts + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::TranslationX + i); + + Vec3 translation_axis = mTranslationAxis[i]; + + // Calculate displacement along this axis + float d = translation_axis.Dot(u); + mDisplacement[i] = d; // Store for SolveVelocityConstraint + + // Setup limit constraint + bool constraint_active = false; + float constraint_value = 0.0f; + if (IsFixedAxis(axis)) + { + // When constraint is fixed it is always active + constraint_value = d - mLimitMin[i]; + constraint_active = true; + } + else if (!IsFreeAxis(axis)) + { + // When constraint is limited, it is only active when outside of the allowed range + if (d <= mLimitMin[i]) + { + constraint_value = d - mLimitMin[i]; + constraint_active = true; + } + else if (d >= mLimitMax[i]) + { + constraint_value = d - mLimitMax[i]; + constraint_active = true; + } + } + + if (constraint_active) + mTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, constraint_value, mLimitsSpringSettings[i]); + else + mTranslationConstraintPart[i].Deactivate(); + + // Setup motor constraint + switch (mMotorState[i]) + { + case EMotorState::Off: + if (HasFriction(axis)) + mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis); + else + mMotorTranslationConstraintPart[i].Deactivate(); + break; + + case EMotorState::Velocity: + mMotorTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis, -mTargetVelocity[i]); + break; + + case EMotorState::Position: + { + const SpringSettings &spring_settings = mMotorSettings[i].mSpringSettings; + if (spring_settings.HasStiffness()) + mMotorTranslationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, r1_plus_u, *mBody2, r2, translation_axis, 0.0f, translation_axis.Dot(u) - mTargetPosition[i], spring_settings); + else + mMotorTranslationConstraintPart[i].Deactivate(); + break; + } + } + } + } + + // Setup rotation constraints + if (IsRotationFullyConstrained()) + { + // All rotation locked: Setup rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + } + else if (IsRotationConstrained() || mRotationMotorActive) + { + // GetRotationInConstraintSpace without redoing the calculation of constraint_body1_to_world + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world; + + // Use swing twist constraint part + if (IsRotationConstrained()) + mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world); + else + mSwingTwistConstraintPart.Deactivate(); + + if (mRotationMotorActive) + { + // Calculate rotation motor axis + Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world); + for (int i = 0; i < 3; ++i) + mRotationAxis[i] = ws_axis.GetColumn3(i); + + // Get target orientation along the shortest path from q + Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation; + + // The definition of the constraint rotation q: + // R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1) + // + // R2' is the rotation of body 2 when reaching the target_orientation: + // R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2) + // + // The difference in body 2 space: + // R2' = R2 * diff_body2 (3) + // + // We want to specify the difference in the constraint space of body 2: + // diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4) + // + // Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5) + // Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6) + // Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7) + // Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* + // <=> target_orientation = q * diff + // <=> diff = q^* * target_orientation + Quat diff = q.Conjugated() * target_orientation; + + // Project diff so that only rotation around axis that have a position motor are remaining + Quat projected_diff; + switch (mRotationPositionMotorActive) + { + case 0b001: + // Keep only rotation around X + projected_diff = diff.GetTwist(Vec3::sAxisX()); + break; + + case 0b010: + // Keep only rotation around Y + projected_diff = diff.GetTwist(Vec3::sAxisY()); + break; + + case 0b100: + // Keep only rotation around Z + projected_diff = diff.GetTwist(Vec3::sAxisZ()); + break; + + case 0b011: + // Remove rotation around Z + // q = swing_xy * twist_z <=> swing_xy = q * twist_z^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisZ()).Conjugated(); + break; + + case 0b101: + // Remove rotation around Y + // q = swing_xz * twist_y <=> swing_xz = q * twist_y^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisY()).Conjugated(); + break; + + case 0b110: + // Remove rotation around X + // q = swing_yz * twist_x <=> swing_yz = q * twist_x^* + projected_diff = diff * diff.GetTwist(Vec3::sAxisX()).Conjugated(); + break; + + case 0b111: + default: // All motors off is handled here but the results are unused + // Keep entire rotation + projected_diff = diff; + break; + } + + // Approximate error angles + // The imaginary part of a quaternion is rotation_axis * sin(angle / 2) + // If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i] + // We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction + Vec3 rotation_error = -2.0f * projected_diff.GetXYZ(); + + // Setup motors + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::RotationX + i); + + Vec3 rotation_axis = mRotationAxis[i]; + + switch (mMotorState[axis]) + { + case EMotorState::Off: + if (HasFriction(axis)) + mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis); + else + mMotorRotationConstraintPart[i].Deactivate(); + break; + + case EMotorState::Velocity: + mMotorRotationConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, rotation_axis, -mTargetAngularVelocity[i]); + break; + + case EMotorState::Position: + { + const SpringSettings &spring_settings = mMotorSettings[axis].mSpringSettings; + if (spring_settings.HasStiffness()) + mMotorRotationConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, rotation_axis, 0.0f, rotation_error[i], spring_settings); + else + mMotorRotationConstraintPart[i].Deactivate(); + break; + } + } + } + } + } +} + +void SixDOFConstraint::ResetWarmStart() +{ + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.Deactivate(); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.Deactivate(); + mRotationConstraintPart.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.Deactivate(); +} + +void SixDOFConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm start translation motors + if (mTranslationMotorActive) + for (int i = 0; i < 3; ++i) + if (mMotorTranslationConstraintPart[i].IsActive()) + mMotorTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio); + + // Warm start rotation motors + if (mRotationMotorActive) + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + if (c.IsActive()) + c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + + // Warm start rotation constraints + if (IsRotationFullyConstrained()) + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + else if (IsRotationConstrained()) + mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + + // Warm start translation constraints + if (IsTranslationFullyConstrained()) + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + else if (IsTranslationConstrained()) + for (int i = 0; i < 3; ++i) + if (mTranslationConstraintPart[i].IsActive()) + mTranslationConstraintPart[i].WarmStart(*mBody1, *mBody2, mTranslationAxis[i], inWarmStartImpulseRatio); +} + +bool SixDOFConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve translation motor + if (mTranslationMotorActive) + for (int i = 0; i < 3; ++i) + if (mMotorTranslationConstraintPart[i].IsActive()) + switch (mMotorState[i]) + { + case EMotorState::Off: + { + // Apply friction only + float max_lambda = mMaxFriction[i] * inDeltaTime; + impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + // Drive motor + impulse |= mMotorTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], inDeltaTime * mMotorSettings[i].mMinForceLimit, inDeltaTime * mMotorSettings[i].mMaxForceLimit); + break; + } + + // Solve rotation motor + if (mRotationMotorActive) + for (int i = 0; i < 3; ++i) + { + EAxis axis = EAxis(EAxis::RotationX + i); + if (mMotorRotationConstraintPart[i].IsActive()) + switch (mMotorState[axis]) + { + case EMotorState::Off: + { + // Apply friction only + float max_lambda = mMaxFriction[axis] * inDeltaTime; + impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + // Drive motor + impulse |= mMotorRotationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mRotationAxis[i], inDeltaTime * mMotorSettings[axis].mMinTorqueLimit, inDeltaTime * mMotorSettings[axis].mMaxTorqueLimit); + break; + } + } + + // Solve rotation constraint + if (IsRotationFullyConstrained()) + impulse |= mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + else if (IsRotationConstrained()) + impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + if (IsTranslationFullyConstrained()) + impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + else if (IsTranslationConstrained()) + for (int i = 0; i < 3; ++i) + if (mTranslationConstraintPart[i].IsActive()) + { + // If the axis is not fixed it must be limited (or else the constraint would not be active) + // Calculate the min and max constraint force based on on which side we're limited + float limit_min = -FLT_MAX, limit_max = FLT_MAX; + if (!IsFixedAxis(EAxis(EAxis::TranslationX + i))) + { + JPH_ASSERT(!IsFreeAxis(EAxis(EAxis::TranslationX + i))); + if (mDisplacement[i] <= mLimitMin[i]) + limit_min = 0; + else if (mDisplacement[i] >= mLimitMax[i]) + limit_max = 0; + } + + impulse |= mTranslationConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mTranslationAxis[i], limit_min, limit_max); + } + + return impulse; +} + +bool SixDOFConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + if (IsRotationFullyConstrained()) + { + // Rotation locked: Solve rotation constraint + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + // Definition of initial orientation r0: q2 = q1 r0 + // Initial rotation (see: GetRotationInConstraintSpace): q2 = q1 c1 c2^-1 + // So: r0^-1 = (c1 c2^-1)^-1 = c2 * c1^-1 + Quat constraint_to_body1 = mConstraintToBody1 * Quat::sEulerAngles(GetRotationLimitsMin()); + Quat inv_initial_orientation = mConstraintToBody2 * constraint_to_body1.Conjugated(); + + // Solve rotation violations + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + impulse |= mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inv_initial_orientation, inBaumgarte); + } + else if (IsRotationConstrained()) + { + // Rotation partially constraint + + // Solve rotation violations + Quat q = GetRotationInConstraintSpace(); + impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte); + } + + // Solve position violations + if (IsTranslationFullyConstrained()) + { + // Translation locked: Solve point constraint + Vec3 local_space_position1 = mLocalSpacePosition1 + mConstraintToBody1 * GetTranslationLimitsMin(); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), local_space_position1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + } + else if (IsTranslationConstrained()) + { + // Translation partially locked: Solve per axis + for (int i = 0; i < 3; ++i) + if (mLimitsSpringSettings[i].mFrequency <= 0.0f) // If not soft limit + { + // Update world space positions (the bodies may have moved) + Vec3 r1_plus_u, r2, u; + GetPositionConstraintProperties(r1_plus_u, r2, u); + + // Quaternion that rotates from body1's constraint space to world space + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + + // Calculate axis + Vec3 translation_axis; + switch (i) + { + case 0: translation_axis = constraint_body1_to_world.RotateAxisX(); break; + case 1: translation_axis = constraint_body1_to_world.RotateAxisY(); break; + default: JPH_ASSERT(i == 2); translation_axis = constraint_body1_to_world.RotateAxisZ(); break; + } + + // Determine position error + float error = 0.0f; + EAxis axis(EAxis(EAxis::TranslationX + i)); + if (IsFixedAxis(axis)) + error = u.Dot(translation_axis) - mLimitMin[axis]; + else if (!IsFreeAxis(axis)) + { + float displacement = u.Dot(translation_axis); + if (displacement <= mLimitMin[axis]) + error = displacement - mLimitMin[axis]; + else if (displacement >= mLimitMax[axis]) + error = displacement - mLimitMax[axis]; + } + + if (error != 0.0f) + { + // Setup axis constraint part and solve it + mTranslationConstraintPart[i].CalculateConstraintProperties(*mBody1, r1_plus_u, *mBody2, r2, translation_axis); + impulse |= mTranslationConstraintPart[i].SolvePositionConstraint(*mBody1, *mBody2, translation_axis, error, inBaumgarte); + } + } + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER +void SixDOFConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RVec3 position1 = mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1; + Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1; + Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2; + + // Draw constraint orientation + inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize); + + if ((IsRotationConstrained() || mRotationPositionMotorActive != 0) && !IsRotationFullyConstrained()) + { + // Draw current swing and twist + Quat q = GetRotationInConstraintSpace(); + Quat q_swing, q_twist; + q.GetSwingTwist(q_swing, q_twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite); + } + + // Draw target rotation + Quat m_swing, m_twist; + mTargetOrientation.GetSwingTwist(m_swing, m_twist); + if (mMotorState[EAxis::RotationX] == EMotorState::Position) + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_twist).RotateAxisY(), Color::sYellow); + if (mMotorState[EAxis::RotationY] == EMotorState::Position || mMotorState[EAxis::RotationZ] == EMotorState::Position) + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * m_swing).RotateAxisX(), Color::sYellow); + + // Draw target angular velocity + Vec3 target_angular_velocity = Vec3::sZero(); + for (int i = 0; i < 3; ++i) + if (mMotorState[EAxis::RotationX + i] == EMotorState::Velocity) + target_angular_velocity.SetComponent(i, mTargetAngularVelocity[i]); + if (target_angular_velocity != Vec3::sZero()) + inRenderer->DrawArrow(position1, position1 + rotation2 * target_angular_velocity, Color::sRed, 0.1f); +} + +void SixDOFConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get matrix that transforms from constraint space to world space + RMat44 constraint_body1_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1); + + // Draw limits + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid) + inRenderer->DrawSwingPyramidLimits(constraint_body1_to_world, mLimitMin[EAxis::RotationY], mLimitMax[EAxis::RotationY], mLimitMin[EAxis::RotationZ], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + else + inRenderer->DrawSwingConeLimits(constraint_body1_to_world, mLimitMax[EAxis::RotationY], mLimitMax[EAxis::RotationZ], mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + inRenderer->DrawPie(constraint_body1_to_world.GetTranslation(), mDrawConstraintSize, constraint_body1_to_world.GetAxisX(), constraint_body1_to_world.GetAxisY(), mLimitMin[EAxis::RotationX], mLimitMax[EAxis::RotationX], Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void SixDOFConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + for (const AxisConstraintPart &c : mTranslationConstraintPart) + c.SaveState(inStream); + mPointConstraintPart.SaveState(inStream); + mSwingTwistConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + for (const AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.SaveState(inStream); + for (const AngleConstraintPart &c : mMotorRotationConstraintPart) + c.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetPosition); + inStream.Write(mTargetOrientation); +} + +void SixDOFConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + for (AxisConstraintPart &c : mTranslationConstraintPart) + c.RestoreState(inStream); + mPointConstraintPart.RestoreState(inStream); + mSwingTwistConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + for (AxisConstraintPart &c : mMotorTranslationConstraintPart) + c.RestoreState(inStream); + for (AngleConstraintPart &c : mMotorRotationConstraintPart) + c.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetPosition); + inStream.Read(mTargetOrientation); + + CacheTranslationMotorActive(); + CacheRotationMotorActive(); + CacheRotationPositionMotorActive(); +} + +Ref SixDOFConstraint::GetConstraintSettings() const +{ + SixDOFConstraintSettings *settings = new SixDOFConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPosition1 = RVec3(mLocalSpacePosition1); + settings->mAxisX1 = mConstraintToBody1.RotateAxisX(); + settings->mAxisY1 = mConstraintToBody1.RotateAxisY(); + settings->mPosition2 = RVec3(mLocalSpacePosition2); + settings->mAxisX2 = mConstraintToBody2.RotateAxisX(); + settings->mAxisY2 = mConstraintToBody2.RotateAxisY(); + settings->mSwingType = mSwingTwistConstraintPart.GetSwingType(); + memcpy(settings->mLimitMin, mLimitMin, sizeof(mLimitMin)); + memcpy(settings->mLimitMax, mLimitMax, sizeof(mLimitMax)); + memcpy(settings->mMaxFriction, mMaxFriction, sizeof(mMaxFriction)); + for (int i = 0; i < EAxis::Num; ++i) + settings->mMotorSettings[i] = mMotorSettings[i]; + return settings; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.h b/WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.h new file mode 100644 index 000000000..1bc03eced --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SixDOFConstraint.h @@ -0,0 +1,289 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// 6 Degree Of Freedom Constraint setup structure. Allows control over each of the 6 degrees of freedom. +class JPH_EXPORT SixDOFConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SixDOFConstraintSettings) + + /// Constraint is split up into translation/rotation around X, Y and Z axis. + enum EAxis + { + TranslationX, + TranslationY, + TranslationZ, + + RotationX, + RotationY, + RotationZ, + + Num, + NumTranslation = TranslationZ + 1, + }; + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPosition1 = RVec3::sZero(); + Vec3 mAxisX1 = Vec3::sAxisX(); + Vec3 mAxisY1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPosition2 = RVec3::sZero(); + Vec3 mAxisX2 = Vec3::sAxisX(); + Vec3 mAxisY2 = Vec3::sAxisY(); + + /// Friction settings. + /// For translation: Max friction force in N. 0 = no friction. + /// For rotation: Max friction torque in Nm. 0 = no friction. + float mMaxFriction[EAxis::Num] = { 0, 0, 0, 0, 0, 0 }; + + /// The type of swing constraint that we want to use. + ESwingType mSwingType = ESwingType::Cone; + + /// Limits. + /// For translation: Min and max linear limits in m (0 is frame of body 1 and 2 coincide). + /// For rotation: Min and max angular limits in rad (0 is frame of body 1 and 2 coincide). See comments at Axis enum for limit ranges. + /// + /// Remove degree of freedom by setting min = FLT_MAX and max = -FLT_MAX. The constraint will be driven to 0 for this axis. + /// + /// Free movement over an axis is allowed when min = -FLT_MAX and max = FLT_MAX. + /// + /// Rotation limit around X-Axis: When limited, should be \f$\in [-\pi, \pi]\f$. Can be asymmetric around zero. + /// + /// Rotation limit around Y-Z Axis: Forms a pyramid or cone shaped limit: + /// * For pyramid, should be \f$\in [-\pi, \pi]\f$ and does not need to be symmetrical around zero. + /// * For cone should be \f$\in [0, \pi]\f$ and needs to be symmetrical around zero (min limit is assumed to be -max limit). + float mLimitMin[EAxis::Num] = { -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX }; + float mLimitMax[EAxis::Num] = { FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX }; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + /// Only soft translation limits are supported, soft rotation limits are not currently supported. + SpringSettings mLimitsSpringSettings[EAxis::NumTranslation]; + + /// Make axis free (unconstrained) + void MakeFreeAxis(EAxis inAxis) { mLimitMin[inAxis] = -FLT_MAX; mLimitMax[inAxis] = FLT_MAX; } + bool IsFreeAxis(EAxis inAxis) const { return mLimitMin[inAxis] == -FLT_MAX && mLimitMax[inAxis] == FLT_MAX; } + + /// Make axis fixed (fixed at value 0) + void MakeFixedAxis(EAxis inAxis) { mLimitMin[inAxis] = FLT_MAX; mLimitMax[inAxis] = -FLT_MAX; } + bool IsFixedAxis(EAxis inAxis) const { return mLimitMin[inAxis] >= mLimitMax[inAxis]; } + + /// Set a valid range for the constraint (if inMax < inMin, the axis will become fixed) + void SetLimitedAxis(EAxis inAxis, float inMin, float inMax) { mLimitMin[inAxis] = inMin; mLimitMax[inAxis] = inMax; } + + /// Motor settings for each axis + MotorSettings mMotorSettings[EAxis::Num]; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// 6 Degree Of Freedom Constraint. Allows control over each of the 6 degrees of freedom. +class JPH_EXPORT SixDOFConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Get Axis from settings class + using EAxis = SixDOFConstraintSettings::EAxis; + + /// Construct six DOF constraint + SixDOFConstraint(Body &inBody1, Body &inBody2, const SixDOFConstraintSettings &inSettings); + + /// Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SixDOF; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); } + + /// Update the translation limits for this constraint + void SetTranslationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax); + + /// Update the rotational limits for this constraint + void SetRotationLimits(Vec3Arg inLimitMin, Vec3Arg inLimitMax); + + /// Get constraint Limits + float GetLimitsMin(EAxis inAxis) const { return mLimitMin[inAxis]; } + float GetLimitsMax(EAxis inAxis) const { return mLimitMax[inAxis]; } + Vec3 GetTranslationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMin[EAxis::TranslationX])); } + Vec3 GetTranslationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMax[EAxis::TranslationX])); } + Vec3 GetRotationLimitsMin() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMin[EAxis::RotationX])); } + Vec3 GetRotationLimitsMax() const { return Vec3::sLoadFloat3Unsafe(*reinterpret_cast(&mLimitMax[EAxis::RotationX])); } + + /// Check which axis are fixed/free + inline bool IsFixedAxis(EAxis inAxis) const { return (mFixedAxis & (1 << inAxis)) != 0; } + inline bool IsFreeAxis(EAxis inAxis) const { return (mFreeAxis & (1 << inAxis)) != 0; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings(EAxis inAxis) const { JPH_ASSERT(inAxis < EAxis::NumTranslation); return mLimitsSpringSettings[inAxis]; } + void SetLimitsSpringSettings(EAxis inAxis, const SpringSettings& inLimitsSpringSettings) { JPH_ASSERT(inAxis < EAxis::NumTranslation); mLimitsSpringSettings[inAxis] = inLimitsSpringSettings; CacheHasSpringLimits(); } + + /// Set the max friction for each axis + void SetMaxFriction(EAxis inAxis, float inFriction); + float GetMaxFriction(EAxis inAxis) const { return mMaxFriction[inAxis]; } + + /// Get rotation of constraint in constraint space + Quat GetRotationInConstraintSpace() const; + + /// Motor settings + MotorSettings & GetMotorSettings(EAxis inAxis) { return mMotorSettings[inAxis]; } + const MotorSettings & GetMotorSettings(EAxis inAxis) const { return mMotorSettings[inAxis]; } + + /// Motor controls. + /// Translation motors work in constraint space of body 1. + /// Rotation motors work in constraint space of body 2 (!). + void SetMotorState(EAxis inAxis, EMotorState inState); + EMotorState GetMotorState(EAxis inAxis) const { return mMotorState[inAxis]; } + + /// Set the target velocity in body 1 constraint space + Vec3 GetTargetVelocityCS() const { return mTargetVelocity; } + void SetTargetVelocityCS(Vec3Arg inVelocity) { mTargetVelocity = inVelocity; } + + /// Set the target angular velocity in body 2 constraint space (!) + void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } + Vec3 GetTargetAngularVelocityCS() const { return mTargetAngularVelocity; } + + /// Set the target position in body 1 constraint space + Vec3 GetTargetPositionCS() const { return mTargetPosition; } + void SetTargetPositionCS(Vec3Arg inPosition) { mTargetPosition = inPosition; } + + /// Set the target orientation in body 1 constraint space + void SetTargetOrientationCS(QuatArg inOrientation); + Quat GetTargetOrientationCS() const { return mTargetOrientation; } + + /// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2). + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q. + void SetTargetOrientationBS(QuatArg inOrientation) { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return IsTranslationFullyConstrained()? mPointConstraintPart.GetTotalLambda() : Vec3(mTranslationConstraintPart[0].GetTotalLambda(), mTranslationConstraintPart[1].GetTotalLambda(), mTranslationConstraintPart[2].GetTotalLambda()); } + inline Vec3 GetTotalLambdaRotation() const { return IsRotationFullyConstrained()? mRotationConstraintPart.GetTotalLambda() : Vec3(mSwingTwistConstraintPart.GetTotalTwistLambda(), mSwingTwistConstraintPart.GetTotalSwingYLambda(), mSwingTwistConstraintPart.GetTotalSwingZLambda()); } + inline Vec3 GetTotalLambdaMotorTranslation() const { return Vec3(mMotorTranslationConstraintPart[0].GetTotalLambda(), mMotorTranslationConstraintPart[1].GetTotalLambda(), mMotorTranslationConstraintPart[2].GetTotalLambda()); } + inline Vec3 GetTotalLambdaMotorRotation() const { return Vec3(mMotorRotationConstraintPart[0].GetTotalLambda(), mMotorRotationConstraintPart[1].GetTotalLambda(), mMotorRotationConstraintPart[2].GetTotalLambda()); } + +private: + // Calculate properties needed for the position constraint + inline void GetPositionConstraintProperties(Vec3 &outR1PlusU, Vec3 &outR2, Vec3 &outU) const; + + // Sanitize the translation limits + inline void UpdateTranslationLimits(); + + // Propagate the rotation limits to the constraint part + inline void UpdateRotationLimits(); + + // Update the cached state of which axis are free and which ones are fixed + inline void UpdateFixedFreeAxis(); + + // Cache the state of mTranslationMotorActive + void CacheTranslationMotorActive(); + + // Cache the state of mRotationMotorActive + void CacheRotationMotorActive(); + + // Cache the state of mRotationPositionMotorActive + void CacheRotationPositionMotorActive(); + + /// Cache the state of mHasSpringLimits + void CacheHasSpringLimits(); + + // Constraint settings helper functions + inline bool IsTranslationConstrained() const { return (mFreeAxis & 0b111) != 0b111; } + inline bool IsTranslationFullyConstrained() const { return (mFixedAxis & 0b111) == 0b111 && !mHasSpringLimits; } + inline bool IsRotationConstrained() const { return (mFreeAxis & 0b111000) != 0b111000; } + inline bool IsRotationFullyConstrained() const { return (mFixedAxis & 0b111000) == 0b111000; } + inline bool HasFriction(EAxis inAxis) const { return !IsFixedAxis(inAxis) && mMaxFriction[inAxis] > 0.0f; } + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Transforms from constraint space to body space + Quat mConstraintToBody1; + Quat mConstraintToBody2; + + // Limits + uint8 mFreeAxis = 0; // Bitmask of free axis (bit 0 = TranslationX) + uint8 mFixedAxis = 0; // Bitmask of fixed axis (bit 0 = TranslationX) + bool mTranslationMotorActive = false; // If any of the translational frictions / motors are active + bool mRotationMotorActive = false; // If any of the rotational frictions / motors are active + uint8 mRotationPositionMotorActive = 0; // Bitmask of axis that have position motor active (bit 0 = RotationX) + bool mHasSpringLimits = false; // If any of the limit springs have a non-zero frequency/stiffness + float mLimitMin[EAxis::Num]; + float mLimitMax[EAxis::Num]; + SpringSettings mLimitsSpringSettings[EAxis::NumTranslation]; + + // Motor settings for each axis + MotorSettings mMotorSettings[EAxis::Num]; + + // Friction settings for each axis + float mMaxFriction[EAxis::Num]; + + // Motor controls + EMotorState mMotorState[EAxis::Num] = { EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off, EMotorState::Off }; + Vec3 mTargetVelocity = Vec3::sZero(); + Vec3 mTargetAngularVelocity = Vec3::sZero(); + Vec3 mTargetPosition = Vec3::sZero(); + Quat mTargetOrientation = Quat::sIdentity(); + + // RUN TIME PROPERTIES FOLLOW + + // Constraint space axis in world space + Vec3 mTranslationAxis[3]; + Vec3 mRotationAxis[3]; + + // Translation displacement (valid when translation axis has a range limit) + float mDisplacement[3]; + + // Individual constraint parts for translation, or a combined point constraint part if all axis are fixed + AxisConstraintPart mTranslationConstraintPart[3]; + PointConstraintPart mPointConstraintPart; + + // Individual constraint parts for rotation or a combined constraint part if rotation is fixed + SwingTwistConstraintPart mSwingTwistConstraintPart; + RotationEulerConstraintPart mRotationConstraintPart; + + // Motor or friction constraints + AxisConstraintPart mMotorTranslationConstraintPart[3]; + AngleConstraintPart mMotorRotationConstraintPart[3]; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SliderConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/SliderConstraint.cpp new file mode 100644 index 000000000..75335c32c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SliderConstraint.cpp @@ -0,0 +1,501 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace literals; + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SliderConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SliderConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SliderConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mAutoDetectPoint) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis1) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mPoint2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mSliderAxis2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mNormalAxis2) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMin) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mLimitsMax) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mMode, "mSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mFrequency, "mFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(SliderConstraintSettings, mLimitsSpringSettings.mDamping, "mDamping") + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMaxFrictionForce) + JPH_ADD_ATTRIBUTE(SliderConstraintSettings, mMotorSettings) +} + +void SliderConstraintSettings::SetSliderAxis(Vec3Arg inSliderAxis) +{ + JPH_ASSERT(mSpace == EConstraintSpace::WorldSpace); + + mSliderAxis1 = mSliderAxis2 = inSliderAxis; + mNormalAxis1 = mNormalAxis2 = inSliderAxis.GetNormalizedPerpendicular(); +} + +void SliderConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mAutoDetectPoint); + inStream.Write(mPoint1); + inStream.Write(mSliderAxis1); + inStream.Write(mNormalAxis1); + inStream.Write(mPoint2); + inStream.Write(mSliderAxis2); + inStream.Write(mNormalAxis2); + inStream.Write(mLimitsMin); + inStream.Write(mLimitsMax); + inStream.Write(mMaxFrictionForce); + mLimitsSpringSettings.SaveBinaryState(inStream); + mMotorSettings.SaveBinaryState(inStream); +} + +void SliderConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mAutoDetectPoint); + inStream.Read(mPoint1); + inStream.Read(mSliderAxis1); + inStream.Read(mNormalAxis1); + inStream.Read(mPoint2); + inStream.Read(mSliderAxis2); + inStream.Read(mNormalAxis2); + inStream.Read(mLimitsMin); + inStream.Read(mLimitsMax); + inStream.Read(mMaxFrictionForce); + mLimitsSpringSettings.RestoreBinaryState(inStream); + mMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SliderConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SliderConstraint(inBody1, inBody2, *this); +} + +SliderConstraint::SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mMaxFrictionForce(inSettings.mMaxFrictionForce), + mMotorSettings(inSettings.mMotorSettings) +{ + // Store inverse of initial rotation from body 1 to body 2 in body 1 space + mInvInitialOrientation = RotationEulerConstraintPart::sGetInvInitialOrientationXY(inSettings.mSliderAxis1, inSettings.mNormalAxis1, inSettings.mSliderAxis2, inSettings.mNormalAxis2); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + RMat44 inv_transform1 = inBody1.GetInverseCenterOfMassTransform(); + RMat44 inv_transform2 = inBody2.GetInverseCenterOfMassTransform(); + + if (inSettings.mAutoDetectPoint) + { + // Determine anchor point: If any of the bodies can never be dynamic use the other body as anchor point + RVec3 anchor; + if (!inBody1.CanBeKinematicOrDynamic()) + anchor = inBody2.GetCenterOfMassPosition(); + else if (!inBody2.CanBeKinematicOrDynamic()) + anchor = inBody1.GetCenterOfMassPosition(); + else + { + // Otherwise use weighted anchor point towards the lightest body + Real inv_m1 = Real(inBody1.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real inv_m2 = Real(inBody2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked()); + Real total_inv_mass = inv_m1 + inv_m2; + if (total_inv_mass != 0.0_r) + anchor = (inv_m1 * inBody1.GetCenterOfMassPosition() + inv_m2 * inBody2.GetCenterOfMassPosition()) / total_inv_mass; + else + anchor = inBody1.GetCenterOfMassPosition(); + } + + // Store local positions + mLocalSpacePosition1 = Vec3(inv_transform1 * anchor); + mLocalSpacePosition2 = Vec3(inv_transform2 * anchor); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inv_transform1 * inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inv_transform2 * inSettings.mPoint2); + } + + // If all properties were specified in world space, take them to local space now + mLocalSpaceSliderAxis1 = inv_transform1.Multiply3x3(inSettings.mSliderAxis1).Normalized(); + mLocalSpaceNormal1 = inv_transform1.Multiply3x3(inSettings.mNormalAxis1).Normalized(); + + // Constraints were specified in world space, so we should have replaced c1 with q10^-1 c1 and c2 with q20^-1 c2 + // => r0^-1 = (q20^-1 c2) (q10^-1 c1)^1 = q20^-1 (c2 c1^-1) q10 + mInvInitialOrientation = inBody2.GetRotation().Conjugated() * mInvInitialOrientation * inBody1.GetRotation(); + } + else + { + // Store local positions + mLocalSpacePosition1 = Vec3(inSettings.mPoint1); + mLocalSpacePosition2 = Vec3(inSettings.mPoint2); + + // Store local space axis + mLocalSpaceSliderAxis1 = inSettings.mSliderAxis1; + mLocalSpaceNormal1 = inSettings.mNormalAxis1; + } + + // Calculate 2nd local space normal + mLocalSpaceNormal2 = mLocalSpaceSliderAxis1.Cross(mLocalSpaceNormal1); + + // Store limits + JPH_ASSERT(inSettings.mLimitsMin != inSettings.mLimitsMax || inSettings.mLimitsSpringSettings.mFrequency > 0.0f, "Better use a fixed constraint"); + SetLimits(inSettings.mLimitsMin, inSettings.mLimitsMax); + + // Store spring settings + SetLimitsSpringSettings(inSettings.mLimitsSpringSettings); +} + +void SliderConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +float SliderConstraint::GetCurrentPosition() const +{ + // See: CalculateR1R2U and CalculateSlidingAxisAndPosition + Vec3 r1 = mBody1->GetRotation() * mLocalSpacePosition1; + Vec3 r2 = mBody2->GetRotation() * mLocalSpacePosition2; + Vec3 u = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + r2 - r1; + return u.Dot(mBody1->GetRotation() * mLocalSpaceSliderAxis1); +} + +void SliderConstraint::SetLimits(float inLimitsMin, float inLimitsMax) +{ + JPH_ASSERT(inLimitsMin <= 0.0f); + JPH_ASSERT(inLimitsMax >= 0.0f); + mLimitsMin = inLimitsMin; + mLimitsMax = inLimitsMax; + mHasLimits = mLimitsMin != -FLT_MAX || mLimitsMax != FLT_MAX; +} + +void SliderConstraint::CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate points relative to body + mR1 = inRotation1 * mLocalSpacePosition1; + mR2 = inRotation2 * mLocalSpacePosition2; + + // Calculate X2 + R2 - X1 - R1 + mU = Vec3(mBody2->GetCenterOfMassPosition() - mBody1->GetCenterOfMassPosition()) + mR2 - mR1; +} + +void SliderConstraint::CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2) +{ + // Calculate world space normals + mN1 = inRotation1 * mLocalSpaceNormal1; + mN2 = inRotation1 * mLocalSpaceNormal2; + + mPositionConstraintPart.CalculateConstraintProperties(*mBody1, inRotation1, mR1 + mU, *mBody2, inRotation2, mR2, mN1, mN2); +} + +void SliderConstraint::CalculateSlidingAxisAndPosition(Mat44Arg inRotation1) +{ + if (mHasLimits || mMotorState != EMotorState::Off || mMaxFrictionForce > 0.0f) + { + // Calculate world space slider axis + mWorldSpaceSliderAxis = inRotation1 * mLocalSpaceSliderAxis1; + + // Calculate slide distance along axis + mD = mU.Dot(mWorldSpaceSliderAxis); + } +} + +void SliderConstraint::CalculatePositionLimitsConstraintProperties(float inDeltaTime) +{ + // Check if distance is within limits + bool below_min = mD <= mLimitsMin; + if (mHasLimits && (below_min || mD >= mLimitsMax)) + mPositionLimitsConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - (below_min? mLimitsMin : mLimitsMax), mLimitsSpringSettings); + else + mPositionLimitsConstraintPart.Deactivate(); +} + +void SliderConstraint::CalculateMotorConstraintProperties(float inDeltaTime) +{ + switch (mMotorState) + { + case EMotorState::Off: + if (mMaxFrictionForce > 0.0f) + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis); + else + mMotorConstraintPart.Deactivate(); + break; + + case EMotorState::Velocity: + mMotorConstraintPart.CalculateConstraintProperties(*mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, -mTargetVelocity); + break; + + case EMotorState::Position: + if (mMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart.CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, mR1 + mU, *mBody2, mR2, mWorldSpaceSliderAxis, 0.0f, mD - mTargetPosition, mMotorSettings.mSpringSettings); + else + mMotorConstraintPart.Deactivate(); + break; + } +} + +void SliderConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Calculate constraint properties that are constant while bodies don't move + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculatePositionConstraintProperties(rotation1, rotation2); + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, *mBody2, rotation2); + CalculateSlidingAxisAndPosition(rotation1); + CalculatePositionLimitsConstraintProperties(inDeltaTime); + CalculateMotorConstraintProperties(inDeltaTime); +} + +void SliderConstraint::ResetWarmStart() +{ + mMotorConstraintPart.Deactivate(); + mPositionConstraintPart.Deactivate(); + mRotationConstraintPart.Deactivate(); + mPositionLimitsConstraintPart.Deactivate(); +} + +void SliderConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + mMotorConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio); + mPositionConstraintPart.WarmStart(*mBody1, *mBody2, mN1, mN2, inWarmStartImpulseRatio); + mRotationConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPositionLimitsConstraintPart.WarmStart(*mBody1, *mBody2, mWorldSpaceSliderAxis, inWarmStartImpulseRatio); +} + +bool SliderConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + // Solve motor + bool motor = false; + if (mMotorConstraintPart.IsActive()) + { + switch (mMotorState) + { + case EMotorState::Off: + { + float max_lambda = mMaxFrictionForce * inDeltaTime; + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, -max_lambda, max_lambda); + break; + } + + case EMotorState::Velocity: + case EMotorState::Position: + motor = mMotorConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, inDeltaTime * mMotorSettings.mMinForceLimit, inDeltaTime * mMotorSettings.mMaxForceLimit); + break; + } + } + + // Solve position constraint along 2 axis + bool pos = mPositionConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mN1, mN2); + + // Solve rotation constraint + bool rot = mRotationConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve limits along slider axis + bool limit = false; + if (mPositionLimitsConstraintPart.IsActive()) + { + float min_lambda, max_lambda; + if (mLimitsMin == mLimitsMax) + { + min_lambda = -FLT_MAX; + max_lambda = FLT_MAX; + } + else if (mD <= mLimitsMin) + { + min_lambda = 0.0f; + max_lambda = FLT_MAX; + } + else + { + min_lambda = -FLT_MAX; + max_lambda = 0.0f; + } + limit = mPositionLimitsConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, min_lambda, max_lambda); + } + + return motor || pos || rot || limit; +} + +bool SliderConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + // Motor operates on velocities only, don't call SolvePositionConstraint + + // Solve position constraint along 2 axis + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculatePositionConstraintProperties(rotation1, rotation2); + bool pos = mPositionConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mU, mN1, mN2, inBaumgarte); + + // Solve rotation constraint + mRotationConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), *mBody2, Mat44::sRotation(mBody2->GetRotation())); + bool rot = mRotationConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mInvInitialOrientation, inBaumgarte); + + // Solve limits along slider axis + bool limit = false; + if (mHasLimits && mLimitsSpringSettings.mFrequency <= 0.0f) + { + rotation1 = Mat44::sRotation(mBody1->GetRotation()); + rotation2 = Mat44::sRotation(mBody2->GetRotation()); + CalculateR1R2U(rotation1, rotation2); + CalculateSlidingAxisAndPosition(rotation1); + CalculatePositionLimitsConstraintProperties(inDeltaTime); + if (mPositionLimitsConstraintPart.IsActive()) + { + if (mD <= mLimitsMin) + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMin, inBaumgarte); + else + { + JPH_ASSERT(mD >= mLimitsMax); + limit = mPositionLimitsConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, mWorldSpaceSliderAxis, mD - mLimitsMax, inBaumgarte); + } + } + } + + return pos || rot || limit; +} + +#ifdef JPH_DEBUG_RENDERER +void SliderConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Transform the local positions into world space + Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1); + RVec3 position1 = transform1 * mLocalSpacePosition1; + RVec3 position2 = transform2 * mLocalSpacePosition2; + + // Draw constraint + inRenderer->DrawMarker(position1, Color::sRed, 0.1f); + inRenderer->DrawMarker(position2, Color::sGreen, 0.1f); + inRenderer->DrawLine(position1, position2, Color::sGreen); + + // Draw motor + switch (mMotorState) + { + case EMotorState::Position: + inRenderer->DrawMarker(position1 + mTargetPosition * slider_axis, Color::sYellow, 1.0f); + break; + + case EMotorState::Velocity: + { + Vec3 cur_vel = (mBody2->GetLinearVelocity() - mBody1->GetLinearVelocity()).Dot(slider_axis) * slider_axis; + inRenderer->DrawLine(position2, position2 + cur_vel, Color::sBlue); + inRenderer->DrawArrow(position2 + cur_vel, position2 + mTargetVelocity * slider_axis, Color::sRed, 0.1f); + break; + } + + case EMotorState::Off: + break; + } +} + +void SliderConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + if (mHasLimits) + { + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform(); + + // Transform the local positions into world space + Vec3 slider_axis = transform1.Multiply3x3(mLocalSpaceSliderAxis1); + RVec3 position1 = transform1 * mLocalSpacePosition1; + RVec3 position2 = transform2 * mLocalSpacePosition2; + + // Calculate the limits in world space + RVec3 limits_min = position1 + mLimitsMin * slider_axis; + RVec3 limits_max = position1 + mLimitsMax * slider_axis; + + inRenderer->DrawLine(limits_min, position1, Color::sWhite); + inRenderer->DrawLine(position2, limits_max, Color::sWhite); + + inRenderer->DrawMarker(limits_min, Color::sWhite, 0.1f); + inRenderer->DrawMarker(limits_max, Color::sWhite, 0.1f); + } +} +#endif // JPH_DEBUG_RENDERER + +void SliderConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mMotorConstraintPart.SaveState(inStream); + mPositionConstraintPart.SaveState(inStream); + mRotationConstraintPart.SaveState(inStream); + mPositionLimitsConstraintPart.SaveState(inStream); + + inStream.Write(mMotorState); + inStream.Write(mTargetVelocity); + inStream.Write(mTargetPosition); +} + +void SliderConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mMotorConstraintPart.RestoreState(inStream); + mPositionConstraintPart.RestoreState(inStream); + mRotationConstraintPart.RestoreState(inStream); + mPositionLimitsConstraintPart.RestoreState(inStream); + + inStream.Read(mMotorState); + inStream.Read(mTargetVelocity); + inStream.Read(mTargetPosition); +} + +Ref SliderConstraint::GetConstraintSettings() const +{ + SliderConstraintSettings *settings = new SliderConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPoint1 = RVec3(mLocalSpacePosition1); + settings->mSliderAxis1 = mLocalSpaceSliderAxis1; + settings->mNormalAxis1 = mLocalSpaceNormal1; + settings->mPoint2 = RVec3(mLocalSpacePosition2); + Mat44 inv_initial_rotation = Mat44::sRotation(mInvInitialOrientation); + settings->mSliderAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceSliderAxis1); + settings->mNormalAxis2 = inv_initial_rotation.Multiply3x3(mLocalSpaceNormal1); + settings->mLimitsMin = mLimitsMin; + settings->mLimitsMax = mLimitsMax; + settings->mLimitsSpringSettings = mLimitsSpringSettings; + settings->mMaxFrictionForce = mMaxFrictionForce; + settings->mMotorSettings = mMotorSettings; + return settings; +} + +Mat44 SliderConstraint::GetConstraintToBody1Matrix() const +{ + return Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(mLocalSpacePosition1, 1)); +} + +Mat44 SliderConstraint::GetConstraintToBody2Matrix() const +{ + Mat44 mat = Mat44::sRotation(mInvInitialOrientation).Multiply3x3(Mat44(Vec4(mLocalSpaceSliderAxis1, 0), Vec4(mLocalSpaceNormal1, 0), Vec4(mLocalSpaceNormal2, 0), Vec4(0, 0, 0, 1))); + mat.SetTranslation(mLocalSpacePosition2); + return mat; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SliderConstraint.h b/WickedEngine/Jolt/Physics/Constraints/SliderConstraint.h new file mode 100644 index 000000000..2838517f2 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SliderConstraint.h @@ -0,0 +1,198 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Slider constraint settings, used to create a slider constraint +class JPH_EXPORT SliderConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SliderConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint. + /// Note that the rotation constraint will be solved from body 1. This means that if body 1 and body 2 have different masses / inertias (kinematic body = infinite mass / inertia), body 1 should be the heaviest body. + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// Simple way of setting the slider and normal axis in world space (assumes the bodies are already oriented correctly when the constraint is created) + void SetSliderAxis(Vec3Arg inSliderAxis); + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + /// When mSpace is WorldSpace mPoint1 and mPoint2 can be automatically calculated based on the positions of the bodies when the constraint is created (the current relative position/orientation is chosen as the '0' position). Set this to false if you want to supply the attachment points yourself. + bool mAutoDetectPoint = false; + + /// Body 1 constraint reference frame (space determined by mSpace). + /// Slider axis is the axis along which movement is possible (direction), normal axis is a perpendicular vector to define the frame. + RVec3 mPoint1 = RVec3::sZero(); + Vec3 mSliderAxis1 = Vec3::sAxisX(); + Vec3 mNormalAxis1 = Vec3::sAxisY(); + + /// Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPoint2 = RVec3::sZero(); + Vec3 mSliderAxis2 = Vec3::sAxisX(); + Vec3 mNormalAxis2 = Vec3::sAxisY(); + + /// When the bodies move so that mPoint1 coincides with mPoint2 the slider position is defined to be 0, movement will be limited between [mLimitsMin, mLimitsMax] where mLimitsMin e [-inf, 0] and mLimitsMax e [0, inf] + float mLimitsMin = -FLT_MAX; + float mLimitsMax = FLT_MAX; + + /// When enabled, this makes the limits soft. When the constraint exceeds the limits, a spring force will pull it back. + SpringSettings mLimitsSpringSettings; + + /// Maximum amount of friction force to apply (N) when not driven by a motor. + float mMaxFrictionForce = 0.0f; + + /// In case the constraint is powered, this determines the motor settings around the sliding axis + MotorSettings mMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A slider constraint allows movement in only 1 axis (and no rotation). Also known as a prismatic constraint. +class JPH_EXPORT SliderConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct slider constraint + SliderConstraint(Body &inBody1, Body &inBody2, const SliderConstraintSettings &inSettings); + + // Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Slider; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override; + virtual Mat44 GetConstraintToBody2Matrix() const override; + + /// Get the current distance from the rest position + float GetCurrentPosition() const; + + /// Friction control + void SetMaxFrictionForce(float inFrictionForce) { mMaxFrictionForce = inFrictionForce; } + float GetMaxFrictionForce() const { return mMaxFrictionForce; } + + /// Motor settings + MotorSettings & GetMotorSettings() { return mMotorSettings; } + const MotorSettings & GetMotorSettings() const { return mMotorSettings; } + + // Motor controls + void SetMotorState(EMotorState inState) { JPH_ASSERT(inState == EMotorState::Off || mMotorSettings.IsValid()); mMotorState = inState; } + EMotorState GetMotorState() const { return mMotorState; } + void SetTargetVelocity(float inVelocity) { mTargetVelocity = inVelocity; } + float GetTargetVelocity() const { return mTargetVelocity; } + void SetTargetPosition(float inPosition) { mTargetPosition = mHasLimits? Clamp(inPosition, mLimitsMin, mLimitsMax) : inPosition; } + float GetTargetPosition() const { return mTargetPosition; } + + /// Update the limits of the slider constraint (see SliderConstraintSettings) + void SetLimits(float inLimitsMin, float inLimitsMax); + float GetLimitsMin() const { return mLimitsMin; } + float GetLimitsMax() const { return mLimitsMax; } + bool HasLimits() const { return mHasLimits; } + + /// Update the limits spring settings + const SpringSettings & GetLimitsSpringSettings() const { return mLimitsSpringSettings; } + SpringSettings & GetLimitsSpringSettings() { return mLimitsSpringSettings; } + void SetLimitsSpringSettings(const SpringSettings &inLimitsSpringSettings) { mLimitsSpringSettings = inLimitsSpringSettings; } + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vector<2> GetTotalLambdaPosition() const { return mPositionConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaPositionLimits() const { return mPositionLimitsConstraintPart.GetTotalLambda(); } + inline Vec3 GetTotalLambdaRotation() const { return mRotationConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaMotor() const { return mMotorConstraintPart.GetTotalLambda(); } + +private: + // Internal helper function to calculate the values below + void CalculateR1R2U(Mat44Arg inRotation1, Mat44Arg inRotation2); + void CalculateSlidingAxisAndPosition(Mat44Arg inRotation1); + void CalculatePositionConstraintProperties(Mat44Arg inRotation1, Mat44Arg inRotation2); + void CalculatePositionLimitsConstraintProperties(float inDeltaTime); + void CalculateMotorConstraintProperties(float inDeltaTime); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Local space sliding direction + Vec3 mLocalSpaceSliderAxis1; + + // Local space normals to the sliding direction (in body 1 space) + Vec3 mLocalSpaceNormal1; + Vec3 mLocalSpaceNormal2; + + // Inverse of initial rotation from body 1 to body 2 in body 1 space + Quat mInvInitialOrientation; + + // Slider limits + bool mHasLimits; + float mLimitsMin; + float mLimitsMax; + + // Soft constraint limits + SpringSettings mLimitsSpringSettings; + + // Friction + float mMaxFrictionForce; + + // Motor controls + MotorSettings mMotorSettings; + EMotorState mMotorState = EMotorState::Off; + float mTargetVelocity = 0.0f; + float mTargetPosition = 0.0f; + + // RUN TIME PROPERTIES FOLLOW + + // Positions where the point constraint acts on (middle point between center of masses) + Vec3 mR1; + Vec3 mR2; + + // X2 + R2 - X1 - R1 + Vec3 mU; + + // World space sliding direction + Vec3 mWorldSpaceSliderAxis; + + // Normals to the slider axis + Vec3 mN1; + Vec3 mN2; + + // Distance along the slide axis + float mD = 0.0f; + + // The constraint parts + DualAxisConstraintPart mPositionConstraintPart; + RotationEulerConstraintPart mRotationConstraintPart; + AxisConstraintPart mPositionLimitsConstraintPart; + AxisConstraintPart mMotorConstraintPart; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SpringSettings.cpp b/WickedEngine/Jolt/Physics/Constraints/SpringSettings.cpp new file mode 100644 index 000000000..c2c32400f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SpringSettings.cpp @@ -0,0 +1,35 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SpringSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE(SpringSettings, mMode) + JPH_ADD_ATTRIBUTE(SpringSettings, mFrequency) + JPH_ADD_ATTRIBUTE(SpringSettings, mDamping) +} + +void SpringSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMode); + inStream.Write(mFrequency); + inStream.Write(mDamping); +} + +void SpringSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMode); + inStream.Read(mFrequency); + inStream.Read(mDamping); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SpringSettings.h b/WickedEngine/Jolt/Physics/Constraints/SpringSettings.h new file mode 100644 index 000000000..b2f6b7e24 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SpringSettings.h @@ -0,0 +1,70 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Enum used by constraints to specify how the spring is defined +enum class ESpringMode : uint8 +{ + FrequencyAndDamping, ///< Frequency and damping are specified + StiffnessAndDamping, ///< Stiffness and damping are specified +}; + +/// Settings for a linear or angular spring +class JPH_EXPORT SpringSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SpringSettings) + + /// Constructor + SpringSettings() = default; + SpringSettings(const SpringSettings &) = default; + SpringSettings & operator = (const SpringSettings &) = default; + SpringSettings(ESpringMode inMode, float inFrequencyOrStiffness, float inDamping) : mMode(inMode), mFrequency(inFrequencyOrStiffness), mDamping(inDamping) { } + + /// Saves the contents of the spring settings in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores contents from the binary stream inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Check if the spring has a valid frequency / stiffness, if not the spring will be hard + inline bool HasStiffness() const { return mFrequency > 0.0f; } + + /// Selects the way in which the spring is defined + /// If the mode is StiffnessAndDamping then mFrequency becomes the stiffness (k) and mDamping becomes the damping ratio (c) in the spring equation F = -k * x - c * v. Otherwise the properties are as documented. + ESpringMode mMode = ESpringMode::FrequencyAndDamping; + + union + { + /// Valid when mSpringMode = ESpringMode::FrequencyAndDamping. + /// If mFrequency > 0 the constraint will be soft and mFrequency specifies the oscillation frequency in Hz. + /// If mFrequency <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows). + float mFrequency = 0.0f; + + /// Valid when mSpringMode = ESpringMode::StiffnessAndDamping. + /// If mStiffness > 0 the constraint will be soft and mStiffness specifies the stiffness (k) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring. + /// If mStiffness <= 0, mDamping is ignored and the constraint will have hard limits (as hard as the time step / the number of velocity / position solver steps allows). + /// + /// Note that stiffness values are large numbers. To calculate a ballpark value for the needed stiffness you can use: + /// force = stiffness * delta_spring_length = mass * gravity <=> stiffness = mass * gravity / delta_spring_length. + /// So if your object weighs 1500 kg and the spring compresses by 2 meters, you need a stiffness in the order of 1500 * 9.81 / 2 ~ 7500 N/m. + float mStiffness; + }; + + /// When mSpringMode = ESpringMode::FrequencyAndDamping mDamping is the damping ratio (0 = no damping, 1 = critical damping). + /// When mSpringMode = ESpringMode::StiffnessAndDamping mDamping is the damping (c) in the spring equation F = -k * x - c * v for a linear or T = -k * theta - c * w for an angular spring. + /// Note that if you set mDamping = 0, you will not get an infinite oscillation. Because we integrate physics using an explicit Euler scheme, there is always energy loss. + /// This is done to keep the simulation from exploding, because with a damping of 0 and even the slightest rounding error, the oscillation could become bigger and bigger until the simulation explodes. + float mDamping = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.cpp new file mode 100644 index 000000000..bcd74ff30 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.cpp @@ -0,0 +1,524 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(SwingTwistConstraintSettings) +{ + JPH_ADD_BASE_CLASS(SwingTwistConstraintSettings, TwoBodyConstraintSettings) + + JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSpace) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis1) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPosition2) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistAxis2) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneAxis2) + JPH_ADD_ENUM_ATTRIBUTE(SwingTwistConstraintSettings, mSwingType) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mNormalHalfConeAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mPlaneHalfConeAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMinAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMaxAngle) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mMaxFrictionTorque) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mSwingMotorSettings) + JPH_ADD_ATTRIBUTE(SwingTwistConstraintSettings, mTwistMotorSettings) +} + +void SwingTwistConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mSpace); + inStream.Write(mPosition1); + inStream.Write(mTwistAxis1); + inStream.Write(mPlaneAxis1); + inStream.Write(mPosition2); + inStream.Write(mTwistAxis2); + inStream.Write(mPlaneAxis2); + inStream.Write(mSwingType); + inStream.Write(mNormalHalfConeAngle); + inStream.Write(mPlaneHalfConeAngle); + inStream.Write(mTwistMinAngle); + inStream.Write(mTwistMaxAngle); + inStream.Write(mMaxFrictionTorque); + mSwingMotorSettings.SaveBinaryState(inStream); + mTwistMotorSettings.SaveBinaryState(inStream); +} + +void SwingTwistConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mSpace); + inStream.Read(mPosition1); + inStream.Read(mTwistAxis1); + inStream.Read(mPlaneAxis1); + inStream.Read(mPosition2); + inStream.Read(mTwistAxis2); + inStream.Read(mPlaneAxis2); + inStream.Read(mSwingType); + inStream.Read(mNormalHalfConeAngle); + inStream.Read(mPlaneHalfConeAngle); + inStream.Read(mTwistMinAngle); + inStream.Read(mTwistMaxAngle); + inStream.Read(mMaxFrictionTorque); + mSwingMotorSettings.RestoreBinaryState(inStream); + mTwistMotorSettings.RestoreBinaryState(inStream); +} + +TwoBodyConstraint *SwingTwistConstraintSettings::Create(Body &inBody1, Body &inBody2) const +{ + return new SwingTwistConstraint(inBody1, inBody2, *this); +} + +void SwingTwistConstraint::UpdateLimits() +{ + // Pass limits on to swing twist constraint part + mSwingTwistConstraintPart.SetLimits(mTwistMinAngle, mTwistMaxAngle, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle); +} + +SwingTwistConstraint::SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings) : + TwoBodyConstraint(inBody1, inBody2, inSettings), + mNormalHalfConeAngle(inSettings.mNormalHalfConeAngle), + mPlaneHalfConeAngle(inSettings.mPlaneHalfConeAngle), + mTwistMinAngle(inSettings.mTwistMinAngle), + mTwistMaxAngle(inSettings.mTwistMaxAngle), + mMaxFrictionTorque(inSettings.mMaxFrictionTorque), + mSwingMotorSettings(inSettings.mSwingMotorSettings), + mTwistMotorSettings(inSettings.mTwistMotorSettings) +{ + // Override swing type + mSwingTwistConstraintPart.SetSwingType(inSettings.mSwingType); + + // Calculate rotation needed to go from constraint space to body1 local space + Vec3 normal_axis1 = inSettings.mPlaneAxis1.Cross(inSettings.mTwistAxis1); + Mat44 c_to_b1(Vec4(inSettings.mTwistAxis1, 0), Vec4(normal_axis1, 0), Vec4(inSettings.mPlaneAxis1, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody1 = c_to_b1.GetQuaternion(); + + // Calculate rotation needed to go from constraint space to body2 local space + Vec3 normal_axis2 = inSettings.mPlaneAxis2.Cross(inSettings.mTwistAxis2); + Mat44 c_to_b2(Vec4(inSettings.mTwistAxis2, 0), Vec4(normal_axis2, 0), Vec4(inSettings.mPlaneAxis2, 0), Vec4(0, 0, 0, 1)); + mConstraintToBody2 = c_to_b2.GetQuaternion(); + + if (inSettings.mSpace == EConstraintSpace::WorldSpace) + { + // If all properties were specified in world space, take them to local space now + mLocalSpacePosition1 = Vec3(inBody1.GetInverseCenterOfMassTransform() * inSettings.mPosition1); + mConstraintToBody1 = inBody1.GetRotation().Conjugated() * mConstraintToBody1; + + mLocalSpacePosition2 = Vec3(inBody2.GetInverseCenterOfMassTransform() * inSettings.mPosition2); + mConstraintToBody2 = inBody2.GetRotation().Conjugated() * mConstraintToBody2; + } + else + { + mLocalSpacePosition1 = Vec3(inSettings.mPosition1); + mLocalSpacePosition2 = Vec3(inSettings.mPosition2); + } + + UpdateLimits(); +} + +void SwingTwistConstraint::NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) +{ + if (mBody1->GetID() == inBodyID) + mLocalSpacePosition1 -= inDeltaCOM; + else if (mBody2->GetID() == inBodyID) + mLocalSpacePosition2 -= inDeltaCOM; +} + +Quat SwingTwistConstraint::GetRotationInConstraintSpace() const +{ + // Let b1, b2 be the center of mass transform of body1 and body2 (For body1 this is mBody1->GetCenterOfMassTransform()) + // Let c1, c2 be the transform that takes a vector from constraint space to local space of body1 and body2 (For body1 this is Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1)) + // Let q be the rotation of the constraint in constraint space + // b2 takes a vector from the local space of body2 to world space + // To express this in terms of b1: b2 = b1 * c1 * q * c2^-1 + // c2^-1 goes from local body 2 space to constraint space + // q rotates the constraint + // c1 goes from constraint space to body 1 local space + // b1 goes from body 1 local space to world space + // So when the body rotations are given, q = (b1 * c1)^-1 * b2 c2 + // Or: q = (q1 * c1)^-1 * (q2 * c2) if we're only interested in rotations + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + return constraint_body1_to_world.Conjugated() * constraint_body2_to_world; +} + +void SwingTwistConstraint::SetSwingMotorState(EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mSwingMotorSettings.IsValid()); + + if (mSwingMotorState != inState) + { + mSwingMotorState = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } +} + +void SwingTwistConstraint::SetTwistMotorState(EMotorState inState) +{ + JPH_ASSERT(inState == EMotorState::Off || mTwistMotorSettings.IsValid()); + + if (mTwistMotorState != inState) + { + mTwistMotorState = inState; + + // Ensure that warm starting next frame doesn't apply any impulses (motor parts are repurposed for different modes) + mMotorConstraintPart[0].Deactivate(); + } +} + +void SwingTwistConstraint::SetTargetOrientationCS(QuatArg inOrientation) +{ + Quat q_swing, q_twist; + inOrientation.GetSwingTwist(q_swing, q_twist); + + uint clamped_axis; + mSwingTwistConstraintPart.ClampSwingTwist(q_swing, q_twist, clamped_axis); + + if (clamped_axis != 0) + mTargetOrientation = q_swing * q_twist; + else + mTargetOrientation = inOrientation; +} + +void SwingTwistConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + // Setup point constraint + Mat44 rotation1 = Mat44::sRotation(mBody1->GetRotation()); + Mat44 rotation2 = Mat44::sRotation(mBody2->GetRotation()); + mPointConstraintPart.CalculateConstraintProperties(*mBody1, rotation1, mLocalSpacePosition1, *mBody2, rotation2, mLocalSpacePosition2); + + // GetRotationInConstraintSpace written out since we reuse the sub expressions + Quat constraint_body1_to_world = mBody1->GetRotation() * mConstraintToBody1; + Quat constraint_body2_to_world = mBody2->GetRotation() * mConstraintToBody2; + Quat q = constraint_body1_to_world.Conjugated() * constraint_body2_to_world; + + // Calculate constraint properties for the swing twist limit + mSwingTwistConstraintPart.CalculateConstraintProperties(*mBody1, *mBody2, q, constraint_body1_to_world); + + if (mSwingMotorState != EMotorState::Off || mTwistMotorState != EMotorState::Off || mMaxFrictionTorque > 0.0f) + { + // Calculate rotation motor axis + Mat44 ws_axis = Mat44::sRotation(constraint_body2_to_world); + for (int i = 0; i < 3; ++i) + mWorldSpaceMotorAxis[i] = ws_axis.GetColumn3(i); + + Vec3 rotation_error; + if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position) + { + // Get target orientation along the shortest path from q + Quat target_orientation = q.Dot(mTargetOrientation) > 0.0f? mTargetOrientation : -mTargetOrientation; + + // The definition of the constraint rotation q: + // R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (1) + // + // R2' is the rotation of body 2 when reaching the target_orientation: + // R2' * ConstraintToBody2 = R1 * ConstraintToBody1 * target_orientation (2) + // + // The difference in body 2 space: + // R2' = R2 * diff_body2 (3) + // + // We want to specify the difference in the constraint space of body 2: + // diff_body2 = ConstraintToBody2 * diff * ConstraintToBody2^* (4) + // + // Extracting R2' from 2: R2' = R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* (5) + // Combining 3 & 4: R2' = R2 * ConstraintToBody2 * diff * ConstraintToBody2^* (6) + // Combining 1 & 6: R2' = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* (7) + // Combining 5 & 7: R1 * ConstraintToBody1 * target_orientation * ConstraintToBody2^* = R1 * ConstraintToBody1 * q * diff * ConstraintToBody2^* + // <=> target_orientation = q * diff + // <=> diff = q^* * target_orientation + Quat diff = q.Conjugated() * target_orientation; + + // Approximate error angles + // The imaginary part of a quaternion is rotation_axis * sin(angle / 2) + // If angle is small, sin(x) = x so angle[i] ~ 2.0f * rotation_axis[i] + // We'll be making small time steps, so if the angle is not small at least the sign will be correct and we'll move in the right direction + rotation_error = -2.0f * diff.GetXYZ(); + } + + // Swing motor + switch (mSwingMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + { + // Enable friction + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f); + } + else + { + // Disable friction + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } + break; + + case EMotorState::Velocity: + // Use motor to create angular velocity around desired axis + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], -mTargetAngularVelocity[i]); + break; + + case EMotorState::Position: + // Use motor to drive rotation error to zero + if (mSwingMotorSettings.mSpringSettings.HasStiffness()) + { + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[i], 0.0f, rotation_error[i], mSwingMotorSettings.mSpringSettings); + } + else + { + for (int i = 1; i < 3; ++i) + mMotorConstraintPart[i].Deactivate(); + } + break; + } + + // Twist motor + switch (mTwistMotorState) + { + case EMotorState::Off: + if (mMaxFrictionTorque > 0.0f) + { + // Enable friction + mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f); + } + else + { + // Disable friction + mMotorConstraintPart[0].Deactivate(); + } + break; + + case EMotorState::Velocity: + // Use motor to create angular velocity around desired axis + mMotorConstraintPart[0].CalculateConstraintProperties(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], -mTargetAngularVelocity[0]); + break; + + case EMotorState::Position: + // Use motor to drive rotation error to zero + if (mTwistMotorSettings.mSpringSettings.HasStiffness()) + mMotorConstraintPart[0].CalculateConstraintPropertiesWithSettings(inDeltaTime, *mBody1, *mBody2, mWorldSpaceMotorAxis[0], 0.0f, rotation_error[0], mTwistMotorSettings.mSpringSettings); + else + mMotorConstraintPart[0].Deactivate(); + break; + } + } + else + { + // Disable rotation motor + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + } +} + +void SwingTwistConstraint::ResetWarmStart() +{ + for (AngleConstraintPart &c : mMotorConstraintPart) + c.Deactivate(); + mSwingTwistConstraintPart.Deactivate(); + mPointConstraintPart.Deactivate(); +} + +void SwingTwistConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + // Warm starting: Apply previous frame impulse + for (AngleConstraintPart &c : mMotorConstraintPart) + c.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mSwingTwistConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); + mPointConstraintPart.WarmStart(*mBody1, *mBody2, inWarmStartImpulseRatio); +} + +bool SwingTwistConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve twist rotation motor + if (mMotorConstraintPart[0].IsActive()) + { + // Twist limits + float min_twist_limit, max_twist_limit; + if (mTwistMotorState == EMotorState::Off) + { + max_twist_limit = inDeltaTime * mMaxFrictionTorque; + min_twist_limit = -max_twist_limit; + } + else + { + min_twist_limit = inDeltaTime * mTwistMotorSettings.mMinTorqueLimit; + max_twist_limit = inDeltaTime * mTwistMotorSettings.mMaxTorqueLimit; + } + + impulse |= mMotorConstraintPart[0].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[0], min_twist_limit, max_twist_limit); + } + + // Solve swing rotation motor + if (mMotorConstraintPart[1].IsActive()) + { + // Swing parts should turn on / off together + JPH_ASSERT(mMotorConstraintPart[2].IsActive()); + + // Swing limits + float min_swing_limit, max_swing_limit; + if (mSwingMotorState == EMotorState::Off) + { + max_swing_limit = inDeltaTime * mMaxFrictionTorque; + min_swing_limit = -max_swing_limit; + } + else + { + min_swing_limit = inDeltaTime * mSwingMotorSettings.mMinTorqueLimit; + max_swing_limit = inDeltaTime * mSwingMotorSettings.mMaxTorqueLimit; + } + + for (int i = 1; i < 3; ++i) + impulse |= mMotorConstraintPart[i].SolveVelocityConstraint(*mBody1, *mBody2, mWorldSpaceMotorAxis[i], min_swing_limit, max_swing_limit); + } + else + { + // Swing parts should turn on / off together + JPH_ASSERT(!mMotorConstraintPart[2].IsActive()); + } + + // Solve rotation limits + impulse |= mSwingTwistConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + // Solve position constraint + impulse |= mPointConstraintPart.SolveVelocityConstraint(*mBody1, *mBody2); + + return impulse; +} + +bool SwingTwistConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + // Solve rotation violations + Quat q = GetRotationInConstraintSpace(); + impulse |= mSwingTwistConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, q, mConstraintToBody1, mConstraintToBody2, inBaumgarte); + + // Solve position violations + mPointConstraintPart.CalculateConstraintProperties(*mBody1, Mat44::sRotation(mBody1->GetRotation()), mLocalSpacePosition1, *mBody2, Mat44::sRotation(mBody2->GetRotation()), mLocalSpacePosition2); + impulse |= mPointConstraintPart.SolvePositionConstraint(*mBody1, *mBody2, inBaumgarte); + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER +void SwingTwistConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + // Get constraint properties in world space + RMat44 transform1 = mBody1->GetCenterOfMassTransform(); + RVec3 position1 = transform1 * mLocalSpacePosition1; + Quat rotation1 = mBody1->GetRotation() * mConstraintToBody1; + Quat rotation2 = mBody2->GetRotation() * mConstraintToBody2; + + // Draw constraint orientation + inRenderer->DrawCoordinateSystem(RMat44::sRotationTranslation(rotation1, position1), mDrawConstraintSize); + + // Draw current swing and twist + Quat q = GetRotationInConstraintSpace(); + Quat q_swing, q_twist; + q.GetSwingTwist(q_swing, q_twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_twist).RotateAxisY(), Color::sWhite); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * q_swing).RotateAxisX(), Color::sWhite); + + if (mSwingMotorState == EMotorState::Velocity || mTwistMotorState == EMotorState::Velocity) + { + // Draw target angular velocity + inRenderer->DrawArrow(position1, position1 + rotation2 * mTargetAngularVelocity, Color::sRed, 0.1f); + } + if (mSwingMotorState == EMotorState::Position || mTwistMotorState == EMotorState::Position) + { + // Draw motor swing and twist + Quat swing, twist; + mTargetOrientation.GetSwingTwist(swing, twist); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * twist).RotateAxisY(), Color::sYellow); + inRenderer->DrawLine(position1, position1 + mDrawConstraintSize * (rotation1 * swing).RotateAxisX(), Color::sCyan); + } +} + +void SwingTwistConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ + // Get matrix that transforms from constraint space to world space + RMat44 constraint_to_world = RMat44::sRotationTranslation(mBody1->GetRotation() * mConstraintToBody1, mBody1->GetCenterOfMassTransform() * mLocalSpacePosition1); + + // Draw limits + if (mSwingTwistConstraintPart.GetSwingType() == ESwingType::Pyramid) + inRenderer->DrawSwingPyramidLimits(constraint_to_world, -mPlaneHalfConeAngle, mPlaneHalfConeAngle, -mNormalHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + else + inRenderer->DrawSwingConeLimits(constraint_to_world, mPlaneHalfConeAngle, mNormalHalfConeAngle, mDrawConstraintSize, Color::sGreen, DebugRenderer::ECastShadow::Off); + inRenderer->DrawPie(constraint_to_world.GetTranslation(), mDrawConstraintSize, constraint_to_world.GetAxisX(), constraint_to_world.GetAxisY(), mTwistMinAngle, mTwistMaxAngle, Color::sPurple, DebugRenderer::ECastShadow::Off); +} +#endif // JPH_DEBUG_RENDERER + +void SwingTwistConstraint::SaveState(StateRecorder &inStream) const +{ + TwoBodyConstraint::SaveState(inStream); + + mPointConstraintPart.SaveState(inStream); + mSwingTwistConstraintPart.SaveState(inStream); + for (const AngleConstraintPart &c : mMotorConstraintPart) + c.SaveState(inStream); + + inStream.Write(mSwingMotorState); + inStream.Write(mTwistMotorState); + inStream.Write(mTargetAngularVelocity); + inStream.Write(mTargetOrientation); +} + +void SwingTwistConstraint::RestoreState(StateRecorder &inStream) +{ + TwoBodyConstraint::RestoreState(inStream); + + mPointConstraintPart.RestoreState(inStream); + mSwingTwistConstraintPart.RestoreState(inStream); + for (AngleConstraintPart &c : mMotorConstraintPart) + c.RestoreState(inStream); + + inStream.Read(mSwingMotorState); + inStream.Read(mTwistMotorState); + inStream.Read(mTargetAngularVelocity); + inStream.Read(mTargetOrientation); +} + +Ref SwingTwistConstraint::GetConstraintSettings() const +{ + SwingTwistConstraintSettings *settings = new SwingTwistConstraintSettings; + ToConstraintSettings(*settings); + settings->mSpace = EConstraintSpace::LocalToBodyCOM; + settings->mPosition1 = RVec3(mLocalSpacePosition1); + settings->mTwistAxis1 = mConstraintToBody1.RotateAxisX(); + settings->mPlaneAxis1 = mConstraintToBody1.RotateAxisZ(); + settings->mPosition2 = RVec3(mLocalSpacePosition2); + settings->mTwistAxis2 = mConstraintToBody2.RotateAxisX(); + settings->mPlaneAxis2 = mConstraintToBody2.RotateAxisZ(); + settings->mSwingType = mSwingTwistConstraintPart.GetSwingType(); + settings->mNormalHalfConeAngle = mNormalHalfConeAngle; + settings->mPlaneHalfConeAngle = mPlaneHalfConeAngle; + settings->mTwistMinAngle = mTwistMinAngle; + settings->mTwistMaxAngle = mTwistMaxAngle; + settings->mMaxFrictionTorque = mMaxFrictionTorque; + settings->mSwingMotorSettings = mSwingMotorSettings; + settings->mTwistMotorSettings = mTwistMotorSettings; + return settings; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.h b/WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.h new file mode 100644 index 000000000..d915c0015 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/SwingTwistConstraint.h @@ -0,0 +1,197 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Swing twist constraint settings, used to create a swing twist constraint +/// All values in this structure are copied to the swing twist constraint and the settings object is no longer needed afterwards. +/// +/// This image describes the limit settings: +/// @image html Docs/SwingTwistConstraint.png +class JPH_EXPORT SwingTwistConstraintSettings final : public TwoBodyConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, SwingTwistConstraintSettings) + + // See: ConstraintSettings::SaveBinaryState + virtual void SaveBinaryState(StreamOut &inStream) const override; + + /// Create an instance of this constraint + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const override; + + /// This determines in which space the constraint is setup, all properties below should be in the specified space + EConstraintSpace mSpace = EConstraintSpace::WorldSpace; + + ///@name Body 1 constraint reference frame (space determined by mSpace) + RVec3 mPosition1 = RVec3::sZero(); + Vec3 mTwistAxis1 = Vec3::sAxisX(); + Vec3 mPlaneAxis1 = Vec3::sAxisY(); + + ///@name Body 2 constraint reference frame (space determined by mSpace) + RVec3 mPosition2 = RVec3::sZero(); + Vec3 mTwistAxis2 = Vec3::sAxisX(); + Vec3 mPlaneAxis2 = Vec3::sAxisY(); + + /// The type of swing constraint that we want to use. + ESwingType mSwingType = ESwingType::Cone; + + ///@name Swing rotation limits + float mNormalHalfConeAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. + float mPlaneHalfConeAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. + + ///@name Twist rotation limits + float mTwistMinAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$. + float mTwistMaxAngle = 0.0f; ///< See image at Detailed Description. Angle in radians. Should be \f$\in [-\pi, \pi]\f$. + + ///@name Friction + float mMaxFrictionTorque = 0.0f; ///< Maximum amount of torque (N m) to apply as friction when the constraint is not powered by a motor + + ///@name In case the constraint is powered, this determines the motor settings around the swing and twist axis + MotorSettings mSwingMotorSettings; + MotorSettings mTwistMotorSettings; + +protected: + // See: ConstraintSettings::RestoreBinaryState + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// A swing twist constraint is a specialized constraint for humanoid ragdolls that allows limited rotation only +/// +/// @see SwingTwistConstraintSettings for a description of the limits +class JPH_EXPORT SwingTwistConstraint final : public TwoBodyConstraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Construct swing twist constraint + SwingTwistConstraint(Body &inBody1, Body &inBody2, const SwingTwistConstraintSettings &inSettings); + + ///@name Generic interface of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::SwingTwist; } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override; + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + + // See: TwoBodyConstraint + virtual Mat44 GetConstraintToBody1Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody1, mLocalSpacePosition1); } + virtual Mat44 GetConstraintToBody2Matrix() const override { return Mat44::sRotationTranslation(mConstraintToBody2, mLocalSpacePosition2); } + + ///@name Constraint reference frame + inline Vec3 GetLocalSpacePosition1() const { return mLocalSpacePosition1; } + inline Vec3 GetLocalSpacePosition2() const { return mLocalSpacePosition2; } + inline Quat GetConstraintToBody1() const { return mConstraintToBody1; } + inline Quat GetConstraintToBody2() const { return mConstraintToBody2; } + + ///@name Constraint limits + inline float GetNormalHalfConeAngle() const { return mNormalHalfConeAngle; } + inline void SetNormalHalfConeAngle(float inAngle) { mNormalHalfConeAngle = inAngle; UpdateLimits(); } + inline float GetPlaneHalfConeAngle() const { return mPlaneHalfConeAngle; } + inline void SetPlaneHalfConeAngle(float inAngle) { mPlaneHalfConeAngle = inAngle; UpdateLimits(); } + inline float GetTwistMinAngle() const { return mTwistMinAngle; } + inline void SetTwistMinAngle(float inAngle) { mTwistMinAngle = inAngle; UpdateLimits(); } + inline float GetTwistMaxAngle() const { return mTwistMaxAngle; } + inline void SetTwistMaxAngle(float inAngle) { mTwistMaxAngle = inAngle; UpdateLimits(); } + + ///@name Motor settings + const MotorSettings & GetSwingMotorSettings() const { return mSwingMotorSettings; } + MotorSettings & GetSwingMotorSettings() { return mSwingMotorSettings; } + const MotorSettings & GetTwistMotorSettings() const { return mTwistMotorSettings; } + MotorSettings & GetTwistMotorSettings() { return mTwistMotorSettings; } + + ///@name Friction control + void SetMaxFrictionTorque(float inFrictionTorque) { mMaxFrictionTorque = inFrictionTorque; } + float GetMaxFrictionTorque() const { return mMaxFrictionTorque; } + + ///@name Motor controls + + /// Controls if the motors are on or off + void SetSwingMotorState(EMotorState inState); + EMotorState GetSwingMotorState() const { return mSwingMotorState; } + void SetTwistMotorState(EMotorState inState); + EMotorState GetTwistMotorState() const { return mTwistMotorState; } + + /// Set the target angular velocity of body 2 in constraint space of body 2 + void SetTargetAngularVelocityCS(Vec3Arg inAngularVelocity) { mTargetAngularVelocity = inAngularVelocity; } + Vec3 GetTargetAngularVelocityCS() const { return mTargetAngularVelocity; } + + /// Set the target orientation in constraint space (drives constraint to: GetRotationInConstraintSpace() == inOrientation) + void SetTargetOrientationCS(QuatArg inOrientation); + Quat GetTargetOrientationCS() const { return mTargetOrientation; } + + /// Set the target orientation in body space (R2 = R1 * inOrientation, where R1 and R2 are the world space rotations for body 1 and 2). + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q (see SwingTwistConstraint::GetSwingTwist) and R2 = R1 * inOrientation for q. + void SetTargetOrientationBS(QuatArg inOrientation) { SetTargetOrientationCS(mConstraintToBody1.Conjugated() * inOrientation * mConstraintToBody2); } + + /// Get current rotation of constraint in constraint space. + /// Solve: R2 * ConstraintToBody2 = R1 * ConstraintToBody1 * q for q. + Quat GetRotationInConstraintSpace() const; + + ///@name Get Lagrange multiplier from last physics update (the linear/angular impulse applied to satisfy the constraint) + inline Vec3 GetTotalLambdaPosition() const { return mPointConstraintPart.GetTotalLambda(); } + inline float GetTotalLambdaTwist() const { return mSwingTwistConstraintPart.GetTotalTwistLambda(); } + inline float GetTotalLambdaSwingY() const { return mSwingTwistConstraintPart.GetTotalSwingYLambda(); } + inline float GetTotalLambdaSwingZ() const { return mSwingTwistConstraintPart.GetTotalSwingZLambda(); } + inline Vec3 GetTotalLambdaMotor() const { return Vec3(mMotorConstraintPart[0].GetTotalLambda(), mMotorConstraintPart[1].GetTotalLambda(), mMotorConstraintPart[2].GetTotalLambda()); } + +private: + // Update the limits in the swing twist constraint part + void UpdateLimits(); + + // CONFIGURATION PROPERTIES FOLLOW + + // Local space constraint positions + Vec3 mLocalSpacePosition1; + Vec3 mLocalSpacePosition2; + + // Transforms from constraint space to body space + Quat mConstraintToBody1; + Quat mConstraintToBody2; + + // Limits + float mNormalHalfConeAngle; + float mPlaneHalfConeAngle; + float mTwistMinAngle; + float mTwistMaxAngle; + + // Friction + float mMaxFrictionTorque; + + // Motor controls + MotorSettings mSwingMotorSettings; + MotorSettings mTwistMotorSettings; + EMotorState mSwingMotorState = EMotorState::Off; + EMotorState mTwistMotorState = EMotorState::Off; + Vec3 mTargetAngularVelocity = Vec3::sZero(); + Quat mTargetOrientation = Quat::sIdentity(); + + // RUN TIME PROPERTIES FOLLOW + + // Rotation axis for motor constraint parts + Vec3 mWorldSpaceMotorAxis[3]; + + // The constraint parts + PointConstraintPart mPointConstraintPart; + SwingTwistConstraintPart mSwingTwistConstraintPart; + AngleConstraintPart mMotorConstraintPart[3]; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.cpp b/WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.cpp new file mode 100644 index 000000000..9ee7cc5d8 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.cpp @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(TwoBodyConstraintSettings) +{ + JPH_ADD_BASE_CLASS(TwoBodyConstraintSettings, ConstraintSettings) +} + +void TwoBodyConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + // Activate bodies + BodyID body_ids[2]; + int num_bodies = 0; + if (mBody1->IsDynamic() && !mBody1->IsActive()) + body_ids[num_bodies++] = mBody1->GetID(); + if (mBody2->IsDynamic() && !mBody2->IsActive()) + body_ids[num_bodies++] = mBody2->GetID(); + if (num_bodies > 0) + inBodyManager.ActivateBodies(body_ids, num_bodies); + + // Link the bodies into the same island + ioBuilder.LinkConstraint(inConstraintIndex, mBody1->GetIndexInActiveBodiesInternal(), mBody2->GetIndexInActiveBodiesInternal()); +} + +uint TwoBodyConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const +{ + return ioSplitter.AssignSplit(mBody1, mBody2); +} + +#ifdef JPH_DEBUG_RENDERER + +void TwoBodyConstraint::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const +{ + RMat44 transform1 = mBody1->GetCenterOfMassTransform() * GetConstraintToBody1Matrix(); + RMat44 transform2 = mBody2->GetCenterOfMassTransform() * GetConstraintToBody2Matrix(); + inRenderer->DrawCoordinateSystem(transform1, 1.1f * mDrawConstraintSize); + inRenderer->DrawCoordinateSystem(transform2, mDrawConstraintSize); +} + +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.h b/WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.h new file mode 100644 index 000000000..028c073af --- /dev/null +++ b/WickedEngine/Jolt/Physics/Constraints/TwoBodyConstraint.h @@ -0,0 +1,65 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class TwoBodyConstraint; + +/// Base class for settings for all constraints that involve 2 bodies +class JPH_EXPORT TwoBodyConstraintSettings : public ConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, TwoBodyConstraintSettings) + + /// Create an instance of this constraint + /// You can use Body::sFixedToWorld for inBody1 if you want to attach inBody2 to the world + virtual TwoBodyConstraint * Create(Body &inBody1, Body &inBody2) const = 0; +}; + +/// Base class for all constraints that involve 2 bodies. Body1 is usually considered the parent, Body2 the child. +class JPH_EXPORT TwoBodyConstraint : public Constraint +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TwoBodyConstraint(Body &inBody1, Body &inBody2, const TwoBodyConstraintSettings &inSettings) : Constraint(inSettings), mBody1(&inBody1), mBody2(&inBody2) { } + + /// Get the type of a constraint + virtual EConstraintType GetType() const override { return EConstraintType::TwoBodyConstraint; } + + /// Solver interface + virtual bool IsActive() const override { return Constraint::IsActive() && (mBody1->IsActive() || mBody2->IsActive()) && (mBody2->IsDynamic() || mBody1->IsDynamic()); } +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + /// Access to the connected bodies + Body * GetBody1() const { return mBody1; } + Body * GetBody2() const { return mBody2; } + + /// Calculates the transform that transforms from constraint space to body 1 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc. + virtual Mat44 GetConstraintToBody1Matrix() const = 0; + + /// Calculates the transform that transforms from constraint space to body 2 space. The first column of the matrix is the primary constraint axis (e.g. the hinge axis / slider direction), second column the secondary etc. + virtual Mat44 GetConstraintToBody2Matrix() const = 0; + + /// Link bodies that are connected by this constraint in the island builder + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override; + + /// Link bodies that are connected by this constraint in the same split. Returns the split index. + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override; + +protected: + /// The two bodies involved + Body * mBody1; + Body * mBody2; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/DeterminismLog.cpp b/WickedEngine/Jolt/Physics/DeterminismLog.cpp new file mode 100644 index 000000000..7985a36bf --- /dev/null +++ b/WickedEngine/Jolt/Physics/DeterminismLog.cpp @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_ENABLE_DETERMINISM_LOG + +JPH_NAMESPACE_BEGIN + +DeterminismLog DeterminismLog::sLog; + +JPH_NAMESPACE_END + +#endif // JPH_ENABLE_DETERMINISM_LOG diff --git a/WickedEngine/Jolt/Physics/DeterminismLog.h b/WickedEngine/Jolt/Physics/DeterminismLog.h new file mode 100644 index 000000000..e2930ff3c --- /dev/null +++ b/WickedEngine/Jolt/Physics/DeterminismLog.h @@ -0,0 +1,159 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +//#define JPH_ENABLE_DETERMINISM_LOG +#ifdef JPH_ENABLE_DETERMINISM_LOG + +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +/// A simple class that logs the state of the simulation. The resulting text file can be used to diff between platforms and find issues in determinism. +class DeterminismLog +{ +private: + JPH_INLINE uint32 Convert(float inValue) const + { + return *(uint32 *)&inValue; + } + + JPH_INLINE uint64 Convert(double inValue) const + { + return *(uint64 *)&inValue; + } + +public: + DeterminismLog() + { + mLog.open("detlog.txt", std::ios::out | std::ios::trunc | std::ios::binary); // Binary because we don't want a difference between Unix and Windows line endings. + mLog.fill('0'); + } + + DeterminismLog & operator << (char inValue) + { + mLog << inValue; + return *this; + } + + DeterminismLog & operator << (const char *inValue) + { + mLog << std::dec << inValue; + return *this; + } + + DeterminismLog & operator << (const string &inValue) + { + mLog << std::dec << inValue; + return *this; + } + + DeterminismLog & operator << (const BodyID &inValue) + { + mLog << std::hex << std::setw(8) << inValue.GetIndexAndSequenceNumber(); + return *this; + } + + DeterminismLog & operator << (const SubShapeID &inValue) + { + mLog << std::hex << std::setw(8) << inValue.GetValue(); + return *this; + } + + DeterminismLog & operator << (float inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue); + return *this; + } + + DeterminismLog & operator << (int inValue) + { + mLog << inValue; + return *this; + } + + DeterminismLog & operator << (uint32 inValue) + { + mLog << std::hex << std::setw(8) << inValue; + return *this; + } + + DeterminismLog & operator << (uint64 inValue) + { + mLog << std::hex << std::setw(16) << inValue; + return *this; + } + + DeterminismLog & operator << (Vec3Arg inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()); + return *this; + } + + DeterminismLog & operator << (DVec3Arg inValue) + { + mLog << std::hex << std::setw(16) << Convert(inValue.GetX()) << " " << std::setw(16) << Convert(inValue.GetY()) << " " << std::setw(16) << Convert(inValue.GetZ()); + return *this; + } + + DeterminismLog & operator << (Vec4Arg inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.GetX()) << " " << std::setw(8) << Convert(inValue.GetY()) << " " << std::setw(8) << Convert(inValue.GetZ()) << " " << std::setw(8) << Convert(inValue.GetW()); + return *this; + } + + DeterminismLog & operator << (const Float3 &inValue) + { + mLog << std::hex << std::setw(8) << Convert(inValue.x) << " " << std::setw(8) << Convert(inValue.y) << " " << std::setw(8) << Convert(inValue.z); + return *this; + } + + DeterminismLog & operator << (Mat44Arg inValue) + { + *this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetColumn4(3); + return *this; + } + + DeterminismLog & operator << (DMat44Arg inValue) + { + *this << inValue.GetColumn4(0) << " " << inValue.GetColumn4(1) << " " << inValue.GetColumn4(2) << " " << inValue.GetTranslation(); + return *this; + } + + DeterminismLog & operator << (QuatArg inValue) + { + *this << inValue.GetXYZW(); + return *this; + } + + // Singleton instance + static DeterminismLog sLog; + +private: + std::ofstream mLog; +}; + +/// Will log something to the determinism log, usage: JPH_DET_LOG("label " << value); +#define JPH_DET_LOG(...) DeterminismLog::sLog << __VA_ARGS__ << '\n' + +JPH_NAMESPACE_END + +#else + +JPH_SUPPRESS_WARNING_PUSH +JPH_SUPPRESS_WARNINGS + +/// By default we log nothing +#define JPH_DET_LOG(...) + +JPH_SUPPRESS_WARNING_POP + +#endif // JPH_ENABLE_DETERMINISM_LOG diff --git a/WickedEngine/Jolt/Physics/EActivation.h b/WickedEngine/Jolt/Physics/EActivation.h new file mode 100644 index 000000000..08c10c20d --- /dev/null +++ b/WickedEngine/Jolt/Physics/EActivation.h @@ -0,0 +1,16 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used by AddBody to determine if the body needs to be initially active +enum class EActivation +{ + Activate, ///< Activate the body, making it part of the simulation + DontActivate ///< Leave activation state as it is (will not deactivate an active body) +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/EPhysicsUpdateError.h b/WickedEngine/Jolt/Physics/EPhysicsUpdateError.h new file mode 100644 index 000000000..c9edd6ded --- /dev/null +++ b/WickedEngine/Jolt/Physics/EPhysicsUpdateError.h @@ -0,0 +1,37 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Enum used by PhysicsSystem to report error conditions during the PhysicsSystem::Update call. This is a bit field, multiple errors can trigger in the same update. +enum class EPhysicsUpdateError : uint32 +{ + None = 0, ///< No errors + ManifoldCacheFull = 1 << 0, ///< The manifold cache is full, this means that the total number of contacts between bodies is too high. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init. + BodyPairCacheFull = 1 << 1, ///< The body pair cache is full, this means that too many bodies contacted. Some contacts were ignored. Increase inMaxBodyPairs in PhysicsSystem::Init. + ContactConstraintsFull = 1 << 2, ///< The contact constraints buffer is full. Some contacts were ignored. Increase inMaxContactConstraints in PhysicsSystem::Init. +}; + +/// OR operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator | (EPhysicsUpdateError inA, EPhysicsUpdateError inB) +{ + return static_cast(static_cast(inA) | static_cast(inB)); +} + +/// OR operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator |= (EPhysicsUpdateError &ioA, EPhysicsUpdateError inB) +{ + ioA = ioA | inB; + return ioA; +} + +/// AND operator for EPhysicsUpdateError +inline EPhysicsUpdateError operator & (EPhysicsUpdateError inA, EPhysicsUpdateError inB) +{ + return static_cast(static_cast(inA) & static_cast(inB)); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/IslandBuilder.cpp b/WickedEngine/Jolt/Physics/IslandBuilder.cpp new file mode 100644 index 000000000..ed1064df9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/IslandBuilder.cpp @@ -0,0 +1,484 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +IslandBuilder::~IslandBuilder() +{ + JPH_ASSERT(mConstraintLinks == nullptr); + JPH_ASSERT(mContactLinks == nullptr); + JPH_ASSERT(mBodyIslands == nullptr); + JPH_ASSERT(mBodyIslandEnds == nullptr); + JPH_ASSERT(mConstraintIslands == nullptr); + JPH_ASSERT(mConstraintIslandEnds == nullptr); + JPH_ASSERT(mContactIslands == nullptr); + JPH_ASSERT(mContactIslandEnds == nullptr); + JPH_ASSERT(mIslandsSorted == nullptr); + + delete [] mBodyLinks; +} + +void IslandBuilder::Init(uint32 inMaxActiveBodies) +{ + mMaxActiveBodies = inMaxActiveBodies; + + // Link each body to itself, BuildBodyIslands() will restore this so that we don't need to do this each step + JPH_ASSERT(mBodyLinks == nullptr); + mBodyLinks = new BodyLink [mMaxActiveBodies]; + for (uint32 i = 0; i < mMaxActiveBodies; ++i) + mBodyLinks[i].mLinkedTo.store(i, memory_order_relaxed); +} + +void IslandBuilder::PrepareContactConstraints(uint32 inMaxContacts, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Need to call Init first + JPH_ASSERT(mBodyLinks != nullptr); + + // Check that the builder has been reset + JPH_ASSERT(mNumContacts == 0); + JPH_ASSERT(mNumIslands == 0); + + // Create contact link buffer, not initialized so each contact needs to be explicitly set + JPH_ASSERT(mContactLinks == nullptr); + mContactLinks = (uint32 *)inTempAllocator->Allocate(inMaxContacts * sizeof(uint32)); + mMaxContacts = inMaxContacts; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + // Create validation structures + JPH_ASSERT(mLinkValidation == nullptr); + mLinkValidation = (LinkValidation *)inTempAllocator->Allocate(inMaxContacts * sizeof(LinkValidation)); + mNumLinkValidation = 0; +#endif +} + +void IslandBuilder::PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Need to call Init first + JPH_ASSERT(mBodyLinks != nullptr); + + // Check that the builder has been reset + JPH_ASSERT(mNumIslands == 0); + + // Store number of constraints + mNumConstraints = inNumConstraints; + + // Create constraint link buffer, not initialized so each constraint needs to be explicitly set + JPH_ASSERT(mConstraintLinks == nullptr); + mConstraintLinks = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32)); +} + +uint32 IslandBuilder::GetLowestBodyIndex(uint32 inActiveBodyIndex) const +{ + uint32 index = inActiveBodyIndex; + for (;;) + { + uint32 link_to = mBodyLinks[index].mLinkedTo.load(memory_order_relaxed); + if (link_to == index) + break; + index = link_to; + } + return index; +} + +void IslandBuilder::LinkBodies(uint32 inFirst, uint32 inSecond) +{ + JPH_PROFILE_FUNCTION(); + + // Both need to be active, we don't want to create an island with static objects + if (inFirst >= mMaxActiveBodies || inSecond >= mMaxActiveBodies) + return; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + // Add link to the validation list + if (mNumLinkValidation < uint32(mMaxContacts)) + mLinkValidation[mNumLinkValidation++] = { inFirst, inSecond }; + else + JPH_ASSERT(false, "Out of links"); +#endif + + // Start the algorithm with the two bodies + uint32 first_link_to = inFirst; + uint32 second_link_to = inSecond; + + for (;;) + { + // Follow the chain until we get to the body with lowest index + // If the swap compare below fails, we'll keep searching from the lowest index for the new lowest index + first_link_to = GetLowestBodyIndex(first_link_to); + second_link_to = GetLowestBodyIndex(second_link_to); + + // If the targets are the same, the bodies are already connected + if (first_link_to != second_link_to) + { + // We always link the highest to the lowest + if (first_link_to < second_link_to) + { + // Attempt to link the second to the first + // Since we found this body to be at the end of the chain it must point to itself, and if it + // doesn't it has been reparented and we need to retry the algorithm + if (!mBodyLinks[second_link_to].mLinkedTo.compare_exchange_weak(second_link_to, first_link_to, memory_order_relaxed)) + continue; + } + else + { + // Attempt to link the first to the second + // Since we found this body to be at the end of the chain it must point to itself, and if it + // doesn't it has been reparented and we need to retry the algorithm + if (!mBodyLinks[first_link_to].mLinkedTo.compare_exchange_weak(first_link_to, second_link_to, memory_order_relaxed)) + continue; + } + } + + // Linking succeeded! + // Chains of bodies can become really long, resulting in an O(N) loop to find the lowest body index + // to prevent this we attempt to update the link of the bodies that were passed in to directly point + // to the lowest index that we found. If the value became lower than our lowest link, some other + // thread must have relinked these bodies in the mean time so we won't update the value. + uint32 lowest_link_to = min(first_link_to, second_link_to); + AtomicMin(mBodyLinks[inFirst].mLinkedTo, lowest_link_to, memory_order_relaxed); + AtomicMin(mBodyLinks[inSecond].mLinkedTo, lowest_link_to, memory_order_relaxed); + break; + } +} + +void IslandBuilder::LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond) +{ + LinkBodies(inFirst, inSecond); + + JPH_ASSERT(inConstraintIndex < mNumConstraints); + uint32 min_value = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two + JPH_ASSERT(min_value != Body::cInactiveIndex); // At least one of the bodies must be active + mConstraintLinks[inConstraintIndex] = min_value; +} + +void IslandBuilder::LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond) +{ + JPH_ASSERT(inContactIndex < mMaxContacts); + mContactLinks[inContactIndex] = min(inFirst, inSecond); // Use fact that invalid index is 0xffffffff, we want the active body of two +} + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + +void IslandBuilder::ValidateIslands(uint32 inNumActiveBodies) const +{ + JPH_PROFILE_FUNCTION(); + + // Go through all links so far + for (uint32 i = 0; i < mNumLinkValidation; ++i) + { + // If the bodies in this link ended up in different groups we have a problem + if (mBodyLinks[mLinkValidation[i].mFirst].mIslandIndex != mBodyLinks[mLinkValidation[i].mSecond].mIslandIndex) + { + Trace("Fail: %u, %u", mLinkValidation[i].mFirst, mLinkValidation[i].mSecond); + Trace("Num Active: %u", inNumActiveBodies); + + for (uint32 j = 0; j < mNumLinkValidation; ++j) + Trace("builder.Link(%u, %u);", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + + IslandBuilder tmp; + tmp.Init(inNumActiveBodies); + for (uint32 j = 0; j < mNumLinkValidation; ++j) + { + Trace("Link %u -> %u", mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + tmp.LinkBodies(mLinkValidation[j].mFirst, mLinkValidation[j].mSecond); + for (uint32 t = 0; t < inNumActiveBodies; ++t) + Trace("%u -> %u", t, (uint32)tmp.mBodyLinks[t].mLinkedTo); + } + + JPH_ASSERT(false, "IslandBuilder validation failed"); + } + } +} + +#endif + +void IslandBuilder::BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Store the amount of active bodies + mNumActiveBodies = inNumActiveBodies; + + // Create output arrays for body ID's, don't call constructors + JPH_ASSERT(mBodyIslands == nullptr); + mBodyIslands = (BodyID *)inTempAllocator->Allocate(inNumActiveBodies * sizeof(BodyID)); + + // Create output array for start index of each island. At this point we don't know how many islands there will be, but we know it cannot be more than inNumActiveBodies. + // Note: We allocate 1 extra entry because we always increment the count of the next island. + uint32 *body_island_starts = (uint32 *)inTempAllocator->Allocate((inNumActiveBodies + 1) * sizeof(uint32)); + + // First island always starts at 0 + body_island_starts[0] = 0; + + // Calculate island index for all bodies + JPH_ASSERT(mNumIslands == 0); + for (uint32 i = 0; i < inNumActiveBodies; ++i) + { + BodyLink &link = mBodyLinks[i]; + uint32 s = link.mLinkedTo.load(memory_order_relaxed); + if (s != i) + { + // Links to another body, take island index from other body (this must have been filled in already since we're looping from low to high) + JPH_ASSERT(s < uint32(i)); + uint32 island_index = mBodyLinks[s].mIslandIndex; + link.mIslandIndex = island_index; + + // Increment the start of the next island + body_island_starts[island_index + 1]++; + } + else + { + // Does not link to other body, this is the start of a new island + link.mIslandIndex = mNumIslands; + ++mNumIslands; + + // Set the start of the next island to 1 + body_island_starts[mNumIslands] = 1; + } + } + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + ValidateIslands(inNumActiveBodies); +#endif + + // Make the start array absolute (so far we only counted) + for (uint32 island = 1; island < mNumIslands; ++island) + body_island_starts[island] += body_island_starts[island - 1]; + + // Convert the to a linear list grouped by island + for (uint32 i = 0; i < inNumActiveBodies; ++i) + { + BodyLink &link = mBodyLinks[i]; + + // Copy the body to the correct location in the array and increment it + uint32 &start = body_island_starts[link.mIslandIndex]; + mBodyIslands[start] = inActiveBodies[i]; + start++; + + // Reset linked to field for the next update + link.mLinkedTo.store(i, memory_order_relaxed); + } + + // We should now have a full array + JPH_ASSERT(mNumIslands == 0 || body_island_starts[mNumIslands - 1] == inNumActiveBodies); + + // We've incremented all body indices so that they now point at the end instead of the starts + JPH_ASSERT(mBodyIslandEnds == nullptr); + mBodyIslandEnds = body_island_starts; +} + +void IslandBuilder::BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const +{ + JPH_PROFILE_FUNCTION(); + + // Check if there's anything to do + if (inNumConstraints == 0) + return; + + // Create output arrays for constraints + // Note: For the end indices we allocate 1 extra entry so we don't have to do an if in the inner loop + uint32 *constraints = (uint32 *)inTempAllocator->Allocate(inNumConstraints * sizeof(uint32)); + uint32 *constraint_ends = (uint32 *)inTempAllocator->Allocate((mNumIslands + 1) * sizeof(uint32)); + + // Reset sizes + for (uint32 island = 0; island < mNumIslands; ++island) + constraint_ends[island] = 0; + + // Loop over array and increment start relative position for the next island + for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint) + { + uint32 body_idx = inConstraintToBody[constraint]; + uint32 next_island_idx = mBodyLinks[body_idx].mIslandIndex + 1; + JPH_ASSERT(next_island_idx <= mNumIslands); + constraint_ends[next_island_idx]++; + } + + // Make start positions absolute + for (uint32 island = 1; island < mNumIslands; ++island) + constraint_ends[island] += constraint_ends[island - 1]; + + // Loop over array and collect constraints + for (uint32 constraint = 0; constraint < inNumConstraints; ++constraint) + { + uint32 body_idx = inConstraintToBody[constraint]; + uint32 island_idx = mBodyLinks[body_idx].mIslandIndex; + constraints[constraint_ends[island_idx]++] = constraint; + } + + JPH_ASSERT(outConstraints == nullptr); + outConstraints = constraints; + JPH_ASSERT(outConstraintsEnd == nullptr); + outConstraintsEnd = constraint_ends; +} + +void IslandBuilder::SortIslands(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + if (mNumContacts > 0 || mNumConstraints > 0) + { + // Allocate mapping table + JPH_ASSERT(mIslandsSorted == nullptr); + mIslandsSorted = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32)); + + // Initialize index + for (uint32 island = 0; island < mNumIslands; ++island) + mIslandsSorted[island] = island; + + // Determine the sum of contact constraints / constraints per island + uint32 *num_constraints = (uint32 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint32)); + if (mNumContacts > 0 && mNumConstraints > 0) + { + num_constraints[0] = mConstraintIslandEnds[0] + mContactIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1] + + mContactIslandEnds[island] - mContactIslandEnds[island - 1]; + } + else if (mNumContacts > 0) + { + num_constraints[0] = mContactIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mContactIslandEnds[island] - mContactIslandEnds[island - 1]; + } + else + { + num_constraints[0] = mConstraintIslandEnds[0]; + for (uint32 island = 1; island < mNumIslands; ++island) + num_constraints[island] = mConstraintIslandEnds[island] - mConstraintIslandEnds[island - 1]; + } + + // Sort so the biggest islands go first, this means that the jobs that take longest will be running + // first which improves the chance that all jobs finish at the same time. + QuickSort(mIslandsSorted, mIslandsSorted + mNumIslands, [num_constraints](uint32 inLHS, uint32 inRHS) { + return num_constraints[inLHS] > num_constraints[inRHS]; + }); + + inTempAllocator->Free(num_constraints, mNumIslands * sizeof(uint32)); + } +} + +void IslandBuilder::Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + mNumContacts = inNumContacts; + + BuildBodyIslands(inActiveBodies, inNumActiveBodies, inTempAllocator); + BuildConstraintIslands(mConstraintLinks, mNumConstraints, mConstraintIslands, mConstraintIslandEnds, inTempAllocator); + BuildConstraintIslands(mContactLinks, mNumContacts, mContactIslands, mContactIslandEnds, inTempAllocator); + SortIslands(inTempAllocator); + + mNumPositionSteps = (uint8 *)inTempAllocator->Allocate(mNumIslands * sizeof(uint8)); +} + +void IslandBuilder::GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + uint32 sorted_index = mIslandsSorted != nullptr? mIslandsSorted[inIslandIndex] : inIslandIndex; + outBodiesBegin = sorted_index > 0? mBodyIslands + mBodyIslandEnds[sorted_index - 1] : mBodyIslands; + outBodiesEnd = mBodyIslands + mBodyIslandEnds[sorted_index]; +} + +bool IslandBuilder::GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + if (mNumConstraints == 0) + { + outConstraintsBegin = nullptr; + outConstraintsEnd = nullptr; + return false; + } + else + { + uint32 sorted_index = mIslandsSorted[inIslandIndex]; + outConstraintsBegin = sorted_index > 0? mConstraintIslands + mConstraintIslandEnds[sorted_index - 1] : mConstraintIslands; + outConstraintsEnd = mConstraintIslands + mConstraintIslandEnds[sorted_index]; + return outConstraintsBegin != outConstraintsEnd; + } +} + +bool IslandBuilder::GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const +{ + JPH_ASSERT(inIslandIndex < mNumIslands); + if (mNumContacts == 0) + { + outContactsBegin = nullptr; + outContactsEnd = nullptr; + return false; + } + else + { + uint32 sorted_index = mIslandsSorted[inIslandIndex]; + outContactsBegin = sorted_index > 0? mContactIslands + mContactIslandEnds[sorted_index - 1] : mContactIslands; + outContactsEnd = mContactIslands + mContactIslandEnds[sorted_index]; + return outContactsBegin != outContactsEnd; + } +} + +void IslandBuilder::ResetIslands(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + inTempAllocator->Free(mNumPositionSteps, mNumIslands * sizeof(uint8)); + + if (mIslandsSorted != nullptr) + { + inTempAllocator->Free(mIslandsSorted, mNumIslands * sizeof(uint32)); + mIslandsSorted = nullptr; + } + + if (mContactIslands != nullptr) + { + inTempAllocator->Free(mContactIslandEnds, (mNumIslands + 1) * sizeof(uint32)); + mContactIslandEnds = nullptr; + inTempAllocator->Free(mContactIslands, mNumContacts * sizeof(uint32)); + mContactIslands = nullptr; + } + + if (mConstraintIslands != nullptr) + { + inTempAllocator->Free(mConstraintIslandEnds, (mNumIslands + 1) * sizeof(uint32)); + mConstraintIslandEnds = nullptr; + inTempAllocator->Free(mConstraintIslands, mNumConstraints * sizeof(uint32)); + mConstraintIslands = nullptr; + } + + inTempAllocator->Free(mBodyIslandEnds, (mNumActiveBodies + 1) * sizeof(uint32)); + mBodyIslandEnds = nullptr; + inTempAllocator->Free(mBodyIslands, mNumActiveBodies * sizeof(uint32)); + mBodyIslands = nullptr; + + inTempAllocator->Free(mConstraintLinks, mNumConstraints * sizeof(uint32)); + mConstraintLinks = nullptr; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + inTempAllocator->Free(mLinkValidation, mMaxContacts * sizeof(LinkValidation)); + mLinkValidation = nullptr; +#endif + + inTempAllocator->Free(mContactLinks, mMaxContacts * sizeof(uint32)); + mContactLinks = nullptr; + + mNumActiveBodies = 0; + mNumConstraints = 0; + mMaxContacts = 0; + mNumContacts = 0; + mNumIslands = 0; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/IslandBuilder.h b/WickedEngine/Jolt/Physics/IslandBuilder.h new file mode 100644 index 000000000..4c2f097d6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/IslandBuilder.h @@ -0,0 +1,125 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class TempAllocator; + +//#define JPH_VALIDATE_ISLAND_BUILDER + +/// Keeps track of connected bodies and builds islands for multithreaded velocity/position update +class IslandBuilder : public NonCopyable +{ +public: + /// Destructor + ~IslandBuilder(); + + /// Initialize the island builder with the maximum amount of bodies that could be active + void Init(uint32 inMaxActiveBodies); + + /// Prepare for simulation step by allocating space for the contact constraints + void PrepareContactConstraints(uint32 inMaxContactConstraints, TempAllocator *inTempAllocator); + + /// Prepare for simulation step by allocating space for the non-contact constraints + void PrepareNonContactConstraints(uint32 inNumConstraints, TempAllocator *inTempAllocator); + + /// Link two bodies by their index in the BodyManager::mActiveBodies list to form islands + void LinkBodies(uint32 inFirst, uint32 inSecond); + + /// Link a constraint to a body by their index in the BodyManager::mActiveBodies + void LinkConstraint(uint32 inConstraintIndex, uint32 inFirst, uint32 inSecond); + + /// Link a contact to a body by their index in the BodyManager::mActiveBodies + void LinkContact(uint32 inContactIndex, uint32 inFirst, uint32 inSecond); + + /// Finalize the islands after all bodies have been Link()-ed + void Finalize(const BodyID *inActiveBodies, uint32 inNumActiveBodies, uint32 inNumContacts, TempAllocator *inTempAllocator); + + /// Get the amount of islands formed + uint32 GetNumIslands() const { return mNumIslands; } + + /// Get iterator for a particular island, return false if there are no constraints + void GetBodiesInIsland(uint32 inIslandIndex, BodyID *&outBodiesBegin, BodyID *&outBodiesEnd) const; + bool GetConstraintsInIsland(uint32 inIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd) const; + bool GetContactsInIsland(uint32 inIslandIndex, uint32 *&outContactsBegin, uint32 *&outContactsEnd) const; + + /// The number of position iterations for each island + void SetNumPositionSteps(uint32 inIslandIndex, uint inNumPositionSteps) { JPH_ASSERT(inIslandIndex < mNumIslands); JPH_ASSERT(inNumPositionSteps < 256); mNumPositionSteps[inIslandIndex] = uint8(inNumPositionSteps); } + uint GetNumPositionSteps(uint32 inIslandIndex) const { JPH_ASSERT(inIslandIndex < mNumIslands); return mNumPositionSteps[inIslandIndex]; } + + /// After you're done calling the three functions above, call this function to free associated data + void ResetIslands(TempAllocator *inTempAllocator); + +private: + /// Returns the index of the lowest body in the group + uint32 GetLowestBodyIndex(uint32 inActiveBodyIndex) const; + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + /// Helper function to validate all islands so far generated + void ValidateIslands(uint32 inNumActiveBodies) const; +#endif + + // Helper functions to build various islands + void BuildBodyIslands(const BodyID *inActiveBodies, uint32 inNumActiveBodies, TempAllocator *inTempAllocator); + void BuildConstraintIslands(const uint32 *inConstraintToBody, uint32 inNumConstraints, uint32 *&outConstraints, uint32 *&outConstraintsEnd, TempAllocator *inTempAllocator) const; + + /// Sorts the islands so that the islands with most constraints go first + void SortIslands(TempAllocator *inTempAllocator); + + /// Intermediate data structure that for each body keeps track what the lowest index of the body is that it is connected to + struct BodyLink + { + JPH_OVERRIDE_NEW_DELETE + + atomic mLinkedTo; ///< An index in mBodyLinks pointing to another body in this island with a lower index than this body + uint32 mIslandIndex; ///< The island index of this body (filled in during Finalize) + }; + + // Intermediate data + BodyLink * mBodyLinks = nullptr; ///< Maps bodies to the first body in the island + uint32 * mConstraintLinks = nullptr; ///< Maps constraint index to body index (which maps to island index) + uint32 * mContactLinks = nullptr; ///< Maps contact constraint index to body index (which maps to island index) + + // Final data + BodyID * mBodyIslands = nullptr; ///< Bodies ordered by island + uint32 * mBodyIslandEnds = nullptr; ///< End index of each body island + + uint32 * mConstraintIslands = nullptr; ///< Constraints ordered by island + uint32 * mConstraintIslandEnds = nullptr; ///< End index of each constraint island + + uint32 * mContactIslands = nullptr; ///< Contacts ordered by island + uint32 * mContactIslandEnds = nullptr; ///< End index of each contact island + + uint32 * mIslandsSorted = nullptr; ///< A list of island indices in order of most constraints first + + uint8 * mNumPositionSteps = nullptr; ///< Number of position steps for each island + + // Counters + uint32 mMaxActiveBodies; ///< Maximum size of the active bodies list (see BodyManager::mActiveBodies) + uint32 mNumActiveBodies = 0; ///< Number of active bodies passed to + uint32 mNumConstraints = 0; ///< Size of the constraint list (see ConstraintManager::mConstraints) + uint32 mMaxContacts = 0; ///< Maximum amount of contacts supported + uint32 mNumContacts = 0; ///< Size of the contacts list (see ContactConstraintManager::mNumConstraints) + uint32 mNumIslands = 0; ///< Final number of islands + +#ifdef JPH_VALIDATE_ISLAND_BUILDER + /// Structure to keep track of all added links to validate that islands were generated correctly + struct LinkValidation + { + uint32 mFirst; + uint32 mSecond; + }; + + LinkValidation * mLinkValidation = nullptr; + atomic mNumLinkValidation; +#endif +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/LargeIslandSplitter.cpp b/WickedEngine/Jolt/Physics/LargeIslandSplitter.cpp new file mode 100644 index 000000000..6ee1a5a11 --- /dev/null +++ b/WickedEngine/Jolt/Physics/LargeIslandSplitter.cpp @@ -0,0 +1,579 @@ +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +//#define JPH_LARGE_ISLAND_SPLITTER_DEBUG + +JPH_NAMESPACE_BEGIN + +LargeIslandSplitter::EStatus LargeIslandSplitter::Splits::FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration) +{ + { + // First check if we can get a new batch (doing a relaxed read to avoid hammering an atomic with an atomic subtract) + // Note this also avoids overflowing the status counter if we're done but there's still one thread processing items + uint64 status = mStatus.load(memory_order_relaxed); + if (sGetIteration(status) >= mNumIterations) + return EStatus::AllBatchesDone; + + // Check for special value that indicates that the splits are still being built + // (note we do not check for this condition again below as we reset all splits before kicking off jobs that fetch batches of work) + if (status == StatusItemMask) + return EStatus::WaitingForBatch; + + uint item = sGetItem(status); + uint split_index = sGetSplit(status); + if (split_index == cNonParallelSplitIdx) + { + // Non parallel split needs to be taken as a single batch, only the thread that takes element 0 will do it + if (item != 0) + return EStatus::WaitingForBatch; + } + else + { + // Parallel split is split into batches + JPH_ASSERT(split_index < mNumSplits); + const Split &split = mSplits[split_index]; + if (item >= split.GetNumItems()) + return EStatus::WaitingForBatch; + } + } + + // Then try to actually get the batch + uint64 status = mStatus.fetch_add(cBatchSize, memory_order_acquire); + int iteration = sGetIteration(status); + if (iteration >= mNumIterations) + return EStatus::AllBatchesDone; + + uint split_index = sGetSplit(status); + JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx); + const Split &split = mSplits[split_index]; + uint item_begin = sGetItem(status); + if (split_index == cNonParallelSplitIdx) + { + if (item_begin == 0) + { + // Non-parallel split always goes as a single batch + outConstraintsBegin = split.mConstraintBufferBegin; + outConstraintsEnd = split.mConstraintBufferEnd; + outContactsBegin = split.mContactBufferBegin; + outContactsEnd = split.mContactBufferEnd; + outFirstIteration = iteration == 0; + return EStatus::BatchRetrieved; + } + else + { + // Otherwise we're done with this split + return EStatus::WaitingForBatch; + } + } + + // Parallel split is split into batches + uint num_constraints = split.GetNumConstraints(); + uint num_contacts = split.GetNumContacts(); + uint num_items = num_constraints + num_contacts; + if (item_begin >= num_items) + return EStatus::WaitingForBatch; + + uint item_end = min(item_begin + cBatchSize, num_items); + if (item_end >= num_constraints) + { + if (item_begin < num_constraints) + { + // Partially from constraints and partially from contacts + outConstraintsBegin = split.mConstraintBufferBegin + item_begin; + outConstraintsEnd = split.mConstraintBufferEnd; + } + else + { + // Only contacts + outConstraintsBegin = 0; + outConstraintsEnd = 0; + } + + outContactsBegin = split.mContactBufferBegin + (max(item_begin, num_constraints) - num_constraints); + outContactsEnd = split.mContactBufferBegin + (item_end - num_constraints); + } + else + { + // Only constraints + outConstraintsBegin = split.mConstraintBufferBegin + item_begin; + outConstraintsEnd = split.mConstraintBufferBegin + item_end; + + outContactsBegin = 0; + outContactsEnd = 0; + } + + outFirstIteration = iteration == 0; + return EStatus::BatchRetrieved; +} + +void LargeIslandSplitter::Splits::MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch) +{ + // We fetched this batch, nobody should change the split and or iteration until we mark the last batch as processed so we can safely get the current status + uint64 status = mStatus.load(memory_order_relaxed); + uint split_index = sGetSplit(status); + JPH_ASSERT(split_index < mNumSplits || split_index == cNonParallelSplitIdx); + const Split &split = mSplits[split_index]; + uint num_items_in_split = split.GetNumItems(); + + // Determine if this is the last iteration before possibly incrementing it + int iteration = sGetIteration(status); + outLastIteration = iteration == mNumIterations - 1; + + // Add the number of items we processed to the total number of items processed + // Note: This needs to happen after we read the status as other threads may update the status after we mark items as processed + JPH_ASSERT(inNumProcessed > 0); // Logic will break if we mark a block of 0 items as processed + uint total_items_processed = mItemsProcessed.fetch_add(inNumProcessed, memory_order_acq_rel) + inNumProcessed; + + // Check if we're at the end of the split + if (total_items_processed >= num_items_in_split) + { + JPH_ASSERT(total_items_processed == num_items_in_split); // Should not overflow, that means we're retiring more items than we should process + + // Set items processed back to 0 for the next split/iteration + mItemsProcessed.store(0, memory_order_release); + + // Determine next split + do + { + if (split_index == cNonParallelSplitIdx) + { + // At start of next iteration + split_index = 0; + ++iteration; + } + else + { + // At start of next split + ++split_index; + } + + // If we're beyond the end of splits, go to the non-parallel split + if (split_index >= mNumSplits) + split_index = cNonParallelSplitIdx; + } + while (iteration < mNumIterations + && mSplits[split_index].GetNumItems() == 0); // We don't support processing empty splits, skip to the next split in this case + + mStatus.store((uint64(iteration) << StatusIterationShift) | (uint64(split_index) << StatusSplitShift), memory_order_release); + } + + // Track if this is the final batch + outFinalBatch = iteration >= mNumIterations; +} + +LargeIslandSplitter::~LargeIslandSplitter() +{ + JPH_ASSERT(mSplitMasks == nullptr); + JPH_ASSERT(mContactAndConstraintsSplitIdx == nullptr); + JPH_ASSERT(mContactAndConstraintIndices == nullptr); + JPH_ASSERT(mSplitIslands == nullptr); +} + +void LargeIslandSplitter::Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Count the total number of constraints and contacts that we will be putting in splits + mContactAndConstraintsSize = 0; + for (uint32 island = 0; island < inIslandBuilder.GetNumIslands(); ++island) + { + // Get the contacts in this island + uint32 *contacts_start, *contacts_end; + inIslandBuilder.GetContactsInIsland(island, contacts_start, contacts_end); + uint num_contacts_in_island = uint(contacts_end - contacts_start); + + // Get the constraints in this island + uint32 *constraints_start, *constraints_end; + inIslandBuilder.GetConstraintsInIsland(island, constraints_start, constraints_end); + uint num_constraints_in_island = uint(constraints_end - constraints_start); + + uint island_size = num_contacts_in_island + num_constraints_in_island; + if (island_size >= cLargeIslandTreshold) + { + mNumSplitIslands++; + mContactAndConstraintsSize += island_size; + } + else + break; // If this island doesn't have enough constraints, the next islands won't either since they're sorted from big to small + } + + if (mContactAndConstraintsSize > 0) + { + mNumActiveBodies = inNumActiveBodies; + + // Allocate split mask buffer + mSplitMasks = (SplitMask *)inTempAllocator->Allocate(mNumActiveBodies * sizeof(SplitMask)); + + // Allocate contact and constraint buffer + uint contact_and_constraint_indices_size = mContactAndConstraintsSize * sizeof(uint32); + mContactAndConstraintsSplitIdx = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size); + mContactAndConstraintIndices = (uint32 *)inTempAllocator->Allocate(contact_and_constraint_indices_size); + + // Allocate island split buffer + mSplitIslands = (Splits *)inTempAllocator->Allocate(mNumSplitIslands * sizeof(Splits)); + + // Prevent any of the splits from being picked up as work + for (uint i = 0; i < mNumSplitIslands; ++i) + mSplitIslands[i].ResetStatus(); + } +} + +uint LargeIslandSplitter::AssignSplit(const Body *inBody1, const Body *inBody2) +{ + uint32 idx1 = inBody1->GetIndexInActiveBodiesInternal(); + uint32 idx2 = inBody2->GetIndexInActiveBodiesInternal(); + + // Test if either index is negative + if (idx1 == Body::cInactiveIndex || !inBody1->IsDynamic()) + { + // Body 1 is not active or a kinematic body, so we only need to set 1 body + JPH_ASSERT(idx2 < mNumActiveBodies); + SplitMask &mask = mSplitMasks[idx2]; + uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx); + mask |= SplitMask(1U << split); + return split; + } + else if (idx2 == Body::cInactiveIndex || !inBody2->IsDynamic()) + { + // Body 2 is not active or a kinematic body, so we only need to set 1 body + JPH_ASSERT(idx1 < mNumActiveBodies); + SplitMask &mask = mSplitMasks[idx1]; + uint split = min(CountTrailingZeros(~uint32(mask)), cNonParallelSplitIdx); + mask |= SplitMask(1U << split); + return split; + } + else + { + // If both bodies are active, we need to set 2 bodies + JPH_ASSERT(idx1 < mNumActiveBodies); + JPH_ASSERT(idx2 < mNumActiveBodies); + SplitMask &mask1 = mSplitMasks[idx1]; + SplitMask &mask2 = mSplitMasks[idx2]; + uint split = min(CountTrailingZeros((~uint32(mask1)) & (~uint32(mask2))), cNonParallelSplitIdx); + SplitMask mask = SplitMask(1U << split); + mask1 |= mask; + mask2 |= mask; + return split; + } +} + +uint LargeIslandSplitter::AssignToNonParallelSplit(const Body *inBody) +{ + uint32 idx = inBody->GetIndexInActiveBodiesInternal(); + if (idx != Body::cInactiveIndex) + { + JPH_ASSERT(idx < mNumActiveBodies); + mSplitMasks[idx] |= 1U << cNonParallelSplitIdx; + } + + return cNonParallelSplitIdx; +} + +bool LargeIslandSplitter::SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator) +{ + JPH_PROFILE_FUNCTION(); + + // Get the contacts in this island + uint32 *contacts_start, *contacts_end; + inIslandBuilder.GetContactsInIsland(inIslandIndex, contacts_start, contacts_end); + uint num_contacts_in_island = uint(contacts_end - contacts_start); + + // Get the constraints in this island + uint32 *constraints_start, *constraints_end; + inIslandBuilder.GetConstraintsInIsland(inIslandIndex, constraints_start, constraints_end); + uint num_constraints_in_island = uint(constraints_end - constraints_start); + + // Check if it exceeds the threshold + uint island_size = num_contacts_in_island + num_constraints_in_island; + if (island_size < cLargeIslandTreshold) + return false; + + // Get bodies in this island + BodyID *bodies_start, *bodies_end; + inIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_start, bodies_end); + + // Reset the split mask for all bodies in this island + Body const * const *bodies = inBodyManager.GetBodies().data(); + for (const BodyID *b = bodies_start; b < bodies_end; ++b) + mSplitMasks[bodies[b->GetIndex()]->GetIndexInActiveBodiesInternal()] = 0; + + // Count the number of contacts and constraints per split + uint num_contacts_in_split[cNumSplits] = { }; + uint num_constraints_in_split[cNumSplits] = { }; + + // Get space to store split indices + uint offset = mContactAndConstraintsNextFree.fetch_add(island_size, memory_order_relaxed); + uint32 *contact_split_idx = mContactAndConstraintsSplitIdx + offset; + uint32 *constraint_split_idx = contact_split_idx + num_contacts_in_island; + + // Assign the contacts to a split + uint32 *cur_contact_split_idx = contact_split_idx; + for (const uint32 *c = contacts_start; c < contacts_end; ++c) + { + const Body *body1, *body2; + inContactManager.GetAffectedBodies(*c, body1, body2); + uint split = AssignSplit(body1, body2); + num_contacts_in_split[split]++; + *cur_contact_split_idx++ = split; + + if (body1->IsDynamic()) + ioStepsCalculator(body1->GetMotionPropertiesUnchecked()); + if (body2->IsDynamic()) + ioStepsCalculator(body2->GetMotionPropertiesUnchecked()); + } + + // Assign the constraints to a split + uint32 *cur_constraint_split_idx = constraint_split_idx; + for (const uint32 *c = constraints_start; c < constraints_end; ++c) + { + const Constraint *constraint = inActiveConstraints[*c]; + uint split = constraint->BuildIslandSplits(*this); + num_constraints_in_split[split]++; + *cur_constraint_split_idx++ = split; + + ioStepsCalculator(constraint); + } + + ioStepsCalculator.Finalize(); + + // Start with 0 splits + uint split_remap_table[cNumSplits]; + uint new_split_idx = mNextSplitIsland.fetch_add(1, memory_order_relaxed); + JPH_ASSERT(new_split_idx < mNumSplitIslands); + Splits &splits = mSplitIslands[new_split_idx]; + splits.mIslandIndex = inIslandIndex; + splits.mNumSplits = 0; + splits.mNumIterations = ioStepsCalculator.GetNumVelocitySteps() + 1; // Iteration 0 is used for warm starting + splits.mNumVelocitySteps = ioStepsCalculator.GetNumVelocitySteps(); + splits.mNumPositionSteps = ioStepsCalculator.GetNumPositionSteps(); + splits.mItemsProcessed.store(0, memory_order_release); + + // Allocate space to store the sorted constraint and contact indices per split + uint32 *constraint_buffer_cur[cNumSplits], *contact_buffer_cur[cNumSplits]; + for (uint s = 0; s < cNumSplits; ++s) + { + // If this split doesn't contain enough constraints and contacts, we will combine it with the non parallel split + if (num_constraints_in_split[s] + num_contacts_in_split[s] < cSplitCombineTreshold + && s < cNonParallelSplitIdx) // The non-parallel split cannot merge into itself + { + // Remap it + split_remap_table[s] = cNonParallelSplitIdx; + + // Add the counts to the non parallel split + num_contacts_in_split[cNonParallelSplitIdx] += num_contacts_in_split[s]; + num_constraints_in_split[cNonParallelSplitIdx] += num_constraints_in_split[s]; + } + else + { + // This split is valid, map it to the next empty slot + uint target_split; + if (s < cNonParallelSplitIdx) + target_split = splits.mNumSplits++; + else + target_split = cNonParallelSplitIdx; + Split &split = splits.mSplits[target_split]; + split_remap_table[s] = target_split; + + // Allocate space for contacts + split.mContactBufferBegin = offset; + split.mContactBufferEnd = split.mContactBufferBegin + num_contacts_in_split[s]; + + // Allocate space for constraints + split.mConstraintBufferBegin = split.mContactBufferEnd; + split.mConstraintBufferEnd = split.mConstraintBufferBegin + num_constraints_in_split[s]; + + // Store start for each split + contact_buffer_cur[target_split] = mContactAndConstraintIndices + split.mContactBufferBegin; + constraint_buffer_cur[target_split] = mContactAndConstraintIndices + split.mConstraintBufferBegin; + + // Update offset + offset = split.mConstraintBufferEnd; + } + } + + // Split the contacts + for (uint c = 0; c < num_contacts_in_island; ++c) + { + uint split = split_remap_table[contact_split_idx[c]]; + *contact_buffer_cur[split]++ = contacts_start[c]; + } + + // Split the constraints + for (uint c = 0; c < num_constraints_in_island; ++c) + { + uint split = split_remap_table[constraint_split_idx[c]]; + *constraint_buffer_cur[split]++ = constraints_start[c]; + } + +#ifdef JPH_LARGE_ISLAND_SPLITTER_DEBUG + // Trace the size of all splits + uint sum = 0; + String stats; + for (uint s = 0; s < cNumSplits; ++s) + { + // If we've processed all splits, jump to the non-parallel split + if (s >= splits.GetNumSplits()) + s = cNonParallelSplitIdx; + + const Split &split = splits.mSplits[s]; + stats += StringFormat("g:%d:%d:%d, ", s, split.GetNumContacts(), split.GetNumConstraints()); + sum += split.GetNumItems(); + } + stats += StringFormat("sum: %d", sum); + Trace(stats.c_str()); +#endif // JPH_LARGE_ISLAND_SPLITTER_DEBUG + +#ifdef JPH_ENABLE_ASSERTS + for (uint s = 0; s < cNumSplits; ++s) + { + // If there are no more splits, process the non-parallel split + if (s >= splits.mNumSplits) + s = cNonParallelSplitIdx; + + // Check that we wrote all elements + Split &split = splits.mSplits[s]; + JPH_ASSERT(contact_buffer_cur[s] == mContactAndConstraintIndices + split.mContactBufferEnd); + JPH_ASSERT(constraint_buffer_cur[s] == mContactAndConstraintIndices + split.mConstraintBufferEnd); + } + +#ifdef JPH_DEBUG + // Validate that the splits are indeed not touching the same body + for (uint s = 0; s < splits.mNumSplits; ++s) + { + Array body_used(mNumActiveBodies, false); + + // Validate contacts + uint32 split_contacts_begin, split_contacts_end; + splits.GetContactsInSplit(s, split_contacts_begin, split_contacts_end); + for (uint32 *c = mContactAndConstraintIndices + split_contacts_begin; c < mContactAndConstraintIndices + split_contacts_end; ++c) + { + const Body *body1, *body2; + inContactManager.GetAffectedBodies(*c, body1, body2); + + uint32 idx1 = body1->GetIndexInActiveBodiesInternal(); + if (idx1 != Body::cInactiveIndex && body1->IsDynamic()) + { + JPH_ASSERT(!body_used[idx1]); + body_used[idx1] = true; + } + + uint32 idx2 = body2->GetIndexInActiveBodiesInternal(); + if (idx2 != Body::cInactiveIndex && body2->IsDynamic()) + { + JPH_ASSERT(!body_used[idx2]); + body_used[idx2] = true; + } + } + } +#endif // JPH_DEBUG +#endif // JPH_ENABLE_ASSERTS + + // Allow other threads to pick up this split island now + splits.StartFirstBatch(); + return true; +} + +LargeIslandSplitter::EStatus LargeIslandSplitter::FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration) +{ + // We can't be done when all islands haven't been submitted yet + uint num_splits_created = mNextSplitIsland.load(memory_order_acquire); + bool all_done = num_splits_created == mNumSplitIslands; + + // Loop over all split islands to find work + uint32 constraints_begin, constraints_end, contacts_begin, contacts_end; + for (Splits *s = mSplitIslands; s < mSplitIslands + num_splits_created; ++s) + switch (s->FetchNextBatch(constraints_begin, constraints_end, contacts_begin, contacts_end, outFirstIteration)) + { + case EStatus::AllBatchesDone: + break; + + case EStatus::WaitingForBatch: + all_done = false; + break; + + case EStatus::BatchRetrieved: + outSplitIslandIndex = uint(s - mSplitIslands); + outConstraintsBegin = mContactAndConstraintIndices + constraints_begin; + outConstraintsEnd = mContactAndConstraintIndices + constraints_end; + outContactsBegin = mContactAndConstraintIndices + contacts_begin; + outContactsEnd = mContactAndConstraintIndices + contacts_end; + return EStatus::BatchRetrieved; + } + + return all_done? EStatus::AllBatchesDone : EStatus::WaitingForBatch; +} + +void LargeIslandSplitter::MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch) +{ + uint num_items_processed = uint(inConstraintsEnd - inConstraintsBegin) + uint(inContactsEnd - inContactsBegin); + + JPH_ASSERT(inSplitIslandIndex < mNextSplitIsland.load(memory_order_relaxed)); + Splits &splits = mSplitIslands[inSplitIslandIndex]; + splits.MarkBatchProcessed(num_items_processed, outLastIteration, outFinalBatch); +} + +void LargeIslandSplitter::PrepareForSolvePositions() +{ + for (Splits *s = mSplitIslands, *s_end = mSplitIslands + mNumSplitIslands; s < s_end; ++s) + { + // Set the number of iterations to the number of position steps + s->mNumIterations = s->mNumPositionSteps; + + // We can start again from the first batch + s->StartFirstBatch(); + } +} + +void LargeIslandSplitter::Reset(TempAllocator *inTempAllocator) +{ + JPH_PROFILE_FUNCTION(); + + // Everything should have been used + JPH_ASSERT(mContactAndConstraintsNextFree.load(memory_order_relaxed) == mContactAndConstraintsSize); + JPH_ASSERT(mNextSplitIsland.load(memory_order_relaxed) == mNumSplitIslands); + + // Free split islands + if (mNumSplitIslands > 0) + { + inTempAllocator->Free(mSplitIslands, mNumSplitIslands * sizeof(Splits)); + mSplitIslands = nullptr; + + mNumSplitIslands = 0; + mNextSplitIsland.store(0, memory_order_relaxed); + } + + // Free contact and constraint buffers + if (mContactAndConstraintsSize > 0) + { + inTempAllocator->Free(mContactAndConstraintIndices, mContactAndConstraintsSize * sizeof(uint32)); + mContactAndConstraintIndices = nullptr; + + inTempAllocator->Free(mContactAndConstraintsSplitIdx, mContactAndConstraintsSize * sizeof(uint32)); + mContactAndConstraintsSplitIdx = nullptr; + + mContactAndConstraintsSize = 0; + mContactAndConstraintsNextFree.store(0, memory_order_relaxed); + } + + // Free split masks + if (mSplitMasks != nullptr) + { + inTempAllocator->Free(mSplitMasks, mNumActiveBodies * sizeof(SplitMask)); + mSplitMasks = nullptr; + + mNumActiveBodies = 0; + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/LargeIslandSplitter.h b/WickedEngine/Jolt/Physics/LargeIslandSplitter.h new file mode 100644 index 000000000..8e61d0930 --- /dev/null +++ b/WickedEngine/Jolt/Physics/LargeIslandSplitter.h @@ -0,0 +1,185 @@ +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class BodyID; +class IslandBuilder; +class TempAllocator; +class Constraint; +class BodyManager; +class ContactConstraintManager; +class CalculateSolverSteps; + +/// Assigns bodies in large islands to multiple groups that can run in parallel +/// +/// This basically implements what is described in: High-Performance Physical Simulations on Next-Generation Architecture with Many Cores by Chen et al. +/// See: http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf section "PARALLELIZATION METHODOLOGY" +class LargeIslandSplitter : public NonCopyable +{ +private: + using SplitMask = uint32; + +public: + static constexpr uint cNumSplits = sizeof(SplitMask) * 8; + static constexpr uint cNonParallelSplitIdx = cNumSplits - 1; + static constexpr uint cLargeIslandTreshold = 128; ///< If the number of constraints + contacts in an island is larger than this, we will try to split the island + + /// Status code for retrieving a batch + enum class EStatus + { + WaitingForBatch, ///< Work is expected to be available later + BatchRetrieved, ///< Work is being returned + AllBatchesDone, ///< No further work is expected from this + }; + + /// Describes a split of constraints and contacts + struct Split + { + inline uint GetNumContacts() const { return mContactBufferEnd - mContactBufferBegin; } + inline uint GetNumConstraints() const { return mConstraintBufferEnd - mConstraintBufferBegin; } + inline uint GetNumItems() const { return GetNumContacts() + GetNumConstraints(); } + + uint32 mContactBufferBegin; ///< Begin of the contact buffer (offset relative to mContactAndConstraintIndices) + uint32 mContactBufferEnd; ///< End of the contact buffer + + uint32 mConstraintBufferBegin; ///< Begin of the constraint buffer (offset relative to mContactAndConstraintIndices) + uint32 mConstraintBufferEnd; ///< End of the constraint buffer + }; + + /// Structure that describes the resulting splits from the large island splitter + class Splits + { + public: + inline uint GetNumSplits() const + { + return mNumSplits; + } + + inline void GetConstraintsInSplit(uint inSplitIndex, uint32 &outConstraintsBegin, uint32 &outConstraintsEnd) const + { + const Split &split = mSplits[inSplitIndex]; + outConstraintsBegin = split.mConstraintBufferBegin; + outConstraintsEnd = split.mConstraintBufferEnd; + } + + inline void GetContactsInSplit(uint inSplitIndex, uint32 &outContactsBegin, uint32 &outContactsEnd) const + { + const Split &split = mSplits[inSplitIndex]; + outContactsBegin = split.mContactBufferBegin; + outContactsEnd = split.mContactBufferEnd; + } + + /// Reset current status so that no work can be picked up from this split + inline void ResetStatus() + { + mStatus.store(StatusItemMask, memory_order_relaxed); + } + + /// Make the first batch available to other threads + inline void StartFirstBatch() + { + uint split_index = mNumSplits > 0? 0 : cNonParallelSplitIdx; + mStatus.store(uint64(split_index) << StatusSplitShift, memory_order_release); + } + + /// Fetch the next batch to process + EStatus FetchNextBatch(uint32 &outConstraintsBegin, uint32 &outConstraintsEnd, uint32 &outContactsBegin, uint32 &outContactsEnd, bool &outFirstIteration); + + /// Mark a batch as processed + void MarkBatchProcessed(uint inNumProcessed, bool &outLastIteration, bool &outFinalBatch); + + enum EIterationStatus : uint64 + { + StatusIterationMask = 0xffff000000000000, + StatusIterationShift = 48, + StatusSplitMask = 0x0000ffff00000000, + StatusSplitShift = 32, + StatusItemMask = 0x00000000ffffffff, + }; + + static inline int sGetIteration(uint64 inStatus) + { + return int((inStatus & StatusIterationMask) >> StatusIterationShift); + } + + static inline uint sGetSplit(uint64 inStatus) + { + return uint((inStatus & StatusSplitMask) >> StatusSplitShift); + } + + static inline uint sGetItem(uint64 inStatus) + { + return uint(inStatus & StatusItemMask); + } + + Split mSplits[cNumSplits]; ///< Data per split + uint32 mIslandIndex; ///< Index of the island that was split + uint mNumSplits; ///< Number of splits that were created (excluding the non-parallel split) + int mNumIterations; ///< Number of iterations to do + int mNumVelocitySteps; ///< Number of velocity steps to do (cached for 2nd sub step) + int mNumPositionSteps; ///< Number of position steps to do + atomic mStatus; ///< Status of the split, see EIterationStatus + atomic mItemsProcessed; ///< Number of items that have been marked as processed + }; + +public: + /// Destructor + ~LargeIslandSplitter(); + + /// Prepare the island splitter by allocating memory + void Prepare(const IslandBuilder &inIslandBuilder, uint32 inNumActiveBodies, TempAllocator *inTempAllocator); + + /// Assign two bodies to a split. Returns the split index. + uint AssignSplit(const Body *inBody1, const Body *inBody2); + + /// Force a body to be in a non parallel split. Returns the split index. + uint AssignToNonParallelSplit(const Body *inBody); + + /// Splits up an island, the created splits will be added to the list of batches and can be fetched with FetchNextBatch. Returns false if the island did not need splitting. + bool SplitIsland(uint32 inIslandIndex, const IslandBuilder &inIslandBuilder, const BodyManager &inBodyManager, const ContactConstraintManager &inContactManager, Constraint **inActiveConstraints, CalculateSolverSteps &ioStepsCalculator); + + /// Fetch the next batch to process, returns a handle in outSplitIslandIndex that must be provided to MarkBatchProcessed when complete + EStatus FetchNextBatch(uint &outSplitIslandIndex, uint32 *&outConstraintsBegin, uint32 *&outConstraintsEnd, uint32 *&outContactsBegin, uint32 *&outContactsEnd, bool &outFirstIteration); + + /// Mark a batch as processed + void MarkBatchProcessed(uint inSplitIslandIndex, const uint32 *inConstraintsBegin, const uint32 *inConstraintsEnd, const uint32 *inContactsBegin, const uint32 *inContactsEnd, bool &outLastIteration, bool &outFinalBatch); + + /// Get the island index of the island that was split for a particular split island index + inline uint32 GetIslandIndex(uint inSplitIslandIndex) const + { + JPH_ASSERT(inSplitIslandIndex < mNumSplitIslands); + return mSplitIslands[inSplitIslandIndex].mIslandIndex; + } + + /// Prepare the island splitter for iterating over the split islands again for position solving. Marks all batches as startable. + void PrepareForSolvePositions(); + + /// Reset the island splitter + void Reset(TempAllocator *inTempAllocator); + +private: + static constexpr uint cSplitCombineTreshold = 32; ///< If the number of constraints + contacts in a split is lower than this, we will merge this split into the 'non-parallel split' + static constexpr uint cBatchSize = 16; ///< Number of items to process in a constraint batch + + uint32 mNumActiveBodies = 0; ///< Cached number of active bodies + + SplitMask * mSplitMasks = nullptr; ///< Bits that indicate for each body in the BodyManager::mActiveBodies list which split they already belong to + + uint32 * mContactAndConstraintsSplitIdx = nullptr; ///< Buffer to store the split index per constraint or contact + uint32 * mContactAndConstraintIndices = nullptr; ///< Buffer to store the ordered constraint indices per split + uint mContactAndConstraintsSize = 0; ///< Total size of mContactAndConstraintsSplitIdx and mContactAndConstraintIndices + atomic mContactAndConstraintsNextFree { 0 }; ///< Next element that is free in both buffers + + uint mNumSplitIslands = 0; ///< Total number of islands that required splitting + Splits * mSplitIslands = nullptr; ///< List of islands that required splitting + atomic mNextSplitIsland = 0; ///< Next split island to pick from mSplitIslands +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsLock.cpp b/WickedEngine/Jolt/Physics/PhysicsLock.cpp new file mode 100644 index 000000000..690b5e582 --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsLock.cpp @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +#ifdef JPH_ENABLE_ASSERTS + +JPH_NAMESPACE_BEGIN + +thread_local PhysicsLock::LockData PhysicsLock::sLocks[4]; + +JPH_NAMESPACE_END + +#endif diff --git a/WickedEngine/Jolt/Physics/PhysicsLock.h b/WickedEngine/Jolt/Physics/PhysicsLock.h new file mode 100644 index 000000000..62cf72e99 --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsLock.h @@ -0,0 +1,169 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_ENABLE_ASSERTS + +/// This is the list of locks used by the physics engine, they need to be locked in a particular order (from top of the list to bottom of the list) in order to prevent deadlocks +enum class EPhysicsLockTypes +{ + BroadPhaseQuery = 1 << 0, + PerBody = 1 << 1, + BodiesList = 1 << 2, + BroadPhaseUpdate = 1 << 3, + ConstraintsList = 1 << 4, + ActiveBodiesList = 1 << 5, +}; + +/// A token that indicates the context of a lock (we use 1 per physics system and we use the body manager pointer because it's convenient) +class BodyManager; +using PhysicsLockContext = const BodyManager *; + +#endif // !JPH_ENABLE_ASSERTS + +/// Helpers to safely lock the different mutexes that are part of the physics system while preventing deadlock +/// Class that keeps track per thread which lock are taken and if the order of locking is correct +class PhysicsLock +{ +public: +#ifdef JPH_ENABLE_ASSERTS + /// Call before taking the lock + static inline void sCheckLock(PhysicsLockContext inContext, EPhysicsLockTypes inType) + { + uint32 &mutexes = sGetLockedMutexes(inContext); + JPH_ASSERT(uint32(inType) > mutexes, "A lock of same or higher priority was already taken, this can create a deadlock!"); + mutexes = mutexes | uint32(inType); + } + + /// Call after releasing the lock + static inline void sCheckUnlock(PhysicsLockContext inContext, EPhysicsLockTypes inType) + { + uint32 &mutexes = sGetLockedMutexes(inContext); + JPH_ASSERT((mutexes & uint32(inType)) != 0, "Mutex was not locked!"); + mutexes = mutexes & ~uint32(inType); + } +#endif // !JPH_ENABLE_ASSERTS + + template + static inline void sLock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);) + inMutex.lock(); + } + + template + static inline void sUnlock(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);) + inMutex.unlock(); + } + + template + static inline void sLockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckLock(inContext, inType);) + inMutex.lock_shared(); + } + + template + static inline void sUnlockShared(LockType &inMutex JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) + { + JPH_IF_ENABLE_ASSERTS(sCheckUnlock(inContext, inType);) + inMutex.unlock_shared(); + } + +#ifdef JPH_ENABLE_ASSERTS +private: + struct LockData + { + uint32 mLockedMutexes = 0; + PhysicsLockContext mContext = nullptr; + }; + + static thread_local LockData sLocks[4]; + + // Helper function to find the locked mutexes for a particular context + static uint32 & sGetLockedMutexes(PhysicsLockContext inContext) + { + // If we find a matching context we can use it + for (LockData &l : sLocks) + if (l.mContext == inContext) + return l.mLockedMutexes; + + // Otherwise we look for an entry that is not in use + for (LockData &l : sLocks) + if (l.mLockedMutexes == 0) + { + l.mContext = inContext; + return l.mLockedMutexes; + } + + JPH_ASSERT(false, "Too many physics systems locked at the same time!"); + return sLocks[0].mLockedMutexes; + } +#endif // !JPH_ENABLE_ASSERTS +}; + +/// Helper class that is similar to std::unique_lock +template +class UniqueLock : public NonCopyable +{ +public: + explicit UniqueLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) : + mLock(inLock) +#ifdef JPH_ENABLE_ASSERTS + , mContext(inContext), + mType(inType) +#endif // JPH_ENABLE_ASSERTS + { + PhysicsLock::sLock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + + ~UniqueLock() + { + PhysicsLock::sUnlock(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + +private: + LockType & mLock; +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mContext; + EPhysicsLockTypes mType; +#endif // JPH_ENABLE_ASSERTS +}; + +/// Helper class that is similar to std::shared_lock +template +class SharedLock : public NonCopyable +{ +public: + explicit SharedLock(LockType &inLock JPH_IF_ENABLE_ASSERTS(, PhysicsLockContext inContext, EPhysicsLockTypes inType)) : + mLock(inLock) +#ifdef JPH_ENABLE_ASSERTS + , mContext(inContext) + , mType(inType) +#endif // JPH_ENABLE_ASSERTS + { + PhysicsLock::sLockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + + ~SharedLock() + { + PhysicsLock::sUnlockShared(mLock JPH_IF_ENABLE_ASSERTS(, mContext, mType)); + } + +private: + LockType & mLock; +#ifdef JPH_ENABLE_ASSERTS + PhysicsLockContext mContext; + EPhysicsLockTypes mType; +#endif // JPH_ENABLE_ASSERTS +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsScene.cpp b/WickedEngine/Jolt/Physics/PhysicsScene.cpp new file mode 100644 index 000000000..1e9c60d23 --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsScene.cpp @@ -0,0 +1,261 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene) +{ + JPH_ADD_ATTRIBUTE(PhysicsScene, mBodies) + JPH_ADD_ATTRIBUTE(PhysicsScene, mConstraints) + JPH_ADD_ATTRIBUTE(PhysicsScene, mSoftBodies) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(PhysicsScene::ConnectedConstraint) +{ + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mSettings) + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody1) + JPH_ADD_ATTRIBUTE(PhysicsScene::ConnectedConstraint, mBody2) +} + +void PhysicsScene::AddBody(const BodyCreationSettings &inBody) +{ + mBodies.push_back(inBody); +} + +void PhysicsScene::AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2) +{ + mConstraints.emplace_back(inConstraint, inBody1, inBody2); +} + +void PhysicsScene::AddSoftBody(const SoftBodyCreationSettings &inSoftBody) +{ + mSoftBodies.push_back(inSoftBody); +} + +bool PhysicsScene::FixInvalidScales() +{ + const Vec3 unit_scale = Vec3::sReplicate(1.0f); + + bool success = true; + for (BodyCreationSettings &b : mBodies) + { + // Test if there is an invalid scale in the shape hierarchy + const Shape *shape = b.GetShape(); + if (!shape->IsValidScale(unit_scale)) + { + // Fix it up + Shape::ShapeResult result = shape->ScaleShape(unit_scale); + if (result.IsValid()) + b.SetShape(result.Get()); + else + success = false; + } + } + return success; +} + +bool PhysicsScene::CreateBodies(PhysicsSystem *inSystem) const +{ + BodyInterface &bi = inSystem->GetBodyInterface(); + + BodyIDVector body_ids; + body_ids.reserve(mBodies.size() + mSoftBodies.size()); + + // Create bodies + for (const BodyCreationSettings &b : mBodies) + { + const Body *body = bi.CreateBody(b); + if (body == nullptr) + break; + body_ids.push_back(body->GetID()); + } + + // Create soft bodies + for (const SoftBodyCreationSettings &b : mSoftBodies) + { + const Body *body = bi.CreateSoftBody(b); + if (body == nullptr) + break; + body_ids.push_back(body->GetID()); + } + + // Batch add bodies + BodyIDVector temp_body_ids = body_ids; // Body ID's get shuffled by AddBodiesPrepare + BodyInterface::AddState add_state = bi.AddBodiesPrepare(temp_body_ids.data(), (int)temp_body_ids.size()); + bi.AddBodiesFinalize(temp_body_ids.data(), (int)temp_body_ids.size(), add_state, EActivation::Activate); + + // If not all bodies are created, creating constraints will be unreliable + if (body_ids.size() != mBodies.size() + mSoftBodies.size()) + return false; + + // Create constraints + for (const ConnectedConstraint &cc : mConstraints) + { + BodyID body1_id = cc.mBody1 == cFixedToWorld? BodyID() : body_ids[cc.mBody1]; + BodyID body2_id = cc.mBody2 == cFixedToWorld? BodyID() : body_ids[cc.mBody2]; + Constraint *c = bi.CreateConstraint(cc.mSettings, body1_id, body2_id); + inSystem->AddConstraint(c); + } + + // Everything was created + return true; +} + +void PhysicsScene::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const +{ + BodyCreationSettings::ShapeToIDMap shape_to_id; + BodyCreationSettings::MaterialToIDMap material_to_id; + BodyCreationSettings::GroupFilterToIDMap group_filter_to_id; + SoftBodyCreationSettings::SharedSettingsToIDMap settings_to_id; + + // Save bodies + inStream.Write((uint32)mBodies.size()); + for (const BodyCreationSettings &b : mBodies) + b.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr); + + // Save constraints + inStream.Write((uint32)mConstraints.size()); + for (const ConnectedConstraint &cc : mConstraints) + { + cc.mSettings->SaveBinaryState(inStream); + inStream.Write(cc.mBody1); + inStream.Write(cc.mBody2); + } + + // Save soft bodies + inStream.Write((uint32)mSoftBodies.size()); + for (const SoftBodyCreationSettings &b : mSoftBodies) + b.SaveWithChildren(inStream, &settings_to_id, &material_to_id, inSaveGroupFilter? &group_filter_to_id : nullptr); +} + +PhysicsScene::PhysicsSceneResult PhysicsScene::sRestoreFromBinaryState(StreamIn &inStream) +{ + PhysicsSceneResult result; + + // Create scene + Ref scene = new PhysicsScene(); + + BodyCreationSettings::IDToShapeMap id_to_shape; + BodyCreationSettings::IDToMaterialMap id_to_material; + BodyCreationSettings::IDToGroupFilterMap id_to_group_filter; + SoftBodyCreationSettings::IDToSharedSettingsMap id_to_settings; + + // Reserve some memory to avoid frequent reallocations + id_to_shape.reserve(1024); + id_to_material.reserve(128); + id_to_group_filter.reserve(128); + + // Read bodies + uint32 len = 0; + inStream.Read(len); + scene->mBodies.resize(len); + for (BodyCreationSettings &b : scene->mBodies) + { + // Read creation settings + BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter); + if (bcs_result.HasError()) + { + result.SetError(bcs_result.GetError()); + return result; + } + b = bcs_result.Get(); + } + + // Read constraints + len = 0; + inStream.Read(len); + scene->mConstraints.resize(len); + for (ConnectedConstraint &cc : scene->mConstraints) + { + ConstraintSettings::ConstraintResult c_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (c_result.HasError()) + { + result.SetError(c_result.GetError()); + return result; + } + cc.mSettings = StaticCast(c_result.Get()); + inStream.Read(cc.mBody1); + inStream.Read(cc.mBody2); + } + + // Read soft bodies + len = 0; + inStream.Read(len); + scene->mSoftBodies.resize(len); + for (SoftBodyCreationSettings &b : scene->mSoftBodies) + { + // Read creation settings + SoftBodyCreationSettings::SBCSResult sbcs_result = SoftBodyCreationSettings::sRestoreWithChildren(inStream, id_to_settings, id_to_material, id_to_group_filter); + if (sbcs_result.HasError()) + { + result.SetError(sbcs_result.GetError()); + return result; + } + b = sbcs_result.Get(); + } + + result.Set(scene); + return result; +} + +void PhysicsScene::FromPhysicsSystem(const PhysicsSystem *inSystem) +{ + // This map will track where each body went in mBodies + using BodyIDToIdxMap = UnorderedMap; + BodyIDToIdxMap body_id_to_idx; + + // Map invalid ID + body_id_to_idx[BodyID()] = cFixedToWorld; + + // Get all bodies + BodyIDVector body_ids; + inSystem->GetBodies(body_ids); + + // Loop over all bodies + const BodyLockInterface &bli = inSystem->GetBodyLockInterface(); + for (const BodyID &id : body_ids) + { + BodyLockRead lock(bli, id); + if (lock.Succeeded()) + { + // Store location of body + body_id_to_idx[id] = (uint32)mBodies.size(); + + const Body &body = lock.GetBody(); + + // Convert to body creation settings + if (body.IsRigidBody()) + AddBody(body.GetBodyCreationSettings()); + else + AddSoftBody(body.GetSoftBodyCreationSettings()); + } + } + + // Loop over all constraints + Constraints constraints = inSystem->GetConstraints(); + for (const Constraint *c : constraints) + if (c->GetType() == EConstraintType::TwoBodyConstraint) + { + // Cast to two body constraint + const TwoBodyConstraint *tbc = static_cast(c); + + // Find the body indices + BodyIDToIdxMap::const_iterator b1 = body_id_to_idx.find(tbc->GetBody1()->GetID()); + BodyIDToIdxMap::const_iterator b2 = body_id_to_idx.find(tbc->GetBody2()->GetID()); + JPH_ASSERT(b1 != body_id_to_idx.end() && b2 != body_id_to_idx.end()); + + // Create constraint settings and add the constraint + Ref settings = c->GetConstraintSettings(); + AddConstraint(StaticCast(settings), b1->second, b2->second); + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsScene.h b/WickedEngine/Jolt/Physics/PhysicsScene.h new file mode 100644 index 000000000..530b79d61 --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsScene.h @@ -0,0 +1,104 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Contains the creation settings of a set of bodies +class JPH_EXPORT PhysicsScene : public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, PhysicsScene) + + /// Add a body to the scene + void AddBody(const BodyCreationSettings &inBody); + + /// Body constant to use to indicate that the constraint is attached to the fixed world + static constexpr uint32 cFixedToWorld = 0xffffffff; + + /// Add a constraint to the scene + /// @param inConstraint Constraint settings + /// @param inBody1 Index in the bodies list of first body to attach constraint to + /// @param inBody2 Index in the bodies list of the second body to attach constraint to + void AddConstraint(const TwoBodyConstraintSettings *inConstraint, uint32 inBody1, uint32 inBody2); + + /// Add a soft body to the scene + void AddSoftBody(const SoftBodyCreationSettings &inSoftBody); + + /// Get number of bodies in this scene + size_t GetNumBodies() const { return mBodies.size(); } + + /// Access to the body settings for this scene + const Array & GetBodies() const { return mBodies; } + Array & GetBodies() { return mBodies; } + + /// A constraint and how it is connected to the bodies in the scene + class ConnectedConstraint + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, ConnectedConstraint) + + ConnectedConstraint() = default; + ConnectedConstraint(const TwoBodyConstraintSettings *inSettings, uint inBody1, uint inBody2) : mSettings(inSettings), mBody1(inBody1), mBody2(inBody2) { } + + RefConst mSettings; ///< Constraint settings + uint32 mBody1; ///< Index of first body (in mBodies) + uint32 mBody2; ///< Index of second body (in mBodies) + }; + + /// Get number of constraints in this scene + size_t GetNumConstraints() const { return mConstraints.size(); } + + /// Access to the constraints for this scene + const Array & GetConstraints() const { return mConstraints; } + Array & GetConstraints() { return mConstraints; } + + /// Get number of bodies in this scene + size_t GetNumSoftBodies() const { return mSoftBodies.size(); } + + /// Access to the soft body settings for this scene + const Array & GetSoftBodies() const { return mSoftBodies; } + Array & GetSoftBodies() { return mSoftBodies; } + + /// Instantiate all bodies, returns false if not all bodies could be created + bool CreateBodies(PhysicsSystem *inSystem) const; + + /// Go through all body creation settings and fix shapes that are scaled incorrectly (note this will change the scene a bit). + /// @return False when not all scales could be fixed. + bool FixInvalidScales(); + + /// Saves the state of this object in binary form to inStream. + /// @param inStream The stream to save the state to + /// @param inSaveShapes If the shapes should be saved as well (these could be shared between physics scenes, in which case the calling application may want to write custom code to restore them) + /// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared) + void SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const; + + using PhysicsSceneResult = Result>; + + /// Restore a saved scene from inStream + static PhysicsSceneResult sRestoreFromBinaryState(StreamIn &inStream); + + /// For debugging purposes: Construct a scene from the current state of the physics system + void FromPhysicsSystem(const PhysicsSystem *inSystem); + +private: + /// The bodies that are part of this scene + Array mBodies; + + /// Constraints that are part of this scene + Array mConstraints; + + /// Soft bodies that are part of this scene + Array mSoftBodies; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsSettings.h b/WickedEngine/Jolt/Physics/PhysicsSettings.h new file mode 100644 index 000000000..eb8ecbbea --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsSettings.h @@ -0,0 +1,119 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// If objects are closer than this distance, they are considered to be colliding (used for GJK) (unit: meter) +constexpr float cDefaultCollisionTolerance = 1.0e-4f; + +/// A factor that determines the accuracy of the penetration depth calculation. If the change of the squared distance is less than tolerance * current_penetration_depth^2 the algorithm will terminate. (unit: dimensionless) +constexpr float cDefaultPenetrationTolerance = 1.0e-4f; ///< Stop when there's less than 1% change + +/// How much padding to add around objects +constexpr float cDefaultConvexRadius = 0.05f; + +/// Used by (Tapered)CapsuleShape to determine when supporting face is an edge rather than a point (unit: meter) +static constexpr float cCapsuleProjectionSlop = 0.02f; + +/// Maximum amount of jobs to allow +constexpr int cMaxPhysicsJobs = 2048; + +/// Maximum amount of barriers to allow +constexpr int cMaxPhysicsBarriers = 8; + +struct PhysicsSettings +{ + JPH_OVERRIDE_NEW_DELETE + + /// Size of body pairs array, corresponds to the maximum amount of potential body pairs that can be in flight at any time. + /// Setting this to a low value will use less memory but slow down simulation as threads may run out of narrow phase work. + int mMaxInFlightBodyPairs = 16384; + + /// How many PhysicsStepListeners to notify in 1 batch + int mStepListenersBatchSize = 8; + + /// How many step listener batches are needed before spawning another job (set to INT_MAX if no parallelism is desired) + int mStepListenerBatchesPerJob = 1; + + /// Baumgarte stabilization factor (how much of the position error to 'fix' in 1 update) (unit: dimensionless, 0 = nothing, 1 = 100%) + float mBaumgarte = 0.2f; + + /// Radius around objects inside which speculative contact points will be detected. Note that if this is too big + /// you will get ghost collisions as speculative contacts are based on the closest points during the collision detection + /// step which may not be the actual closest points by the time the two objects hit (unit: meters) + float mSpeculativeContactDistance = 0.02f; + + /// How much bodies are allowed to sink into each other (unit: meters) + float mPenetrationSlop = 0.02f; + + /// Fraction of its inner radius a body must move per step to enable casting for the LinearCast motion quality + float mLinearCastThreshold = 0.75f; + + /// Fraction of its inner radius a body may penetrate another body for the LinearCast motion quality + float mLinearCastMaxPenetration = 0.25f; + + /// Max squared distance to use to determine if two points are on the same plane for determining the contact manifold between two shape faces (unit: meter^2) + float mManifoldToleranceSq = 1.0e-6f; + + /// Maximum distance to correct in a single iteration when solving position constraints (unit: meters) + float mMaxPenetrationDistance = 0.2f; + + /// Maximum relative delta position for body pairs to be able to reuse collision results from last frame (units: meter^2) + float mBodyPairCacheMaxDeltaPositionSq = Square(0.001f); ///< 1 mm + + /// Maximum relative delta orientation for body pairs to be able to reuse collision results from last frame, stored as cos(max angle / 2) + float mBodyPairCacheCosMaxDeltaRotationDiv2 = 0.99984769515639123915701155881391f; ///< cos(2 degrees / 2) + + /// Maximum angle between normals that allows manifolds between different sub shapes of the same body pair to be combined + float mContactNormalCosMaxDeltaRotation = 0.99619469809174553229501040247389f; ///< cos(5 degree) + + /// Maximum allowed distance between old and new contact point to preserve contact forces for warm start (units: meter^2) + float mContactPointPreserveLambdaMaxDistSq = Square(0.01f); ///< 1 cm + + /// Number of solver velocity iterations to run + /// Note that this needs to be >= 2 in order for friction to work (friction is applied using the non-penetration impulse from the previous iteration) + uint mNumVelocitySteps = 10; + + /// Number of solver position iterations to run + uint mNumPositionSteps = 2; + + /// Minimal velocity needed before a collision can be elastic (unit: m) + float mMinVelocityForRestitution = 1.0f; + + /// Time before object is allowed to go to sleep (unit: seconds) + float mTimeBeforeSleep = 0.5f; + + /// Velocity of points on bounding box of object below which an object can be considered sleeping (unit: m/s) + float mPointVelocitySleepThreshold = 0.03f; + + /// By default the simulation is deterministic, it is possible to turn this off by setting this setting to false. This will make the simulation run faster but it will no longer be deterministic. + bool mDeterministicSimulation = true; + + ///@name These variables are mainly for debugging purposes, they allow turning on/off certain subsystems. You probably want to leave them alone. + ///@{ + + /// Whether or not to use warm starting for constraints (initially applying previous frames impulses) + bool mConstraintWarmStart = true; + + /// Whether or not to use the body pair cache, which removes the need for narrow phase collision detection when orientation between two bodies didn't change + bool mUseBodyPairContactCache = true; + + /// Whether or not to reduce manifolds with similar contact normals into one contact manifold (see description at Body::SetUseManifoldReduction) + bool mUseManifoldReduction = true; + + /// If we split up large islands into smaller parallel batches of work (to improve performance) + bool mUseLargeIslandSplitter = true; + + /// If objects can go to sleep or not + bool mAllowSleeping = true; + + /// When false, we prevent collision against non-active (shared) edges. Mainly for debugging the algorithm. + bool mCheckActiveEdges = true; + + ///@} +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsStepListener.h b/WickedEngine/Jolt/Physics/PhysicsStepListener.h new file mode 100644 index 000000000..80cf0519b --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsStepListener.h @@ -0,0 +1,27 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// A listener class that receives a callback before every physics simulation step +class JPH_EXPORT PhysicsStepListener +{ +public: + /// Ensure virtual destructor + virtual ~PhysicsStepListener() = default; + + /// Called before every simulation step (received inCollisionSteps times for every PhysicsSystem::Update(...) call) + /// This is called while all body and constraint mutexes are locked. You can read/write bodies and constraints but not add/remove them. + /// Multiple listeners can be executed in parallel and it is the responsibility of the listener to avoid race conditions. + /// The best way to do this is to have each step listener operate on a subset of the bodies and constraints + /// and making sure that these bodies and constraints are not touched by any other step listener. + /// Note that this function is not called if there aren't any active bodies or when the physics system is updated with 0 delta time. + virtual void OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsSystem.cpp b/WickedEngine/Jolt/Physics/PhysicsSystem.cpp new file mode 100644 index 000000000..2aad1c0b6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsSystem.cpp @@ -0,0 +1,2702 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +bool PhysicsSystem::sDrawMotionQualityLinearCast = false; +#endif // JPH_DEBUG_RENDERER + +//#define BROAD_PHASE BroadPhaseBruteForce +#define BROAD_PHASE BroadPhaseQuadTree + +static const Color cColorUpdateBroadPhaseFinalize = Color::sGetDistinctColor(1); +static const Color cColorUpdateBroadPhasePrepare = Color::sGetDistinctColor(2); +static const Color cColorFindCollisions = Color::sGetDistinctColor(3); +static const Color cColorApplyGravity = Color::sGetDistinctColor(4); +static const Color cColorSetupVelocityConstraints = Color::sGetDistinctColor(5); +static const Color cColorBuildIslandsFromConstraints = Color::sGetDistinctColor(6); +static const Color cColorDetermineActiveConstraints = Color::sGetDistinctColor(7); +static const Color cColorFinalizeIslands = Color::sGetDistinctColor(8); +static const Color cColorContactRemovedCallbacks = Color::sGetDistinctColor(9); +static const Color cColorBodySetIslandIndex = Color::sGetDistinctColor(10); +static const Color cColorStartNextStep = Color::sGetDistinctColor(11); +static const Color cColorSolveVelocityConstraints = Color::sGetDistinctColor(12); +static const Color cColorPreIntegrateVelocity = Color::sGetDistinctColor(13); +static const Color cColorIntegrateVelocity = Color::sGetDistinctColor(14); +static const Color cColorPostIntegrateVelocity = Color::sGetDistinctColor(15); +static const Color cColorResolveCCDContacts = Color::sGetDistinctColor(16); +static const Color cColorSolvePositionConstraints = Color::sGetDistinctColor(17); +static const Color cColorFindCCDContacts = Color::sGetDistinctColor(18); +static const Color cColorStepListeners = Color::sGetDistinctColor(19); +static const Color cColorSoftBodyPrepare = Color::sGetDistinctColor(20); +static const Color cColorSoftBodyCollide = Color::sGetDistinctColor(21); +static const Color cColorSoftBodySimulate = Color::sGetDistinctColor(22); +static const Color cColorSoftBodyFinalize = Color::sGetDistinctColor(23); + +PhysicsSystem::~PhysicsSystem() +{ + // Remove broadphase + delete mBroadPhase; +} + +void PhysicsSystem::Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter) +{ + mObjectVsBroadPhaseLayerFilter = &inObjectVsBroadPhaseLayerFilter; + mObjectLayerPairFilter = &inObjectLayerPairFilter; + + // Initialize body manager + mBodyManager.Init(inMaxBodies, inNumBodyMutexes, inBroadPhaseLayerInterface); + + // Create broadphase + mBroadPhase = new BROAD_PHASE(); + mBroadPhase->Init(&mBodyManager, inBroadPhaseLayerInterface); + + // Init contact constraint manager + mContactManager.Init(inMaxBodyPairs, inMaxContactConstraints); + + // Init islands builder + mIslandBuilder.Init(inMaxBodies); + + // Initialize body interface + mBodyInterfaceLocking.Init(mBodyLockInterfaceLocking, mBodyManager, *mBroadPhase); + mBodyInterfaceNoLock.Init(mBodyLockInterfaceNoLock, mBodyManager, *mBroadPhase); + + // Initialize narrow phase query + mNarrowPhaseQueryLocking.Init(mBodyLockInterfaceLocking, *mBroadPhase); + mNarrowPhaseQueryNoLock.Init(mBodyLockInterfaceNoLock, *mBroadPhase); +} + +void PhysicsSystem::OptimizeBroadPhase() +{ + mBroadPhase->Optimize(); +} + +void PhysicsSystem::AddStepListener(PhysicsStepListener *inListener) +{ + lock_guard lock(mStepListenersMutex); + + JPH_ASSERT(std::find(mStepListeners.begin(), mStepListeners.end(), inListener) == mStepListeners.end()); + mStepListeners.push_back(inListener); +} + +void PhysicsSystem::RemoveStepListener(PhysicsStepListener *inListener) +{ + lock_guard lock(mStepListenersMutex); + + StepListeners::iterator i = std::find(mStepListeners.begin(), mStepListeners.end(), inListener); + JPH_ASSERT(i != mStepListeners.end()); + *i = mStepListeners.back(); + mStepListeners.pop_back(); +} + +EPhysicsUpdateError PhysicsSystem::Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem) +{ + JPH_PROFILE_FUNCTION(); + + JPH_DET_LOG("PhysicsSystem::Update: dt: " << inDeltaTime << " steps: " << inCollisionSteps); + + JPH_ASSERT(inCollisionSteps > 0); + JPH_ASSERT(inDeltaTime >= 0.0f); + + // Sync point for the broadphase. This will allow it to do clean up operations without having any mutexes locked yet. + mBroadPhase->FrameSync(); + + // If there are no active bodies or there's no time delta + uint32 num_active_rigid_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + uint32 num_active_soft_bodies = mBodyManager.GetNumActiveBodies(EBodyType::SoftBody); + if ((num_active_rigid_bodies == 0 && num_active_soft_bodies == 0) || inDeltaTime <= 0.0f) + { + mBodyManager.LockAllBodies(); + + // Update broadphase + mBroadPhase->LockModifications(); + BroadPhase::UpdateState update_state = mBroadPhase->UpdatePrepare(); + mBroadPhase->UpdateFinalize(update_state); + mBroadPhase->UnlockModifications(); + + // Call contact removal callbacks from contacts that existed in the previous update + mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(0, 0); + + mBodyManager.UnlockAllBodies(); + return EPhysicsUpdateError::None; + } + + // Calculate ratio between current and previous frame delta time to scale initial constraint forces + float step_delta_time = inDeltaTime / inCollisionSteps; + float warm_start_impulse_ratio = mPhysicsSettings.mConstraintWarmStart && mPreviousStepDeltaTime > 0.0f? step_delta_time / mPreviousStepDeltaTime : 0.0f; + mPreviousStepDeltaTime = step_delta_time; + + // Create the context used for passing information between jobs + PhysicsUpdateContext context(*inTempAllocator); + context.mPhysicsSystem = this; + context.mJobSystem = inJobSystem; + context.mBarrier = inJobSystem->CreateBarrier(); + context.mIslandBuilder = &mIslandBuilder; + context.mStepDeltaTime = step_delta_time; + context.mWarmStartImpulseRatio = warm_start_impulse_ratio; + context.mSteps.resize(inCollisionSteps); + + // Allocate space for body pairs + JPH_ASSERT(context.mBodyPairs == nullptr); + context.mBodyPairs = static_cast(inTempAllocator->Allocate(sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs)); + + // Lock all bodies for write so that we can freely touch them + mStepListenersMutex.lock(); + mBodyManager.LockAllBodies(); + mBroadPhase->LockModifications(); + + // Get max number of concurrent jobs + int max_concurrency = context.GetMaxConcurrency(); + + // Calculate how many step listener jobs we spawn + int num_step_listener_jobs = mStepListeners.empty()? 0 : max(1, min((int)mStepListeners.size() / mPhysicsSettings.mStepListenersBatchSize / mPhysicsSettings.mStepListenerBatchesPerJob, max_concurrency)); + + // Number of gravity jobs depends on the amount of active bodies. + // Launch max 1 job per batch of active bodies + // Leave 1 thread for update broadphase prepare and 1 for determine active constraints + int num_apply_gravity_jobs = max(1, min(((int)num_active_rigid_bodies + cApplyGravityBatchSize - 1) / cApplyGravityBatchSize, max_concurrency - 2)); + + // Number of determine active constraints jobs to run depends on number of constraints. + // Leave 1 thread for update broadphase prepare and 1 for apply gravity + int num_determine_active_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cDetermineActiveConstraintsBatchSize - 1) / cDetermineActiveConstraintsBatchSize, max_concurrency - 2)); + + // Number of setup velocity constraints jobs to run depends on number of constraints. + int num_setup_velocity_constraints_jobs = max(1, min(((int)mConstraintManager.GetNumConstraints() + cSetupVelocityConstraintsBatchSize - 1) / cSetupVelocityConstraintsBatchSize, max_concurrency)); + + // Number of find collisions jobs to run depends on number of active bodies. + // Note that when we have more than 1 thread, we always spawn at least 2 find collisions jobs so that the first job can wait for build islands from constraints + // (which may activate additional bodies that need to be processed) while the second job can start processing collision work. + int num_find_collisions_jobs = max(max_concurrency == 1? 1 : 2, min(((int)num_active_rigid_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_concurrency)); + + // Number of integrate velocity jobs depends on number of active bodies. + int num_integrate_velocity_jobs = max(1, min(((int)num_active_rigid_bodies + cIntegrateVelocityBatchSize - 1) / cIntegrateVelocityBatchSize, max_concurrency)); + + { + JPH_PROFILE("Build Jobs"); + + // Iterate over collision steps + for (int step_idx = 0; step_idx < inCollisionSteps; ++step_idx) + { + bool is_first_step = step_idx == 0; + bool is_last_step = step_idx == inCollisionSteps - 1; + + PhysicsUpdateContext::Step &step = context.mSteps[step_idx]; + step.mContext = &context; + step.mIsFirst = is_first_step; + step.mIsLast = is_last_step; + + // Create job to do broadphase finalization + // This job must finish before integrating velocities. Until then the positions will not be updated neither will bodies be added / removed. + step.mUpdateBroadphaseFinalize = inJobSystem->CreateJob("UpdateBroadPhaseFinalize", cColorUpdateBroadPhaseFinalize, [&context, &step]() + { + // Validate that all find collision jobs have stopped + JPH_ASSERT(step.mActiveFindCollisionJobs == 0); + + // Finalize the broadphase update + context.mPhysicsSystem->mBroadPhase->UpdateFinalize(step.mBroadPhaseUpdateState); + + // Signal that it is done + step.mPreIntegrateVelocity.RemoveDependency(); + }, num_find_collisions_jobs + 2); // depends on: find collisions, broadphase prepare update, finish building jobs + + // The immediate jobs below are only immediate for the first step, the all finished job will kick them for the next step + int previous_step_dependency_count = is_first_step? 0 : 1; + + // Start job immediately: Start the prepare broadphase + // Must be done under body lock protection since the order is body locks then broadphase mutex + // If this is turned around the RemoveBody call will hang since it locks in that order + step.mBroadPhasePrepare = inJobSystem->CreateJob("UpdateBroadPhasePrepare", cColorUpdateBroadPhasePrepare, [&context, &step]() + { + // Prepare the broadphase update + step.mBroadPhaseUpdateState = context.mPhysicsSystem->mBroadPhase->UpdatePrepare(); + + // Now the finalize can run (if other dependencies are met too) + step.mUpdateBroadphaseFinalize.RemoveDependency(); + }, previous_step_dependency_count); + + // This job will find all collisions + step.mBodyPairQueues.resize(max_concurrency); + step.mMaxBodyPairsPerQueue = mPhysicsSettings.mMaxInFlightBodyPairs / max_concurrency; + step.mActiveFindCollisionJobs = ~PhysicsUpdateContext::JobMask(0) >> (sizeof(PhysicsUpdateContext::JobMask) * 8 - num_find_collisions_jobs); + step.mFindCollisions.resize(num_find_collisions_jobs); + for (int i = 0; i < num_find_collisions_jobs; ++i) + { + // Build islands from constraints may activate additional bodies, so the first job will wait for this to finish in order to not miss any active bodies + int num_dep_build_islands_from_constraints = i == 0? 1 : 0; + step.mFindCollisions[i] = inJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [&step, i]() + { + step.mContext->mPhysicsSystem->JobFindCollisions(&step, i); + }, num_apply_gravity_jobs + num_determine_active_constraints_jobs + 1 + num_dep_build_islands_from_constraints); // depends on: apply gravity, determine active constraints, finish building jobs, build islands from constraints + } + + if (is_first_step) + { + #ifdef JPH_ENABLE_ASSERTS + // Don't allow write operations to the active bodies list + mBodyManager.SetActiveBodiesLocked(true); + #endif + + // Store the number of active bodies at the start of the step + step.mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + + // Lock all constraints + mConstraintManager.LockAllConstraints(); + + // Allocate memory for storing the active constraints + JPH_ASSERT(context.mActiveConstraints == nullptr); + context.mActiveConstraints = static_cast(inTempAllocator->Allocate(mConstraintManager.GetNumConstraints() * sizeof(Constraint *))); + + // Prepare contact buffer + mContactManager.PrepareConstraintBuffer(&context); + + // Setup island builder + mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), context.mTempAllocator); + } + + // This job applies gravity to all active bodies + step.mApplyGravity.resize(num_apply_gravity_jobs); + for (int i = 0; i < num_apply_gravity_jobs; ++i) + step.mApplyGravity[i] = inJobSystem->CreateJob("ApplyGravity", cColorApplyGravity, [&context, &step]() + { + context.mPhysicsSystem->JobApplyGravity(&context, &step); + + JobHandle::sRemoveDependencies(step.mFindCollisions); + }, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners) + + // This job will setup velocity constraints for non-collision constraints + step.mSetupVelocityConstraints.resize(num_setup_velocity_constraints_jobs); + for (int i = 0; i < num_setup_velocity_constraints_jobs; ++i) + step.mSetupVelocityConstraints[i] = inJobSystem->CreateJob("SetupVelocityConstraints", cColorSetupVelocityConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSetupVelocityConstraints(context.mStepDeltaTime, &step); + + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + }, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs + + // This job will build islands from constraints + step.mBuildIslandsFromConstraints = inJobSystem->CreateJob("BuildIslandsFromConstraints", cColorBuildIslandsFromConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobBuildIslandsFromConstraints(&context, &step); + + step.mFindCollisions[0].RemoveDependency(); // The first collisions job cannot start running until we've finished building islands and activated all bodies + step.mFinalizeIslands.RemoveDependency(); + }, num_determine_active_constraints_jobs + 1); // depends on: determine active constraints, finish building jobs + + // This job determines active constraints + step.mDetermineActiveConstraints.resize(num_determine_active_constraints_jobs); + for (int i = 0; i < num_determine_active_constraints_jobs; ++i) + step.mDetermineActiveConstraints[i] = inJobSystem->CreateJob("DetermineActiveConstraints", cColorDetermineActiveConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobDetermineActiveConstraints(&step); + + step.mBuildIslandsFromConstraints.RemoveDependency(); + + // Kick these jobs last as they will use up all CPU cores leaving no space for the previous job, we prefer setup velocity constraints to finish first so we kick it first + JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints); + JobHandle::sRemoveDependencies(step.mFindCollisions); + }, num_step_listener_jobs > 0? num_step_listener_jobs : previous_step_dependency_count); // depends on: step listeners (or previous step if no step listeners) + + // This job calls the step listeners + step.mStepListeners.resize(num_step_listener_jobs); + for (int i = 0; i < num_step_listener_jobs; ++i) + step.mStepListeners[i] = inJobSystem->CreateJob("StepListeners", cColorStepListeners, [&context, &step]() + { + // Call the step listeners + context.mPhysicsSystem->JobStepListeners(&step); + + // Kick apply gravity and determine active constraint jobs + JobHandle::sRemoveDependencies(step.mApplyGravity); + JobHandle::sRemoveDependencies(step.mDetermineActiveConstraints); + }, previous_step_dependency_count); + + // Unblock the previous step + if (!is_first_step) + context.mSteps[step_idx - 1].mStartNextStep.RemoveDependency(); + + // This job will finalize the simulation islands + step.mFinalizeIslands = inJobSystem->CreateJob("FinalizeIslands", cColorFinalizeIslands, [&context, &step]() + { + // Validate that all find collision jobs have stopped + JPH_ASSERT(step.mActiveFindCollisionJobs == 0); + + context.mPhysicsSystem->JobFinalizeIslands(&context); + + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + step.mBodySetIslandIndex.RemoveDependency(); + }, num_find_collisions_jobs + 2); // depends on: find collisions, build islands from constraints, finish building jobs + + // Unblock previous job + // Note: technically we could release find collisions here but we don't want to because that could make them run before 'setup velocity constraints' which means that job won't have a thread left + step.mBuildIslandsFromConstraints.RemoveDependency(); + + // This job will call the contact removed callbacks + step.mContactRemovedCallbacks = inJobSystem->CreateJob("ContactRemovedCallbacks", cColorContactRemovedCallbacks, [&context, &step]() + { + context.mPhysicsSystem->JobContactRemovedCallbacks(&step); + + if (step.mStartNextStep.IsValid()) + step.mStartNextStep.RemoveDependency(); + }, 1); // depends on the find ccd contacts + + // This job will set the island index on each body (only used for debug drawing purposes) + // It will also delete any bodies that have been destroyed in the last frame + step.mBodySetIslandIndex = inJobSystem->CreateJob("BodySetIslandIndex", cColorBodySetIslandIndex, [&context, &step]() + { + context.mPhysicsSystem->JobBodySetIslandIndex(); + + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + }, 2); // depends on: finalize islands, finish building jobs + + // Job to start the next collision step + if (!is_last_step) + { + PhysicsUpdateContext::Step *next_step = &context.mSteps[step_idx + 1]; + step.mStartNextStep = inJobSystem->CreateJob("StartNextStep", cColorStartNextStep, [this, next_step]() + { + #ifdef JPH_DEBUG + // Validate that the cached bounds are correct + mBodyManager.ValidateActiveBodyBounds(); + #endif // JPH_DEBUG + + // Store the number of active bodies at the start of the step + next_step->mNumActiveBodiesAtStepStart = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + + // Clear the large island splitter + TempAllocator *temp_allocator = next_step->mContext->mTempAllocator; + mLargeIslandSplitter.Reset(temp_allocator); + + // Clear the island builder + mIslandBuilder.ResetIslands(temp_allocator); + + // Setup island builder + mIslandBuilder.PrepareContactConstraints(mContactManager.GetMaxConstraints(), temp_allocator); + + // Restart the contact manager + mContactManager.RecycleConstraintBuffer(); + + // Kick the jobs of the next step (in the same order as the first step) + next_step->mBroadPhasePrepare.RemoveDependency(); + if (next_step->mStepListeners.empty()) + { + // Kick the gravity and active constraints jobs immediately + JobHandle::sRemoveDependencies(next_step->mApplyGravity); + JobHandle::sRemoveDependencies(next_step->mDetermineActiveConstraints); + } + else + { + // Kick the step listeners job first + JobHandle::sRemoveDependencies(next_step->mStepListeners); + } + }, 3); // depends on: update soft bodies, contact removed callbacks, finish building the previous step + } + + // This job will solve the velocity constraints + step.mSolveVelocityConstraints.resize(max_concurrency); + for (int i = 0; i < max_concurrency; ++i) + step.mSolveVelocityConstraints[i] = inJobSystem->CreateJob("SolveVelocityConstraints", cColorSolveVelocityConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSolveVelocityConstraints(&context, &step); + + step.mPreIntegrateVelocity.RemoveDependency(); + }, num_setup_velocity_constraints_jobs + 2); // depends on: finalize islands, setup velocity constraints, finish building jobs. + + // We prefer setup velocity constraints to finish first so we kick it first + JobHandle::sRemoveDependencies(step.mSetupVelocityConstraints); + JobHandle::sRemoveDependencies(step.mFindCollisions); + + // Finalize islands is a dependency on find collisions so it can go last + step.mFinalizeIslands.RemoveDependency(); + + // This job will prepare the position update of all active bodies + step.mPreIntegrateVelocity = inJobSystem->CreateJob("PreIntegrateVelocity", cColorPreIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobPreIntegrateVelocity(&context, &step); + + JobHandle::sRemoveDependencies(step.mIntegrateVelocity); + }, 2 + max_concurrency); // depends on: broadphase update finalize, solve velocity constraints, finish building jobs. + + // Unblock previous jobs + step.mUpdateBroadphaseFinalize.RemoveDependency(); + JobHandle::sRemoveDependencies(step.mSolveVelocityConstraints); + + // This job will update the positions of all active bodies + step.mIntegrateVelocity.resize(num_integrate_velocity_jobs); + for (int i = 0; i < num_integrate_velocity_jobs; ++i) + step.mIntegrateVelocity[i] = inJobSystem->CreateJob("IntegrateVelocity", cColorIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobIntegrateVelocity(&context, &step); + + step.mPostIntegrateVelocity.RemoveDependency(); + }, 2); // depends on: pre integrate velocity, finish building jobs. + + // Unblock previous job + step.mPreIntegrateVelocity.RemoveDependency(); + + // This job will finish the position update of all active bodies + step.mPostIntegrateVelocity = inJobSystem->CreateJob("PostIntegrateVelocity", cColorPostIntegrateVelocity, [&context, &step]() + { + context.mPhysicsSystem->JobPostIntegrateVelocity(&context, &step); + + step.mResolveCCDContacts.RemoveDependency(); + }, num_integrate_velocity_jobs + 1); // depends on: integrate velocity, finish building jobs + + // Unblock previous jobs + JobHandle::sRemoveDependencies(step.mIntegrateVelocity); + + // This job will update the positions and velocities for all bodies that need continuous collision detection + step.mResolveCCDContacts = inJobSystem->CreateJob("ResolveCCDContacts", cColorResolveCCDContacts, [&context, &step]() + { + context.mPhysicsSystem->JobResolveCCDContacts(&context, &step); + + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + }, 2); // depends on: integrate velocities, detect ccd contacts (added dynamically), finish building jobs. + + // Unblock previous job + step.mPostIntegrateVelocity.RemoveDependency(); + + // Fixes up drift in positions and updates the broadphase with new body positions + step.mSolvePositionConstraints.resize(max_concurrency); + for (int i = 0; i < max_concurrency; ++i) + step.mSolvePositionConstraints[i] = inJobSystem->CreateJob("SolvePositionConstraints", cColorSolvePositionConstraints, [&context, &step]() + { + context.mPhysicsSystem->JobSolvePositionConstraints(&context, &step); + + // Kick the next step + if (step.mSoftBodyPrepare.IsValid()) + step.mSoftBodyPrepare.RemoveDependency(); + }, 3); // depends on: resolve ccd contacts, body set island index, finish building jobs. + + // Unblock previous jobs. + step.mResolveCCDContacts.RemoveDependency(); + step.mBodySetIslandIndex.RemoveDependency(); + + // The soft body prepare job will create other jobs if needed + step.mSoftBodyPrepare = inJobSystem->CreateJob("SoftBodyPrepare", cColorSoftBodyPrepare, [&context, &step]() + { + context.mPhysicsSystem->JobSoftBodyPrepare(&context, &step); + }, max_concurrency); // depends on: solve position constraints. + + // Unblock previous jobs + JobHandle::sRemoveDependencies(step.mSolvePositionConstraints); + } + } + + // Build the list of jobs to wait for + JobSystem::Barrier *barrier = context.mBarrier; + { + JPH_PROFILE("Build job barrier"); + + StaticArray handles; + for (const PhysicsUpdateContext::Step &step : context.mSteps) + { + if (step.mBroadPhasePrepare.IsValid()) + handles.push_back(step.mBroadPhasePrepare); + for (const JobHandle &h : step.mStepListeners) + handles.push_back(h); + for (const JobHandle &h : step.mDetermineActiveConstraints) + handles.push_back(h); + for (const JobHandle &h : step.mApplyGravity) + handles.push_back(h); + for (const JobHandle &h : step.mFindCollisions) + handles.push_back(h); + if (step.mUpdateBroadphaseFinalize.IsValid()) + handles.push_back(step.mUpdateBroadphaseFinalize); + for (const JobHandle &h : step.mSetupVelocityConstraints) + handles.push_back(h); + handles.push_back(step.mBuildIslandsFromConstraints); + handles.push_back(step.mFinalizeIslands); + handles.push_back(step.mBodySetIslandIndex); + for (const JobHandle &h : step.mSolveVelocityConstraints) + handles.push_back(h); + handles.push_back(step.mPreIntegrateVelocity); + for (const JobHandle &h : step.mIntegrateVelocity) + handles.push_back(h); + handles.push_back(step.mPostIntegrateVelocity); + handles.push_back(step.mResolveCCDContacts); + for (const JobHandle &h : step.mSolvePositionConstraints) + handles.push_back(h); + handles.push_back(step.mContactRemovedCallbacks); + if (step.mSoftBodyPrepare.IsValid()) + handles.push_back(step.mSoftBodyPrepare); + if (step.mStartNextStep.IsValid()) + handles.push_back(step.mStartNextStep); + } + barrier->AddJobs(handles.data(), handles.size()); + } + + // Wait until all jobs finish + // Note we don't just wait for the last job. If we would and another job + // would be scheduled in between there is the possibility of a deadlock. + // The other job could try to e.g. add/remove a body which would try to + // lock a body mutex while this thread has already locked the mutex + inJobSystem->WaitForJobs(barrier); + + // We're done with the barrier for this update + inJobSystem->DestroyBarrier(barrier); + +#ifdef JPH_DEBUG + // Validate that the cached bounds are correct + mBodyManager.ValidateActiveBodyBounds(); +#endif // JPH_DEBUG + + // Clear the large island splitter + mLargeIslandSplitter.Reset(inTempAllocator); + + // Clear the island builder + mIslandBuilder.ResetIslands(inTempAllocator); + + // Clear the contact manager + mContactManager.FinishConstraintBuffer(); + + // Free active constraints + inTempAllocator->Free(context.mActiveConstraints, mConstraintManager.GetNumConstraints() * sizeof(Constraint *)); + context.mActiveConstraints = nullptr; + + // Free body pairs + inTempAllocator->Free(context.mBodyPairs, sizeof(BodyPair) * mPhysicsSettings.mMaxInFlightBodyPairs); + context.mBodyPairs = nullptr; + + // Unlock the broadphase + mBroadPhase->UnlockModifications(); + + // Unlock all constraints + mConstraintManager.UnlockAllConstraints(); + +#ifdef JPH_ENABLE_ASSERTS + // Allow write operations to the active bodies list + mBodyManager.SetActiveBodiesLocked(false); +#endif + + // Unlock all bodies + mBodyManager.UnlockAllBodies(); + + // Unlock step listeners + mStepListenersMutex.unlock(); + + // Return any errors + EPhysicsUpdateError errors = static_cast(context.mErrors.load(memory_order_acquire)); + JPH_ASSERT(errors == EPhysicsUpdateError::None, "An error occurred during the physics update, see EPhysicsUpdateError for more information"); + return errors; +} + +void PhysicsSystem::JobStepListeners(PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // Read positions (broadphase updates concurrently so we can't write), read/write velocities + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); + + // Can activate bodies only (we cache the amount of active bodies at the beginning of the step in mNumActiveBodiesAtStepStart so we cannot deactivate here) + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + float step_time = ioStep->mContext->mStepDeltaTime; + uint32 batch_size = mPhysicsSettings.mStepListenersBatchSize; + for (;;) + { + // Get the start of a new batch + uint32 batch = ioStep->mStepListenerReadIdx.fetch_add(batch_size); + if (batch >= mStepListeners.size()) + break; + + // Call the listeners + for (uint32 i = batch, i_end = min((uint32)mStepListeners.size(), batch + batch_size); i < i_end; ++i) + mStepListeners[i]->OnStep(step_time, *this); + } +} + +void PhysicsSystem::JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const +{ +#ifdef JPH_ENABLE_ASSERTS + // No body access + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + uint32 num_constraints = mConstraintManager.GetNumConstraints(); + uint32 num_active_constraints; + Constraint **active_constraints = (Constraint **)JPH_STACK_ALLOC(cDetermineActiveConstraintsBatchSize * sizeof(Constraint *)); + + for (;;) + { + // Atomically fetch a batch of constraints + uint32 constraint_idx = ioStep->mDetermineActiveConstraintReadIdx.fetch_add(cDetermineActiveConstraintsBatchSize); + if (constraint_idx >= num_constraints) + break; + + // Calculate the end of the batch + uint32 constraint_idx_end = min(num_constraints, constraint_idx + cDetermineActiveConstraintsBatchSize); + + // Store the active constraints at the start of the step (bodies get activated during the step which in turn may activate constraints leading to an inconsistent shapshot) + mConstraintManager.GetActiveConstraints(constraint_idx, constraint_idx_end, active_constraints, num_active_constraints); + + // Copy the block of active constraints to the global list of active constraints + if (num_active_constraints > 0) + { + uint32 active_constraint_idx = ioStep->mNumActiveConstraints.fetch_add(num_active_constraints); + memcpy(ioStep->mContext->mActiveConstraints + active_constraint_idx, active_constraints, num_active_constraints * sizeof(Constraint *)); + } + } +} + +void PhysicsSystem::JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update velocities and need the rotation to do so + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + // Get list of active bodies that we had at the start of the physics update. + // Any body that is activated as part of the simulation step does not receive gravity this frame. + // Note that bodies may be activated during this job but not deactivated, this means that only elements + // will be added to the array. Since the array is made to not reallocate, this is a safe operation. + const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody); + uint32 num_active_bodies_at_step_start = ioStep->mNumActiveBodiesAtStepStart; + + // Fetch delta time once outside the loop + float delta_time = ioContext->mStepDeltaTime; + + // Update velocities from forces + for (;;) + { + // Atomically fetch a batch of bodies + uint32 active_body_idx = ioStep->mApplyGravityReadIdx.fetch_add(cApplyGravityBatchSize); + if (active_body_idx >= num_active_bodies_at_step_start) + break; + + // Calculate the end of the batch + uint32 active_body_idx_end = min(num_active_bodies_at_step_start, active_body_idx + cApplyGravityBatchSize); + + // Process the batch + while (active_body_idx < active_body_idx_end) + { + Body &body = mBodyManager.GetBody(active_bodies[active_body_idx]); + if (body.IsDynamic()) + { + MotionProperties *mp = body.GetMotionProperties(); + Quat rotation = body.GetRotation(); + + if (body.GetApplyGyroscopicForce()) + mp->ApplyGyroscopicForceInternal(rotation, delta_time); + + mp->ApplyForceTorqueAndDragInternal(rotation, mGravity, delta_time); + } + active_body_idx++; + } + } +} + +void PhysicsSystem::JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); +#endif + + uint32 num_constraints = ioStep->mNumActiveConstraints; + + for (;;) + { + // Atomically fetch a batch of constraints + uint32 constraint_idx = ioStep->mSetupVelocityConstraintsReadIdx.fetch_add(cSetupVelocityConstraintsBatchSize); + if (constraint_idx >= num_constraints) + break; + + ConstraintManager::sSetupVelocityConstraints(ioStep->mContext->mActiveConstraints + constraint_idx, min(cSetupVelocityConstraintsBatchSize, num_constraints - constraint_idx), inDeltaTime); + } +} + +void PhysicsSystem::JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We read constraints and positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); + + // Can only activate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + // Prepare the island builder + mIslandBuilder.PrepareNonContactConstraints(ioStep->mNumActiveConstraints, ioContext->mTempAllocator); + + // Build the islands + ConstraintManager::sBuildIslands(ioStep->mContext->mActiveConstraints, ioStep->mNumActiveConstraints, mIslandBuilder, mBodyManager); +} + +void PhysicsSystem::TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const +{ + // Get how many jobs we can spawn and check if we can spawn more + uint max_jobs = ioStep->mBodyPairQueues.size(); + if (CountBits(ioStep->mActiveFindCollisionJobs) >= max_jobs) + return; + + // Count how many body pairs we have waiting + uint32 num_body_pairs = 0; + for (const PhysicsUpdateContext::BodyPairQueue &queue : ioStep->mBodyPairQueues) + num_body_pairs += queue.mWriteIdx - queue.mReadIdx; + + // Count how many active bodies we have waiting + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody) - ioStep->mActiveBodyReadIdx; + + // Calculate how many jobs we would like + uint desired_num_jobs = min((num_body_pairs + cNarrowPhaseBatchSize - 1) / cNarrowPhaseBatchSize + (num_active_bodies + cActiveBodiesBatchSize - 1) / cActiveBodiesBatchSize, max_jobs); + + for (;;) + { + // Get the bit mask of active jobs and see if we can spawn more + PhysicsUpdateContext::JobMask current_active_jobs = ioStep->mActiveFindCollisionJobs; + if (CountBits(current_active_jobs) >= desired_num_jobs) + break; + + // Loop through all possible job indices + for (uint job_index = 0; job_index < max_jobs; ++job_index) + { + // Test if it has been started + PhysicsUpdateContext::JobMask job_mask = PhysicsUpdateContext::JobMask(1) << job_index; + if ((current_active_jobs & job_mask) == 0) + { + // Try to claim the job index + PhysicsUpdateContext::JobMask prev_value = ioStep->mActiveFindCollisionJobs.fetch_or(job_mask); + if ((prev_value & job_mask) == 0) + { + // Add dependencies from the find collisions job to the next jobs + ioStep->mUpdateBroadphaseFinalize.AddDependency(); + ioStep->mFinalizeIslands.AddDependency(); + + // Start the job + JobHandle job = ioStep->mContext->mJobSystem->CreateJob("FindCollisions", cColorFindCollisions, [step = ioStep, job_index]() + { + step->mContext->mPhysicsSystem->JobFindCollisions(step, job_index); + }); + + // Add the job to the job barrier so the main updating thread can execute the job too + ioStep->mContext->mBarrier->AddJob(job); + + // Spawn only 1 extra job at a time + return; + } + } + } + } +} + +static void sFinalizeContactAllocator(PhysicsUpdateContext::Step &ioStep, const ContactConstraintManager::ContactAllocator &inAllocator) +{ + // Atomically accumulate the number of found manifolds and body pairs + ioStep.mNumBodyPairs.fetch_add(inAllocator.mNumBodyPairs, memory_order_relaxed); + ioStep.mNumManifolds.fetch_add(inAllocator.mNumManifolds, memory_order_relaxed); + + // Combine update errors + ioStep.mContext->mErrors.fetch_or((uint32)inAllocator.mErrors, memory_order_relaxed); +} + +void PhysicsSystem::JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex) +{ +#ifdef JPH_ENABLE_ASSERTS + // We read positions and read velocities (for elastic collisions) + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); + + // Can only activate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + // Allocation context for allocating new contact points + ContactAllocator contact_allocator(mContactManager.GetContactAllocator()); + + // Determine initial queue to read pairs from if no broadphase work can be done + // (always start looking at results from the next job) + int read_queue_idx = (inJobIndex + 1) % ioStep->mBodyPairQueues.size(); + + for (;;) + { + // Check if there are active bodies to be processed + uint32 active_bodies_read_idx = ioStep->mActiveBodyReadIdx; + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + if (active_bodies_read_idx < num_active_bodies) + { + // Take a batch of active bodies + uint32 active_bodies_read_idx_end = min(num_active_bodies, active_bodies_read_idx + cActiveBodiesBatchSize); + if (ioStep->mActiveBodyReadIdx.compare_exchange_strong(active_bodies_read_idx, active_bodies_read_idx_end)) + { + // Callback when a new body pair is found + class MyBodyPairCallback : public BodyPairCollector + { + public: + // Constructor + MyBodyPairCallback(PhysicsUpdateContext::Step *inStep, ContactAllocator &ioContactAllocator, int inJobIndex) : + mStep(inStep), + mContactAllocator(ioContactAllocator), + mJobIndex(inJobIndex) + { + } + + // Callback function when a body pair is found + virtual void AddHit(const BodyPair &inPair) override + { + // Check if we have space in our write queue + PhysicsUpdateContext::BodyPairQueue &queue = mStep->mBodyPairQueues[mJobIndex]; + uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx; + if (body_pairs_in_queue >= mStep->mMaxBodyPairsPerQueue) + { + // Buffer full, process the pair now + mStep->mContext->mPhysicsSystem->ProcessBodyPair(mContactAllocator, inPair); + } + else + { + // Store the pair in our own queue + mStep->mContext->mBodyPairs[mJobIndex * mStep->mMaxBodyPairsPerQueue + queue.mWriteIdx % mStep->mMaxBodyPairsPerQueue] = inPair; + ++queue.mWriteIdx; + } + } + + private: + PhysicsUpdateContext::Step * mStep; + ContactAllocator & mContactAllocator; + int mJobIndex; + }; + MyBodyPairCallback add_pair(ioStep, contact_allocator, inJobIndex); + + // Copy active bodies to temporary array, broadphase will reorder them + uint32 batch_size = active_bodies_read_idx_end - active_bodies_read_idx; + BodyID *active_bodies = (BodyID *)JPH_STACK_ALLOC(batch_size * sizeof(BodyID)); + memcpy(active_bodies, mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody) + active_bodies_read_idx, batch_size * sizeof(BodyID)); + + // Find pairs in the broadphase + mBroadPhase->FindCollidingPairs(active_bodies, batch_size, mPhysicsSettings.mSpeculativeContactDistance, *mObjectVsBroadPhaseLayerFilter, *mObjectLayerPairFilter, add_pair); + + // Check if we have enough pairs in the buffer to start a new job + const PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[inJobIndex]; + uint32 body_pairs_in_queue = queue.mWriteIdx - queue.mReadIdx; + if (body_pairs_in_queue >= cNarrowPhaseBatchSize) + TrySpawnJobFindCollisions(ioStep); + } + } + else + { + // Lockless loop to get the next body pair from the pairs buffer + const PhysicsUpdateContext *context = ioStep->mContext; + int first_read_queue_idx = read_queue_idx; + for (;;) + { + PhysicsUpdateContext::BodyPairQueue &queue = ioStep->mBodyPairQueues[read_queue_idx]; + + // Get the next pair to process + uint32 pair_idx = queue.mReadIdx; + + // If the pair hasn't been written yet + if (pair_idx >= queue.mWriteIdx) + { + // Go to the next queue + read_queue_idx = (read_queue_idx + 1) % ioStep->mBodyPairQueues.size(); + + // If we're back at the first queue, we've looked at all of them and found nothing + if (read_queue_idx == first_read_queue_idx) + { + // Collect information from the contact allocator and accumulate it in the step. + sFinalizeContactAllocator(*ioStep, contact_allocator); + + // Mark this job as inactive + ioStep->mActiveFindCollisionJobs.fetch_and(~PhysicsUpdateContext::JobMask(1 << inJobIndex)); + + // Trigger the next jobs + ioStep->mUpdateBroadphaseFinalize.RemoveDependency(); + ioStep->mFinalizeIslands.RemoveDependency(); + return; + } + + // Try again reading from the next queue + continue; + } + + // Copy the body pair out of the buffer + const BodyPair bp = context->mBodyPairs[read_queue_idx * ioStep->mMaxBodyPairsPerQueue + pair_idx % ioStep->mMaxBodyPairsPerQueue]; + + // Mark this pair as taken + if (queue.mReadIdx.compare_exchange_strong(pair_idx, pair_idx + 1)) + { + // Process the actual body pair + ProcessBodyPair(contact_allocator, bp); + break; + } + } + } + } +} + +void PhysicsSystem::ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair) +{ + JPH_PROFILE_FUNCTION(); + + // Fetch body pair + Body *body1 = &mBodyManager.GetBody(inBodyPair.mBodyA); + Body *body2 = &mBodyManager.GetBody(inBodyPair.mBodyB); + JPH_ASSERT(body1->IsActive()); + + JPH_DET_LOG("ProcessBodyPair: id1: " << inBodyPair.mBodyA << " id2: " << inBodyPair.mBodyB << " p1: " << body1->GetCenterOfMassPosition() << " p2: " << body2->GetCenterOfMassPosition() << " r1: " << body1->GetRotation() << " r2: " << body2->GetRotation()); + + // Check for soft bodies + if (body2->IsSoftBody()) + { + // If the 2nd body is a soft body and not active, we activate it now + if (!body2->IsActive()) + mBodyManager.ActivateBodies(&inBodyPair.mBodyB, 1); + + // Soft body processing is done later in the pipeline + return; + } + + // Ensure that body1 has the higher motion type (i.e. dynamic trumps kinematic), this ensures that we do the collision detection in the space of a moving body, + // which avoids accuracy problems when testing a very large static object against a small dynamic object + // Ensure that body1 id < body2 id when motion types are the same. + if (body1->GetMotionType() < body2->GetMotionType() + || (body1->GetMotionType() == body2->GetMotionType() && inBodyPair.mBodyB < inBodyPair.mBodyA)) + swap(body1, body2); + + // Check if the contact points from the previous frame are reusable and if so copy them + bool pair_handled = false, constraint_created = false; + if (mPhysicsSettings.mUseBodyPairContactCache && !(body1->IsCollisionCacheInvalid() || body2->IsCollisionCacheInvalid())) + mContactManager.GetContactsFromCache(ioContactAllocator, *body1, *body2, pair_handled, constraint_created); + + // If the cache hasn't handled this body pair do actual collision detection + if (!pair_handled) + { + // Create entry in the cache for this body pair + // Needs to happen irrespective if we found a collision or not (we want to remember that no collision was found too) + ContactConstraintManager::BodyPairHandle body_pair_handle = mContactManager.AddBodyPair(ioContactAllocator, *body1, *body2); + if (body_pair_handle == nullptr) + return; // Out of cache space + + // Create the query settings + CollideShapeSettings settings; + settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll; + settings.mMaxSeparationDistance = body1->IsSensor() || body2->IsSensor()? 0.0f : mPhysicsSettings.mSpeculativeContactDistance; + settings.mActiveEdgeMovementDirection = body1->GetLinearVelocity() - body2->GetLinearVelocity(); + + // Get transforms relative to body1 + RVec3 offset = body1->GetCenterOfMassPosition(); + Mat44 transform1 = Mat44::sRotation(body1->GetRotation()); + Mat44 transform2 = body2->GetCenterOfMassTransform().PostTranslated(-offset).ToMat44(); + + if (mPhysicsSettings.mUseManifoldReduction // Check global flag + && body1->GetUseManifoldReductionWithBody(*body2)) // Check body flag + { + // Version WITH contact manifold reduction + + class MyManifold : public ContactManifold + { + public: + Vec3 mFirstWorldSpaceNormal; + }; + + // A temporary structure that allows us to keep track of the all manifolds between this body pair + using Manifolds = StaticArray; + + // Create collector + class ReductionCollideShapeCollector : public CollideShapeCollector + { + public: + ReductionCollideShapeCollector(PhysicsSystem *inSystem, const Body *inBody1, const Body *inBody2) : + mSystem(inSystem), + mBody1(inBody1), + mBody2(inBody2) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // The first body should be the one with the highest motion type + JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType()); + JPH_ASSERT(!ShouldEarlyOut()); + + // Test if we want to accept this hit + if (mValidateBodyPair) + { + switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult)) + { + case ValidateResult::AcceptContact: + // We're just accepting this one, nothing to do + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept and stop calling the validate callback + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + // Skip this contact + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Skip this and early out + ForceEarlyOut(); + return; + } + } + + // Calculate normal + Vec3 world_space_normal = inResult.mPenetrationAxis.Normalized(); + + // Check if we can add it to an existing manifold + Manifolds::iterator manifold; + float contact_normal_cos_max_delta_rot = mSystem->mPhysicsSettings.mContactNormalCosMaxDeltaRotation; + for (manifold = mManifolds.begin(); manifold != mManifolds.end(); ++manifold) + if (world_space_normal.Dot(manifold->mFirstWorldSpaceNormal) >= contact_normal_cos_max_delta_rot) + { + // Update average normal + manifold->mWorldSpaceNormal += world_space_normal; + manifold->mPenetrationDepth = max(manifold->mPenetrationDepth, inResult.mPenetrationDepth); + break; + } + if (manifold == mManifolds.end()) + { + // Check if array is full + if (mManifolds.size() == mManifolds.capacity()) + { + // Full, find manifold with least amount of penetration + manifold = mManifolds.begin(); + for (Manifolds::iterator m = mManifolds.begin() + 1; m < mManifolds.end(); ++m) + if (m->mPenetrationDepth < manifold->mPenetrationDepth) + manifold = m; + + // If this contacts penetration is smaller than the smallest manifold, we skip this contact + if (inResult.mPenetrationDepth < manifold->mPenetrationDepth) + return; + + // Replace the manifold + *manifold = { { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal }; + } + else + { + // Not full, create new manifold + mManifolds.push_back({ { mBody1->GetCenterOfMassPosition(), world_space_normal, inResult.mPenetrationDepth, inResult.mSubShapeID1, inResult.mSubShapeID2, { }, { } }, world_space_normal }); + manifold = mManifolds.end() - 1; + } + } + + // Determine contact points + const PhysicsSettings &settings = mSystem->mPhysicsSettings; + ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, mBody1->GetCenterOfMassPosition())); + + // Prune if we have more than 32 points (this means we could run out of space in the next iteration) + if (manifold->mRelativeContactPointsOn1.size() > 32) + PruneContactPoints(manifold->mFirstWorldSpaceNormal, manifold->mRelativeContactPointsOn1, manifold->mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold->mBaseOffset)); + } + + PhysicsSystem * mSystem; + const Body * mBody1; + const Body * mBody2; + bool mValidateBodyPair = true; + Manifolds mManifolds; + }; + ReductionCollideShapeCollector collector(this, body1, body2); + + // Perform collision detection between the two shapes + SubShapeIDCreator part1, part2; + auto f = body1->GetEnhancedInternalEdgeRemovalWithBody(*body2)? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; + f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, { }); + + // Add the contacts + for (ContactManifold &manifold : collector.mManifolds) + { + // Normalize the normal (is a sum of all normals from merged manifolds) + manifold.mWorldSpaceNormal = manifold.mWorldSpaceNormal.Normalized(); + + // If we still have too many points, prune them now + if (manifold.mRelativeContactPointsOn1.size() > 4) + PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Actually add the contact points to the manager + constraint_created |= mContactManager.AddContactConstraint(ioContactAllocator, body_pair_handle, *body1, *body2, manifold); + } + } + else + { + // Version WITHOUT contact manifold reduction + + // Create collector + class NonReductionCollideShapeCollector : public CollideShapeCollector + { + public: + NonReductionCollideShapeCollector(PhysicsSystem *inSystem, ContactAllocator &ioContactAllocator, Body *inBody1, Body *inBody2, const ContactConstraintManager::BodyPairHandle &inPairHandle) : + mSystem(inSystem), + mContactAllocator(ioContactAllocator), + mBody1(inBody1), + mBody2(inBody2), + mBodyPairHandle(inPairHandle) + { + } + + virtual void AddHit(const CollideShapeResult &inResult) override + { + // The first body should be the one with the highest motion type + JPH_ASSERT(mBody1->GetMotionType() >= mBody2->GetMotionType()); + JPH_ASSERT(!ShouldEarlyOut()); + + // Test if we want to accept this hit + if (mValidateBodyPair) + { + switch (mSystem->mContactManager.ValidateContactPoint(*mBody1, *mBody2, mBody1->GetCenterOfMassPosition(), inResult)) + { + case ValidateResult::AcceptContact: + // We're just accepting this one, nothing to do + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept and stop calling the validate callback + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + // Skip this contact + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Skip this and early out + ForceEarlyOut(); + return; + } + } + + // Determine contact points + ContactManifold manifold; + manifold.mBaseOffset = mBody1->GetCenterOfMassPosition(); + const PhysicsSettings &settings = mSystem->mPhysicsSettings; + ManifoldBetweenTwoFaces(inResult.mContactPointOn1, inResult.mContactPointOn2, inResult.mPenetrationAxis, Square(settings.mSpeculativeContactDistance) + settings.mManifoldToleranceSq, inResult.mShape1Face, inResult.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Calculate normal + manifold.mWorldSpaceNormal = inResult.mPenetrationAxis.Normalized(); + + // Store penetration depth + manifold.mPenetrationDepth = inResult.mPenetrationDepth; + + // Prune if we have more than 4 points + if (manifold.mRelativeContactPointsOn1.size() > 4) + PruneContactPoints(manifold.mWorldSpaceNormal, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + + // Set other properties + manifold.mSubShapeID1 = inResult.mSubShapeID1; + manifold.mSubShapeID2 = inResult.mSubShapeID2; + + // Actually add the contact points to the manager + mConstraintCreated |= mSystem->mContactManager.AddContactConstraint(mContactAllocator, mBodyPairHandle, *mBody1, *mBody2, manifold); + } + + PhysicsSystem * mSystem; + ContactAllocator & mContactAllocator; + Body * mBody1; + Body * mBody2; + ContactConstraintManager::BodyPairHandle mBodyPairHandle; + bool mValidateBodyPair = true; + bool mConstraintCreated = false; + }; + NonReductionCollideShapeCollector collector(this, ioContactAllocator, body1, body2, body_pair_handle); + + // Perform collision detection between the two shapes + SubShapeIDCreator part1, part2; + auto f = body1->GetEnhancedInternalEdgeRemovalWithBody(*body2)? InternalEdgeRemovingCollector::sCollideShapeVsShape : CollisionDispatch::sCollideShapeVsShape; + f(body1->GetShape(), body2->GetShape(), Vec3::sReplicate(1.0f), Vec3::sReplicate(1.0f), transform1, transform2, part1, part2, settings, collector, { }); + + constraint_created = collector.mConstraintCreated; + } + } + + // If a contact constraint was created, we need to do some extra work + if (constraint_created) + { + // Wake up sleeping bodies + BodyID body_ids[2]; + int num_bodies = 0; + if (body1->IsDynamic() && !body1->IsActive()) + body_ids[num_bodies++] = body1->GetID(); + if (body2->IsDynamic() && !body2->IsActive()) + body_ids[num_bodies++] = body2->GetID(); + if (num_bodies > 0) + mBodyManager.ActivateBodies(body_ids, num_bodies); + + // Link the two bodies + mIslandBuilder.LinkBodies(body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal()); + } +} + +void PhysicsSystem::JobFinalizeIslands(PhysicsUpdateContext *ioContext) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only touch island data + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Finish collecting the islands, at this point the active body list doesn't change so it's safe to access + mIslandBuilder.Finalize(mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody), mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), mContactManager.GetNumConstraints(), ioContext->mTempAllocator); + + // Prepare the large island splitter + if (mPhysicsSettings.mUseLargeIslandSplitter) + mLargeIslandSplitter.Prepare(mIslandBuilder, mBodyManager.GetNumActiveBodies(EBodyType::RigidBody), ioContext->mTempAllocator); +} + +void PhysicsSystem::JobBodySetIslandIndex() +{ +#ifdef JPH_ENABLE_ASSERTS + // We only touch island data + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Loop through the result and tag all bodies with an island index + for (uint32 island_idx = 0, n = mIslandBuilder.GetNumIslands(); island_idx < n; ++island_idx) + { + BodyID *body_start, *body_end; + mIslandBuilder.GetBodiesInIsland(island_idx, body_start, body_end); + for (const BodyID *body = body_start; body < body_end; ++body) + mBodyManager.GetBody(*body).GetMotionProperties()->SetIslandIndexInternal(island_idx); + } +} + +JPH_SUPPRESS_WARNING_PUSH +JPH_CLANG_SUPPRESS_WARNING("-Wundefined-func-template") // ConstraintManager::sWarmStartVelocityConstraints / ContactConstraintManager::WarmStartVelocityConstraints is instantiated in the cpp file + +void PhysicsSystem::JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update velocities and need to read positions to do so + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + float delta_time = ioContext->mStepDeltaTime; + Constraint **active_constraints = ioContext->mActiveConstraints; + + // Only the first step to correct for the delta time difference in the previous update + float warm_start_impulse_ratio = ioStep->mIsFirst? ioContext->mWarmStartImpulseRatio : 1.0f; + + bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter; + do + { + // First try to get work from large islands + if (check_split_islands) + { + bool first_iteration; + uint split_island_index; + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration)) + { + case LargeIslandSplitter::EStatus::BatchRetrieved: + { + if (first_iteration) + { + // Iteration 0 is used to warm start the batch (we added 1 to the number of iterations in LargeIslandSplitter::SplitIsland) + DummyCalculateSolverSteps dummy; + ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, dummy); + mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, dummy); + } + else + { + // Solve velocity constraints + ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time); + mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end); + } + + // Mark the batch as processed + bool last_iteration, final_batch; + mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch); + + // Save back the lambdas in the contact cache for the warm start of the next physics update + if (last_iteration) + mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + + // We processed work, loop again + continue; + } + case LargeIslandSplitter::EStatus::WaitingForBatch: + break; + case LargeIslandSplitter::EStatus::AllBatchesDone: + check_split_islands = false; + break; + } + } + + // If that didn't succeed try to process an island + if (check_islands) + { + // Next island + uint32 island_idx = ioStep->mSolveVelocityConstraintsNextIsland++; + if (island_idx >= mIslandBuilder.GetNumIslands()) + { + // We processed all islands, stop checking islands + check_islands = false; + continue; + } + + JPH_PROFILE("Island"); + + // Get iterators for this island + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + bool has_constraints = mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end); + bool has_contacts = mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end); + + // If we don't have any contacts or constraints, we know that none of the following islands have any contacts or constraints + // (because they're sorted by most constraints first). This means we're done. + if (!has_contacts && !has_constraints) + { + #ifdef JPH_ENABLE_ASSERTS + // Validate our assumption that the next islands don't have any constraints or contacts + for (; island_idx < mIslandBuilder.GetNumIslands(); ++island_idx) + { + JPH_ASSERT(!mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end)); + JPH_ASSERT(!mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end)); + } + #endif // JPH_ENABLE_ASSERTS + + check_islands = false; + continue; + } + + // Sorting is costly but needed for a deterministic simulation, allow the user to turn this off + if (mPhysicsSettings.mDeterministicSimulation) + { + // Sort constraints to give a deterministic simulation + ConstraintManager::sSortConstraints(active_constraints, constraints_begin, constraints_end); + + // Sort contacts to give a deterministic simulation + mContactManager.SortContacts(contacts_begin, contacts_end); + } + + // Split up large islands + CalculateSolverSteps steps_calculator(mPhysicsSettings); + if (mPhysicsSettings.mUseLargeIslandSplitter + && mLargeIslandSplitter.SplitIsland(island_idx, mIslandBuilder, mBodyManager, mContactManager, active_constraints, steps_calculator)) + continue; // Loop again to try to fetch the newly split island + + // We didn't create a split, just run the solver now for this entire island. Begin by warm starting. + ConstraintManager::sWarmStartVelocityConstraints(active_constraints, constraints_begin, constraints_end, warm_start_impulse_ratio, steps_calculator); + mContactManager.WarmStartVelocityConstraints(contacts_begin, contacts_end, warm_start_impulse_ratio, steps_calculator); + steps_calculator.Finalize(); + + // Store the number of position steps for later + mIslandBuilder.SetNumPositionSteps(island_idx, steps_calculator.GetNumPositionSteps()); + + // Solve velocity constraints + for (uint velocity_step = 0; velocity_step < steps_calculator.GetNumVelocitySteps(); ++velocity_step) + { + bool applied_impulse = ConstraintManager::sSolveVelocityConstraints(active_constraints, constraints_begin, constraints_end, delta_time); + applied_impulse |= mContactManager.SolveVelocityConstraints(contacts_begin, contacts_end); + if (!applied_impulse) + break; + } + + // Save back the lambdas in the contact cache for the warm start of the next physics update + mContactManager.StoreAppliedImpulses(contacts_begin, contacts_end); + + // We processed work, loop again + continue; + } + + // If we didn't find any work, give up a time slice + std::this_thread::yield(); + } + while (check_islands || check_split_islands); +} + +JPH_SUPPRESS_WARNING_POP + +void PhysicsSystem::JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ + // Reserve enough space for all bodies that may need a cast + TempAllocator *temp_allocator = ioContext->mTempAllocator; + JPH_ASSERT(ioStep->mCCDBodies == nullptr); + ioStep->mCCDBodiesCapacity = mBodyManager.GetNumActiveCCDBodies(); + ioStep->mCCDBodies = (CCDBody *)temp_allocator->Allocate(ioStep->mCCDBodiesCapacity * sizeof(CCDBody)); + + // Initialize the mapping table between active body and CCD body + JPH_ASSERT(ioStep->mActiveBodyToCCDBody == nullptr); + ioStep->mNumActiveBodyToCCDBody = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + ioStep->mActiveBodyToCCDBody = (int *)temp_allocator->Allocate(ioStep->mNumActiveBodyToCCDBody * sizeof(int)); + + // Prepare the split island builder for solving the position constraints + mLargeIslandSplitter.PrepareForSolvePositions(); +} + +void PhysicsSystem::JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We update positions and need velocity to do so, we also clamp velocities so need to write to them + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); +#endif + + float delta_time = ioContext->mStepDeltaTime; + const BodyID *active_bodies = mBodyManager.GetActiveBodiesUnsafe(EBodyType::RigidBody); + uint32 num_active_bodies = mBodyManager.GetNumActiveBodies(EBodyType::RigidBody); + uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx; + + // We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement. + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + + for (;;) + { + // Atomically fetch a batch of bodies + uint32 active_body_idx = ioStep->mIntegrateVelocityReadIdx.fetch_add(cIntegrateVelocityBatchSize); + if (active_body_idx >= num_active_bodies) + break; + + // Calculate the end of the batch + uint32 active_body_idx_end = min(num_active_bodies, active_body_idx + cIntegrateVelocityBatchSize); + + // Process the batch + while (active_body_idx < active_body_idx_end) + { + // Update the positions using an Symplectic Euler step (which integrates using the updated velocity v1' rather + // than the original velocity v1): + // x1' = x1 + h * v1' + // At this point the active bodies list does not change, so it is safe to access the array. + BodyID body_id = active_bodies[active_body_idx]; + Body &body = mBodyManager.GetBody(body_id); + MotionProperties *mp = body.GetMotionProperties(); + + JPH_DET_LOG("JobIntegrateVelocity: id: " << body_id << " v: " << body.GetLinearVelocity() << " w: " << body.GetAngularVelocity()); + + // Clamp velocities (not for kinematic bodies) + if (body.IsDynamic()) + { + mp->ClampLinearVelocity(); + mp->ClampAngularVelocity(); + } + + // Update the rotation of the body according to the angular velocity + // For motion type discrete we need to do this anyway, for motion type linear cast we have multiple choices + // 1. Rotate the body first and then sweep + // 2. First sweep and then rotate the body at the end + // 3. Pick some in between rotation (e.g. half way), then sweep and finally rotate the remainder + // (1) has some clear advantages as when a long thin body hits a surface away from the center of mass, this will result in a large angular velocity and a limited reduction in linear velocity. + // When simulation the rotation first before doing the translation, the body will be able to rotate away from the contact point allowing the center of mass to approach the surface. When using + // approach (2) in this case what will happen is that we will immediately detect the same collision again (the body has not rotated and the body was already colliding at the end of the previous + // time step) resulting in a lot of stolen time and the body appearing to be frozen in an unnatural pose (like it is glued at an angle to the surface). (2) obviously has some negative side effects + // too as simulating the rotation first may cause it to tunnel through a small object that the linear cast might have otherwise detected. In any case a linear cast is not good for detecting + // tunneling due to angular rotation, so we don't care about that too much (you'd need a full cast to take angular effects into account). + body.AddRotationStep(body.GetAngularVelocity() * delta_time); + + // Get delta position + Vec3 delta_pos = body.GetLinearVelocity() * delta_time; + + // If the position should be updated (or if it is delayed because of CCD) + bool update_position = true; + + switch (mp->GetMotionQuality()) + { + case EMotionQuality::Discrete: + // No additional collision checking to be done + break; + + case EMotionQuality::LinearCast: + if (body.IsDynamic() // Kinematic bodies cannot be stopped + && !body.IsSensor()) // We don't support CCD sensors + { + // Determine inner radius (the smallest sphere that fits into the shape) + float inner_radius = body.GetShape()->GetInnerRadius(); + JPH_ASSERT(inner_radius > 0.0f, "The shape has no inner radius, this makes the shape unsuitable for the linear cast motion quality as we cannot move it without risking tunneling."); + + // Measure translation in this step and check if it above the threshold to perform a linear cast + float linear_cast_threshold_sq = Square(mPhysicsSettings.mLinearCastThreshold * inner_radius); + if (delta_pos.LengthSq() > linear_cast_threshold_sq) + { + // This body needs a cast + uint32 ccd_body_idx = ioStep->mNumCCDBodies++; + JPH_ASSERT(active_body_idx < ioStep->mNumActiveBodyToCCDBody); + ioStep->mActiveBodyToCCDBody[active_body_idx] = ccd_body_idx; + new (&ioStep->mCCDBodies[ccd_body_idx]) CCDBody(body_id, delta_pos, linear_cast_threshold_sq, min(mPhysicsSettings.mPenetrationSlop, mPhysicsSettings.mLinearCastMaxPenetration * inner_radius)); + + update_position = false; + } + } + break; + } + + if (update_position) + { + // Move the body now + body.AddPositionStep(delta_pos); + + // If the body was activated due to an earlier CCD step it will have an index in the active + // body list that it higher than the highest one we processed during FindCollisions + // which means it hasn't been assigned an island and will not be updated by an island + // this means that we need to update its bounds manually + if (mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions) + { + body.CalculateWorldSpaceBoundsInternal(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = body.GetID(); + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + } + + // We did not create a CCD body + ioStep->mActiveBodyToCCDBody[active_body_idx] = -1; + } + + active_body_idx++; + } + } + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); +} + +void PhysicsSystem::JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const +{ + // Validate that our reservations were correct + JPH_ASSERT(ioStep->mNumCCDBodies <= mBodyManager.GetNumActiveCCDBodies()); + + if (ioStep->mNumCCDBodies == 0) + { + // No continuous collision detection jobs -> kick the next job ourselves + ioStep->mContactRemovedCallbacks.RemoveDependency(); + } + else + { + // Run the continuous collision detection jobs + int num_continuous_collision_jobs = min(int(ioStep->mNumCCDBodies + cNumCCDBodiesPerJob - 1) / cNumCCDBodiesPerJob, ioContext->GetMaxConcurrency()); + ioStep->mResolveCCDContacts.AddDependency(num_continuous_collision_jobs); + ioStep->mContactRemovedCallbacks.AddDependency(num_continuous_collision_jobs - 1); // Already had 1 dependency + for (int i = 0; i < num_continuous_collision_jobs; ++i) + { + JobHandle job = ioContext->mJobSystem->CreateJob("FindCCDContacts", cColorFindCCDContacts, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobFindCCDContacts(ioContext, ioStep); + + ioStep->mResolveCCDContacts.RemoveDependency(); + ioStep->mContactRemovedCallbacks.RemoveDependency(); + }); + ioContext->mBarrier->AddJob(job); + } + } +} + +// Helper function to calculate the motion of a body during this CCD step +inline static Vec3 sCalculateBodyMotion(const Body &inBody, float inDeltaTime) +{ + // If the body is linear casting, the body has not yet moved so we need to calculate its motion + if (inBody.IsDynamic() && inBody.GetMotionProperties()->GetMotionQuality() == EMotionQuality::LinearCast) + return inDeltaTime * inBody.GetLinearVelocity(); + + // Body has already moved, so we don't need to correct for anything + return Vec3::sZero(); +} + +// Helper function that finds the CCD body corresponding to a body (if it exists) +inline static PhysicsUpdateContext::Step::CCDBody *sGetCCDBody(const Body &inBody, PhysicsUpdateContext::Step *inStep) +{ + // Only rigid bodies can have a CCD body + if (!inBody.IsRigidBody()) + return nullptr; + + // If the body has no motion properties it cannot have a CCD body + const MotionProperties *motion_properties = inBody.GetMotionPropertiesUnchecked(); + if (motion_properties == nullptr) + return nullptr; + + // If it is not active it cannot have a CCD body + uint32 active_index = motion_properties->GetIndexInActiveBodiesInternal(); + if (active_index == Body::cInactiveIndex) + return nullptr; + + // Check if the active body has a corresponding CCD body + JPH_ASSERT(active_index < inStep->mNumActiveBodyToCCDBody); // Ensure that the body has a mapping to CCD body + int ccd_index = inStep->mActiveBodyToCCDBody[active_index]; + if (ccd_index < 0) + return nullptr; + + PhysicsUpdateContext::Step::CCDBody *ccd_body = &inStep->mCCDBodies[ccd_index]; + JPH_ASSERT(ccd_body->mBodyID1 == inBody.GetID(), "We found the wrong CCD body!"); + return ccd_body; +} + +void PhysicsSystem::JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We only read positions, but the validate callback may read body positions and velocities + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); +#endif + + // Allocation context for allocating new contact points + ContactAllocator contact_allocator(mContactManager.GetContactAllocator()); + + // Settings + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mBackFaceModeTriangles = EBackFaceMode::IgnoreBackFaces; + settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces; + settings.mReturnDeepestPoint = true; + settings.mCollectFacesMode = ECollectFacesMode::CollectFaces; + settings.mActiveEdgeMode = mPhysicsSettings.mCheckActiveEdges? EActiveEdgeMode::CollideOnlyWithActive : EActiveEdgeMode::CollideWithAll; + + for (;;) + { + // Fetch the next body to cast + uint32 idx = ioStep->mNextCCDBody++; + if (idx >= ioStep->mNumCCDBodies) + break; + CCDBody &ccd_body = ioStep->mCCDBodies[idx]; + const Body &body = mBodyManager.GetBody(ccd_body.mBodyID1); + + // Filter out layers + DefaultBroadPhaseLayerFilter broadphase_layer_filter = GetDefaultBroadPhaseLayerFilter(body.GetObjectLayer()); + DefaultObjectLayerFilter object_layer_filter = GetDefaultLayerFilter(body.GetObjectLayer()); + + #ifdef JPH_DEBUG_RENDERER + // Draw start and end shape of cast + if (sDrawMotionQualityLinearCast) + { + RMat44 com = body.GetCenterOfMassTransform(); + body.GetShape()->Draw(DebugRenderer::sInstance, com, Vec3::sReplicate(1.0f), Color::sGreen, false, true); + DebugRenderer::sInstance->DrawArrow(com.GetTranslation(), com.GetTranslation() + ccd_body.mDeltaPosition, Color::sGreen, 0.1f); + body.GetShape()->Draw(DebugRenderer::sInstance, com.PostTranslated(ccd_body.mDeltaPosition), Vec3::sReplicate(1.0f), Color::sRed, false, true); + } + #endif // JPH_DEBUG_RENDERER + + // Create a collector that will find the maximum distance allowed to travel while not penetrating more than 'max penetration' + class CCDNarrowPhaseCollector : public CastShapeCollector + { + public: + CCDNarrowPhaseCollector(const BodyManager &inBodyManager, ContactConstraintManager &inContactConstraintManager, CCDBody &inCCDBody, ShapeCastResult &inResult, float inDeltaTime) : + mBodyManager(inBodyManager), + mContactConstraintManager(inContactConstraintManager), + mCCDBody(inCCDBody), + mResult(inResult), + mDeltaTime(inDeltaTime) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + JPH_PROFILE_FUNCTION(); + + // Check if this is a possible earlier hit than the one before + float fraction = inResult.mFraction; + if (fraction < mCCDBody.mFractionPlusSlop) + { + // Normalize normal + Vec3 normal = inResult.mPenetrationAxis.Normalized(); + + // Calculate how much we can add to the fraction to penetrate the collision point by mMaxPenetration. + // Note that the normal is pointing towards body 2! + // Let the extra distance that we can travel along delta_pos be 'dist': mMaxPenetration / dist = cos(angle between normal and delta_pos) = normal . delta_pos / |delta_pos| + // <=> dist = mMaxPenetration * |delta_pos| / normal . delta_pos + // Converting to a faction: delta_fraction = dist / |delta_pos| = mLinearCastTreshold / normal . delta_pos + float denominator = normal.Dot(mCCDBody.mDeltaPosition); + if (denominator > mCCDBody.mMaxPenetration) // Avoid dividing by zero, if extra hit fraction > 1 there's also no point in continuing + { + float fraction_plus_slop = fraction + mCCDBody.mMaxPenetration / denominator; + if (fraction_plus_slop < mCCDBody.mFractionPlusSlop) + { + const Body &body2 = mBodyManager.GetBody(inResult.mBodyID2); + + // Check if we've already accepted all hits from this body + if (mValidateBodyPair) + { + // Validate the contact result + const Body &body1 = mBodyManager.GetBody(mCCDBody.mBodyID1); + ValidateResult validate_result = mContactConstraintManager.ValidateContactPoint(body1, body2, body1.GetCenterOfMassPosition(), inResult); // Note that the center of mass of body 1 is the start of the sweep and is used as base offset below + switch (validate_result) + { + case ValidateResult::AcceptContact: + // Just continue + break; + + case ValidateResult::AcceptAllContactsForThisBodyPair: + // Accept this and all following contacts from this body + mValidateBodyPair = false; + break; + + case ValidateResult::RejectContact: + return; + + case ValidateResult::RejectAllContactsForThisBodyPair: + // Reject this and all following contacts from this body + mRejectAll = true; + ForceEarlyOut(); + return; + } + } + + // This is the earliest hit so far, store it + mCCDBody.mContactNormal = normal; + mCCDBody.mBodyID2 = inResult.mBodyID2; + mCCDBody.mSubShapeID2 = inResult.mSubShapeID2; + mCCDBody.mFraction = fraction; + mCCDBody.mFractionPlusSlop = fraction_plus_slop; + mResult = inResult; + + // Result was assuming body 2 is not moving, but it is, so we need to correct for it + Vec3 movement2 = fraction * sCalculateBodyMotion(body2, mDeltaTime); + if (!movement2.IsNearZero()) + { + mResult.mContactPointOn1 += movement2; + mResult.mContactPointOn2 += movement2; + for (Vec3 &v : mResult.mShape1Face) + v += movement2; + for (Vec3 &v : mResult.mShape2Face) + v += movement2; + } + + // Update early out fraction + UpdateEarlyOutFraction(fraction_plus_slop); + } + } + } + } + + bool mValidateBodyPair; ///< If we still have to call the ValidateContactPoint for this body pair + bool mRejectAll; ///< Reject all further contacts between this body pair + + private: + const BodyManager & mBodyManager; + ContactConstraintManager & mContactConstraintManager; + CCDBody & mCCDBody; + ShapeCastResult & mResult; + float mDeltaTime; + BodyID mAcceptedBodyID; + }; + + // Narrowphase collector + ShapeCastResult cast_shape_result; + CCDNarrowPhaseCollector np_collector(mBodyManager, mContactManager, ccd_body, cast_shape_result, ioContext->mStepDeltaTime); + + // This collector wraps the narrowphase collector and collects the closest hit + class CCDBroadPhaseCollector : public CastShapeBodyCollector + { + public: + CCDBroadPhaseCollector(const CCDBody &inCCDBody, const Body &inBody1, const RShapeCast &inShapeCast, ShapeCastSettings &inShapeCastSettings, CCDNarrowPhaseCollector &ioCollector, const BodyManager &inBodyManager, PhysicsUpdateContext::Step *inStep, float inDeltaTime) : + mCCDBody(inCCDBody), + mBody1(inBody1), + mBody1Extent(inShapeCast.mShapeWorldBounds.GetExtent()), + mShapeCast(inShapeCast), + mShapeCastSettings(inShapeCastSettings), + mCollector(ioCollector), + mBodyManager(inBodyManager), + mStep(inStep), + mDeltaTime(inDeltaTime) + { + } + + virtual void AddHit(const BroadPhaseCastResult &inResult) override + { + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inResult.mFraction <= GetEarlyOutFraction(), "This hit should not have been passed on to the collector"); + + // Test if we're colliding with ourselves + if (mBody1.GetID() == inResult.mBodyID) + return; + + // Avoid treating duplicates, if both bodies are doing CCD then only consider collision if body ID < other body ID + const Body &body2 = mBodyManager.GetBody(inResult.mBodyID); + const CCDBody *ccd_body2 = sGetCCDBody(body2, mStep); + if (ccd_body2 != nullptr && mCCDBody.mBodyID1 > ccd_body2->mBodyID1) + return; + + // Test group filter + if (!mBody1.GetCollisionGroup().CanCollide(body2.GetCollisionGroup())) + return; + + // TODO: For now we ignore sensors + if (body2.IsSensor()) + return; + + // Get relative movement of these two bodies + Vec3 direction = mShapeCast.mDirection - sCalculateBodyMotion(body2, mDeltaTime); + + // Test if the remaining movement is less than our movement threshold + if (direction.LengthSq() < mCCDBody.mLinearCastThresholdSq) + return; + + // Get the bounds of 2, widen it by the extent of 1 and test a ray to see if it hits earlier than the current early out fraction + AABox bounds = body2.GetWorldSpaceBounds(); + bounds.mMin -= mBody1Extent; + bounds.mMax += mBody1Extent; + float hit_fraction = RayAABox(Vec3(mShapeCast.mCenterOfMassStart.GetTranslation()), RayInvDirection(direction), bounds.mMin, bounds.mMax); + if (hit_fraction > GetPositiveEarlyOutFraction()) // If early out fraction <= 0, we have the possibility of finding a deeper hit so we need to clamp the early out fraction + return; + + // Reset collector (this is a new body pair) + mCollector.ResetEarlyOutFraction(GetEarlyOutFraction()); + mCollector.mValidateBodyPair = true; + mCollector.mRejectAll = false; + + // Provide direction as hint for the active edges algorithm + mShapeCastSettings.mActiveEdgeMovementDirection = direction; + + // Do narrow phase collision check + RShapeCast relative_cast(mShapeCast.mShape, mShapeCast.mScale, mShapeCast.mCenterOfMassStart, direction, mShapeCast.mShapeWorldBounds); + body2.GetTransformedShape().CastShape(relative_cast, mShapeCastSettings, mShapeCast.mCenterOfMassStart.GetTranslation(), mCollector); + + // Update early out fraction based on narrow phase collector + if (!mCollector.mRejectAll) + UpdateEarlyOutFraction(mCollector.GetEarlyOutFraction()); + } + + const CCDBody & mCCDBody; + const Body & mBody1; + Vec3 mBody1Extent; + RShapeCast mShapeCast; + ShapeCastSettings & mShapeCastSettings; + CCDNarrowPhaseCollector & mCollector; + const BodyManager & mBodyManager; + PhysicsUpdateContext::Step *mStep; + float mDeltaTime; + }; + + // Check if we collide with any other body. Note that we use the non-locking interface as we know the broadphase cannot be modified at this point. + RShapeCast shape_cast(body.GetShape(), Vec3::sReplicate(1.0f), body.GetCenterOfMassTransform(), ccd_body.mDeltaPosition); + CCDBroadPhaseCollector bp_collector(ccd_body, body, shape_cast, settings, np_collector, mBodyManager, ioStep, ioContext->mStepDeltaTime); + mBroadPhase->CastAABoxNoLock({ shape_cast.mShapeWorldBounds, shape_cast.mDirection }, bp_collector, broadphase_layer_filter, object_layer_filter); + + // Check if there was a hit + if (ccd_body.mFractionPlusSlop < 1.0f) + { + const Body &body2 = mBodyManager.GetBody(ccd_body.mBodyID2); + + // Determine contact manifold + ContactManifold manifold; + manifold.mBaseOffset = shape_cast.mCenterOfMassStart.GetTranslation(); + ManifoldBetweenTwoFaces(cast_shape_result.mContactPointOn1, cast_shape_result.mContactPointOn2, cast_shape_result.mPenetrationAxis, mPhysicsSettings.mManifoldToleranceSq, cast_shape_result.mShape1Face, cast_shape_result.mShape2Face, manifold.mRelativeContactPointsOn1, manifold.mRelativeContactPointsOn2 JPH_IF_DEBUG_RENDERER(, manifold.mBaseOffset)); + manifold.mSubShapeID1 = cast_shape_result.mSubShapeID1; + manifold.mSubShapeID2 = cast_shape_result.mSubShapeID2; + manifold.mPenetrationDepth = cast_shape_result.mPenetrationDepth; + manifold.mWorldSpaceNormal = ccd_body.mContactNormal; + + // Call contact point callbacks + mContactManager.OnCCDContactAdded(contact_allocator, body, body2, manifold, ccd_body.mContactSettings); + + if (ccd_body.mContactSettings.mIsSensor) + { + // If this is a sensor, we don't want to solve the contact + ccd_body.mFractionPlusSlop = 1.0f; + ccd_body.mBodyID2 = BodyID(); + } + else + { + // Calculate the average position from the manifold (this will result in the same impulse applied as when we apply impulses to all contact points) + if (manifold.mRelativeContactPointsOn2.size() > 1) + { + Vec3 average_contact_point = Vec3::sZero(); + for (const Vec3 &v : manifold.mRelativeContactPointsOn2) + average_contact_point += v; + average_contact_point /= (float)manifold.mRelativeContactPointsOn2.size(); + ccd_body.mContactPointOn2 = manifold.mBaseOffset + average_contact_point; + } + else + ccd_body.mContactPointOn2 = manifold.mBaseOffset + cast_shape_result.mContactPointOn2; + } + } + } + + // Collect information from the contact allocator and accumulate it in the step. + sFinalizeContactAllocator(*ioStep, contact_allocator); +} + +void PhysicsSystem::JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // Read/write body access + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); + + // We activate bodies that we collide with + BodyManager::GrantActiveBodiesAccess grant_active(true, false); +#endif + + uint32 num_active_bodies_after_find_collisions = ioStep->mActiveBodyReadIdx; + TempAllocator *temp_allocator = ioContext->mTempAllocator; + + // Check if there's anything to do + uint num_ccd_bodies = ioStep->mNumCCDBodies; + if (num_ccd_bodies > 0) + { + // Sort on fraction so that we process earliest collisions first + // This is needed to make the simulation deterministic and also to be able to stop contact processing + // between body pairs if an earlier hit was found involving the body by another CCD body + // (if it's body ID < this CCD body's body ID - see filtering logic in CCDBroadPhaseCollector) + CCDBody **sorted_ccd_bodies = (CCDBody **)temp_allocator->Allocate(num_ccd_bodies * sizeof(CCDBody *)); + JPH_SCOPE_EXIT([temp_allocator, sorted_ccd_bodies, num_ccd_bodies]{ temp_allocator->Free(sorted_ccd_bodies, num_ccd_bodies * sizeof(CCDBody *)); }); + { + JPH_PROFILE("Sort"); + + // We don't want to copy the entire struct (it's quite big), so we create a pointer array first + CCDBody *src_ccd_bodies = ioStep->mCCDBodies; + CCDBody **dst_ccd_bodies = sorted_ccd_bodies; + CCDBody **dst_ccd_bodies_end = dst_ccd_bodies + num_ccd_bodies; + while (dst_ccd_bodies < dst_ccd_bodies_end) + *(dst_ccd_bodies++) = src_ccd_bodies++; + + // Which we then sort + QuickSort(sorted_ccd_bodies, sorted_ccd_bodies + num_ccd_bodies, [](const CCDBody *inBody1, const CCDBody *inBody2) + { + if (inBody1->mFractionPlusSlop != inBody2->mFractionPlusSlop) + return inBody1->mFractionPlusSlop < inBody2->mFractionPlusSlop; + + return inBody1->mBodyID1 < inBody2->mBodyID1; + }); + } + + // We can collide with bodies that are not active, we track them here so we can activate them in one go at the end. + // This is also needed because we can't modify the active body array while we iterate it. + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_activate = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_activate = 0; + + // We can move bodies that are not part of an island. In this case we need to notify the broadphase of the movement. + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + + for (uint i = 0; i < num_ccd_bodies; ++i) + { + const CCDBody *ccd_body = sorted_ccd_bodies[i]; + Body &body1 = mBodyManager.GetBody(ccd_body->mBodyID1); + MotionProperties *body_mp = body1.GetMotionProperties(); + + // If there was a hit + if (!ccd_body->mBodyID2.IsInvalid()) + { + Body &body2 = mBodyManager.GetBody(ccd_body->mBodyID2); + + // Determine if the other body has a CCD body + CCDBody *ccd_body2 = sGetCCDBody(body2, ioStep); + if (ccd_body2 != nullptr) + { + JPH_ASSERT(ccd_body2->mBodyID2 != ccd_body->mBodyID1, "If we collided with another body, that other body should have ignored collisions with us!"); + + // Check if the other body found a hit that is further away + if (ccd_body2->mFraction > ccd_body->mFraction) + { + // Reset the colliding body of the other CCD body. The other body will shorten its distance traveled and will not do any collision response (we'll do that). + // This means that at this point we have triggered a contact point add/persist for our further hit by accident for the other body. + // We accept this as calling the contact point callbacks here would require persisting the manifolds up to this point and doing the callbacks single threaded. + ccd_body2->mBodyID2 = BodyID(); + ccd_body2->mFractionPlusSlop = ccd_body->mFraction; + } + } + + // If the other body moved less than us before hitting something, we're not colliding with it so we again have triggered contact point add/persist callbacks by accident. + // We'll just move to the collision position anyway (as that's the last position we know is good), but we won't do any collision response. + if (ccd_body2 == nullptr || ccd_body2->mFraction >= ccd_body->mFraction) + { + const ContactSettings &contact_settings = ccd_body->mContactSettings; + + // Calculate contact point velocity for body 1 + Vec3 r1_plus_u = Vec3(ccd_body->mContactPointOn2 - (body1.GetCenterOfMassPosition() + ccd_body->mFraction * ccd_body->mDeltaPosition)); + Vec3 v1 = body1.GetPointVelocityCOM(r1_plus_u); + + // Calculate inverse mass for body 1 + float inv_m1 = contact_settings.mInvMassScale1 * body_mp->GetInverseMass(); + + if (body2.IsRigidBody()) + { + // Calculate contact point velocity for body 2 + Vec3 r2 = Vec3(ccd_body->mContactPointOn2 - body2.GetCenterOfMassPosition()); + Vec3 v2 = body2.GetPointVelocityCOM(r2); + + // Calculate relative contact velocity + Vec3 relative_velocity = v2 - v1; + float normal_velocity = relative_velocity.Dot(ccd_body->mContactNormal); + + // Calculate velocity bias due to restitution + float normal_velocity_bias; + if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution) + normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity; + else + normal_velocity_bias = 0.0f; + + // Get inverse mass of body 2 + float inv_m2 = body2.GetMotionPropertiesUnchecked() != nullptr? contact_settings.mInvMassScale2 * body2.GetMotionPropertiesUnchecked()->GetInverseMassUnchecked() : 0.0f; + + // Solve contact constraint + AxisConstraintPart contact_constraint; + contact_constraint.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, ccd_body->mContactNormal, normal_velocity_bias); + contact_constraint.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, ccd_body->mContactNormal, -FLT_MAX, FLT_MAX); + + // Apply friction + if (contact_settings.mCombinedFriction > 0.0f) + { + // Calculate friction direction by removing normal velocity from the relative velocity + Vec3 friction_direction = relative_velocity - normal_velocity * ccd_body->mContactNormal; + float friction_direction_len_sq = friction_direction.LengthSq(); + if (friction_direction_len_sq > 1.0e-12f) + { + // Normalize friction direction + friction_direction /= sqrt(friction_direction_len_sq); + + // Calculate max friction impulse + float max_lambda_f = contact_settings.mCombinedFriction * contact_constraint.GetTotalLambda(); + + AxisConstraintPart friction; + friction.CalculateConstraintPropertiesWithMassOverride(body1, inv_m1, contact_settings.mInvInertiaScale1, r1_plus_u, body2, inv_m2, contact_settings.mInvInertiaScale2, r2, friction_direction); + friction.SolveVelocityConstraintWithMassOverride(body1, inv_m1, body2, inv_m2, friction_direction, -max_lambda_f, max_lambda_f); + } + } + + // Clamp velocity of body 2 + if (body2.IsDynamic()) + { + MotionProperties *body2_mp = body2.GetMotionProperties(); + body2_mp->ClampLinearVelocity(); + body2_mp->ClampAngularVelocity(); + } + } + else + { + SoftBodyMotionProperties *soft_mp = static_cast(body2.GetMotionProperties()); + const SoftBodyShape *soft_shape = static_cast(body2.GetShape()); + + // Convert the sub shape ID of the soft body to a face + uint32 face_idx = soft_shape->GetFaceIndex(ccd_body->mSubShapeID2); + const SoftBodyMotionProperties::Face &face = soft_mp->GetFace(face_idx); + + // Get vertices of the face + SoftBodyMotionProperties::Vertex &vtx0 = soft_mp->GetVertex(face.mVertex[0]); + SoftBodyMotionProperties::Vertex &vtx1 = soft_mp->GetVertex(face.mVertex[1]); + SoftBodyMotionProperties::Vertex &vtx2 = soft_mp->GetVertex(face.mVertex[2]); + + // Inverse mass of the face + float vtx0_mass = vtx0.mInvMass > 0.0f? 1.0f / vtx0.mInvMass : 1.0e10f; + float vtx1_mass = vtx1.mInvMass > 0.0f? 1.0f / vtx1.mInvMass : 1.0e10f; + float vtx2_mass = vtx2.mInvMass > 0.0f? 1.0f / vtx2.mInvMass : 1.0e10f; + float inv_m2 = 1.0f / (vtx0_mass + vtx1_mass + vtx2_mass); + + // Calculate barycentric coordinates of the contact point on the soft body's face + float u, v, w; + RMat44 inv_body2_transform = body2.GetInverseCenterOfMassTransform(); + Vec3 local_contact = Vec3(inv_body2_transform * ccd_body->mContactPointOn2); + ClosestPoint::GetBaryCentricCoordinates(vtx0.mPosition - local_contact, vtx1.mPosition - local_contact, vtx2.mPosition - local_contact, u, v, w); + + // Calculate contact point velocity for the face + Vec3 v2 = inv_body2_transform.Multiply3x3Transposed(u * vtx0.mVelocity + v * vtx1.mVelocity + w * vtx2.mVelocity); + float normal_velocity = (v2 - v1).Dot(ccd_body->mContactNormal); + + // Calculate velocity bias due to restitution + float normal_velocity_bias; + if (contact_settings.mCombinedRestitution > 0.0f && normal_velocity < -mPhysicsSettings.mMinVelocityForRestitution) + normal_velocity_bias = contact_settings.mCombinedRestitution * normal_velocity; + else + normal_velocity_bias = 0.0f; + + // Calculate resulting velocity change (the math here is similar to AxisConstraintPart but without an inertia term for body 2 as we treat it as a point mass) + Vec3 r1_plus_u_x_n = r1_plus_u.Cross(ccd_body->mContactNormal); + Vec3 invi1_r1_plus_u_x_n = contact_settings.mInvInertiaScale1 * body1.GetInverseInertia().Multiply3x3(r1_plus_u_x_n); + float jv = r1_plus_u_x_n.Dot(body_mp->GetAngularVelocity()) - normal_velocity - normal_velocity_bias; + float inv_effective_mass = inv_m1 + inv_m2 + invi1_r1_plus_u_x_n.Dot(r1_plus_u_x_n); + float lambda = jv / inv_effective_mass; + body_mp->SubLinearVelocityStep((lambda * inv_m1) * ccd_body->mContactNormal); + body_mp->SubAngularVelocityStep(lambda * invi1_r1_plus_u_x_n); + Vec3 delta_v2 = inv_body2_transform.Multiply3x3(lambda * ccd_body->mContactNormal); + vtx0.mVelocity += delta_v2 * vtx0.mInvMass; + vtx1.mVelocity += delta_v2 * vtx1.mInvMass; + vtx2.mVelocity += delta_v2 * vtx2.mInvMass; + } + + // Clamp velocity of body 1 + body_mp->ClampLinearVelocity(); + body_mp->ClampAngularVelocity(); + + // Activate the 2nd body if it is not already active + if (body2.IsDynamic() && !body2.IsActive()) + { + bodies_to_activate[num_bodies_to_activate++] = ccd_body->mBodyID2; + if (num_bodies_to_activate == cBodiesBatch) + { + // Batch is full, activate now + mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate); + num_bodies_to_activate = 0; + } + } + + #ifdef JPH_DEBUG_RENDERER + if (sDrawMotionQualityLinearCast) + { + // Draw the collision location + RMat44 collision_transform = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFraction * ccd_body->mDeltaPosition); + body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform, Vec3::sReplicate(1.0f), Color::sYellow, false, true); + + // Draw the collision location + slop + RMat44 collision_transform_plus_slop = body1.GetCenterOfMassTransform().PostTranslated(ccd_body->mFractionPlusSlop * ccd_body->mDeltaPosition); + body1.GetShape()->Draw(DebugRenderer::sInstance, collision_transform_plus_slop, Vec3::sReplicate(1.0f), Color::sOrange, false, true); + + // Draw contact normal + DebugRenderer::sInstance->DrawArrow(ccd_body->mContactPointOn2, ccd_body->mContactPointOn2 - ccd_body->mContactNormal, Color::sYellow, 0.1f); + + // Draw post contact velocity + DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetLinearVelocity(), Color::sOrange, 0.1f); + DebugRenderer::sInstance->DrawArrow(collision_transform.GetTranslation(), collision_transform.GetTranslation() + body1.GetAngularVelocity(), Color::sPurple, 0.1f); + } + #endif // JPH_DEBUG_RENDERER + } + } + + // Update body position + body1.AddPositionStep(ccd_body->mDeltaPosition * ccd_body->mFractionPlusSlop); + + // If the body was activated due to an earlier CCD step it will have an index in the active + // body list that it higher than the highest one we processed during FindCollisions + // which means it hasn't been assigned an island and will not be updated by an island + // this means that we need to update its bounds manually + if (body_mp->GetIndexInActiveBodiesInternal() >= num_active_bodies_after_find_collisions) + { + body1.CalculateWorldSpaceBoundsInternal(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = body1.GetID(); + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + } + } + + // Activate the requested bodies + if (num_bodies_to_activate > 0) + mBodyManager.ActivateBodies(bodies_to_activate, num_bodies_to_activate); + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + } + + // Ensure we free the CCD bodies array now, will not call the destructor! + temp_allocator->Free(ioStep->mActiveBodyToCCDBody, ioStep->mNumActiveBodyToCCDBody * sizeof(int)); + ioStep->mActiveBodyToCCDBody = nullptr; + ioStep->mNumActiveBodyToCCDBody = 0; + temp_allocator->Free(ioStep->mCCDBodies, ioStep->mCCDBodiesCapacity * sizeof(CCDBody)); + ioStep->mCCDBodies = nullptr; + ioStep->mCCDBodiesCapacity = 0; +} + +void PhysicsSystem::JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We don't touch any bodies + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::None); +#endif + + // Reset the Body::EFlags::InvalidateContactCache flag for all bodies + mBodyManager.ValidateContactCacheForAllBodies(); + + // Finalize the contact cache (this swaps the read and write versions of the contact cache) + // Trigger all contact removed callbacks by looking at last step contact points that have not been flagged as reused + mContactManager.FinalizeContactCacheAndCallContactPointRemovedCallbacks(ioStep->mNumBodyPairs, ioStep->mNumManifolds); +} + +class PhysicsSystem::BodiesToSleep : public NonCopyable +{ +public: + static constexpr int cBodiesToSleepSize = 512; + static constexpr int cMaxBodiesToPutInBuffer = 128; + + inline BodiesToSleep(BodyManager &inBodyManager, BodyID *inBodiesToSleepBuffer) : mBodyManager(inBodyManager), mBodiesToSleepBuffer(inBodiesToSleepBuffer), mBodiesToSleepCur(inBodiesToSleepBuffer) { } + + inline ~BodiesToSleep() + { + // Flush the bodies to sleep buffer + int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer); + if (num_bodies_in_buffer > 0) + mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer); + } + + inline void PutToSleep(const BodyID *inBegin, const BodyID *inEnd) + { + int num_bodies_to_sleep = int(inEnd - inBegin); + if (num_bodies_to_sleep > cMaxBodiesToPutInBuffer) + { + // Too many bodies, deactivate immediately + mBodyManager.DeactivateBodies(inBegin, num_bodies_to_sleep); + } + else + { + // Check if there's enough space in the bodies to sleep buffer + int num_bodies_in_buffer = int(mBodiesToSleepCur - mBodiesToSleepBuffer); + if (num_bodies_in_buffer + num_bodies_to_sleep > cBodiesToSleepSize) + { + // Flush the bodies to sleep buffer + mBodyManager.DeactivateBodies(mBodiesToSleepBuffer, num_bodies_in_buffer); + mBodiesToSleepCur = mBodiesToSleepBuffer; + } + + // Copy the bodies in the buffer + memcpy(mBodiesToSleepCur, inBegin, num_bodies_to_sleep * sizeof(BodyID)); + mBodiesToSleepCur += num_bodies_to_sleep; + } + } + +private: + BodyManager & mBodyManager; + BodyID * mBodiesToSleepBuffer; + BodyID * mBodiesToSleepCur; +}; + +void PhysicsSystem::CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep) +{ + // Get the bodies that belong to this island + BodyID *bodies_begin, *bodies_end; + mIslandBuilder.GetBodiesInIsland(inIslandIndex, bodies_begin, bodies_end); + + // Only check sleeping in the last step + // Also resets force and torque used during the apply gravity phase + if (ioStep->mIsLast) + { + JPH_PROFILE("Check Sleeping"); + + static_assert(int(ECanSleep::CannotSleep) == 0 && int(ECanSleep::CanSleep) == 1, "Loop below makes this assumption"); + int all_can_sleep = mPhysicsSettings.mAllowSleeping? int(ECanSleep::CanSleep) : int(ECanSleep::CannotSleep); + + float time_before_sleep = mPhysicsSettings.mTimeBeforeSleep; + float max_movement = mPhysicsSettings.mPointVelocitySleepThreshold * time_before_sleep; + + for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + { + Body &body = mBodyManager.GetBody(*body_id); + + // Update bounding box + body.CalculateWorldSpaceBoundsInternal(); + + // Update sleeping + all_can_sleep &= int(body.UpdateSleepStateInternal(ioContext->mStepDeltaTime, max_movement, time_before_sleep)); + + // Reset force and torque + MotionProperties *mp = body.GetMotionProperties(); + mp->ResetForce(); + mp->ResetTorque(); + } + + // If all bodies indicate they can sleep we can deactivate them + if (all_can_sleep == int(ECanSleep::CanSleep)) + ioBodiesToSleep.PutToSleep(bodies_begin, bodies_end); + } + else + { + JPH_PROFILE("Update Bounds"); + + // Update bounding box only for all other steps + for (const BodyID *body_id = bodies_begin; body_id < bodies_end; ++body_id) + { + Body &body = mBodyManager.GetBody(*body_id); + body.CalculateWorldSpaceBoundsInternal(); + } + } + + // Notify broadphase of changed objects (find ccd contacts can do linear casts in the next step, so we need to do this every step) + // Note: Shuffles the BodyID's around!!! + mBroadPhase->NotifyBodiesAABBChanged(bodies_begin, int(bodies_end - bodies_begin), false); +} + +void PhysicsSystem::JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ +#ifdef JPH_ENABLE_ASSERTS + // We fix up position errors + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::ReadWrite); + + // Can only deactivate bodies + BodyManager::GrantActiveBodiesAccess grant_active(false, true); +#endif + + float delta_time = ioContext->mStepDeltaTime; + float baumgarte = mPhysicsSettings.mBaumgarte; + Constraint **active_constraints = ioContext->mActiveConstraints; + + // Keep a buffer of bodies that need to go to sleep in order to not constantly lock the active bodies mutex and create contention between all solving threads + BodiesToSleep bodies_to_sleep(mBodyManager, (BodyID *)JPH_STACK_ALLOC(BodiesToSleep::cBodiesToSleepSize * sizeof(BodyID))); + + bool check_islands = true, check_split_islands = mPhysicsSettings.mUseLargeIslandSplitter; + do + { + // First try to get work from large islands + if (check_split_islands) + { + bool first_iteration; + uint split_island_index; + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + switch (mLargeIslandSplitter.FetchNextBatch(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, first_iteration)) + { + case LargeIslandSplitter::EStatus::BatchRetrieved: + // Solve the batch + ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte); + mContactManager.SolvePositionConstraints(contacts_begin, contacts_end); + + // Mark the batch as processed + bool last_iteration, final_batch; + mLargeIslandSplitter.MarkBatchProcessed(split_island_index, constraints_begin, constraints_end, contacts_begin, contacts_end, last_iteration, final_batch); + + // The final batch will update all bounds and check sleeping + if (final_batch) + CheckSleepAndUpdateBounds(mLargeIslandSplitter.GetIslandIndex(split_island_index), ioContext, ioStep, bodies_to_sleep); + + // We processed work, loop again + continue; + case LargeIslandSplitter::EStatus::WaitingForBatch: + break; + case LargeIslandSplitter::EStatus::AllBatchesDone: + check_split_islands = false; + break; + } + } + + // If that didn't succeed try to process an island + if (check_islands) + { + // Next island + uint32 island_idx = ioStep->mSolvePositionConstraintsNextIsland++; + if (island_idx >= mIslandBuilder.GetNumIslands()) + { + // We processed all islands, stop checking islands + check_islands = false; + continue; + } + + JPH_PROFILE("Island"); + + // Get iterators for this island + uint32 *constraints_begin, *constraints_end, *contacts_begin, *contacts_end; + mIslandBuilder.GetConstraintsInIsland(island_idx, constraints_begin, constraints_end); + mIslandBuilder.GetContactsInIsland(island_idx, contacts_begin, contacts_end); + + // If this island is a large island, it will be picked up as a batch and we don't need to do anything here + uint num_items = uint(constraints_end - constraints_begin) + uint(contacts_end - contacts_begin); + if (mPhysicsSettings.mUseLargeIslandSplitter + && num_items >= LargeIslandSplitter::cLargeIslandTreshold) + continue; + + // Check if this island needs solving + if (num_items > 0) + { + // Iterate + uint num_position_steps = mIslandBuilder.GetNumPositionSteps(island_idx); + for (uint position_step = 0; position_step < num_position_steps; ++position_step) + { + bool applied_impulse = ConstraintManager::sSolvePositionConstraints(active_constraints, constraints_begin, constraints_end, delta_time, baumgarte); + applied_impulse |= mContactManager.SolvePositionConstraints(contacts_begin, contacts_end); + if (!applied_impulse) + break; + } + } + + // After solving we will update all bounds and check sleeping + CheckSleepAndUpdateBounds(island_idx, ioContext, ioStep, bodies_to_sleep); + + // We processed work, loop again + continue; + } + + // If we didn't find any work, give up a time slice + std::this_thread::yield(); + } + while (check_islands || check_split_islands); +} + +void PhysicsSystem::JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) +{ + JPH_PROFILE_FUNCTION(); + + { + #ifdef JPH_ENABLE_ASSERTS + // Reading soft body positions + BodyAccess::Grant grant(BodyAccess::EAccess::None, BodyAccess::EAccess::Read); + #endif + + // Get the active soft bodies + BodyIDVector active_bodies; + mBodyManager.GetActiveBodies(EBodyType::SoftBody, active_bodies); + + // Quit if there are no active soft bodies + if (active_bodies.empty()) + { + // Kick the next step + if (ioStep->mStartNextStep.IsValid()) + ioStep->mStartNextStep.RemoveDependency(); + return; + } + + // Sort to get a deterministic update order + QuickSort(active_bodies.begin(), active_bodies.end()); + + // Allocate soft body contexts + ioContext->mNumSoftBodies = (uint)active_bodies.size(); + ioContext->mSoftBodyUpdateContexts = (SoftBodyUpdateContext *)ioContext->mTempAllocator->Allocate(ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext)); + + // Initialize soft body contexts + for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx) + { + new (sb_ctx) SoftBodyUpdateContext; + Body &body = mBodyManager.GetBody(active_bodies[sb_ctx - ioContext->mSoftBodyUpdateContexts]); + SoftBodyMotionProperties *mp = static_cast(body.GetMotionProperties()); + mp->InitializeUpdateContext(ioContext->mStepDeltaTime, body, *this, *sb_ctx); + } + } + + // We're ready to collide the first soft body + ioContext->mSoftBodyToCollide.store(0, memory_order_release); + + // Determine number of jobs to spawn + int num_soft_body_jobs = ioContext->GetMaxConcurrency(); + + // Create finalize job + ioStep->mSoftBodyFinalize = ioContext->mJobSystem->CreateJob("SoftBodyFinalize", cColorSoftBodyFinalize, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobSoftBodyFinalize(ioContext); + + // Kick the next step + if (ioStep->mStartNextStep.IsValid()) + ioStep->mStartNextStep.RemoveDependency(); + }, num_soft_body_jobs); // depends on: soft body simulate + ioContext->mBarrier->AddJob(ioStep->mSoftBodyFinalize); + + // Create simulate jobs + ioStep->mSoftBodySimulate.resize(num_soft_body_jobs); + for (int i = 0; i < num_soft_body_jobs; ++i) + ioStep->mSoftBodySimulate[i] = ioContext->mJobSystem->CreateJob("SoftBodySimulate", cColorSoftBodySimulate, [ioStep, i]() + { + ioStep->mContext->mPhysicsSystem->JobSoftBodySimulate(ioStep->mContext, i); + + ioStep->mSoftBodyFinalize.RemoveDependency(); + }, num_soft_body_jobs); // depends on: soft body collide + ioContext->mBarrier->AddJobs(ioStep->mSoftBodySimulate.data(), ioStep->mSoftBodySimulate.size()); + + // Create collision jobs + ioStep->mSoftBodyCollide.resize(num_soft_body_jobs); + for (int i = 0; i < num_soft_body_jobs; ++i) + ioStep->mSoftBodyCollide[i] = ioContext->mJobSystem->CreateJob("SoftBodyCollide", cColorSoftBodyCollide, [ioContext, ioStep]() + { + ioContext->mPhysicsSystem->JobSoftBodyCollide(ioContext); + + for (const JobHandle &h : ioStep->mSoftBodySimulate) + h.RemoveDependency(); + }); // depends on: nothing + ioContext->mBarrier->AddJobs(ioStep->mSoftBodyCollide.data(), ioStep->mSoftBodyCollide.size()); +} + +void PhysicsSystem::JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const +{ +#ifdef JPH_ENABLE_ASSERTS + // Reading rigid body positions and velocities + BodyAccess::Grant grant(BodyAccess::EAccess::Read, BodyAccess::EAccess::Read); +#endif + + for (;;) + { + // Fetch the next soft body + uint sb_idx = ioContext->mSoftBodyToCollide.fetch_add(1, std::memory_order_acquire); + if (sb_idx >= ioContext->mNumSoftBodies) + break; + + // Do a broadphase check + SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[sb_idx]; + sb_ctx.mMotionProperties->DetermineCollidingShapes(sb_ctx, *this, GetBodyLockInterfaceNoLock()); + } +} + +void PhysicsSystem::JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const +{ +#ifdef JPH_ENABLE_ASSERTS + // Updating velocities of soft bodies, allow the contact listener to read the soft body state + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::Read); +#endif + + // Calculate at which body we start to distribute the workload across the threads + uint num_soft_bodies = ioContext->mNumSoftBodies; + uint start_idx = inThreadIndex * num_soft_bodies / ioContext->GetMaxConcurrency(); + + // Keep running partial updates until everything has been updated + uint status; + do + { + // Reset status + status = 0; + + // Update all soft bodies + for (uint i = 0; i < num_soft_bodies; ++i) + { + // Fetch the soft body context + SoftBodyUpdateContext &sb_ctx = ioContext->mSoftBodyUpdateContexts[(start_idx + i) % num_soft_bodies]; + + // To avoid trashing the cache too much, we prefer to stick to one soft body until we cannot progress it any further + uint sb_status; + do + { + sb_status = (uint)sb_ctx.mMotionProperties->ParallelUpdate(sb_ctx, mPhysicsSettings); + status |= sb_status; + } while (sb_status == (uint)SoftBodyMotionProperties::EStatus::DidWork); + } + + // If we didn't perform any work, yield the thread so that something else can run + if (!(status & (uint)SoftBodyMotionProperties::EStatus::DidWork)) + std::this_thread::yield(); + } + while (status != (uint)SoftBodyMotionProperties::EStatus::Done); +} + +void PhysicsSystem::JobSoftBodyFinalize(PhysicsUpdateContext *ioContext) +{ +#ifdef JPH_ENABLE_ASSERTS + // Updating rigid body velocities and soft body positions / velocities + BodyAccess::Grant grant(BodyAccess::EAccess::ReadWrite, BodyAccess::EAccess::ReadWrite); + + // Can activate and deactivate bodies + BodyManager::GrantActiveBodiesAccess grant_active(true, true); +#endif + + static constexpr int cBodiesBatch = 64; + BodyID *bodies_to_update_bounds = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_update_bounds = 0; + BodyID *bodies_to_put_to_sleep = (BodyID *)JPH_STACK_ALLOC(cBodiesBatch * sizeof(BodyID)); + int num_bodies_to_put_to_sleep = 0; + + for (SoftBodyUpdateContext *sb_ctx = ioContext->mSoftBodyUpdateContexts, *sb_ctx_end = ioContext->mSoftBodyUpdateContexts + ioContext->mNumSoftBodies; sb_ctx < sb_ctx_end; ++sb_ctx) + { + // Apply the rigid body velocity deltas + sb_ctx->mMotionProperties->UpdateRigidBodyVelocities(*sb_ctx, GetBodyInterfaceNoLock()); + + // Update the position + sb_ctx->mBody->SetPositionAndRotationInternal(sb_ctx->mBody->GetPosition() + sb_ctx->mDeltaPosition, sb_ctx->mBody->GetRotation(), false); + + BodyID id = sb_ctx->mBody->GetID(); + bodies_to_update_bounds[num_bodies_to_update_bounds++] = id; + if (num_bodies_to_update_bounds == cBodiesBatch) + { + // Buffer full, flush now + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + num_bodies_to_update_bounds = 0; + } + + if (sb_ctx->mCanSleep == ECanSleep::CanSleep) + { + // This body should go to sleep + bodies_to_put_to_sleep[num_bodies_to_put_to_sleep++] = id; + if (num_bodies_to_put_to_sleep == cBodiesBatch) + { + mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep); + num_bodies_to_put_to_sleep = 0; + } + } + } + + // Notify change bounds on requested bodies + if (num_bodies_to_update_bounds > 0) + mBroadPhase->NotifyBodiesAABBChanged(bodies_to_update_bounds, num_bodies_to_update_bounds, false); + + // Notify bodies to go to sleep + if (num_bodies_to_put_to_sleep > 0) + mBodyManager.DeactivateBodies(bodies_to_put_to_sleep, num_bodies_to_put_to_sleep); + + // Free soft body contexts + ioContext->mTempAllocator->Free(ioContext->mSoftBodyUpdateContexts, ioContext->mNumSoftBodies * sizeof(SoftBodyUpdateContext)); +} + +void PhysicsSystem::SaveState(StateRecorder &inStream, EStateRecorderState inState, const StateRecorderFilter *inFilter) const +{ + JPH_PROFILE_FUNCTION(); + + inStream.Write(inState); + + if (uint8(inState) & uint8(EStateRecorderState::Global)) + { + inStream.Write(mPreviousStepDeltaTime); + inStream.Write(mGravity); + } + + if (uint8(inState) & uint8(EStateRecorderState::Bodies)) + mBodyManager.SaveState(inStream, inFilter); + + if (uint8(inState) & uint8(EStateRecorderState::Contacts)) + mContactManager.SaveState(inStream, inFilter); + + if (uint8(inState) & uint8(EStateRecorderState::Constraints)) + mConstraintManager.SaveState(inStream, inFilter); +} + +bool PhysicsSystem::RestoreState(StateRecorder &inStream) +{ + JPH_PROFILE_FUNCTION(); + + EStateRecorderState state = EStateRecorderState::All; // Set this value for validation. If a partial state is saved, validation will not work anyway. + inStream.Read(state); + + if (uint8(state) & uint8(EStateRecorderState::Global)) + { + inStream.Read(mPreviousStepDeltaTime); + inStream.Read(mGravity); + } + + if (uint8(state) & uint8(EStateRecorderState::Bodies)) + { + if (!mBodyManager.RestoreState(inStream)) + return false; + + // Update bounding boxes for all bodies in the broadphase + Array bodies; + for (const Body *b : mBodyManager.GetBodies()) + if (BodyManager::sIsValidBodyPointer(b) && b->IsInBroadPhase()) + bodies.push_back(b->GetID()); + if (!bodies.empty()) + mBroadPhase->NotifyBodiesAABBChanged(&bodies[0], (int)bodies.size()); + } + + if (uint8(state) & uint8(EStateRecorderState::Contacts)) + { + if (!mContactManager.RestoreState(inStream)) + return false; + } + + if (uint8(state) & uint8(EStateRecorderState::Constraints)) + { + if (!mConstraintManager.RestoreState(inStream)) + return false; + } + + return true; +} + +void PhysicsSystem::SaveBodyState(const Body &inBody, StateRecorder &inStream) const +{ + mBodyManager.SaveBodyState(inBody, inStream); +} + +void PhysicsSystem::RestoreBodyState(Body &ioBody, StateRecorder &inStream) +{ + mBodyManager.RestoreBodyState(ioBody, inStream); + + BodyID id = ioBody.GetID(); + mBroadPhase->NotifyBodiesAABBChanged(&id, 1); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsSystem.h b/WickedEngine/Jolt/Physics/PhysicsSystem.h new file mode 100644 index 000000000..3234e5b53 --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsSystem.h @@ -0,0 +1,320 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class JobSystem; +class StateRecorder; +class TempAllocator; +class PhysicsStepListener; +class SoftBodyContactListener; + +/// The main class for the physics system. It contains all rigid bodies and simulates them. +/// +/// The main simulation is performed by the Update() call on multiple threads (if the JobSystem is configured to use them). Please refer to the general architecture overview in the Docs folder for more information. +class JPH_EXPORT PhysicsSystem : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / Destructor + PhysicsSystem() : mContactManager(mPhysicsSettings) JPH_IF_ENABLE_ASSERTS(, mConstraintManager(&mBodyManager)) { } + ~PhysicsSystem(); + + /// Initialize the system. + /// @param inMaxBodies Maximum number of bodies to support. + /// @param inNumBodyMutexes Number of body mutexes to use. Should be a power of 2 in the range [1, 64], use 0 to auto detect. + /// @param inMaxBodyPairs Maximum amount of body pairs to process (anything else will fall through the world), this number should generally be much higher than the max amount of contact points as there will be lots of bodies close that are not actually touching. + /// @param inMaxContactConstraints Maximum amount of contact constraints to process (anything else will fall through the world). + /// @param inBroadPhaseLayerInterface Information on the mapping of object layers to broad phase layers. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + /// @param inObjectVsBroadPhaseLayerFilter Filter callback function that is used to determine if an object layer collides with a broad phase layer. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + /// @param inObjectLayerPairFilter Filter callback function that is used to determine if two object layers collide. Since this is a virtual interface, the instance needs to stay alive during the lifetime of the PhysicsSystem. + void Init(uint inMaxBodies, uint inNumBodyMutexes, uint inMaxBodyPairs, uint inMaxContactConstraints, const BroadPhaseLayerInterface &inBroadPhaseLayerInterface, const ObjectVsBroadPhaseLayerFilter &inObjectVsBroadPhaseLayerFilter, const ObjectLayerPairFilter &inObjectLayerPairFilter); + + /// Listener that is notified whenever a body is activated/deactivated + void SetBodyActivationListener(BodyActivationListener *inListener) { mBodyManager.SetBodyActivationListener(inListener); } + BodyActivationListener * GetBodyActivationListener() const { return mBodyManager.GetBodyActivationListener(); } + + /// Listener that is notified whenever a contact point between two bodies is added/updated/removed + void SetContactListener(ContactListener *inListener) { mContactManager.SetContactListener(inListener); } + ContactListener * GetContactListener() const { return mContactManager.GetContactListener(); } + + /// Listener that is notified whenever a contact point between a soft body and another body + void SetSoftBodyContactListener(SoftBodyContactListener *inListener) { mSoftBodyContactListener = inListener; } + SoftBodyContactListener * GetSoftBodyContactListener() const { return mSoftBodyContactListener; } + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(ContactConstraintManager::CombineFunction inCombineFriction) { mContactManager.SetCombineFriction(inCombineFriction); } + ContactConstraintManager::CombineFunction GetCombineFriction() const { return mContactManager.GetCombineFriction(); } + + /// Set the function that combines the restitution of two bodies and returns it + /// Default method is max(restitution1, restitution1) + void SetCombineRestitution(ContactConstraintManager::CombineFunction inCombineRestition) { mContactManager.SetCombineRestitution(inCombineRestition); } + ContactConstraintManager::CombineFunction GetCombineRestitution() const { return mContactManager.GetCombineRestitution(); } + + /// Control the main constants of the physics simulation + void SetPhysicsSettings(const PhysicsSettings &inSettings) { mPhysicsSettings = inSettings; } + const PhysicsSettings & GetPhysicsSettings() const { return mPhysicsSettings; } + + /// Access to the body interface. This interface allows to to create / remove bodies and to change their properties. + const BodyInterface & GetBodyInterface() const { return mBodyInterfaceLocking; } + BodyInterface & GetBodyInterface() { return mBodyInterfaceLocking; } + const BodyInterface & GetBodyInterfaceNoLock() const { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care! + BodyInterface & GetBodyInterfaceNoLock() { return mBodyInterfaceNoLock; } ///< Version that does not lock the bodies, use with great care! + + /// Access to the broadphase interface that allows coarse collision queries + const BroadPhaseQuery & GetBroadPhaseQuery() const { return *mBroadPhase; } + + /// Interface that allows fine collision queries against first the broad phase and then the narrow phase. + const NarrowPhaseQuery & GetNarrowPhaseQuery() const { return mNarrowPhaseQueryLocking; } + const NarrowPhaseQuery & GetNarrowPhaseQueryNoLock() const { return mNarrowPhaseQueryNoLock; } ///< Version that does not lock the bodies, use with great care! + + /// Add constraint to the world + void AddConstraint(Constraint *inConstraint) { mConstraintManager.Add(&inConstraint, 1); } + + /// Remove constraint from the world + void RemoveConstraint(Constraint *inConstraint) { mConstraintManager.Remove(&inConstraint, 1); } + + /// Batch add constraints. Note that the inConstraints array is allowed to have nullptrs, these will be ignored. + void AddConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Add(inConstraints, inNumber); } + + /// Batch remove constraints. Note that the inConstraints array is allowed to have nullptrs, these will be ignored. + void RemoveConstraints(Constraint **inConstraints, int inNumber) { mConstraintManager.Remove(inConstraints, inNumber); } + + /// Get a list of all constraints + Constraints GetConstraints() const { return mConstraintManager.GetConstraints(); } + + /// Optimize the broadphase, needed only if you've added many bodies prior to calling Update() for the first time. + void OptimizeBroadPhase(); + + /// Adds a new step listener + void AddStepListener(PhysicsStepListener *inListener); + + /// Removes a step listener + void RemoveStepListener(PhysicsStepListener *inListener); + + /// Simulate the system. + /// The world steps for a total of inDeltaTime seconds. This is divided in inCollisionSteps iterations. + /// Each iteration consists of collision detection followed by an integration step. + /// This function internally spawns jobs using inJobSystem and waits for them to complete, so no jobs will be running when this function returns. + EPhysicsUpdateError Update(float inDeltaTime, int inCollisionSteps, TempAllocator *inTempAllocator, JobSystem *inJobSystem); + + /// Saving state for replay + void SaveState(StateRecorder &inStream, EStateRecorderState inState = EStateRecorderState::All, const StateRecorderFilter *inFilter = nullptr) const; + + /// Restoring state for replay. Returns false if failed. + bool RestoreState(StateRecorder &inStream); + + /// Saving state of a single body. + void SaveBodyState(const Body &inBody, StateRecorder &inStream) const; + + /// Restoring state of a single body. + void RestoreBodyState(Body &ioBody, StateRecorder &inStream); + +#ifdef JPH_DEBUG_RENDERER + // Drawing properties + static bool sDrawMotionQualityLinearCast; ///< Draw debug info for objects that perform continuous collision detection through the linear cast motion quality + + /// Draw the state of the bodies (debugging purposes) + void DrawBodies(const BodyManager::DrawSettings &inSettings, DebugRenderer *inRenderer, const BodyDrawFilter *inBodyFilter = nullptr) { mBodyManager.Draw(inSettings, mPhysicsSettings, inRenderer, inBodyFilter); } + + /// Draw the constraints only (debugging purposes) + void DrawConstraints(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraints(inRenderer); } + + /// Draw the constraint limits only (debugging purposes) + void DrawConstraintLimits(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintLimits(inRenderer); } + + /// Draw the constraint reference frames only (debugging purposes) + void DrawConstraintReferenceFrame(DebugRenderer *inRenderer) { mConstraintManager.DrawConstraintReferenceFrame(inRenderer); } +#endif // JPH_DEBUG_RENDERER + + /// Set gravity value + void SetGravity(Vec3Arg inGravity) { mGravity = inGravity; } + Vec3 GetGravity() const { return mGravity; } + + /// Returns a locking interface that won't actually lock the body. Use with great care! + inline const BodyLockInterfaceNoLock & GetBodyLockInterfaceNoLock() const { return mBodyLockInterfaceNoLock; } + + /// Returns a locking interface that locks the body so other threads cannot modify it. + inline const BodyLockInterfaceLocking & GetBodyLockInterface() const { return mBodyLockInterfaceLocking; } + + /// Get an broadphase layer filter that uses the default pair filter and a specified object layer to determine if broadphase layers collide + DefaultBroadPhaseLayerFilter GetDefaultBroadPhaseLayerFilter(ObjectLayer inLayer) const { return DefaultBroadPhaseLayerFilter(*mObjectVsBroadPhaseLayerFilter, inLayer); } + + /// Get an object layer filter that uses the default pair filter and a specified layer to determine if layers collide + DefaultObjectLayerFilter GetDefaultLayerFilter(ObjectLayer inLayer) const { return DefaultObjectLayerFilter(*mObjectLayerPairFilter, inLayer); } + + /// Gets the current amount of bodies that are in the body manager + uint GetNumBodies() const { return mBodyManager.GetNumBodies(); } + + /// Gets the current amount of active bodies that are in the body manager + uint32 GetNumActiveBodies(EBodyType inType) const { return mBodyManager.GetNumActiveBodies(inType); } + + /// Get the maximum amount of bodies that this physics system supports + uint GetMaxBodies() const { return mBodyManager.GetMaxBodies(); } + + /// Helper struct that counts the number of bodies of each type + using BodyStats = BodyManager::BodyStats; + + /// Get stats about the bodies in the body manager (slow, iterates through all bodies) + BodyStats GetBodyStats() const { return mBodyManager.GetBodyStats(); } + + /// Get copy of the list of all bodies under protection of a lock. + /// @param outBodyIDs On return, this will contain the list of BodyIDs + void GetBodies(BodyIDVector &outBodyIDs) const { return mBodyManager.GetBodyIDs(outBodyIDs); } + + /// Get copy of the list of active bodies under protection of a lock. + /// @param inType The type of bodies to get + /// @param outBodyIDs On return, this will contain the list of BodyIDs + void GetActiveBodies(EBodyType inType, BodyIDVector &outBodyIDs) const { return mBodyManager.GetActiveBodies(inType, outBodyIDs); } + + /// Get the list of active bodies, use GetNumActiveBodies() to find out how long the list is. + /// Note: Not thread safe. The active bodies list can change at any moment when other threads are doing work. Use GetActiveBodies() if you need a thread safe version. + const BodyID * GetActiveBodiesUnsafe(EBodyType inType) const { return mBodyManager.GetActiveBodiesUnsafe(inType); } + + /// Check if 2 bodies were in contact during the last simulation step. Since contacts are only detected between active bodies, so at least one of the bodies must be active in order for this function to work. + /// It queries the state at the time of the last PhysicsSystem::Update and will return true if the bodies were in contact, even if one of the bodies was moved / removed afterwards. + /// This function can be called from any thread when the PhysicsSystem::Update is not running. During PhysicsSystem::Update this function is only valid during contact callbacks: + /// - During the ContactListener::OnContactAdded callback this function can be used to determine if a different contact pair between the bodies was active in the previous simulation step (function returns true) or if this is the first step that the bodies are touching (function returns false). + /// - During the ContactListener::OnContactRemoved callback this function can be used to determine if this is the last contact pair between the bodies (function returns false) or if there are other contacts still present (function returns true). + bool WereBodiesInContact(const BodyID &inBody1ID, const BodyID &inBody2ID) const { return mContactManager.WereBodiesInContact(inBody1ID, inBody2ID); } + + /// Get the bounding box of all bodies in the physics system + AABox GetBounds() const { return mBroadPhase->GetBounds(); } + +#ifdef JPH_TRACK_BROADPHASE_STATS + /// Trace the accumulated broadphase stats to the TTY + void ReportBroadphaseStats() { mBroadPhase->ReportStats(); } +#endif // JPH_TRACK_BROADPHASE_STATS + +private: + using CCDBody = PhysicsUpdateContext::Step::CCDBody; + + // Various job entry points + void JobStepListeners(PhysicsUpdateContext::Step *ioStep); + void JobDetermineActiveConstraints(PhysicsUpdateContext::Step *ioStep) const; + void JobApplyGravity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSetupVelocityConstraints(float inDeltaTime, PhysicsUpdateContext::Step *ioStep) const; + void JobBuildIslandsFromConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobFindCollisions(PhysicsUpdateContext::Step *ioStep, int inJobIndex); + void JobFinalizeIslands(PhysicsUpdateContext *ioContext); + void JobBodySetIslandIndex(); + void JobSolveVelocityConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobPreIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobIntegrateVelocity(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobPostIntegrateVelocity(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep) const; + void JobFindCCDContacts(const PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobResolveCCDContacts(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobContactRemovedCallbacks(const PhysicsUpdateContext::Step *ioStep); + void JobSolvePositionConstraints(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSoftBodyPrepare(PhysicsUpdateContext *ioContext, PhysicsUpdateContext::Step *ioStep); + void JobSoftBodyCollide(PhysicsUpdateContext *ioContext) const; + void JobSoftBodySimulate(PhysicsUpdateContext *ioContext, uint inThreadIndex) const; + void JobSoftBodyFinalize(PhysicsUpdateContext *ioContext); + + /// Tries to spawn a new FindCollisions job if max concurrency hasn't been reached yet + void TrySpawnJobFindCollisions(PhysicsUpdateContext::Step *ioStep) const; + + using ContactAllocator = ContactConstraintManager::ContactAllocator; + + /// Process narrow phase for a single body pair + void ProcessBodyPair(ContactAllocator &ioContactAllocator, const BodyPair &inBodyPair); + + /// This helper batches up bodies that need to put to sleep to avoid contention on the activation mutex + class BodiesToSleep; + + /// Called at the end of JobSolveVelocityConstraints to check if bodies need to go to sleep and to update their bounding box in the broadphase + void CheckSleepAndUpdateBounds(uint32 inIslandIndex, const PhysicsUpdateContext *ioContext, const PhysicsUpdateContext::Step *ioStep, BodiesToSleep &ioBodiesToSleep); + + /// Number of constraints to process at once in JobDetermineActiveConstraints + static constexpr int cDetermineActiveConstraintsBatchSize = 64; + + /// Number of constraints to process at once in JobSetupVelocityConstraints, we want a low number of threads working on this so we take fairly large batches + static constexpr int cSetupVelocityConstraintsBatchSize = 256; + + /// Number of bodies to process at once in JobApplyGravity + static constexpr int cApplyGravityBatchSize = 64; + + /// Number of active bodies to test for collisions per batch + static constexpr int cActiveBodiesBatchSize = 16; + + /// Number of active bodies to integrate velocities for + static constexpr int cIntegrateVelocityBatchSize = 64; + + /// Number of contacts that need to be queued before another narrow phase job is started + static constexpr int cNarrowPhaseBatchSize = 16; + + /// Number of continuous collision shape casts that need to be queued before another job is started + static constexpr int cNumCCDBodiesPerJob = 4; + + /// Broadphase layer filter that decides if two objects can collide + const ObjectVsBroadPhaseLayerFilter *mObjectVsBroadPhaseLayerFilter = nullptr; + + /// Object layer filter that decides if two objects can collide + const ObjectLayerPairFilter *mObjectLayerPairFilter = nullptr; + + /// The body manager keeps track which bodies are in the simulation + BodyManager mBodyManager; + + /// Body locking interfaces + BodyLockInterfaceNoLock mBodyLockInterfaceNoLock { mBodyManager }; + BodyLockInterfaceLocking mBodyLockInterfaceLocking { mBodyManager }; + + /// Body interfaces + BodyInterface mBodyInterfaceNoLock; + BodyInterface mBodyInterfaceLocking; + + /// Narrow phase query interface + NarrowPhaseQuery mNarrowPhaseQueryNoLock; + NarrowPhaseQuery mNarrowPhaseQueryLocking; + + /// The broadphase does quick collision detection between body pairs + BroadPhase * mBroadPhase = nullptr; + + /// The soft body contact listener + SoftBodyContactListener * mSoftBodyContactListener = nullptr; + + /// Simulation settings + PhysicsSettings mPhysicsSettings; + + /// The contact manager resolves all contacts during a simulation step + ContactConstraintManager mContactManager; + + /// All non-contact constraints + ConstraintManager mConstraintManager; + + /// Keeps track of connected bodies and builds islands for multithreaded velocity/position update + IslandBuilder mIslandBuilder; + + /// Will split large islands into smaller groups of bodies that can be processed in parallel + LargeIslandSplitter mLargeIslandSplitter; + + /// Mutex protecting mStepListeners + Mutex mStepListenersMutex; + + /// List of physics step listeners + using StepListeners = Array; + StepListeners mStepListeners; + + /// This is the global gravity vector + Vec3 mGravity = Vec3(0, -9.81f, 0); + + /// Previous frame's delta time of one sub step to allow scaling previous frame's constraint impulses + float mPreviousStepDeltaTime = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsUpdateContext.cpp b/WickedEngine/Jolt/Physics/PhysicsUpdateContext.cpp new file mode 100644 index 000000000..7c60ae6be --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsUpdateContext.cpp @@ -0,0 +1,23 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +PhysicsUpdateContext::PhysicsUpdateContext(TempAllocator &inTempAllocator) : + mTempAllocator(&inTempAllocator), + mSteps(inTempAllocator) +{ +} + +PhysicsUpdateContext::~PhysicsUpdateContext() +{ + JPH_ASSERT(mBodyPairs == nullptr); + JPH_ASSERT(mActiveConstraints == nullptr); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/PhysicsUpdateContext.h b/WickedEngine/Jolt/Physics/PhysicsUpdateContext.h new file mode 100644 index 000000000..fe99c46cc --- /dev/null +++ b/WickedEngine/Jolt/Physics/PhysicsUpdateContext.h @@ -0,0 +1,172 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class IslandBuilder; +class Constraint; +class TempAllocator; +class SoftBodyUpdateContext; + +/// Information used during the Update call +class PhysicsUpdateContext : public NonCopyable +{ +public: + /// Destructor + explicit PhysicsUpdateContext(TempAllocator &inTempAllocator); + ~PhysicsUpdateContext(); + + static constexpr int cMaxConcurrency = 32; ///< Maximum supported amount of concurrent jobs + + using JobHandleArray = StaticArray; + + struct Step; + + struct BodyPairQueue + { + atomic mWriteIdx { 0 }; ///< Next index to write in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue) + uint8 mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Moved to own cache line to avoid conflicts with consumer jobs + + atomic mReadIdx { 0 }; ///< Next index to read in mBodyPair array (need to add thread index * mMaxBodyPairsPerQueue and modulo mMaxBodyPairsPerQueue) + uint8 mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Moved to own cache line to avoid conflicts with producer/consumer jobs + }; + + using BodyPairQueues = StaticArray; + + using JobMask = uint32; ///< A mask that has as many bits as we can have concurrent jobs + static_assert(sizeof(JobMask) * 8 >= cMaxConcurrency); + + /// Structure that contains data needed for each collision step. + struct Step + { + Step() = default; + Step(const Step &) { JPH_ASSERT(false); } // vector needs a copy constructor, but we're never going to call it + + PhysicsUpdateContext *mContext; ///< The physics update context + + bool mIsFirst; ///< If this is the first step + bool mIsLast; ///< If this is the last step + + BroadPhase::UpdateState mBroadPhaseUpdateState; ///< Handle returned by Broadphase::UpdatePrepare + + uint32 mNumActiveBodiesAtStepStart; ///< Number of bodies that were active at the start of the physics update step. Only these bodies will receive gravity (they are the first N in the active body list). + + atomic mDetermineActiveConstraintReadIdx { 0 }; ///< Next constraint for determine active constraints + uint8 mPadding1[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mNumActiveConstraints { 0 }; ///< Number of constraints in the mActiveConstraints array + uint8 mPadding2[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mSetupVelocityConstraintsReadIdx { 0 }; ///< Next constraint for setting up velocity constraints + uint8 mPadding3[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mStepListenerReadIdx { 0 }; ///< Next step listener to call + uint8 mPadding4[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mApplyGravityReadIdx { 0 }; ///< Next body to apply gravity to + uint8 mPadding5[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + atomic mActiveBodyReadIdx { 0 }; ///< Index of fist active body that has not yet been processed by the broadphase + uint8 mPadding6[JPH_CACHE_LINE_SIZE - sizeof(atomic)];///< Padding to avoid sharing cache line with the next atomic + + BodyPairQueues mBodyPairQueues; ///< Queues in which to put body pairs that need to be tested by the narrowphase + + uint32 mMaxBodyPairsPerQueue; ///< Amount of body pairs that we can queue per queue + + atomic mActiveFindCollisionJobs; ///< A bitmask that indicates which jobs are still active + + atomic mNumBodyPairs { 0 }; ///< The number of body pairs found in this step (used to size the contact cache in the next step) + atomic mNumManifolds { 0 }; ///< The number of manifolds found in this step (used to size the contact cache in the next step) + + atomic mSolveVelocityConstraintsNextIsland { 0 }; ///< Next island that needs to be processed for the solve velocity constraints step (doesn't need own cache line since position jobs don't run at same time) + atomic mSolvePositionConstraintsNextIsland { 0 }; ///< Next island that needs to be processed for the solve position constraints step (doesn't need own cache line since velocity jobs don't run at same time) + + /// Contains the information needed to cast a body through the scene to do continuous collision detection + struct CCDBody + { + CCDBody(BodyID inBodyID1, Vec3Arg inDeltaPosition, float inLinearCastThresholdSq, float inMaxPenetration) : mDeltaPosition(inDeltaPosition), mBodyID1(inBodyID1), mLinearCastThresholdSq(inLinearCastThresholdSq), mMaxPenetration(inMaxPenetration) { } + + Vec3 mDeltaPosition; ///< Desired rotation step + Vec3 mContactNormal; ///< World space normal of closest hit (only valid if mFractionPlusSlop < 1) + RVec3 mContactPointOn2; ///< World space contact point on body 2 of closest hit (only valid if mFractionPlusSlop < 1) + BodyID mBodyID1; ///< Body 1 (the body that is performing collision detection) + BodyID mBodyID2; ///< Body 2 (the body of the closest hit, only valid if mFractionPlusSlop < 1) + SubShapeID mSubShapeID2; ///< Sub shape of body 2 that was hit (only valid if mFractionPlusSlop < 1) + float mFraction = 1.0f; ///< Fraction at which the hit occurred + float mFractionPlusSlop = 1.0f; ///< Fraction at which the hit occurred + extra delta to allow body to penetrate by mMaxPenetration + float mLinearCastThresholdSq; ///< Maximum allowed squared movement before doing a linear cast (determined by inner radius of shape) + float mMaxPenetration; ///< Maximum allowed penetration (determined by inner radius of shape) + ContactSettings mContactSettings; ///< The contact settings for this contact + }; + atomic mIntegrateVelocityReadIdx { 0 }; ///< Next active body index to take when integrating velocities + CCDBody * mCCDBodies = nullptr; ///< List of bodies that need to do continuous collision detection + uint32 mCCDBodiesCapacity = 0; ///< Capacity of the mCCDBodies list + atomic mNumCCDBodies = 0; ///< Number of CCD bodies in mCCDBodies + atomic mNextCCDBody { 0 }; ///< Next unprocessed body index in mCCDBodies + int * mActiveBodyToCCDBody = nullptr; ///< A mapping between an index in BodyManager::mActiveBodies and the index in mCCDBodies + uint32 mNumActiveBodyToCCDBody = 0; ///< Number of indices in mActiveBodyToCCDBody + + // Jobs in order of execution (some run in parallel) + JobHandle mBroadPhasePrepare; ///< Prepares the new tree in the background + JobHandleArray mStepListeners; ///< Listeners to notify of the beginning of a physics step + JobHandleArray mDetermineActiveConstraints; ///< Determine which constraints will be active during this step + JobHandleArray mApplyGravity; ///< Update velocities of bodies with gravity + JobHandleArray mFindCollisions; ///< Find all collisions between active bodies an the world + JobHandle mUpdateBroadphaseFinalize; ///< Swap the newly built tree with the current tree + JobHandleArray mSetupVelocityConstraints; ///< Calculate properties for all constraints in the constraint manager + JobHandle mBuildIslandsFromConstraints; ///< Go over all constraints and assign the bodies they're attached to to an island + JobHandle mFinalizeIslands; ///< Finalize calculation simulation islands + JobHandle mBodySetIslandIndex; ///< Set the current island index on each body (not used by the simulation, only for drawing purposes) + JobHandleArray mSolveVelocityConstraints; ///< Solve the constraints in the velocity domain + JobHandle mPreIntegrateVelocity; ///< Setup integration of all body positions + JobHandleArray mIntegrateVelocity; ///< Integrate all body positions + JobHandle mPostIntegrateVelocity; ///< Finalize integration of all body positions + JobHandle mResolveCCDContacts; ///< Updates the positions and velocities for all bodies that need continuous collision detection + JobHandleArray mSolvePositionConstraints; ///< Solve all constraints in the position domain + JobHandle mContactRemovedCallbacks; ///< Calls the contact removed callbacks + JobHandle mSoftBodyPrepare; ///< Prepares updating the soft bodies + JobHandleArray mSoftBodyCollide; ///< Finds all colliding shapes for soft bodies + JobHandleArray mSoftBodySimulate; ///< Simulates all particles + JobHandle mSoftBodyFinalize; ///< Finalizes the soft body update + JobHandle mStartNextStep; ///< Job that kicks the next step (empty for the last step) + }; + + using Steps = Array>; + + /// Maximum amount of concurrent jobs on this machine + int GetMaxConcurrency() const { const int max_concurrency = PhysicsUpdateContext::cMaxConcurrency; return min(max_concurrency, mJobSystem->GetMaxConcurrency()); } ///< Need to put max concurrency in temp var as min requires a reference + + PhysicsSystem * mPhysicsSystem; ///< The physics system we belong to + TempAllocator * mTempAllocator; ///< Temporary allocator used during the update + JobSystem * mJobSystem; ///< Job system that processes jobs + JobSystem::Barrier * mBarrier; ///< Barrier used to wait for all physics jobs to complete + + float mStepDeltaTime; ///< Delta time for a simulation step (collision step) + float mWarmStartImpulseRatio; ///< Ratio of this step delta time vs last step + atomic mErrors { 0 }; ///< Errors that occurred during the update, actual type is EPhysicsUpdateError + + Constraint ** mActiveConstraints = nullptr; ///< Constraints that were active at the start of the physics update step (activating bodies can activate constraints and we need a consistent snapshot). Only these constraints will be resolved. + + BodyPair * mBodyPairs = nullptr; ///< A list of body pairs found by the broadphase + + IslandBuilder * mIslandBuilder; ///< Keeps track of connected bodies and builds islands for multithreaded velocity/position update + + Steps mSteps; + + uint mNumSoftBodies; ///< Number of active soft bodies in the simulation + SoftBodyUpdateContext * mSoftBodyUpdateContexts = nullptr; ///< Contexts for updating soft bodies + atomic mSoftBodyToCollide { 0 }; ///< Next soft body to take when running SoftBodyCollide jobs +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.cpp b/WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.cpp new file mode 100644 index 000000000..9d7e5b675 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.cpp @@ -0,0 +1,705 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::Part) +{ + JPH_ADD_BASE_CLASS(RagdollSettings::Part, BodyCreationSettings) + + JPH_ADD_ATTRIBUTE(RagdollSettings::Part, mToParent) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings::AdditionalConstraint) +{ + JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mBodyIdx) + JPH_ADD_ATTRIBUTE(RagdollSettings::AdditionalConstraint, mConstraint) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(RagdollSettings) +{ + JPH_ADD_ATTRIBUTE(RagdollSettings, mSkeleton) + JPH_ADD_ATTRIBUTE(RagdollSettings, mParts) + JPH_ADD_ATTRIBUTE(RagdollSettings, mAdditionalConstraints) +} + +static inline BodyInterface &sGetBodyInterface(PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? inSystem->GetBodyInterface() : inSystem->GetBodyInterfaceNoLock(); +} + +static inline const BodyLockInterface &sGetBodyLockInterface(const PhysicsSystem *inSystem, bool inLockBodies) +{ + return inLockBodies? static_cast(inSystem->GetBodyLockInterface()) : static_cast(inSystem->GetBodyLockInterfaceNoLock()); +} + +bool RagdollSettings::Stabilize() +{ + // Based on: Stop my Constraints from Blowing Up! - Oliver Strunk (Havok) + // Do 2 things: + // 1. Limit the mass ratios between parents and children (slide 16) + // 2. Increase the inertia of parents so that they're bigger or equal to the sum of their children (slide 34) + + // If we don't have any joints there's nothing to stabilize + if (mSkeleton->GetJointCount() == 0) + return true; + + // The skeleton can contain one or more static bodies. We can't modify the mass for those so we start a new stabilization chain for each joint under a static body until we reach the next static body. + // This array keeps track of which joints have been processed. + Array visited; + visited.resize(mSkeleton->GetJointCount()); + for (size_t v = 0; v < visited.size(); ++v) + { + // Mark static bodies as visited so we won't process these + Part &p = mParts[v]; + bool has_mass_properties = p.HasMassProperties(); + visited[v] = !has_mass_properties; + + if (has_mass_properties && p.mOverrideMassProperties != EOverrideMassProperties::MassAndInertiaProvided) + { + // Mass properties not yet calculated, do it now + p.mMassPropertiesOverride = p.GetMassProperties(); + p.mOverrideMassProperties = EOverrideMassProperties::MassAndInertiaProvided; + } + } + + // Find first unvisited part that either has no parent or that has a parent that was visited + for (int first_idx = 0; first_idx < mSkeleton->GetJointCount(); ++first_idx) + { + int first_idx_parent = mSkeleton->GetJoint(first_idx).mParentJointIndex; + if (!visited[first_idx] && (first_idx_parent == -1 || visited[first_idx_parent])) + { + // Find all children of first_idx and their children up to the next static part + int next_to_process = 0; + Array indices; + indices.reserve(mSkeleton->GetJointCount()); + visited[first_idx] = true; + indices.push_back(first_idx); + do + { + int parent_idx = indices[next_to_process++]; + for (int child_idx = 0; child_idx < mSkeleton->GetJointCount(); ++child_idx) + if (!visited[child_idx] && mSkeleton->GetJoint(child_idx).mParentJointIndex == parent_idx) + { + visited[child_idx] = true; + indices.push_back(child_idx); + } + } while (next_to_process < (int)indices.size()); + + // If there's only 1 body, we can't redistribute mass + if (indices.size() == 1) + continue; + + const float cMinMassRatio = 0.8f; + const float cMaxMassRatio = 1.2f; + + // Ensure that the mass ratio from parent to child is within a range + float total_mass_ratio = 1.0f; + Array mass_ratios; + mass_ratios.resize(mSkeleton->GetJointCount()); + mass_ratios[indices[0]] = 1.0f; + for (int i = 1; i < (int)indices.size(); ++i) + { + int child_idx = indices[i]; + int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex; + float ratio = mParts[child_idx].mMassPropertiesOverride.mMass / mParts[parent_idx].mMassPropertiesOverride.mMass; + mass_ratios[child_idx] = mass_ratios[parent_idx] * Clamp(ratio, cMinMassRatio, cMaxMassRatio); + total_mass_ratio += mass_ratios[child_idx]; + } + + // Calculate total mass of this chain + float total_mass = 0.0f; + for (int idx : indices) + total_mass += mParts[idx].mMassPropertiesOverride.mMass; + + // Calculate how much mass belongs to a ratio of 1 + float ratio_to_mass = total_mass / total_mass_ratio; + + // Adjust all masses and inertia tensors for the new mass + for (int i : indices) + { + Part &p = mParts[i]; + float old_mass = p.mMassPropertiesOverride.mMass; + float new_mass = mass_ratios[i] * ratio_to_mass; + p.mMassPropertiesOverride.mMass = new_mass; + p.mMassPropertiesOverride.mInertia *= new_mass / old_mass; + p.mMassPropertiesOverride.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1)); + } + + const float cMaxInertiaIncrease = 2.0f; + + // Get the principal moments of inertia for all parts + struct Principal + { + Mat44 mRotation; + Vec3 mDiagonal; + float mChildSum = 0.0f; + }; + Array principals; + principals.resize(mParts.size()); + for (int i : indices) + if (!mParts[i].mMassPropertiesOverride.DecomposePrincipalMomentsOfInertia(principals[i].mRotation, principals[i].mDiagonal)) + { + JPH_ASSERT(false, "Failed to decompose the inertia tensor!"); + return false; + } + + // Calculate sum of child inertias + // Walk backwards so we sum the leaves first + for (int i = (int)indices.size() - 1; i > 0; --i) + { + int child_idx = indices[i]; + int parent_idx = mSkeleton->GetJoint(child_idx).mParentJointIndex; + principals[parent_idx].mChildSum += principals[child_idx].mDiagonal[0] + principals[child_idx].mChildSum; + } + + // Adjust inertia tensors for all parts + for (int i : indices) + { + Part &p = mParts[i]; + Principal &principal = principals[i]; + if (principal.mChildSum != 0.0f) + { + // Calculate minimum inertia this object should have based on it children + float minimum = min(cMaxInertiaIncrease * principal.mDiagonal[0], principal.mChildSum); + principal.mDiagonal = Vec3::sMax(principal.mDiagonal, Vec3::sReplicate(minimum)); + + // Recalculate moment of inertia in body space + p.mMassPropertiesOverride.mInertia = principal.mRotation * Mat44::sScale(principal.mDiagonal) * principal.mRotation.Inversed3x3(); + } + } + } + } + + return true; +} + +void RagdollSettings::DisableParentChildCollisions(const Mat44 *inJointMatrices, float inMinSeparationDistance) +{ + int joint_count = mSkeleton->GetJointCount(); + JPH_ASSERT(joint_count == (int)mParts.size()); + + // Create a group filter table that disables collisions between parent and child + Ref group_filter = new GroupFilterTable(joint_count); + for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx) + { + int parent_joint = mSkeleton->GetJoint(joint_idx).mParentJointIndex; + if (parent_joint >= 0) + group_filter->DisableCollision(joint_idx, parent_joint); + } + + // If joint matrices are provided + if (inJointMatrices != nullptr) + { + // Loop over all joints + for (int j1 = 0; j1 < joint_count; ++j1) + { + // Shape and transform for joint 1 + const Part &part1 = mParts[j1]; + const Shape *shape1 = part1.GetShape(); + Vec3 scale1; + Mat44 com1 = (inJointMatrices[j1].PreTranslated(shape1->GetCenterOfMass())).Decompose(scale1); + + // Loop over all other joints + for (int j2 = j1 + 1; j2 < joint_count; ++j2) + if (group_filter->IsCollisionEnabled(j1, j2)) // Only if collision is still enabled we need to test + { + // Shape and transform for joint 2 + const Part &part2 = mParts[j2]; + const Shape *shape2 = part2.GetShape(); + Vec3 scale2; + Mat44 com2 = (inJointMatrices[j2].PreTranslated(shape2->GetCenterOfMass())).Decompose(scale2); + + // Collision settings + CollideShapeSettings settings; + settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll; + settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces; + settings.mMaxSeparationDistance = inMinSeparationDistance; + + // Only check if one of the two bodies can become dynamic + if (part1.HasMassProperties() || part2.HasMassProperties()) + { + // If there is a collision, disable the collision between the joints + AnyHitCollisionCollector collector; + if (part1.HasMassProperties()) // Ensure that the first shape is always a dynamic one (we can't check mesh vs convex but we can check convex vs mesh) + CollisionDispatch::sCollideShapeVsShape(shape1, shape2, scale1, scale2, com1, com2, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector); + else + CollisionDispatch::sCollideShapeVsShape(shape2, shape1, scale2, scale1, com2, com1, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector); + if (collector.HadHit()) + group_filter->DisableCollision(j1, j2); + } + } + } + } + + // Loop over the body parts and assign them a sub group ID and the group filter + for (int joint_idx = 0; joint_idx < joint_count; ++joint_idx) + { + Part &part = mParts[joint_idx]; + part.mCollisionGroup.SetSubGroupID(joint_idx); + part.mCollisionGroup.SetGroupFilter(group_filter); + } +} + +void RagdollSettings::SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const +{ + BodyCreationSettings::ShapeToIDMap shape_to_id; + BodyCreationSettings::MaterialToIDMap material_to_id; + BodyCreationSettings::GroupFilterToIDMap group_filter_to_id; + + // Save skeleton + mSkeleton->SaveBinaryState(inStream); + + // Save parts + inStream.Write((uint32)mParts.size()); + for (const Part &p : mParts) + { + // Write body creation settings + p.SaveWithChildren(inStream, inSaveShapes? &shape_to_id : nullptr, inSaveShapes? &material_to_id : nullptr, inSaveGroupFilter? &group_filter_to_id : nullptr); + + // Save constraint + inStream.Write(p.mToParent != nullptr); + if (p.mToParent != nullptr) + p.mToParent->SaveBinaryState(inStream); + } + + // Save additional constraints + inStream.Write((uint32)mAdditionalConstraints.size()); + for (const AdditionalConstraint &c : mAdditionalConstraints) + { + // Save bodies indices + inStream.Write(c.mBodyIdx); + + // Save constraint + c.mConstraint->SaveBinaryState(inStream); + } +} + +RagdollSettings::RagdollResult RagdollSettings::sRestoreFromBinaryState(StreamIn &inStream) +{ + RagdollResult result; + + // Restore skeleton + Skeleton::SkeletonResult skeleton_result = Skeleton::sRestoreFromBinaryState(inStream); + if (skeleton_result.HasError()) + { + result.SetError(skeleton_result.GetError()); + return result; + } + + // Create ragdoll + Ref ragdoll = new RagdollSettings(); + ragdoll->mSkeleton = skeleton_result.Get(); + + BodyCreationSettings::IDToShapeMap id_to_shape; + BodyCreationSettings::IDToMaterialMap id_to_material; + BodyCreationSettings::IDToGroupFilterMap id_to_group_filter; + + // Reserve some memory to avoid frequent reallocations + id_to_shape.reserve(1024); + id_to_material.reserve(128); + id_to_group_filter.reserve(128); + + // Read parts + uint32 len = 0; + inStream.Read(len); + ragdoll->mParts.resize(len); + for (Part &p : ragdoll->mParts) + { + // Read creation settings + BodyCreationSettings::BCSResult bcs_result = BodyCreationSettings::sRestoreWithChildren(inStream, id_to_shape, id_to_material, id_to_group_filter); + if (bcs_result.HasError()) + { + result.SetError(bcs_result.GetError()); + return result; + } + static_cast(p) = bcs_result.Get(); + + // Read constraint + bool has_constraint = false; + inStream.Read(has_constraint); + if (has_constraint) + { + ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (constraint_result.HasError()) + { + result.SetError(constraint_result.GetError()); + return result; + } + p.mToParent = DynamicCast(constraint_result.Get()); + } + } + + // Read additional constraints + len = 0; + inStream.Read(len); + ragdoll->mAdditionalConstraints.resize(len); + for (AdditionalConstraint &c : ragdoll->mAdditionalConstraints) + { + // Read body indices + inStream.Read(c.mBodyIdx); + + // Read constraint + ConstraintSettings::ConstraintResult constraint_result = ConstraintSettings::sRestoreFromBinaryState(inStream); + if (constraint_result.HasError()) + { + result.SetError(constraint_result.GetError()); + return result; + } + c.mConstraint = DynamicCast(constraint_result.Get()); + } + + // Create mapping tables + ragdoll->CalculateBodyIndexToConstraintIndex(); + ragdoll->CalculateConstraintIndexToBodyIdxPair(); + + result.Set(ragdoll); + return result; +} + +Ragdoll *RagdollSettings::CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const +{ + Ragdoll *r = new Ragdoll(inSystem); + r->mRagdollSettings = this; + r->mBodyIDs.reserve(mParts.size()); + r->mConstraints.reserve(mParts.size() + mAdditionalConstraints.size()); + + // Create bodies and constraints + BodyInterface &bi = inSystem->GetBodyInterface(); + Body **bodies = (Body **)JPH_STACK_ALLOC(mParts.size() * sizeof(Body *)); + int joint_idx = 0; + for (const Part &p : mParts) + { + Body *body2 = bi.CreateBody(p); + if (body2 == nullptr) + { + // Out of bodies, failed to create ragdoll + delete r; + return nullptr; + } + body2->GetCollisionGroup().SetGroupID(inCollisionGroup); + body2->SetUserData(inUserData); + + // Temporarily store body pointer for hooking up constraints + bodies[joint_idx] = body2; + + // Create constraint + if (p.mToParent != nullptr) + { + Body *body1 = bodies[mSkeleton->GetJoint(joint_idx).mParentJointIndex]; + r->mConstraints.push_back(p.mToParent->Create(*body1, *body2)); + } + + // Store body ID and constraint in parallel arrays + r->mBodyIDs.push_back(body2->GetID()); + + ++joint_idx; + } + + // Add additional constraints + for (const AdditionalConstraint &c : mAdditionalConstraints) + { + Body *body1 = bodies[c.mBodyIdx[0]]; + Body *body2 = bodies[c.mBodyIdx[1]]; + r->mConstraints.push_back(c.mConstraint->Create(*body1, *body2)); + } + + return r; +} + +void RagdollSettings::CalculateBodyIndexToConstraintIndex() +{ + mBodyIndexToConstraintIndex.clear(); + mBodyIndexToConstraintIndex.reserve(mParts.size()); + + int constraint_index = 0; + for (const Part &p : mParts) + { + if (p.mToParent != nullptr) + mBodyIndexToConstraintIndex.push_back(constraint_index++); + else + mBodyIndexToConstraintIndex.push_back(-1); + } +} + +void RagdollSettings::CalculateConstraintIndexToBodyIdxPair() +{ + mConstraintIndexToBodyIdxPair.clear(); + mConstraintIndexToBodyIdxPair.reserve(mParts.size() + mAdditionalConstraints.size()); + + // Add constraints between parts + int joint_idx = 0; + for (const Part &p : mParts) + { + if (p.mToParent != nullptr) + { + int parent_joint_idx = mSkeleton->GetJoint(joint_idx).mParentJointIndex; + mConstraintIndexToBodyIdxPair.emplace_back(parent_joint_idx, joint_idx); + } + + ++joint_idx; + } + + // Add additional constraints + for (const AdditionalConstraint &c : mAdditionalConstraints) + mConstraintIndexToBodyIdxPair.emplace_back(c.mBodyIdx[0], c.mBodyIdx[1]); +} + +Ragdoll::~Ragdoll() +{ + // Destroy all bodies + mSystem->GetBodyInterface().DestroyBodies(mBodyIDs.data(), (int)mBodyIDs.size()); +} + +void Ragdoll::AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies) +{ + // Scope for JPH_STACK_ALLOC + { + // Create copy of body ids since they will be shuffled + int num_bodies = (int)mBodyIDs.size(); + BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID)); + memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID)); + + // Insert bodies as a batch + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + BodyInterface::AddState add_state = bi.AddBodiesPrepare(bodies, num_bodies); + bi.AddBodiesFinalize(bodies, num_bodies, add_state, inActivationMode); + } + + // Add all constraints + mSystem->AddConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size()); +} + +void Ragdoll::RemoveFromPhysicsSystem(bool inLockBodies) +{ + // Remove all constraints before removing the bodies + mSystem->RemoveConstraints((Constraint **)mConstraints.data(), (int)mConstraints.size()); + + // Scope for JPH_STACK_ALLOC + { + // Create copy of body ids since they will be shuffled + int num_bodies = (int)mBodyIDs.size(); + BodyID *bodies = (BodyID *)JPH_STACK_ALLOC(num_bodies * sizeof(BodyID)); + memcpy(bodies, mBodyIDs.data(), num_bodies * sizeof(BodyID)); + + // Remove all bodies as a batch + sGetBodyInterface(mSystem, inLockBodies).RemoveBodies(bodies, num_bodies); + } +} + +void Ragdoll::Activate(bool inLockBodies) +{ + sGetBodyInterface(mSystem, inLockBodies).ActivateBodies(mBodyIDs.data(), (int)mBodyIDs.size()); +} + +bool Ragdoll::IsActive(bool inLockBodies) const +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Test if any body is active + for (int b = 0; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + if (body->IsActive()) + return true; + } + + return false; +} + +void Ragdoll::SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies) +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiWrite lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Update group ID + for (int b = 0; b < body_count; ++b) + { + Body *body = lock.GetBody(b); + body->GetCollisionGroup().SetGroupID(inGroupID); + } +} + +void Ragdoll::SetPose(const SkeletonPose &inPose, bool inLockBodies) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + SetPose(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inLockBodies); +} + +void Ragdoll::SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies) +{ + // Move bodies instantly into the correct position + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (int i = 0; i < (int)mBodyIDs.size(); ++i) + { + const Mat44 &joint = inJointMatrices[i]; + bi.SetPositionAndRotation(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), EActivation::DontActivate); + } +} + +void Ragdoll::GetPose(SkeletonPose &outPose, bool inLockBodies) +{ + JPH_ASSERT(outPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + RVec3 root_offset; + GetPose(root_offset, outPose.GetJointMatrices().data(), inLockBodies); + outPose.SetRootOffset(root_offset); +} + +void Ragdoll::GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies) +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + if (body_count == 0) + return; + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Get root matrix + const Body *root = lock.GetBody(0); + RMat44 root_transform = root->GetWorldTransform(); + outRootOffset = root_transform.GetTranslation(); + outJointMatrices[0] = Mat44(root_transform.GetColumn4(0), root_transform.GetColumn4(1), root_transform.GetColumn4(2), Vec4(0, 0, 0, 1)); + + // Get other matrices + for (int b = 1; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + RMat44 transform = body->GetWorldTransform(); + outJointMatrices[b] = Mat44(transform.GetColumn4(0), transform.GetColumn4(1), transform.GetColumn4(2), Vec4(Vec3(transform.GetTranslation() - outRootOffset), 1)); + } +} + +void Ragdoll::ResetWarmStart() +{ + for (TwoBodyConstraint *c : mConstraints) + c->ResetWarmStart(); +} + +void Ragdoll::DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + DriveToPoseUsingKinematics(inPose.GetRootOffset(), inPose.GetJointMatrices().data(), inDeltaTime, inLockBodies); +} + +void Ragdoll::DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies) +{ + // Move bodies into the correct position using kinematics + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (int i = 0; i < (int)mBodyIDs.size(); ++i) + { + const Mat44 &joint = inJointMatrices[i]; + bi.MoveKinematic(mBodyIDs[i], inRootOffset + joint.GetTranslation(), joint.GetQuaternion(), inDeltaTime); + } +} + +void Ragdoll::DriveToPoseUsingMotors(const SkeletonPose &inPose) +{ + JPH_ASSERT(inPose.GetSkeleton() == mRagdollSettings->mSkeleton); + + // Move bodies into the correct position using constraints + for (int i = 0; i < (int)inPose.GetJointMatrices().size(); ++i) + { + int constraint_idx = mRagdollSettings->GetConstraintIndexForBodyIndex(i); + if (constraint_idx >= 0) + { + // Get desired rotation of this body relative to its parent + const SkeletalAnimation::JointState &joint_state = inPose.GetJoint(i); + + // Drive constraint to target + TwoBodyConstraint *constraint = mConstraints[constraint_idx]; + EConstraintSubType sub_type = constraint->GetSubType(); + if (sub_type == EConstraintSubType::SwingTwist) + { + SwingTwistConstraint *st_constraint = static_cast(constraint); + st_constraint->SetSwingMotorState(EMotorState::Position); + st_constraint->SetTwistMotorState(EMotorState::Position); + st_constraint->SetTargetOrientationBS(joint_state.mRotation); + } + else + JPH_ASSERT(false, "Constraint type not implemented!"); + } + } +} + +void Ragdoll::SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.SetLinearAndAngularVelocity(body_id, inLinearVelocity, inAngularVelocity); +} + +void Ragdoll::SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.SetLinearVelocity(body_id, inLinearVelocity); +} + +void Ragdoll::AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.AddLinearVelocity(body_id, inLinearVelocity); +} + +void Ragdoll::AddImpulse(Vec3Arg inImpulse, bool inLockBodies) +{ + BodyInterface &bi = sGetBodyInterface(mSystem, inLockBodies); + for (BodyID body_id : mBodyIDs) + bi.AddImpulse(body_id, inImpulse); +} + +void Ragdoll::GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies) const +{ + BodyLockRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs[0]); + if (lock.Succeeded()) + { + const Body &body = lock.GetBody(); + outPosition = body.GetPosition(); + outRotation = body.GetRotation(); + } + else + { + outPosition = RVec3::sZero(); + outRotation = Quat::sIdentity(); + } +} + +AABox Ragdoll::GetWorldSpaceBounds(bool inLockBodies) const +{ + // Lock the bodies + int body_count = (int)mBodyIDs.size(); + BodyLockMultiRead lock(sGetBodyLockInterface(mSystem, inLockBodies), mBodyIDs.data(), body_count); + + // Encapsulate all bodies + AABox bounds; + for (int b = 0; b < body_count; ++b) + { + const Body *body = lock.GetBody(b); + if (body != nullptr) + bounds.Encapsulate(body->GetWorldSpaceBounds()); + } + return bounds; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.h b/WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.h new file mode 100644 index 000000000..c31586f25 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Ragdoll/Ragdoll.h @@ -0,0 +1,240 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class Ragdoll; +class PhysicsSystem; + +/// Contains the structure of a ragdoll +class JPH_EXPORT RagdollSettings : public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, RagdollSettings) + + /// Stabilize the constraints of the ragdoll + /// @return True on success, false on failure. + bool Stabilize(); + + /// After the ragdoll has been fully configured, call this function to automatically create and add a GroupFilterTable collision filter to all bodies + /// and configure them so that parent and children don't collide. + /// + /// This will: + /// - Create a GroupFilterTable and assign it to all of the bodies in a ragdoll. + /// - Each body in your ragdoll will get a SubGroupID that is equal to the joint index in the Skeleton that it is attached to. + /// - Loop over all joints in the Skeleton and call GroupFilterTable::DisableCollision(joint index, parent joint index). + /// - When a pose is provided through inJointMatrices the function will detect collisions between joints + /// (they must be separated by more than inMinSeparationDistance to be treated as not colliding) and automatically disable collisions. + /// + /// When you create an instance using Ragdoll::CreateRagdoll pass in a unique GroupID for each ragdoll (e.g. a simple counter), note that this number + /// should be unique throughout the PhysicsSystem, so if you have different types of ragdolls they should not share the same GroupID. + void DisableParentChildCollisions(const Mat44 *inJointMatrices = nullptr, float inMinSeparationDistance = 0.0f); + + /// Saves the state of this object in binary form to inStream. + /// @param inStream The stream to save the state to + /// @param inSaveShapes If the shapes should be saved as well (these could be shared between ragdolls, in which case the calling application may want to write custom code to restore them) + /// @param inSaveGroupFilter If the group filter should be saved as well (these could be shared) + void SaveBinaryState(StreamOut &inStream, bool inSaveShapes, bool inSaveGroupFilter) const; + + using RagdollResult = Result>; + + /// Restore a saved ragdoll from inStream + static RagdollResult sRestoreFromBinaryState(StreamIn &inStream); + + /// Create ragdoll instance from these settings + /// @return Newly created ragdoll or null when out of bodies + Ragdoll * CreateRagdoll(CollisionGroup::GroupID inCollisionGroup, uint64 inUserData, PhysicsSystem *inSystem) const; + + /// Access to the skeleton of this ragdoll + const Skeleton * GetSkeleton() const { return mSkeleton; } + Skeleton * GetSkeleton() { return mSkeleton; } + + /// Calculate the map needed for GetBodyIndexToConstraintIndex() + void CalculateBodyIndexToConstraintIndex(); + + /// Get table that maps a body index to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body. + /// Note that this will only tell you which constraint connects the body to its parent, it will not look in the additional constraint list. + const Array & GetBodyIndexToConstraintIndex() const { return mBodyIndexToConstraintIndex; } + + /// Map a single body index to a constraint index + int GetConstraintIndexForBodyIndex(int inBodyIndex) const { return mBodyIndexToConstraintIndex[inBodyIndex]; } + + /// Calculate the map needed for GetConstraintIndexToBodyIdxPair() + void CalculateConstraintIndexToBodyIdxPair(); + + using BodyIdxPair = pair; + + /// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + const Array & GetConstraintIndexToBodyIdxPair() const { return mConstraintIndexToBodyIdxPair; } + + /// Map a single constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + BodyIdxPair GetBodyIndicesForConstraintIndex(int inConstraintIndex) const { return mConstraintIndexToBodyIdxPair[inConstraintIndex]; } + + /// A single rigid body sub part of the ragdoll + class Part : public BodyCreationSettings + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Part) + + Ref mToParent; + }; + + /// List of ragdoll parts + using PartVector = Array; ///< The constraint that connects this part to its parent part (should be null for the root) + + /// A constraint that connects two bodies in a ragdoll (for non parent child related constraints) + class AdditionalConstraint + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AdditionalConstraint) + + /// Constructors + AdditionalConstraint() = default; + AdditionalConstraint(int inBodyIdx1, int inBodyIdx2, TwoBodyConstraintSettings *inConstraint) : mBodyIdx { inBodyIdx1, inBodyIdx2 }, mConstraint(inConstraint) { } + + int mBodyIdx[2]; ///< Indices of the bodies that this constraint connects + Ref mConstraint; ///< The constraint that connects these bodies + }; + + /// List of additional constraints + using AdditionalConstraintVector = Array; + + /// The skeleton for this ragdoll + Ref mSkeleton; + + /// For each of the joints, the body and constraint attaching it to its parent body (1-on-1 with mSkeleton.GetJoints()) + PartVector mParts; + + /// A list of constraints that connects two bodies in a ragdoll (for non parent child related constraints) + AdditionalConstraintVector mAdditionalConstraints; + +private: + /// Table that maps a body index (index in mBodyIDs) to the constraint index with which it is connected to its parent. -1 if there is no constraint associated with the body. + Array mBodyIndexToConstraintIndex; + + /// Table that maps a constraint index (index in mConstraints) to the indices of the bodies that the constraint is connected to (index in mBodyIDs) + Array mConstraintIndexToBodyIdxPair; +}; + +/// Runtime ragdoll information +class JPH_EXPORT Ragdoll : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit Ragdoll(PhysicsSystem *inSystem) : mSystem(inSystem) { } + + /// Destructor + ~Ragdoll(); + + /// Add bodies and constraints to the system and optionally activate the bodies + void AddToPhysicsSystem(EActivation inActivationMode, bool inLockBodies = true); + + /// Remove bodies and constraints from the system + void RemoveFromPhysicsSystem(bool inLockBodies = true); + + /// Wake up all bodies in the ragdoll + void Activate(bool inLockBodies = true); + + /// Check if one or more of the bodies in the ragdoll are active. + /// Note that this involves locking the bodies (if inLockBodies is true) and looping over them. An alternative and possibly faster + /// way could be to install a BodyActivationListener and count the number of active bodies of a ragdoll as they're activated / deactivated + /// (basically check if the body that activates / deactivates is in GetBodyIDs() and increment / decrement a counter). + bool IsActive(bool inLockBodies = true) const; + + /// Set the group ID on all bodies in the ragdoll + void SetGroupID(CollisionGroup::GroupID inGroupID, bool inLockBodies = true); + + /// Set the ragdoll to a pose (calls BodyInterface::SetPositionAndRotation to instantly move the ragdoll) + void SetPose(const SkeletonPose &inPose, bool inLockBodies = true); + + /// Lower level version of SetPose that directly takes the world space joint matrices + void SetPose(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, bool inLockBodies = true); + + /// Get the ragdoll pose (uses the world transform of the bodies to calculate the pose) + void GetPose(SkeletonPose &outPose, bool inLockBodies = true); + + /// Lower level version of GetPose that directly returns the world space joint matrices + void GetPose(RVec3 &outRootOffset, Mat44 *outJointMatrices, bool inLockBodies = true); + + /// This function calls ResetWarmStart on all constraints. It can be used after calling SetPose to reset previous frames impulses. See: Constraint::ResetWarmStart. + void ResetWarmStart(); + + /// Drive the ragdoll to a specific pose by setting velocities on each of the bodies so that it will reach inPose in inDeltaTime + void DriveToPoseUsingKinematics(const SkeletonPose &inPose, float inDeltaTime, bool inLockBodies = true); + + /// Lower level version of DriveToPoseUsingKinematics that directly takes the world space joint matrices + void DriveToPoseUsingKinematics(RVec3Arg inRootOffset, const Mat44 *inJointMatrices, float inDeltaTime, bool inLockBodies = true); + + /// Drive the ragdoll to a specific pose by activating the motors on each constraint + void DriveToPoseUsingMotors(const SkeletonPose &inPose); + + /// Control the linear and velocity of all bodies in the ragdoll + void SetLinearAndAngularVelocity(Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, bool inLockBodies = true); + + /// Set the world space linear velocity of all bodies in the ragdoll. + void SetLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add a world space velocity (in m/s) to all bodies in the ragdoll. + void AddLinearVelocity(Vec3Arg inLinearVelocity, bool inLockBodies = true); + + /// Add impulse to all bodies of the ragdoll (center of mass of each of them) + void AddImpulse(Vec3Arg inImpulse, bool inLockBodies = true); + + /// Get the position and orientation of the root of the ragdoll + void GetRootTransform(RVec3 &outPosition, Quat &outRotation, bool inLockBodies = true) const; + + /// Get number of bodies in the ragdoll + size_t GetBodyCount() const { return mBodyIDs.size(); } + + /// Access a body ID + BodyID GetBodyID(int inBodyIndex) const { return mBodyIDs[inBodyIndex]; } + + /// Access to the array of body IDs + const Array & GetBodyIDs() const { return mBodyIDs; } + + /// Get number of constraints in the ragdoll + size_t GetConstraintCount() const { return mConstraints.size(); } + + /// Access a constraint by index + TwoBodyConstraint * GetConstraint(int inConstraintIndex) { return mConstraints[inConstraintIndex]; } + + /// Access a constraint by index + const TwoBodyConstraint * GetConstraint(int inConstraintIndex) const { return mConstraints[inConstraintIndex]; } + + /// Get world space bounding box for all bodies of the ragdoll + AABox GetWorldSpaceBounds(bool inLockBodies = true) const; + + /// Get the settings object that created this ragdoll + const RagdollSettings * GetRagdollSettings() const { return mRagdollSettings; } + +private: + /// For RagdollSettings::CreateRagdoll function + friend class RagdollSettings; + + /// The settings that created this ragdoll + RefConst mRagdollSettings; + + /// The bodies and constraints that this ragdoll consists of (1-on-1 with mRagdollSettings->mParts) + Array mBodyIDs; + + /// Array of constraints that connect the bodies together + Array> mConstraints; + + /// Cached physics system + PhysicsSystem * mSystem; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyContactListener.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyContactListener.h new file mode 100644 index 000000000..27e185375 --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyContactListener.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +class Body; +class SoftBodyManifold; + +/// Return value for the OnSoftBodyContactValidate callback. Determines if the contact will be processed or not. +enum class SoftBodyValidateResult +{ + AcceptContact, ///< Accept this contact + RejectContact, ///< Reject this contact +}; + +/// Contact settings for a soft body contact. +/// The values are filled in with their defaults by the system so the callback doesn't need to modify anything, but it can if it wants to. +class SoftBodyContactSettings +{ +public: + float mInvMassScale1 = 1.0f; ///< Scale factor for the inverse mass of the soft body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvMassScale2 = 1.0f; ///< Scale factor for the inverse mass of the other body (0 = infinite mass, 1 = use original mass, 2 = body has half the mass). For the same contact pair, you should strive to keep the value the same over time. + float mInvInertiaScale2 = 1.0f; ///< Scale factor for the inverse inertia of the other body (usually same as mInvMassScale2) + bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) +}; + +/// A listener class that receives collision contact events for soft bodies against rigid bodies. +/// It can be registered with the PhysicsSystem. +class SoftBodyContactListener +{ +public: + /// Ensure virtual destructor + virtual ~SoftBodyContactListener() = default; + + /// Called whenever the soft body's aabox overlaps with another body's aabox (so receiving this callback doesn't tell if any of the vertices will collide). + /// This callback can be used to change the behavior of the collision response for all vertices in the soft body or to completely reject the contact. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// @param inSoftBody The soft body that collided. It is safe to access this as the soft body is only updated on the current thread. + /// @param inOtherBody The other body that collided. Note that accessing the position/orientation/velocity of inOtherBody may result in a race condition as other threads may be modifying the body at the same time. + /// @param ioSettings The settings for all contact points that are generated by this collision. + /// @return Whether the contact should be processed or not. + virtual SoftBodyValidateResult OnSoftBodyContactValidate([[maybe_unused]] const Body &inSoftBody, [[maybe_unused]] const Body &inOtherBody, [[maybe_unused]] SoftBodyContactSettings &ioSettings) { return SoftBodyValidateResult::AcceptContact; } + + /// Called after all contact points for a soft body have been handled. You only receive one callback per body pair per simulation step and can use inManifold to iterate through all contacts. + /// Note that this callback is called when all bodies are locked, so don't use any locking functions! + /// You will receive a single callback for a soft body per simulation step for performance reasons, this callback will apply to all vertices in the soft body. + /// @param inSoftBody The soft body that collided. It is safe to access this as the soft body is only updated on the current thread. + /// @param inManifold The manifold that describes the contact surface between the two bodies. Other bodies may be modified by other threads during this callback. + virtual void OnSoftBodyContactAdded([[maybe_unused]] const Body &inSoftBody, const SoftBodyManifold &inManifold) { /* Do nothing */ } +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp new file mode 100644 index 000000000..3ae026df6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.cpp @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodyCreationSettings) +{ + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mSettings) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPosition) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRotation) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUserData) + JPH_ADD_ENUM_ATTRIBUTE(SoftBodyCreationSettings, mObjectLayer) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mCollisionGroup) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mNumIterations) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mLinearDamping) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMaxLinearVelocity) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mRestitution) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mFriction) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mPressure) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mGravityFactor) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mUpdatePosition) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mMakeRotationIdentity) + JPH_ADD_ATTRIBUTE(SoftBodyCreationSettings, mAllowSleeping) +} + +void SoftBodyCreationSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mPosition); + inStream.Write(mRotation); + inStream.Write(mUserData); + inStream.Write(mObjectLayer); + mCollisionGroup.SaveBinaryState(inStream); + inStream.Write(mNumIterations); + inStream.Write(mLinearDamping); + inStream.Write(mMaxLinearVelocity); + inStream.Write(mRestitution); + inStream.Write(mFriction); + inStream.Write(mPressure); + inStream.Write(mGravityFactor); + inStream.Write(mUpdatePosition); + inStream.Write(mMakeRotationIdentity); + inStream.Write(mAllowSleeping); +} + +void SoftBodyCreationSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mPosition); + inStream.Read(mRotation); + inStream.Read(mUserData); + inStream.Read(mObjectLayer); + mCollisionGroup.RestoreBinaryState(inStream); + inStream.Read(mNumIterations); + inStream.Read(mLinearDamping); + inStream.Read(mMaxLinearVelocity); + inStream.Read(mRestitution); + inStream.Read(mFriction); + inStream.Read(mPressure); + inStream.Read(mGravityFactor); + inStream.Read(mUpdatePosition); + inStream.Read(mMakeRotationIdentity); + inStream.Read(mAllowSleeping); +} + +void SoftBodyCreationSettings::SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const +{ + // Save creation settings + SaveBinaryState(inStream); + + // Save shared settings + if (ioSharedSettingsMap != nullptr && ioMaterialMap != nullptr) + mSettings->SaveWithMaterials(inStream, *ioSharedSettingsMap, *ioMaterialMap); + else + inStream.Write(~uint32(0)); + + // Save group filter + StreamUtils::SaveObjectReference(inStream, mCollisionGroup.GetGroupFilter(), ioGroupFilterMap); +} + +SoftBodyCreationSettings::SBCSResult SoftBodyCreationSettings::sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap) +{ + SBCSResult result; + + // Read creation settings + SoftBodyCreationSettings settings; + settings.RestoreBinaryState(inStream); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Error reading body creation settings"); + return result; + } + + // Read shared settings + SoftBodySharedSettings::SettingsResult settings_result = SoftBodySharedSettings::sRestoreWithMaterials(inStream, ioSharedSettingsMap, ioMaterialMap); + if (settings_result.HasError()) + { + result.SetError(settings_result.GetError()); + return result; + } + settings.mSettings = settings_result.Get(); + + // Read group filter + Result gfresult = StreamUtils::RestoreObjectReference(inStream, ioGroupFilterMap); + if (gfresult.HasError()) + { + result.SetError(gfresult.GetError()); + return result; + } + settings.mCollisionGroup.SetGroupFilter(gfresult.Get()); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h new file mode 100644 index 000000000..9d153290e --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyCreationSettings.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class contains the information needed to create a soft body object +/// Note: Soft bodies are still in development and come with several caveats. Read the Architecture and API documentation for more information! +class JPH_EXPORT SoftBodyCreationSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodyCreationSettings) + + /// Constructor + SoftBodyCreationSettings() = default; + SoftBodyCreationSettings(const SoftBodySharedSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, ObjectLayer inObjectLayer) : mSettings(inSettings), mPosition(inPosition), mRotation(inRotation), mObjectLayer(inObjectLayer) { } + + /// Saves the state of this object in binary form to inStream. Doesn't store the shared settings nor the group filter. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the shared settings nor the group filter. + void RestoreBinaryState(StreamIn &inStream); + + using GroupFilterToIDMap = StreamUtils::ObjectToIDMap; + using IDToGroupFilterMap = StreamUtils::IDToObjectMap; + using SharedSettingsToIDMap = SoftBodySharedSettings::SharedSettingsToIDMap; + using IDToSharedSettingsMap = SoftBodySharedSettings::IDToSharedSettingsMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this body creation settings, its shared settings and group filter. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while saving multiple shapes to the same stream in order to avoid writing duplicates. + /// Pass nullptr to ioSharedSettingsMap and ioMaterial map to skip saving shared settings and materials + /// Pass nullptr to ioGroupFilterMap to skip saving group filters + void SaveWithChildren(StreamOut &inStream, SharedSettingsToIDMap *ioSharedSettingsMap, MaterialToIDMap *ioMaterialMap, GroupFilterToIDMap *ioGroupFilterMap) const; + + using SBCSResult = Result; + + /// Restore a shape, all its children and materials. Pass in an empty map in ioSharedSettingsMap / ioMaterialMap / ioGroupFilterMap or reuse the same map while reading multiple shapes from the same stream in order to restore duplicates. + static SBCSResult sRestoreWithChildren(StreamIn &inStream, IDToSharedSettingsMap &ioSharedSettingsMap, IDToMaterialMap &ioMaterialMap, IDToGroupFilterMap &ioGroupFilterMap); + + RefConst mSettings; ///< Defines the configuration of this soft body + + RVec3 mPosition { RVec3::sZero() }; ///< Initial position of the soft body + Quat mRotation { Quat::sIdentity() }; ///< Initial rotation of the soft body + + /// User data value (can be used by application) + uint64 mUserData = 0; + + ///@name Collision settings + ObjectLayer mObjectLayer = 0; ///< The collision layer this body belongs to (determines if two objects can collide) + CollisionGroup mCollisionGroup; ///< The collision group this body belongs to (determines if two objects can collide) + + uint32 mNumIterations = 5; ///< Number of solver iterations + float mLinearDamping = 0.1f; ///< Linear damping: dv/dt = -mLinearDamping * v + float mMaxLinearVelocity = 500.0f; ///< Maximum linear velocity that a vertex can reach (m/s) + float mRestitution = 0.0f; ///< Restitution when colliding + float mFriction = 0.2f; ///< Friction coefficient when colliding + float mPressure = 0.0f; ///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure + float mGravityFactor = 1.0f; ///< Value to multiply gravity with for this body + bool mUpdatePosition = true; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) + bool mMakeRotationIdentity = true; ///< Bake specified mRotation in the vertices and set the body rotation to identity (simulation is slightly more accurate if the rotation of a soft body is kept to identity) + bool mAllowSleeping = true; ///< If this body can go to sleep or not +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyManifold.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyManifold.h new file mode 100644 index 000000000..ea3a65b3b --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyManifold.h @@ -0,0 +1,59 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// An interface to query which vertices of a soft body are colliding with other bodies +class SoftBodyManifold +{ +public: + /// Get the vertices of the soft body for iterating + const Array & GetVertices() const { return mVertices; } + + /// Check if a vertex has collided with something in this update + JPH_INLINE bool HasContact(const SoftBodyVertex &inVertex) const + { + return inVertex.mHasContact; + } + + /// Get the local space contact point (multiply by GetCenterOfMassTransform() of the soft body to get world space) + JPH_INLINE Vec3 GetLocalContactPoint(const SoftBodyVertex &inVertex) const + { + return inVertex.mPosition - inVertex.mCollisionPlane.SignedDistance(inVertex.mPosition) * inVertex.mCollisionPlane.GetNormal(); + } + + /// Get the contact normal for the vertex (assumes there is a contact). + JPH_INLINE Vec3 GetContactNormal(const SoftBodyVertex &inVertex) const + { + return -inVertex.mCollisionPlane.GetNormal(); + } + + /// Get the body with which the vertex has collided in this update + JPH_INLINE BodyID GetContactBodyID(const SoftBodyVertex &inVertex) const + { + return inVertex.mHasContact? mCollidingShapes[inVertex.mCollidingShapeIndex].mBodyID : BodyID(); + } + +private: + /// Allow SoftBodyMotionProperties to construct us + friend class SoftBodyMotionProperties; + + /// Constructor + explicit SoftBodyManifold(const SoftBodyMotionProperties *inMotionProperties) : + mVertices(inMotionProperties->mVertices), + mCollidingShapes(inMotionProperties->mCollidingShapes) + { + } + + using CollidingShape = SoftBodyMotionProperties::CollidingShape; + + const Array & mVertices; + const Array & mCollidingShapes; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp new file mode 100644 index 000000000..e609b64e7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp @@ -0,0 +1,1206 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +using namespace JPH::literals; + +void SoftBodyMotionProperties::CalculateMassAndInertia() +{ + MassProperties mp; + + for (const Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + Vec3 pos = v.mPosition; + + // Accumulate mass + float mass = 1.0f / v.mInvMass; + mp.mMass += mass; + + // Inertia tensor, diagonal + // See equations https://en.wikipedia.org/wiki/Moment_of_inertia section 'Inertia Tensor' + for (int i = 0; i < 3; ++i) + mp.mInertia(i, i) += mass * (Square(pos[(i + 1) % 3]) + Square(pos[(i + 2) % 3])); + + // Inertia tensor off diagonal + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + if (i != j) + mp.mInertia(i, j) -= mass * pos[i] * pos[j]; + } + else + { + // If one vertex is kinematic, the entire body will have infinite mass and inertia + SetInverseMass(0.0f); + SetInverseInertia(Vec3::sZero(), Quat::sIdentity()); + return; + } + + SetMassProperties(EAllowedDOFs::All, mp); +} + +void SoftBodyMotionProperties::Initialize(const SoftBodyCreationSettings &inSettings) +{ + // Store settings + mSettings = inSettings.mSettings; + mNumIterations = inSettings.mNumIterations; + mPressure = inSettings.mPressure; + mUpdatePosition = inSettings.mUpdatePosition; + + // Initialize vertices + mVertices.resize(inSettings.mSettings->mVertices.size()); + Mat44 rotation = inSettings.mMakeRotationIdentity? Mat44::sRotation(inSettings.mRotation) : Mat44::sIdentity(); + for (Array::size_type v = 0, s = mVertices.size(); v < s; ++v) + { + const SoftBodySharedSettings::Vertex &in_vertex = inSettings.mSettings->mVertices[v]; + Vertex &out_vertex = mVertices[v]; + out_vertex.mPreviousPosition = out_vertex.mPosition = rotation * Vec3(in_vertex.mPosition); + out_vertex.mVelocity = rotation.Multiply3x3(Vec3(in_vertex.mVelocity)); + out_vertex.mCollidingShapeIndex = -1; + out_vertex.mHasContact = false; + out_vertex.mLargestPenetration = -FLT_MAX; + out_vertex.mInvMass = in_vertex.mInvMass; + mLocalBounds.Encapsulate(out_vertex.mPosition); + } + + // Allocate space for skinned vertices + if (!inSettings.mSettings->mSkinnedConstraints.empty()) + mSkinState.resize(mVertices.size()); + + // We don't know delta time yet, so we can't predict the bounds and use the local bounds as the predicted bounds + mLocalPredictedBounds = mLocalBounds; + + CalculateMassAndInertia(); +} + +float SoftBodyMotionProperties::GetVolumeTimesSix() const +{ + float six_volume = 0.0f; + for (const Face &f : mSettings->mFaces) + { + Vec3 x1 = mVertices[f.mVertex[0]].mPosition; + Vec3 x2 = mVertices[f.mVertex[1]].mPosition; + Vec3 x3 = mVertices[f.mVertex[2]].mPosition; + six_volume += x1.Cross(x2).Dot(x3); // We pick zero as the origin as this is the center of the bounding box so should give good accuracy + } + return six_volume; +} + +void SoftBodyMotionProperties::DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface) +{ + JPH_PROFILE_FUNCTION(); + + struct Collector : public CollideShapeBodyCollector + { + Collector(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface, Array &ioHits) : + mContext(inContext), + mInverseTransform(inContext.mCenterOfMassTransform.InversedRotationTranslation()), + mBodyLockInterface(inBodyLockInterface), + mCombineFriction(inSystem.GetCombineFriction()), + mCombineRestitution(inSystem.GetCombineRestitution()), + mHits(ioHits) + { + } + + virtual void AddHit(const BodyID &inResult) override + { + BodyLockRead lock(mBodyLockInterface, inResult); + if (lock.Succeeded()) + { + const Body &soft_body = *mContext.mBody; + const Body &body = lock.GetBody(); + if (body.IsRigidBody() // TODO: We should support soft body vs soft body + && soft_body.GetCollisionGroup().CanCollide(body.GetCollisionGroup())) + { + SoftBodyContactSettings settings; + settings.mIsSensor = body.IsSensor(); + + if (mContext.mContactListener == nullptr) + { + // If we have no contact listener, we can ignore sensors + if (settings.mIsSensor) + return; + } + else + { + // Call the contact listener to see if we should accept this contact + if (mContext.mContactListener->OnSoftBodyContactValidate(soft_body, body, settings) != SoftBodyValidateResult::AcceptContact) + return; + + // Check if there will be any interaction + if (!settings.mIsSensor + && settings.mInvMassScale1 == 0.0f + && (body.GetMotionType() != EMotionType::Dynamic || settings.mInvMassScale2 == 0.0f)) + return; + } + + CollidingShape cs; + cs.mCenterOfMassTransform = (mInverseTransform * body.GetCenterOfMassTransform()).ToMat44(); + cs.mShape = body.GetShape(); + cs.mBodyID = inResult; + cs.mMotionType = body.GetMotionType(); + cs.mIsSensor = settings.mIsSensor; + cs.mUpdateVelocities = false; + cs.mFriction = mCombineFriction(soft_body, SubShapeID(), body, SubShapeID()); + cs.mRestitution = mCombineRestitution(soft_body, SubShapeID(), body, SubShapeID()); + cs.mSoftBodyInvMassScale = settings.mInvMassScale1; + if (cs.mMotionType == EMotionType::Dynamic) + { + const MotionProperties *mp = body.GetMotionProperties(); + cs.mInvMass = settings.mInvMassScale2 * mp->GetInverseMass(); + cs.mInvInertia = settings.mInvInertiaScale2 * mp->GetInverseInertiaForRotation(cs.mCenterOfMassTransform.GetRotation()); + cs.mOriginalLinearVelocity = cs.mLinearVelocity = mInverseTransform.Multiply3x3(mp->GetLinearVelocity()); + cs.mOriginalAngularVelocity = cs.mAngularVelocity = mInverseTransform.Multiply3x3(mp->GetAngularVelocity()); + } + mHits.push_back(cs); + } + } + } + + private: + const SoftBodyUpdateContext &mContext; + RMat44 mInverseTransform; + const BodyLockInterface & mBodyLockInterface; + ContactConstraintManager::CombineFunction mCombineFriction; + ContactConstraintManager::CombineFunction mCombineRestitution; + Array & mHits; + }; + + Collector collector(inContext, inSystem, inBodyLockInterface, mCollidingShapes); + AABox bounds = mLocalBounds; + bounds.Encapsulate(mLocalPredictedBounds); + bounds = bounds.Transformed(inContext.mCenterOfMassTransform); + bounds.ExpandBy(Vec3::sReplicate(mSettings->mVertexRadius)); + ObjectLayer layer = inContext.mBody->GetObjectLayer(); + DefaultBroadPhaseLayerFilter broadphase_layer_filter = inSystem.GetDefaultBroadPhaseLayerFilter(layer); + DefaultObjectLayerFilter object_layer_filter = inSystem.GetDefaultLayerFilter(layer); + inSystem.GetBroadPhaseQuery().CollideAABox(bounds, collector, broadphase_layer_filter, object_layer_filter); +} + +void SoftBodyMotionProperties::DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices) +{ + JPH_PROFILE_FUNCTION(); + + // Generate collision planes + for (const CollidingShape &cs : mCollidingShapes) + cs.mShape->CollideSoftBodyVertices(cs.mCenterOfMassTransform, Vec3::sReplicate(1.0f), mVertices.data() + inVertexStart, inNumVertices, inContext.mDeltaTime, inContext.mDisplacementDueToGravity, int(&cs - mCollidingShapes.data())); +} + +void SoftBodyMotionProperties::ApplyPressure(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float pressure_coefficient = mPressure; + if (pressure_coefficient > 0.0f) + { + // Calculate total volume + float six_volume = GetVolumeTimesSix(); + if (six_volume > 0.0f) + { + // Apply pressure + // p = F / A = n R T / V (see https://en.wikipedia.org/wiki/Pressure) + // Our pressure coefficient is n R T so the impulse is: + // P = F dt = pressure_coefficient / V * A * dt + float coefficient = pressure_coefficient * dt / six_volume; // Need to still multiply by 6 for the volume + for (const Face &f : mSettings->mFaces) + { + Vec3 x1 = mVertices[f.mVertex[0]].mPosition; + Vec3 x2 = mVertices[f.mVertex[1]].mPosition; + Vec3 x3 = mVertices[f.mVertex[2]].mPosition; + + Vec3 impulse = coefficient * (x2 - x1).Cross(x3 - x1); // Area is half the cross product so need to still divide by 2 + for (uint32 i : f.mVertex) + { + Vertex &v = mVertices[i]; + v.mVelocity += v.mInvMass * impulse; // Want to divide by 3 because we spread over 3 vertices + } + } + } + } +} + +void SoftBodyMotionProperties::IntegratePositions(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float linear_damping = max(0.0f, 1.0f - GetLinearDamping() * dt); // See: MotionProperties::ApplyForceTorqueAndDragInternal + + // Integrate + Vec3 sub_step_gravity = inContext.mGravity * dt; + Vec3 sub_step_impulse = GetAccumulatedForce() * dt; + for (Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + // Gravity + v.mVelocity += sub_step_gravity + sub_step_impulse * v.mInvMass; + + // Damping + v.mVelocity *= linear_damping; + + // Integrate + v.mPreviousPosition = v.mPosition; + v.mPosition += v.mVelocity * dt; + } + else + { + // Integrate + v.mPreviousPosition = v.mPosition; + v.mPosition += v.mVelocity * dt; + } +} + +void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + for (const DihedralBend *b = mSettings->mDihedralBendConstraints.data() + inStartIndex, *b_end = mSettings->mDihedralBendConstraints.data() + inEndIndex; b < b_end; ++b) + { + Vertex &v0 = mVertices[b->mVertex[0]]; + Vertex &v1 = mVertices[b->mVertex[1]]; + Vertex &v2 = mVertices[b->mVertex[2]]; + Vertex &v3 = mVertices[b->mVertex[3]]; + + // Get positions + Vec3 x0 = v0.mPosition; + Vec3 x1 = v1.mPosition; + Vec3 x2 = v2.mPosition; + Vec3 x3 = v3.mPosition; + + /* + x2 + e1/ \e3 + / \ + x0----x1 + \ e0 / + e2\ /e4 + x3 + */ + + // Calculate the shared edge of the triangles + Vec3 e = x1 - x0; + float e_len = e.Length(); + if (e_len < 1.0e-6f) + continue; + + // Calculate the normals of the triangles + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 n1 = (x2 - x0).Cross(x1x2); + Vec3 n2 = x1x3.Cross(x3 - x0); + float n1_len_sq = n1.LengthSq(); + float n2_len_sq = n2.LengthSq(); + float n1_len_sq_n2_len_sq = n1_len_sq * n2_len_sq; + if (n1_len_sq_n2_len_sq < 1.0e-24f) + continue; + + // Calculate constraint equation + // As per "Strain Based Dynamics" Appendix A we need to negate the gradients when (n1 x n2) . e > 0, instead we make sure that the sign of the constraint equation is correct + float sign = Sign(n2.Cross(n1).Dot(e)); + float d = n1.Dot(n2) / sqrt(n1_len_sq_n2_len_sq); + float c = sign * ACosApproximate(d) - b->mInitialAngle; + + // Ensure the range is -PI to PI + if (c > JPH_PI) + c -= 2.0f * JPH_PI; + else if (c < -JPH_PI) + c += 2.0f * JPH_PI; + + // Calculate gradient of constraint equation + // Taken from "Strain Based Dynamics" - Matthias Muller et al. (Appendix A) + // with p1 = x2, p2 = x3, p3 = x0 and p4 = x1 + // which in turn is based on "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. (Section 4) + n1 /= n1_len_sq; + n2 /= n2_len_sq; + Vec3 d0c = (x1x2.Dot(e) * n1 + x1x3.Dot(e) * n2) / e_len; + Vec3 d2c = e_len * n1; + Vec3 d3c = e_len * n2; + + // The sum of the gradients must be zero (see "Strain Based Dynamics" section 4) + Vec3 d1c = -d0c - d2c - d3c; + + // Get masses + float w0 = v0.mInvMass; + float w1 = v1.mInvMass; + float w2 = v2.mInvMass; + float w3 = v3.mInvMass; + + // Calculate -lambda + float denom = w0 * d0c.LengthSq() + w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + b->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + float minus_lambda = c / denom; + + // Apply correction + v0.mPosition = x0 - minus_lambda * w0 * d0c; + v1.mPosition = x1 - minus_lambda * w1 * d1c; + v2.mPosition = x2 - minus_lambda * w2 * d2c; + v3.mPosition = x3 - minus_lambda * w3 * d3c; + } +} + +void SoftBodyMotionProperties::ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + // Satisfy volume constraints + for (const Volume *v = mSettings->mVolumeConstraints.data() + inStartIndex, *v_end = mSettings->mVolumeConstraints.data() + inEndIndex; v < v_end; ++v) + { + Vertex &v1 = mVertices[v->mVertex[0]]; + Vertex &v2 = mVertices[v->mVertex[1]]; + Vertex &v3 = mVertices[v->mVertex[2]]; + Vertex &v4 = mVertices[v->mVertex[3]]; + + Vec3 x1 = v1.mPosition; + Vec3 x2 = v2.mPosition; + Vec3 x3 = v3.mPosition; + Vec3 x4 = v4.mPosition; + + // Calculate constraint equation + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 x1x4 = x4 - x1; + float c = abs(x1x2.Cross(x1x3).Dot(x1x4)) - v->mSixRestVolume; + + // Calculate gradient of constraint equation + Vec3 d1c = (x4 - x2).Cross(x3 - x2); + Vec3 d2c = x1x3.Cross(x1x4); + Vec3 d3c = x1x4.Cross(x1x2); + Vec3 d4c = x1x2.Cross(x1x3); + + // Get masses + float w1 = v1.mInvMass; + float w2 = v2.mInvMass; + float w3 = v3.mInvMass; + float w4 = v4.mInvMass; + + // Calculate -lambda + float denom = w1 * d1c.LengthSq() + w2 * d2c.LengthSq() + w3 * d3c.LengthSq() + w4 * d4c.LengthSq() + v->mCompliance * inv_dt_sq; + if (denom < 1.0e-12f) + continue; + float minus_lambda = c / denom; + + // Apply correction + v1.mPosition = x1 - minus_lambda * w1 * d1c; + v2.mPosition = x2 - minus_lambda * w2 * d2c; + v3.mPosition = x3 - minus_lambda * w3 * d3c; + v4.mPosition = x4 - minus_lambda * w4 * d4c; + } +} + +void SoftBodyMotionProperties::ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + // Early out if nothing to do + if (mSettings->mSkinnedConstraints.empty() || !mEnableSkinConstraints) + return; + + JPH_PROFILE_FUNCTION(); + + // We're going to iterate multiple times over the skin constraints, update the skinned position accordingly. + // If we don't do this, the simulation will see a big jump and the first iteration will cause a big velocity change in the system. + float factor = mSkinStatePreviousPositionValid? inContext.mNextIteration.load(std::memory_order_relaxed) / float(mNumIterations) : 1.0f; + float prev_factor = 1.0f - factor; + + // Apply the constraints + Vertex *vertices = mVertices.data(); + const SkinState *skin_states = mSkinState.data(); + for (const Skinned *s = mSettings->mSkinnedConstraints.data() + inStartIndex, *s_end = mSettings->mSkinnedConstraints.data() + inEndIndex; s < s_end; ++s) + { + Vertex &vertex = vertices[s->mVertex]; + const SkinState &skin_state = skin_states[s->mVertex]; + float max_distance = s->mMaxDistance * mSkinnedMaxDistanceMultiplier; + + // Calculate the skinned position by interpolating from previous to current position + Vec3 skin_pos = prev_factor * skin_state.mPreviousPosition + factor * skin_state.mPosition; + + if (max_distance > 0.0f) + { + // Move vertex if it violated the back stop + if (s->mBackStopDistance < max_distance) + { + // Center of the back stop sphere + Vec3 center = skin_pos - skin_state.mNormal * (s->mBackStopDistance + s->mBackStopRadius); + + // Check if we're inside the back stop sphere + Vec3 delta = vertex.mPosition - center; + float delta_len_sq = delta.LengthSq(); + if (delta_len_sq < Square(s->mBackStopRadius)) + { + // Push the vertex to the surface of the back stop sphere + float delta_len = sqrt(delta_len_sq); + vertex.mPosition = delta_len > 0.0f? + center + delta * (s->mBackStopRadius / delta_len) + : center + skin_state.mNormal * s->mBackStopRadius; + } + } + + // Clamp vertex distance to max distance from skinned position + if (max_distance < FLT_MAX) + { + Vec3 delta = vertex.mPosition - skin_pos; + float delta_len_sq = delta.LengthSq(); + float max_distance_sq = Square(max_distance); + if (delta_len_sq > max_distance_sq) + vertex.mPosition = skin_pos + delta * sqrt(max_distance_sq / delta_len_sq); + } + } + else + { + // Kinematic: Just update the vertex position + vertex.mPosition = skin_pos; + } + } +} + +void SoftBodyMotionProperties::ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + float inv_dt_sq = 1.0f / Square(inContext.mSubStepDeltaTime); + + // Satisfy edge constraints + for (const Edge *e = mSettings->mEdgeConstraints.data() + inStartIndex, *e_end = mSettings->mEdgeConstraints.data() + inEndIndex; e < e_end; ++e) + { + Vertex &v0 = mVertices[e->mVertex[0]]; + Vertex &v1 = mVertices[e->mVertex[1]]; + + // Get positions + Vec3 x0 = v0.mPosition; + Vec3 x1 = v1.mPosition; + + // Calculate current length + Vec3 delta = x1 - x0; + float length = delta.Length(); + + // Apply correction + float denom = length * (v0.mInvMass + v1.mInvMass + e->mCompliance * inv_dt_sq); + if (denom < 1.0e-12f) + continue; + Vec3 correction = delta * (length - e->mRestLength) / denom; + v0.mPosition = x0 + v0.mInvMass * correction; + v1.mPosition = x1 - v1.mInvMass * correction; + } +} + +void SoftBodyMotionProperties::ApplyLRAConstraints(uint inStartIndex, uint inEndIndex) +{ + JPH_PROFILE_FUNCTION(); + + // Satisfy LRA constraints + Vertex *vertices = mVertices.data(); + for (const LRA *lra = mSettings->mLRAConstraints.data() + inStartIndex, *lra_end = mSettings->mLRAConstraints.data() + inEndIndex; lra < lra_end; ++lra) + { + JPH_ASSERT(lra->mVertex[0] < mVertices.size()); + JPH_ASSERT(lra->mVertex[1] < mVertices.size()); + const Vertex &vertex0 = vertices[lra->mVertex[0]]; + Vertex &vertex1 = vertices[lra->mVertex[1]]; + + Vec3 x0 = vertex0.mPosition; + Vec3 delta = vertex1.mPosition - x0; + float delta_len_sq = delta.LengthSq(); + if (delta_len_sq > Square(lra->mMaxDistance)) + vertex1.mPosition = x0 + delta * lra->mMaxDistance / sqrt(delta_len_sq); + } +} + +void SoftBodyMotionProperties::ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext) +{ + JPH_PROFILE_FUNCTION(); + + float dt = inContext.mSubStepDeltaTime; + float restitution_treshold = -2.0f * inContext.mGravity.Length() * dt; + float vertex_radius = mSettings->mVertexRadius; + for (Vertex &v : mVertices) + if (v.mInvMass > 0.0f) + { + // Remember previous velocity for restitution calculations + Vec3 prev_v = v.mVelocity; + + // XPBD velocity update + v.mVelocity = (v.mPosition - v.mPreviousPosition) / dt; + + // Satisfy collision constraint + if (v.mCollidingShapeIndex >= 0) + { + // Check if there is a collision + float projected_distance = -v.mCollisionPlane.SignedDistance(v.mPosition) + vertex_radius; + if (projected_distance > 0.0f) + { + // Remember that there was a collision + v.mHasContact = true; + mHasContact = true; + + // Sensors should not have a collision response + CollidingShape &cs = mCollidingShapes[v.mCollidingShapeIndex]; + if (!cs.mIsSensor) + { + // Note that we already calculated the velocity, so this does not affect the velocity (next iteration starts by setting previous position to current position) + Vec3 contact_normal = v.mCollisionPlane.GetNormal(); + v.mPosition += contact_normal * projected_distance; + + // Apply friction as described in Detailed Rigid Body Simulation with Extended Position Based Dynamics - Matthias Muller et al. + // See section 3.6: + // Inverse mass: w1 = 1 / m1, w2 = 1 / m2 + (r2 x n)^T I^-1 (r2 x n) = 0 for a static object + // r2 are the contact point relative to the center of mass of body 2 + // Lagrange multiplier for contact: lambda = -c / (w1 + w2) + // Where c is the constraint equation (the distance to the plane, negative because penetrating) + // Contact normal force: fn = lambda / dt^2 + // Delta velocity due to friction dv = -vt / |vt| * min(dt * friction * fn * (w1 + w2), |vt|) = -vt * min(-friction * c / (|vt| * dt), 1) + // Note that I think there is an error in the paper, I added a mass term, see: https://github.com/matthias-research/pages/issues/29 + // Relative velocity: vr = v1 - v2 - omega2 x r2 + // Normal velocity: vn = vr . contact_normal + // Tangential velocity: vt = vr - contact_normal * vn + // Impulse: p = dv / (w1 + w2) + // Changes in particle velocities: + // v1 = v1 + p / m1 + // v2 = v2 - p / m2 (no change when colliding with a static body) + // w2 = w2 - I^-1 (r2 x p) (no change when colliding with a static body) + if (cs.mMotionType == EMotionType::Dynamic) + { + // Calculate normal and tangential velocity (equation 30) + Vec3 r2 = v.mPosition - cs.mCenterOfMassTransform.GetTranslation(); + Vec3 v2 = cs.GetPointVelocity(r2); + Vec3 relative_velocity = v.mVelocity - v2; + Vec3 v_normal = contact_normal * contact_normal.Dot(relative_velocity); + Vec3 v_tangential = relative_velocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Calculate resulting inverse mass of vertex + float vertex_inv_mass = cs.mSoftBodyInvMassScale * v.mInvMass; + + // Calculate inverse effective mass + Vec3 r2_cross_n = r2.Cross(contact_normal); + float w2 = cs.mInvMass + r2_cross_n.Dot(cs.mInvInertia * r2_cross_n); + float w1_plus_w2 = vertex_inv_mass + w2; + if (w1_plus_w2 > 0.0f) + { + // Calculate delta relative velocity due to friction (modified equation 31) + Vec3 dv; + if (v_tangential_length > 0.0f) + dv = v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + else + dv = Vec3::sZero(); + + // Calculate delta relative velocity due to restitution (equation 35) + dv += v_normal; + float prev_v_normal = (prev_v - v2).Dot(contact_normal); + if (prev_v_normal < restitution_treshold) + dv += cs.mRestitution * prev_v_normal * contact_normal; + + // Calculate impulse + Vec3 p = dv / w1_plus_w2; + + // Apply impulse to particle + v.mVelocity -= p * vertex_inv_mass; + + // Apply impulse to rigid body + cs.mLinearVelocity += p * cs.mInvMass; + cs.mAngularVelocity += cs.mInvInertia * r2.Cross(p); + + // Mark that the velocities of the body we hit need to be updated + cs.mUpdateVelocities = true; + } + } + else if (cs.mSoftBodyInvMassScale > 0.0f) + { + // Body is not movable, equations are simpler + + // Calculate normal and tangential velocity (equation 30) + Vec3 v_normal = contact_normal * contact_normal.Dot(v.mVelocity); + Vec3 v_tangential = v.mVelocity - v_normal; + float v_tangential_length = v_tangential.Length(); + + // Apply friction (modified equation 31) + if (v_tangential_length > 0.0f) + v.mVelocity -= v_tangential * min(cs.mFriction * projected_distance / (v_tangential_length * dt), 1.0f); + + // Apply restitution (equation 35) + v.mVelocity -= v_normal; + float prev_v_normal = prev_v.Dot(contact_normal); + if (prev_v_normal < restitution_treshold) + v.mVelocity -= cs.mRestitution * prev_v_normal * contact_normal; + } + } + } + } + } +} + +void SoftBodyMotionProperties::UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + JPH_PROFILE_FUNCTION(); + + // Contact callback + if (mHasContact && ioContext.mContactListener != nullptr) + ioContext.mContactListener->OnSoftBodyContactAdded(*ioContext.mBody, SoftBodyManifold(this)); + + // Loop through vertices once more to update the global state + float dt = ioContext.mDeltaTime; + float max_linear_velocity_sq = Square(GetMaxLinearVelocity()); + float max_v_sq = 0.0f; + Vec3 linear_velocity = Vec3::sZero(), angular_velocity = Vec3::sZero(); + mLocalPredictedBounds = mLocalBounds = { }; + mHasContact = false; + for (Vertex &v : mVertices) + { + // Calculate max square velocity + float v_sq = v.mVelocity.LengthSq(); + max_v_sq = max(max_v_sq, v_sq); + + // Clamp if velocity is too high + if (v_sq > max_linear_velocity_sq) + v.mVelocity *= sqrt(max_linear_velocity_sq / v_sq); + + // Calculate local linear/angular velocity + linear_velocity += v.mVelocity; + angular_velocity += v.mPosition.Cross(v.mVelocity); + + // Update local bounding box + mLocalBounds.Encapsulate(v.mPosition); + + // Create predicted position for the next frame in order to detect collisions before they happen + mLocalPredictedBounds.Encapsulate(v.mPosition + v.mVelocity * dt + ioContext.mDisplacementDueToGravity); + + // Reset collision data for the next iteration + v.mCollidingShapeIndex = -1; + v.mHasContact = false; + v.mLargestPenetration = -FLT_MAX; + } + + // Calculate linear/angular velocity of the body by averaging all vertices and bringing the value to world space + float num_vertices_divider = float(max(int(mVertices.size()), 1)); + SetLinearVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(linear_velocity / num_vertices_divider)); + SetAngularVelocity(ioContext.mCenterOfMassTransform.Multiply3x3(angular_velocity / num_vertices_divider)); + + if (mUpdatePosition) + { + // Shift the body so that the position is the center of the local bounds + Vec3 delta = mLocalBounds.GetCenter(); + ioContext.mDeltaPosition = ioContext.mCenterOfMassTransform.Multiply3x3(delta); + for (Vertex &v : mVertices) + v.mPosition -= delta; + + // Update the skin state too since we will use this position as the previous position in the next update + for (SkinState &s : mSkinState) + s.mPosition -= delta; + JPH_IF_DEBUG_RENDERER(mSkinStateTransform.SetTranslation(mSkinStateTransform.GetTranslation() + ioContext.mDeltaPosition);) + + // Offset bounds to match new position + mLocalBounds.Translate(-delta); + mLocalPredictedBounds.Translate(-delta); + } + else + ioContext.mDeltaPosition = Vec3::sZero(); + + // Test if we should go to sleep + if (GetAllowSleeping()) + { + if (max_v_sq > inPhysicsSettings.mPointVelocitySleepThreshold) + { + ResetSleepTestTimer(); + ioContext.mCanSleep = ECanSleep::CannotSleep; + } + else + ioContext.mCanSleep = AccumulateSleepTime(dt, inPhysicsSettings.mTimeBeforeSleep); + } + else + ioContext.mCanSleep = ECanSleep::CannotSleep; + + // If SkinVertices is not called after this then don't use the previous position as the skin is static + mSkinStatePreviousPositionValid = false; + + // Reset force accumulator + ResetForce(); +} + +void SoftBodyMotionProperties::UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface) +{ + JPH_PROFILE_FUNCTION(); + + // Write back velocity deltas + for (const CollidingShape &cs : mCollidingShapes) + if (cs.mUpdateVelocities) + inBodyInterface.AddLinearAndAngularVelocity(cs.mBodyID, inContext.mCenterOfMassTransform.Multiply3x3(cs.mLinearVelocity - cs.mOriginalLinearVelocity), inContext.mCenterOfMassTransform.Multiply3x3(cs.mAngularVelocity - cs.mOriginalAngularVelocity)); + + // Clear colliding shapes to avoid hanging on to references to shapes + mCollidingShapes.clear(); +} + +void SoftBodyMotionProperties::InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext) +{ + JPH_PROFILE_FUNCTION(); + + // Store body + ioContext.mBody = &inSoftBody; + ioContext.mMotionProperties = this; + ioContext.mContactListener = inSystem.GetSoftBodyContactListener(); + + // Convert gravity to local space + ioContext.mCenterOfMassTransform = inSoftBody.GetCenterOfMassTransform(); + ioContext.mGravity = ioContext.mCenterOfMassTransform.Multiply3x3Transposed(GetGravityFactor() * inSystem.GetGravity()); + + // Calculate delta time for sub step + ioContext.mDeltaTime = inDeltaTime; + ioContext.mSubStepDeltaTime = inDeltaTime / mNumIterations; + + // Calculate total displacement we'll have due to gravity over all sub steps + // The total displacement as produced by our integrator can be written as: Sum(i * g * dt^2, i = 0..mNumIterations). + // This is bigger than 0.5 * g * dt^2 because we first increment the velocity and then update the position + // Using Sum(i, i = 0..n) = n * (n + 1) / 2 we can write this as: + ioContext.mDisplacementDueToGravity = (0.5f * mNumIterations * (mNumIterations + 1) * Square(ioContext.mSubStepDeltaTime)) * ioContext.mGravity; +} + +void SoftBodyMotionProperties::StartNextIteration(const SoftBodyUpdateContext &ioContext) +{ + ApplyPressure(ioContext); + + IntegratePositions(ioContext); +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext) +{ + // Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it) + uint num_vertices = (uint)mVertices.size(); + if (ioContext.mNextCollisionVertex.load(memory_order_relaxed) < num_vertices) + { + // Fetch next batch of vertices to process + uint next_vertex = ioContext.mNextCollisionVertex.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_acquire); + if (next_vertex < num_vertices) + { + // Process collision planes + uint num_vertices_to_process = min(SoftBodyUpdateContext::cVertexCollisionBatch, num_vertices - next_vertex); + DetermineCollisionPlanes(ioContext, next_vertex, num_vertices_to_process); + uint vertices_processed = ioContext.mNumCollisionVerticesProcessed.fetch_add(SoftBodyUpdateContext::cVertexCollisionBatch, memory_order_release) + num_vertices_to_process; + if (vertices_processed >= num_vertices) + { + // Start the first iteration + JPH_IF_ENABLE_ASSERTS(uint iteration =) ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); + JPH_ASSERT(iteration == 0); + StartNextIteration(ioContext); + ioContext.mState.store(SoftBodyUpdateContext::EState::ApplyConstraints, memory_order_release); + } + return EStatus::DidWork; + } + } + return EStatus::NoWork; +} + +void SoftBodyMotionProperties::ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex) +{ + // Determine start and end + SoftBodySharedSettings::UpdateGroup start { 0, 0, 0, 0, 0 }; + const SoftBodySharedSettings::UpdateGroup &prev = inGroupIndex > 0? mSettings->mUpdateGroups[inGroupIndex - 1] : start; + const SoftBodySharedSettings::UpdateGroup ¤t = mSettings->mUpdateGroups[inGroupIndex]; + + // Process volume constraints + ApplyVolumeConstraints(ioContext, prev.mVolumeEndIndex, current.mVolumeEndIndex); + + // Process bend constraints + ApplyDihedralBendConstraints(ioContext, prev.mDihedralBendEndIndex, current.mDihedralBendEndIndex); + + // Process skinned constraints + ApplySkinConstraints(ioContext, prev.mSkinnedEndIndex, current.mSkinnedEndIndex); + + // Process edges + ApplyEdgeConstraints(ioContext, prev.mEdgeEndIndex, current.mEdgeEndIndex); + + // Process LRA constraints + ApplyLRAConstraints(prev.mLRAEndIndex, current.mLRAEndIndex); +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + uint num_groups = (uint)mSettings->mUpdateGroups.size(); + JPH_ASSERT(num_groups > 0, "SoftBodySharedSettings::Optimize should have been called!"); + --num_groups; // Last group is the non-parallel group, we don't want to execute it in parallel + + // Do a relaxed read first to see if there is any work to do (this prevents us from doing expensive atomic operations and also prevents us from continuously incrementing the counter and overflowing it) + uint next_group = ioContext.mNextConstraintGroup.load(memory_order_relaxed); + if (next_group < num_groups || (num_groups == 0 && next_group == 0)) + { + // Fetch the next group process + next_group = ioContext.mNextConstraintGroup.fetch_add(1, memory_order_acquire); + if (next_group < num_groups || (num_groups == 0 && next_group == 0)) + { + uint num_groups_processed = 0; + if (num_groups > 0) + { + // Process this group + ProcessGroup(ioContext, next_group); + + // Increment total number of groups processed + num_groups_processed = ioContext.mNumConstraintGroupsProcessed.fetch_add(1, memory_order_relaxed) + 1; + } + + if (num_groups_processed >= num_groups) + { + // Finish the iteration + JPH_PROFILE("FinishIteration"); + + // Process non-parallel group + ProcessGroup(ioContext, num_groups); + + ApplyCollisionConstraintsAndUpdateVelocities(ioContext); + + uint iteration = ioContext.mNextIteration.fetch_add(1, memory_order_relaxed); + if (iteration < mNumIterations) + { + // Start a new iteration + StartNextIteration(ioContext); + + // Reset group logic + ioContext.mNumConstraintGroupsProcessed.store(0, memory_order_relaxed); + ioContext.mNextConstraintGroup.store(0, memory_order_release); + } + else + { + // On final iteration we update the state + UpdateSoftBodyState(ioContext, inPhysicsSettings); + + ioContext.mState.store(SoftBodyUpdateContext::EState::Done, memory_order_release); + return EStatus::Done; + } + } + + return EStatus::DidWork; + } + } + return EStatus::NoWork; +} + +SoftBodyMotionProperties::EStatus SoftBodyMotionProperties::ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings) +{ + switch (ioContext.mState.load(memory_order_relaxed)) + { + case SoftBodyUpdateContext::EState::DetermineCollisionPlanes: + return ParallelDetermineCollisionPlanes(ioContext); + + case SoftBodyUpdateContext::EState::ApplyConstraints: + return ParallelApplyConstraints(ioContext, inPhysicsSettings); + + case SoftBodyUpdateContext::EState::Done: + return EStatus::Done; + + default: + JPH_ASSERT(false); + return EStatus::NoWork; + } +} + +void SoftBodyMotionProperties::SkinVertices([[maybe_unused]] RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, [[maybe_unused]] uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator) +{ + // Calculate the skin matrices + uint num_skin_matrices = uint(mSettings->mInvBindMatrices.size()); + uint skin_matrices_size = num_skin_matrices * sizeof(Mat44); + Mat44 *skin_matrices = (Mat44 *)ioTempAllocator.Allocate(skin_matrices_size); + JPH_SCOPE_EXIT([&ioTempAllocator, skin_matrices, skin_matrices_size]{ ioTempAllocator.Free(skin_matrices, skin_matrices_size); }); + const Mat44 *skin_matrices_end = skin_matrices + num_skin_matrices; + const InvBind *inv_bind_matrix = mSettings->mInvBindMatrices.data(); + for (Mat44 *s = skin_matrices; s < skin_matrices_end; ++s, ++inv_bind_matrix) + *s = inJointMatrices[inv_bind_matrix->mJointIndex] * inv_bind_matrix->mInvBind; + + // Skin the vertices + JPH_IF_DEBUG_RENDERER(mSkinStateTransform = inCenterOfMassTransform;) + JPH_IF_ENABLE_ASSERTS(uint num_vertices = uint(mSettings->mVertices.size());) + JPH_ASSERT(mSkinState.size() == num_vertices); + const SoftBodySharedSettings::Vertex *in_vertices = mSettings->mVertices.data(); + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + // Get bind pose + JPH_ASSERT(s.mVertex < num_vertices); + Vec3 bind_pos = Vec3::sLoadFloat3Unsafe(in_vertices[s.mVertex].mPosition); + + // Skin vertex + Vec3 pos = Vec3::sZero(); + for (const SkinWeight &w : s.mWeights) + { + // We assume that the first zero weight is the end of the list + if (w.mWeight == 0.0f) + break; + + JPH_ASSERT(w.mInvBindIndex < num_skin_matrices); + pos += w.mWeight * (skin_matrices[w.mInvBindIndex] * bind_pos); + } + SkinState &skin_state = mSkinState[s.mVertex]; + skin_state.mPreviousPosition = skin_state.mPosition; + skin_state.mPosition = pos; + } + + // Calculate the normals + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vec3 normal = Vec3::sZero(); + uint32 num_faces = s.mNormalInfo >> 24; + if (num_faces > 0) + { + // Calculate normal + const uint32 *f = &mSettings->mSkinnedConstraintNormals[s.mNormalInfo & 0xffffff]; + const uint32 *f_end = f + num_faces; + while (f < f_end) + { + const Face &face = mSettings->mFaces[*f]; + Vec3 v0 = mSkinState[face.mVertex[0]].mPosition; + Vec3 v1 = mSkinState[face.mVertex[1]].mPosition; + Vec3 v2 = mSkinState[face.mVertex[2]].mPosition; + normal += (v1 - v0).Cross(v2 - v0).NormalizedOr(Vec3::sZero()); + ++f; + } + normal = normal.NormalizedOr(Vec3::sZero()); + } + mSkinState[s.mVertex].mNormal = normal; + } + + if (inHardSkinAll) + { + // Hard skin all vertices and reset their velocities + for (const Skinned &s : mSettings->mSkinnedConstraints) + { + Vertex &vertex = mVertices[s.mVertex]; + SkinState &skin_state = mSkinState[s.mVertex]; + skin_state.mPreviousPosition = skin_state.mPosition; + vertex.mPosition = skin_state.mPosition; + vertex.mVelocity = Vec3::sZero(); + } + } + else if (!mEnableSkinConstraints) + { + // Hard skin only the kinematic vertices as we will not solve the skin constraints later + for (const Skinned &s : mSettings->mSkinnedConstraints) + if (s.mMaxDistance == 0.0f) + { + Vertex &vertex = mVertices[s.mVertex]; + vertex.mPosition = mSkinState[s.mVertex].mPosition; + } + } + + // Indicate that the previous positions are valid for the coming update + mSkinStatePreviousPositionValid = true; +} + +void SoftBodyMotionProperties::CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem) +{ + JPH_PROFILE_FUNCTION(); + + // Create update context + SoftBodyUpdateContext context; + InitializeUpdateContext(inDeltaTime, ioSoftBody, inSystem, context); + + // Determine bodies we're colliding with + DetermineCollidingShapes(context, inSystem, inSystem.GetBodyLockInterface()); + + // Call the internal update until it finishes + EStatus status; + const PhysicsSettings &settings = inSystem.GetPhysicsSettings(); + while ((status = ParallelUpdate(context, settings)) == EStatus::DidWork) + continue; + JPH_ASSERT(status == EStatus::Done); + + // Update the state of the bodies we've collided with + UpdateRigidBodyVelocities(context, inSystem.GetBodyInterface()); + + // Update position of the soft body + if (mUpdatePosition) + inSystem.GetBodyInterface().SetPosition(ioSoftBody.GetID(), ioSoftBody.GetPosition() + context.mDeltaPosition, EActivation::DontActivate); +} + +#ifdef JPH_DEBUG_RENDERER + +void SoftBodyMotionProperties::DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + for (const Vertex &v : mVertices) + inRenderer->DrawMarker(inCenterOfMassTransform * v.mPosition, v.mInvMass > 0.0f? Color::sGreen : Color::sRed, 0.05f); +} + +void SoftBodyMotionProperties::DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + for (const Vertex &v : mVertices) + inRenderer->DrawArrow(inCenterOfMassTransform * v.mPosition, inCenterOfMassTransform * (v.mPosition + v.mVelocity), Color::sYellow, 0.01f); +} + +template +inline void SoftBodyMotionProperties::DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const +{ + uint start = 0; + for (uint i = 0; i < (uint)mSettings->mUpdateGroups.size(); ++i) + { + uint end = inGetEndIndex(mSettings->mUpdateGroups[i]); + + Color base_color; + if (inConstraintColor != ESoftBodyConstraintColor::ConstraintType) + base_color = Color::sGetDistinctColor((uint)mSettings->mUpdateGroups.size() - i - 1); // Ensure that color 0 is always the last group + else + base_color = inBaseColor; + + for (uint idx = start; idx < end; ++idx) + { + Color color = inConstraintColor == ESoftBodyConstraintColor::ConstraintOrder? base_color * (float(idx - start) / (end - start)) : base_color; + inDrawConstraint(idx, color); + } + + start = end; + } +} + +void SoftBodyMotionProperties::DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mEdgeEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Edge &e = mSettings->mEdgeConstraints[inIndex]; + inRenderer->DrawLine(inCenterOfMassTransform * mVertices[e.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[e.mVertex[1]].mPosition, inColor); + }, + Color::sWhite); +} + +void SoftBodyMotionProperties::DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mDihedralBendEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const DihedralBend &b = mSettings->mDihedralBendConstraints[inIndex]; + + RVec3 x0 = inCenterOfMassTransform * mVertices[b.mVertex[0]].mPosition; + RVec3 x1 = inCenterOfMassTransform * mVertices[b.mVertex[1]].mPosition; + RVec3 x2 = inCenterOfMassTransform * mVertices[b.mVertex[2]].mPosition; + RVec3 x3 = inCenterOfMassTransform * mVertices[b.mVertex[3]].mPosition; + RVec3 c_edge = 0.5_r * (x0 + x1); + RVec3 c0 = (x0 + x1 + x2) / 3.0_r; + RVec3 c1 = (x0 + x1 + x3) / 3.0_r; + + inRenderer->DrawArrow(0.9_r * x0 + 0.1_r * x1, 0.1_r * x0 + 0.9_r * x1, inColor, 0.01f); + inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c0, inColor); + inRenderer->DrawLine(c_edge, 0.1_r * c_edge + 0.9_r * c1, inColor); + }, + Color::sGreen); +} + +void SoftBodyMotionProperties::DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mVolumeEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Volume &v = mSettings->mVolumeConstraints[inIndex]; + + RVec3 x1 = inCenterOfMassTransform * mVertices[v.mVertex[0]].mPosition; + RVec3 x2 = inCenterOfMassTransform * mVertices[v.mVertex[1]].mPosition; + RVec3 x3 = inCenterOfMassTransform * mVertices[v.mVertex[2]].mPosition; + RVec3 x4 = inCenterOfMassTransform * mVertices[v.mVertex[3]].mPosition; + + inRenderer->DrawTriangle(x1, x3, x2, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x2, x3, x4, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x1, x4, x3, inColor, DebugRenderer::ECastShadow::On); + inRenderer->DrawTriangle(x1, x2, x4, inColor, DebugRenderer::ECastShadow::On); + }, + Color::sYellow); +} + +void SoftBodyMotionProperties::DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mSkinnedEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const Skinned &s = mSettings->mSkinnedConstraints[inIndex]; + const SkinState &skin_state = mSkinState[s.mVertex]; + inRenderer->DrawArrow(mSkinStateTransform * skin_state.mPosition, mSkinStateTransform * (skin_state.mPosition + 0.1f * skin_state.mNormal), inColor, 0.01f); + inRenderer->DrawLine(mSkinStateTransform * skin_state.mPosition, inCenterOfMassTransform * mVertices[s.mVertex].mPosition, Color::sBlue); + }, + Color::sOrange); +} + +void SoftBodyMotionProperties::DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const +{ + DrawConstraints(inConstraintColor, + [](const SoftBodySharedSettings::UpdateGroup &inGroup) { + return inGroup.mLRAEndIndex; + }, + [this, inRenderer, &inCenterOfMassTransform](uint inIndex, ColorArg inColor) { + const LRA &l = mSettings->mLRAConstraints[inIndex]; + inRenderer->DrawLine(inCenterOfMassTransform * mVertices[l.mVertex[0]].mPosition, inCenterOfMassTransform * mVertices[l.mVertex[1]].mPosition, inColor); + }, + Color::sGrey); +} + +void SoftBodyMotionProperties::DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const +{ + inRenderer->DrawWireBox(inCenterOfMassTransform, mLocalPredictedBounds, Color::sRed); +} + +#endif // JPH_DEBUG_RENDERER + +void SoftBodyMotionProperties::SaveState(StateRecorder &inStream) const +{ + MotionProperties::SaveState(inStream); + + for (const Vertex &v : mVertices) + { + inStream.Write(v.mPreviousPosition); + inStream.Write(v.mPosition); + inStream.Write(v.mVelocity); + } + + for (const SkinState &s : mSkinState) + { + inStream.Write(s.mPreviousPosition); + inStream.Write(s.mPosition); + inStream.Write(s.mNormal); + } + + inStream.Write(mLocalBounds.mMin); + inStream.Write(mLocalBounds.mMax); + inStream.Write(mLocalPredictedBounds.mMin); + inStream.Write(mLocalPredictedBounds.mMax); +} + +void SoftBodyMotionProperties::RestoreState(StateRecorder &inStream) +{ + MotionProperties::RestoreState(inStream); + + for (Vertex &v : mVertices) + { + inStream.Read(v.mPreviousPosition); + inStream.Read(v.mPosition); + inStream.Read(v.mVelocity); + } + + for (SkinState &s : mSkinState) + { + inStream.Read(s.mPreviousPosition); + inStream.Read(s.mPosition); + inStream.Read(s.mNormal); + } + + inStream.Read(mLocalBounds.mMin); + inStream.Read(mLocalBounds.mMax); + inStream.Read(mLocalPredictedBounds.mMin); + inStream.Read(mLocalPredictedBounds.mMax); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h new file mode 100644 index 000000000..5d07c3795 --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyMotionProperties.h @@ -0,0 +1,267 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class BodyInterface; +class BodyLockInterface; +struct PhysicsSettings; +class Body; +class Shape; +class SoftBodyCreationSettings; +class TempAllocator; +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +enum class ESoftBodyConstraintColor; +#endif // JPH_DEBUG_RENDERER + +/// This class contains the runtime information of a soft body. +// +// Based on: XPBD, Extended Position Based Dynamics, Matthias Muller, Ten Minute Physics +// See: https://matthias-research.github.io/pages/tenMinutePhysics/09-xpbd.pdf +class JPH_EXPORT SoftBodyMotionProperties : public MotionProperties +{ +public: + using Vertex = SoftBodyVertex; + using Edge = SoftBodySharedSettings::Edge; + using Face = SoftBodySharedSettings::Face; + using DihedralBend = SoftBodySharedSettings::DihedralBend; + using Volume = SoftBodySharedSettings::Volume; + using InvBind = SoftBodySharedSettings::InvBind; + using SkinWeight = SoftBodySharedSettings::SkinWeight; + using Skinned = SoftBodySharedSettings::Skinned; + using LRA = SoftBodySharedSettings::LRA; + + /// Initialize the soft body motion properties + void Initialize(const SoftBodyCreationSettings &inSettings); + + /// Get the shared settings of the soft body + const SoftBodySharedSettings * GetSettings() const { return mSettings; } + + /// Get the vertices of the soft body + const Array & GetVertices() const { return mVertices; } + Array & GetVertices() { return mVertices; } + + /// Access an individual vertex + const Vertex & GetVertex(uint inIndex) const { return mVertices[inIndex]; } + Vertex & GetVertex(uint inIndex) { return mVertices[inIndex]; } + + /// Get the materials of the soft body + const PhysicsMaterialList & GetMaterials() const { return mSettings->mMaterials; } + + /// Get the faces of the soft body + const Array & GetFaces() const { return mSettings->mFaces; } + + /// Access to an individual face + const Face & GetFace(uint inIndex) const { return mSettings->mFaces[inIndex]; } + + /// Get the number of solver iterations + uint32 GetNumIterations() const { return mNumIterations; } + void SetNumIterations(uint32 inNumIterations) { mNumIterations = inNumIterations; } + + /// Get the pressure of the soft body + float GetPressure() const { return mPressure; } + void SetPressure(float inPressure) { mPressure = inPressure; } + + /// Update the position of the body while simulating (set to false for something that is attached to the static world) + bool GetUpdatePosition() const { return mUpdatePosition; } + void SetUpdatePosition(bool inUpdatePosition) { mUpdatePosition = inUpdatePosition; } + + /// Global setting to turn on/off skin constraints + bool GetEnableSkinConstraints() const { return mEnableSkinConstraints; } + void SetEnableSkinConstraints(bool inEnableSkinConstraints) { mEnableSkinConstraints = inEnableSkinConstraints; } + + /// Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints. 0 to hard skin all vertices. + float GetSkinnedMaxDistanceMultiplier() const { return mSkinnedMaxDistanceMultiplier; } + void SetSkinnedMaxDistanceMultiplier(float inSkinnedMaxDistanceMultiplier) { mSkinnedMaxDistanceMultiplier = inSkinnedMaxDistanceMultiplier; } + + /// Get local bounding box + const AABox & GetLocalBounds() const { return mLocalBounds; } + + /// Get the volume of the soft body. Note can become negative if the shape is inside out! + float GetVolume() const { return GetVolumeTimesSix() / 6.0f; } + + /// Calculate the total mass and inertia of this body based on the current state of the vertices + void CalculateMassAndInertia(); + +#ifdef JPH_DEBUG_RENDERER + /// Draw the state of a soft body + void DrawVertices(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; + void DrawVertexVelocities(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; + void DrawEdgeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawBendConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawVolumeConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawSkinConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawLRAConstraints(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, ESoftBodyConstraintColor inConstraintColor) const; + void DrawPredictedBounds(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform) const; +#endif // JPH_DEBUG_RENDERER + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + + /// Restoring state for replay + void RestoreState(StateRecorder &inStream); + + /// Skin vertices to supplied joints, information is used by the skinned constraints. + /// @param inCenterOfMassTransform Value of Body::GetCenterOfMassTransform(). + /// @param inJointMatrices The joint matrices must be expressed relative to inCenterOfMassTransform. + /// @param inNumJoints Indicates how large the inJointMatrices array is (used only for validating out of bounds). + /// @param inHardSkinAll Can be used to position all vertices on the skinned vertices and can be used to hard reset the soft body. + /// @param ioTempAllocator Allocator. + void SkinVertices(RMat44Arg inCenterOfMassTransform, const Mat44 *inJointMatrices, uint inNumJoints, bool inHardSkinAll, TempAllocator &ioTempAllocator); + + /// This function allows you to update the soft body immediately without going through the PhysicsSystem. + /// This is useful if the soft body is teleported and needs to 'settle' or it can be used if a the soft body + /// is not added to the PhysicsSystem and needs to be updated manually. One reason for not adding it to the + /// PhyicsSystem is that you might want to update a soft body immediately after updating an animated object + /// that has the soft body attached to it. If the soft body is added to the PhysicsSystem it will be updated + /// by it, so calling this function will effectively update it twice. Note that when you use this function, + /// only the current thread will be used, whereas if you update through the PhysicsSystem, multiple threads may + /// be used. + /// Note that this will bypass any sleep checks. Since the dynamic objects that the soft body touches + /// will not move during this call, there can be simulation artifacts if you call this function multiple times + /// without running the physics simulation step. + void CustomUpdate(float inDeltaTime, Body &ioSoftBody, PhysicsSystem &inSystem); + + //////////////////////////////////////////////////////////// + // FUNCTIONS BELOW THIS LINE ARE FOR INTERNAL USE ONLY + //////////////////////////////////////////////////////////// + + /// Initialize the update context. Not part of the public API. + void InitializeUpdateContext(float inDeltaTime, Body &inSoftBody, const PhysicsSystem &inSystem, SoftBodyUpdateContext &ioContext); + + /// Do a broad phase check and collect all bodies that can possibly collide with this soft body. Not part of the public API. + void DetermineCollidingShapes(const SoftBodyUpdateContext &inContext, const PhysicsSystem &inSystem, const BodyLockInterface &inBodyLockInterface); + + /// Return code for ParallelUpdate + enum class EStatus + { + NoWork = 1 << 0, ///< No work was done because other threads were still working on a batch that cannot run concurrently + DidWork = 1 << 1, ///< Work was done to progress the update + Done = 1 << 2, ///< All work is done + }; + + /// Update the soft body, will process a batch of work. Not part of the public API. + EStatus ParallelUpdate(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Update the velocities of all rigid bodies that we collided with. Not part of the public API. + void UpdateRigidBodyVelocities(const SoftBodyUpdateContext &inContext, BodyInterface &inBodyInterface); + +private: + // SoftBodyManifold needs to have access to CollidingShape + friend class SoftBodyManifold; + + // Collect information about the colliding bodies + struct CollidingShape + { + /// Get the velocity of a point on this body + Vec3 GetPointVelocity(Vec3Arg inPointRelativeToCOM) const + { + return mLinearVelocity + mAngularVelocity.Cross(inPointRelativeToCOM); + } + + Mat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + RefConst mShape; ///< Shape of the body we hit + BodyID mBodyID; ///< Body ID of the body we hit + EMotionType mMotionType; ///< Motion type of the body we hit + bool mIsSensor; ///< If the contact should be treated as a sensor vs body contact (no collision response) + float mInvMass; ///< Inverse mass of the body we hit + float mFriction; ///< Combined friction of the two bodies + float mRestitution; ///< Combined restitution of the two bodies + float mSoftBodyInvMassScale; ///< Scale factor for the inverse mass of the soft body vertices + bool mUpdateVelocities; ///< If the linear/angular velocity changed and the body needs to be updated + Mat44 mInvInertia; ///< Inverse inertia in local space to the soft body + Vec3 mLinearVelocity; ///< Linear velocity of the body in local space to the soft body + Vec3 mAngularVelocity; ///< Angular velocity of the body in local space to the soft body + Vec3 mOriginalLinearVelocity; ///< Linear velocity of the body in local space to the soft body at start + Vec3 mOriginalAngularVelocity; ///< Angular velocity of the body in local space to the soft body at start + }; + + // Information about the state of all skinned vertices + struct SkinState + { + Vec3 mPreviousPosition = Vec3::sZero(); ///< Previous position of the skinned vertex, used to interpolate between the previous and current position + Vec3 mPosition = Vec3::sNaN(); ///< Current position of the skinned vertex + Vec3 mNormal = Vec3::sNaN(); ///< Normal of the skinned vertex + }; + + /// Do a narrow phase check and determine the closest feature that we can collide with + void DetermineCollisionPlanes(const SoftBodyUpdateContext &inContext, uint inVertexStart, uint inNumVertices); + + /// Apply pressure force and update the vertex velocities + void ApplyPressure(const SoftBodyUpdateContext &inContext); + + /// Integrate the positions of all vertices by 1 sub step + void IntegratePositions(const SoftBodyUpdateContext &inContext); + + /// Enforce all bend constraints + void ApplyDihedralBendConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all volume constraints + void ApplyVolumeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all skin constraints + void ApplySkinConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all edge constraints + void ApplyEdgeConstraints(const SoftBodyUpdateContext &inContext, uint inStartIndex, uint inEndIndex); + + /// Enforce all LRA constraints + void ApplyLRAConstraints(uint inStartIndex, uint inEndIndex); + + /// Enforce all collision constraints & update all velocities according the XPBD algorithm + void ApplyCollisionConstraintsAndUpdateVelocities(const SoftBodyUpdateContext &inContext); + + /// Update the state of the soft body (position, velocity, bounds) + void UpdateSoftBodyState(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Executes tasks that need to run on the start of an iteration (i.e. the stuff that can't run in parallel) + void StartNextIteration(const SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on batches of collision planes + EStatus ParallelDetermineCollisionPlanes(SoftBodyUpdateContext &ioContext); + + /// Helper function for ParallelUpdate that works on batches of constraints + EStatus ParallelApplyConstraints(SoftBodyUpdateContext &ioContext, const PhysicsSettings &inPhysicsSettings); + + /// Helper function to update a single group of constraints + void ProcessGroup(const SoftBodyUpdateContext &ioContext, uint inGroupIndex); + + /// Returns 6 times the volume of the soft body + float GetVolumeTimesSix() const; + +#ifdef JPH_DEBUG_RENDERER + /// Helper function to draw constraints + template + inline void DrawConstraints(ESoftBodyConstraintColor inConstraintColor, const GetEndIndex &inGetEndIndex, const DrawConstraint &inDrawConstraint, ColorArg inBaseColor) const; + + RMat44 mSkinStateTransform = RMat44::sIdentity(); ///< The matrix that transforms mSkinState to world space +#endif // JPH_DEBUG_RENDERER + + RefConst mSettings; ///< Configuration of the particles and constraints + Array mVertices; ///< Current state of all vertices in the simulation + Array mCollidingShapes; ///< List of colliding shapes retrieved during the last update + Array mSkinState; ///< List of skinned positions (1-on-1 with mVertices but only those that are used by the skinning constraints are filled in) + AABox mLocalBounds; ///< Bounding box of all vertices + AABox mLocalPredictedBounds; ///< Predicted bounding box for all vertices using extrapolation of velocity by last step delta time + uint32 mNumIterations; ///< Number of solver iterations + float mPressure; ///< n * R * T, amount of substance * ideal gas constant * absolute temperature, see https://en.wikipedia.org/wiki/Pressure + float mSkinnedMaxDistanceMultiplier = 1.0f; ///< Multiplier applied to Skinned::mMaxDistance to allow tightening or loosening of the skin constraints + bool mUpdatePosition; ///< Update the position of the body while simulating (set to false for something that is attached to the static world) + bool mHasContact = false; ///< True if the soft body has collided with anything in the last update + bool mEnableSkinConstraints = true; ///< If skin constraints are enabled + bool mSkinStatePreviousPositionValid = false; ///< True if the skinning was updated in the last update so that the previous position of the skin state is valid +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.cpp b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.cpp new file mode 100644 index 000000000..391bdef1b --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.cpp @@ -0,0 +1,338 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +uint SoftBodyShape::GetSubShapeIDBits() const +{ + // Ensure we have enough bits to encode our shape [0, n - 1] + uint32 n = (uint32)mSoftBodyMotionProperties->GetFaces().size() - 1; + return 32 - CountLeadingZeros(n); +} + +uint32 SoftBodyShape::GetFaceIndex(const SubShapeID &inSubShapeID) const +{ + SubShapeID remainder; + uint32 face_index = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + return face_index; +} + +AABox SoftBodyShape::GetLocalBounds() const +{ + return mSoftBodyMotionProperties->GetLocalBounds(); +} + +bool SoftBodyShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const +{ + JPH_PROFILE_FUNCTION(); + + uint num_triangle_bits = GetSubShapeIDBits(); + uint triangle_idx = uint(-1); + + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3); + if (fraction < ioHit.mFraction) + { + // Store fraction + ioHit.mFraction = fraction; + + // Store triangle index + triangle_idx = uint(&f - mSoftBodyMotionProperties->GetFaces().data()); + } + } + + if (triangle_idx == uint(-1)) + return false; + + ioHit.mSubShapeID2 = inSubShapeIDCreator.PushID(triangle_idx, num_triangle_bits).GetID(); + return true; +} + +void SoftBodyShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + JPH_PROFILE_FUNCTION(); + + // Test shape filter + if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID())) + return; + + uint num_triangle_bits = GetSubShapeIDBits(); + + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + // Back facing check + if (inRayCastSettings.mBackFaceMode == EBackFaceMode::IgnoreBackFaces && (x2 - x1).Cross(x3 - x1).Dot(inRay.mDirection) > 0.0f) + return; + + // Test ray against triangle + float fraction = RayTriangle(inRay.mOrigin, inRay.mDirection, x1, x2, x3); + if (fraction < ioCollector.GetEarlyOutFraction()) + { + // Better hit than the current hit + RayCastResult hit; + hit.mBodyID = TransformedShape::sGetBodyID(ioCollector.GetContext()); + hit.mFraction = fraction; + hit.mSubShapeID2 = inSubShapeIDCreator.PushID(uint(&f - mSoftBodyMotionProperties->GetFaces().data()), num_triangle_bits).GetID(); + ioCollector.AddHit(hit); + } + } +} + +void SoftBodyShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const +{ + sCollidePointUsingRayCast(*this, inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter); +} + +void SoftBodyShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const +{ + /* Not implemented */ +} + +const PhysicsMaterial *SoftBodyShape::GetMaterial(const SubShapeID &inSubShapeID) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + return mSoftBodyMotionProperties->GetMaterials()[f.mMaterialIndex]; +} + +Vec3 SoftBodyShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + return (x2 - x1).Cross(x3 - x1).NormalizedOr(Vec3::sAxisY()); +} + +void SoftBodyShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const +{ + SubShapeID remainder; + uint triangle_idx = inSubShapeID.PopID(GetSubShapeIDBits(), remainder); + JPH_ASSERT(remainder.IsEmpty()); + + const SoftBodyMotionProperties::Face &f = mSoftBodyMotionProperties->GetFace(triangle_idx); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + + for (uint32 i : f.mVertex) + outVertices.push_back(inCenterOfMassTransform * (inScale * vertices[i].mPosition)); +} + +void SoftBodyShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const +{ + outSubmergedVolume = 0.0f; + outTotalVolume = mSoftBodyMotionProperties->GetVolume(); + outCenterOfBuoyancy = Vec3::sZero(); +} + +#ifdef JPH_DEBUG_RENDERER + +void SoftBodyShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const +{ + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + for (const SoftBodyMotionProperties::Face &f : mSoftBodyMotionProperties->GetFaces()) + { + RVec3 x1 = inCenterOfMassTransform * vertices[f.mVertex[0]].mPosition; + RVec3 x2 = inCenterOfMassTransform * vertices[f.mVertex[1]].mPosition; + RVec3 x3 = inCenterOfMassTransform * vertices[f.mVertex[2]].mPosition; + + inRenderer->DrawTriangle(x1, x2, x3, inColor, DebugRenderer::ECastShadow::On); + } +} + +#endif // JPH_DEBUG_RENDERER + +struct SoftBodyShape::SBSGetTrianglesContext +{ + Mat44 mCenterOfMassTransform; + int mTriangleIndex; +}; + +void SoftBodyShape::GetTrianglesStart(GetTrianglesContext &ioContext, [[maybe_unused]] const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const +{ + SBSGetTrianglesContext &context = reinterpret_cast(ioContext); + context.mCenterOfMassTransform = Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(inScale); + context.mTriangleIndex = 0; +} + +int SoftBodyShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const +{ + SBSGetTrianglesContext &context = reinterpret_cast(ioContext); + + const Array &faces = mSoftBodyMotionProperties->GetFaces(); + const Array &vertices = mSoftBodyMotionProperties->GetVertices(); + const PhysicsMaterialList &materials = mSoftBodyMotionProperties->GetMaterials(); + + int num_triangles = min(inMaxTrianglesRequested, (int)faces.size() - context.mTriangleIndex); + for (int i = 0; i < num_triangles; ++i) + { + const SoftBodyMotionProperties::Face &f = faces[context.mTriangleIndex + i]; + + Vec3 x1 = context.mCenterOfMassTransform * vertices[f.mVertex[0]].mPosition; + Vec3 x2 = context.mCenterOfMassTransform * vertices[f.mVertex[1]].mPosition; + Vec3 x3 = context.mCenterOfMassTransform * vertices[f.mVertex[2]].mPosition; + + x1.StoreFloat3(outTriangleVertices++); + x2.StoreFloat3(outTriangleVertices++); + x3.StoreFloat3(outTriangleVertices++); + + if (outMaterials != nullptr) + *outMaterials++ = materials[f.mMaterialIndex]; + } + + context.mTriangleIndex += num_triangles; + return num_triangles; +} + +Shape::Stats SoftBodyShape::GetStats() const +{ + return Stats(sizeof(*this), (uint)mSoftBodyMotionProperties->GetFaces().size()); +} + +float SoftBodyShape::GetVolume() const +{ + return mSoftBodyMotionProperties->GetVolume(); +} + +void SoftBodyShape::sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetType() == EShapeType::Convex); + const ConvexShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape2 = static_cast(inShape2); + + const Array &vertices = shape2->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape2->GetSubShapeIDBits(); + + CollideConvexVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, [[maybe_unused]] const ShapeFilter &inShapeFilter) +{ + JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Sphere); + const SphereShape *shape1 = static_cast(inShape1); + JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape2 = static_cast(inShape2); + + const Array &vertices = shape2->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape2->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape2->GetSubShapeIDBits(); + + CollideSphereVsTriangles collider(shape1, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1.GetID(), inCollideShapeSettings, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + collider.Collide(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape = static_cast(inShape); + + const Array &vertices = shape->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape->GetSubShapeIDBits(); + + CastConvexVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, [[maybe_unused]] const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector) +{ + JPH_ASSERT(inShape->GetSubType() == EShapeSubType::SoftBody); + const SoftBodyShape *shape = static_cast(inShape); + + const Array &vertices = shape->mSoftBodyMotionProperties->GetVertices(); + const Array &faces = shape->mSoftBodyMotionProperties->GetFaces(); + uint num_triangle_bits = shape->GetSubShapeIDBits(); + + CastSphereVsTriangles caster(inShapeCast, inShapeCastSettings, inScale, inCenterOfMassTransform2, inSubShapeIDCreator1, ioCollector); + for (const SoftBodyMotionProperties::Face &f : faces) + { + Vec3 x1 = vertices[f.mVertex[0]].mPosition; + Vec3 x2 = vertices[f.mVertex[1]].mPosition; + Vec3 x3 = vertices[f.mVertex[2]].mPosition; + + caster.Cast(x1, x2, x3, 0b111, inSubShapeIDCreator2.PushID(uint(&f - faces.data()), num_triangle_bits).GetID()); + } +} + +void SoftBodyShape::sRegister() +{ + ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::SoftBody); + f.mConstruct = nullptr; // Not supposed to be constructed by users! + f.mColor = Color::sDarkGreen; + + for (EShapeSubType s : sConvexSubShapeTypes) + { + CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::SoftBody, sCollideConvexVsSoftBody); + CollisionDispatch::sRegisterCastShape(s, EShapeSubType::SoftBody, sCastConvexVsSoftBody); + } + + // Specialized collision functions + CollisionDispatch::sRegisterCollideShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCollideSphereVsSoftBody); + CollisionDispatch::sRegisterCastShape(EShapeSubType::Sphere, EShapeSubType::SoftBody, sCastSphereVsSoftBody); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.h new file mode 100644 index 000000000..605ec80a2 --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyShape.h @@ -0,0 +1,73 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +class SoftBodyMotionProperties; +class CollideShapeSettings; + +/// Shape used exclusively for soft bodies. Adds the ability to perform collision checks against soft bodies. +class JPH_EXPORT SoftBodyShape final : public Shape +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + SoftBodyShape() : Shape(EShapeType::SoftBody, EShapeSubType::SoftBody) { } + + /// Determine amount of bits needed to encode sub shape id + uint GetSubShapeIDBits() const; + + /// Convert a sub shape ID back to a face index + uint32 GetFaceIndex(const SubShapeID &inSubShapeID) const; + + // See Shape + virtual bool MustBeStatic() const override { return false; } + virtual Vec3 GetCenterOfMass() const override { return Vec3::sZero(); } + virtual AABox GetLocalBounds() const override; + virtual uint GetSubShapeIDBitsRecursive() const override { return GetSubShapeIDBits(); } + virtual float GetInnerRadius() const override { return 0.0f; } + virtual MassProperties GetMassProperties() const override { return MassProperties(); } + virtual const PhysicsMaterial * GetMaterial(const SubShapeID &inSubShapeID) const override; + virtual Vec3 GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const override; + virtual void GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const override; + virtual void GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy +#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen + , RVec3Arg inBaseOffset +#endif + ) const override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const override; +#endif // JPH_DEBUG_RENDERER + virtual bool CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const override; + virtual void CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + virtual void CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter = { }) const override; + virtual void CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, SoftBodyVertex *ioVertices, uint inNumVertices, float inDeltaTime, Vec3Arg inDisplacementDueToGravity, int inCollidingShapeIndex) const override; + virtual void GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const override; + virtual int GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials = nullptr) const override; + virtual Stats GetStats() const override; + virtual float GetVolume() const override; + + // Register shape functions with the registry + static void sRegister(); + +private: + // Helper functions called by CollisionDispatch + static void sCollideConvexVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCollideSphereVsSoftBody(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter); + static void sCastConvexVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + static void sCastSphereVsSoftBody(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector); + + struct SBSGetTrianglesContext; + + friend class BodyManager; + + const SoftBodyMotionProperties *mSoftBodyMotionProperties; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp b/WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp new file mode 100644 index 000000000..413415f9f --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp @@ -0,0 +1,1023 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_SUPPRESS_WARNINGS_STD_BEGIN +#include +JPH_SUPPRESS_WARNINGS_STD_END + +JPH_NAMESPACE_BEGIN + +template, class Compare = std::less> using PriorityQueue = std::priority_queue; + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Vertex) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mPosition) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mVelocity) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Vertex, mInvMass) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Face) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Face, mMaterialIndex) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Edge) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mRestLength) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Edge, mCompliance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::DihedralBend) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mCompliance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::DihedralBend, mInitialAngle) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Volume) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mSixRestVolume) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Volume, mCompliance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::InvBind) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mJointIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::InvBind, mInvBind) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::SkinWeight) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mInvBindIndex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::SkinWeight, mWeight) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::Skinned) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mWeights) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mMaxDistance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopDistance) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::Skinned, mBackStopRadius) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings::LRA) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mVertex) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings::LRA, mMaxDistance) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SoftBodySharedSettings) +{ + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertices) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mFaces) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mEdgeConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mDihedralBendConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVolumeConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mSkinnedConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mInvBindMatrices) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mLRAConstraints) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mMaterials) + JPH_ADD_ATTRIBUTE(SoftBodySharedSettings, mVertexRadius) +} + +void SoftBodySharedSettings::CalculateClosestKinematic() +{ + // Check if we already calculated this + if (!mClosestKinematic.empty()) + return; + + // Reserve output size + mClosestKinematic.resize(mVertices.size()); + + // Create a list of connected vertices + Array> connectivity; + connectivity.resize(mVertices.size()); + for (const Edge &e : mEdgeConstraints) + { + connectivity[e.mVertex[0]].push_back(e.mVertex[1]); + connectivity[e.mVertex[1]].push_back(e.mVertex[0]); + } + + // Use Dijkstra's algorithm to find the closest kinematic vertex for each vertex + // See: https://en.wikipedia.org/wiki/Dijkstra's_algorithm + // + // An element in the open list + struct Open + { + // Order so that we get the shortest distance first + bool operator < (const Open &inRHS) const + { + return mDistance > inRHS.mDistance; + } + + uint32 mVertex; + float mDistance; + }; + + // Start with all kinematic elements + PriorityQueue to_visit; + for (uint32 v = 0; v < mVertices.size(); ++v) + if (mVertices[v].mInvMass == 0.0f) + { + mClosestKinematic[v].mVertex = v; + mClosestKinematic[v].mDistance = 0.0f; + to_visit.push({ v, 0.0f }); + } + + // Visit all vertices remembering the closest kinematic vertex and its distance + while (!to_visit.empty()) + { + // Pop element from the open list + Open current = to_visit.top(); + to_visit.pop(); + + // Loop through all of its connected vertices + for (uint32 v : connectivity[current.mVertex]) + { + // Calculate distance from the current vertex to this target vertex and check if it is smaller + float new_distance = current.mDistance + (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current.mVertex].mPosition)).Length(); + if (new_distance < mClosestKinematic[v].mDistance) + { + // Remember new closest vertex + mClosestKinematic[v].mVertex = mClosestKinematic[current.mVertex].mVertex; + mClosestKinematic[v].mDistance = new_distance; + to_visit.push({ v, new_distance }); + } + } + } +} + +void SoftBodySharedSettings::CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType, float inAngleTolerance) +{ + struct EdgeHelper + { + uint32 mVertex[2]; + uint32 mEdgeIdx; + }; + + // Create list of all edges + Array edges; + edges.reserve(mFaces.size() * 3); + for (const Face &f : mFaces) + for (int i = 0; i < 3; ++i) + { + uint32 v0 = f.mVertex[i]; + uint32 v1 = f.mVertex[(i + 1) % 3]; + + EdgeHelper e; + e.mVertex[0] = min(v0, v1); + e.mVertex[1] = max(v0, v1); + e.mEdgeIdx = uint32(&f - mFaces.data()) * 3 + i; + edges.push_back(e); + } + + // Sort the edges + QuickSort(edges.begin(), edges.end(), [](const EdgeHelper &inLHS, const EdgeHelper &inRHS) { return inLHS.mVertex[0] < inRHS.mVertex[0] || (inLHS.mVertex[0] == inRHS.mVertex[0] && inLHS.mVertex[1] < inRHS.mVertex[1]); }); + + // Only add edges if one of the vertices is movable + auto add_edge = [this](uint32 inVtx1, uint32 inVtx2, float inCompliance1, float inCompliance2) { + if ((mVertices[inVtx1].mInvMass > 0.0f || mVertices[inVtx2].mInvMass > 0.0f) + && inCompliance1 < FLT_MAX && inCompliance2 < FLT_MAX) + { + Edge temp_edge; + temp_edge.mVertex[0] = inVtx1; + temp_edge.mVertex[1] = inVtx2; + temp_edge.mCompliance = 0.5f * (inCompliance1 + inCompliance2); + temp_edge.mRestLength = (Vec3(mVertices[inVtx2].mPosition) - Vec3(mVertices[inVtx1].mPosition)).Length(); + JPH_ASSERT(temp_edge.mRestLength > 0.0f); + mEdgeConstraints.push_back(temp_edge); + } + }; + + // Helper function to get the attributes of a vertex + auto attr = [inVertexAttributes, inVertexAttributesLength](uint32 inVertex) { + return inVertexAttributes[min(inVertex, inVertexAttributesLength - 1)]; + }; + + // Create the constraints + float sq_sin_tolerance = Square(Sin(inAngleTolerance)); + float sq_cos_tolerance = Square(Cos(inAngleTolerance)); + mEdgeConstraints.clear(); + mEdgeConstraints.reserve(edges.size()); + for (Array::size_type i = 0; i < edges.size(); ++i) + { + const EdgeHelper &e0 = edges[i]; + + // Get attributes for the vertices of the edge + const VertexAttributes &a0 = attr(e0.mVertex[0]); + const VertexAttributes &a1 = attr(e0.mVertex[1]); + + // Flag that indicates if this edge is a shear edge (if 2 triangles form a quad-like shape and this edge is on the diagonal) + bool is_shear = false; + + // Test if there are any shared edges + for (Array::size_type j = i + 1; j < edges.size(); ++j) + { + const EdgeHelper &e1 = edges[j]; + if (e0.mVertex[0] == e1.mVertex[0] && e0.mVertex[1] == e1.mVertex[1]) + { + // Get opposing vertices + const Face &f0 = mFaces[e0.mEdgeIdx / 3]; + const Face &f1 = mFaces[e1.mEdgeIdx / 3]; + uint32 vopposite0 = f0.mVertex[(e0.mEdgeIdx + 2) % 3]; + uint32 vopposite1 = f1.mVertex[(e1.mEdgeIdx + 2) % 3]; + const VertexAttributes &a_opposite0 = attr(vopposite0); + const VertexAttributes &a_opposite1 = attr(vopposite1); + + // Faces should be roughly in a plane + Vec3 n0 = (Vec3(mVertices[f0.mVertex[2]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f0.mVertex[1]].mPosition) - Vec3(mVertices[f0.mVertex[0]].mPosition)); + Vec3 n1 = (Vec3(mVertices[f1.mVertex[2]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)).Cross(Vec3(mVertices[f1.mVertex[1]].mPosition) - Vec3(mVertices[f1.mVertex[0]].mPosition)); + if (Square(n0.Dot(n1)) > sq_cos_tolerance * n0.LengthSq() * n1.LengthSq()) + { + // Faces should approximately form a quad + Vec3 e0_dir = Vec3(mVertices[vopposite0].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition); + Vec3 e1_dir = Vec3(mVertices[vopposite1].mPosition) - Vec3(mVertices[e0.mVertex[0]].mPosition); + if (Square(e0_dir.Dot(e1_dir)) < sq_sin_tolerance * e0_dir.LengthSq() * e1_dir.LengthSq()) + { + // Shear constraint + add_edge(vopposite0, vopposite1, a_opposite0.mShearCompliance, a_opposite1.mShearCompliance); + is_shear = true; + } + } + + // Bend constraint + switch (inBendType) + { + case EBendType::None: + // Do nothing + break; + + case EBendType::Distance: + // Create an edge constraint to represent the bend constraint + // Use the bend compliance of the shared edge + if (!is_shear) + add_edge(vopposite0, vopposite1, a0.mBendCompliance, a1.mBendCompliance); + break; + + case EBendType::Dihedral: + // Test if both opposite vertices are free to move + if ((mVertices[vopposite0].mInvMass > 0.0f || mVertices[vopposite1].mInvMass > 0.0f) + && a0.mBendCompliance < FLT_MAX && a1.mBendCompliance < FLT_MAX) + { + // Create a bend constraint + // Use the bend compliance of the shared edge + mDihedralBendConstraints.emplace_back(e0.mVertex[0], e0.mVertex[1], vopposite0, vopposite1, 0.5f * (a0.mBendCompliance + a1.mBendCompliance)); + } + break; + } + } + else + { + // Start iterating from the first non-shared edge + i = j - 1; + break; + } + } + + // Create a edge constraint for the current edge + add_edge(e0.mVertex[0], e0.mVertex[1], is_shear? a0.mShearCompliance : a0.mCompliance, is_shear? a1.mShearCompliance : a1.mCompliance); + } + mEdgeConstraints.shrink_to_fit(); + + // Calculate the initial angle for all bend constraints + CalculateBendConstraintConstants(); + + // Check if any vertices have LRA constraints + bool has_lra_constraints = false; + for (const VertexAttributes *va = inVertexAttributes; va < inVertexAttributes + inVertexAttributesLength; ++va) + if (va->mLRAType != ELRAType::None) + { + has_lra_constraints = true; + break; + } + if (has_lra_constraints) + { + // Ensure we have calculated the closest kinematic vertex for each vertex + CalculateClosestKinematic(); + + // Find non-kinematic vertices + for (uint32 v = 0; v < (uint32)mVertices.size(); ++v) + if (mVertices[v].mInvMass > 0.0f) + { + // Check if a closest vertex was found + uint32 closest = mClosestKinematic[v].mVertex; + if (closest != 0xffffffff) + { + // Check which LRA constraint to create + const VertexAttributes &va = attr(v); + switch (va.mLRAType) + { + case ELRAType::None: + break; + + case ELRAType::EuclideanDistance: + mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * (Vec3(mVertices[closest].mPosition) - Vec3(mVertices[v].mPosition)).Length()); + break; + + case ELRAType::GeodesicDistance: + mLRAConstraints.emplace_back(closest, v, va.mLRAMaxDistanceMultiplier * mClosestKinematic[v].mDistance); + break; + } + } + } + } +} + +void SoftBodySharedSettings::CalculateEdgeLengths() +{ + for (Edge &e : mEdgeConstraints) + { + e.mRestLength = (Vec3(mVertices[e.mVertex[1]].mPosition) - Vec3(mVertices[e.mVertex[0]].mPosition)).Length(); + JPH_ASSERT(e.mRestLength > 0.0f); + } +} + +void SoftBodySharedSettings::CalculateLRALengths(float inMaxDistanceMultiplier) +{ + for (LRA &l : mLRAConstraints) + { + l.mMaxDistance = inMaxDistanceMultiplier * (Vec3(mVertices[l.mVertex[1]].mPosition) - Vec3(mVertices[l.mVertex[0]].mPosition)).Length(); + JPH_ASSERT(l.mMaxDistance > 0.0f); + } +} + +void SoftBodySharedSettings::CalculateBendConstraintConstants() +{ + for (DihedralBend &b : mDihedralBendConstraints) + { + // Get positions + Vec3 x0 = Vec3(mVertices[b.mVertex[0]].mPosition); + Vec3 x1 = Vec3(mVertices[b.mVertex[1]].mPosition); + Vec3 x2 = Vec3(mVertices[b.mVertex[2]].mPosition); + Vec3 x3 = Vec3(mVertices[b.mVertex[3]].mPosition); + + /* + x2 + e1/ \e3 + / \ + x0----x1 + \ e0 / + e2\ /e4 + x3 + */ + + // Calculate edges + Vec3 e0 = x1 - x0; + Vec3 e1 = x2 - x0; + Vec3 e2 = x3 - x0; + + // Normals of both triangles + Vec3 n1 = e0.Cross(e1); + Vec3 n2 = e2.Cross(e0); + float denom = sqrt(n1.LengthSq() * n2.LengthSq()); + if (denom < 1.0e-12f) + b.mInitialAngle = 0.0f; + else + { + float sign = Sign(n2.Cross(n1).Dot(e0)); + b.mInitialAngle = sign * ACosApproximate(n1.Dot(n2) / denom); // Runtime uses the approximation too + } + } +} + +void SoftBodySharedSettings::CalculateVolumeConstraintVolumes() +{ + for (Volume &v : mVolumeConstraints) + { + Vec3 x1(mVertices[v.mVertex[0]].mPosition); + Vec3 x2(mVertices[v.mVertex[1]].mPosition); + Vec3 x3(mVertices[v.mVertex[2]].mPosition); + Vec3 x4(mVertices[v.mVertex[3]].mPosition); + + Vec3 x1x2 = x2 - x1; + Vec3 x1x3 = x3 - x1; + Vec3 x1x4 = x4 - x1; + + v.mSixRestVolume = abs(x1x2.Cross(x1x3).Dot(x1x4)); + } +} + +void SoftBodySharedSettings::CalculateSkinnedConstraintNormals() +{ + // Clear any previous results + mSkinnedConstraintNormals.clear(); + + // If there are no skinned constraints, we're done + if (mSkinnedConstraints.empty()) + return; + + // First collect all vertices that are skinned + UnorderedSet skinned_vertices; + skinned_vertices.reserve(mSkinnedConstraints.size()); + for (const Skinned &s : mSkinnedConstraints) + skinned_vertices.insert(s.mVertex); + + // Now collect all faces that connect only to skinned vertices + UnorderedMap> connected_faces; + connected_faces.reserve(mVertices.size()); + for (const Face &f : mFaces) + { + // Must connect to only skinned vertices + bool valid = true; + for (uint32 v : f.mVertex) + valid &= skinned_vertices.find(v) != skinned_vertices.end(); + if (!valid) + continue; + + // Store faces that connect to vertices + for (uint32 v : f.mVertex) + connected_faces[v].insert(uint32(&f - mFaces.data())); + } + + // Populate the list of connecting faces per skinned vertex + mSkinnedConstraintNormals.reserve(mFaces.size()); + for (Skinned &s : mSkinnedConstraints) + { + uint32 start = uint32(mSkinnedConstraintNormals.size()); + JPH_ASSERT((start >> 24) == 0); + const UnorderedSet &faces = connected_faces[s.mVertex]; + uint32 num = uint32(faces.size()); + JPH_ASSERT(num < 256); + mSkinnedConstraintNormals.insert(mSkinnedConstraintNormals.end(), faces.begin(), faces.end()); + QuickSort(mSkinnedConstraintNormals.begin() + start, mSkinnedConstraintNormals.begin() + start + num); + s.mNormalInfo = start + (num << 24); + } + mSkinnedConstraintNormals.shrink_to_fit(); +} + +void SoftBodySharedSettings::Optimize(OptimizationResults &outResults) +{ + // Clear any previous results + mUpdateGroups.clear(); + + // Create a list of connected vertices + struct Connection + { + uint32 mVertex; + uint32 mCount; + }; + Array> connectivity; + connectivity.resize(mVertices.size()); + auto add_connection = [&connectivity](uint inV1, uint inV2) { + for (int i = 0; i < 2; ++i) + { + bool found = false; + for (Connection &c : connectivity[inV1]) + if (c.mVertex == inV2) + { + c.mCount++; + found = true; + break; + } + if (!found) + connectivity[inV1].push_back({ inV2, 1 }); + + swap(inV1, inV2); + } + }; + for (const Edge &c : mEdgeConstraints) + add_connection(c.mVertex[0], c.mVertex[1]); + for (const LRA &c : mLRAConstraints) + add_connection(c.mVertex[0], c.mVertex[1]); + for (const DihedralBend &c : mDihedralBendConstraints) + { + add_connection(c.mVertex[0], c.mVertex[1]); + add_connection(c.mVertex[0], c.mVertex[2]); + add_connection(c.mVertex[0], c.mVertex[3]); + add_connection(c.mVertex[1], c.mVertex[2]); + add_connection(c.mVertex[1], c.mVertex[3]); + add_connection(c.mVertex[2], c.mVertex[3]); + } + for (const Volume &c : mVolumeConstraints) + { + add_connection(c.mVertex[0], c.mVertex[1]); + add_connection(c.mVertex[0], c.mVertex[2]); + add_connection(c.mVertex[0], c.mVertex[3]); + add_connection(c.mVertex[1], c.mVertex[2]); + add_connection(c.mVertex[1], c.mVertex[3]); + add_connection(c.mVertex[2], c.mVertex[3]); + } + // Skinned constraints only update 1 vertex, so we don't need special logic here + + // Maps each of the vertices to a group index + Array group_idx; + group_idx.resize(mVertices.size(), -1); + + // Which group we are currently filling and its vertices + int current_group_idx = 0; + Array current_group; + + // Start greedy algorithm to group vertices + for (;;) + { + // Find the bounding box of the ungrouped vertices + AABox bounds; + for (uint i = 0; i < (uint)mVertices.size(); ++i) + if (group_idx[i] == -1) + bounds.Encapsulate(Vec3(mVertices[i].mPosition)); + + // Determine longest and shortest axis + Vec3 bounds_size = bounds.GetSize(); + uint max_axis = bounds_size.GetHighestComponentIndex(); + uint min_axis = bounds_size.GetLowestComponentIndex(); + if (min_axis == max_axis) + min_axis = (min_axis + 1) % 3; + uint mid_axis = 3 - min_axis - max_axis; + + // Find the vertex that has the lowest value on the axis with the largest extent + uint current_vertex = UINT_MAX; + Float3 current_vertex_position { FLT_MAX, FLT_MAX, FLT_MAX }; + for (uint i = 0; i < (uint)mVertices.size(); ++i) + if (group_idx[i] == -1) + { + const Float3 &vertex_position = mVertices[i].mPosition; + float max_axis_value = vertex_position[max_axis]; + float mid_axis_value = vertex_position[mid_axis]; + float min_axis_value = vertex_position[min_axis]; + + if (max_axis_value < current_vertex_position[max_axis] + || (max_axis_value == current_vertex_position[max_axis] + && (mid_axis_value < current_vertex_position[mid_axis] + || (mid_axis_value == current_vertex_position[mid_axis] + && min_axis_value < current_vertex_position[min_axis])))) + { + current_vertex_position = mVertices[i].mPosition; + current_vertex = i; + } + } + if (current_vertex == UINT_MAX) + break; + + // Initialize the current group with 1 vertex + current_group.push_back(current_vertex); + group_idx[current_vertex] = current_group_idx; + + // Fill up the group + for (;;) + { + // Find the vertex that is most connected to the current group + uint best_vertex = UINT_MAX; + uint best_num_connections = 0; + float best_dist_sq = FLT_MAX; + for (uint i = 0; i < (uint)current_group.size(); ++i) // For all vertices in the current group + for (const Connection &c : connectivity[current_group[i]]) // For all connections to other vertices + { + uint v = c.mVertex; + if (group_idx[v] == -1) // Ungrouped vertices only + { + // Count the number of connections to this group + uint num_connections = 0; + for (const Connection &v2 : connectivity[v]) + if (group_idx[v2.mVertex] == current_group_idx) + num_connections += v2.mCount; + + // Calculate distance to group centroid + float dist_sq = (Vec3(mVertices[v].mPosition) - Vec3(mVertices[current_group.front()].mPosition)).LengthSq(); + + if (best_vertex == UINT_MAX + || num_connections > best_num_connections + || (num_connections == best_num_connections && dist_sq < best_dist_sq)) + { + best_vertex = v; + best_num_connections = num_connections; + best_dist_sq = dist_sq; + } + } + } + + // Add the best vertex to the current group + if (best_vertex != UINT_MAX) + { + current_group.push_back(best_vertex); + group_idx[best_vertex] = current_group_idx; + } + + // Create a new group? + if (current_group.size() >= SoftBodyUpdateContext::cVertexConstraintBatch // If full, yes + || (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2 && best_vertex == UINT_MAX)) // If half full and we found no connected vertex, yes + { + current_group.clear(); + current_group_idx++; + break; + } + + // If we didn't find a connected vertex, we need to find a new starting vertex + if (best_vertex == UINT_MAX) + break; + } + } + + // If the last group is more than half full, we'll keep it as a separate group, otherwise we merge it with the 'non parallel' group + if (current_group.size() > SoftBodyUpdateContext::cVertexConstraintBatch / 2) + ++current_group_idx; + + // We no longer need the current group array, free the memory + current_group.clear(); + current_group.shrink_to_fit(); + + // We're done with the connectivity list, free the memory + connectivity.clear(); + connectivity.shrink_to_fit(); + + // Assign the constraints to their groups + struct Group + { + uint GetSize() const + { + return (uint)mEdgeConstraints.size() + (uint)mLRAConstraints.size() + (uint)mDihedralBendConstraints.size() + (uint)mVolumeConstraints.size() + (uint)mSkinnedConstraints.size(); + } + + Array mEdgeConstraints; + Array mLRAConstraints; + Array mDihedralBendConstraints; + Array mVolumeConstraints; + Array mSkinnedConstraints; + }; + Array groups; + groups.resize(current_group_idx + 1); // + non parallel group + for (const Edge &e : mEdgeConstraints) + { + int g1 = group_idx[e.mVertex[0]]; + int g2 = group_idx[e.mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0); + if (g1 == g2) // In the same group + groups[g1].mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data())); + else // In different groups -> parallel group + groups.back().mEdgeConstraints.push_back(uint(&e - mEdgeConstraints.data())); + } + for (const LRA &l : mLRAConstraints) + { + int g1 = group_idx[l.mVertex[0]]; + int g2 = group_idx[l.mVertex[1]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0); + if (g1 == g2) // In the same group + groups[g1].mLRAConstraints.push_back(uint(&l - mLRAConstraints.data())); + else // In different groups -> parallel group + groups.back().mLRAConstraints.push_back(uint(&l - mLRAConstraints.data())); + } + for (const DihedralBend &d : mDihedralBendConstraints) + { + int g1 = group_idx[d.mVertex[0]]; + int g2 = group_idx[d.mVertex[1]]; + int g3 = group_idx[d.mVertex[2]]; + int g4 = group_idx[d.mVertex[3]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0); + if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group + groups[g1].mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data())); + else // In different groups -> parallel group + groups.back().mDihedralBendConstraints.push_back(uint(&d - mDihedralBendConstraints.data())); + } + for (const Volume &v : mVolumeConstraints) + { + int g1 = group_idx[v.mVertex[0]]; + int g2 = group_idx[v.mVertex[1]]; + int g3 = group_idx[v.mVertex[2]]; + int g4 = group_idx[v.mVertex[3]]; + JPH_ASSERT(g1 >= 0 && g2 >= 0 && g3 >= 0 && g4 >= 0); + if (g1 == g2 && g1 == g3 && g1 == g4) // In the same group + groups[g1].mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data())); + else // In different groups -> parallel group + groups.back().mVolumeConstraints.push_back(uint(&v - mVolumeConstraints.data())); + } + for (const Skinned &s : mSkinnedConstraints) + { + int g1 = group_idx[s.mVertex]; + JPH_ASSERT(g1 >= 0); + groups[g1].mSkinnedConstraints.push_back(uint(&s - mSkinnedConstraints.data())); + } + + // Sort the parallel groups from big to small (this means the big groups will be scheduled first and have more time to complete) + QuickSort(groups.begin(), groups.end() - 1, [](const Group &inLHS, const Group &inRHS) { return inLHS.GetSize() > inRHS.GetSize(); }); + + // Make sure we know the closest kinematic vertex so we can sort + CalculateClosestKinematic(); + + // Sort within each group + for (Group &group : groups) + { + // Sort the edge constraints + QuickSort(group.mEdgeConstraints.begin(), group.mEdgeConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Edge &e1 = mEdgeConstraints[inLHS]; + const Edge &e2 = mEdgeConstraints[inRHS]; + + // First sort so that the edge with the smallest distance to a kinematic vertex comes first + float d1 = min(mClosestKinematic[e1.mVertex[0]].mDistance, mClosestKinematic[e1.mVertex[1]].mDistance); + float d2 = min(mClosestKinematic[e2.mVertex[0]].mDistance, mClosestKinematic[e2.mVertex[1]].mDistance); + if (d1 != d2) + return d1 < d2; + + // Order the edges so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges). + // Note we could also re-order the vertices but that would be much more of a burden to the end user + uint32 m1 = e1.GetMinVertexIndex(); + uint32 m2 = e2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the LRA constraints + QuickSort(group.mLRAConstraints.begin(), group.mLRAConstraints.end(), [this](uint inLHS, uint inRHS) + { + const LRA &l1 = mLRAConstraints[inLHS]; + const LRA &l2 = mLRAConstraints[inRHS]; + + // First sort so that the longest constraint comes first (meaning the shortest constraint has the most influence on the end result) + // Most of the time there will be a single LRA constraint per vertex and since the LRA constraint only modifies a single vertex, + // updating one constraint will not violate another constraint. + if (l1.mMaxDistance != l2.mMaxDistance) + return l1.mMaxDistance > l2.mMaxDistance; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = l1.GetMinVertexIndex(); + uint32 m2 = l2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the dihedral bend constraints + QuickSort(group.mDihedralBendConstraints.begin(), group.mDihedralBendConstraints.end(), [this](uint inLHS, uint inRHS) + { + const DihedralBend &b1 = mDihedralBendConstraints[inLHS]; + const DihedralBend &b2 = mDihedralBendConstraints[inRHS]; + + // First sort so that the constraint with the smallest distance to a kinematic vertex comes first + float d1 = min( + min(mClosestKinematic[b1.mVertex[0]].mDistance, mClosestKinematic[b1.mVertex[1]].mDistance), + min(mClosestKinematic[b1.mVertex[2]].mDistance, mClosestKinematic[b1.mVertex[3]].mDistance)); + float d2 = min( + min(mClosestKinematic[b2.mVertex[0]].mDistance, mClosestKinematic[b2.mVertex[1]].mDistance), + min(mClosestKinematic[b2.mVertex[2]].mDistance, mClosestKinematic[b2.mVertex[3]].mDistance)); + if (d1 != d2) + return d1 < d2; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = b1.GetMinVertexIndex(); + uint32 m2 = b2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the volume constraints + QuickSort(group.mVolumeConstraints.begin(), group.mVolumeConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Volume &v1 = mVolumeConstraints[inLHS]; + const Volume &v2 = mVolumeConstraints[inRHS]; + + // First sort so that the constraint with the smallest distance to a kinematic vertex comes first + float d1 = min( + min(mClosestKinematic[v1.mVertex[0]].mDistance, mClosestKinematic[v1.mVertex[1]].mDistance), + min(mClosestKinematic[v1.mVertex[2]].mDistance, mClosestKinematic[v1.mVertex[3]].mDistance)); + float d2 = min( + min(mClosestKinematic[v2.mVertex[0]].mDistance, mClosestKinematic[v2.mVertex[1]].mDistance), + min(mClosestKinematic[v2.mVertex[2]].mDistance, mClosestKinematic[v2.mVertex[3]].mDistance)); + if (d1 != d2) + return d1 < d2; + + // Order constraints so that the ones with the smallest index go first + uint32 m1 = v1.GetMinVertexIndex(); + uint32 m2 = v2.GetMinVertexIndex(); + if (m1 != m2) + return m1 < m2; + + return inLHS < inRHS; + }); + + // Sort the skinned constraints + QuickSort(group.mSkinnedConstraints.begin(), group.mSkinnedConstraints.end(), [this](uint inLHS, uint inRHS) + { + const Skinned &s1 = mSkinnedConstraints[inLHS]; + const Skinned &s2 = mSkinnedConstraints[inRHS]; + + // Order the skinned constraints so that the ones with the smallest index go first (hoping to get better cache locality when we process the edges). + if (s1.mVertex != s2.mVertex) + return s1.mVertex < s2.mVertex; + + return inLHS < inRHS; + }); + } + + // Temporary store constraints as we reorder them + Array temp_edges; + temp_edges.swap(mEdgeConstraints); + mEdgeConstraints.reserve(temp_edges.size()); + outResults.mEdgeRemap.reserve(temp_edges.size()); + + Array temp_lra; + temp_lra.swap(mLRAConstraints); + mLRAConstraints.reserve(temp_lra.size()); + outResults.mLRARemap.reserve(temp_lra.size()); + + Array temp_dihedral_bend; + temp_dihedral_bend.swap(mDihedralBendConstraints); + mDihedralBendConstraints.reserve(temp_dihedral_bend.size()); + outResults.mDihedralBendRemap.reserve(temp_dihedral_bend.size()); + + Array temp_volume; + temp_volume.swap(mVolumeConstraints); + mVolumeConstraints.reserve(temp_volume.size()); + outResults.mVolumeRemap.reserve(temp_volume.size()); + + Array temp_skinned; + temp_skinned.swap(mSkinnedConstraints); + mSkinnedConstraints.reserve(temp_skinned.size()); + outResults.mSkinnedRemap.reserve(temp_skinned.size()); + + // Finalize update groups + for (const Group &group : groups) + { + // Reorder edge constraints for this group + for (uint idx : group.mEdgeConstraints) + { + mEdgeConstraints.push_back(temp_edges[idx]); + outResults.mEdgeRemap.push_back(idx); + } + + // Reorder LRA constraints for this group + for (uint idx : group.mLRAConstraints) + { + mLRAConstraints.push_back(temp_lra[idx]); + outResults.mLRARemap.push_back(idx); + } + + // Reorder dihedral bend constraints for this group + for (uint idx : group.mDihedralBendConstraints) + { + mDihedralBendConstraints.push_back(temp_dihedral_bend[idx]); + outResults.mDihedralBendRemap.push_back(idx); + } + + // Reorder volume constraints for this group + for (uint idx : group.mVolumeConstraints) + { + mVolumeConstraints.push_back(temp_volume[idx]); + outResults.mVolumeRemap.push_back(idx); + } + + // Reorder skinned constraints for this group + for (uint idx : group.mSkinnedConstraints) + { + mSkinnedConstraints.push_back(temp_skinned[idx]); + outResults.mSkinnedRemap.push_back(idx); + } + + // Store end indices + mUpdateGroups.push_back({ (uint)mEdgeConstraints.size(), (uint)mLRAConstraints.size(), (uint)mDihedralBendConstraints.size(), (uint)mVolumeConstraints.size(), (uint)mSkinnedConstraints.size() }); + } + + // Free closest kinematic buffer + mClosestKinematic.clear(); + mClosestKinematic.shrink_to_fit(); +} + +Ref SoftBodySharedSettings::Clone() const +{ + Ref clone = new SoftBodySharedSettings; + clone->mVertices = mVertices; + clone->mFaces = mFaces; + clone->mEdgeConstraints = mEdgeConstraints; + clone->mDihedralBendConstraints = mDihedralBendConstraints; + clone->mVolumeConstraints = mVolumeConstraints; + clone->mSkinnedConstraints = mSkinnedConstraints; + clone->mSkinnedConstraintNormals = mSkinnedConstraintNormals; + clone->mInvBindMatrices = mInvBindMatrices; + clone->mLRAConstraints = mLRAConstraints; + clone->mMaterials = mMaterials; + clone->mVertexRadius = mVertexRadius; + clone->mUpdateGroups = mUpdateGroups; + return clone; +} + +void SoftBodySharedSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mVertices); + inStream.Write(mFaces); + inStream.Write(mEdgeConstraints); + inStream.Write(mDihedralBendConstraints); + inStream.Write(mVolumeConstraints); + inStream.Write(mSkinnedConstraints); + inStream.Write(mSkinnedConstraintNormals); + inStream.Write(mLRAConstraints); + inStream.Write(mVertexRadius); + inStream.Write(mUpdateGroups); + + // Can't write mInvBindMatrices directly because the class contains padding + inStream.Write(mInvBindMatrices, [](const InvBind &inElement, StreamOut &inS) { + inS.Write(inElement.mJointIndex); + inS.Write(inElement.mInvBind); + }); +} + +void SoftBodySharedSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mVertices); + inStream.Read(mFaces); + inStream.Read(mEdgeConstraints); + inStream.Read(mDihedralBendConstraints); + inStream.Read(mVolumeConstraints); + inStream.Read(mSkinnedConstraints); + inStream.Read(mSkinnedConstraintNormals); + inStream.Read(mLRAConstraints); + inStream.Read(mVertexRadius); + inStream.Read(mUpdateGroups); + + inStream.Read(mInvBindMatrices, [](StreamIn &inS, InvBind &outElement) { + inS.Read(outElement.mJointIndex); + inS.Read(outElement.mInvBind); + }); +} + +void SoftBodySharedSettings::SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const +{ + SharedSettingsToIDMap::const_iterator settings_iter = ioSettingsMap.find(this); + if (settings_iter == ioSettingsMap.end()) + { + // Write settings ID + uint32 settings_id = (uint32)ioSettingsMap.size(); + ioSettingsMap[this] = settings_id; + inStream.Write(settings_id); + + // Write the settings + SaveBinaryState(inStream); + + // Write materials + StreamUtils::SaveObjectArray(inStream, mMaterials, &ioMaterialMap); + } + else + { + // Known settings, just write the ID + inStream.Write(settings_iter->second); + } +} + +SoftBodySharedSettings::SettingsResult SoftBodySharedSettings::sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap) +{ + SettingsResult result; + + // Read settings id + uint32 settings_id; + inStream.Read(settings_id); + if (inStream.IsEOF() || inStream.IsFailed()) + { + result.SetError("Failed to read settings id"); + return result; + } + + // Check nullptr settings + if (settings_id == ~uint32(0)) + { + result.Set(nullptr); + return result; + } + + // Check if we already read this settings + if (settings_id < ioSettingsMap.size()) + { + result.Set(ioSettingsMap[settings_id]); + return result; + } + + // Create new object + Ref settings = new SoftBodySharedSettings; + + // Read state + settings->RestoreBinaryState(inStream); + + // Read materials + Result mlresult = StreamUtils::RestoreObjectArray(inStream, ioMaterialMap); + if (mlresult.HasError()) + { + result.SetError(mlresult.GetError()); + return result; + } + settings->mMaterials = mlresult.Get(); + + // Add the settings to the map + ioSettingsMap.push_back(settings); + + result.Set(settings); + return result; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.h new file mode 100644 index 000000000..1c72128dc --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodySharedSettings.h @@ -0,0 +1,335 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// This class defines the setup of all particles and their constraints. +/// It is used during the simulation and can be shared between multiple soft bodies. +class JPH_EXPORT SoftBodySharedSettings : public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SoftBodySharedSettings) + + /// Which type of bend constraint should be created + enum class EBendType + { + None, ///< No bend constraints will be created + Distance, ///< A simple distance constraint + Dihedral, ///< A dihedral bend constraint (most expensive, but also supports triangles that are initially not in the same plane) + }; + + /// The type of long range attachment constraint to create + enum class ELRAType + { + None, ///< Don't create a LRA constraint + EuclideanDistance, ///< Create a LRA constraint based on Euclidean distance between the closest kinematic vertex and this vertex + GeodesicDistance, ///< Create a LRA constraint based on the geodesic distance between the closest kinematic vertex and this vertex (follows the edge constraints) + }; + + /// Per vertex attributes used during the CreateConstraints function. + /// For an edge or shear constraint, the compliance is averaged between the two attached vertices. + /// For a bend constraint, the compliance is averaged between the two vertices on the shared edge. + struct JPH_EXPORT VertexAttributes + { + /// Constructor + VertexAttributes() = default; + VertexAttributes(float inCompliance, float inShearCompliance, float inBendCompliance, ELRAType inLRAType = ELRAType::None, float inLRAMaxDistanceMultiplier = 1.0f) : mCompliance(inCompliance), mShearCompliance(inShearCompliance), mBendCompliance(inBendCompliance), mLRAType(inLRAType), mLRAMaxDistanceMultiplier(inLRAMaxDistanceMultiplier) { } + + float mCompliance = 0.0f; ///< The compliance of the normal edges. Set to FLT_MAX to disable regular edges for any edge involving this vertex. + float mShearCompliance = 0.0f; ///< The compliance of the shear edges. Set to FLT_MAX to disable shear edges for any edge involving this vertex. + float mBendCompliance = FLT_MAX; ///< The compliance of the bend edges. Set to FLT_MAX to disable bend edges for any bend constraint involving this vertex. + ELRAType mLRAType = ELRAType::None; ///< The type of long range attachment constraint to create. + float mLRAMaxDistanceMultiplier = 1.0f; ///< Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose. + }; + + /// Automatically create constraints based on the faces of the soft body + /// @param inVertexAttributes A list of attributes for each vertex (1-on-1 with mVertices, note that if the list is smaller than mVertices the last element will be repeated). This defines the properties of the constraints that are created. + /// @param inVertexAttributesLength The length of inVertexAttributes + /// @param inBendType The type of bend constraint to create + /// @param inAngleTolerance Shear edges are created when two connected triangles form a quad (are roughly in the same plane and form a square with roughly 90 degree angles). This defines the tolerance (in radians). + void CreateConstraints(const VertexAttributes *inVertexAttributes, uint inVertexAttributesLength, EBendType inBendType = EBendType::Distance, float inAngleTolerance = DegreesToRadians(8.0f)); + + /// Calculate the initial lengths of all springs of the edges of this soft body (if you use CreateConstraint, this is already done) + void CalculateEdgeLengths(); + + /// Calculate the max lengths for the long range attachment constraints based on Euclidean distance (if you use CreateConstraints, this is already done) + /// @param inMaxDistanceMultiplier Multiplier for the max distance of the LRA constraint, e.g. 1.01 means the max distance is 1% longer than the calculated distance in the rest pose. + void CalculateLRALengths(float inMaxDistanceMultiplier = 1.0f); + + /// Calculate the constants for the bend constraints (if you use CreateConstraints, this is already done) + void CalculateBendConstraintConstants(); + + /// Calculates the initial volume of all tetrahedra of this soft body + void CalculateVolumeConstraintVolumes(); + + /// Calculate information needed to be able to calculate the skinned constraint normals at run-time + void CalculateSkinnedConstraintNormals(); + + /// Information about the optimization of the soft body, the indices of certain elements may have changed. + class OptimizationResults + { + public: + Array mEdgeRemap; ///< Maps old edge index to new edge index + Array mLRARemap; ///< Maps old LRA index to new LRA index + Array mDihedralBendRemap; ///< Maps old dihedral bend index to new dihedral bend index + Array mVolumeRemap; ///< Maps old volume constraint index to new volume constraint index + Array mSkinnedRemap; ///< Maps old skinned constraint index to new skinned constraint index + }; + + /// Optimize the soft body settings for simulation. This will reorder constraints so they can be executed in parallel. + void Optimize(OptimizationResults &outResults); + + /// Optimize the soft body settings without results + void Optimize() { OptimizationResults results; Optimize(results); } + + /// Clone this object + Ref Clone() const; + + /// Saves the state of this object in binary form to inStream. Doesn't store the material list. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. Doesn't restore the material list. + void RestoreBinaryState(StreamIn &inStream); + + using SharedSettingsToIDMap = StreamUtils::ObjectToIDMap; + using IDToSharedSettingsMap = StreamUtils::IDToObjectMap; + using MaterialToIDMap = StreamUtils::ObjectToIDMap; + using IDToMaterialMap = StreamUtils::IDToObjectMap; + + /// Save this shared settings and its materials. Pass in an empty map ioSettingsMap / ioMaterialMap or reuse the same map while saving multiple settings objects to the same stream in order to avoid writing duplicates. + void SaveWithMaterials(StreamOut &inStream, SharedSettingsToIDMap &ioSettingsMap, MaterialToIDMap &ioMaterialMap) const; + + using SettingsResult = Result>; + + /// Restore a shape and materials. Pass in an empty map in ioSettingsMap / ioMaterialMap or reuse the same map while reading multiple settings objects from the same stream in order to restore duplicates. + static SettingsResult sRestoreWithMaterials(StreamIn &inStream, IDToSharedSettingsMap &ioSettingsMap, IDToMaterialMap &ioMaterialMap); + + /// A vertex is a particle, the data in this structure is only used during creation of the soft body and not during simulation + struct JPH_EXPORT Vertex + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Vertex) + + /// Constructor + Vertex() = default; + Vertex(const Float3 &inPosition, const Float3 &inVelocity = Float3(0, 0, 0), float inInvMass = 1.0f) : mPosition(inPosition), mVelocity(inVelocity), mInvMass(inInvMass) { } + + Float3 mPosition { 0, 0, 0 }; ///< Initial position of the vertex + Float3 mVelocity { 0, 0, 0 }; ///< Initial velocity of the vertex + float mInvMass = 1.0f; ///< Initial inverse of the mass of the vertex + }; + + /// A face defines the surface of the body + struct JPH_EXPORT Face + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Face) + + /// Constructor + Face() = default; + Face(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inMaterialIndex = 0) : mVertex { inVertex1, inVertex2, inVertex3 }, mMaterialIndex(inMaterialIndex) { } + + /// Check if this is a degenerate face (a face which points to the same vertex twice) + bool IsDegenerate() const { return mVertex[0] == mVertex[1] || mVertex[0] == mVertex[2] || mVertex[1] == mVertex[2]; } + + uint32 mVertex[3]; ///< Indices of the vertices that form the face + uint32 mMaterialIndex = 0; ///< Index of the material of the face in SoftBodySharedSettings::mMaterials + }; + + /// An edge keeps two vertices at a constant distance using a spring: |x1 - x2| = rest length + struct JPH_EXPORT Edge + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Edge) + + /// Constructor + Edge() = default; + Edge(uint32 inVertex1, uint32 inVertex2, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(mVertex[0], mVertex[1]); } + + uint32 mVertex[2]; ///< Indices of the vertices that form the edge + float mRestLength = 1.0f; ///< Rest length of the spring + float mCompliance = 0.0f; ///< Inverse of the stiffness of the spring + }; + + /** + * A dihedral bend constraint keeps the angle between two triangles constant along their shared edge. + * + * x2 + * / \ + * / t0 \ + * x0----x1 + * \ t1 / + * \ / + * x3 + * + * x0..x3 are the vertices, t0 and t1 are the triangles that share the edge x0..x1 + * + * Based on: + * - "Position Based Dynamics" - Matthias Muller et al. + * - "Strain Based Dynamics" - Matthias Muller et al. + * - "Simulation of Clothing with Folds and Wrinkles" - R. Bridson et al. + */ + struct JPH_EXPORT DihedralBend + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, DihedralBend) + + /// Constructor + DihedralBend() = default; + DihedralBend(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); } + + uint32 mVertex[4]; ///< Indices of the vertices of the 2 triangles that share an edge (the first 2 vertices are the shared edge) + float mCompliance = 0.0f; ///< Inverse of the stiffness of the constraint + float mInitialAngle = 0.0f; ///< Initial angle between the normals of the triangles (pi - dihedral angle). + }; + + /// Volume constraint, keeps the volume of a tetrahedron constant + struct JPH_EXPORT Volume + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Volume) + + /// Constructor + Volume() = default; + Volume(uint32 inVertex1, uint32 inVertex2, uint32 inVertex3, uint32 inVertex4, float inCompliance = 0.0f) : mVertex { inVertex1, inVertex2, inVertex3, inVertex4 }, mCompliance(inCompliance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(min(mVertex[0], mVertex[1]), min(mVertex[2], mVertex[3])); } + + uint32 mVertex[4]; ///< Indices of the vertices that form the tetrhedron + float mSixRestVolume = 1.0f; ///< 6 times the rest volume of the tetrahedron (calculated by CalculateVolumeConstraintVolumes()) + float mCompliance = 0.0f; ///< Inverse of the stiffness of the constraint + }; + + /// An inverse bind matrix take a skinned vertex from its bind pose into joint local space + class JPH_EXPORT InvBind + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, InvBind) + + public: + /// Constructor + InvBind() = default; + InvBind(uint32 inJointIndex, Mat44Arg inInvBind) : mJointIndex(inJointIndex), mInvBind(inInvBind) { } + + uint32 mJointIndex = 0; ///< Joint index to which this is attached + Mat44 mInvBind = Mat44::sIdentity(); ///< The inverse bind matrix, this takes a vertex in its bind pose (Vertex::mPosition) to joint local space + }; + + /// A joint and its skin weight + class JPH_EXPORT SkinWeight + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkinWeight) + + public: + /// Constructor + SkinWeight() = default; + SkinWeight(uint32 inInvBindIndex, float inWeight) : mInvBindIndex(inInvBindIndex), mWeight(inWeight) { } + + uint32 mInvBindIndex = 0; ///< Index in mInvBindMatrices + float mWeight = 0.0f; ///< Weight with which it is skinned + }; + + /// A constraint that skins a vertex to joints and limits the distance that the simulated vertex can travel from this vertex + class JPH_EXPORT Skinned + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skinned) + + public: + /// Constructor + Skinned() = default; + Skinned(uint32 inVertex, float inMaxDistance, float inBackStopDistance, float inBackStopRadius) : mVertex(inVertex), mMaxDistance(inMaxDistance), mBackStopDistance(inBackStopDistance), mBackStopRadius(inBackStopRadius) { } + + /// Normalize the weights so that they add up to 1 + void NormalizeWeights() + { + // Get the total weight + float total = 0.0f; + for (const SkinWeight &w : mWeights) + total += w.mWeight; + + // Normalize + if (total > 0.0f) + for (SkinWeight &w : mWeights) + w.mWeight /= total; + } + + /// Maximum number of skin weights + static constexpr uint cMaxSkinWeights = 4; + + uint32 mVertex = 0; ///< Index in mVertices which indicates which vertex is being skinned + SkinWeight mWeights[cMaxSkinWeights]; ///< Skin weights, the bind pose of the vertex is assumed to be stored in Vertex::mPosition. The first weight that is zero indicates the end of the list. Weights should add up to 1. + float mMaxDistance = FLT_MAX; ///< Maximum distance that this vertex can reach from the skinned vertex, disabled when FLT_MAX. 0 when you want to hard skin the vertex to the skinned vertex. + float mBackStopDistance = FLT_MAX; ///< Disabled if mBackStopDistance >= mMaxDistance. The faces surrounding mVertex determine an average normal. mBackStopDistance behind the vertex in the opposite direction of this normal, the back stop sphere starts. The simulated vertex will be pushed out of this sphere and it can be used to approximate the volume of the skinned mesh behind the skinned vertex. + float mBackStopRadius = 40.0f; ///< Radius of the backstop sphere. By default this is a fairly large radius so the sphere approximates a plane. + uint32 mNormalInfo = 0; ///< Information needed to calculate the normal of this vertex, lowest 24 bit is start index in mSkinnedConstraintNormals, highest 8 bit is number of faces (generated by CalculateSkinnedConstraintNormals()) + }; + + /// A long range attachment constraint, this is a constraint that sets a max distance between a kinematic vertex and a dynamic vertex + /// See: "Long Range Attachments - A Method to Simulate Inextensible Clothing in Computer Games", Tae-Yong Kim, Nuttapong Chentanez and Matthias Mueller-Fischer + class JPH_EXPORT LRA + { + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, LRA) + + public: + /// Constructor + LRA() = default; + LRA(uint32 inVertex1, uint32 inVertex2, float inMaxDistance) : mVertex { inVertex1, inVertex2 }, mMaxDistance(inMaxDistance) { } + + /// Return the lowest vertex index of this constraint + uint32 GetMinVertexIndex() const { return min(mVertex[0], mVertex[1]); } + + uint32 mVertex[2]; ///< The vertices that are connected. The first vertex should be kinematic, the 2nd dynamic. + float mMaxDistance = 0.0f; ///< The maximum distance between the vertices + }; + + /// Add a face to this soft body + void AddFace(const Face &inFace) { JPH_ASSERT(!inFace.IsDegenerate()); mFaces.push_back(inFace); } + + Array mVertices; ///< The list of vertices or particles of the body + Array mFaces; ///< The list of faces of the body + Array mEdgeConstraints; ///< The list of edges or springs of the body + Array mDihedralBendConstraints; ///< The list of dihedral bend constraints of the body + Array mVolumeConstraints; ///< The list of volume constraints of the body that keep the volume of tetrahedra in the soft body constant + Array mSkinnedConstraints; ///< The list of vertices that are constrained to a skinned vertex + Array mInvBindMatrices; ///< The list of inverse bind matrices for skinning vertices + Array mLRAConstraints; ///< The list of long range attachment constraints + PhysicsMaterialList mMaterials { PhysicsMaterial::sDefault }; ///< The materials of the faces of the body, referenced by Face::mMaterialIndex + float mVertexRadius = 0.0f; ///< How big the particles are, can be used to push the vertices a little bit away from the surface of other bodies to prevent z-fighting + +private: + friend class SoftBodyMotionProperties; + + /// Calculate the closest kinematic vertex array + void CalculateClosestKinematic(); + + /// Tracks the closest kinematic vertex + struct ClosestKinematic + { + uint32 mVertex = 0xffffffff; ///< Vertex index of closest kinematic vertex + float mDistance = FLT_MAX; ///< Distance to the closest kinematic vertex + }; + + /// Tracks the end indices of the various constraint groups + struct UpdateGroup + { + uint mEdgeEndIndex; ///< The end index of the edge constraints in this group + uint mLRAEndIndex; ///< The end index of the LRA constraints in this group + uint mDihedralBendEndIndex; ///< The end index of the dihedral bend constraints in this group + uint mVolumeEndIndex; ///< The end index of the volume constraints in this group + uint mSkinnedEndIndex; ///< The end index of the skinned constraints in this group + }; + + Array mClosestKinematic; ///< The closest kinematic vertex to each vertex in mVertices + Array mUpdateGroups; ///< The end indices for each group of constraints that can be updated in parallel + Array mSkinnedConstraintNormals; ///< A list of indices in the mFaces array used by mSkinnedConstraints, calculated by CalculateSkinnedConstraintNormals() +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h new file mode 100644 index 000000000..df8d108b0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyUpdateContext.h @@ -0,0 +1,54 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class SoftBodyMotionProperties; +class SoftBodyContactListener; + +/// Temporary data used by the update of a soft body +class SoftBodyUpdateContext : public NonCopyable +{ +public: + static constexpr uint cVertexCollisionBatch = 64; ///< Number of vertices to process in a batch in DetermineCollisionPlanes + static constexpr uint cVertexConstraintBatch = 256; ///< Number of vertices to group for processing batches of constraints in ApplyEdgeConstraints + + // Input + Body * mBody; ///< Body that is being updated + SoftBodyMotionProperties * mMotionProperties; ///< Motion properties of that body + SoftBodyContactListener * mContactListener; ///< Contact listener to fire callbacks to + RMat44 mCenterOfMassTransform; ///< Transform of the body relative to the soft body + Vec3 mGravity; ///< Gravity vector in local space of the soft body + Vec3 mDisplacementDueToGravity; ///< Displacement of the center of mass due to gravity in the current time step + float mDeltaTime; ///< Delta time for the current time step + float mSubStepDeltaTime; ///< Delta time for each sub step + + /// Describes progress in the current update + enum class EState + { + DetermineCollisionPlanes, ///< Determine collision planes for vertices in parallel + ApplyConstraints, ///< Apply constraints in parallel + Done ///< Update is finished + }; + + // State of the update + atomic mState { EState::DetermineCollisionPlanes };///< Current state of the update + atomic mNextCollisionVertex { 0 }; ///< Next vertex to process for DetermineCollisionPlanes + atomic mNumCollisionVerticesProcessed { 0 }; ///< Number of vertices processed by DetermineCollisionPlanes, used to determine if we can start simulating + atomic mNextIteration { 0 }; ///< Next simulation iteration to process + atomic mNextConstraintGroup { 0 }; ///< Next constraint group to process + atomic mNumConstraintGroupsProcessed { 0 }; ///< Number of groups processed, used to determine if we can go to the next iteration + + // Output + Vec3 mDeltaPosition; ///< Delta position of the body in the current time step, should be applied after the update + ECanSleep mCanSleep; ///< Can the body sleep? Should be applied after the update +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/SoftBody/SoftBodyVertex.h b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyVertex.h new file mode 100644 index 000000000..34e7bb32f --- /dev/null +++ b/WickedEngine/Jolt/Physics/SoftBody/SoftBodyVertex.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Run time information for a single particle of a soft body +/// Note that at run-time you should only modify the inverse mass and/or velocity of a vertex to control the soft body. +/// Modifying the position can lead to missed collisions. +/// The other members are used internally by the soft body solver. +class SoftBodyVertex +{ +public: + Vec3 mPreviousPosition; ///< Position at the previous time step + Vec3 mPosition; ///< Position, relative to the center of mass of the soft body + Vec3 mVelocity; ///< Velocity, relative to the center of mass of the soft body + Plane mCollisionPlane; ///< Nearest collision plane, relative to the center of mass of the soft body + int mCollidingShapeIndex; ///< Index in the colliding shapes list of the body we may collide with + bool mHasContact; ///< True if the vertex has collided with anything in the last update + float mLargestPenetration; ///< Used while finding the collision plane, stores the largest penetration found so far + float mInvMass; ///< Inverse mass (1 / mass) +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/StateRecorder.h b/WickedEngine/Jolt/Physics/StateRecorder.h new file mode 100644 index 000000000..7ec0844e6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/StateRecorder.h @@ -0,0 +1,66 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class Body; +class Constraint; +class BodyID; + +/// A bit field that determines which aspects of the simulation to save +enum class EStateRecorderState : uint8 +{ + None = 0, ///< Save nothing + Global = 1, ///< Save global physics system state (delta time, gravity, etc.) + Bodies = 2, ///< Save the state of bodies + Contacts = 4, ///< Save the state of contacts + Constraints = 8, ///< Save the state of constraints + All = Global | Bodies | Contacts | Constraints ///< Save all state +}; + +/// User callbacks that allow determining which parts of the simulation should be saved by a StateRecorder +class JPH_EXPORT StateRecorderFilter +{ +public: + /// Destructor + virtual ~StateRecorderFilter() = default; + + /// If the state of a specific body should be saved + virtual bool ShouldSaveBody([[maybe_unused]] const Body &inBody) const { return true; } + + /// If the state of a specific constraint should be saved + virtual bool ShouldSaveConstraint([[maybe_unused]] const Constraint &inConstraint) const { return true; } + + /// If the state of a specific contact should be saved + virtual bool ShouldSaveContact([[maybe_unused]] const BodyID &inBody1, [[maybe_unused]] const BodyID &inBody2) const { return true; } +}; + +/// Class that records the state of a physics system. Can be used to check if the simulation is deterministic by putting the recorder in validation mode. +/// Can be used to restore the state to an earlier point in time. Note that only the state that is modified by the simulation is saved, configuration settings +/// like body friction or restitution, motion quality etc. are not saved and need to be saved by the user if desired. +class JPH_EXPORT StateRecorder : public StreamIn, public StreamOut +{ +public: + /// Constructor + StateRecorder() = default; + StateRecorder(const StateRecorder &inRHS) : mIsValidating(inRHS.mIsValidating) { } + + /// Sets the stream in validation mode. In this case the physics system ensures that before it calls ReadBytes that it will + /// ensure that those bytes contain the current state. This makes it possible to step and save the state, restore to the previous + /// step and step again and when the recorded state is not the same it can restore the expected state and any byte that changes + /// due to a ReadBytes function can be caught to find out which part of the simulation is not deterministic. + /// Note that validation only works when saving the full state of the simulation (EStateRecorderState::All, StateRecorderFilter == nullptr). + void SetValidating(bool inValidating) { mIsValidating = inValidating; } + bool IsValidating() const { return mIsValidating; } + +private: + bool mIsValidating = false; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/StateRecorderImpl.cpp b/WickedEngine/Jolt/Physics/StateRecorderImpl.cpp new file mode 100644 index 000000000..762413589 --- /dev/null +++ b/WickedEngine/Jolt/Physics/StateRecorderImpl.cpp @@ -0,0 +1,90 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void StateRecorderImpl::WriteBytes(const void *inData, size_t inNumBytes) +{ + mStream.write((const char *)inData, inNumBytes); +} + +void StateRecorderImpl::Rewind() +{ + mStream.seekg(0, std::stringstream::beg); +} + +void StateRecorderImpl::Clear() +{ + mStream.clear(); + mStream.str({}); +} + +void StateRecorderImpl::ReadBytes(void *outData, size_t inNumBytes) +{ + if (IsValidating()) + { + // Read data in temporary buffer to compare with current value + void *data = JPH_STACK_ALLOC(inNumBytes); + mStream.read((char *)data, inNumBytes); + if (memcmp(data, outData, inNumBytes) != 0) + { + // Mismatch, print error + Trace("Mismatch reading %u bytes", (uint)inNumBytes); + for (size_t i = 0; i < inNumBytes; ++i) + { + int b1 = reinterpret_cast(outData)[i]; + int b2 = reinterpret_cast(data)[i]; + if (b1 != b2) + Trace("Offset %d: %02X -> %02X", i, b1, b2); + } + JPH_BREAKPOINT; + } + + // Copy the temporary data to the final destination + memcpy(outData, data, inNumBytes); + return; + } + + mStream.read((char *)outData, inNumBytes); +} + +bool StateRecorderImpl::IsEqual(StateRecorderImpl &inReference) +{ + // Get length of new state + mStream.seekg(0, std::stringstream::end); + std::streamoff this_len = mStream.tellg(); + mStream.seekg(0, std::stringstream::beg); + + // Get length of old state + inReference.mStream.seekg(0, std::stringstream::end); + std::streamoff reference_len = inReference.mStream.tellg(); + inReference.mStream.seekg(0, std::stringstream::beg); + + // Compare size + bool fail = reference_len != this_len; + if (fail) + { + Trace("Failed to properly recover state, different stream length!"); + return false; + } + + // Compare byte by byte + for (std::streamoff i = 0, l = this_len; !fail && i < l; ++i) + { + fail = inReference.mStream.get() != mStream.get(); + if (fail) + { + Trace("Failed to properly recover state, different at offset %d!", (int)i); + return false; + } + } + + return true; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/StateRecorderImpl.h b/WickedEngine/Jolt/Physics/StateRecorderImpl.h new file mode 100644 index 000000000..b852679c0 --- /dev/null +++ b/WickedEngine/Jolt/Physics/StateRecorderImpl.h @@ -0,0 +1,47 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of the StateRecorder class that uses a stringstream as underlying store and that implements checking if the state doesn't change upon reading +class JPH_EXPORT StateRecorderImpl final : public StateRecorder +{ +public: + /// Constructor + StateRecorderImpl() = default; + StateRecorderImpl(StateRecorderImpl &&inRHS) : StateRecorder(inRHS), mStream(std::move(inRHS.mStream)) { } + + /// Write a string of bytes to the binary stream + virtual void WriteBytes(const void *inData, size_t inNumBytes) override; + + /// Rewind the stream for reading + void Rewind(); + + /// Clear the stream for reuse + void Clear(); + + /// Read a string of bytes from the binary stream + virtual void ReadBytes(void *outData, size_t inNumBytes) override; + + // See StreamIn + virtual bool IsEOF() const override { return mStream.eof(); } + + // See StreamIn / StreamOut + virtual bool IsFailed() const override { return mStream.fail(); } + + /// Compare this state with a reference state and ensure they are the same + bool IsEqual(StateRecorderImpl &inReference); + + /// Convert the binary data to a string + string GetData() const { return mStream.str(); } + +private: + std::stringstream mStream; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.cpp b/WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.cpp new file mode 100644 index 000000000..1bfa564f7 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.cpp @@ -0,0 +1,293 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(MotorcycleControllerSettings) +{ + JPH_ADD_BASE_CLASS(MotorcycleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mMaxLeanAngle) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringConstant) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringDamping) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficient) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSpringIntegrationCoefficientDecay) + JPH_ADD_ATTRIBUTE(MotorcycleControllerSettings, mLeanSmoothingFactor) +} + +VehicleController *MotorcycleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new MotorcycleController(*this, inConstraint); +} + +void MotorcycleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + WheeledVehicleControllerSettings::SaveBinaryState(inStream); + + inStream.Write(mMaxLeanAngle); + inStream.Write(mLeanSpringConstant); + inStream.Write(mLeanSpringDamping); + inStream.Write(mLeanSpringIntegrationCoefficient); + inStream.Write(mLeanSpringIntegrationCoefficientDecay); + inStream.Write(mLeanSmoothingFactor); +} + +void MotorcycleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + WheeledVehicleControllerSettings::RestoreBinaryState(inStream); + + inStream.Read(mMaxLeanAngle); + inStream.Read(mLeanSpringConstant); + inStream.Read(mLeanSpringDamping); + inStream.Read(mLeanSpringIntegrationCoefficient); + inStream.Read(mLeanSpringIntegrationCoefficientDecay); + inStream.Read(mLeanSmoothingFactor); +} + +MotorcycleController::MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + WheeledVehicleController(inSettings, inConstraint), + mMaxLeanAngle(inSettings.mMaxLeanAngle), + mLeanSpringConstant(inSettings.mLeanSpringConstant), + mLeanSpringDamping(inSettings.mLeanSpringDamping), + mLeanSpringIntegrationCoefficient(inSettings.mLeanSpringIntegrationCoefficient), + mLeanSpringIntegrationCoefficientDecay(inSettings.mLeanSpringIntegrationCoefficientDecay), + mLeanSmoothingFactor(inSettings.mLeanSmoothingFactor) +{ +} + +float MotorcycleController::GetWheelBase() const +{ + float low = FLT_MAX, high = -FLT_MAX; + + for (const Wheel *w : mConstraint.GetWheels()) + { + const WheelSettings *s = w->GetSettings(); + + // Measure distance along the forward axis by looking at the fully extended suspension. + // If the suspension force point is active, use that instead. + Vec3 force_point = s->mEnableSuspensionForcePoint? s->mSuspensionForcePoint : s->mPosition + s->mSuspensionDirection * s->mSuspensionMaxLength; + float value = force_point.Dot(mConstraint.GetLocalForward()); + + // Update min and max + low = min(low, value); + high = max(high, value); + } + + return high - low; +} + +void MotorcycleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + WheeledVehicleController::PreCollide(inDeltaTime, inPhysicsSystem); + + const Body *body = mConstraint.GetVehicleBody(); + Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward(); + float wheel_base = GetWheelBase(); + Vec3 world_up = mConstraint.GetWorldUp(); + + if (mEnableLeanController) + { + // Calculate the target lean vector, this is in the direction of the total applied impulse by the ground on the wheels + Vec3 target_lean = Vec3::sZero(); + for (const Wheel *w : mConstraint.GetWheels()) + if (w->HasContact()) + target_lean += w->GetContactNormal() * w->GetSuspensionLambda() + w->GetContactLateral() * w->GetLateralLambda(); + + // Normalize the impulse + target_lean = target_lean.NormalizedOr(world_up); + + // Smooth the impulse to avoid jittery behavior + mTargetLean = mLeanSmoothingFactor * mTargetLean + (1.0f - mLeanSmoothingFactor) * target_lean; + + // Remove forward component, we can only lean sideways + mTargetLean -= forward * mTargetLean.Dot(forward); + mTargetLean = mTargetLean.NormalizedOr(world_up); + + // Clamp the target lean against the max lean angle + Vec3 adjusted_world_up = world_up - forward * world_up.Dot(forward); + adjusted_world_up = adjusted_world_up.NormalizedOr(world_up); + float w_angle = -Sign(mTargetLean.Cross(adjusted_world_up).Dot(forward)) * ACos(mTargetLean.Dot(adjusted_world_up)); + if (abs(w_angle) > mMaxLeanAngle) + mTargetLean = Quat::sRotation(forward, Sign(w_angle) * mMaxLeanAngle) * adjusted_world_up; + + // Integrate the delta angle + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up)); + mLeanSpringIntegratedDeltaAngle += d_angle * inDeltaTime; + } + else + { + // Controller not enabled, reset target lean + mTargetLean = world_up; + + // Reset integrated delta angle + mLeanSpringIntegratedDeltaAngle = 0; + } + + JPH_DET_LOG("WheeledVehicleController::PreCollide: mTargetLean: " << mTargetLean); + + // Calculate max steering angle based on the max lean angle we're willing to take + // See: https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Leaning + // LeanAngle = Atan(Velocity^2 / (Gravity * TurnRadius)) + // And: https://en.wikipedia.org/wiki/Turning_radius (we're ignoring the tire width) + // The CasterAngle is the added according to https://en.wikipedia.org/wiki/Bicycle_and_motorcycle_dynamics#Turning (this is the same formula but without small angle approximation) + // TurnRadius = WheelBase / (Sin(SteerAngle) * Cos(CasterAngle)) + // => SteerAngle = ASin(WheelBase * Tan(LeanAngle) * Gravity / (Velocity^2 * Cos(CasterAngle)) + // The caster angle is different for each wheel so we can only calculate part of the equation here + float max_steer_angle_factor = wheel_base * Tan(mMaxLeanAngle) * (mConstraint.IsGravityOverridden()? mConstraint.GetGravityOverride() : inPhysicsSystem.GetGravity()).Length(); + + // Calculate forward velocity + float velocity = body->GetLinearVelocity().Dot(forward); + float velocity_sq = Square(velocity); + + // Decompose steering into sign and direction + float steer_strength = abs(mRightInput); + float steer_sign = -Sign(mRightInput); + + for (Wheel *w_base : mConstraint.GetWheels()) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *s = w->GetSettings(); + + // Check if this wheel can steer + if (s->mMaxSteerAngle != 0.0f) + { + // Calculate cos(caster angle), the angle between the steering axis and the up vector + float cos_caster_angle = s->mSteeringAxis.Dot(mConstraint.GetLocalUp()); + + // Calculate steer angle + float steer_angle = steer_strength * w->GetSettings()->mMaxSteerAngle; + + // Clamp to max steering angle + if (mEnableLeanSteeringLimit + && velocity_sq > 1.0e-6f && cos_caster_angle > 1.0e-6f) + { + float max_steer_angle = ASin(max_steer_angle_factor / (velocity_sq * cos_caster_angle)); + steer_angle = min(steer_angle, max_steer_angle); + } + + // Set steering angle + w->SetSteerAngle(steer_sign * steer_angle); + } + } + + // Reset applied impulse + mAppliedImpulse = 0; +} + +bool MotorcycleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = WheeledVehicleController::SolveLongitudinalAndLateralConstraints(inDeltaTime); + + if (mEnableLeanController) + { + // Only apply a lean impulse if all wheels are in contact, otherwise we can easily spin out + bool all_in_contact = true; + for (const Wheel *w : mConstraint.GetWheels()) + if (!w->HasContact() || w->GetSuspensionLambda() <= 0.0f) + { + all_in_contact = false; + break; + } + + if (all_in_contact) + { + Body *body = mConstraint.GetVehicleBody(); + const MotionProperties *mp = body->GetMotionProperties(); + + Vec3 forward = body->GetRotation() * mConstraint.GetLocalForward(); + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + + // Calculate delta to target angle and derivative + float d_angle = -Sign(mTargetLean.Cross(up).Dot(forward)) * ACos(mTargetLean.Dot(up)); + float ddt_angle = body->GetAngularVelocity().Dot(forward); + + // Calculate impulse to apply to get to target lean angle + float total_impulse = (mLeanSpringConstant * d_angle - mLeanSpringDamping * ddt_angle + mLeanSpringIntegrationCoefficient * mLeanSpringIntegratedDeltaAngle) * inDeltaTime; + + // Remember angular velocity pre angular impulse + Vec3 old_w = mp->GetAngularVelocity(); + + // Apply impulse taking into account the impulse we've applied earlier + float delta_impulse = total_impulse - mAppliedImpulse; + body->AddAngularImpulse(delta_impulse * forward); + mAppliedImpulse = total_impulse; + + // Calculate delta angular velocity due to angular impulse + Vec3 dw = mp->GetAngularVelocity() - old_w; + Vec3 linear_acceleration = Vec3::sZero(); + float total_lambda = 0.0f; + for (Wheel *w_base : mConstraint.GetWheels()) + { + const WheelWV *w = static_cast(w_base); + + // We weigh the importance of each contact point according to the contact force + float lambda = w->GetSuspensionLambda(); + total_lambda += lambda; + + // Linear acceleration of contact point is dw x com_to_contact + Vec3 r = Vec3(w->GetContactPosition() - body->GetCenterOfMassPosition()); + linear_acceleration += lambda * dw.Cross(r); + } + + // Apply linear impulse to COM to cancel the average velocity change on the wheels due to the angular impulse + Vec3 linear_impulse = -linear_acceleration / (total_lambda * mp->GetInverseMass()); + body->AddImpulse(linear_impulse); + + // Return true if we applied an impulse + impulse |= delta_impulse != 0.0f; + } + else + { + // Decay the integrated angle because we won't be applying a torque this frame + // Uses 1st order Taylor approximation of e^(-decay * dt) = 1 - decay * dt + mLeanSpringIntegratedDeltaAngle *= max(0.0f, 1.0f - mLeanSpringIntegrationCoefficientDecay * inDeltaTime); + } + } + + return impulse; +} + +void MotorcycleController::SaveState(StateRecorder &inStream) const +{ + WheeledVehicleController::SaveState(inStream); + + inStream.Write(mTargetLean); +} + +void MotorcycleController::RestoreState(StateRecorder &inStream) +{ + WheeledVehicleController::RestoreState(inStream); + + inStream.Read(mTargetLean); +} + +#ifdef JPH_DEBUG_RENDERER + +void MotorcycleController::Draw(DebugRenderer *inRenderer) const +{ + WheeledVehicleController::Draw(inRenderer); + + // Draw current and desired lean angle + Body *body = mConstraint.GetVehicleBody(); + RVec3 center_of_mass = body->GetCenterOfMassPosition(); + Vec3 up = body->GetRotation() * mConstraint.GetLocalUp(); + inRenderer->DrawArrow(center_of_mass, center_of_mass + up, Color::sYellow, 0.1f); + inRenderer->DrawArrow(center_of_mass, center_of_mass + mTargetLean, Color::sRed, 0.1f); +} + +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.h b/WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.h new file mode 100644 index 000000000..59ee17cdc --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/MotorcycleController.h @@ -0,0 +1,116 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2023 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Settings of a two wheeled motorcycle (adds a spring to balance the motorcycle) +/// Note: The motor cycle controller is still in development and may need a lot of tweaks/hacks to work properly! +class JPH_EXPORT MotorcycleControllerSettings : public WheeledVehicleControllerSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, MotorcycleControllerSettings) + + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + /// How far we're willing to make the bike lean over in turns (in radians) + float mMaxLeanAngle = DegreesToRadians(45.0f); + + /// Spring constant for the lean spring + float mLeanSpringConstant = 5000.0f; + + /// Spring damping constant for the lean spring + float mLeanSpringDamping = 1000.0f; + + /// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller + float mLeanSpringIntegrationCoefficient = 0.0f; + + /// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value + float mLeanSpringIntegrationCoefficientDecay = 4.0f; + + /// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes) + /// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current + float mLeanSmoothingFactor = 0.8f; +}; + +/// Runtime controller class +class JPH_EXPORT MotorcycleController : public WheeledVehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + MotorcycleController(const MotorcycleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Get the distance between the front and back wheels + float GetWheelBase() const; + + /// Enable or disable the lean spring. This allows you to temporarily disable the lean spring to allow the motorcycle to fall over. + void EnableLeanController(bool inEnable) { mEnableLeanController = inEnable; } + + /// Check if the lean spring is enabled. + bool IsLeanControllerEnabled() const { return mEnableLeanController; } + + /// Enable or disable the lean steering limit. When enabled (default) the steering angle is limited based on the vehicle speed to prevent steering that would cause an inertial force that causes the motorcycle to topple over. + void EnableLeanSteeringLimit(bool inEnable) { mEnableLeanSteeringLimit = inEnable; } + bool IsLeanSteeringLimitEnabled() const { return mEnableLeanSteeringLimit; } + + /// Spring constant for the lean spring + void SetLeanSpringConstant(float inConstant) { mLeanSpringConstant = inConstant; } + float GetLeanSpringConstant() const { return mLeanSpringConstant; } + + /// Spring damping constant for the lean spring + void SetLeanSpringDamping(float inDamping) { mLeanSpringDamping = inDamping; } + float GetLeanSpringDamping() const { return mLeanSpringDamping; } + + /// The lean spring applies an additional force equal to this coefficient * Integral(delta angle, 0, t), this effectively makes the lean spring a PID controller + void SetLeanSpringIntegrationCoefficient(float inCoefficient) { mLeanSpringIntegrationCoefficient = inCoefficient; } + float GetLeanSpringIntegrationCoefficient() const { return mLeanSpringIntegrationCoefficient; } + + /// How much to decay the angle integral when the wheels are not touching the floor: new_value = e^(-decay * t) * initial_value + void SetLeanSpringIntegrationCoefficientDecay(float inDecay) { mLeanSpringIntegrationCoefficientDecay = inDecay; } + float GetLeanSpringIntegrationCoefficientDecay() const { return mLeanSpringIntegrationCoefficientDecay; } + + /// How much to smooth the lean angle (0 = no smoothing, 1 = lean angle never changes) + /// Note that this is frame rate dependent because the formula is: smoothing_factor * previous + (1 - smoothing_factor) * current + void SetLeanSmoothingFactor(float inFactor) { mLeanSmoothingFactor = inFactor; } + float GetLeanSmoothingFactor() const { return mLeanSmoothingFactor; } + +protected: + // See: VehicleController + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Configuration properties + bool mEnableLeanController = true; + bool mEnableLeanSteeringLimit = true; + float mMaxLeanAngle; + float mLeanSpringConstant; + float mLeanSpringDamping; + float mLeanSpringIntegrationCoefficient; + float mLeanSpringIntegrationCoefficientDecay; + float mLeanSmoothingFactor; + + // Run-time calculated target lean vector + Vec3 mTargetLean = Vec3::sZero(); + + // Integrated error for the lean spring + float mLeanSpringIntegratedDeltaAngle = 0.0f; + + // Run-time total angular impulse applied to turn the cycle towards the target lean angle + float mAppliedImpulse = 0.0f; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.cpp b/WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.cpp new file mode 100644 index 000000000..c941a8295 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.cpp @@ -0,0 +1,531 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TrackedVehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(TrackedVehicleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mEngine) + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTransmission) + JPH_ADD_ATTRIBUTE(TrackedVehicleControllerSettings, mTracks) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsTV) +{ + JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLongitudinalFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsTV, mLateralFriction) +} + +void WheelSettingsTV::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLongitudinalFriction); + inStream.Write(mLateralFriction); +} + +void WheelSettingsTV::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLongitudinalFriction); + inStream.Read(mLateralFriction); +} + +WheelTV::WheelTV(const WheelSettingsTV &inSettings) : + Wheel(inSettings) +{ +} + +void WheelTV::CalculateAngularVelocity(const VehicleConstraint &inConstraint) +{ + const WheelSettingsTV *settings = GetSettings(); + const Wheels &wheels = inConstraint.GetWheels(); + const VehicleTrack &track = static_cast(inConstraint.GetController())->GetTracks()[mTrackIndex]; + + // Calculate angular velocity of this wheel + mAngularVelocity = track.mAngularVelocity * wheels[track.mDrivenWheel]->GetSettings()->mRadius / settings->mRadius; +} + +void WheelTV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint) +{ + CalculateAngularVelocity(inConstraint); + + // Update rotation of wheel + mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI); + + // Reset brake impulse, will be set during post collision again + mBrakeImpulse = 0.0f; + + if (mContactBody != nullptr) + { + // Friction at the point of this wheel between track and floor + const WheelSettingsTV *settings = GetSettings(); + VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction(); + mCombinedLongitudinalFriction = settings->mLongitudinalFriction; + mCombinedLateralFriction = settings->mLateralFriction; + combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID); + } + else + { + // No collision + mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f; + } +} + +VehicleController *TrackedVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new TrackedVehicleController(*this, inConstraint); +} + +TrackedVehicleControllerSettings::TrackedVehicleControllerSettings() +{ + // Numbers guestimated from: https://en.wikipedia.org/wiki/M1_Abrams + mEngine.mMinRPM = 500.0f; + mEngine.mMaxRPM = 4000.0f; + mEngine.mMaxTorque = 500.0f; // Note actual torque for M1 is around 5000 but we need a reduced mass in order to keep the simulation sane + + mTransmission.mShiftDownRPM = 1000.0f; + mTransmission.mShiftUpRPM = 3500.0f; + mTransmission.mGearRatios = { 4.0f, 3.0f, 2.0f, 1.0f }; + mTransmission.mReverseGearRatios = { -4.0f, -3.0f }; +} + +void TrackedVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + mEngine.SaveBinaryState(inStream); + + mTransmission.SaveBinaryState(inStream); + + for (const VehicleTrackSettings &t : mTracks) + t.SaveBinaryState(inStream); +} + +void TrackedVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + mEngine.RestoreBinaryState(inStream); + + mTransmission.RestoreBinaryState(inStream); + + for (VehicleTrackSettings &t : mTracks) + t.RestoreBinaryState(inStream); +} + +TrackedVehicleController::TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + VehicleController(inConstraint) +{ + // Copy engine settings + static_cast(mEngine) = inSettings.mEngine; + JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f); + JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM); + mEngine.SetCurrentRPM(mEngine.mMinRPM); + + // Copy transmission settings + static_cast(mTransmission) = inSettings.mTransmission; +#ifdef JPH_ENABLE_ASSERTS + for (float r : inSettings.mTransmission.mGearRatios) + JPH_ASSERT(r > 0.0f); + for (float r : inSettings.mTransmission.mReverseGearRatios) + JPH_ASSERT(r < 0.0f); +#endif // JPH_ENABLE_ASSERTS + JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f); + JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f); + JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM); + JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM); + + // Copy track settings + for (uint i = 0; i < size(mTracks); ++i) + { + const VehicleTrackSettings &d = inSettings.mTracks[i]; + static_cast(mTracks[i]) = d; + JPH_ASSERT(d.mInertia >= 0.0f); + JPH_ASSERT(d.mAngularDamping >= 0.0f); + JPH_ASSERT(d.mMaxBrakeTorque >= 0.0f); + JPH_ASSERT(d.mDifferentialRatio > 0.0f); + } +} + +bool TrackedVehicleController::AllowSleep() const +{ + return mForwardInput == 0.0f // No user input + && mTransmission.AllowSleep() // Transmission is not shifting + && mEngine.AllowSleep(); // Engine is idling +} + +void TrackedVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + Wheels &wheels = mConstraint.GetWheels(); + + // Fill in track index + for (size_t t = 0; t < size(mTracks); ++t) + for (uint w : mTracks[t].mWheels) + static_cast(wheels[w])->mTrackIndex = (uint)t; + + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + for (VehicleTrack &t : mTracks) + t.mAngularVelocity *= max(0.0f, 1.0f - t.mAngularDamping * inDeltaTime); +} + +void TrackedVehicleController::SyncLeftRightTracks() +{ + // Apply left to right ratio according to track inertias + VehicleTrack &tl = mTracks[(int)ETrackSide::Left]; + VehicleTrack &tr = mTracks[(int)ETrackSide::Right]; + + if (mLeftRatio * mRightRatio > 0.0f) + { + // Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = -dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks + float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mLeftRatio * tr.mInertia + mRightRatio * tl.mInertia); + tl.mAngularVelocity += impulse * tl.mInertia; + tr.mAngularVelocity -= impulse * tr.mInertia; + } + else + { + // Solve: (tl.mAngularVelocity + dl) / (tr.mAngularVelocity + dr) = mLeftRatio / mRightRatio and dl * tr.mInertia = dr * tl.mInertia, where dl/dr are the delta angular velocities for left and right tracks + float impulse = (mLeftRatio * tr.mAngularVelocity - mRightRatio * tl.mAngularVelocity) / (mRightRatio * tl.mInertia - mLeftRatio * tr.mInertia); + tl.mAngularVelocity += impulse * tl.mInertia; + tr.mAngularVelocity += impulse * tr.mInertia; + } +} + +void TrackedVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + + Wheels &wheels = mConstraint.GetWheels(); + + // Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again) + for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index) + { + WheelTV *w = static_cast(wheels[wheel_index]); + w->Update(wheel_index, inDeltaTime, mConstraint); + } + + // First calculate engine speed based on speed of all wheels + bool can_engine_apply_torque = false; + if (mTransmission.GetCurrentGear() != 0 && mTransmission.GetClutchFriction() > 1.0e-3f) + { + float transmission_ratio = mTransmission.GetCurrentRatio(); + bool forward = transmission_ratio >= 0.0f; + float fastest_wheel_speed = forward? -FLT_MAX : FLT_MAX; + for (const VehicleTrack &t : mTracks) + { + if (forward) + fastest_wheel_speed = max(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio); + else + fastest_wheel_speed = min(fastest_wheel_speed, t.mAngularVelocity * t.mDifferentialRatio); + for (uint w : t.mWheels) + if (wheels[w]->HasContact()) + { + can_engine_apply_torque = true; + break; + } + } + + // Update RPM only if the tracks are connected to the engine + if (fastest_wheel_speed > -FLT_MAX && fastest_wheel_speed < FLT_MAX) + mEngine.SetCurrentRPM(fastest_wheel_speed * mTransmission.GetCurrentRatio() * VehicleEngine::cAngularVelocityToRPM); + } + else + { + // Update engine with damping + mEngine.ApplyDamping(inDeltaTime); + + // In auto transmission mode, don't accelerate the engine when switching gears + float forward_input = mTransmission.mMode == ETransmissionMode::Manual? abs(mForwardInput) : 0.0f; + + // Engine not connected to wheels, update RPM based on engine inertia alone + mEngine.ApplyTorque(mEngine.GetTorque(forward_input), inDeltaTime); + } + + // Update transmission + // Note: only allow switching gears up when the tracks are rolling in the same direction + mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, mLeftRatio * mRightRatio > 0.0f && can_engine_apply_torque); + + // Calculate the amount of torque the transmission gives to the differentials + float transmission_ratio = mTransmission.GetCurrentRatio(); + float transmission_torque = mTransmission.GetClutchFriction() * transmission_ratio * mEngine.GetTorque(abs(mForwardInput)); + if (transmission_torque != 0.0f) + { + // Apply the transmission torque to the wheels + for (uint i = 0; i < size(mTracks); ++i) + { + VehicleTrack &t = mTracks[i]; + + // Get wheel rotation ratio for this track + float ratio = i == 0? mLeftRatio : mRightRatio; + + // Calculate the max angular velocity of the driven wheel of the track given current engine RPM + // Note this adds 0.1% slop to avoid numerical accuracy issues + float track_max_angular_velocity = mEngine.GetCurrentRPM() / (transmission_ratio * t.mDifferentialRatio * ratio * VehicleEngine::cAngularVelocityToRPM) * 1.001f; + + // Calculate torque on the driven wheel + float differential_torque = t.mDifferentialRatio * ratio * transmission_torque; + + // Apply torque to driven wheel + if (t.mAngularVelocity * track_max_angular_velocity < 0.0f || abs(t.mAngularVelocity) < abs(track_max_angular_velocity)) + t.mAngularVelocity += differential_torque * inDeltaTime / t.mInertia; + } + } + + // Ensure that we have the correct ratio between the two tracks + SyncLeftRightTracks(); + + // Braking + for (VehicleTrack &t : mTracks) + { + // Calculate brake torque + float brake_torque = mBrakeInput * t.mMaxBrakeTorque; + if (brake_torque > 0.0f) + { + // Calculate how much torque is needed to stop the track from rotating in this time step + float brake_torque_to_lock_track = abs(t.mAngularVelocity) * t.mInertia / inDeltaTime; + if (brake_torque > brake_torque_to_lock_track) + { + // Wheels are locked + t.mAngularVelocity = 0.0f; + brake_torque -= brake_torque_to_lock_track; + } + else + { + // Slow down the track + t.mAngularVelocity -= Sign(t.mAngularVelocity) * brake_torque * inDeltaTime / t.mInertia; + } + } + + if (brake_torque > 0.0f) + { + // Sum the radius of all wheels touching the floor + float total_radius = 0.0f; + for (uint wheel_index : t.mWheels) + { + const WheelTV *w = static_cast(wheels[wheel_index]); + + if (w->HasContact()) + total_radius += w->GetSettings()->mRadius; + } + + if (total_radius > 0.0f) + { + brake_torque /= total_radius; + for (uint wheel_index : t.mWheels) + { + WheelTV *w = static_cast(wheels[wheel_index]); + if (w->HasContact()) + { + // Impulse: p = F * dt = Torque / Wheel_Radius * dt, Torque = Total_Torque * Wheel_Radius / Summed_Radius => p = Total_Torque * dt / Summed_Radius + w->mBrakeImpulse = brake_torque * inDeltaTime; + } + } + } + } + } + + // Update wheel angular velocity based on that of the track + for (Wheel *w_base : wheels) + { + WheelTV *w = static_cast(w_base); + w->CalculateAngularVelocity(mConstraint); + } +} + +bool TrackedVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = false; + + for (Wheel *w_base : mConstraint.GetWheels()) + if (w_base->HasContact()) + { + WheelTV *w = static_cast(w_base); + const WheelSettingsTV *settings = w->GetSettings(); + VehicleTrack &track = mTracks[w->mTrackIndex]; + + // Calculate max impulse that we can apply on the ground + float max_longitudinal_friction_impulse = w->mCombinedLongitudinalFriction * w->GetSuspensionLambda(); + + // Calculate relative velocity between wheel contact point and floor in longitudinal direction + Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity(); + float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal()); + + // Calculate brake force to apply + float min_longitudinal_impulse, max_longitudinal_impulse; + if (w->mBrakeImpulse != 0.0f) + { + // Limit brake force by max tire friction + float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse); + + // Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle) + if (relative_longitudinal_velocity >= 0.0f) + { + min_longitudinal_impulse = -brake_impulse; + max_longitudinal_impulse = 0.0f; + } + else + { + min_longitudinal_impulse = 0.0f; + max_longitudinal_impulse = brake_impulse; + } + + // Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + } + else + { + // Assume we want to apply an angular impulse that makes the delta velocity between track and ground zero in one time step, calculate the amount of linear impulse needed to do that + float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius; + float linear_impulse = (track.mAngularVelocity - desired_angular_velocity) * track.mInertia / settings->mRadius; + + // Limit the impulse by max track friction + float prev_lambda = w->GetLongitudinalLambda(); + min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse); + + // Longitudinal impulse + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + + // Update the angular velocity of the track according to the lambda that was applied + track.mAngularVelocity -= (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / track.mInertia; + SyncLeftRightTracks(); + } + } + + for (Wheel *w_base : mConstraint.GetWheels()) + if (w_base->HasContact()) + { + WheelTV *w = static_cast(w_base); + + // Update angular velocity of wheel for the next iteration + w->CalculateAngularVelocity(mConstraint); + + // Lateral friction + float max_lateral_friction_impulse = w->mCombinedLateralFriction * w->GetSuspensionLambda(); + impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_friction_impulse, max_lateral_friction_impulse); + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const +{ + float constraint_size = mConstraint.GetDrawConstraintSize(); + + // Draw RPM + Body *body = mConstraint.GetVehicleBody(); + Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp(); + RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition; + Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward(); + mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM); + + // Draw current vehicle state + String status = StringFormat("Forward: %.1f, LRatio: %.1f, RRatio: %.1f, Brake: %.1f\n" + "Gear: %d, Clutch: %.1f, EngineRPM: %.0f, V: %.1f km/h", + (double)mForwardInput, (double)mLeftRatio, (double)mRightRatio, (double)mBrakeInput, + mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6); + inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size); + + for (const VehicleTrack &t : mTracks) + { + const WheelTV *w = static_cast(mConstraint.GetWheels()[t.mDrivenWheel]); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass()); + + DebugRenderer::sInstance->DrawText3D(ws_position, StringFormat("W: %.1f", (double)t.mAngularVelocity), Color::sWhite, constraint_size); + } + + RMat44 body_transform = body->GetWorldTransform(); + + for (const Wheel *w_base : mConstraint.GetWheels()) + { + const WheelTV *w = static_cast(w_base); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Draw suspension + RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength; + RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength; + inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed); + inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen); + + // Draw current length + RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength(); + inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size); + + // Draw wheel basis + Vec3 wheel_forward, wheel_up, wheel_right; + mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right); + wheel_forward = body_transform.Multiply3x3(wheel_forward); + wheel_up = body_transform.Multiply3x3(wheel_up); + wheel_right = body_transform.Multiply3x3(wheel_right); + Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue); + inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow); + + // Draw wheel + RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos); + wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle())); + inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe); + + if (w->HasContact()) + { + // Draw contact + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue); + + DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("S: %.2f", (double)w->GetSuspensionLength()), Color::sWhite, constraint_size); + } + } +} + +#endif // JPH_DEBUG_RENDERER + +void TrackedVehicleController::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mForwardInput); + inStream.Write(mLeftRatio); + inStream.Write(mRightRatio); + inStream.Write(mBrakeInput); + + mEngine.SaveState(inStream); + mTransmission.SaveState(inStream); + + for (const VehicleTrack &t : mTracks) + t.SaveState(inStream); +} + +void TrackedVehicleController::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mForwardInput); + inStream.Read(mLeftRatio); + inStream.Read(mRightRatio); + inStream.Read(mBrakeInput); + + mEngine.RestoreState(inStream); + mTransmission.RestoreState(inStream); + + for (VehicleTrack &t : mTracks) + t.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.h b/WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.h new file mode 100644 index 000000000..826959dd6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/TrackedVehicleController.h @@ -0,0 +1,166 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// WheelSettings object specifically for TrackedVehicleController +class JPH_EXPORT WheelSettingsTV : public WheelSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsTV) + + // See: WheelSettings + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + float mLongitudinalFriction = 4.0f; ///< Friction in forward direction of tire + float mLateralFriction = 2.0f; ///< Friction in sideway direction of tire +}; + +/// Wheel object specifically for TrackedVehicleController +class JPH_EXPORT WheelTV : public Wheel +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit WheelTV(const WheelSettingsTV &inWheel); + + /// Override GetSettings and cast to the correct class + const WheelSettingsTV * GetSettings() const { return StaticCast(mSettings); } + + /// Update the angular velocity of the wheel based on the angular velocity of the track + void CalculateAngularVelocity(const VehicleConstraint &inConstraint); + + /// Update the wheel rotation based on the current angular velocity + void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint); + + int mTrackIndex = -1; ///< Index in mTracks to which this wheel is attached (calculated on initialization) + float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and track) + float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and track) + float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction), spread out from brake impulse applied on track +}; + +/// Settings of a vehicle with tank tracks +/// +/// Default settings are based around what I could find about the M1 Abrams tank. +/// Note to avoid issues with very heavy objects vs very light objects the mass of the tank should be a lot lower (say 10x) than that of a real tank. That means that the engine/brake torque is also 10x less. +class JPH_EXPORT TrackedVehicleControllerSettings : public VehicleControllerSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, TrackedVehicleControllerSettings) + + // Constructor + TrackedVehicleControllerSettings(); + + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + VehicleEngineSettings mEngine; ///< The properties of the engine + VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box) + VehicleTrackSettings mTracks[(int)ETrackSide::Num]; ///< List of tracks and their properties +}; + +/// Runtime controller class for vehicle with tank tracks +class JPH_EXPORT TrackedVehicleController : public VehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + TrackedVehicleController(const TrackedVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Set input from driver + /// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + /// @param inLeftRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + /// @param inRightRatio Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + /// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetDriverInput(float inForward, float inLeftRatio, float inRightRatio, float inBrake) { JPH_ASSERT(inLeftRatio != 0.0f && inRightRatio != 0.0f); mForwardInput = inForward; mLeftRatio = inLeftRatio; mRightRatio = inRightRatio; mBrakeInput = inBrake; } + + /// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + void SetForwardInput(float inForward) { mForwardInput = inForward; } + float GetForwardInput() const { return mForwardInput; } + + /// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + void SetLeftRatio(float inLeftRatio) { JPH_ASSERT(inLeftRatio != 0.0f); mLeftRatio = inLeftRatio; } + float GetLeftRatio() const { return mLeftRatio; } + + /// Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + void SetRightRatio(float inRightRatio) { JPH_ASSERT(inRightRatio != 0.0f); mRightRatio = inRightRatio; } + float GetRightRatio() const { return mRightRatio; } + + /// Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; } + float GetBrakeInput() const { return mBrakeInput; } + + /// Get current engine state + const VehicleEngine & GetEngine() const { return mEngine; } + + /// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleEngine & GetEngine() { return mEngine; } + + /// Get current transmission state + const VehicleTransmission & GetTransmission() const { return mTransmission; } + + /// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTransmission & GetTransmission() { return mTransmission; } + + /// Get the tracks this vehicle has + const VehicleTracks & GetTracks() const { return mTracks; } + + /// Get the tracks this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTracks & GetTracks() { return mTracks; } + +#ifdef JPH_DEBUG_RENDERER + /// Debug drawing of RPM meter + void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } +#endif // JPH_DEBUG_RENDERER + +protected: + /// Synchronize angular velocities of left and right tracks according to their ratios + void SyncLeftRightTracks(); + + // See: VehicleController + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsTV))); return new WheelTV(static_cast(inWheel)); } + virtual bool AllowSleep() const override; + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Control information + float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + float mLeftRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the left track (used for steering) + float mRightRatio = 1.0f; ///< Value between -1 and 1 indicating an extra multiplier to the rotation rate of the right track (used for steering) + float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed + + // Simulation information + VehicleEngine mEngine; ///< Engine state of the vehicle + VehicleTransmission mTransmission; ///< Transmission state of the vehicle + VehicleTracks mTracks; ///< Tracks of the vehicle + +#ifdef JPH_DEBUG_RENDERER + // Debug settings + Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint + float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp new file mode 100644 index 000000000..859bcafa1 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.cpp @@ -0,0 +1,33 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleAntiRollBar) +{ + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mLeftWheel) + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mRightWheel) + JPH_ADD_ATTRIBUTE(VehicleAntiRollBar, mStiffness) +} + +void VehicleAntiRollBar::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLeftWheel); + inStream.Write(mRightWheel); + inStream.Write(mStiffness); +} + +void VehicleAntiRollBar::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLeftWheel); + inStream.Read(mRightWheel); + inStream.Read(mStiffness); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.h new file mode 100644 index 000000000..197c3a0d2 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleAntiRollBar.h @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// An anti rollbar is a stiff spring that connects two wheels to reduce the amount of roll the vehicle makes in sharp corners +/// See: https://en.wikipedia.org/wiki/Anti-roll_bar +class JPH_EXPORT VehicleAntiRollBar +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleAntiRollBar) + + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + int mLeftWheel = 0; ///< Index (in mWheels) that represents the left wheel of this anti-rollbar + int mRightWheel = 1; ///< Index (in mWheels) that represents the right wheel of this anti-rollbar + float mStiffness = 1000.0f; ///< Stiffness (spring constant in N/m) of anti rollbar, can be 0 to disable the anti-rollbar +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp new file mode 100644 index 000000000..562468357 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.cpp @@ -0,0 +1,376 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float wheel_radius = wheel_settings->mRadius; + float ray_length = wheel_settings->mSuspensionMaxLength + wheel_radius; + RRayCast ray { inOrigin, ray_length * inDirection }; + + class MyCollector : public CastRayCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RRayCast &inRay, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) : + mPhysicsSystem(inPhysicsSystem), + mRay(inRay), + mUpDirection(inUpDirection), + mCosMaxSlopeAngle(inCosMaxSlopeAngle) + { + } + + virtual void AddHit(const RayCastResult &inResult) override + { + // Test if this collision is closer than the previous one + if (inResult.mFraction < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Test that we're not hitting a vertical wall + RVec3 contact_pos = mRay.GetPointOnRay(inResult.mFraction); + Vec3 normal = body->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, contact_pos); + if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle) + { + // Update early out fraction to this hit + UpdateEarlyOutFraction(inResult.mFraction); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = contact_pos; + mContactNormal = normal; + } + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + RRayCast mRay; + Vec3 mUpDirection; + float mCosMaxSlopeAngle; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + }; + + RayCastSettings settings; + + MyCollector collector(inPhysicsSystem, ray, mUp, mCosMaxSlopeAngle); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max(0.0f, ray_length * collector.GetEarlyOutFraction() - wheel_radius); + + return true; +} + +void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Reproject the contact position using the suspension ray and the plane formed by the contact position and normal + ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection; + + // The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius + ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + SphereShape sphere(mRadius); + sphere.SetEmbedded(); + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float wheel_radius = wheel_settings->mRadius; + float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius; + RShapeCast shape_cast(&sphere, Vec3::sReplicate(1.0f), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length); + + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = true; + + class MyCollector : public CastShapeCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) : + mPhysicsSystem(inPhysicsSystem), + mShapeCast(inShapeCast), + mUpDirection(inUpDirection), + mCosMaxSlopeAngle(inCosMaxSlopeAngle) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Test if this collision is closer/deeper than the previous one + float early_out = inResult.GetEarlyOutFraction(); + if (early_out < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Test that we're not hitting a vertical wall + Vec3 normal = -inResult.mPenetrationAxis.Normalized(); + if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle) + { + // Update early out fraction to this hit + UpdateEarlyOutFraction(early_out); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2; + mContactNormal = normal; + mFraction = inResult.mFraction; + } + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + const RShapeCast & mShapeCast; + Vec3 mUpDirection; + float mCosMaxSlopeAngle; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + float mFraction; + }; + + MyCollector collector(inPhysicsSystem, shape_cast, mUp, mCosMaxSlopeAngle); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max(0.0f, shape_cast_length * collector.mFraction + mRadius - wheel_radius); + + return true; +} + +void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal + // This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction + float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal); + float fraction = (mRadius + oc_dot_n) / d_dot_n; + ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal; + + // Calculate the new suspension length in the same way as the cast sphere normally does + ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const +{ + const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer); + const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter; + + const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer); + const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter; + + const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID); + const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter; + + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float max_suspension_length = wheel_settings->mSuspensionMaxLength; + + // Get the wheel transform given that the cylinder rotates around the Y axis + RMat44 shape_cast_start = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX()); + shape_cast_start.SetTranslation(inOrigin); + + // Construct a cylinder with the dimensions of the wheel + float wheel_half_width = 0.5f * wheel_settings->mWidth; + CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction); + cylinder.SetEmbedded(); + + RShapeCast shape_cast(&cylinder, Vec3::sReplicate(1.0f), shape_cast_start, inDirection * max_suspension_length); + + ShapeCastSettings settings; + settings.mUseShrunkenShapeAndConvexRadius = true; + settings.mReturnDeepestPoint = true; + + class MyCollector : public CastShapeCollector + { + public: + MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast) : + mPhysicsSystem(inPhysicsSystem), + mShapeCast(inShapeCast) + { + } + + virtual void AddHit(const ShapeCastResult &inResult) override + { + // Test if this collision is closer/deeper than the previous one + float early_out = inResult.GetEarlyOutFraction(); + if (early_out < GetEarlyOutFraction()) + { + // Lock the body + BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2); + JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail + const Body *body = &lock.GetBody(); + + if (body->IsSensor()) + return; + + // Update early out fraction to this hit + UpdateEarlyOutFraction(early_out); + + // Get the contact properties + mBody = body; + mSubShapeID2 = inResult.mSubShapeID2; + mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2; + mContactNormal = -inResult.mPenetrationAxis.Normalized(); + mFraction = inResult.mFraction; + } + } + + // Configuration + PhysicsSystem & mPhysicsSystem; + const RShapeCast & mShapeCast; + + // Resulting closest collision + const Body * mBody = nullptr; + SubShapeID mSubShapeID2; + RVec3 mContactPosition; + Vec3 mContactNormal; + float mFraction; + }; + + MyCollector collector(inPhysicsSystem, shape_cast); + inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter); + if (collector.mBody == nullptr) + return false; + + outBody = const_cast(collector.mBody); + outSubShapeID = collector.mSubShapeID2; + outContactPosition = collector.mContactPosition; + outContactNormal = collector.mContactNormal; + outSuspensionLength = max_suspension_length * collector.mFraction; + + return true; +} + +void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const +{ + // Recalculate the contact points assuming the contact point is on an infinite plane + const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings(); + float d_dot_n = inDirection.Dot(ioContactNormal); + if (d_dot_n < -1.0e-6f) + { + // Wheel size + float half_width = 0.5f * wheel_settings->mWidth; + float radius = wheel_settings->mRadius; + + // Get the inverse local space contact normal for a cylinder pointing along Y + RMat44 wheel_transform = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX()); + Vec3 inverse_local_normal = -wheel_transform.Multiply3x3Transposed(ioContactNormal); + + // Get the support point of this normal in local space of the cylinder + // See CylinderShape::Cylinder::GetSupport + float x = inverse_local_normal.GetX(), y = inverse_local_normal.GetY(), z = inverse_local_normal.GetZ(); + float o = sqrt(Square(x) + Square(z)); + Vec3 support_point; + if (o > 0.0f) + support_point = Vec3((radius * x) / o, Sign(y) * half_width, (radius * z) / o); + else + support_point = Vec3(0, Sign(y) * half_width, 0); + + // Rotate back to world space + support_point = wheel_transform.Multiply3x3(support_point); + + // Now we can use inOrigin + support_point as the start of a ray of our suspension to the contact plane + // as know that it is the first point on the wheel that will hit the plane + RVec3 origin = inOrigin + support_point; + + // Calculate contact position and suspension length, the is the same as VehicleCollisionTesterRay + // but we don't need to take the radius into account anymore + Vec3 oc(ioContactPosition - origin); + ioContactPosition = origin + oc.Dot(ioContactNormal) / d_dot_n * inDirection; + ioSuspensionLength = Clamp(oc.Dot(inDirection), 0.0f, wheel_settings->mSuspensionMaxLength); + } + else + { + // If the normal is pointing away we assume there's no collision anymore + ioSuspensionLength = wheel_settings->mSuspensionMaxLength; + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.h new file mode 100644 index 000000000..7c222756c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleCollisionTester.h @@ -0,0 +1,146 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class VehicleConstraint; +class BroadPhaseLayerFilter; +class ObjectLayerFilter; +class BodyFilter; + +/// Class that does collision detection between wheels and ground +class JPH_EXPORT VehicleCollisionTester : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructors + VehicleCollisionTester() = default; + explicit VehicleCollisionTester(ObjectLayer inObjectLayer) : mObjectLayer(inObjectLayer) { } + + /// Virtual destructor + virtual ~VehicleCollisionTester() = default; + + /// Object layer to use for collision detection, this is used when the filters are not overridden + ObjectLayer GetObjectLayer() const { return mObjectLayer; } + void SetObjectLayer(ObjectLayer inObjectLayer) { mObjectLayer = inObjectLayer; } + + /// Access to the broad phase layer filter, when set this overrides the object layer supplied in the constructor + void SetBroadPhaseLayerFilter(const BroadPhaseLayerFilter *inFilter) { mBroadPhaseLayerFilter = inFilter; } + const BroadPhaseLayerFilter * GetBroadPhaseLayerFilter() const { return mBroadPhaseLayerFilter; } + + /// Access to the object layer filter, when set this overrides the object layer supplied in the constructor + void SetObjectLayerFilter(const ObjectLayerFilter *inFilter) { mObjectLayerFilter = inFilter; } + const ObjectLayerFilter * GetObjectLayerFilter() const { return mObjectLayerFilter; } + + /// Access to the body filter, when set this overrides the default filter that filters out the vehicle body + void SetBodyFilter(const BodyFilter *inFilter) { mBodyFilter = inFilter; } + const BodyFilter * GetBodyFilter() const { return mBodyFilter; } + + /// Do a collision test with the world + /// @param inPhysicsSystem The physics system that should be tested against + /// @param inVehicleConstraint The vehicle constraint + /// @param inWheelIndex Index of the wheel that we're testing collision for + /// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point + /// @param inDirection Direction for the test (unit vector, world space) + /// @param inVehicleBodyID This body should be filtered out during collision detection to avoid self collisions + /// @param outBody Body that the wheel collided with + /// @param outSubShapeID Sub shape ID that the wheel collided with + /// @param outContactPosition Contact point between wheel and floor, in world space + /// @param outContactNormal Contact normal between wheel and floor, pointing away from the floor + /// @param outSuspensionLength New length of the suspension [0, inSuspensionMaxLength] + /// @return True when collision found, false if not + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const = 0; + + /// Do a cheap contact properties prediction based on the contact properties from the last collision test (provided as input parameters) + /// @param inPhysicsSystem The physics system that should be tested against + /// @param inVehicleConstraint The vehicle constraint + /// @param inWheelIndex Index of the wheel that we're testing collision for + /// @param inOrigin Origin for the test, corresponds to the world space position for the suspension attachment point + /// @param inDirection Direction for the test (unit vector, world space) + /// @param inVehicleBodyID The body ID for the vehicle itself + /// @param ioBody Body that the wheel previously collided with + /// @param ioSubShapeID Sub shape ID that the wheel collided with during the last check + /// @param ioContactPosition Contact point between wheel and floor during the last check, in world space + /// @param ioContactNormal Contact normal between wheel and floor during the last check, pointing away from the floor + /// @param ioSuspensionLength New length of the suspension [0, inSuspensionMaxLength] + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const = 0; + +protected: + const BroadPhaseLayerFilter * mBroadPhaseLayerFilter = nullptr; + const ObjectLayerFilter * mObjectLayerFilter = nullptr; + const BodyFilter * mBodyFilter = nullptr; + ObjectLayer mObjectLayer = cObjectLayerInvalid; +}; + +/// Collision tester that tests collision using a raycast +class JPH_EXPORT VehicleCollisionTesterRay : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inUp World space up vector, used to avoid colliding with vertical walls. + /// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls. + VehicleCollisionTesterRay(ObjectLayer inObjectLayer, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + Vec3 mUp; + float mCosMaxSlopeAngle; +}; + +/// Collision tester that tests collision using a sphere cast +class JPH_EXPORT VehicleCollisionTesterCastSphere : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inUp World space up vector, used to avoid colliding with vertical walls. + /// @param inRadius Radius of sphere + /// @param inMaxSlopeAngle Max angle (rad) that is considered for colliding wheels. This is to avoid colliding with vertical walls. + VehicleCollisionTesterCastSphere(ObjectLayer inObjectLayer, float inRadius, Vec3Arg inUp = Vec3::sAxisY(), float inMaxSlopeAngle = DegreesToRadians(80.0f)) : VehicleCollisionTester(inObjectLayer), mRadius(inRadius), mUp(inUp), mCosMaxSlopeAngle(Cos(inMaxSlopeAngle)) { } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + float mRadius; + Vec3 mUp; + float mCosMaxSlopeAngle; +}; + +/// Collision tester that tests collision using a cylinder shape +class JPH_EXPORT VehicleCollisionTesterCastCylinder : public VehicleCollisionTester +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + /// @param inObjectLayer Object layer to test collision with + /// @param inConvexRadiusFraction Fraction of half the wheel width (or wheel radius if it is smaller) that is used as the convex radius + VehicleCollisionTesterCastCylinder(ObjectLayer inObjectLayer, float inConvexRadiusFraction = 0.1f) : VehicleCollisionTester(inObjectLayer), mConvexRadiusFraction(inConvexRadiusFraction) { JPH_ASSERT(mConvexRadiusFraction >= 0.0f && mConvexRadiusFraction <= 1.0f); } + + // See: VehicleCollisionTester + virtual bool Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const override; + virtual void PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const override; + +private: + float mConvexRadiusFraction; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.cpp new file mode 100644 index 000000000..03325124c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.cpp @@ -0,0 +1,697 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(VehicleConstraintSettings) +{ + JPH_ADD_BASE_CLASS(VehicleConstraintSettings, ConstraintSettings) + + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mUp) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mForward) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mMaxPitchRollAngle) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mWheels) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mAntiRollBars) + JPH_ADD_ATTRIBUTE(VehicleConstraintSettings, mController) +} + +void VehicleConstraintSettings::SaveBinaryState(StreamOut &inStream) const +{ + ConstraintSettings::SaveBinaryState(inStream); + + inStream.Write(mUp); + inStream.Write(mForward); + inStream.Write(mMaxPitchRollAngle); + + uint32 num_anti_rollbars = (uint32)mAntiRollBars.size(); + inStream.Write(num_anti_rollbars); + for (const VehicleAntiRollBar &r : mAntiRollBars) + r.SaveBinaryState(inStream); + + uint32 num_wheels = (uint32)mWheels.size(); + inStream.Write(num_wheels); + for (const WheelSettings *w : mWheels) + w->SaveBinaryState(inStream); + + inStream.Write(mController->GetRTTI()->GetHash()); + mController->SaveBinaryState(inStream); +} + +void VehicleConstraintSettings::RestoreBinaryState(StreamIn &inStream) +{ + ConstraintSettings::RestoreBinaryState(inStream); + + inStream.Read(mUp); + inStream.Read(mForward); + inStream.Read(mMaxPitchRollAngle); + + uint32 num_anti_rollbars = 0; + inStream.Read(num_anti_rollbars); + mAntiRollBars.resize(num_anti_rollbars); + for (VehicleAntiRollBar &r : mAntiRollBars) + r.RestoreBinaryState(inStream); + + uint32 num_wheels = 0; + inStream.Read(num_wheels); + mWheels.resize(num_wheels); + for (WheelSettings *w : mWheels) + w->RestoreBinaryState(inStream); + + uint32 hash = 0; + inStream.Read(hash); + const RTTI *rtti = Factory::sInstance->Find(hash); + mController = reinterpret_cast(rtti->CreateObject()); + mController->RestoreBinaryState(inStream); +} + +VehicleConstraint::VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings) : + Constraint(inSettings), + mBody(&inVehicleBody), + mForward(inSettings.mForward), + mUp(inSettings.mUp), + mWorldUp(inSettings.mUp) +{ + // Check sanity of incoming settings + JPH_ASSERT(inSettings.mUp.IsNormalized()); + JPH_ASSERT(inSettings.mForward.IsNormalized()); + JPH_ASSERT(!inSettings.mWheels.empty()); + + // Store max pitch/roll angle + SetMaxPitchRollAngle(inSettings.mMaxPitchRollAngle); + + // Copy anti-rollbar settings + mAntiRollBars.resize(inSettings.mAntiRollBars.size()); + for (uint i = 0; i < mAntiRollBars.size(); ++i) + { + const VehicleAntiRollBar &r = inSettings.mAntiRollBars[i]; + mAntiRollBars[i] = r; + JPH_ASSERT(r.mStiffness >= 0.0f); + } + + // Construct our controller class + mController = inSettings.mController->ConstructController(*this); + + // Create wheels + mWheels.resize(inSettings.mWheels.size()); + for (uint i = 0; i < mWheels.size(); ++i) + mWheels[i] = mController->ConstructWheel(*inSettings.mWheels[i]); + + // Use the body ID as a seed for the step counter so that not all vehicles will update at the same time + mCurrentStep = uint32(Hash64(inVehicleBody.GetID().GetIndex())); +} + +VehicleConstraint::~VehicleConstraint() +{ + // Destroy controller + delete mController; + + // Destroy our wheels + for (Wheel *w : mWheels) + delete w; +} + +void VehicleConstraint::GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const +{ + const WheelSettings *settings = inWheel->mSettings; + + Quat steer_rotation = Quat::sRotation(settings->mSteeringAxis, inWheel->mSteerAngle); + outUp = steer_rotation * settings->mWheelUp; + outForward = steer_rotation * settings->mWheelForward; + outRight = outForward.Cross(outUp).Normalized(); + outForward = outUp.Cross(outRight).Normalized(); +} + +Mat44 VehicleConstraint::GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const +{ + JPH_ASSERT(inWheelIndex < mWheels.size()); + + const Wheel *wheel = mWheels[inWheelIndex]; + const WheelSettings *settings = wheel->mSettings; + + // Use the two vectors provided to calculate a matrix that takes us from wheel model space to X = right, Y = up, Z = forward (the space where we will rotate the wheel) + Mat44 wheel_to_rotational = Mat44(Vec4(inWheelRight, 0), Vec4(inWheelUp, 0), Vec4(inWheelUp.Cross(inWheelRight), 0), Vec4(0, 0, 0, 1)).Transposed(); + + // Calculate the matrix that takes us from the rotational space to vehicle local space + Vec3 local_forward, local_up, local_right; + GetWheelLocalBasis(wheel, local_forward, local_up, local_right); + Vec3 local_wheel_pos = settings->mPosition + settings->mSuspensionDirection * wheel->mSuspensionLength; + Mat44 rotational_to_local(Vec4(local_right, 0), Vec4(local_up, 0), Vec4(local_forward, 0), Vec4(local_wheel_pos, 1)); + + // Calculate transform of rotated wheel + return rotational_to_local * Mat44::sRotationX(wheel->mAngle) * wheel_to_rotational; +} + +RMat44 VehicleConstraint::GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const +{ + return mBody->GetWorldTransform() * GetWheelLocalTransform(inWheelIndex, inWheelRight, inWheelUp); +} + +void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + + // Callback to higher-level systems. We do it before PreCollide, in case steering changes. + if (mPreStepCallback != nullptr) + mPreStepCallback(*this, inDeltaTime, inPhysicsSystem); + + if (mIsGravityOverridden) + { + // If gravity is overridden, we replace the normal gravity calculations + if (mBody->IsActive()) + { + MotionProperties *mp = mBody->GetMotionProperties(); + mp->SetGravityFactor(0.0f); + mBody->AddForce(mGravityOverride / mp->GetInverseMass()); + } + + // And we calculate the world up using the custom gravity + mWorldUp = (-mGravityOverride).NormalizedOr(mWorldUp); + } + else + { + // Calculate new world up vector by inverting gravity + mWorldUp = (-inPhysicsSystem.GetGravity()).NormalizedOr(mWorldUp); + } + + // Callback on our controller + mController->PreCollide(inDeltaTime, inPhysicsSystem); + + // Calculate if this constraint is active by checking if our main vehicle body is active or any of the bodies we touch are active + mIsActive = mBody->IsActive(); + + // Test how often we need to update the wheels + uint num_steps_between_collisions = mIsActive? mNumStepsBetweenCollisionTestActive : mNumStepsBetweenCollisionTestInactive; + + RMat44 body_transform = mBody->GetWorldTransform(); + + // Test collision for wheels + for (uint wheel_index = 0; wheel_index < mWheels.size(); ++wheel_index) + { + Wheel *w = mWheels[wheel_index]; + const WheelSettings *settings = w->mSettings; + + // Calculate suspension origin and direction + RVec3 ws_origin = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Test if we need to update this wheel + if (num_steps_between_collisions == 0 + || (mCurrentStep + wheel_index) % num_steps_between_collisions != 0) + { + // Simplified wheel contact test + if (!w->mContactBodyID.IsInvalid()) + { + // Test if the body is still valid + w->mContactBody = inPhysicsSystem.GetBodyLockInterfaceNoLock().TryGetBody(w->mContactBodyID); + if (w->mContactBody == nullptr) + { + // It's not, forget the contact + w->mContactBodyID = BodyID(); + w->mContactSubShapeID = SubShapeID(); + w->mSuspensionLength = settings->mSuspensionMaxLength; + } + else + { + // Extrapolate the wheel contact properties + mVehicleCollisionTester->PredictContactProperties(inPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength); + } + } + } + else + { + // Full wheel contact test, start by resetting the contact data + w->mContactBodyID = BodyID(); + w->mContactBody = nullptr; + w->mContactSubShapeID = SubShapeID(); + w->mSuspensionLength = settings->mSuspensionMaxLength; + + // Test collision to find the floor + if (mVehicleCollisionTester->Collide(inPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength)) + { + // Store ID (pointer is not valid outside of the simulation step) + w->mContactBodyID = w->mContactBody->GetID(); + } + } + + if (w->mContactBody != nullptr) + { + // Store contact velocity, cache this as the contact body may be removed + w->mContactPointVelocity = w->mContactBody->GetPointVelocity(w->mContactPosition); + + // Determine plane constant for axle contact plane + w->mAxlePlaneConstant = RVec3(w->mContactNormal).Dot(ws_origin + w->mSuspensionLength * ws_direction); + + // Check if body is active, if so the entire vehicle should be active + mIsActive |= w->mContactBody->IsActive(); + + // Determine world space forward using steering angle and body rotation + Vec3 forward, up, right; + GetWheelLocalBasis(w, forward, up, right); + forward = body_transform.Multiply3x3(forward); + right = body_transform.Multiply3x3(right); + + // The longitudinal axis is in the up/forward plane + w->mContactLongitudinal = w->mContactNormal.Cross(right); + + // Make sure that the longitudinal axis is aligned with the forward axis + if (w->mContactLongitudinal.Dot(forward) < 0.0f) + w->mContactLongitudinal = -w->mContactLongitudinal; + + // Normalize it + w->mContactLongitudinal = w->mContactLongitudinal.NormalizedOr(w->mContactNormal.GetNormalizedPerpendicular()); + + // The lateral axis is perpendicular to contact normal and longitudinal axis + w->mContactLateral = w->mContactLongitudinal.Cross(w->mContactNormal).Normalized(); + } + } + + // Callback to higher-level systems. We do it immediately after wheel collision. + if (mPostCollideCallback != nullptr) + mPostCollideCallback(*this, inDeltaTime, inPhysicsSystem); + + // Calculate anti-rollbar impulses + for (const VehicleAntiRollBar &r : mAntiRollBars) + { + Wheel *lw = mWheels[r.mLeftWheel]; + Wheel *rw = mWheels[r.mRightWheel]; + + if (lw->mContactBody != nullptr && rw->mContactBody != nullptr) + { + // Calculate the impulse to apply based on the difference in suspension length + float difference = rw->mSuspensionLength - lw->mSuspensionLength; + float impulse = difference * r.mStiffness * inDeltaTime; + lw->mAntiRollBarImpulse = -impulse; + rw->mAntiRollBarImpulse = impulse; + } + else + { + // When one of the wheels is not on the ground we don't apply any impulses + lw->mAntiRollBarImpulse = rw->mAntiRollBarImpulse = 0.0f; + } + } + + // Callback on our controller + mController->PostCollide(inDeltaTime, inPhysicsSystem); + + // Callback to higher-level systems. We do it before the sleep section, in case velocities change. + if (mPostStepCallback != nullptr) + mPostStepCallback(*this, inDeltaTime, inPhysicsSystem); + + // If the wheels are rotating, we don't want to go to sleep yet + bool allow_sleep = mController->AllowSleep(); + if (allow_sleep) + for (const Wheel *w : mWheels) + if (abs(w->mAngularVelocity) > DegreesToRadians(10.0f)) + { + allow_sleep = false; + break; + } + if (mBody->GetAllowSleeping() != allow_sleep) + mBody->SetAllowSleeping(allow_sleep); + + // Increment step counter + ++mCurrentStep; +} + +void VehicleConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) +{ + // Find dynamic bodies that our wheels are touching + BodyID *body_ids = (BodyID *)JPH_STACK_ALLOC((mWheels.size() + 1) * sizeof(BodyID)); + int num_bodies = 0; + bool needs_to_activate = false; + for (const Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + // Avoid adding duplicates + bool duplicate = false; + BodyID id = w->mContactBody->GetID(); + for (int i = 0; i < num_bodies; ++i) + if (body_ids[i] == id) + { + duplicate = true; + break; + } + if (duplicate) + continue; + + if (w->mContactBody->IsDynamic()) + { + body_ids[num_bodies++] = id; + needs_to_activate |= !w->mContactBody->IsActive(); + } + } + + // Activate bodies, note that if we get here we have already told the system that we're active so that means our main body needs to be active too + if (!mBody->IsActive()) + { + // Our main body is not active, activate it too + body_ids[num_bodies] = mBody->GetID(); + inBodyManager.ActivateBodies(body_ids, num_bodies + 1); + } + else if (needs_to_activate) + { + // Only activate bodies the wheels are touching + inBodyManager.ActivateBodies(body_ids, num_bodies); + } + + // Link the bodies into the same island + uint32 min_active_index = Body::cInactiveIndex; + for (int i = 0; i < num_bodies; ++i) + { + const Body &body = inBodyManager.GetBody(body_ids[i]); + min_active_index = min(min_active_index, body.GetIndexInActiveBodiesInternal()); + ioBuilder.LinkBodies(mBody->GetIndexInActiveBodiesInternal(), body.GetIndexInActiveBodiesInternal()); + } + + // Link the constraint in the island + ioBuilder.LinkConstraint(inConstraintIndex, mBody->GetIndexInActiveBodiesInternal(), min_active_index); +} + +uint VehicleConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const +{ + return ioSplitter.AssignToNonParallelSplit(mBody); +} + +void VehicleConstraint::CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const +{ + // Determine point to apply force to + RVec3 force_point; + if (inWheel.mSettings->mEnableSuspensionForcePoint) + force_point = mBody->GetWorldTransform() * inWheel.mSettings->mSuspensionForcePoint; + else + force_point = inWheel.mContactPosition; + + // Calculate r1 + u and r2 + outR1PlusU = Vec3(force_point - mBody->GetCenterOfMassPosition()); + outR2 = Vec3(force_point - inWheel.mContactBody->GetCenterOfMassPosition()); +} + +void VehicleConstraint::CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform) +{ + // Check if a limit was specified + if (mCosMaxPitchRollAngle > -1.0f) + { + // Calculate cos of angle between world up vector and vehicle up vector + Vec3 vehicle_up = inBodyTransform.Multiply3x3(mUp); + mCosPitchRollAngle = mWorldUp.Dot(vehicle_up); + if (mCosPitchRollAngle < mCosMaxPitchRollAngle) + { + // Calculate rotation axis to rotate vehicle towards up + Vec3 rotation_axis = mWorldUp.Cross(vehicle_up); + float len = rotation_axis.Length(); + if (len > 0.0f) + mPitchRollRotationAxis = rotation_axis / len; + + mPitchRollPart.CalculateConstraintProperties(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis); + } + else + mPitchRollPart.Deactivate(); + } + else + mPitchRollPart.Deactivate(); +} + +void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime) +{ + RMat44 body_transform = mBody->GetWorldTransform(); + + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + const WheelSettings *settings = w->mSettings; + + Vec3 neg_contact_normal = -w->mContactNormal; + + Vec3 r1_plus_u, r2; + CalculateSuspensionForcePoint(*w, r1_plus_u, r2); + + // Suspension spring + if (settings->mSuspensionMaxLength > settings->mSuspensionMinLength) + { + float stiffness, damping; + if (settings->mSuspensionSpring.mMode == ESpringMode::FrequencyAndDamping) + { + // Calculate effective mass based on vehicle configuration (the stiffness of the spring should not be affected by the dynamics of the vehicle): K = 1 / (J M^-1 J^T) + // Note that if no suspension force point is supplied we don't know where the force is applied so we assume it is applied at average suspension length + Vec3 force_point = settings->mEnableSuspensionForcePoint? settings->mSuspensionForcePoint : settings->mPosition + 0.5f * (settings->mSuspensionMinLength + settings->mSuspensionMaxLength) * settings->mSuspensionDirection; + Vec3 force_point_x_neg_up = force_point.Cross(-mUp); + const MotionProperties *mp = mBody->GetMotionProperties(); + float effective_mass = 1.0f / (mp->GetInverseMass() + force_point_x_neg_up.Dot(mp->GetLocalSpaceInverseInertia().Multiply3x3(force_point_x_neg_up))); + + // Convert frequency and damping to stiffness and damping + float omega = 2.0f * JPH_PI * settings->mSuspensionSpring.mFrequency; + stiffness = effective_mass * Square(omega); + damping = 2.0f * effective_mass * settings->mSuspensionSpring.mDamping * omega; + } + else + { + // In this case we can simply copy the properties + stiffness = settings->mSuspensionSpring.mStiffness; + damping = settings->mSuspensionSpring.mDamping; + } + + // Calculate the damping and frequency of the suspension spring given the angle between the suspension direction and the contact normal + // If the angle between the suspension direction and the inverse of the contact normal is alpha then the force on the spring relates to the force along the contact normal as: + // + // Fspring = Fnormal * cos(alpha) + // + // The spring force is: + // + // Fspring = -k * x + // + // where k is the spring constant and x is the displacement of the spring. So we have: + // + // Fnormal * cos(alpha) = -k * x <=> Fnormal = -k / cos(alpha) * x + // + // So we can see this as a spring with spring constant: + // + // k' = k / cos(alpha) + // + // In the same way the velocity relates like: + // + // Vspring = Vnormal * cos(alpha) + // + // Which results in the modified damping constant c: + // + // c' = c / cos(alpha) + // + // Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much. + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + float cos_angle = max(0.1f, ws_direction.Dot(neg_contact_normal)); + stiffness /= cos_angle; + damping /= cos_angle; + + // Get the value of the constraint equation + float c = w->mSuspensionLength - settings->mSuspensionMaxLength - settings->mSuspensionPreloadLength; + + w->mSuspensionPart.CalculateConstraintPropertiesWithStiffnessAndDamping(inDeltaTime, *mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal, w->mAntiRollBarImpulse, c, stiffness, damping); + } + else + w->mSuspensionPart.Deactivate(); + + // Check if we reached the 'max up' position and if so add a hard velocity constraint that stops any further movement in the normal direction + if (w->mSuspensionLength < settings->mSuspensionMinLength) + w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal); + else + w->mSuspensionMaxUpPart.Deactivate(); + + // Friction and propulsion + w->mLongitudinalPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLongitudinal); + w->mLateralPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, -w->mContactLateral); + } + else + { + // No contact -> disable everything + w->mSuspensionPart.Deactivate(); + w->mSuspensionMaxUpPart.Deactivate(); + w->mLongitudinalPart.Deactivate(); + w->mLateralPart.Deactivate(); + } + + CalculatePitchRollConstraintProperties(body_transform); +} + +void VehicleConstraint::ResetWarmStart() +{ + for (Wheel *w : mWheels) + { + w->mSuspensionPart.Deactivate(); + w->mSuspensionMaxUpPart.Deactivate(); + w->mLongitudinalPart.Deactivate(); + w->mLateralPart.Deactivate(); + } + + mPitchRollPart.Deactivate(); +} + +void VehicleConstraint::WarmStartVelocityConstraint(float inWarmStartImpulseRatio) +{ + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + w->mSuspensionPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio); + w->mSuspensionMaxUpPart.WarmStart(*mBody, *w->mContactBody, neg_contact_normal, inWarmStartImpulseRatio); + w->mLongitudinalPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLongitudinal, 0.0f); // Don't warm start the longitudinal part (the engine/brake force, we don't want to preserve anything from the last frame) + w->mLateralPart.WarmStart(*mBody, *w->mContactBody, -w->mContactLateral, inWarmStartImpulseRatio); + } + + mPitchRollPart.WarmStart(*mBody, Body::sFixedToWorld, inWarmStartImpulseRatio); +} + +bool VehicleConstraint::SolveVelocityConstraint(float inDeltaTime) +{ + bool impulse = false; + + // Solve suspension + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + // Suspension spring, note that it can only push and not pull + if (w->mSuspensionPart.IsActive()) + impulse |= w->mSuspensionPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX); + + // When reaching the minimal suspension length only allow forces pushing the bodies away + if (w->mSuspensionMaxUpPart.IsActive()) + impulse |= w->mSuspensionMaxUpPart.SolveVelocityConstraint(*mBody, *w->mContactBody, neg_contact_normal, 0.0f, FLT_MAX); + } + + // Solve the horizontal movement of the vehicle + impulse |= mController->SolveLongitudinalAndLateralConstraints(inDeltaTime); + + // Apply the pitch / roll constraint to avoid the vehicle from toppling over + if (mPitchRollPart.IsActive()) + impulse |= mPitchRollPart.SolveVelocityConstraint(*mBody, Body::sFixedToWorld, mPitchRollRotationAxis, 0, FLT_MAX); + + return impulse; +} + +bool VehicleConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumgarte) +{ + bool impulse = false; + + RMat44 body_transform = mBody->GetWorldTransform(); + + for (Wheel *w : mWheels) + if (w->mContactBody != nullptr) + { + const WheelSettings *settings = w->mSettings; + + // Check if we reached the 'max up' position now that the body has possibly moved + // We do this by calculating the axle position at minimum suspension length and making sure it does not go through the + // plane defined by the contact normal and the axle position when the contact happened + // TODO: This assumes that only the vehicle moved and not the ground as we kept the axle contact plane in world space + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + RVec3 ws_position = body_transform * settings->mPosition; + RVec3 min_suspension_pos = ws_position + settings->mSuspensionMinLength * ws_direction; + float max_up_error = float(RVec3(w->mContactNormal).Dot(min_suspension_pos) - w->mAxlePlaneConstant); + if (max_up_error < 0.0f) + { + Vec3 neg_contact_normal = -w->mContactNormal; + + // Recalculate constraint properties since the body may have moved + Vec3 r1_plus_u, r2; + CalculateSuspensionForcePoint(*w, r1_plus_u, r2); + w->mSuspensionMaxUpPart.CalculateConstraintProperties(*mBody, r1_plus_u, *w->mContactBody, r2, neg_contact_normal); + + impulse |= w->mSuspensionMaxUpPart.SolvePositionConstraint(*mBody, *w->mContactBody, neg_contact_normal, max_up_error, inBaumgarte); + } + } + + // Apply the pitch / roll constraint to avoid the vehicle from toppling over + CalculatePitchRollConstraintProperties(body_transform); + if (mPitchRollPart.IsActive()) + impulse |= mPitchRollPart.SolvePositionConstraint(*mBody, Body::sFixedToWorld, mCosPitchRollAngle - mCosMaxPitchRollAngle, inBaumgarte); + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void VehicleConstraint::DrawConstraint(DebugRenderer *inRenderer) const +{ + mController->Draw(inRenderer); +} + +void VehicleConstraint::DrawConstraintLimits(DebugRenderer *inRenderer) const +{ +} + +#endif // JPH_DEBUG_RENDERER + +void VehicleConstraint::SaveState(StateRecorder &inStream) const +{ + Constraint::SaveState(inStream); + + mController->SaveState(inStream); + + for (const Wheel *w : mWheels) + { + inStream.Write(w->mAngularVelocity); + inStream.Write(w->mAngle); + inStream.Write(w->mContactBodyID); // Used by MotorcycleController::PreCollide + inStream.Write(w->mContactPosition); // Used by VehicleCollisionTester::PredictContactProperties + inStream.Write(w->mContactNormal); // Used by MotorcycleController::PreCollide + inStream.Write(w->mContactLateral); // Used by MotorcycleController::PreCollide + inStream.Write(w->mSuspensionLength); // Used by VehicleCollisionTester::PredictContactProperties + + w->mSuspensionPart.SaveState(inStream); + w->mSuspensionMaxUpPart.SaveState(inStream); + w->mLongitudinalPart.SaveState(inStream); + w->mLateralPart.SaveState(inStream); + } + + inStream.Write(mPitchRollRotationAxis); // When rotation is too small we use last frame so we need to store it + mPitchRollPart.SaveState(inStream); + inStream.Write(mCurrentStep); +} + +void VehicleConstraint::RestoreState(StateRecorder &inStream) +{ + Constraint::RestoreState(inStream); + + mController->RestoreState(inStream); + + for (Wheel *w : mWheels) + { + inStream.Read(w->mAngularVelocity); + inStream.Read(w->mAngle); + inStream.Read(w->mContactBodyID); + inStream.Read(w->mContactPosition); + inStream.Read(w->mContactNormal); + inStream.Read(w->mContactLateral); + inStream.Read(w->mSuspensionLength); + w->mContactBody = nullptr; // No longer valid + + w->mSuspensionPart.RestoreState(inStream); + w->mSuspensionMaxUpPart.RestoreState(inStream); + w->mLongitudinalPart.RestoreState(inStream); + w->mLateralPart.RestoreState(inStream); + } + + inStream.Read(mPitchRollRotationAxis); + mPitchRollPart.RestoreState(inStream); + inStream.Read(mCurrentStep); +} + +Ref VehicleConstraint::GetConstraintSettings() const +{ + JPH_ASSERT(false); // Not implemented yet + return nullptr; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.h new file mode 100644 index 000000000..cbe4d7a5e --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleConstraint.h @@ -0,0 +1,246 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// Configuration for constraint that simulates a wheeled vehicle. +/// +/// The properties in this constraint are largely based on "Car Physics for Games" by Marco Monster. +/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html +class JPH_EXPORT VehicleConstraintSettings : public ConstraintSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, VehicleConstraintSettings) + + /// Saves the contents of the constraint settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const override; + + Vec3 mUp { 0, 1, 0 }; ///< Vector indicating the up direction of the vehicle (in local space to the body) + Vec3 mForward { 0, 0, 1 }; ///< Vector indicating forward direction of the vehicle (in local space to the body) + float mMaxPitchRollAngle = JPH_PI; ///< Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off. + Array> mWheels; ///< List of wheels and their properties + Array mAntiRollBars; ///< List of anti rollbars and their properties + Ref mController; ///< Defines how the vehicle can accelerate / decelerate + +protected: + /// This function should not be called directly, it is used by sRestoreFromBinaryState. + virtual void RestoreBinaryState(StreamIn &inStream) override; +}; + +/// Constraint that simulates a vehicle +/// Note: Don't forget to register the constraint as a StepListener with the PhysicsSystem! +/// +/// When the vehicle drives over very light objects (rubble) you may see the car body dip down. This is a known issue and is an artifact of the iterative solver that Jolt is using. +/// Basically if a light object is sandwiched between two heavy objects (the static floor and the car body), the light object is not able to transfer enough force from the ground to +/// the car body to keep the car body up. You can see this effect in the HeavyOnLightTest sample, the boxes on the right have a lot of penetration because they're on top of light objects. +/// +/// There are a couple of ways to improve this: +/// +/// 1. You can increase the number of velocity steps (global settings PhysicsSettings::mNumVelocitySteps or if you only want to increase it on +/// the vehicle you can use VehicleConstraintSettings::mNumVelocityStepsOverride). E.g. going from 10 to 30 steps in the HeavyOnLightTest sample makes the penetration a lot less. +/// The number of position steps can also be increased (the first prevents the body from going down, the second corrects it if the problem did +/// occur which inevitably happens due to numerical drift). This solution costs CPU cycles. +/// +/// 2. You can reduce the mass difference between the vehicle body and the rubble on the floor (by making the rubble heavier or the car lighter). +/// +/// 3. You could filter out collisions between the vehicle collision test and the rubble completely. This would make the wheels ignore the rubble but would cause the vehicle to drive +/// through it as if nothing happened. You could create fake wheels (keyframed bodies) that move along with the vehicle and that only collide with rubble (and not the vehicle or the ground). +/// This would cause the vehicle to push away the rubble without the rubble being able to affect the vehicle (unless it hits the main body of course). +/// +/// Note that when driving over rubble, you may see the wheel jump up and down quite quickly because one frame a collision is found and the next frame not. +/// To alleviate this, it may be needed to smooth the motion of the visual mesh for the wheel. +class JPH_EXPORT VehicleConstraint : public Constraint, public PhysicsStepListener +{ +public: + /// Constructor / destructor + VehicleConstraint(Body &inVehicleBody, const VehicleConstraintSettings &inSettings); + virtual ~VehicleConstraint() override; + + /// Get the type of a constraint + virtual EConstraintSubType GetSubType() const override { return EConstraintSubType::Vehicle; } + + /// Defines the maximum pitch/roll angle (rad), can be used to avoid the car from getting upside down. The vehicle up direction will stay within a cone centered around the up axis with half top angle mMaxPitchRollAngle, set to pi to turn off. + void SetMaxPitchRollAngle(float inMaxPitchRollAngle) { mCosMaxPitchRollAngle = Cos(inMaxPitchRollAngle); } + + /// Set the interface that tests collision between wheel and ground + void SetVehicleCollisionTester(const VehicleCollisionTester *inTester) { mVehicleCollisionTester = inTester; } + + /// Callback function to combine the friction of a tire with the friction of the body it is colliding with. + /// On input ioLongitudinalFriction and ioLateralFriction contain the friction of the tire, on output they should contain the combined friction with inBody2. + using CombineFunction = function; + + /// Set the function that combines the friction of two bodies and returns it + /// Default method is the geometric mean: sqrt(friction1 * friction2). + void SetCombineFriction(const CombineFunction &inCombineFriction) { mCombineFriction = inCombineFriction; } + const CombineFunction & GetCombineFriction() const { return mCombineFriction; } + + /// Callback function to notify of current stage in PhysicsStepListener::OnStep. + using StepCallback = function; + + /// Callback function to notify that PhysicsStepListener::OnStep has started for this vehicle. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. control steering. This is the last moment that the position/orientation of the vehicle can be changed. + /// Wheel collision checks have not been performed yet. + const StepCallback & GetPreStepCallback() const { return mPreStepCallback; } + void SetPreStepCallback(const StepCallback &inPreStepCallback) { mPreStepCallback = inPreStepCallback; } + + /// Callback function to notify that PhysicsStepListener::OnStep has just completed wheel collision checks. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. detect tire contact or to modify the velocity of the vehicle based on the wheel contacts. + /// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed. + const StepCallback & GetPostCollideCallback() const { return mPostCollideCallback; } + void SetPostCollideCallback(const StepCallback &inPostCollideCallback) { mPostCollideCallback = inPostCollideCallback; } + + /// Callback function to notify that PhysicsStepListener::OnStep has completed for this vehicle. Default is to do nothing. + /// Can be used to allow higher-level code to e.g. control the vehicle in the air. + /// You should not change the position of the vehicle in this callback as the wheel collision checks have already been performed. + const StepCallback & GetPostStepCallback() const { return mPostStepCallback; } + void SetPostStepCallback(const StepCallback &inPostStepCallback) { mPostStepCallback = inPostStepCallback; } + + /// Override gravity for this vehicle. Note that overriding gravity will set the gravity factor of the vehicle body to 0 and apply gravity in the PhysicsStepListener instead. + void OverrideGravity(Vec3Arg inGravity) { mGravityOverride = inGravity; mIsGravityOverridden = true; } + bool IsGravityOverridden() const { return mIsGravityOverridden; } + Vec3 GetGravityOverride() const { return mGravityOverride; } + void ResetGravityOverride() { mIsGravityOverridden = false; mBody->GetMotionProperties()->SetGravityFactor(1.0f); } ///< Note that resetting the gravity override will restore the gravity factor of the vehicle body to 1. + + /// Get the local space forward vector of the vehicle + Vec3 GetLocalForward() const { return mForward; } + + /// Get the local space up vector of the vehicle + Vec3 GetLocalUp() const { return mUp; } + + /// Vector indicating the world space up direction (used to limit vehicle pitch/roll), calculated every frame by inverting gravity + Vec3 GetWorldUp() const { return mWorldUp; } + + /// Access to the vehicle body + Body * GetVehicleBody() const { return mBody; } + + /// Access to the vehicle controller interface (determines acceleration / deceleration) + const VehicleController * GetController() const { return mController; } + + /// Access to the vehicle controller interface (determines acceleration / deceleration) + VehicleController * GetController() { return mController; } + + /// Get the state of the wheels + const Wheels & GetWheels() const { return mWheels; } + + /// Get the state of a wheels (writable interface, allows you to make changes to the configuration which will take effect the next time step) + Wheels & GetWheels() { return mWheels; } + + /// Get the state of a wheel + Wheel * GetWheel(uint inIdx) { return mWheels[inIdx]; } + const Wheel * GetWheel(uint inIdx) const { return mWheels[inIdx]; } + + /// Get the basis vectors for the wheel in local space to the vehicle body (note: basis does not rotate when the wheel rotates around its axis) + /// @param inWheel Wheel to fetch basis for + /// @param outForward Forward vector for the wheel + /// @param outUp Up vector for the wheel + /// @param outRight Right vector for the wheel + void GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const; + + /// Get the transform of a wheel in local space to the vehicle body, returns a matrix that transforms a cylinder aligned with the Y axis in body space (not COM space) + /// @param inWheelIndex Index of the wheel to fetch + /// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels) + /// @param inWheelUp Unit vector that indicates up in model space of the wheel + Mat44 GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const; + + /// Get the transform of a wheel in world space, returns a matrix that transforms a cylinder aligned with the Y axis in world space + /// @param inWheelIndex Index of the wheel to fetch + /// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels) + /// @param inWheelUp Unit vector that indicates up in model space of the wheel + RMat44 GetWheelWorldTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const; + + /// Number of simulation steps between wheel collision tests when the vehicle is active. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc. + /// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion. + /// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle. + /// If you set this to test less than every step, you may see simulation artifacts. This setting can be used to reduce the cost of simulating vehicles in the distance. + void SetNumStepsBetweenCollisionTestActive(uint inSteps) { mNumStepsBetweenCollisionTestActive = inSteps; } + uint GetNumStepsBetweenCollisionTestActive() const { return mNumStepsBetweenCollisionTestActive; } + + /// Number of simulation steps between wheel collision tests when the vehicle is inactive. Default is 1. 0 = never, 1 = every step, 2 = every other step, etc. + /// Note that if a vehicle has multiple wheels and the number of steps > 1, the wheels will be tested in a round robin fashion. + /// If there are multiple vehicles, the tests will be spread out based on the BodyID of the vehicle. + /// This number can be lower than the number of steps when the vehicle is active as the only purpose of this test is + /// to allow the vehicle to wake up in response to bodies moving into the wheels but not touching the body of the vehicle. + void SetNumStepsBetweenCollisionTestInactive(uint inSteps) { mNumStepsBetweenCollisionTestInactive = inSteps; } + uint GetNumStepsBetweenCollisionTestInactive() const { return mNumStepsBetweenCollisionTestInactive; } + + // Generic interface of a constraint + virtual bool IsActive() const override { return mIsActive && Constraint::IsActive(); } + virtual void NotifyShapeChanged(const BodyID &inBodyID, Vec3Arg inDeltaCOM) override { /* Do nothing */ } + virtual void SetupVelocityConstraint(float inDeltaTime) override; + virtual void ResetWarmStart() override; + virtual void WarmStartVelocityConstraint(float inWarmStartImpulseRatio) override; + virtual bool SolveVelocityConstraint(float inDeltaTime) override; + virtual bool SolvePositionConstraint(float inDeltaTime, float inBaumgarte) override; + virtual void BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager) override; + virtual uint BuildIslandSplits(LargeIslandSplitter &ioSplitter) const override; +#ifdef JPH_DEBUG_RENDERER + virtual void DrawConstraint(DebugRenderer *inRenderer) const override; + virtual void DrawConstraintLimits(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; + virtual Ref GetConstraintSettings() const override; + +private: + // See: PhysicsStepListener + virtual void OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + + // Calculate the position where the suspension and traction forces should be applied in world space, relative to the center of mass of both bodies + void CalculateSuspensionForcePoint(const Wheel &inWheel, Vec3 &outR1PlusU, Vec3 &outR2) const; + + // Calculate the constraint properties for mPitchRollPart + void CalculatePitchRollConstraintProperties(RMat44Arg inBodyTransform); + + // Gravity override + bool mIsGravityOverridden = false; ///< If the gravity is currently overridden + Vec3 mGravityOverride = Vec3::sZero(); ///< Gravity override value, replaces PhysicsSystem::GetGravity() when mIsGravityOverridden is true + + // Simulation information + Body * mBody; ///< Body of the vehicle + Vec3 mForward; ///< Local space forward vector for the vehicle + Vec3 mUp; ///< Local space up vector for the vehicle + Vec3 mWorldUp; ///< Vector indicating the world space up direction (used to limit vehicle pitch/roll) + Wheels mWheels; ///< Wheel states of the vehicle + Array mAntiRollBars; ///< Anti rollbars of the vehicle + VehicleController * mController; ///< Controls the acceleration / deceleration of the vehicle + bool mIsActive = false; ///< If this constraint is active + uint mNumStepsBetweenCollisionTestActive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is active + uint mNumStepsBetweenCollisionTestInactive = 1; ///< Number of simulation steps between wheel collision tests when the vehicle is inactive + uint mCurrentStep = 0; ///< Current step number, used to determine when to test a wheel + + // Prevent vehicle from toppling over + float mCosMaxPitchRollAngle; ///< Cos of the max pitch/roll angle + float mCosPitchRollAngle; ///< Cos of the current pitch/roll angle + Vec3 mPitchRollRotationAxis { 0, 1, 0 }; ///< Current axis along which to apply torque to prevent the car from toppling over + AngleConstraintPart mPitchRollPart; ///< Constraint part that prevents the car from toppling over + + // Interfaces + RefConst mVehicleCollisionTester; ///< Class that performs testing of collision for the wheels + CombineFunction mCombineFriction = [](uint, float &ioLongitudinalFriction, float &ioLateralFriction, const Body &inBody2, const SubShapeID &) + { + float body_friction = inBody2.GetFriction(); + + ioLongitudinalFriction = sqrt(ioLongitudinalFriction * body_friction); + ioLateralFriction = sqrt(ioLateralFriction * body_friction); + }; + + // Callbacks + StepCallback mPreStepCallback; + StepCallback mPostCollideCallback; + StepCallback mPostStepCallback; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleController.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleController.cpp new file mode 100644 index 000000000..ffa8ff1a6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleController.cpp @@ -0,0 +1,17 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(VehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(VehicleControllerSettings, SerializableObject) +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleController.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleController.h new file mode 100644 index 000000000..b916a8bfa --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleController.h @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; +class VehicleController; +class VehicleConstraint; +class WheelSettings; +class Wheel; +class StateRecorder; + +/// Basic settings object for interface that controls acceleration / deceleration of the vehicle +class JPH_EXPORT VehicleControllerSettings : public SerializableObject, public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_ABSTRACT(JPH_EXPORT, VehicleControllerSettings) + + /// Saves the contents of the controller settings in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const = 0; + + /// Restore the contents of the controller settings in binary form from inStream. + virtual void RestoreBinaryState(StreamIn &inStream) = 0; + + /// Create an instance of the vehicle controller class + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const = 0; +}; + +/// Runtime data for interface that controls acceleration / deceleration of the vehicle +class JPH_EXPORT VehicleController : public RefTarget, public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / destructor + explicit VehicleController(VehicleConstraint &inConstraint) : mConstraint(inConstraint) { } + virtual ~VehicleController() = default; + +protected: + // The functions below are only for the VehicleConstraint + friend class VehicleConstraint; + + // Create a new instance of wheel + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const = 0; + + // If the vehicle is allowed to go to sleep + virtual bool AllowSleep() const = 0; + + // Called before the wheel probes have been done + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0; + + // Called after the wheel probes have been done + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) = 0; + + // Solve longitudinal and lateral constraint parts for all of the wheels + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) = 0; + + // Saving state for replay + virtual void SaveState(StateRecorder &inStream) const = 0; + virtual void RestoreState(StateRecorder &inStream) = 0; + +#ifdef JPH_DEBUG_RENDERER + // Drawing interface + virtual void Draw(DebugRenderer *inRenderer) const = 0; +#endif // JPH_DEBUG_RENDERER + + VehicleConstraint & mConstraint; ///< The vehicle constraint we belong to +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.cpp new file mode 100644 index 000000000..ef7cf4cb9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.cpp @@ -0,0 +1,81 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleDifferentialSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftWheel) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mRightWheel) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mDifferentialRatio) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLeftRightSplit) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mLimitedSlipRatio) + JPH_ADD_ATTRIBUTE(VehicleDifferentialSettings, mEngineTorqueRatio) +} + +void VehicleDifferentialSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mLeftWheel); + inStream.Write(mRightWheel); + inStream.Write(mDifferentialRatio); + inStream.Write(mLeftRightSplit); + inStream.Write(mLimitedSlipRatio); + inStream.Write(mEngineTorqueRatio); +} + +void VehicleDifferentialSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mLeftWheel); + inStream.Read(mRightWheel); + inStream.Read(mDifferentialRatio); + inStream.Read(mLeftRightSplit); + inStream.Read(mLimitedSlipRatio); + inStream.Read(mEngineTorqueRatio); +} + +void VehicleDifferentialSettings::CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const +{ + // Start with the default torque ratio + outLeftTorqueFraction = 1.0f - mLeftRightSplit; + outRightTorqueFraction = mLeftRightSplit; + + if (mLimitedSlipRatio < FLT_MAX) + { + JPH_ASSERT(mLimitedSlipRatio > 1.0f); + + // This is a limited slip differential, adjust torque ratios according to wheel speeds + float omega_l = max(1.0e-3f, abs(inLeftAngularVelocity)); // prevent div by zero by setting a minimum velocity and ignoring that the wheels may be rotating in different directions + float omega_r = max(1.0e-3f, abs(inRightAngularVelocity)); + float omega_min = min(omega_l, omega_r); + float omega_max = max(omega_l, omega_r); + + // Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mLimitedSlipRotationRatio + float alpha = min((omega_max / omega_min - 1.0f) / (mLimitedSlipRatio - 1.0f), 1.0f); + JPH_ASSERT(alpha >= 0.0f); + float one_min_alpha = 1.0f - alpha; + + if (omega_l < omega_r) + { + // Redirect more power to the left wheel + outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha + alpha; + outRightTorqueFraction = outRightTorqueFraction * one_min_alpha; + } + else + { + // Redirect more power to the right wheel + outLeftTorqueFraction = outLeftTorqueFraction * one_min_alpha; + outRightTorqueFraction = outRightTorqueFraction * one_min_alpha + alpha; + } + } + + // Assert the values add up to 1 + JPH_ASSERT(abs(outLeftTorqueFraction + outRightTorqueFraction - 1.0f) < 1.0e-6f); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.h new file mode 100644 index 000000000..34a501153 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleDifferential.h @@ -0,0 +1,39 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class JPH_EXPORT VehicleDifferentialSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleDifferentialSettings) + + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + /// Calculate the torque ratio between left and right wheel + /// @param inLeftAngularVelocity Angular velocity of left wheel (rad / s) + /// @param inRightAngularVelocity Angular velocity of right wheel (rad / s) + /// @param outLeftTorqueFraction Fraction of torque that should go to the left wheel + /// @param outRightTorqueFraction Fraction of torque that should go to the right wheel + void CalculateTorqueRatio(float inLeftAngularVelocity, float inRightAngularVelocity, float &outLeftTorqueFraction, float &outRightTorqueFraction) const; + + int mLeftWheel = -1; ///< Index (in mWheels) that represents the left wheel of this differential (can be -1 to indicate no wheel) + int mRightWheel = -1; ///< Index (in mWheels) that represents the right wheel of this differential (can be -1 to indicate no wheel) + float mDifferentialRatio = 3.42f; ///< Ratio between rotation speed of gear box and wheels + float mLeftRightSplit = 0.5f; ///< Defines how the engine torque is split across the left and right wheel (0 = left, 0.5 = center, 1 = right) + float mLimitedSlipRatio = 1.4f; ///< Ratio max / min wheel speed. When this ratio is exceeded, all torque gets distributed to the slowest moving wheel. This allows implementing a limited slip differential. Set to FLT_MAX for an open differential. Value should be > 1. + float mEngineTorqueRatio = 1.0f; ///< How much of the engines torque is applied to this differential (0 = none, 1 = full), make sure the sum of all differentials is 1. +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.cpp new file mode 100644 index 000000000..1352cc0d5 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.cpp @@ -0,0 +1,122 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleEngineSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxTorque) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMinRPM) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mMaxRPM) + JPH_ADD_ATTRIBUTE(VehicleEngineSettings, mNormalizedTorque) +} + +VehicleEngineSettings::VehicleEngineSettings() +{ + mNormalizedTorque.Reserve(3); + mNormalizedTorque.AddPoint(0.0f, 0.8f); + mNormalizedTorque.AddPoint(0.66f, 1.0f); + mNormalizedTorque.AddPoint(1.0f, 0.8f); +} + +void VehicleEngineSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMaxTorque); + inStream.Write(mMinRPM); + inStream.Write(mMaxRPM); + mNormalizedTorque.SaveBinaryState(inStream); +} + +void VehicleEngineSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMaxTorque); + inStream.Read(mMinRPM); + inStream.Read(mMaxRPM); + mNormalizedTorque.RestoreBinaryState(inStream); +} + +void VehicleEngine::ApplyTorque(float inTorque, float inDeltaTime) +{ + // Accelerate engine using torque + mCurrentRPM += cAngularVelocityToRPM * inTorque * inDeltaTime / mInertia; + ClampRPM(); +} + +void VehicleEngine::ApplyDamping(float inDeltaTime) +{ + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mCurrentRPM *= max(0.0f, 1.0f - mAngularDamping * inDeltaTime); + ClampRPM(); +} + +#ifdef JPH_DEBUG_RENDERER + +void VehicleEngine::DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const +{ + // Function to draw part of a pie + auto draw_pie = [this, inRenderer, inSize, inPosition, inForward, inUp](float inMinRPM, float inMaxRPM, Color inColor) { + inRenderer->DrawPie(inPosition, inSize, inForward, inUp, ConvertRPMToAngle(inMinRPM), ConvertRPMToAngle(inMaxRPM), inColor, DebugRenderer::ECastShadow::Off); + }; + + // Draw segment under min RPM + draw_pie(0, mMinRPM, Color::sGrey); + + // Draw segment until inShiftDownRPM + if (mCurrentRPM < inShiftDownRPM) + { + draw_pie(mMinRPM, mCurrentRPM, Color::sRed); + draw_pie(mCurrentRPM, inShiftDownRPM, Color::sDarkRed); + } + else + { + draw_pie(mMinRPM, inShiftDownRPM, Color::sRed); + } + + // Draw segment between inShiftDownRPM and inShiftUpRPM + if (mCurrentRPM > inShiftDownRPM && mCurrentRPM < inShiftUpRPM) + { + draw_pie(inShiftDownRPM, mCurrentRPM, Color::sOrange); + draw_pie(mCurrentRPM, inShiftUpRPM, Color::sDarkOrange); + } + else + { + draw_pie(inShiftDownRPM, inShiftUpRPM, mCurrentRPM <= inShiftDownRPM? Color::sDarkOrange : Color::sOrange); + } + + // Draw segment above inShiftUpRPM + if (mCurrentRPM > inShiftUpRPM) + { + draw_pie(inShiftUpRPM, mCurrentRPM, Color::sGreen); + draw_pie(mCurrentRPM, mMaxRPM, Color::sDarkGreen); + } + else + { + draw_pie(inShiftUpRPM, mMaxRPM, Color::sDarkGreen); + } +} + +#endif // JPH_DEBUG_RENDERER + +void VehicleEngine::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mCurrentRPM); +} + +void VehicleEngine::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mCurrentRPM); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.h new file mode 100644 index 000000000..25cf6391b --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleEngine.h @@ -0,0 +1,93 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER + class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Generic properties for a vehicle engine +class JPH_EXPORT VehicleEngineSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleEngineSettings) + + /// Constructor + VehicleEngineSettings(); + + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + float mMaxTorque = 500.0f; ///< Max amount of torque (Nm) that the engine can deliver + float mMinRPM = 1000.0f; ///< Min amount of revolutions per minute (rpm) the engine can produce without stalling + float mMaxRPM = 6000.0f; ///< Max amount of revolutions per minute (rpm) the engine can generate + LinearCurve mNormalizedTorque; ///< Y-axis: Curve that describes a ratio of the max torque the engine can produce (0 = 0, 1 = mMaxTorque). X-axis: the fraction of the RPM of the engine (0 = mMinRPM, 1 = mMaxRPM) + float mInertia = 0.5f; ///< Moment of inertia (kg m^2) of the engine + float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w +}; + +/// Runtime data for engine +class JPH_EXPORT VehicleEngine : public VehicleEngineSettings +{ +public: + /// Multiply an angular velocity (rad/s) with this value to get rounds per minute (RPM) + static constexpr float cAngularVelocityToRPM = 60.0f / (2.0f * JPH_PI); + + /// Clamp the RPM between min and max RPM + inline void ClampRPM() { mCurrentRPM = Clamp(mCurrentRPM, mMinRPM, mMaxRPM); } + + /// Current rotation speed of engine in rounds per minute + float GetCurrentRPM() const { return mCurrentRPM; } + + /// Update rotation speed of engine in rounds per minute + void SetCurrentRPM(float inRPM) { mCurrentRPM = inRPM; ClampRPM(); } + + /// Get current angular velocity of the engine in radians / second + inline float GetAngularVelocity() const { return mCurrentRPM / cAngularVelocityToRPM; } + + /// Get the amount of torque (N m) that the engine can supply + /// @param inAcceleration How much the gas pedal is pressed [0, 1] + float GetTorque(float inAcceleration) const { return inAcceleration * mMaxTorque * mNormalizedTorque.GetValue(mCurrentRPM / mMaxRPM); } + + /// Apply a torque to the engine rotation speed + /// @param inTorque Torque in N m + /// @param inDeltaTime Delta time in seconds + void ApplyTorque(float inTorque, float inDeltaTime); + + /// Update the engine RPM for damping + /// @param inDeltaTime Delta time in seconds + void ApplyDamping(float inDeltaTime); + +#ifdef JPH_DEBUG_RENDERER + // Function that converts RPM to an angle in radians for debugging purposes + float ConvertRPMToAngle(float inRPM) const { return (-0.75f + 1.5f * inRPM / mMaxRPM) * JPH_PI; } + + /// Debug draw a RPM meter + void DrawRPM(DebugRenderer *inRenderer, RVec3Arg inPosition, Vec3Arg inForward, Vec3Arg inUp, float inSize, float inShiftDownRPM, float inShiftUpRPM) const; +#endif // JPH_DEBUG_RENDERER + + /// If the engine is idle we allow the vehicle to sleep + bool AllowSleep() const { return mCurrentRPM <= 1.01f * mMinRPM; } + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + +private: + float mCurrentRPM = mMinRPM; ///< Current rotation speed of engine in rounds per minute +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.cpp new file mode 100644 index 000000000..3bef2e804 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.cpp @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTrackSettings) +{ + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDrivenWheel) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mWheels) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mInertia) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mAngularDamping) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mMaxBrakeTorque) + JPH_ADD_ATTRIBUTE(VehicleTrackSettings, mDifferentialRatio) +} + +void VehicleTrackSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mDrivenWheel); + inStream.Write(mWheels); + inStream.Write(mInertia); + inStream.Write(mAngularDamping); + inStream.Write(mMaxBrakeTorque); + inStream.Write(mDifferentialRatio); +} + +void VehicleTrackSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mDrivenWheel); + inStream.Read(mWheels); + inStream.Read(mInertia); + inStream.Read(mAngularDamping); + inStream.Read(mMaxBrakeTorque); + inStream.Read(mDifferentialRatio); +} + +void VehicleTrack::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mAngularVelocity); +} + +void VehicleTrack::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mAngularVelocity); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.h new file mode 100644 index 000000000..c90acccc6 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleTrack.h @@ -0,0 +1,56 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// On which side of the vehicle the track is located (for steering) +enum class ETrackSide : uint +{ + Left = 0, + Right = 1, + Num = 2 +}; + +/// Generic properties for tank tracks +class JPH_EXPORT VehicleTrackSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTrackSettings) + + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + uint mDrivenWheel; ///< Which wheel on the track is connected to the engine + Array mWheels; ///< Indices of wheels that are inside this track, should include the driven wheel too + float mInertia = 10.0f; ///< Moment of inertia (kg m^2) of the track and its wheels as seen on the driven wheel + float mAngularDamping = 0.5f; ///< Damping factor of track and its wheels: dw/dt = -c * w as seen on the driven wheel + float mMaxBrakeTorque = 15000.0f; ///< How much torque (Nm) the brakes can apply on the driven wheel + float mDifferentialRatio = 6.0f; ///< Ratio between rotation speed of gear box and driven wheel of track +}; + +/// Runtime data for tank tracks +class JPH_EXPORT VehicleTrack : public VehicleTrackSettings +{ +public: + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + + float mAngularVelocity = 0.0f; ///< Angular velocity of the driven wheel, will determine the speed of the entire track +}; + +using VehicleTracks = VehicleTrack[(int)ETrackSide::Num]; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.cpp b/WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.cpp new file mode 100644 index 000000000..43336a537 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.cpp @@ -0,0 +1,159 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(VehicleTransmissionSettings) +{ + JPH_ADD_ENUM_ATTRIBUTE(VehicleTransmissionSettings, mMode) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mGearRatios) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mReverseGearRatios) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchTime) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchReleaseTime) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mSwitchLatency) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftUpRPM) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mShiftDownRPM) + JPH_ADD_ATTRIBUTE(VehicleTransmissionSettings, mClutchStrength) +} + +void VehicleTransmissionSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mMode); + inStream.Write(mGearRatios); + inStream.Write(mReverseGearRatios); + inStream.Write(mSwitchTime); + inStream.Write(mClutchReleaseTime); + inStream.Write(mSwitchLatency); + inStream.Write(mShiftUpRPM); + inStream.Write(mShiftDownRPM); + inStream.Write(mClutchStrength); +} + +void VehicleTransmissionSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mMode); + inStream.Read(mGearRatios); + inStream.Read(mReverseGearRatios); + inStream.Read(mSwitchTime); + inStream.Read(mClutchReleaseTime); + inStream.Read(mSwitchLatency); + inStream.Read(mShiftUpRPM); + inStream.Read(mShiftDownRPM); + inStream.Read(mClutchStrength); +} + +void VehicleTransmission::Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp) +{ + // Update current gear and calculate clutch friction + if (mMode == ETransmissionMode::Auto) + { + // Switch gears based on rpm + int old_gear = mCurrentGear; + if (mCurrentGear == 0 // In neutral + || inForwardInput * float(mCurrentGear) < 0.0f) // Changing between forward / reverse + { + // Switch to first gear or reverse depending on input + mCurrentGear = inForwardInput > 0.0f? 1 : (inForwardInput < 0.0f? -1 : 0); + } + else if (mGearSwitchLatencyTimeLeft == 0.0f) // If not in the timout after switching gears + { + if (inCanShiftUp && inCurrentRPM > mShiftUpRPM) + { + if (mCurrentGear < 0) + { + // Shift up, reverse + if (mCurrentGear > -(int)mReverseGearRatios.size()) + mCurrentGear--; + } + else + { + // Shift up, forward + if (mCurrentGear < (int)mGearRatios.size()) + mCurrentGear++; + } + } + else if (inCurrentRPM < mShiftDownRPM) + { + if (mCurrentGear < 0) + { + // Shift down, reverse + int max_gear = inForwardInput != 0.0f? -1 : 0; + if (mCurrentGear < max_gear) + mCurrentGear++; + } + else + { + // Shift down, forward + int min_gear = inForwardInput != 0.0f? 1 : 0; + if (mCurrentGear > min_gear) + mCurrentGear--; + } + } + } + + if (old_gear != mCurrentGear) + { + // We've shifted gear, start switch countdown + mGearSwitchTimeLeft = old_gear != 0? mSwitchTime : 0.0f; + mClutchReleaseTimeLeft = mClutchReleaseTime; + mGearSwitchLatencyTimeLeft = mSwitchLatency; + mClutchFriction = 0.0f; + } + else if (mGearSwitchTimeLeft > 0.0f) + { + // If still switching gears, count down + mGearSwitchTimeLeft = max(0.0f, mGearSwitchTimeLeft - inDeltaTime); + mClutchFriction = 0.0f; + } + else if (mClutchReleaseTimeLeft > 0.0f) + { + // After switching the gears we slowly release the clutch + mClutchReleaseTimeLeft = max(0.0f, mClutchReleaseTimeLeft - inDeltaTime); + mClutchFriction = 1.0f - mClutchReleaseTimeLeft / mClutchReleaseTime; + } + else + { + // Clutch has full friction + mClutchFriction = 1.0f; + + // Count down switch latency + mGearSwitchLatencyTimeLeft = max(0.0f, mGearSwitchLatencyTimeLeft - inDeltaTime); + } + } +} + +float VehicleTransmission::GetCurrentRatio() const +{ + if (mCurrentGear < 0) + return mReverseGearRatios[-mCurrentGear - 1]; + else if (mCurrentGear == 0) + return 0.0f; + else + return mGearRatios[mCurrentGear - 1]; +} + +void VehicleTransmission::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mCurrentGear); + inStream.Write(mClutchFriction); + inStream.Write(mGearSwitchTimeLeft); + inStream.Write(mClutchReleaseTimeLeft); + inStream.Write(mGearSwitchLatencyTimeLeft); +} + +void VehicleTransmission::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mCurrentGear); + inStream.Read(mClutchFriction); + inStream.Read(mGearSwitchTimeLeft); + inStream.Read(mClutchReleaseTimeLeft); + inStream.Read(mGearSwitchLatencyTimeLeft); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.h b/WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.h new file mode 100644 index 000000000..1d3306a8f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/VehicleTransmission.h @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// How gears are shifted +enum class ETransmissionMode : uint8 +{ + Auto, ///< Automatically shift gear up and down + Manual, ///< Manual gear shift (call SetTransmissionInput) +}; + +/// Configuration for the transmission of a vehicle (gear box) +class JPH_EXPORT VehicleTransmissionSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, VehicleTransmissionSettings) + + /// Saves the contents in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + void RestoreBinaryState(StreamIn &inStream); + + ETransmissionMode mMode = ETransmissionMode::Auto; ///< How to switch gears + Array mGearRatios { 2.66f, 1.78f, 1.3f, 1.0f, 0.74f }; ///< Ratio in rotation rate between engine and gear box, first element is 1st gear, 2nd element 2nd gear etc. + Array mReverseGearRatios { -2.90f }; ///< Ratio in rotation rate between engine and gear box when driving in reverse + float mSwitchTime = 0.5f; ///< How long it takes to switch gears (s), only used in auto mode + float mClutchReleaseTime = 0.3f; ///< How long it takes to release the clutch (go to full friction), only used in auto mode + float mSwitchLatency = 0.5f; ///< How long to wait after releasing the clutch before another switch is attempted (s), only used in auto mode + float mShiftUpRPM = 4000.0f; ///< If RPM of engine is bigger then this we will shift a gear up, only used in auto mode + float mShiftDownRPM = 2000.0f; ///< If RPM of engine is smaller then this we will shift a gear down, only used in auto mode + float mClutchStrength = 10.0f; ///< Strength of the clutch when fully engaged. Total torque a clutch applies is Torque = ClutchStrength * (Velocity Engine - Avg Velocity Wheels At Clutch) (units: k m^2 s^-1) +}; + +/// Runtime data for transmission +class JPH_EXPORT VehicleTransmission : public VehicleTransmissionSettings +{ +public: + /// Set input from driver regarding the transmission (only relevant when transmission is set to manual mode) + /// @param inCurrentGear Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + /// @param inClutchFriction Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + void Set(int inCurrentGear, float inClutchFriction) { mCurrentGear = inCurrentGear; mClutchFriction = inClutchFriction; } + + /// Update the current gear and clutch friction if the transmission is in auto mode + /// @param inDeltaTime Time step delta time in s + /// @param inCurrentRPM Current RPM for engine + /// @param inForwardInput Hint if the user wants to drive forward (> 0) or backwards (< 0) + /// @param inCanShiftUp Indicates if we want to allow the transmission to shift up (e.g. pass false if wheels are slipping) + void Update(float inDeltaTime, float inCurrentRPM, float inForwardInput, bool inCanShiftUp); + + /// Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + int GetCurrentGear() const { return mCurrentGear; } + + /// Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + float GetClutchFriction() const { return mClutchFriction; } + + /// If the auto box is currently switching gears + bool IsSwitchingGear() const { return mGearSwitchTimeLeft > 0.0f; } + + /// Return the transmission ratio based on the current gear (ratio between engine and differential) + float GetCurrentRatio() const; + + /// Only allow sleeping when the transmission is idle + bool AllowSleep() const { return mGearSwitchTimeLeft <= 0.0f && mClutchReleaseTimeLeft <= 0.0f && mGearSwitchLatencyTimeLeft <= 0.0f; } + + /// Saving state for replay + void SaveState(StateRecorder &inStream) const; + void RestoreState(StateRecorder &inStream); + +private: + int mCurrentGear = 0; ///< Current gear, -1 = reverse, 0 = neutral, 1 = 1st gear etc. + float mClutchFriction = 1.0f; ///< Value between 0 and 1 indicating how much friction the clutch gives (0 = no friction, 1 = full friction) + float mGearSwitchTimeLeft = 0.0f; ///< When switching gears this will be > 0 and will cause the engine to not provide any torque to the wheels for a short time (used for automatic gear switching only) + float mClutchReleaseTimeLeft = 0.0f; ///< After switching gears this will be > 0 and will cause the clutch friction to go from 0 to 1 (used for automatic gear switching only) + float mGearSwitchLatencyTimeLeft = 0.0f; ///< After releasing the clutch this will be > 0 and will prevent another gear switch (used for automatic gear switching only) +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/Wheel.cpp b/WickedEngine/Jolt/Physics/Vehicle/Wheel.cpp new file mode 100644 index 000000000..e43ffb8c9 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/Wheel.cpp @@ -0,0 +1,93 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettings) +{ + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionForcePoint) + JPH_ADD_ATTRIBUTE(WheelSettings, mPosition) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionDirection) + JPH_ADD_ATTRIBUTE(WheelSettings, mSteeringAxis) + JPH_ADD_ATTRIBUTE(WheelSettings, mWheelForward) + JPH_ADD_ATTRIBUTE(WheelSettings, mWheelUp) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMinLength) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMaxLength) + JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionPreloadLength) + JPH_ADD_ENUM_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mMode, "mSuspensionSpringMode") + JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mFrequency, "mSuspensionFrequency") // Renaming attributes to stay compatible with old versions of the library + JPH_ADD_ATTRIBUTE_WITH_ALIAS(WheelSettings, mSuspensionSpring.mDamping, "mSuspensionDamping") + JPH_ADD_ATTRIBUTE(WheelSettings, mRadius) + JPH_ADD_ATTRIBUTE(WheelSettings, mWidth) + JPH_ADD_ATTRIBUTE(WheelSettings, mEnableSuspensionForcePoint) +} + +void WheelSettings::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mSuspensionForcePoint); + inStream.Write(mPosition); + inStream.Write(mSuspensionDirection); + inStream.Write(mSteeringAxis); + inStream.Write(mWheelForward); + inStream.Write(mWheelUp); + inStream.Write(mSuspensionMinLength); + inStream.Write(mSuspensionMaxLength); + inStream.Write(mSuspensionPreloadLength); + mSuspensionSpring.SaveBinaryState(inStream); + inStream.Write(mRadius); + inStream.Write(mWidth); + inStream.Write(mEnableSuspensionForcePoint); +} + +void WheelSettings::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mSuspensionForcePoint); + inStream.Read(mPosition); + inStream.Read(mSuspensionDirection); + inStream.Read(mSteeringAxis); + inStream.Read(mWheelForward); + inStream.Read(mWheelUp); + inStream.Read(mSuspensionMinLength); + inStream.Read(mSuspensionMaxLength); + inStream.Read(mSuspensionPreloadLength); + mSuspensionSpring.RestoreBinaryState(inStream); + inStream.Read(mRadius); + inStream.Read(mWidth); + inStream.Read(mEnableSuspensionForcePoint); +} + +Wheel::Wheel(const WheelSettings &inSettings) : + mSettings(&inSettings), + mSuspensionLength(inSettings.mSuspensionMaxLength) +{ + JPH_ASSERT(inSettings.mSuspensionDirection.IsNormalized()); + JPH_ASSERT(inSettings.mSteeringAxis.IsNormalized()); + JPH_ASSERT(inSettings.mWheelForward.IsNormalized()); + JPH_ASSERT(inSettings.mWheelUp.IsNormalized()); + JPH_ASSERT(inSettings.mSuspensionMinLength >= 0.0f); + JPH_ASSERT(inSettings.mSuspensionMaxLength >= inSettings.mSuspensionMinLength); + JPH_ASSERT(inSettings.mSuspensionPreloadLength >= 0.0f); + JPH_ASSERT(inSettings.mSuspensionSpring.mFrequency > 0.0f); + JPH_ASSERT(inSettings.mSuspensionSpring.mDamping >= 0.0f); + JPH_ASSERT(inSettings.mRadius > 0.0f); + JPH_ASSERT(inSettings.mWidth >= 0.0f); +} + +bool Wheel::SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse) +{ + return mLongitudinalPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLongitudinal, inMinImpulse, inMaxImpulse); +} + +bool Wheel::SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse) +{ + return mLateralPart.SolveVelocityConstraint(*inConstraint.GetVehicleBody(), *mContactBody, -mContactLateral, inMinImpulse, inMaxImpulse); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/Wheel.h b/WickedEngine/Jolt/Physics/Vehicle/Wheel.h new file mode 100644 index 000000000..eb5f6874c --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/Wheel.h @@ -0,0 +1,148 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class VehicleConstraint; + +/// Base class for wheel settings, each VehicleController can implement a derived class of this +class JPH_EXPORT WheelSettings : public SerializableObject, public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettings) + + /// Saves the contents in binary form to inStream. + virtual void SaveBinaryState(StreamOut &inStream) const; + + /// Restores the contents in binary form to inStream. + virtual void RestoreBinaryState(StreamIn &inStream); + + Vec3 mPosition { 0, 0, 0 }; ///< Attachment point of wheel suspension in local space of the body + Vec3 mSuspensionForcePoint { 0, 0, 0 }; ///< Where tire forces (suspension and traction) are applied, in local space of the body. A good default is the center of the wheel in its neutral pose. See mEnableSuspensionForcePoint. + Vec3 mSuspensionDirection { 0, -1, 0 }; ///< Direction of the suspension in local space of the body, should point down + Vec3 mSteeringAxis { 0, 1, 0 }; ///< Direction of the steering axis in local space of the body, should point up (e.g. for a bike would be -mSuspensionDirection) + Vec3 mWheelUp { 0, 1, 0 }; ///< Up direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mUp but can be used to give the wheel camber or for a bike would be -mSuspensionDirection) + Vec3 mWheelForward { 0, 0, 1 }; ///< Forward direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mForward but can be used to give the wheel toe, does not need to be perpendicular to mWheelUp) + float mSuspensionMinLength = 0.3f; ///< How long the suspension is in max raised position relative to the attachment point (m) + float mSuspensionMaxLength = 0.5f; ///< How long the suspension is in max droop position relative to the attachment point (m) + float mSuspensionPreloadLength = 0.0f; ///< The natural length (m) of the suspension spring is defined as mSuspensionMaxLength + mSuspensionPreloadLength. Can be used to preload the suspension as the spring is compressed by mSuspensionPreloadLength when the suspension is in max droop position. Note that this means when the vehicle touches the ground there is a discontinuity so it will also make the vehicle more bouncy as we're updating with discrete time steps. + SpringSettings mSuspensionSpring { ESpringMode::FrequencyAndDamping, 1.5f, 0.5f }; ///< Settings for the suspension spring + float mRadius = 0.3f; ///< Radius of the wheel (m) + float mWidth = 0.1f; ///< Width of the wheel (m) + bool mEnableSuspensionForcePoint = false; ///< Enables mSuspensionForcePoint, if disabled, the forces are applied at the collision contact point. This leads to a more accurate simulation when interacting with dynamic objects but makes the vehicle less stable. When setting this to true, all forces will be applied to a fixed point on the vehicle body. +}; + +/// Base class for runtime data for a wheel, each VehicleController can implement a derived class of this +class JPH_EXPORT Wheel : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor / destructor + explicit Wheel(const WheelSettings &inSettings); + virtual ~Wheel() = default; + + /// Get settings for the wheel + const WheelSettings * GetSettings() const { return mSettings; } + + /// Get the angular velocity (rad/s) for this wheel, note that positive means the wheel is rotating such that the car moves forward + float GetAngularVelocity() const { return mAngularVelocity; } + + /// Update the angular velocity (rad/s) + void SetAngularVelocity(float inVel) { mAngularVelocity = inVel; } + + /// Get the current rotation angle of the wheel in radians [0, 2 pi] + float GetRotationAngle() const { return mAngle; } + + /// Set the current rotation angle of the wheel in radians [0, 2 pi] + void SetRotationAngle(float inAngle) { mAngle = inAngle; } + + /// Get the current steer angle of the wheel in radians [-pi, pi], positive is to the left + float GetSteerAngle() const { return mSteerAngle; } + + /// Set the current steer angle of the wheel in radians [-pi, pi] + void SetSteerAngle(float inAngle) { mSteerAngle = inAngle; } + + /// Returns true if the wheel is touching an object + inline bool HasContact() const { return !mContactBodyID.IsInvalid(); } + + /// Returns the body ID of the body that this wheel is touching + BodyID GetContactBodyID() const { return mContactBodyID; } + + /// Returns the sub shape ID where we're contacting the body + SubShapeID GetContactSubShapeID() const { return mContactSubShapeID; } + + /// Returns the current contact position in world space (note by the time you call this the vehicle has moved) + RVec3 GetContactPosition() const { JPH_ASSERT(HasContact()); return mContactPosition; } + + /// Velocity of the contact point (m / s, not relative to the wheel but in world space) + Vec3 GetContactPointVelocity() const { JPH_ASSERT(HasContact()); return mContactPointVelocity; } + + /// Returns the current contact normal in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactNormal() const { JPH_ASSERT(HasContact()); return mContactNormal; } + + /// Returns longitudinal direction (direction along the wheel relative to floor) in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactLongitudinal() const { JPH_ASSERT(HasContact()); return mContactLongitudinal; } + + /// Returns lateral direction (sideways direction) in world space (note by the time you call this the vehicle has moved) + Vec3 GetContactLateral() const { JPH_ASSERT(HasContact()); return mContactLateral; } + + /// Get the length of the suspension for a wheel (m) relative to the suspension attachment point (hard point) + float GetSuspensionLength() const { return mSuspensionLength; } + + /// Check if the suspension hit its upper limit + bool HasHitHardPoint() const { return mSuspensionMaxUpPart.IsActive(); } + + /// Get the total impulse (N s) that was applied by the suspension + float GetSuspensionLambda() const { return mSuspensionPart.GetTotalLambda() + mSuspensionMaxUpPart.GetTotalLambda(); } + + /// Get total impulse (N s) applied along the forward direction of the wheel + float GetLongitudinalLambda() const { return mLongitudinalPart.GetTotalLambda(); } + + /// Get total impulse (N s) applied along the sideways direction of the wheel + float GetLateralLambda() const { return mLateralPart.GetTotalLambda(); } + + /// Internal function that should only be called by the controller. Used to apply impulses in the forward direction of the vehicle. + bool SolveLongitudinalConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse); + + /// Internal function that should only be called by the controller. Used to apply impulses in the sideways direction of the vehicle. + bool SolveLateralConstraintPart(const VehicleConstraint &inConstraint, float inMinImpulse, float inMaxImpulse); + +protected: + friend class VehicleConstraint; + + RefConst mSettings; ///< Configuration settings for this wheel + BodyID mContactBodyID; ///< ID of body for ground + SubShapeID mContactSubShapeID; ///< Sub shape ID for ground + Body * mContactBody = nullptr; ///< Body for ground + float mSuspensionLength; ///< Current length of the suspension + RVec3 mContactPosition; ///< Position of the contact point between wheel and ground + Vec3 mContactPointVelocity; ///< Velocity of the contact point (m / s, not relative to the wheel but in world space) + Vec3 mContactNormal; ///< Normal of the contact point between wheel and ground + Vec3 mContactLongitudinal; ///< Vector perpendicular to normal in the forward direction + Vec3 mContactLateral; ///< Vector perpendicular to normal and longitudinal direction in the right direction + Real mAxlePlaneConstant; ///< Constant for the contact plane of the axle, defined as ContactNormal . (WorldSpaceSuspensionPoint + SuspensionLength * WorldSpaceSuspensionDirection) + float mAntiRollBarImpulse = 0.0f; ///< Amount of impulse applied to the suspension from the anti-rollbars + + float mSteerAngle = 0.0f; ///< Rotation around the suspension direction, positive is to the left + float mAngularVelocity = 0.0f; ///< Rotation speed of wheel, positive when the wheels cause the vehicle to move forwards (rad/s) + float mAngle = 0.0f; ///< Current rotation of the wheel (rad, [0, 2 pi]) + + AxisConstraintPart mSuspensionPart; ///< Controls movement up/down along the contact normal + AxisConstraintPart mSuspensionMaxUpPart; ///< Adds a hard limit when reaching the minimal suspension length + AxisConstraintPart mLongitudinalPart; ///< Controls movement forward/backward + AxisConstraintPart mLateralPart; ///< Controls movement sideways (slip) +}; + +using Wheels = Array; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.cpp b/WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.cpp new file mode 100644 index 000000000..8e4a71a4f --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.cpp @@ -0,0 +1,845 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +//#define JPH_TRACE_VEHICLE_STATS + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheeledVehicleControllerSettings) +{ + JPH_ADD_BASE_CLASS(WheeledVehicleControllerSettings, VehicleControllerSettings) + + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mEngine) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mTransmission) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentials) + JPH_ADD_ATTRIBUTE(WheeledVehicleControllerSettings, mDifferentialLimitedSlipRatio) +} + +JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettingsWV) +{ + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mInertia) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mAngularDamping) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxSteerAngle) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLongitudinalFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mLateralFriction) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxBrakeTorque) + JPH_ADD_ATTRIBUTE(WheelSettingsWV, mMaxHandBrakeTorque) +} + +WheelSettingsWV::WheelSettingsWV() +{ + mLongitudinalFriction.Reserve(3); + mLongitudinalFriction.AddPoint(0.0f, 0.0f); + mLongitudinalFriction.AddPoint(0.06f, 1.2f); + mLongitudinalFriction.AddPoint(0.2f, 1.0f); + + mLateralFriction.Reserve(3); + mLateralFriction.AddPoint(0.0f, 0.0f); + mLateralFriction.AddPoint(3.0f, 1.2f); + mLateralFriction.AddPoint(20.0f, 1.0f); +} + +void WheelSettingsWV::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write(mInertia); + inStream.Write(mAngularDamping); + inStream.Write(mMaxSteerAngle); + mLongitudinalFriction.SaveBinaryState(inStream); + mLateralFriction.SaveBinaryState(inStream); + inStream.Write(mMaxBrakeTorque); + inStream.Write(mMaxHandBrakeTorque); +} + +void WheelSettingsWV::RestoreBinaryState(StreamIn &inStream) +{ + inStream.Read(mInertia); + inStream.Read(mAngularDamping); + inStream.Read(mMaxSteerAngle); + mLongitudinalFriction.RestoreBinaryState(inStream); + mLateralFriction.RestoreBinaryState(inStream); + inStream.Read(mMaxBrakeTorque); + inStream.Read(mMaxHandBrakeTorque); +} + +WheelWV::WheelWV(const WheelSettingsWV &inSettings) : + Wheel(inSettings) +{ + JPH_ASSERT(inSettings.mInertia >= 0.0f); + JPH_ASSERT(inSettings.mAngularDamping >= 0.0f); + JPH_ASSERT(abs(inSettings.mMaxSteerAngle) <= 0.5f * JPH_PI); + JPH_ASSERT(inSettings.mMaxBrakeTorque >= 0.0f); + JPH_ASSERT(inSettings.mMaxHandBrakeTorque >= 0.0f); +} + +void WheelWV::Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint) +{ + const WheelSettingsWV *settings = GetSettings(); + + // Angular damping: dw/dt = -c * w + // Solution: w(t) = w(0) * e^(-c * t) or w2 = w1 * e^(-c * dt) + // Taylor expansion of e^(-c * dt) = 1 - c * dt + ... + // Since dt is usually in the order of 1/60 and c is a low number too this approximation is good enough + mAngularVelocity *= max(0.0f, 1.0f - settings->mAngularDamping * inDeltaTime); + + // Update rotation of wheel + mAngle = fmod(mAngle + mAngularVelocity * inDeltaTime, 2.0f * JPH_PI); + + if (mContactBody != nullptr) + { + const Body *body = inConstraint.GetVehicleBody(); + + // Calculate relative velocity between wheel contact point and floor + Vec3 relative_velocity = body->GetPointVelocity(mContactPosition) - mContactPointVelocity; + + // Cancel relative velocity in the normal plane + relative_velocity -= mContactNormal.Dot(relative_velocity) * mContactNormal; + float relative_longitudinal_velocity = relative_velocity.Dot(mContactLongitudinal); + + // Calculate longitudinal friction based on difference between velocity of rolling wheel and drive surface + float relative_longitudinal_velocity_denom = Sign(relative_longitudinal_velocity) * max(1.0e-3f, abs(relative_longitudinal_velocity)); // Ensure we don't divide by zero + mLongitudinalSlip = abs((mAngularVelocity * settings->mRadius - relative_longitudinal_velocity) / relative_longitudinal_velocity_denom); + float longitudinal_slip_friction = settings->mLongitudinalFriction.GetValue(mLongitudinalSlip); + + // Calculate lateral friction based on slip angle + float relative_velocity_len = relative_velocity.Length(); + mLateralSlip = relative_velocity_len < 1.0e-3f ? 0.0f : ACos(abs(relative_longitudinal_velocity) / relative_velocity_len); + float lateral_slip_angle = RadiansToDegrees(mLateralSlip); + float lateral_slip_friction = settings->mLateralFriction.GetValue(lateral_slip_angle); + + // Tire friction + VehicleConstraint::CombineFunction combine_friction = inConstraint.GetCombineFriction(); + mCombinedLongitudinalFriction = longitudinal_slip_friction; + mCombinedLateralFriction = lateral_slip_friction; + combine_friction(inWheelIndex, mCombinedLongitudinalFriction, mCombinedLateralFriction, *mContactBody, mContactSubShapeID); + } + else + { + // No collision + mLongitudinalSlip = 0.0f; + mLateralSlip = 0.0f; + mCombinedLongitudinalFriction = mCombinedLateralFriction = 0.0f; + } +} + +VehicleController *WheeledVehicleControllerSettings::ConstructController(VehicleConstraint &inConstraint) const +{ + return new WheeledVehicleController(*this, inConstraint); +} + +void WheeledVehicleControllerSettings::SaveBinaryState(StreamOut &inStream) const +{ + mEngine.SaveBinaryState(inStream); + + mTransmission.SaveBinaryState(inStream); + + uint32 num_differentials = (uint32)mDifferentials.size(); + inStream.Write(num_differentials); + for (const VehicleDifferentialSettings &d : mDifferentials) + d.SaveBinaryState(inStream); + + inStream.Write(mDifferentialLimitedSlipRatio); +} + +void WheeledVehicleControllerSettings::RestoreBinaryState(StreamIn &inStream) +{ + mEngine.RestoreBinaryState(inStream); + + mTransmission.RestoreBinaryState(inStream); + + uint32 num_differentials = 0; + inStream.Read(num_differentials); + mDifferentials.resize(num_differentials); + for (VehicleDifferentialSettings &d : mDifferentials) + d.RestoreBinaryState(inStream); + + inStream.Read(mDifferentialLimitedSlipRatio); +} + +WheeledVehicleController::WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint) : + VehicleController(inConstraint) +{ + // Copy engine settings + static_cast(mEngine) = inSettings.mEngine; + JPH_ASSERT(inSettings.mEngine.mMinRPM >= 0.0f); + JPH_ASSERT(inSettings.mEngine.mMinRPM <= inSettings.mEngine.mMaxRPM); + mEngine.SetCurrentRPM(mEngine.mMinRPM); + + // Copy transmission settings + static_cast(mTransmission) = inSettings.mTransmission; +#ifdef JPH_ENABLE_ASSERTS + for (float r : inSettings.mTransmission.mGearRatios) + JPH_ASSERT(r > 0.0f); + for (float r : inSettings.mTransmission.mReverseGearRatios) + JPH_ASSERT(r < 0.0f); +#endif // JPH_ENABLE_ASSERTS + JPH_ASSERT(inSettings.mTransmission.mSwitchTime >= 0.0f); + JPH_ASSERT(inSettings.mTransmission.mShiftDownRPM > 0.0f); + JPH_ASSERT(inSettings.mTransmission.mMode != ETransmissionMode::Auto || inSettings.mTransmission.mShiftUpRPM < inSettings.mEngine.mMaxRPM); + JPH_ASSERT(inSettings.mTransmission.mShiftUpRPM > inSettings.mTransmission.mShiftDownRPM); + JPH_ASSERT(inSettings.mTransmission.mClutchStrength > 0.0f); + + // Copy differential settings + mDifferentials.resize(inSettings.mDifferentials.size()); + for (uint i = 0; i < mDifferentials.size(); ++i) + { + const VehicleDifferentialSettings &d = inSettings.mDifferentials[i]; + mDifferentials[i] = d; + JPH_ASSERT(d.mDifferentialRatio > 0.0f); + JPH_ASSERT(d.mLeftRightSplit >= 0.0f && d.mLeftRightSplit <= 1.0f); + JPH_ASSERT(d.mEngineTorqueRatio >= 0.0f); + JPH_ASSERT(d.mLimitedSlipRatio > 1.0f); + } + + mDifferentialLimitedSlipRatio = inSettings.mDifferentialLimitedSlipRatio; + JPH_ASSERT(mDifferentialLimitedSlipRatio > 1.0f); +} + +float WheeledVehicleController::GetWheelSpeedAtClutch() const +{ + float wheel_speed_at_clutch = 0.0f; + int num_driven_wheels = 0; + for (const VehicleDifferentialSettings &d : mDifferentials) + { + int wheels[] = { d.mLeftWheel, d.mRightWheel }; + for (int w : wheels) + if (w >= 0) + { + wheel_speed_at_clutch += mConstraint.GetWheel(w)->GetAngularVelocity() * d.mDifferentialRatio; + num_driven_wheels++; + } + } + return wheel_speed_at_clutch / float(num_driven_wheels) * VehicleEngine::cAngularVelocityToRPM * mTransmission.GetCurrentRatio(); +} + +bool WheeledVehicleController::AllowSleep() const +{ + return mForwardInput == 0.0f // No user input + && mTransmission.AllowSleep() // Transmission is not shifting + && mEngine.AllowSleep(); // Engine is idling +} + +void WheeledVehicleController::PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + +#ifdef JPH_TRACE_VEHICLE_STATS + static bool sTracedHeader = false; + if (!sTracedHeader) + { + Trace("Time, ForwardInput, Gear, ClutchFriction, EngineRPM, WheelRPM, Velocity (km/h)"); + sTracedHeader = true; + } + static float sTime = 0.0f; + sTime += inDeltaTime; + Trace("%.3f, %.1f, %d, %.1f, %.1f, %.1f, %.1f", sTime, mForwardInput, mTransmission.GetCurrentGear(), mTransmission.GetClutchFriction(), mEngine.GetCurrentRPM(), GetWheelSpeedAtClutch(), mConstraint.GetVehicleBody()->GetLinearVelocity().Length() * 3.6f); +#endif // JPH_TRACE_VEHICLE_STATS + + for (Wheel *w_base : mConstraint.GetWheels()) + { + WheelWV *w = static_cast(w_base); + + // Set steering angle + w->SetSteerAngle(-mRightInput * w->GetSettings()->mMaxSteerAngle); + } +} + +void WheeledVehicleController::PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) +{ + JPH_PROFILE_FUNCTION(); + + // Remember old RPM so we can detect if we're increasing or decreasing + float old_engine_rpm = mEngine.GetCurrentRPM(); + + Wheels &wheels = mConstraint.GetWheels(); + + // Update wheel angle, do this before applying torque to the wheels (as friction will slow them down again) + for (uint wheel_index = 0, num_wheels = (uint)wheels.size(); wheel_index < num_wheels; ++wheel_index) + { + WheelWV *w = static_cast(wheels[wheel_index]); + w->Update(wheel_index, inDeltaTime, mConstraint); + } + + // In auto transmission mode, don't accelerate the engine when switching gears + float forward_input = abs(mForwardInput); + if (mTransmission.mMode == ETransmissionMode::Auto) + forward_input *= mTransmission.GetClutchFriction(); + + // Apply engine damping + mEngine.ApplyDamping(inDeltaTime); + + // Calculate engine torque + float engine_torque = mEngine.GetTorque(forward_input); + + // Define a struct that contains information about driven differentials (i.e. that have wheels connected) + struct DrivenDifferential + { + const VehicleDifferentialSettings * mDifferential; + float mAngularVelocity; + float mClutchToDifferentialTorqueRatio; + float mTempTorqueFactor; + }; + + // Collect driven differentials and their speeds + Array driven_differentials; + driven_differentials.reserve(mDifferentials.size()); + float differential_omega_min = FLT_MAX, differential_omega_max = 0.0f; + for (const VehicleDifferentialSettings &d : mDifferentials) + { + float avg_omega = 0.0f; + int avg_omega_denom = 0; + int indices[] = { d.mLeftWheel, d.mRightWheel }; + for (int idx : indices) + if (idx != -1) + { + avg_omega += wheels[idx]->GetAngularVelocity(); + avg_omega_denom++; + } + + if (avg_omega_denom > 0) + { + avg_omega = abs(avg_omega * d.mDifferentialRatio / float(avg_omega_denom)); // ignoring that the differentials may be rotating in different directions + driven_differentials.push_back({ &d, avg_omega, d.mEngineTorqueRatio, 0 }); + + // Remember min and max velocity + differential_omega_min = min(differential_omega_min, avg_omega); + differential_omega_max = max(differential_omega_max, avg_omega); + } + } + + if (mDifferentialLimitedSlipRatio < FLT_MAX // Limited slip differential needs to be turned on + && differential_omega_max > differential_omega_min) // There needs to be a velocity difference + { + // Calculate factor based on relative speed of a differential + float sum_factor = 0.0f; + for (DrivenDifferential &d : driven_differentials) + { + // Differential with max velocity gets factor 0, differential with min velocity 1 + d.mTempTorqueFactor = (differential_omega_max - d.mAngularVelocity) / (differential_omega_max - differential_omega_min); + sum_factor += d.mTempTorqueFactor; + } + + // Normalize the result + for (DrivenDifferential &d : driven_differentials) + d.mTempTorqueFactor /= sum_factor; + + // Prevent div by zero + differential_omega_min = max(1.0e-3f, differential_omega_min); + differential_omega_max = max(1.0e-3f, differential_omega_max); + + // Map into a value that is 0 when the wheels are turning at an equal rate and 1 when the wheels are turning at mDifferentialLimitedSlipRatio + float alpha = min((differential_omega_max / differential_omega_min - 1.0f) / (mDifferentialLimitedSlipRatio - 1.0f), 1.0f); + JPH_ASSERT(alpha >= 0.0f); + float one_min_alpha = 1.0f - alpha; + + // Update torque ratio for all differentials + for (DrivenDifferential &d : driven_differentials) + d.mClutchToDifferentialTorqueRatio = one_min_alpha * d.mClutchToDifferentialTorqueRatio + alpha * d.mTempTorqueFactor; + } + +#ifdef JPH_ENABLE_ASSERTS + // Assert the values add up to 1 + float sum_torque_factors = 0.0f; + for (DrivenDifferential &d : driven_differentials) + sum_torque_factors += d.mClutchToDifferentialTorqueRatio; + JPH_ASSERT(abs(sum_torque_factors - 1.0f) < 1.0e-6f); +#endif // JPH_ENABLE_ASSERTS + + // Define a struct that collects information about the wheels that connect to the engine + struct DrivenWheel + { + WheelWV * mWheel; + float mClutchToWheelRatio; + float mClutchToWheelTorqueRatio; + float mEstimatedAngularImpulse; + }; + Array driven_wheels; + driven_wheels.reserve(wheels.size()); + + // Collect driven wheels + float transmission_ratio = mTransmission.GetCurrentRatio(); + for (const DrivenDifferential &dd : driven_differentials) + { + VehicleDifferentialSettings d = *dd.mDifferential; + + WheelWV *wl = d.mLeftWheel != -1? static_cast(wheels[d.mLeftWheel]) : nullptr; + WheelWV *wr = d.mRightWheel != -1? static_cast(wheels[d.mRightWheel]) : nullptr; + + float clutch_to_wheel_ratio = transmission_ratio * d.mDifferentialRatio; + + if (wl != nullptr && wr != nullptr) + { + // Calculate torque ratio + float ratio_l, ratio_r; + d.CalculateTorqueRatio(wl->GetAngularVelocity(), wr->GetAngularVelocity(), ratio_l, ratio_r); + + // Add both wheels + driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_l, 0.0f }); + driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio * ratio_r, 0.0f }); + } + else if (wl != nullptr) + { + // Only left wheel, all power to left + driven_wheels.push_back({ wl, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f }); + } + else if (wr != nullptr) + { + // Only right wheel, all power to right + driven_wheels.push_back({ wr, clutch_to_wheel_ratio, dd.mClutchToDifferentialTorqueRatio, 0.0f }); + } + } + + bool solved = false; + if (!driven_wheels.empty()) + { + // Define the torque at the clutch at time t as: + // + // tc(t):=S*(we(t)-sum(R(j)*ww(j,t),j,1,N)/N) + // + // Where: + // S is the total strength of clutch (= friction * strength) + // we(t) is the engine angular velocity at time t + // R(j) is the total gear ratio of clutch to wheel for wheel j + // ww(j,t) is the angular velocity of wheel j at time t + // N is the amount of wheels + // + // The torque that increases the engine angular velocity at time t is: + // + // te(t):=TE-tc(t) + // + // Where: + // TE is the torque delivered by the engine + // + // The torque that increases the wheel angular velocity for wheel i at time t is: + // + // tw(i,t):=TW(i)+R(i)*F(i)*tc(t) + // + // Where: + // TW(i) is the torque applied to the wheel outside of the engine (brake + torque due to friction with the ground) + // F(i) is the fraction of the engine torque applied from engine to wheel i + // + // Because the angular acceleration and torque are connected through: Torque = I * dw/dt + // + // We have the angular acceleration of the engine at time t: + // + // ddt_we(t):=te(t)/Ie + // + // Where: + // Ie is the inertia of the engine + // + // We have the angular acceleration of wheel i at time t: + // + // ddt_ww(i,t):=tw(i,t)/Iw(i) + // + // Where: + // Iw(i) is the inertia of wheel i + // + // We could take a simple Euler step to calculate the resulting accelerations but because the system is very stiff this turns out to be unstable, so we need to use implicit Euler instead: + // + // we(t+dt)=we(t)+dt*ddt_we(t+dt) + // + // and: + // + // ww(i,t+dt)=ww(i,t)+dt*ddt_ww(i,t+dt) + // + // Expanding both equations (the equations above are in wxMaxima format and this can easily be done by expand(%)): + // + // For wheel: + // + // ww(i,t+dt) + (S*dt*F(i)*R(i)*sum(R(j)*ww(j,t+dt),j,1,N))/(N*Iw(i)) - (S*dt*F(i)*R(i)*we(t+dt))/Iw(i) = ww(i,t)+(dt*TW(i))/Iw(i) + // + // For engine: + // + // we(t+dt) + (S*dt*we(t+dt))/Ie - (S*dt*sum(R(j)*ww(j,t+dt),j,1,N))/(Ie*N) = we(t)+(TE*dt)/Ie + // + // Defining a vector w(t) = (ww(1, t), ww(2, t), ..., ww(N, t), we(t)) we can write both equations as a matrix multiplication: + // + // a * w(t + dt) = b + // + // We then invert the matrix to get the new angular velocities. + + // Dimension of matrix is N + 1 + int n = (int)driven_wheels.size() + 1; + + // Last column of w is for the engine angular velocity + int engine = n - 1; + + // Define a and b + DynMatrix a(n, n); + DynMatrix b(n, 1); + + // Get number of driven wheels as a float + float num_driven_wheels_float = float(driven_wheels.size()); + + // Angular velocity of engine + float w_engine = mEngine.GetAngularVelocity(); + + // Calculate the total strength of the clutch + float clutch_strength = transmission_ratio != 0.0f? mTransmission.GetClutchFriction() * mTransmission.mClutchStrength : 0.0f; + + // dt / Ie + float dt_div_ie = inDeltaTime / mEngine.mInertia; + + // Calculate scale factor for impulses based on previous delta time + float impulse_scale = mPreviousDeltaTime > 0.0f? inDeltaTime / mPreviousDeltaTime : 0.0f; + + // Iterate the rows for the wheels + for (int i = 0; i < (int)driven_wheels.size(); ++i) + { + DrivenWheel &w_i = driven_wheels[i]; + const WheelSettingsWV *settings = w_i.mWheel->GetSettings(); + + // Get wheel inertia + float inertia = settings->mInertia; + + // S * R(i) + float s_r = clutch_strength * w_i.mClutchToWheelRatio; + + // dt * S * R(i) * F(i) / Iw + float dt_s_r_f_div_iw = inDeltaTime * s_r * w_i.mClutchToWheelTorqueRatio / inertia; + + // Fill in the columns of a for wheel j + for (int j = 0; j < (int)driven_wheels.size(); ++j) + { + const DrivenWheel &w_j = driven_wheels[j]; + a(i, j) = dt_s_r_f_div_iw * w_j.mClutchToWheelRatio / num_driven_wheels_float; + } + + // Add ww(i, t+dt) + a(i, i) += 1.0f; + + // Add the column for the engine + a(i, engine) = -dt_s_r_f_div_iw; + + // Calculate external angular impulse operating on the wheel: TW(i) * dt + float dt_tw = 0.0f; + + // Combine brake with hand brake torque + float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque; + if (brake_torque > 0.0f) + { + // We're braking + // Calculate brake angular impulse + float sign; + if (w_i.mWheel->GetAngularVelocity() != 0.0f) + sign = Sign(w_i.mWheel->GetAngularVelocity()); + else + sign = Sign(mTransmission.GetCurrentRatio()); // When wheels have locked up use the transmission ratio to determine the sign + dt_tw = sign * inDeltaTime * brake_torque; + } + + if (w_i.mWheel->HasContact()) + { + // We have wheel contact with the floor + // Note that we don't know the torque due to the ground contact yet, so we use the impulse applied from the last frame to estimate it + // Wheel torque TW = force * radius = lambda / dt * radius + dt_tw += impulse_scale * w_i.mWheel->GetLongitudinalLambda() * settings->mRadius; + } + + w_i.mEstimatedAngularImpulse = dt_tw; + + // Fill in the constant b = ww(i,t)+(dt*TW(i))/Iw(i) + b(i, 0) = w_i.mWheel->GetAngularVelocity() - dt_tw / inertia; + + // To avoid looping over the wheels again, we also fill in the wheel columns of the engine row here + a(engine, i) = -dt_div_ie * s_r / num_driven_wheels_float; + } + + // Finalize the engine row + a(engine, engine) = (1.0f + dt_div_ie * clutch_strength); + b(engine, 0) = w_engine + dt_div_ie * engine_torque; + + // Solve the linear equation + if (GaussianElimination(a, b)) + { + // Update the angular velocities for the wheels + for (int i = 0; i < (int)driven_wheels.size(); ++i) + { + DrivenWheel &w_i = driven_wheels[i]; + const WheelSettingsWV *settings = w_i.mWheel->GetSettings(); + + // Get solved wheel angular velocity + float angular_velocity = b(i, 0); + + // We estimated TW and applied it in the equation above, but we haven't actually applied this torque yet so we undo it here. + // It will be applied when we solve the actual braking / the constraints with the floor. + angular_velocity += w_i.mEstimatedAngularImpulse / settings->mInertia; + + // Update angular velocity + w_i.mWheel->SetAngularVelocity(angular_velocity); + } + + // Update the engine RPM + mEngine.SetCurrentRPM(b(engine, 0) * VehicleEngine::cAngularVelocityToRPM); + + // The speeds have been solved + solved = true; + } + else + { + JPH_ASSERT(false, "New engine/wheel speeds could not be calculated!"); + } + } + + if (!solved) + { + // Engine not connected to wheels, apply all torque to engine rotation + mEngine.ApplyTorque(engine_torque, inDeltaTime); + } + + // Calculate if any of the wheels are slipping, this is used to prevent gear switching + bool wheels_slipping = false; + for (const DrivenWheel &w : driven_wheels) + wheels_slipping |= w.mClutchToWheelTorqueRatio > 0.0f && (!w.mWheel->HasContact() || w.mWheel->mLongitudinalSlip > 0.1f); + + // Only allow shifting up when we're not slipping and we're increasing our RPM. + // After a jump, we have a very high engine RPM but once we hit the ground the RPM should be decreasing and we don't want to shift up + // during that time. + bool can_shift_up = !wheels_slipping && mEngine.GetCurrentRPM() >= old_engine_rpm; + + // Update transmission + mTransmission.Update(inDeltaTime, mEngine.GetCurrentRPM(), mForwardInput, can_shift_up); + + // Braking + for (Wheel *w_base : wheels) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *settings = w->GetSettings(); + + // Combine brake with hand brake torque + float brake_torque = mBrakeInput * settings->mMaxBrakeTorque + mHandBrakeInput * settings->mMaxHandBrakeTorque; + if (brake_torque > 0.0f) + { + // Calculate how much torque is needed to stop the wheels from rotating in this time step + float brake_torque_to_lock_wheels = abs(w->GetAngularVelocity()) * settings->mInertia / inDeltaTime; + if (brake_torque > brake_torque_to_lock_wheels) + { + // Wheels are locked + w->SetAngularVelocity(0.0f); + w->mBrakeImpulse = (brake_torque - brake_torque_to_lock_wheels) * inDeltaTime / settings->mRadius; + } + else + { + // Slow down the wheels + w->ApplyTorque(-Sign(w->GetAngularVelocity()) * brake_torque, inDeltaTime); + w->mBrakeImpulse = 0.0f; + } + } + else + { + // Not braking + w->mBrakeImpulse = 0.0f; + } + } + + // Remember previous delta time so we can scale the impulses correctly + mPreviousDeltaTime = inDeltaTime; +} + +bool WheeledVehicleController::SolveLongitudinalAndLateralConstraints(float inDeltaTime) +{ + bool impulse = false; + + float *max_lateral_friction_impulse = (float *)JPH_STACK_ALLOC(mConstraint.GetWheels().size() * sizeof(float)); + + uint wheel_index = 0; + for (Wheel *w_base : mConstraint.GetWheels()) + { + if (w_base->HasContact()) + { + WheelWV *w = static_cast(w_base); + const WheelSettingsWV *settings = w->GetSettings(); + + // Calculate max impulse that we can apply on the ground + float max_longitudinal_friction_impulse; + mTireMaxImpulseCallback(wheel_index, + max_longitudinal_friction_impulse, max_lateral_friction_impulse[wheel_index], w->GetSuspensionLambda(), + w->mCombinedLongitudinalFriction, w->mCombinedLateralFriction, w->mLongitudinalSlip, w->mLateralSlip, inDeltaTime); + + // Calculate relative velocity between wheel contact point and floor in longitudinal direction + Vec3 relative_velocity = mConstraint.GetVehicleBody()->GetPointVelocity(w->GetContactPosition()) - w->GetContactPointVelocity(); + float relative_longitudinal_velocity = relative_velocity.Dot(w->GetContactLongitudinal()); + + // Calculate brake force to apply + float min_longitudinal_impulse, max_longitudinal_impulse; + if (w->mBrakeImpulse != 0.0f) + { + // Limit brake force by max tire friction + float brake_impulse = min(w->mBrakeImpulse, max_longitudinal_friction_impulse); + + // Check which direction the brakes should be applied (we don't want to apply an impulse that would accelerate the vehicle) + if (relative_longitudinal_velocity >= 0.0f) + { + min_longitudinal_impulse = -brake_impulse; + max_longitudinal_impulse = 0.0f; + } + else + { + min_longitudinal_impulse = 0.0f; + max_longitudinal_impulse = brake_impulse; + } + + // Longitudinal impulse, note that we assume that once the wheels are locked that the brakes have more than enough torque to keep the wheels locked so we exclude any rotation deltas + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + } + else + { + // Assume we want to apply an angular impulse that makes the delta velocity between wheel and ground zero in one time step, calculate the amount of linear impulse needed to do that + float desired_angular_velocity = relative_longitudinal_velocity / settings->mRadius; + float linear_impulse = (w->GetAngularVelocity() - desired_angular_velocity) * settings->mInertia / settings->mRadius; + + // Limit the impulse by max tire friction + float prev_lambda = w->GetLongitudinalLambda(); + min_longitudinal_impulse = max_longitudinal_impulse = Clamp(prev_lambda + linear_impulse, -max_longitudinal_friction_impulse, max_longitudinal_friction_impulse); + + // Longitudinal impulse + impulse |= w->SolveLongitudinalConstraintPart(mConstraint, min_longitudinal_impulse, max_longitudinal_impulse); + + // Update the angular velocity of the wheels according to the lambda that was applied + w->SetAngularVelocity(w->GetAngularVelocity() - (w->GetLongitudinalLambda() - prev_lambda) * settings->mRadius / settings->mInertia); + } + } + ++wheel_index; + } + + wheel_index = 0; + for (Wheel *w_base : mConstraint.GetWheels()) + { + if (w_base->HasContact()) + { + WheelWV *w = static_cast(w_base); + + // Lateral friction + float max_lateral_impulse = max_lateral_friction_impulse[wheel_index]; + impulse |= w->SolveLateralConstraintPart(mConstraint, -max_lateral_impulse, max_lateral_impulse); + } + ++wheel_index; + } + + return impulse; +} + +#ifdef JPH_DEBUG_RENDERER + +void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const +{ + float constraint_size = mConstraint.GetDrawConstraintSize(); + + // Draw RPM + Body *body = mConstraint.GetVehicleBody(); + Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp(); + RVec3 rpm_meter_pos = body->GetPosition() + body->GetRotation() * mRPMMeterPosition; + Vec3 rpm_meter_fwd = body->GetRotation() * mConstraint.GetLocalForward(); + mEngine.DrawRPM(inRenderer, rpm_meter_pos, rpm_meter_fwd, rpm_meter_up, mRPMMeterSize, mTransmission.mShiftDownRPM, mTransmission.mShiftUpRPM); + + if (mTransmission.GetCurrentRatio() != 0.0f) + { + // Calculate average wheel speed at clutch + float wheel_speed_at_clutch = GetWheelSpeedAtClutch(); + + // Draw the average wheel speed measured at clutch to compare engine RPM with wheel RPM + inRenderer->DrawLine(rpm_meter_pos, rpm_meter_pos + Quat::sRotation(rpm_meter_fwd, mEngine.ConvertRPMToAngle(wheel_speed_at_clutch)) * (rpm_meter_up * 1.1f * mRPMMeterSize), Color::sYellow); + } + + // Draw current vehicle state + String status = StringFormat("Forward: %.1f, Right: %.1f\nBrake: %.1f, HandBrake: %.1f\n" + "Gear: %d, Clutch: %.1f\nEngineRPM: %.0f, V: %.1f km/h", + (double)mForwardInput, (double)mRightInput, (double)mBrakeInput, (double)mHandBrakeInput, + mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6); + inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size); + + RMat44 body_transform = body->GetWorldTransform(); + + for (const Wheel *w_base : mConstraint.GetWheels()) + { + const WheelWV *w = static_cast(w_base); + const WheelSettings *settings = w->GetSettings(); + + // Calculate where the suspension attaches to the body in world space + RVec3 ws_position = body_transform * settings->mPosition; + Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection); + + // Draw suspension + RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength; + RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength; + inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed); + inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen); + + // Draw current length + RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength(); + inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size); + + // Draw wheel basis + Vec3 wheel_forward, wheel_up, wheel_right; + mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right); + wheel_forward = body_transform.Multiply3x3(wheel_forward); + wheel_up = body_transform.Multiply3x3(wheel_up); + wheel_right = body_transform.Multiply3x3(wheel_right); + Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen); + inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue); + inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow); + + // Draw wheel + RMat44 wheel_transform(Vec4(wheel_up, 0.0f), Vec4(wheel_right, 0.0f), Vec4(wheel_forward, 0.0f), wheel_pos); + wheel_transform.SetRotation(wheel_transform.GetRotation() * Mat44::sRotationY(-w->GetRotationAngle())); + inRenderer->DrawCylinder(wheel_transform, settings->mWidth * 0.5f, settings->mRadius, w->GetSuspensionLength() <= settings->mSuspensionMinLength? Color::sRed : Color::sGreen, DebugRenderer::ECastShadow::Off, DebugRenderer::EDrawMode::Wireframe); + + if (w->HasContact()) + { + // Draw contact + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed); + inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue); + + DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f, S: %.2f\nSlipLateral: %.1f, SlipLong: %.2f\nFrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)RadiansToDegrees(w->mLateralSlip), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, constraint_size); + } + else + { + // Draw 'no hit' + DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f", (double)w->GetAngularVelocity()), Color::sRed, constraint_size); + } + } +} + +#endif // JPH_DEBUG_RENDERER + +void WheeledVehicleController::SaveState(StateRecorder &inStream) const +{ + inStream.Write(mForwardInput); + inStream.Write(mRightInput); + inStream.Write(mBrakeInput); + inStream.Write(mHandBrakeInput); + inStream.Write(mPreviousDeltaTime); + + mEngine.SaveState(inStream); + mTransmission.SaveState(inStream); +} + +void WheeledVehicleController::RestoreState(StateRecorder &inStream) +{ + inStream.Read(mForwardInput); + inStream.Read(mRightInput); + inStream.Read(mBrakeInput); + inStream.Read(mHandBrakeInput); + inStream.Read(mPreviousDeltaTime); + + mEngine.RestoreState(inStream); + mTransmission.RestoreState(inStream); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.h b/WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.h new file mode 100644 index 000000000..16d53f0e4 --- /dev/null +++ b/WickedEngine/Jolt/Physics/Vehicle/WheeledVehicleController.h @@ -0,0 +1,199 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class PhysicsSystem; + +/// WheelSettings object specifically for WheeledVehicleController +class JPH_EXPORT WheelSettingsWV : public WheelSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheelSettingsWV) + + /// Constructor + WheelSettingsWV(); + + // See: WheelSettings + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + float mInertia = 0.9f; ///< Moment of inertia (kg m^2), for a cylinder this would be 0.5 * M * R^2 which is 0.9 for a wheel with a mass of 20 kg and radius 0.3 m + float mAngularDamping = 0.2f; ///< Angular damping factor of the wheel: dw/dt = -c * w + float mMaxSteerAngle = DegreesToRadians(70.0f); ///< How much this wheel can steer (radians) + LinearCurve mLongitudinalFriction; ///< On the Y-axis: friction in the forward direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip ratio (fraction) defined as (omega_wheel * r_wheel - v_longitudinal) / |v_longitudinal|. You can see slip ratio as the amount the wheel is spinning relative to the floor: 0 means the wheel has full traction and is rolling perfectly in sync with the ground, 1 is for example when the wheel is locked and sliding over the ground. + LinearCurve mLateralFriction; ///< On the Y-axis: friction in the sideways direction of the tire. Friction is normally between 0 (no friction) and 1 (full friction) although friction can be a little bit higher than 1 because of the profile of a tire. On the X-axis: the slip angle (degrees) defined as angle between relative contact velocity and tire direction. + float mMaxBrakeTorque = 1500.0f; ///< How much torque (Nm) the brakes can apply to this wheel + float mMaxHandBrakeTorque = 4000.0f; ///< How much torque (Nm) the hand brake can apply to this wheel (usually only applied to the rear wheels) +}; + +/// Wheel object specifically for WheeledVehicleController +class JPH_EXPORT WheelWV : public Wheel +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + explicit WheelWV(const WheelSettingsWV &inWheel); + + /// Override GetSettings and cast to the correct class + const WheelSettingsWV * GetSettings() const { return StaticCast(mSettings); } + + /// Apply a torque (N m) to the wheel for a particular delta time + void ApplyTorque(float inTorque, float inDeltaTime) + { + mAngularVelocity += inTorque * inDeltaTime / GetSettings()->mInertia; + } + + /// Update the wheel rotation based on the current angular velocity + void Update(uint inWheelIndex, float inDeltaTime, const VehicleConstraint &inConstraint); + + float mLongitudinalSlip = 0.0f; ///< Velocity difference between ground and wheel relative to ground velocity + float mLateralSlip = 0.0f; ///< Angular difference (in radians) between ground and wheel relative to ground velocity + float mCombinedLongitudinalFriction = 0.0f; ///< Combined friction coefficient in longitudinal direction (combines terrain and tires) + float mCombinedLateralFriction = 0.0f; ///< Combined friction coefficient in lateral direction (combines terrain and tires) + float mBrakeImpulse = 0.0f; ///< Amount of impulse that the brakes can apply to the floor (excluding friction) +}; + +/// Settings of a vehicle with regular wheels +/// +/// The properties in this controller are largely based on "Car Physics for Games" by Marco Monster. +/// See: https://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html +class JPH_EXPORT WheeledVehicleControllerSettings : public VehicleControllerSettings +{ +public: + JPH_DECLARE_SERIALIZABLE_VIRTUAL(JPH_EXPORT, WheeledVehicleControllerSettings) + + // See: VehicleControllerSettings + virtual VehicleController * ConstructController(VehicleConstraint &inConstraint) const override; + virtual void SaveBinaryState(StreamOut &inStream) const override; + virtual void RestoreBinaryState(StreamIn &inStream) override; + + VehicleEngineSettings mEngine; ///< The properties of the engine + VehicleTransmissionSettings mTransmission; ///< The properties of the transmission (aka gear box) + Array mDifferentials; ///< List of differentials and their properties + float mDifferentialLimitedSlipRatio = 1.4f; ///< Ratio max / min average wheel speed of each differential (measured at the clutch). When the ratio is exceeded all torque gets distributed to the differential with the minimal average velocity. This allows implementing a limited slip differential between differentials. Set to FLT_MAX for an open differential. Value should be > 1. +}; + +/// Runtime controller class +class JPH_EXPORT WheeledVehicleController : public VehicleController +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + WheeledVehicleController(const WheeledVehicleControllerSettings &inSettings, VehicleConstraint &inConstraint); + + /// Typedefs + using Differentials = Array; + + /// Set input from driver + /// @param inForward Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + /// @param inRight Value between -1 and 1 indicating desired steering angle (1 = right) + /// @param inBrake Value between 0 and 1 indicating how strong the brake pedal is pressed + /// @param inHandBrake Value between 0 and 1 indicating how strong the hand brake is pulled + void SetDriverInput(float inForward, float inRight, float inBrake, float inHandBrake) { mForwardInput = inForward; mRightInput = inRight; mBrakeInput = inBrake; mHandBrakeInput = inHandBrake; } + + /// Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + void SetForwardInput(float inForward) { mForwardInput = inForward; } + float GetForwardInput() const { return mForwardInput; } + + /// Value between -1 and 1 indicating desired steering angle (1 = right) + void SetRightInput(float inRight) { mRightInput = inRight; } + float GetRightInput() const { return mRightInput; } + + /// Value between 0 and 1 indicating how strong the brake pedal is pressed + void SetBrakeInput(float inBrake) { mBrakeInput = inBrake; } + float GetBrakeInput() const { return mBrakeInput; } + + /// Value between 0 and 1 indicating how strong the hand brake is pulled + void SetHandBrakeInput(float inHandBrake) { mHandBrakeInput = inHandBrake; } + float GetHandBrakeInput() const { return mHandBrakeInput; } + + /// Get current engine state + const VehicleEngine & GetEngine() const { return mEngine; } + + /// Get current engine state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleEngine & GetEngine() { return mEngine; } + + /// Get current transmission state + const VehicleTransmission & GetTransmission() const { return mTransmission; } + + /// Get current transmission state (writable interface, allows you to make changes to the configuration which will take effect the next time step) + VehicleTransmission & GetTransmission() { return mTransmission; } + + /// Get the differentials this vehicle has + const Differentials & GetDifferentials() const { return mDifferentials; } + + /// Get the differentials this vehicle has (writable interface, allows you to make changes to the configuration which will take effect the next time step) + Differentials & GetDifferentials() { return mDifferentials; } + + /// Ratio max / min average wheel speed of each differential (measured at the clutch). + float GetDifferentialLimitedSlipRatio() const { return mDifferentialLimitedSlipRatio; } + void SetDifferentialLimitedSlipRatio(float inV) { mDifferentialLimitedSlipRatio = inV; } + + /// Get the average wheel speed of all driven wheels (measured at the clutch) + float GetWheelSpeedAtClutch() const; + + /// Calculate max tire impulses by combining friction, slip, and suspension impulse. Note that the actual applied impulse may be lower (e.g. when the vehicle is stationary on a horizontal surface the actual impulse applied will be 0). + using TireMaxImpulseCallback = function; + const TireMaxImpulseCallback&GetTireMaxImpulseCallback() const { return mTireMaxImpulseCallback; } + void SetTireMaxImpulseCallback(const TireMaxImpulseCallback &inTireMaxImpulseCallback) { mTireMaxImpulseCallback = inTireMaxImpulseCallback; } + +#ifdef JPH_DEBUG_RENDERER + /// Debug drawing of RPM meter + void SetRPMMeter(Vec3Arg inPosition, float inSize) { mRPMMeterPosition = inPosition; mRPMMeterSize = inSize; } +#endif // JPH_DEBUG_RENDERER + +protected: + // See: VehicleController + virtual Wheel * ConstructWheel(const WheelSettings &inWheel) const override { JPH_ASSERT(IsKindOf(&inWheel, JPH_RTTI(WheelSettingsWV))); return new WheelWV(static_cast(inWheel)); } + virtual bool AllowSleep() const override; + virtual void PreCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual void PostCollide(float inDeltaTime, PhysicsSystem &inPhysicsSystem) override; + virtual bool SolveLongitudinalAndLateralConstraints(float inDeltaTime) override; + virtual void SaveState(StateRecorder &inStream) const override; + virtual void RestoreState(StateRecorder &inStream) override; +#ifdef JPH_DEBUG_RENDERER + virtual void Draw(DebugRenderer *inRenderer) const override; +#endif // JPH_DEBUG_RENDERER + + // Control information + float mForwardInput = 0.0f; ///< Value between -1 and 1 for auto transmission and value between 0 and 1 indicating desired driving direction and amount the gas pedal is pressed + float mRightInput = 0.0f; ///< Value between -1 and 1 indicating desired steering angle + float mBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the brake pedal is pressed + float mHandBrakeInput = 0.0f; ///< Value between 0 and 1 indicating how strong the hand brake is pulled + + // Simulation information + VehicleEngine mEngine; ///< Engine state of the vehicle + VehicleTransmission mTransmission; ///< Transmission state of the vehicle + Differentials mDifferentials; ///< Differential states of the vehicle + float mDifferentialLimitedSlipRatio; ///< Ratio max / min average wheel speed of each differential (measured at the clutch). + float mPreviousDeltaTime = 0.0f; ///< Delta time of the last step + + // Callback that calculates the max impulse that the tire can apply to the ground + TireMaxImpulseCallback mTireMaxImpulseCallback = + [](uint, float &outLongitudinalImpulse, float &outLateralImpulse, float inSuspensionImpulse, float inLongitudinalFriction, float inLateralFriction, float, float, float) + { + outLongitudinalImpulse = inLongitudinalFriction * inSuspensionImpulse; + outLateralImpulse = inLateralFriction * inSuspensionImpulse; + }; + +#ifdef JPH_DEBUG_RENDERER + // Debug settings + Vec3 mRPMMeterPosition { 0, 1, 0 }; ///< Position (in local space of the body) of the RPM meter when drawing the constraint + float mRPMMeterSize = 0.5f; ///< Size of the RPM meter when drawing the constraint +#endif // JPH_DEBUG_RENDERER +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/RegisterTypes.cpp b/WickedEngine/Jolt/RegisterTypes.cpp new file mode 100644 index 000000000..747be4982 --- /dev/null +++ b/WickedEngine/Jolt/RegisterTypes.cpp @@ -0,0 +1,198 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, Skeleton) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SkeletalAnimation) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, CompoundShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, StaticCompoundShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, MutableCompoundShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, TriangleShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SphereShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, BoxShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, CapsuleShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, TaperedCapsuleShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, CylinderShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, ScaledShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, MeshShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, ConvexHullShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, HeightFieldShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RotatedTranslatedShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, OffsetCenterOfMassShapeSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RagdollSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PointConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SixDOFConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SliderConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SwingTwistConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, DistanceConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, HingeConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, FixedConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, ConeConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPath) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PathConstraintPathHermite) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, VehicleConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, WheeledVehicleControllerSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, RackAndPinionConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GearConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PulleyConstraintSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, MotorSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsScene) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, PhysicsMaterial) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilter) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, GroupFilterTable) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, BodyCreationSettings) +JPH_DECLARE_RTTI_WITH_NAMESPACE_FOR_FACTORY(JPH_EXPORT, JPH, SoftBodyCreationSettings) + +JPH_NAMESPACE_BEGIN + +bool VerifyJoltVersionIDInternal(uint64 inVersionID) +{ + return inVersionID == JPH_VERSION_ID; +} + +void RegisterTypesInternal(uint64 inVersionID) +{ + // Version check + if (!VerifyJoltVersionIDInternal(inVersionID)) + { + Trace("Version mismatch, make sure you compile the client code with the same Jolt version and compiler definitions!"); + uint64 mismatch = JPH_VERSION_ID ^ inVersionID; + auto check_bit = [mismatch](int inBit, const char *inLabel) { if (mismatch & (uint64(1) << (inBit + 23))) Trace("Mismatching define %s.", inLabel); }; + check_bit(1, "JPH_DOUBLE_PRECISION"); + check_bit(2, "JPH_CROSS_PLATFORM_DETERMINISTIC"); + check_bit(3, "JPH_FLOATING_POINT_EXCEPTIONS_ENABLED"); + check_bit(4, "JPH_PROFILE_ENABLED"); + check_bit(5, "JPH_EXTERNAL_PROFILE"); + check_bit(6, "JPH_DEBUG_RENDERER"); + check_bit(7, "JPH_DISABLE_TEMP_ALLOCATOR"); + check_bit(8, "JPH_DISABLE_CUSTOM_ALLOCATOR"); + check_bit(9, "JPH_OBJECT_LAYER_BITS"); + check_bit(10, "JPH_ENABLE_ASSERTS"); + check_bit(11, "JPH_OBJECT_STREAM"); + std::abort(); + } + +#ifndef JPH_DISABLE_CUSTOM_ALLOCATOR + JPH_ASSERT(Allocate != nullptr && Reallocate != nullptr && Free != nullptr && AlignedAllocate != nullptr && AlignedFree != nullptr, "Need to supply an allocator first or call RegisterDefaultAllocator()"); +#endif // !JPH_DISABLE_CUSTOM_ALLOCATOR + + JPH_ASSERT(Factory::sInstance != nullptr, "Need to create a factory first!"); + + // Initialize dispatcher + CollisionDispatch::sInit(); + + // Register base classes first so that we can specialize them later + CompoundShape::sRegister(); + ConvexShape::sRegister(); + + // Register compounds before others so that we can specialize them later (register them in reverse order of collision complexity) + MutableCompoundShape::sRegister(); + StaticCompoundShape::sRegister(); + + // Leaf classes + TriangleShape::sRegister(); + SphereShape::sRegister(); + BoxShape::sRegister(); + CapsuleShape::sRegister(); + TaperedCapsuleShape::sRegister(); + CylinderShape::sRegister(); + MeshShape::sRegister(); + ConvexHullShape::sRegister(); + HeightFieldShape::sRegister(); + SoftBodyShape::sRegister(); + + // Register these last because their collision functions are simple so we want to execute them first (register them in reverse order of collision complexity) + RotatedTranslatedShape::sRegister(); + OffsetCenterOfMassShape::sRegister(); + ScaledShape::sRegister(); + + // Create list of all types + const RTTI *types[] = { + JPH_RTTI(SkeletalAnimation), + JPH_RTTI(Skeleton), + JPH_RTTI(CompoundShapeSettings), + JPH_RTTI(StaticCompoundShapeSettings), + JPH_RTTI(MutableCompoundShapeSettings), + JPH_RTTI(TriangleShapeSettings), + JPH_RTTI(SphereShapeSettings), + JPH_RTTI(BoxShapeSettings), + JPH_RTTI(CapsuleShapeSettings), + JPH_RTTI(TaperedCapsuleShapeSettings), + JPH_RTTI(CylinderShapeSettings), + JPH_RTTI(ScaledShapeSettings), + JPH_RTTI(MeshShapeSettings), + JPH_RTTI(ConvexHullShapeSettings), + JPH_RTTI(HeightFieldShapeSettings), + JPH_RTTI(RotatedTranslatedShapeSettings), + JPH_RTTI(OffsetCenterOfMassShapeSettings), + JPH_RTTI(RagdollSettings), + JPH_RTTI(PointConstraintSettings), + JPH_RTTI(SixDOFConstraintSettings), + JPH_RTTI(SliderConstraintSettings), + JPH_RTTI(SwingTwistConstraintSettings), + JPH_RTTI(DistanceConstraintSettings), + JPH_RTTI(HingeConstraintSettings), + JPH_RTTI(FixedConstraintSettings), + JPH_RTTI(ConeConstraintSettings), + JPH_RTTI(PathConstraintSettings), + JPH_RTTI(VehicleConstraintSettings), + JPH_RTTI(WheeledVehicleControllerSettings), + JPH_RTTI(PathConstraintPath), + JPH_RTTI(PathConstraintPathHermite), + JPH_RTTI(RackAndPinionConstraintSettings), + JPH_RTTI(GearConstraintSettings), + JPH_RTTI(PulleyConstraintSettings), + JPH_RTTI(MotorSettings), + JPH_RTTI(PhysicsScene), + JPH_RTTI(PhysicsMaterial), + JPH_RTTI(PhysicsMaterialSimple), + JPH_RTTI(GroupFilter), + JPH_RTTI(GroupFilterTable), + JPH_RTTI(BodyCreationSettings), + JPH_RTTI(SoftBodyCreationSettings) + }; + + // Register them all + Factory::sInstance->Register(types, (uint)size(types)); + + // Initialize default physics material + if (PhysicsMaterial::sDefault == nullptr) + PhysicsMaterial::sDefault = new PhysicsMaterialSimple("Default", Color::sGrey); +} + +void UnregisterTypes() +{ + // Unregister all types + if (Factory::sInstance != nullptr) + Factory::sInstance->Clear(); + + // Delete default physics material + PhysicsMaterial::sDefault = nullptr; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/RegisterTypes.h b/WickedEngine/Jolt/RegisterTypes.h new file mode 100644 index 000000000..372ef7f4f --- /dev/null +++ b/WickedEngine/Jolt/RegisterTypes.h @@ -0,0 +1,29 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +JPH_NAMESPACE_BEGIN + +/// Internal helper function +JPH_EXPORT extern bool VerifyJoltVersionIDInternal(uint64 inVersionID); + +/// This function can be used to verify the library ABI is compatible with your +/// application. +/// Use it in this way: `assert(VerifyJoltVersionID());`. +/// Returns `false` if the library used is not compatible with your app. +JPH_INLINE bool VerifyJoltVersionID() { return VerifyJoltVersionIDInternal(JPH_VERSION_ID); } + +/// Internal helper function +JPH_EXPORT extern void RegisterTypesInternal(uint64 inVersionID); + +/// Register all physics types with the factory and install their collision handlers with the CollisionDispatch class. +/// If you have your own custom shape types you probably need to register their handlers with the CollisionDispatch before calling this function. +/// If you implement your own default material (PhysicsMaterial::sDefault) make sure to initialize it before this function or else this function will create one for you. +JPH_INLINE void RegisterTypes() { RegisterTypesInternal(JPH_VERSION_ID); } + +/// Unregisters all types with the factory and cleans up the default material +JPH_EXPORT extern void UnregisterTypes(); + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Renderer/DebugRenderer.cpp b/WickedEngine/Jolt/Renderer/DebugRenderer.cpp new file mode 100644 index 000000000..44025aaff --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRenderer.cpp @@ -0,0 +1,1071 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +DebugRenderer *DebugRenderer::sInstance = nullptr; + +// Number of LOD levels to create +static const int sMaxLevel = 4; + +// Distance for each LOD level, these are tweaked for an object of approx. size 1. Use the lod scale to scale these distances. +static const float sLODDistanceForLevel[] = { 5.0f, 10.0f, 40.0f, FLT_MAX }; + +DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor) +{ + // Set position + inV1.StoreFloat3(&mV[0].mPosition); + inV2.StoreFloat3(&mV[1].mPosition); + inV3.StoreFloat3(&mV[2].mPosition); + + // Set color + mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1); + float normal_len = normal.Length(); + if (normal_len > 0.0f) + normal /= normal_len; + Float3 normal3; + normal.StoreFloat3(&normal3); + mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3; + + // Reset UV's + mV[0].mUV = mV[1].mUV = mV[2].mUV = { 0, 0 }; +} + +DebugRenderer::Triangle::Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection) +{ + // Set position + inV1.StoreFloat3(&mV[0].mPosition); + inV2.StoreFloat3(&mV[1].mPosition); + inV3.StoreFloat3(&mV[2].mPosition); + + // Set color + mV[0].mColor = mV[1].mColor = mV[2].mColor = inColor; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized(); + Float3 normal3; + normal.StoreFloat3(&normal3); + mV[0].mNormal = mV[1].mNormal = mV[2].mNormal = normal3; + + // Set UV's + Vec3 uv1 = inV1 - inUVOrigin; + Vec3 uv2 = inV2 - inUVOrigin; + Vec3 uv3 = inV3 - inUVOrigin; + Vec3 axis2 = normal.Cross(inUVDirection); + mV[0].mUV = { inUVDirection.Dot(uv1), axis2.Dot(uv1) }; + mV[1].mUV = { inUVDirection.Dot(uv2), axis2.Dot(uv2) }; + mV[2].mUV = { inUVDirection.Dot(uv3), axis2.Dot(uv3) }; +} + +DebugRenderer::DebugRenderer() +{ + // Store singleton + JPH_ASSERT(sInstance == nullptr); + sInstance = this; +} + +DebugRenderer::~DebugRenderer() +{ + JPH_ASSERT(sInstance == this); + sInstance = nullptr; +} + +void DebugRenderer::DrawWireBox(const AABox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v2(Real(inBox.mMin.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v3(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v4(Real(inBox.mMin.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v5(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v6(Real(inBox.mMax.GetX()), Real(inBox.mMin.GetY()), Real(inBox.mMax.GetZ())); + RVec3 v7(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMin.GetZ())); + RVec3 v8(Real(inBox.mMax.GetX()), Real(inBox.mMax.GetY()), Real(inBox.mMax.GetZ())); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawWireBox(const OrientedBox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v2(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v3(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v4(inBox.mOrientation * Vec3(-inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v5(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v6(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), -inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + RVec3 v7(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), -inBox.mHalfExtents.GetZ())); + RVec3 v8(inBox.mOrientation * Vec3(inBox.mHalfExtents.GetX(), inBox.mHalfExtents.GetY(), inBox.mHalfExtents.GetZ())); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + // 8 vertices + RVec3 v1 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ()); + RVec3 v2 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ()); + RVec3 v3 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ()); + RVec3 v4 = inMatrix * Vec3(inBox.mMin.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ()); + RVec3 v5 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMin.GetZ()); + RVec3 v6 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMin.GetY(), inBox.mMax.GetZ()); + RVec3 v7 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMin.GetZ()); + RVec3 v8 = inMatrix * Vec3(inBox.mMax.GetX(), inBox.mMax.GetY(), inBox.mMax.GetZ()); + + // 12 edges + DrawLine(v1, v2, inColor); + DrawLine(v1, v3, inColor); + DrawLine(v1, v5, inColor); + DrawLine(v2, v4, inColor); + DrawLine(v2, v6, inColor); + DrawLine(v3, v4, inColor); + DrawLine(v3, v7, inColor); + DrawLine(v4, v8, inColor); + DrawLine(v5, v6, inColor); + DrawLine(v5, v7, inColor); + DrawLine(v6, v8, inColor); + DrawLine(v7, v8, inColor); +} + +void DebugRenderer::DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + Vec3 dx(inSize, 0, 0); + Vec3 dy(0, inSize, 0); + Vec3 dz(0, 0, inSize); + DrawLine(inPosition - dy, inPosition + dy, inColor); + DrawLine(inPosition - dx, inPosition + dx, inColor); + DrawLine(inPosition - dz, inPosition + dz, inColor); +} + +void DebugRenderer::DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + // Draw base line + DrawLine(inFrom, inTo, inColor); + + if (inSize > 0.0f) + { + // Draw arrow head + Vec3 dir = Vec3(inTo - inFrom); + float len = dir.Length(); + if (len != 0.0f) + dir = dir * (inSize / len); + else + dir = Vec3(inSize, 0, 0); + Vec3 perp = inSize * dir.GetNormalizedPerpendicular(); + DrawLine(inTo - dir + perp, inTo, inColor); + DrawLine(inTo - dir - perp, inTo, inColor); + } +} + +void DebugRenderer::DrawCoordinateSystem(RMat44Arg inTransform, float inSize) +{ + JPH_PROFILE_FUNCTION(); + + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(inSize, 0, 0), Color::sRed, 0.1f * inSize); + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, inSize, 0), Color::sGreen, 0.1f * inSize); + DrawArrow(inTransform.GetTranslation(), inTransform * Vec3(0, 0, inSize), Color::sBlue, 0.1f * inSize); +} + +void DebugRenderer::DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize) +{ + // Create orthogonal basis + Vec3 perp1 = inNormal.Cross(Vec3::sAxisY()).NormalizedOr(Vec3::sAxisX()); + Vec3 perp2 = perp1.Cross(inNormal).Normalized(); + perp1 = inNormal.Cross(perp2); + + // Calculate corners + RVec3 corner1 = inPoint + inSize * (perp1 + perp2); + RVec3 corner2 = inPoint + inSize * (perp1 - perp2); + RVec3 corner3 = inPoint + inSize * (-perp1 - perp2); + RVec3 corner4 = inPoint + inSize * (-perp1 + perp2); + + // Draw cross + DrawLine(corner1, corner3, inColor); + DrawLine(corner2, corner4, inColor); + + // Draw square + DrawLine(corner1, corner2, inColor); + DrawLine(corner2, corner3, inColor); + DrawLine(corner3, corner4, inColor); + DrawLine(corner4, corner1, inColor); + + // Draw normal + DrawArrow(inPoint, inPoint + inSize * inNormal, inColor, 0.1f * inSize); +} + +void DebugRenderer::DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor) +{ + JPH_PROFILE_FUNCTION(); + + DrawLine(inV1, inV2, inColor); + DrawLine(inV2, inV3, inColor); + DrawLine(inV3, inV1, inColor); +} + +void DebugRenderer::DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel) +{ + RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius); + + DrawWireUnitSphere(matrix, inColor, inLevel); +} + +void DebugRenderer::DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel) +{ + JPH_PROFILE_FUNCTION(); + + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); + DrawWireUnitSphereRecursive(inMatrix, inColor, -Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), inLevel); +} + +void DebugRenderer::DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel) +{ + if (inLevel == 0) + { + RVec3 d1 = inMatrix * inDir1; + RVec3 d2 = inMatrix * inDir2; + RVec3 d3 = inMatrix * inDir3; + + DrawLine(d1, d2, inColor); + DrawLine(d2, d3, inColor); + DrawLine(d3, d1, inColor); + } + else + { + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + DrawWireUnitSphereRecursive(inMatrix, inColor, inDir1, center1, center3, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center1, center2, center3, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center1, inDir2, center2, inLevel - 1); + DrawWireUnitSphereRecursive(inMatrix, inColor, center3, center2, inDir3, inLevel - 1); + } +} + +void DebugRenderer::Create8thSphereRecursive(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel) +{ + if (inLevel == 0) + { + if (ioIdx1 == 0xffffffff) + { + ioIdx1 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir1).StoreFloat3(&position); + inDir1.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + if (ioIdx2 == 0xffffffff) + { + ioIdx2 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir2).StoreFloat3(&position); + inDir2.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + if (ioIdx3 == 0xffffffff) + { + ioIdx3 = (uint32)ioVertices.size(); + Float3 position, normal; + inGetSupport(inDir3).StoreFloat3(&position); + inDir3.StoreFloat3(&normal); + ioVertices.push_back({ position, normal, inUV, Color::sWhite }); + } + + ioIndices.push_back(ioIdx1); + ioIndices.push_back(ioIdx2); + ioIndices.push_back(ioIdx3); + } + else + { + Vec3 center1 = (inDir1 + inDir2).Normalized(); + Vec3 center2 = (inDir2 + inDir3).Normalized(); + Vec3 center3 = (inDir3 + inDir1).Normalized(); + + uint32 idx1 = 0xffffffff; + uint32 idx2 = 0xffffffff; + uint32 idx3 = 0xffffffff; + + Create8thSphereRecursive(ioIndices, ioVertices, inDir1, ioIdx1, center1, idx1, center3, idx3, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1, center2, idx2, center3, idx3, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center1, idx1, inDir2, ioIdx2, center2, idx2, inUV, inGetSupport, inLevel - 1); + Create8thSphereRecursive(ioIndices, ioVertices, center3, idx3, center2, idx2, inDir3, ioIdx3, inUV, inGetSupport, inLevel - 1); + } +} + +void DebugRenderer::Create8thSphere(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel) +{ + uint32 idx1 = 0xffffffff; + uint32 idx2 = 0xffffffff; + uint32 idx3 = 0xffffffff; + + Create8thSphereRecursive(ioIndices, ioVertices, inDir1, idx1, inDir2, idx2, inDir3, idx3, inUV, inGetSupport, inLevel); +} + +void DebugRenderer::CreateQuad(Array &ioIndices, Array &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4) +{ + // Make room + uint32 start_idx = uint32(ioVertices.size()); + ioVertices.resize(start_idx + 4); + Vertex *vertices = &ioVertices[start_idx]; + + // Set position + inV1.StoreFloat3(&vertices[0].mPosition); + inV2.StoreFloat3(&vertices[1].mPosition); + inV3.StoreFloat3(&vertices[2].mPosition); + inV4.StoreFloat3(&vertices[3].mPosition); + + // Set color + vertices[0].mColor = vertices[1].mColor = vertices[2].mColor = vertices[3].mColor = Color::sWhite; + + // Calculate normal + Vec3 normal = (inV2 - inV1).Cross(inV3 - inV1).Normalized(); + Float3 normal3; + normal.StoreFloat3(&normal3); + vertices[0].mNormal = vertices[1].mNormal = vertices[2].mNormal = vertices[3].mNormal = normal3; + + // Set UV's + vertices[0].mUV = { 0, 0 }; + vertices[1].mUV = { 2, 0 }; + vertices[2].mUV = { 2, 2 }; + vertices[3].mUV = { 0, 2 }; + + // Set indices + ioIndices.push_back(start_idx); + ioIndices.push_back(start_idx + 1); + ioIndices.push_back(start_idx + 2); + + ioIndices.push_back(start_idx); + ioIndices.push_back(start_idx + 2); + ioIndices.push_back(start_idx + 3); +} + +void DebugRenderer::Initialize() +{ + // Box + { + Array box_vertices; + Array box_indices; + + // Get corner points + Vec3 v0 = Vec3(-1, 1, -1); + Vec3 v1 = Vec3( 1, 1, -1); + Vec3 v2 = Vec3( 1, 1, 1); + Vec3 v3 = Vec3(-1, 1, 1); + Vec3 v4 = Vec3(-1, -1, -1); + Vec3 v5 = Vec3( 1, -1, -1); + Vec3 v6 = Vec3( 1, -1, 1); + Vec3 v7 = Vec3(-1, -1, 1); + + // Top + CreateQuad(box_indices, box_vertices, v0, v3, v2, v1); + + // Bottom + CreateQuad(box_indices, box_vertices, v4, v5, v6, v7); + + // Left + CreateQuad(box_indices, box_vertices, v0, v4, v7, v3); + + // Right + CreateQuad(box_indices, box_vertices, v2, v6, v5, v1); + + // Front + CreateQuad(box_indices, box_vertices, v3, v7, v6, v2); + + // Back + CreateQuad(box_indices, box_vertices, v0, v1, v5, v4); + + mBox = new Geometry(CreateTriangleBatch(box_vertices, box_indices), AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + } + + // Support function that returns a unit sphere + auto sphere_support = [](Vec3Arg inDirection) { return inDirection; }; + + // Construct geometries + mSphere = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + mCapsuleBottom = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 0, 1))); + mCapsuleTop = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1))); + mCapsuleMid = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + mOpenCone = new Geometry(AABox(Vec3(-1, 0, -1), Vec3(1, 1, 1))); + mCylinder = new Geometry(AABox(Vec3(-1, -1, -1), Vec3(1, 1, 1))); + + // Iterate over levels + for (int level = sMaxLevel; level >= 1; --level) + { + // Determine at which distance this level should be active + float distance = sLODDistanceForLevel[sMaxLevel - level]; + + // Sphere + mSphere->mLODs.push_back({ CreateTriangleBatchForConvex(sphere_support, level), distance }); + + // Capsule bottom half sphere + { + Array capsule_bottom_vertices; + Array capsule_bottom_indices; + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_bottom_indices, capsule_bottom_vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + mCapsuleBottom->mLODs.push_back({ CreateTriangleBatch(capsule_bottom_vertices, capsule_bottom_indices), distance }); + } + + // Capsule top half sphere + { + Array capsule_top_vertices; + Array capsule_top_indices; + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), sphere_support, level); + Create8thSphere(capsule_top_indices, capsule_top_vertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), sphere_support, level); + mCapsuleTop->mLODs.push_back({ CreateTriangleBatch(capsule_top_vertices, capsule_top_indices), distance }); + } + + // Capsule middle part + { + Array capsule_mid_vertices; + Array capsule_mid_indices; + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f); + + uint32 start_idx = (uint32)capsule_mid_vertices.size(); + + int num_parts = 1 << level; + for (int i = 0; i <= num_parts; ++i) + { + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vt(s, 1.0f, c); + Float3 vb(s, -1.0f, c); + Float3 n(s, 0, c); + + capsule_mid_vertices.push_back({ vt, n, uv, Color::sWhite }); + capsule_mid_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = start_idx + 2 * i; + + capsule_mid_indices.push_back(start); + capsule_mid_indices.push_back(start + 1); + capsule_mid_indices.push_back(start + 3); + + capsule_mid_indices.push_back(start); + capsule_mid_indices.push_back(start + 3); + capsule_mid_indices.push_back(start + 2); + } + } + mCapsuleMid->mLODs.push_back({ CreateTriangleBatch(capsule_mid_vertices, capsule_mid_indices), distance }); + } + + // Open cone + { + Array open_cone_vertices; + Array open_cone_indices; + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.25f) : Float2(0.25f, 0.75f); + + uint32 start_idx = (uint32)open_cone_vertices.size(); + + int num_parts = 2 << level; + Float3 vt(0, 0, 0); + for (int i = 0; i <= num_parts; ++i) + { + // Calculate bottom vertex + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vb(s, 1.0f, c); + + // Calculate normal + // perpendicular = Y cross vb (perpendicular to the plane in which 0, y and vb exists) + // normal = perpendicular cross vb (normal to the edge 0 vb) + Vec3 normal = Vec3(s, -Square(s) - Square(c), c).Normalized(); + Float3 n; normal.StoreFloat3(&n); + + open_cone_vertices.push_back({ vt, n, uv, Color::sWhite }); + open_cone_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = start_idx + 2 * i; + + open_cone_indices.push_back(start); + open_cone_indices.push_back(start + 1); + open_cone_indices.push_back(start + 3); + } + } + mOpenCone->mLODs.push_back({ CreateTriangleBatch(open_cone_vertices, open_cone_indices), distance }); + } + + // Cylinder + { + Array cylinder_vertices; + Array cylinder_indices; + for (int q = 0; q < 4; ++q) + { + Float2 uv = (q & 1) == 0? Float2(0.25f, 0.75f) : Float2(0.25f, 0.25f); + + uint32 center_start_idx = (uint32)cylinder_vertices.size(); + + Float3 nt(0.0f, 1.0f, 0.0f); + Float3 nb(0.0f, -1.0f, 0.0f); + cylinder_vertices.push_back({ Float3(0.0f, 1.0f, 0.0f), nt, uv, Color::sWhite }); + cylinder_vertices.push_back({ Float3(0.0f, -1.0f, 0.0f), nb, uv, Color::sWhite }); + + uint32 vtx_start_idx = (uint32)cylinder_vertices.size(); + + int num_parts = 1 << level; + for (int i = 0; i <= num_parts; ++i) + { + float angle = 0.5f * JPH_PI * (float(q) + float(i) / num_parts); + float s = Sin(angle); + float c = Cos(angle); + Float3 vt(s, 1.0f, c); + Float3 vb(s, -1.0f, c); + Float3 n(s, 0, c); + + cylinder_vertices.push_back({ vt, nt, uv, Color::sWhite }); + cylinder_vertices.push_back({ vb, nb, uv, Color::sWhite }); + cylinder_vertices.push_back({ vt, n, uv, Color::sWhite }); + cylinder_vertices.push_back({ vb, n, uv, Color::sWhite }); + } + + for (int i = 0; i < num_parts; ++i) + { + uint32 start = vtx_start_idx + 4 * i; + + // Top + cylinder_indices.push_back(center_start_idx); + cylinder_indices.push_back(start); + cylinder_indices.push_back(start + 4); + + // Bottom + cylinder_indices.push_back(center_start_idx + 1); + cylinder_indices.push_back(start + 5); + cylinder_indices.push_back(start + 1); + + // Side + cylinder_indices.push_back(start + 2); + cylinder_indices.push_back(start + 3); + cylinder_indices.push_back(start + 7); + + cylinder_indices.push_back(start + 2); + cylinder_indices.push_back(start + 7); + cylinder_indices.push_back(start + 6); + } + } + mCylinder->mLODs.push_back({ CreateTriangleBatch(cylinder_vertices, cylinder_indices), distance }); + } + } +} + +AABox DebugRenderer::sCalculateBounds(const Vertex *inVertices, int inVertexCount) +{ + AABox bounds; + for (const Vertex *v = inVertices, *v_end = inVertices + inVertexCount; v < v_end; ++v) + bounds.Encapsulate(Vec3(v->mPosition)); + return bounds; +} + +DebugRenderer::Batch DebugRenderer::CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles) +{ + JPH_PROFILE_FUNCTION(); + + Array vertices; + + // Create render vertices + vertices.resize(inVertices.size()); + for (size_t v = 0; v < inVertices.size(); ++v) + { + vertices[v].mPosition = inVertices[v]; + vertices[v].mNormal = Float3(0, 0, 0); + vertices[v].mUV = Float2(0, 0); + vertices[v].mColor = Color::sWhite; + } + + // Calculate normals + for (size_t i = 0; i < inTriangles.size(); ++i) + { + const IndexedTriangleNoMaterial &tri = inTriangles[i]; + + // Calculate normal of face + Vec3 vtx[3]; + for (int j = 0; j < 3; ++j) + vtx[j] = Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mPosition); + Vec3 normal = ((vtx[1] - vtx[0]).Cross(vtx[2] - vtx[0])).Normalized(); + + // Add normal to all vertices in face + for (int j = 0; j < 3; ++j) + (Vec3::sLoadFloat3Unsafe(vertices[tri.mIdx[j]].mNormal) + normal).StoreFloat3(&vertices[tri.mIdx[j]].mNormal); + } + + // Renormalize vertex normals + for (size_t i = 0; i < vertices.size(); ++i) + Vec3::sLoadFloat3Unsafe(vertices[i].mNormal).Normalized().StoreFloat3(&vertices[i].mNormal); + + return CreateTriangleBatch(&vertices[0], (int)vertices.size(), &inTriangles[0].mIdx[0], (int)(3 * inTriangles.size())); +} + +DebugRenderer::Batch DebugRenderer::CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds) +{ + JPH_PROFILE_FUNCTION(); + + Array vertices; + Array indices; + Create8thSphere(indices, vertices, Vec3::sAxisX(), Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisY(), -Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisY(), Vec3::sAxisX(), Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisX(), -Vec3::sAxisY(), Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisY(), Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisX(), Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, Vec3::sAxisX(), -Vec3::sAxisY(), -Vec3::sAxisZ(), Float2(0.25f, 0.25f), inGetSupport, inLevel); + Create8thSphere(indices, vertices, -Vec3::sAxisY(), -Vec3::sAxisX(), -Vec3::sAxisZ(), Float2(0.25f, 0.75f), inGetSupport, inLevel); + + if (outBounds != nullptr) + *outBounds = sCalculateBounds(&vertices[0], (int)vertices.size()); + + return CreateTriangleBatch(vertices, indices); +} + +DebugRenderer::GeometryRef DebugRenderer::CreateTriangleGeometryForConvex(SupportFunction inGetSupport) +{ + GeometryRef geometry; + + // Iterate over levels + for (int level = sMaxLevel; level >= 1; --level) + { + // Determine at which distance this level should be active + float distance = sLODDistanceForLevel[sMaxLevel - level]; + + // Create triangle batch and only calculate bounds for highest LOD level + AABox bounds; + Batch batch = CreateTriangleBatchForConvex(inGetSupport, level, geometry == nullptr? &bounds : nullptr); + + // Construct geometry in the first iteration + if (geometry == nullptr) + geometry = new Geometry(bounds); + + // Add the LOD + geometry->mLODs.push_back({ batch, distance }); + } + + return geometry; +} + +void DebugRenderer::DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + RMat44 m = RMat44::sScale(inBox.GetExtent()); + m.SetTranslation(RVec3(inBox.GetCenter())); + DrawGeometry(m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 m = Mat44::sScale(inBox.GetExtent()); + m.SetTranslation(inBox.GetCenter()); + DrawGeometry(inMatrix * m, inColor, mBox, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + RMat44 matrix = RMat44::sTranslation(inCenter) * Mat44::sScale(inRadius); + + DrawUnitSphere(matrix, inColor, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + DrawGeometry(inMatrix, inColor, mSphere, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 scale_matrix = Mat44::sScale(inRadius); + + // Calculate world space bounding box + AABox local_bounds(Vec3(-inRadius, -inHalfHeightOfCylinder - inRadius, -inRadius), Vec3(inRadius, inHalfHeightOfCylinder + inRadius, inRadius)); + AABox world_bounds = local_bounds.Transformed(inMatrix); + + float radius_sq = Square(inRadius); + + // Draw bottom half sphere + RMat44 bottom_matrix = inMatrix * Mat44::sTranslation(Vec3(0, -inHalfHeightOfCylinder, 0)) * scale_matrix; + DrawGeometry(bottom_matrix, world_bounds, radius_sq, inColor, mCapsuleBottom, ECullMode::CullBackFace, inCastShadow, inDrawMode); + + // Draw top half sphere + RMat44 top_matrix = inMatrix * Mat44::sTranslation(Vec3(0, inHalfHeightOfCylinder, 0)) * scale_matrix; + DrawGeometry(top_matrix, world_bounds, radius_sq, inColor, mCapsuleTop, ECullMode::CullBackFace, inCastShadow, inDrawMode); + + // Draw middle part + DrawGeometry(inMatrix * Mat44::sScale(Vec3(inRadius, inHalfHeightOfCylinder, inRadius)), world_bounds, radius_sq, inColor, mCapsuleMid, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + Mat44 local_transform(Vec4(inRadius, 0, 0, 0), Vec4(0, inHalfHeight, 0, 0), Vec4(0, 0, inRadius, 0), Vec4(0, 0, 0, 1)); + RMat44 transform = inMatrix * local_transform; + + DrawGeometry(transform, mCylinder->mBounds.Transformed(transform), Square(inRadius), inColor, mCylinder, ECullMode::CullBackFace, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inPerpendicular.IsNormalized(1.0e-4f)); + JPH_ASSERT(abs(inPerpendicular.Dot(inAxis)) < 1.0e-4f); + + Vec3 axis = Sign(inHalfAngle) * inLength * inAxis; + float scale = inLength * Tan(abs(inHalfAngle)); + if (scale != 0.0f) + { + Vec3 perp1 = scale * inPerpendicular; + Vec3 perp2 = scale * inAxis.Cross(inPerpendicular); + RMat44 transform(Vec4(perp1, 0), Vec4(axis, 0), Vec4(perp2, 0), inTop); + DrawGeometry(transform, inColor, mOpenCone, ECullMode::Off, inCastShadow, inDrawMode); + } +} + +DebugRenderer::Geometry *DebugRenderer::CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices) +{ + // Allocate space for vertices + int num_vertices = 2 * inNumSegments; + Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex)); + Vertex *vertices = vertices_start; + + for (int i = 0; i < inNumSegments; ++i) + { + // Get output vertices + Vertex &top = *(vertices++); + Vertex &bottom = *(vertices++); + + // Get local position + const Vec3 &pos = inVertices[i]; + + // Get local normal + const Vec3 &prev_pos = inVertices[(i + inNumSegments - 1) % inNumSegments]; + const Vec3 &next_pos = inVertices[(i + 1) % inNumSegments]; + Vec3 normal = 0.5f * (next_pos.Cross(pos).NormalizedOr(Vec3::sZero()) + pos.Cross(prev_pos).NormalizedOr(Vec3::sZero())); + + // Store top vertex + top.mPosition = { 0, 0, 0 }; + normal.StoreFloat3(&top.mNormal); + top.mColor = Color::sWhite; + top.mUV = { 0, 0 }; + + // Store bottom vertex + pos.StoreFloat3(&bottom.mPosition); + normal.StoreFloat3(&bottom.mNormal); + bottom.mColor = Color::sWhite; + bottom.mUV = { 0, 0 }; + } + + // Allocate space for indices + int num_indices = 3 * inNumSegments; + uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32)); + uint32 *indices = indices_start; + + // Calculate indices + for (int i = 0; i < inNumSegments; ++i) + { + int first = 2 * i; + int second = (first + 3) % num_vertices; + int third = first + 1; + + // Triangle + *indices++ = first; + *indices++ = second; + *indices++ = third; + } + + // Convert to triangle batch + return new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices)); +} + +void DebugRenderer::DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + // Assert sane input + JPH_ASSERT(inSwingYHalfAngle >= 0.0f && inSwingYHalfAngle <= JPH_PI); + JPH_ASSERT(inSwingZHalfAngle >= 0.0f && inSwingZHalfAngle <= JPH_PI); + JPH_ASSERT(inEdgeLength > 0.0f); + + // Check cache + SwingConeLimits limits { inSwingYHalfAngle, inSwingZHalfAngle }; + GeometryRef &geometry = mSwingConeLimits[limits]; + if (geometry == nullptr) + { + SwingConeBatches::iterator it = mPrevSwingConeLimits.find(limits); + if (it != mPrevSwingConeLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + // Number of segments to draw the cone with + const int num_segments = 64; + int half_num_segments = num_segments / 2; + + // The y and z values of the quaternion are limited to an ellipse, e1 and e2 are the radii of this ellipse + float e1 = Sin(0.5f * inSwingZHalfAngle); + float e2 = Sin(0.5f * inSwingYHalfAngle); + + // Check if the limits will draw something + if ((e1 <= 0.0f && e2 <= 0.0f) || (e2 >= 1.0f && e1 >= 1.0f)) + return; + + // Calculate squared values + float e1_sq = Square(e1); + float e2_sq = Square(e2); + + // Calculate local space vertices for shape + Vec3 ls_vertices[num_segments]; + int tgt_vertex = 0; + for (int side_iter = 0; side_iter < 2; ++side_iter) + for (int segment_iter = 0; segment_iter < half_num_segments; ++segment_iter) + { + float y, z; + if (e2_sq > e1_sq) + { + // Trace the y value of the quaternion + y = e2 - 2.0f * segment_iter * e2 / half_num_segments; + + // Calculate the corresponding z value of the quaternion + float z_sq = e1_sq - e1_sq / e2_sq * Square(y); + z = z_sq <= 0.0f? 0.0f : sqrt(z_sq); + } + else + { + // Trace the z value of the quaternion + z = -e1 + 2.0f * segment_iter * e1 / half_num_segments; + + // Calculate the corresponding y value of the quaternion + float y_sq = e2_sq - e2_sq / e1_sq * Square(z); + y = y_sq <= 0.0f? 0.0f : sqrt(y_sq); + } + + // If we're tracing the opposite side, flip the values + if (side_iter == 1) + { + z = -z; + y = -y; + } + + // Create quaternion + Vec3 q_xyz(0, y, z); + float w = sqrt(1.0f - q_xyz.LengthSq()); + Quat q(Vec4(q_xyz, w)); + + // Store vertex + ls_vertices[tgt_vertex++] = q.RotateAxisX(); + } + + geometry = CreateSwingLimitGeometry(num_segments, ls_vertices); + } + + DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + JPH_PROFILE_FUNCTION(); + + // Assert sane input + JPH_ASSERT(inMinSwingYAngle <= inMaxSwingYAngle && inMinSwingZAngle <= inMaxSwingZAngle); + JPH_ASSERT(inEdgeLength > 0.0f); + + // Check cache + SwingPyramidLimits limits { inMinSwingYAngle, inMaxSwingYAngle, inMinSwingZAngle, inMaxSwingZAngle }; + GeometryRef &geometry = mSwingPyramidLimits[limits]; + if (geometry == nullptr) + { + SwingPyramidBatches::iterator it = mPrevSwingPyramidLimits.find(limits); + if (it != mPrevSwingPyramidLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + // Number of segments to draw the cone with + const int num_segments = 64; + int quarter_num_segments = num_segments / 4; + + // Note that this is q = Quat::sRotation(Vec3::sAxisZ(), z) * Quat::sRotation(Vec3::sAxisY(), y) with q.x set to zero so we don't introduce twist + // This matches the calculation in SwingTwistConstraintPart::ClampSwingTwist + auto get_axis = [](float inY, float inZ) { + float hy = 0.5f * inY; + float hz = 0.5f * inZ; + float cos_hy = Cos(hy); + float cos_hz = Cos(hz); + return Quat(0, Sin(hy) * cos_hz, cos_hy * Sin(hz), cos_hy * cos_hz).Normalized().RotateAxisX(); + }; + + // Calculate local space vertices for shape + Vec3 ls_vertices[num_segments]; + int tgt_vertex = 0; + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle, inMaxSwingZAngle - (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMinSwingYAngle + (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMinSwingZAngle); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle, inMinSwingZAngle + (inMaxSwingZAngle - inMinSwingZAngle) * segment_iter / quarter_num_segments); + for (int segment_iter = 0; segment_iter < quarter_num_segments; ++segment_iter) + ls_vertices[tgt_vertex++] = get_axis(inMaxSwingYAngle - (inMaxSwingYAngle - inMinSwingYAngle) * segment_iter / quarter_num_segments, inMaxSwingZAngle); + + geometry = CreateSwingLimitGeometry(num_segments, ls_vertices); + } + + DrawGeometry(inMatrix * Mat44::sScale(inEdgeLength), inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + if (inMinAngle >= inMaxAngle) + return; + + JPH_PROFILE_FUNCTION(); + + JPH_ASSERT(inAxis.IsNormalized(1.0e-4f)); + JPH_ASSERT(inNormal.IsNormalized(1.0e-4f)); + JPH_ASSERT(abs(inNormal.Dot(inAxis)) < 1.0e-4f); + + // Pies have a unique batch based on the difference between min and max angle + float delta_angle = inMaxAngle - inMinAngle; + GeometryRef &geometry = mPieLimits[delta_angle]; + if (geometry == nullptr) + { + PieBatces::iterator it = mPrevPieLimits.find(delta_angle); + if (it != mPrevPieLimits.end()) + geometry = it->second; + } + if (geometry == nullptr) + { + int num_parts = (int)ceil(64.0f * delta_angle / (2.0f * JPH_PI)); + + Float3 normal = { 0, 1, 0 }; + Float3 center = { 0, 0, 0 }; + + // Allocate space for vertices + int num_vertices = num_parts + 2; + Vertex *vertices_start = (Vertex *)JPH_STACK_ALLOC(num_vertices * sizeof(Vertex)); + Vertex *vertices = vertices_start; + + // Center of circle + *vertices++ = { center, normal, { 0, 0 }, Color::sWhite }; + + // Outer edge of pie + for (int i = 0; i <= num_parts; ++i) + { + float angle = float(i) / float(num_parts) * delta_angle; + + Float3 pos = { Cos(angle), 0, Sin(angle) }; + *vertices++ = { pos, normal, { 0, 0 }, Color::sWhite }; + } + + // Allocate space for indices + int num_indices = num_parts * 3; + uint32 *indices_start = (uint32 *)JPH_STACK_ALLOC(num_indices * sizeof(uint32)); + uint32 *indices = indices_start; + + for (int i = 0; i < num_parts; ++i) + { + *indices++ = 0; + *indices++ = i + 1; + *indices++ = i + 2; + } + + // Convert to triangle batch + geometry = new Geometry(CreateTriangleBatch(vertices_start, num_vertices, indices_start, num_indices), sCalculateBounds(vertices_start, num_vertices)); + } + + // Construct matrix that transforms pie into world space + RMat44 matrix = RMat44(Vec4(inRadius * inAxis, 0), Vec4(inRadius * inNormal, 0), Vec4(inRadius * inNormal.Cross(inAxis), 0), inCenter) * Mat44::sRotationY(-inMinAngle); + + DrawGeometry(matrix, inColor, geometry, ECullMode::Off, inCastShadow, inDrawMode); +} + +void DebugRenderer::NextFrame() +{ + mPrevSwingConeLimits.clear(); + std::swap(mSwingConeLimits, mPrevSwingConeLimits); + + mPrevSwingPyramidLimits.clear(); + std::swap(mSwingPyramidLimits, mPrevSwingPyramidLimits); + + mPrevPieLimits.clear(); + std::swap(mPieLimits, mPrevPieLimits); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/WickedEngine/Jolt/Renderer/DebugRenderer.h b/WickedEngine/Jolt/Renderer/DebugRenderer.h new file mode 100644 index 000000000..dd3e7bc82 --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRenderer.h @@ -0,0 +1,347 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#ifndef JPH_DEBUG_RENDERER_EXPORT + // By default export the debug renderer + #define JPH_DEBUG_RENDERER_EXPORT JPH_EXPORT +#endif // !JPH_DEBUG_RENDERER_EXPORT + +#include +#include +#include +#include +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class OrientedBox; + +/// Simple triangle renderer for debugging purposes. +/// +/// Inherit from this class to provide your own implementation. +/// +/// Implement the following virtual functions: +/// - DrawLine +/// - DrawTriangle +/// - DrawText3D +/// - CreateTriangleBatch +/// - DrawGeometry +/// +/// Make sure you call Initialize() from the constructor of your implementation. +/// +/// The CreateTriangleBatch is used to prepare a batch of triangles to be drawn by a single DrawGeometry call, +/// which means that Jolt can render a complex scene much more efficiently than when each triangle in that scene would have been drawn through DrawTriangle. +/// +/// Note that an implementation that implements CreateTriangleBatch and DrawGeometry is provided by DebugRendererSimple which can be used to start quickly. +class JPH_DEBUG_RENDERER_EXPORT DebugRenderer : public NonCopyable +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRenderer(); + virtual ~DebugRenderer(); + + /// Call once after frame is complete. Releases unused dynamically generated geometry assets. + void NextFrame(); + + /// Draw line + virtual void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) = 0; + + /// Draw wireframe box + void DrawWireBox(const AABox &inBox, ColorArg inColor); + void DrawWireBox(const OrientedBox &inBox, ColorArg inColor); + void DrawWireBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor); + + /// Draw a marker on a position + void DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize); + + /// Draw an arrow + void DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize); + + /// Draw coordinate system (3 arrows, x = red, y = green, z = blue) + void DrawCoordinateSystem(RMat44Arg inTransform, float inSize = 1.0f); + + /// Draw a plane through inPoint with normal inNormal + void DrawPlane(RVec3Arg inPoint, Vec3Arg inNormal, ColorArg inColor, float inSize); + + /// Draw wireframe triangle + void DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor); + + /// Draw a wireframe polygon + template + void DrawWirePolygon(RMat44Arg inTransform, const VERTEX_ARRAY &inVertices, ColorArg inColor, float inArrowSize = 0.0f) { for (typename VERTEX_ARRAY::size_type i = 0; i < inVertices.size(); ++i) DrawArrow(inTransform * inVertices[i], inTransform * inVertices[(i + 1) % inVertices.size()], inColor, inArrowSize); } + + /// Draw wireframe sphere + void DrawWireSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, int inLevel = 3); + void DrawWireUnitSphere(RMat44Arg inMatrix, ColorArg inColor, int inLevel = 3); + + /// Enum that determines if a shadow should be cast or not + enum class ECastShadow + { + On, // This shape should cast a shadow + Off // This shape should not cast a shadow + }; + + /// Determines how triangles are drawn + enum class EDrawMode + { + Solid, ///< Draw as a solid shape + Wireframe, ///< Draw as wireframe + }; + + /// Draw a single back face culled triangle + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::Off) = 0; + + /// Draw a box + void DrawBox(const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + void DrawBox(RMat44Arg inMatrix, const AABox &inBox, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a sphere + void DrawSphere(RVec3Arg inCenter, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + void DrawUnitSphere(RMat44Arg inMatrix, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a capsule with one half sphere at (0, -inHalfHeightOfCylinder, 0) and the other half sphere at (0, inHalfHeightOfCylinder, 0) and radius inRadius. + /// The capsule will be transformed by inMatrix. + void DrawCapsule(RMat44Arg inMatrix, float inHalfHeightOfCylinder, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a cylinder with top (0, inHalfHeight, 0) and bottom (0, -inHalfHeight, 0) and radius inRadius. + /// The cylinder will be transformed by inMatrix + void DrawCylinder(RMat44Arg inMatrix, float inHalfHeight, float inRadius, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a bottomless cone. + /// @param inTop Top of cone, center of base is at inTop + inAxis. + /// @param inAxis Height and direction of cone + /// @param inPerpendicular Perpendicular vector to inAxis. + /// @param inHalfAngle Specifies the cone angle in radians (angle measured between inAxis and cone surface). + /// @param inLength The length of the cone. + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawOpenCone(RVec3Arg inTop, Vec3Arg inAxis, Vec3Arg inPerpendicular, float inHalfAngle, float inLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draws cone rotation limits as used by the SwingTwistConstraintPart. + /// @param inMatrix Matrix that transforms from constraint space to world space + /// @param inSwingYHalfAngle See SwingTwistConstraintPart + /// @param inSwingZHalfAngle See SwingTwistConstraintPart + /// @param inEdgeLength Size of the edge of the cone shape + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawSwingConeLimits(RMat44Arg inMatrix, float inSwingYHalfAngle, float inSwingZHalfAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draws rotation limits as used by the SwingTwistConstraintPart. + /// @param inMatrix Matrix that transforms from constraint space to world space + /// @param inMinSwingYAngle See SwingTwistConstraintPart + /// @param inMaxSwingYAngle See SwingTwistConstraintPart + /// @param inMinSwingZAngle See SwingTwistConstraintPart + /// @param inMaxSwingZAngle See SwingTwistConstraintPart + /// @param inEdgeLength Size of the edge of the cone shape + /// @param inColor Color to use for drawing the cone. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawSwingPyramidLimits(RMat44Arg inMatrix, float inMinSwingYAngle, float inMaxSwingYAngle, float inMinSwingZAngle, float inMaxSwingZAngle, float inEdgeLength, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Draw a pie (part of a circle). + /// @param inCenter The center of the circle. + /// @param inRadius Radius of the circle. + /// @param inNormal The plane normal in which the pie resides. + /// @param inAxis The axis that defines an angle of 0 radians. + /// @param inMinAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians). + /// @param inMaxAngle The pie will be drawn between [inMinAngle, inMaxAngle] (in radians). + /// @param inColor Color to use for drawing the pie. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + void DrawPie(RVec3Arg inCenter, float inRadius, Vec3Arg inNormal, Vec3Arg inAxis, float inMinAngle, float inMaxAngle, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid); + + /// Singleton instance + static DebugRenderer * sInstance; + + /// Vertex format used by the triangle renderer + class Vertex + { + public: + Float3 mPosition; + Float3 mNormal; + Float2 mUV; + Color mColor; + }; + + /// A single triangle + class JPH_DEBUG_RENDERER_EXPORT Triangle + { + public: + Triangle() = default; + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor); + Triangle(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, ColorArg inColor, Vec3Arg inUVOrigin, Vec3Arg inUVDirection); + + Vertex mV[3]; + }; + + /// Handle for a batch of triangles + using Batch = Ref; + + /// A single level of detail + class LOD + { + public: + Batch mTriangleBatch; + float mDistance; + }; + + /// A geometry primitive containing triangle batches for various lods + class Geometry : public RefTarget + { + public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + Geometry(const AABox &inBounds) : mBounds(inBounds) { } + Geometry(const Batch &inBatch, const AABox &inBounds) : mBounds(inBounds) { mLODs.push_back({ inBatch, FLT_MAX }); } + + /// Determine which LOD to render + /// @param inCameraPosition Current position of the camera + /// @param inWorldSpaceBounds World space bounds for this geometry (transform mBounds by model space matrix) + /// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD). + /// @return The selected LOD. + const LOD & GetLOD(Vec3Arg inCameraPosition, const AABox &inWorldSpaceBounds, float inLODScaleSq) const + { + float dist_sq = inWorldSpaceBounds.GetSqDistanceTo(inCameraPosition); + for (const LOD &lod : mLODs) + if (dist_sq <= inLODScaleSq * Square(lod.mDistance)) + return lod; + + return mLODs.back(); + } + + /// All level of details for this mesh + Array mLODs; + + /// Bounding box that encapsulates all LODs + AABox mBounds; + }; + + /// Handle for a lodded triangle batch + using GeometryRef = Ref; + + /// Calculate bounding box for a batch of triangles + static AABox sCalculateBounds(const Vertex *inVertices, int inVertexCount); + + /// Create a batch of triangles that can be drawn efficiently + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) = 0; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) = 0; + Batch CreateTriangleBatch(const Array &inTriangles) { return CreateTriangleBatch(inTriangles.empty()? nullptr : &inTriangles[0], int(inTriangles.size())); } + Batch CreateTriangleBatch(const Array &inVertices, const Array &inIndices) { return CreateTriangleBatch(inVertices.empty()? nullptr : &inVertices[0], int(inVertices.size()), inIndices.empty()? nullptr : &inIndices[0], int(inIndices.size())); } + Batch CreateTriangleBatch(const VertexList &inVertices, const IndexedTriangleNoMaterialList &inTriangles); + + /// Create a primitive for a convex shape using its support function + using SupportFunction = function; + Batch CreateTriangleBatchForConvex(SupportFunction inGetSupport, int inLevel, AABox *outBounds = nullptr); + GeometryRef CreateTriangleGeometryForConvex(SupportFunction inGetSupport); + + /// Determines which polygons are culled + enum class ECullMode + { + CullBackFace, ///< Don't draw backfacing polygons + CullFrontFace, ///< Don't draw front facing polygons + Off ///< Don't do culling and draw both sides + }; + + /// Draw some geometry + /// @param inModelMatrix is the matrix that transforms the geometry to world space. + /// @param inWorldSpaceBounds is the bounding box of the geometry after transforming it into world space. + /// @param inLODScaleSq is the squared scale of the model matrix, it is multiplied with the LOD distances in inGeometry to calculate the real LOD distance (so a number > 1 will force a higher LOD). + /// @param inModelColor is the color with which to multiply the vertex colors in inGeometry. + /// @param inGeometry The geometry to draw. + /// @param inCullMode determines which polygons are culled. + /// @param inCastShadow determines if this geometry should cast a shadow or not. + /// @param inDrawMode determines if we draw the geometry solid or in wireframe. + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) = 0; + void DrawGeometry(RMat44Arg inModelMatrix, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode = ECullMode::CullBackFace, ECastShadow inCastShadow = ECastShadow::On, EDrawMode inDrawMode = EDrawMode::Solid) { DrawGeometry(inModelMatrix, inGeometry->mBounds.Transformed(inModelMatrix), max(max(inModelMatrix.GetAxisX().LengthSq(), inModelMatrix.GetAxisY().LengthSq()), inModelMatrix.GetAxisZ().LengthSq()), inModelColor, inGeometry, inCullMode, inCastShadow, inDrawMode); } + + /// Draw text + virtual void DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor = Color::sWhite, float inHeight = 0.5f) = 0; + +protected: + /// Initialize the system, must be called from the constructor of the DebugRenderer implementation + void Initialize(); + +private: + /// Recursive helper function for DrawWireUnitSphere + void DrawWireUnitSphereRecursive(RMat44Arg inMatrix, ColorArg inColor, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, int inLevel); + + /// Helper functions to create a box + void CreateQuad(Array &ioIndices, Array &ioVertices, Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4); + + /// Helper functions to create a vertex and index buffer for a sphere + void Create8thSphereRecursive(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, uint32 &ioIdx1, Vec3Arg inDir2, uint32 &ioIdx2, Vec3Arg inDir3, uint32 &ioIdx3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel); + void Create8thSphere(Array &ioIndices, Array &ioVertices, Vec3Arg inDir1, Vec3Arg inDir2, Vec3Arg inDir3, const Float2 &inUV, SupportFunction inGetSupport, int inLevel); + + /// Helper function for DrawSwingConeLimits and DrawSwingPyramidLimits + Geometry * CreateSwingLimitGeometry(int inNumSegments, const Vec3 *inVertices); + + // Predefined shapes + GeometryRef mBox; + GeometryRef mSphere; + GeometryRef mCapsuleTop; + GeometryRef mCapsuleMid; + GeometryRef mCapsuleBottom; + GeometryRef mOpenCone; + GeometryRef mCylinder; + + struct SwingConeLimits + { + bool operator == (const SwingConeLimits &inRHS) const + { + return mSwingYHalfAngle == inRHS.mSwingYHalfAngle + && mSwingZHalfAngle == inRHS.mSwingZHalfAngle; + } + + float mSwingYHalfAngle; + float mSwingZHalfAngle; + }; + + JPH_MAKE_HASH_STRUCT(SwingConeLimits, SwingConeLimitsHasher, t.mSwingYHalfAngle, t.mSwingZHalfAngle) + + using SwingConeBatches = UnorderedMap; + SwingConeBatches mSwingConeLimits; + SwingConeBatches mPrevSwingConeLimits; + + struct SwingPyramidLimits + { + bool operator == (const SwingPyramidLimits &inRHS) const + { + return mMinSwingYAngle == inRHS.mMinSwingYAngle + && mMaxSwingYAngle == inRHS.mMaxSwingYAngle + && mMinSwingZAngle == inRHS.mMinSwingZAngle + && mMaxSwingZAngle == inRHS.mMaxSwingZAngle; + } + + float mMinSwingYAngle; + float mMaxSwingYAngle; + float mMinSwingZAngle; + float mMaxSwingZAngle; + }; + + JPH_MAKE_HASH_STRUCT(SwingPyramidLimits, SwingPyramidLimitsHasher, t.mMinSwingYAngle, t.mMaxSwingYAngle, t.mMinSwingZAngle, t.mMaxSwingZAngle) + + using SwingPyramidBatches = UnorderedMap; + SwingPyramidBatches mSwingPyramidLimits; + SwingPyramidBatches mPrevSwingPyramidLimits; + + using PieBatces = UnorderedMap; + PieBatces mPieLimits; + PieBatces mPrevPieLimits; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Renderer/DebugRendererPlayback.cpp b/WickedEngine/Jolt/Renderer/DebugRendererPlayback.cpp new file mode 100644 index 000000000..bea7e4e08 --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRendererPlayback.cpp @@ -0,0 +1,168 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +void DebugRendererPlayback::Parse(StreamIn &inStream) +{ + using ECommand = DebugRendererRecorder::ECommand; + + for (;;) + { + // Read the next command + ECommand command; + inStream.Read(command); + + if (inStream.IsEOF() || inStream.IsFailed()) + return; + + if (command == ECommand::CreateBatch) + { + uint32 id; + inStream.Read(id); + + uint32 triangle_count; + inStream.Read(triangle_count); + + DebugRenderer::Triangle *triangles = new DebugRenderer::Triangle [triangle_count]; + inStream.ReadBytes(triangles, triangle_count * sizeof(DebugRenderer::Triangle)); + + mBatches.insert({ id, mRenderer.CreateTriangleBatch(triangles, triangle_count) }); + + delete [] triangles; + } + else if (command == ECommand::CreateBatchIndexed) + { + uint32 id; + inStream.Read(id); + + uint32 vertex_count; + inStream.Read(vertex_count); + + DebugRenderer::Vertex *vertices = new DebugRenderer::Vertex [vertex_count]; + inStream.ReadBytes(vertices, vertex_count * sizeof(DebugRenderer::Vertex)); + + uint32 index_count; + inStream.Read(index_count); + + uint32 *indices = new uint32 [index_count]; + inStream.ReadBytes(indices, index_count * sizeof(uint32)); + + mBatches.insert({ id, mRenderer.CreateTriangleBatch(vertices, vertex_count, indices, index_count) }); + + delete [] indices; + delete [] vertices; + } + else if (command == ECommand::CreateGeometry) + { + uint32 geometry_id; + inStream.Read(geometry_id); + + AABox bounds; + inStream.Read(bounds.mMin); + inStream.Read(bounds.mMax); + + DebugRenderer::GeometryRef geometry = new DebugRenderer::Geometry(bounds); + mGeometries[geometry_id] = geometry; + + uint32 num_lods; + inStream.Read(num_lods); + for (uint32 l = 0; l < num_lods; ++l) + { + DebugRenderer::LOD lod; + inStream.Read(lod.mDistance); + + uint32 batch_id; + inStream.Read(batch_id); + lod.mTriangleBatch = mBatches.find(batch_id)->second; + + geometry->mLODs.push_back(lod); + } + } + else if (command == ECommand::EndFrame) + { + mFrames.push_back({}); + Frame &frame = mFrames.back(); + + // Read all lines + uint32 num_lines = 0; + inStream.Read(num_lines); + frame.mLines.resize(num_lines); + for (DebugRendererRecorder::LineBlob &line : frame.mLines) + { + inStream.Read(line.mFrom); + inStream.Read(line.mTo); + inStream.Read(line.mColor); + } + + // Read all triangles + uint32 num_triangles = 0; + inStream.Read(num_triangles); + frame.mTriangles.resize(num_triangles); + for (DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles) + { + inStream.Read(triangle.mV1); + inStream.Read(triangle.mV2); + inStream.Read(triangle.mV3); + inStream.Read(triangle.mColor); + inStream.Read(triangle.mCastShadow); + } + + // Read all texts + uint32 num_texts = 0; + inStream.Read(num_texts); + frame.mTexts.resize(num_texts); + for (DebugRendererRecorder::TextBlob &text : frame.mTexts) + { + inStream.Read(text.mPosition); + inStream.Read(text.mString); + inStream.Read(text.mColor); + inStream.Read(text.mHeight); + } + + // Read all geometries + uint32 num_geometries = 0; + inStream.Read(num_geometries); + frame.mGeometries.resize(num_geometries); + for (DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries) + { + inStream.Read(geom.mModelMatrix); + inStream.Read(geom.mModelColor); + inStream.Read(geom.mGeometryID); + inStream.Read(geom.mCullMode); + inStream.Read(geom.mCastShadow); + inStream.Read(geom.mDrawMode); + } + } + else + JPH_ASSERT(false); + } +} + +void DebugRendererPlayback::DrawFrame(uint inFrameNumber) const +{ + const Frame &frame = mFrames[inFrameNumber]; + + for (const DebugRendererRecorder::LineBlob &line : frame.mLines) + mRenderer.DrawLine(line.mFrom, line.mTo, line.mColor); + + for (const DebugRendererRecorder::TriangleBlob &triangle : frame.mTriangles) + mRenderer.DrawTriangle(triangle.mV1, triangle.mV2, triangle.mV3, triangle.mColor, triangle.mCastShadow); + + for (const DebugRendererRecorder::TextBlob &text : frame.mTexts) + mRenderer.DrawText3D(text.mPosition, text.mString, text.mColor, text.mHeight); + + for (const DebugRendererRecorder::GeometryBlob &geom : frame.mGeometries) + mRenderer.DrawGeometry(geom.mModelMatrix, geom.mModelColor, mGeometries.find(geom.mGeometryID)->second, geom.mCullMode, geom.mCastShadow, geom.mDrawMode); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/WickedEngine/Jolt/Renderer/DebugRendererPlayback.h b/WickedEngine/Jolt/Renderer/DebugRendererPlayback.h new file mode 100644 index 000000000..23ed45423 --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRendererPlayback.h @@ -0,0 +1,48 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that can read a recorded stream from DebugRendererRecorder and plays it back trough a DebugRenderer +class JPH_DEBUG_RENDERER_EXPORT DebugRendererPlayback +{ +public: + /// Constructor + DebugRendererPlayback(DebugRenderer &inRenderer) : mRenderer(inRenderer) { } + + /// Parse a stream of frames + void Parse(StreamIn &inStream); + + /// Get the number of parsed frames + uint GetNumFrames() const { return (uint)mFrames.size(); } + + /// Draw a frame + void DrawFrame(uint inFrameNumber) const; + +private: + /// The debug renderer we're using to do the actual rendering + DebugRenderer & mRenderer; + + /// Mapping of ID to batch + UnorderedMap mBatches; + + /// Mapping of ID to geometry + UnorderedMap mGeometries; + + /// The list of parsed frames + using Frame = DebugRendererRecorder::Frame; + Array mFrames; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Renderer/DebugRendererRecorder.cpp b/WickedEngine/Jolt/Renderer/DebugRendererRecorder.cpp new file mode 100644 index 000000000..2e2591295 --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRendererRecorder.cpp @@ -0,0 +1,158 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +void DebugRendererRecorder::DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mLines.push_back({ inFrom, inTo, inColor }); +} + +void DebugRendererRecorder::DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mTriangles.push_back({ inV1, inV2, inV3, inColor, inCastShadow }); +} + +DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) +{ + if (inTriangles == nullptr || inTriangleCount == 0) + return new BatchImpl(0); + + lock_guard lock(mMutex); + + mStream.Write(ECommand::CreateBatch); + + uint32 batch_id = mNextBatchID++; + JPH_ASSERT(batch_id != 0); + mStream.Write(batch_id); + mStream.Write((uint32)inTriangleCount); + mStream.WriteBytes(inTriangles, inTriangleCount * sizeof(Triangle)); + + return new BatchImpl(batch_id); +} + +DebugRenderer::Batch DebugRendererRecorder::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) +{ + if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0) + return new BatchImpl(0); + + lock_guard lock(mMutex); + + mStream.Write(ECommand::CreateBatchIndexed); + + uint32 batch_id = mNextBatchID++; + JPH_ASSERT(batch_id != 0); + mStream.Write(batch_id); + mStream.Write((uint32)inVertexCount); + mStream.WriteBytes(inVertices, inVertexCount * sizeof(Vertex)); + mStream.Write((uint32)inIndexCount); + mStream.WriteBytes(inIndices, inIndexCount * sizeof(uint32)); + + return new BatchImpl(batch_id); +} + +void DebugRendererRecorder::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + lock_guard lock(mMutex); + + // See if this geometry was used before + uint32 &geometry_id = mGeometries[inGeometry]; + if (geometry_id == 0) + { + mStream.Write(ECommand::CreateGeometry); + + // Create a new ID + geometry_id = mNextGeometryID++; + JPH_ASSERT(geometry_id != 0); + mStream.Write(geometry_id); + + // Save bounds + mStream.Write(inGeometry->mBounds.mMin); + mStream.Write(inGeometry->mBounds.mMax); + + // Save the LODs + mStream.Write((uint32)inGeometry->mLODs.size()); + for (const LOD & lod : inGeometry->mLODs) + { + mStream.Write(lod.mDistance); + mStream.Write(static_cast(lod.mTriangleBatch.GetPtr())->mID); + } + } + + mCurrentFrame.mGeometries.push_back({ inModelMatrix, inModelColor, geometry_id, inCullMode, inCastShadow, inDrawMode }); +} + +void DebugRendererRecorder::DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) +{ + lock_guard lock(mMutex); + + mCurrentFrame.mTexts.push_back({ inPosition, inString, inColor, inHeight }); +} + +void DebugRendererRecorder::EndFrame() +{ + lock_guard lock(mMutex); + + mStream.Write(ECommand::EndFrame); + + // Write all lines + mStream.Write((uint32)mCurrentFrame.mLines.size()); + for (const LineBlob &line : mCurrentFrame.mLines) + { + mStream.Write(line.mFrom); + mStream.Write(line.mTo); + mStream.Write(line.mColor); + } + mCurrentFrame.mLines.clear(); + + // Write all triangles + mStream.Write((uint32)mCurrentFrame.mTriangles.size()); + for (const TriangleBlob &triangle : mCurrentFrame.mTriangles) + { + mStream.Write(triangle.mV1); + mStream.Write(triangle.mV2); + mStream.Write(triangle.mV3); + mStream.Write(triangle.mColor); + mStream.Write(triangle.mCastShadow); + } + mCurrentFrame.mTriangles.clear(); + + // Write all texts + mStream.Write((uint32)mCurrentFrame.mTexts.size()); + for (const TextBlob &text : mCurrentFrame.mTexts) + { + mStream.Write(text.mPosition); + mStream.Write(text.mString); + mStream.Write(text.mColor); + mStream.Write(text.mHeight); + } + mCurrentFrame.mTexts.clear(); + + // Write all geometries + mStream.Write((uint32)mCurrentFrame.mGeometries.size()); + for (const GeometryBlob &geom : mCurrentFrame.mGeometries) + { + mStream.Write(geom.mModelMatrix); + mStream.Write(geom.mModelColor); + mStream.Write(geom.mGeometryID); + mStream.Write(geom.mCullMode); + mStream.Write(geom.mCastShadow); + mStream.Write(geom.mDrawMode); + } + mCurrentFrame.mGeometries.clear(); +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/WickedEngine/Jolt/Renderer/DebugRendererRecorder.h b/WickedEngine/Jolt/Renderer/DebugRendererRecorder.h new file mode 100644 index 000000000..9608e03c9 --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRendererRecorder.h @@ -0,0 +1,130 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Implementation of DebugRenderer that records the API invocations to be played back later +class JPH_DEBUG_RENDERER_EXPORT DebugRendererRecorder final : public DebugRenderer +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRendererRecorder(StreamOut &inStream) : mStream(inStream) { Initialize(); } + + /// Implementation of DebugRenderer interface + virtual void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) override; + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override; + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override; + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override; + virtual void DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) override; + + /// Mark the end of a frame + void EndFrame(); + + /// Control commands written into the stream + enum class ECommand : uint8 + { + CreateBatch, + CreateBatchIndexed, + CreateGeometry, + EndFrame + }; + + /// Holds a single line segment + struct LineBlob + { + RVec3 mFrom; + RVec3 mTo; + Color mColor; + }; + + /// Holds a single triangle + struct TriangleBlob + { + RVec3 mV1; + RVec3 mV2; + RVec3 mV3; + Color mColor; + ECastShadow mCastShadow; + }; + + /// Holds a single text entry + struct TextBlob + { + TextBlob() = default; + TextBlob(RVec3Arg inPosition, const string_view &inString, ColorArg inColor, float inHeight) : mPosition(inPosition), mString(inString), mColor(inColor), mHeight(inHeight) { } + + RVec3 mPosition; + String mString; + Color mColor; + float mHeight; + }; + + /// Holds a single geometry draw call + struct GeometryBlob + { + RMat44 mModelMatrix; + Color mModelColor; + uint32 mGeometryID; + ECullMode mCullMode; + ECastShadow mCastShadow; + EDrawMode mDrawMode; + }; + + /// All information for a single frame + struct Frame + { + Array mLines; + Array mTriangles; + Array mTexts; + Array mGeometries; + }; + +private: + /// Implementation specific batch object + class BatchImpl : public RefTargetVirtual + { + public: + JPH_OVERRIDE_NEW_DELETE + + BatchImpl(uint32 inID) : mID(inID) { } + + virtual void AddRef() override { ++mRefCount; } + virtual void Release() override { if (--mRefCount == 0) delete this; } + + atomic mRefCount = 0; + uint32 mID; + }; + + /// Lock that prevents concurrent access to the internal structures + Mutex mMutex; + + /// Stream that recorded data will be sent to + StreamOut & mStream; + + /// Next available ID + uint32 mNextBatchID = 1; + uint32 mNextGeometryID = 1; + + /// Cached geometries and their IDs + UnorderedMap mGeometries; + + /// Data that is being accumulated for the current frame + Frame mCurrentFrame; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Renderer/DebugRendererSimple.cpp b/WickedEngine/Jolt/Renderer/DebugRendererSimple.cpp new file mode 100644 index 000000000..a404d95a0 --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRendererSimple.cpp @@ -0,0 +1,80 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#ifdef JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +DebugRendererSimple::DebugRendererSimple() +{ + Initialize(); +} + +DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) +{ + BatchImpl *batch = new BatchImpl; + if (inTriangles == nullptr || inTriangleCount == 0) + return batch; + + batch->mTriangles.assign(inTriangles, inTriangles + inTriangleCount); + return batch; +} + +DebugRenderer::Batch DebugRendererSimple::CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) +{ + BatchImpl *batch = new BatchImpl; + if (inVertices == nullptr || inVertexCount == 0 || inIndices == nullptr || inIndexCount == 0) + return batch; + + // Convert indexed triangle list to triangle list + batch->mTriangles.resize(inIndexCount / 3); + for (size_t t = 0; t < batch->mTriangles.size(); ++t) + { + Triangle &triangle = batch->mTriangles[t]; + triangle.mV[0] = inVertices[inIndices[t * 3 + 0]]; + triangle.mV[1] = inVertices[inIndices[t * 3 + 1]]; + triangle.mV[2] = inVertices[inIndices[t * 3 + 2]]; + } + + return batch; +} + +void DebugRendererSimple::DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) +{ + // Figure out which LOD to use + const LOD *lod = inGeometry->mLODs.data(); + if (mCameraPosSet) + lod = &inGeometry->GetLOD(Vec3(mCameraPos), inWorldSpaceBounds, inLODScaleSq); + + // Draw the batch + const BatchImpl *batch = static_cast(lod->mTriangleBatch.GetPtr()); + for (const Triangle &triangle : batch->mTriangles) + { + RVec3 v0 = inModelMatrix * Vec3(triangle.mV[0].mPosition); + RVec3 v1 = inModelMatrix * Vec3(triangle.mV[1].mPosition); + RVec3 v2 = inModelMatrix * Vec3(triangle.mV[2].mPosition); + Color color = inModelColor * triangle.mV[0].mColor; + + switch (inDrawMode) + { + case EDrawMode::Wireframe: + DrawLine(v0, v1, color); + DrawLine(v1, v2, color); + DrawLine(v2, v0, color); + break; + + case EDrawMode::Solid: + DrawTriangle(v0, v1, v2, color, inCastShadow); + break; + } + } +} + +JPH_NAMESPACE_END + +#endif // JPH_DEBUG_RENDERER diff --git a/WickedEngine/Jolt/Renderer/DebugRendererSimple.h b/WickedEngine/Jolt/Renderer/DebugRendererSimple.h new file mode 100644 index 000000000..4a23ab758 --- /dev/null +++ b/WickedEngine/Jolt/Renderer/DebugRendererSimple.h @@ -0,0 +1,88 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2024 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#ifndef JPH_DEBUG_RENDERER + #error This file should only be included when JPH_DEBUG_RENDERER is defined +#endif // !JPH_DEBUG_RENDERER + +#include + +JPH_NAMESPACE_BEGIN + +/// Inherit from this class to simplify implementing a debug renderer, start with this implementation: +/// +/// class MyDebugRenderer : public JPH::DebugRendererSimple +/// { +/// public: +/// virtual void DrawLine(JPH::RVec3Arg inFrom, JPH::RVec3Arg inTo, JPH::ColorArg inColor) override +/// { +/// // Implement +/// } +/// +/// virtual void DrawTriangle(JPH::RVec3Arg inV1, JPH::RVec3Arg inV2, JPH::RVec3Arg inV3, JPH::ColorArg inColor, ECastShadow inCastShadow) override +/// { +/// // Implement +/// } +/// +/// virtual void DrawText3D(JPH::RVec3Arg inPosition, const string_view &inString, JPH::ColorArg inColor, float inHeight) override +/// { +/// // Implement +/// } +/// }; +/// +/// Note that this class is meant to be a quick start for implementing a debug renderer, it is not the most efficient way to implement a debug renderer. +class JPH_DEBUG_RENDERER_EXPORT DebugRendererSimple : public DebugRenderer +{ +public: + JPH_OVERRIDE_NEW_DELETE + + /// Constructor + DebugRendererSimple(); + + /// Should be called every frame by the application to provide the camera position. + /// This is used to determine the correct LOD for rendering. + void SetCameraPos(RVec3Arg inCameraPos) + { + mCameraPos = inCameraPos; + mCameraPosSet = true; + } + + /// Fallback implementation that uses DrawLine to draw a triangle (override this if you have a version that renders solid triangles) + virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow) override + { + DrawLine(inV1, inV2, inColor); + DrawLine(inV2, inV3, inColor); + DrawLine(inV3, inV1, inColor); + } + +protected: + /// Implementation of DebugRenderer interface + virtual Batch CreateTriangleBatch(const Triangle *inTriangles, int inTriangleCount) override; + virtual Batch CreateTriangleBatch(const Vertex *inVertices, int inVertexCount, const uint32 *inIndices, int inIndexCount) override; + virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode, ECastShadow inCastShadow, EDrawMode inDrawMode) override; + +private: + /// Implementation specific batch object + class BatchImpl : public RefTargetVirtual + { + public: + JPH_OVERRIDE_NEW_DELETE + + virtual void AddRef() override { ++mRefCount; } + virtual void Release() override { if (--mRefCount == 0) delete this; } + + Array mTriangles; + + private: + atomic mRefCount = 0; + }; + + /// Last provided camera position + RVec3 mCameraPos; + bool mCameraPosSet = false; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/SkeletalAnimation.cpp b/WickedEngine/Jolt/Skeleton/SkeletalAnimation.cpp new file mode 100644 index 000000000..4bf87007e --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/SkeletalAnimation.cpp @@ -0,0 +1,110 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::JointState) +{ + JPH_ADD_ATTRIBUTE(JointState, mRotation) + JPH_ADD_ATTRIBUTE(JointState, mTranslation) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::Keyframe) +{ + JPH_ADD_BASE_CLASS(Keyframe, JointState) + + JPH_ADD_ATTRIBUTE(Keyframe, mTime) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation::AnimatedJoint) +{ + JPH_ADD_ATTRIBUTE(AnimatedJoint, mJointName) + JPH_ADD_ATTRIBUTE(AnimatedJoint, mKeyframes) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(SkeletalAnimation) +{ + JPH_ADD_ATTRIBUTE(SkeletalAnimation, mAnimatedJoints) + JPH_ADD_ATTRIBUTE(SkeletalAnimation, mIsLooping) +} + + +void SkeletalAnimation::JointState::FromMatrix(Mat44Arg inMatrix) +{ + mRotation = inMatrix.GetQuaternion(); + mTranslation = inMatrix.GetTranslation(); +} + +float SkeletalAnimation::GetDuration() const +{ + if (!mAnimatedJoints.empty() && !mAnimatedJoints[0].mKeyframes.empty()) + return mAnimatedJoints[0].mKeyframes.back().mTime; + else + return 0.0f; +} + +void SkeletalAnimation::ScaleJoints(float inScale) +{ + for (SkeletalAnimation::AnimatedJoint &j : mAnimatedJoints) + for (SkeletalAnimation::Keyframe &k : j.mKeyframes) + k.mTranslation *= inScale; +} + +void SkeletalAnimation::Sample(float inTime, SkeletonPose &ioPose) const +{ + // Correct time when animation is looping + JPH_ASSERT(inTime >= 0.0f); + float duration = GetDuration(); + float time = duration > 0.0f && mIsLooping? fmod(inTime, duration) : inTime; + + for (const AnimatedJoint &aj : mAnimatedJoints) + { + // Do binary search for keyframe + int high = (int)aj.mKeyframes.size(), low = -1; + while (high - low > 1) + { + int probe = (high + low) / 2; + if (aj.mKeyframes[probe].mTime < time) + low = probe; + else + high = probe; + } + + JointState &state = ioPose.GetJoint(ioPose.GetSkeleton()->GetJointIndex(aj.mJointName)); + + if (low == -1) + { + // Before first key, return first key + state = static_cast(aj.mKeyframes.front()); + } + else if (high == (int)aj.mKeyframes.size()) + { + // Beyond last key, return last key + state = static_cast(aj.mKeyframes.back()); + } + else + { + // Interpolate + const Keyframe &s1 = aj.mKeyframes[low]; + const Keyframe &s2 = aj.mKeyframes[low + 1]; + + float fraction = (time - s1.mTime) / (s2.mTime - s1.mTime); + JPH_ASSERT(fraction >= 0.0f && fraction <= 1.0f); + + state.mTranslation = (1.0f - fraction) * s1.mTranslation + fraction * s2.mTranslation; + JPH_ASSERT(s1.mRotation.IsNormalized()); + JPH_ASSERT(s2.mRotation.IsNormalized()); + state.mRotation = s1.mRotation.SLERP(s2.mRotation, fraction); + JPH_ASSERT(state.mRotation.IsNormalized()); + } + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/SkeletalAnimation.h b/WickedEngine/Jolt/Skeleton/SkeletalAnimation.h new file mode 100644 index 000000000..805912091 --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/SkeletalAnimation.h @@ -0,0 +1,77 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +class SkeletonPose; + +/// Resource for a skinned animation +class JPH_EXPORT SkeletalAnimation : public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, SkeletalAnimation) + + /// Contains the current state of a joint, a local space transformation relative to its parent joint + class JointState + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, JointState) + + /// Convert from a local space matrix + void FromMatrix(Mat44Arg inMatrix); + + /// Convert to matrix representation + inline Mat44 ToMatrix() const { return Mat44::sRotationTranslation(mRotation, mTranslation); } + + Quat mRotation = Quat::sIdentity(); ///< Local space rotation of the joint + Vec3 mTranslation = Vec3::sZero(); ///< Local space translation of the joint + }; + + /// Contains the state of a single joint at a particular time + class Keyframe : public JointState + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Keyframe) + + float mTime = 0.0f; ///< Time of keyframe in seconds + }; + + using KeyframeVector = Array; + + /// Contains the animation for a single joint + class AnimatedJoint + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, AnimatedJoint) + + String mJointName; ///< Name of the joint + KeyframeVector mKeyframes; ///< List of keyframes over time + }; + + using AnimatedJointVector = Array; + + /// Get the length (in seconds) of this animation + float GetDuration() const; + + /// Scale the size of all joints by inScale + void ScaleJoints(float inScale); + + /// Get the (interpolated) joint transforms at time inTime + void Sample(float inTime, SkeletonPose &ioPose) const; + + /// Get joint samples + const AnimatedJointVector & GetAnimatedJoints() const { return mAnimatedJoints; } + AnimatedJointVector & GetAnimatedJoints() { return mAnimatedJoints; } + +private: + AnimatedJointVector mAnimatedJoints; ///< List of joints and keyframes + bool mIsLooping = true; ///< If this animation loops back to start +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/Skeleton.cpp b/WickedEngine/Jolt/Skeleton/Skeleton.cpp new file mode 100644 index 000000000..5ab7f56e7 --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/Skeleton.cpp @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton::Joint) +{ + JPH_ADD_ATTRIBUTE(Joint, mName) + JPH_ADD_ATTRIBUTE(Joint, mParentName) +} + +JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(Skeleton) +{ + JPH_ADD_ATTRIBUTE(Skeleton, mJoints) +} + +int Skeleton::GetJointIndex(const string_view &inName) const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + if (mJoints[i].mName == inName) + return i; + + return -1; +} + +void Skeleton::CalculateParentJointIndices() +{ + for (Joint &j : mJoints) + j.mParentJointIndex = GetJointIndex(j.mParentName); +} + +bool Skeleton::AreJointsCorrectlyOrdered() const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + if (mJoints[i].mParentJointIndex >= i) + return false; + + return true; +} + +void Skeleton::SaveBinaryState(StreamOut &inStream) const +{ + inStream.Write((uint32)mJoints.size()); + for (const Joint &j : mJoints) + { + inStream.Write(j.mName); + inStream.Write(j.mParentJointIndex); + inStream.Write(j.mParentName); + } +} + +Skeleton::SkeletonResult Skeleton::sRestoreFromBinaryState(StreamIn &inStream) +{ + Ref skeleton = new Skeleton; + + uint32 len = 0; + inStream.Read(len); + skeleton->mJoints.resize(len); + for (Joint &j : skeleton->mJoints) + { + inStream.Read(j.mName); + inStream.Read(j.mParentJointIndex); + inStream.Read(j.mParentName); + } + + SkeletonResult result; + if (inStream.IsEOF() || inStream.IsFailed()) + result.SetError("Failed to read skeleton from stream"); + else + result.Set(skeleton); + return result; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/Skeleton.h b/WickedEngine/Jolt/Skeleton/Skeleton.h new file mode 100644 index 000000000..3c2b84c66 --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/Skeleton.h @@ -0,0 +1,72 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +class StreamIn; +class StreamOut; + +/// Resource that contains the joint hierarchy for a skeleton +class JPH_EXPORT Skeleton : public RefTarget +{ +public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Skeleton) + + using SkeletonResult = Result>; + + /// Declare internal structure for a joint + class Joint + { + public: + JPH_DECLARE_SERIALIZABLE_NON_VIRTUAL(JPH_EXPORT, Joint) + + Joint() = default; + Joint(const string_view &inName, const string_view &inParentName, int inParentJointIndex) : mName(inName), mParentName(inParentName), mParentJointIndex(inParentJointIndex) { } + + String mName; ///< Name of the joint + String mParentName; ///< Name of parent joint + int mParentJointIndex = -1; ///< Index of parent joint (in mJoints) or -1 if it has no parent + }; + + using JointVector = Array; + + ///@name Access to the joints + ///@{ + const JointVector & GetJoints() const { return mJoints; } + JointVector & GetJoints() { return mJoints; } + int GetJointCount() const { return (int)mJoints.size(); } + const Joint & GetJoint(int inJoint) const { return mJoints[inJoint]; } + Joint & GetJoint(int inJoint) { return mJoints[inJoint]; } + uint AddJoint(const string_view &inName, const string_view &inParentName = string_view()) { mJoints.emplace_back(inName, inParentName, -1); return (uint)mJoints.size() - 1; } + uint AddJoint(const string_view &inName, int inParentIndex) { mJoints.emplace_back(inName, inParentIndex >= 0? mJoints[inParentIndex].mName : String(), inParentIndex); return (uint)mJoints.size() - 1; } + ///@} + + /// Find joint by name + int GetJointIndex(const string_view &inName) const; + + /// Fill in parent joint indices based on name + void CalculateParentJointIndices(); + + /// Many of the algorithms that use the Skeleton class require that parent joints are in the mJoints array before their children. + /// This function returns true if this is the case, false if not. + bool AreJointsCorrectlyOrdered() const; + + /// Saves the state of this object in binary form to inStream. + void SaveBinaryState(StreamOut &inStream) const; + + /// Restore the state of this object from inStream. + static SkeletonResult sRestoreFromBinaryState(StreamIn &inStream); + +private: + /// Joints + JointVector mJoints; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/SkeletonMapper.cpp b/WickedEngine/Jolt/Skeleton/SkeletonMapper.cpp new file mode 100644 index 000000000..add2956d5 --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/SkeletonMapper.cpp @@ -0,0 +1,237 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +void SkeletonMapper::Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint) +{ + JPH_ASSERT(mMappings.empty() && mChains.empty() && mUnmapped.empty()); // Should not be initialized yet + + // Count joints + int n1 = inSkeleton1->GetJointCount(); + int n2 = inSkeleton2->GetJointCount(); + JPH_ASSERT(n1 <= n2, "Skeleton 1 should be the low detail skeleton!"); + + // Keep track of mapped joints (initialize to false) + Array mapped1(n1, false); + Array mapped2(n2, false); + + // Find joints that can be mapped directly + for (int j1 = 0; j1 < n1; ++j1) + for (int j2 = 0; j2 < n2; ++j2) + if (inCanMapJoint(inSkeleton1, j1, inSkeleton2, j2)) + { + // Calculate the transform that takes this joint from skeleton 1 to 2 + Mat44 joint_1_to_2 = inNeutralPose1[j1].Inversed() * inNeutralPose2[j2]; + + // Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so) + joint_1_to_2(3, 3) = 1.0f; + + mMappings.emplace_back(j1, j2, joint_1_to_2); + mapped1[j1] = true; + mapped2[j2] = true; + break; + } + + Array cur_chain; // Taken out of the loop to minimize amount of allocations + + // Find joint chains + for (int m1 = 0; m1 < (int)mMappings.size(); ++m1) + { + Array chain2; + int chain2_m = -1; + + for (int m2 = m1 + 1; m2 < (int)mMappings.size(); ++m2) + { + // Find the chain from back from m2 to m1 + int start = mMappings[m1].mJointIdx2; + int end = mMappings[m2].mJointIdx2; + int cur = end; + cur_chain.clear(); // Should preserve memory + do + { + cur_chain.push_back(cur); + cur = inSkeleton2->GetJoint(cur).mParentJointIndex; + } + while (cur >= 0 && cur != start && !mapped2[cur]); + cur_chain.push_back(start); + + if (cur == start // This should be the correct chain + && cur_chain.size() > 2 // It should have joints between the mapped joints + && cur_chain.size() > chain2.size()) // And it should be the longest so far + { + chain2.swap(cur_chain); + chain2_m = m2; + } + } + + if (!chain2.empty()) + { + // Get the chain for 1 + Array chain1; + int start = mMappings[m1].mJointIdx1; + int cur = mMappings[chain2_m].mJointIdx1; + do + { + chain1.push_back(cur); + cur = inSkeleton1->GetJoint(cur).mParentJointIndex; + } + while (cur >= 0 && cur != start && !mapped1[cur]); + chain1.push_back(start); + + // If the chain exists in 1 too + if (cur == start) + { + // Reverse the chains + std::reverse(chain1.begin(), chain1.end()); + std::reverse(chain2.begin(), chain2.end()); + + // Mark elements mapped + for (int j1 : chain1) + mapped1[j1] = true; + for (int j2 : chain2) + mapped2[j2] = true; + + // Insert the chain + mChains.emplace_back(std::move(chain1), std::move(chain2)); + } + } + } + + // Collect unmapped joints from 2 + for (int j2 = 0; j2 < n2; ++j2) + if (!mapped2[j2]) + mUnmapped.emplace_back(j2, inSkeleton2->GetJoint(j2).mParentJointIndex); +} + +void SkeletonMapper::LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2) +{ + JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered()); + + int n = inSkeleton2->GetJointCount(); + + // Copy locked joints to array but don't actually include the first joint (this is physics driven) + for (int i = 0; i < n; ++i) + if (inLockedTranslations[i]) + { + Locked l; + l.mJointIdx = i; + l.mParentJointIdx = inSkeleton2->GetJoint(i).mParentJointIndex; + if (l.mParentJointIdx >= 0) + l.mTranslation = inNeutralPose2[l.mParentJointIdx].Inversed() * inNeutralPose2[i].GetTranslation(); + else + l.mTranslation = inNeutralPose2[i].GetTranslation(); + mLockedTranslations.push_back(l); + } +} + +void SkeletonMapper::LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2) +{ + JPH_ASSERT(!mMappings.empty(), "Call Initialize first!"); + JPH_ASSERT(inSkeleton2->AreJointsCorrectlyOrdered()); + + // The first mapping is the top most one (remember that joints should be ordered so that parents go before children). + // Because we created the mappings from the lowest joint first, this should contain the first mappable joint. + int root_idx = mMappings[0].mJointIdx2; + + // Create temp array to hold locked joints + int n = inSkeleton2->GetJointCount(); + bool *locked_translations = (bool *)JPH_STACK_ALLOC(n * sizeof(bool)); + memset(locked_translations, 0, n * sizeof(bool)); + + // Mark root as locked + locked_translations[root_idx] = true; + + // Loop over all joints and propagate the locked flag to all children + for (int i = root_idx + 1; i < n; ++i) + { + int parent_idx = inSkeleton2->GetJoint(i).mParentJointIndex; + if (parent_idx >= 0) + locked_translations[i] = locked_translations[parent_idx]; + } + + // Unmark root because we don't actually want to include this (this determines the position of the entire ragdoll) + locked_translations[root_idx] = false; + + // Call the generic function + LockTranslations(inSkeleton2, locked_translations, inNeutralPose2); +} + +void SkeletonMapper::Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const +{ + // Apply direct mappings + for (const Mapping &m : mMappings) + outPose2ModelSpace[m.mJointIdx2] = inPose1ModelSpace[m.mJointIdx1] * m.mJoint1To2; + + // Apply chain mappings + for (const Chain &c : mChains) + { + // Calculate end of chain given local space transforms of the joints of the chain + Mat44 &chain_start = outPose2ModelSpace[c.mJointIndices2.front()]; + Mat44 chain_end = chain_start; + for (int j = 1; j < (int)c.mJointIndices2.size(); ++j) + chain_end = chain_end * inPose2LocalSpace[c.mJointIndices2[j]]; + + // Calculate the direction in world space for skeleton 1 and skeleton 2 and the rotation between them + Vec3 actual = chain_end.GetTranslation() - chain_start.GetTranslation(); + Vec3 desired = inPose1ModelSpace[c.mJointIndices1.back()].GetTranslation() - inPose1ModelSpace[c.mJointIndices1.front()].GetTranslation(); + Quat rotation = Quat::sFromTo(actual, desired); + + // Rotate the start of the chain + chain_start.SetRotation(Mat44::sRotation(rotation) * chain_start.GetRotation()); + + // Update all joints but the first and the last joint using their local space transforms + for (int j = 1; j < (int)c.mJointIndices2.size() - 1; ++j) + { + int parent = c.mJointIndices2[j - 1]; + int child = c.mJointIndices2[j]; + outPose2ModelSpace[child] = outPose2ModelSpace[parent] * inPose2LocalSpace[child]; + } + } + + // All unmapped joints take the local pose and convert it to model space + for (const Unmapped &u : mUnmapped) + if (u.mParentJointIdx >= 0) + { + JPH_ASSERT(u.mParentJointIdx < u.mJointIdx, "Joints must be ordered: parents first"); + outPose2ModelSpace[u.mJointIdx] = outPose2ModelSpace[u.mParentJointIdx] * inPose2LocalSpace[u.mJointIdx]; + } + else + outPose2ModelSpace[u.mJointIdx] = inPose2LocalSpace[u.mJointIdx]; + + // Update all locked joint translations + for (const Locked &l : mLockedTranslations) + outPose2ModelSpace[l.mJointIdx].SetTranslation(outPose2ModelSpace[l.mParentJointIdx] * l.mTranslation); +} + +void SkeletonMapper::MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const +{ + // Normally each joint in skeleton 1 should be present in the mapping, so we only need to apply the direct mappings + for (const Mapping &m : mMappings) + outPose1ModelSpace[m.mJointIdx1] = inPose2ModelSpace[m.mJointIdx2] * m.mJoint2To1; +} + +int SkeletonMapper::GetMappedJointIdx(int inJoint1Idx) const +{ + for (const Mapping &m : mMappings) + if (m.mJointIdx1 == inJoint1Idx) + return m.mJointIdx2; + + return -1; +} + +bool SkeletonMapper::IsJointTranslationLocked(int inJoint2Idx) const +{ + for (const Locked &l : mLockedTranslations) + if (l.mJointIdx == inJoint2Idx) + return true; + + return false; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/SkeletonMapper.h b/WickedEngine/Jolt/Skeleton/SkeletonMapper.h new file mode 100644 index 000000000..05cc8866a --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/SkeletonMapper.h @@ -0,0 +1,145 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2022 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Class that is able to map a low detail (ragdoll) skeleton to a high detail (animation) skeleton and vice versa +class JPH_EXPORT SkeletonMapper : public RefTarget +{ +public: + /// A joint that maps 1-on-1 to a joint in the other skeleton + class Mapping + { + public: + Mapping() = default; + Mapping(int inJointIdx1, int inJointIdx2, Mat44Arg inJoint1To2) : mJointIdx1(inJointIdx1), mJointIdx2(inJointIdx2), mJoint1To2(inJoint1To2), mJoint2To1(inJoint1To2.Inversed()) + { + // Ensure bottom right element is 1 (numerical imprecision in the inverse can make this not so) + mJoint2To1(3, 3) = 1.0f; + } + + int mJointIdx1; ///< Index of joint from skeleton 1 + int mJointIdx2; ///< Corresponding index of joint from skeleton 2 + Mat44 mJoint1To2; ///< Transforms this joint from skeleton 1 to 2 + Mat44 mJoint2To1; ///< Inverse of the transform above + }; + + /// A joint chain that starts with a 1-on-1 mapped joint and ends with a 1-on-1 mapped joint with intermediate joints that cannot be mapped + class Chain + { + public: + Chain() = default; + Chain(Array &&inJointIndices1, Array &&inJointIndices2) : mJointIndices1(std::move(inJointIndices1)), mJointIndices2(std::move(inJointIndices2)) { } + + Array mJointIndices1; ///< Joint chain from skeleton 1 + Array mJointIndices2; ///< Corresponding joint chain from skeleton 2 + }; + + /// Joints that could not be mapped from skeleton 1 to 2 + class Unmapped + { + public: + Unmapped() = default; + Unmapped(int inJointIdx, int inParentJointIdx) : mJointIdx(inJointIdx), mParentJointIdx(inParentJointIdx) { } + + int mJointIdx; ///< Joint index of unmappable joint + int mParentJointIdx; ///< Parent joint index of unmappable joint + }; + + /// Joints that should have their translation locked (fixed) + class Locked + { + public: + int mJointIdx; ///< Joint index of joint with locked translation (in skeleton 2) + int mParentJointIdx; ///< Parent joint index of joint with locked translation (in skeleton 2) + Vec3 mTranslation; ///< Translation of neutral pose + }; + + /// A function that is called to determine if a joint can be mapped from source to target skeleton + using CanMapJoint = function; + + /// Default function that checks if the names of the joints are equal + static bool sDefaultCanMapJoint(const Skeleton *inSkeleton1, int inIndex1, const Skeleton *inSkeleton2, int inIndex2) + { + return inSkeleton1->GetJoint(inIndex1).mName == inSkeleton2->GetJoint(inIndex2).mName; + } + + /// Initialize the skeleton mapper. Skeleton 1 should be the (low detail) ragdoll skeleton and skeleton 2 the (high detail) animation skeleton. + /// We assume that each joint in skeleton 1 can be mapped to a joint in skeleton 2 (if not mapping from animation skeleton to ragdoll skeleton will be undefined). + /// Skeleton 2 should have the same hierarchy as skeleton 1 but can contain extra joints between those in skeleton 1 and it can have extra joints at the root and leaves of the skeleton. + /// @param inSkeleton1 Source skeleton to map from. + /// @param inNeutralPose1 Neutral pose of the source skeleton (model space) + /// @param inSkeleton2 Target skeleton to map to. + /// @param inNeutralPose2 Neutral pose of the target skeleton (model space), inNeutralPose1 and inNeutralPose2 must match as closely as possible, preferably the position of the mappable joints should be identical. + /// @param inCanMapJoint Function that checks if joints in skeleton 1 and skeleton 2 are equal. + void Initialize(const Skeleton *inSkeleton1, const Mat44 *inNeutralPose1, const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2, const CanMapJoint &inCanMapJoint = sDefaultCanMapJoint); + + /// This can be called so lock the translation of a specified set of joints in skeleton 2. + /// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress. + /// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the + /// physical simulation and the visual representation. + /// @param inSkeleton2 Target skeleton to map to. + /// @param inLockedTranslations An array of bools the size of inSkeleton2->GetJointCount(), for each joint indicating if the joint is locked. + /// @param inNeutralPose2 Neutral pose to take reference translations from + void LockTranslations(const Skeleton *inSkeleton2, const bool *inLockedTranslations, const Mat44 *inNeutralPose2); + + /// After Initialize(), this can be called to lock the translation of all joints in skeleton 2 below the first mapped joint to those of the neutral pose. + /// Because constraints are never 100% rigid, there's always a little bit of stretch in the ragdoll when the ragdoll is under stress. + /// Locking the translations of the pose will remove the visual stretch from the ragdoll but will introduce a difference between the + /// physical simulation and the visual representation. + /// @param inSkeleton2 Target skeleton to map to. + /// @param inNeutralPose2 Neutral pose to take reference translations from + void LockAllTranslations(const Skeleton *inSkeleton2, const Mat44 *inNeutralPose2); + + /// Map a pose. Joints that were directly mappable will be copied in model space from pose 1 to pose 2. Any joints that are only present in skeleton 2 + /// will get their model space transform calculated through the local space transforms of pose 2. Joints that are part of a joint chain between two + /// mapped joints will be reoriented towards the next joint in skeleton 1. This means that it is possible for unmapped joints to have some animation, + /// but very extreme animation poses will show artifacts. + /// @param inPose1ModelSpace Pose on skeleton 1 in model space + /// @param inPose2LocalSpace Pose on skeleton 2 in local space (used for the joints that cannot be mapped) + /// @param outPose2ModelSpace Model space pose on skeleton 2 (the output of the mapping) + void Map(const Mat44 *inPose1ModelSpace, const Mat44 *inPose2LocalSpace, Mat44 *outPose2ModelSpace) const; + + /// Reverse map a pose, this will only use the mappings and not the chains (it assumes that all joints in skeleton 1 are mapped) + /// @param inPose2ModelSpace Model space pose on skeleton 2 + /// @param outPose1ModelSpace When the function returns this will contain the model space pose for skeleton 1 + void MapReverse(const Mat44 *inPose2ModelSpace, Mat44 *outPose1ModelSpace) const; + + /// Search through the directly mapped joints (mMappings) and find inJoint1Idx, returns the corresponding Joint2Idx or -1 if not found. + int GetMappedJointIdx(int inJoint1Idx) const; + + /// Search through the locked translations (mLockedTranslations) and find if joint inJoint2Idx is locked. + bool IsJointTranslationLocked(int inJoint2Idx) const; + + using MappingVector = Array; + using ChainVector = Array; + using UnmappedVector = Array; + using LockedVector = Array; + + ///@name Access to the mapped joints + ///@{ + const MappingVector & GetMappings() const { return mMappings; } + MappingVector & GetMappings() { return mMappings; } + const ChainVector & GetChains() const { return mChains; } + ChainVector & GetChains() { return mChains; } + const UnmappedVector & GetUnmapped() const { return mUnmapped; } + UnmappedVector & GetUnmapped() { return mUnmapped; } + const LockedVector & GetLockedTranslations() const { return mLockedTranslations; } + LockedVector & GetLockedTranslations() { return mLockedTranslations; } + ///@} + +private: + /// Joint mappings + MappingVector mMappings; + ChainVector mChains; + UnmappedVector mUnmapped; ///< Joint indices that could not be mapped from 1 to 2 (these are indices in 2) + LockedVector mLockedTranslations; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/SkeletonPose.cpp b/WickedEngine/Jolt/Skeleton/SkeletonPose.cpp new file mode 100644 index 000000000..c64bf0497 --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/SkeletonPose.cpp @@ -0,0 +1,87 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#ifdef JPH_DEBUG_RENDERER + #include +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_BEGIN + +void SkeletonPose::SetSkeleton(const Skeleton *inSkeleton) +{ + mSkeleton = inSkeleton; + + mJoints.resize(mSkeleton->GetJointCount()); + mJointMatrices.resize(mSkeleton->GetJointCount()); +} + +void SkeletonPose::CalculateJointMatrices() +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + { + mJointMatrices[i] = mJoints[i].ToMatrix(); + + int parent = mSkeleton->GetJoint(i).mParentJointIndex; + if (parent >= 0) + { + JPH_ASSERT(parent < i, "Joints must be ordered: parents first"); + mJointMatrices[i] = mJointMatrices[parent] * mJointMatrices[i]; + } + } +} + +void SkeletonPose::CalculateJointStates() +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + { + Mat44 local_transform; + int parent = mSkeleton->GetJoint(i).mParentJointIndex; + if (parent >= 0) + local_transform = mJointMatrices[parent].Inversed() * mJointMatrices[i]; + else + local_transform = mJointMatrices[i]; + + JointState &joint = mJoints[i]; + joint.mTranslation = local_transform.GetTranslation(); + joint.mRotation = local_transform.GetQuaternion(); + } +} + +void SkeletonPose::CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const +{ + for (int i = 0; i < (int)mJoints.size(); ++i) + outMatrices[i] = mJoints[i].ToMatrix(); +} + +#ifdef JPH_DEBUG_RENDERER +void SkeletonPose::Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset) const +{ + RMat44 offset = inOffset * RMat44::sTranslation(mRootOffset); + + const Skeleton::JointVector &joints = mSkeleton->GetJoints(); + + for (int b = 0; b < mSkeleton->GetJointCount(); ++b) + { + RMat44 joint_transform = offset * mJointMatrices[b]; + + if (inDrawSettings.mDrawJoints) + { + int parent = joints[b].mParentJointIndex; + if (parent >= 0) + inRenderer->DrawLine(offset * mJointMatrices[parent].GetTranslation(), joint_transform.GetTranslation(), Color::sGreen); + } + + if (inDrawSettings.mDrawJointOrientations) + inRenderer->DrawCoordinateSystem(joint_transform, 0.05f); + + if (inDrawSettings.mDrawJointNames) + inRenderer->DrawText3D(joint_transform.GetTranslation(), joints[b].mName, Color::sWhite, 0.05f); + } +} +#endif // JPH_DEBUG_RENDERER + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/Skeleton/SkeletonPose.h b/WickedEngine/Jolt/Skeleton/SkeletonPose.h new file mode 100644 index 000000000..326227e44 --- /dev/null +++ b/WickedEngine/Jolt/Skeleton/SkeletonPose.h @@ -0,0 +1,82 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +#ifdef JPH_DEBUG_RENDERER +class DebugRenderer; +#endif // JPH_DEBUG_RENDERER + +/// Instance of a skeleton, contains the pose the current skeleton is in +class JPH_EXPORT SkeletonPose +{ +public: + JPH_OVERRIDE_NEW_DELETE + + using JointState = SkeletalAnimation::JointState; + using JointStateVector = Array; + using Mat44Vector = Array; + + ///@name Skeleton + ///@{ + void SetSkeleton(const Skeleton *inSkeleton); + const Skeleton * GetSkeleton() const { return mSkeleton; } + ///@} + + /// Extra offset applied to the root (and therefore also to all of its children) + void SetRootOffset(RVec3Arg inOffset) { mRootOffset = inOffset; } + RVec3 GetRootOffset() const { return mRootOffset; } + + ///@name Properties of the joints + ///@{ + uint GetJointCount() const { return (uint)mJoints.size(); } + const JointStateVector & GetJoints() const { return mJoints; } + JointStateVector & GetJoints() { return mJoints; } + const JointState & GetJoint(int inJoint) const { return mJoints[inJoint]; } + JointState & GetJoint(int inJoint) { return mJoints[inJoint]; } + ///@} + + ///@name Joint matrices + ///@{ + const Mat44Vector & GetJointMatrices() const { return mJointMatrices; } + Mat44Vector & GetJointMatrices() { return mJointMatrices; } + const Mat44 & GetJointMatrix(int inJoint) const { return mJointMatrices[inJoint]; } + Mat44 & GetJointMatrix(int inJoint) { return mJointMatrices[inJoint]; } + ///@} + + /// Convert the joint states to joint matrices + void CalculateJointMatrices(); + + /// Convert joint matrices to joint states + void CalculateJointStates(); + + /// Outputs the joint matrices in local space (ensure that outMatrices has GetJointCount() elements, assumes that values in GetJoints() is up to date) + void CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const; + +#ifdef JPH_DEBUG_RENDERER + /// Draw settings + struct DrawSettings + { + bool mDrawJoints = true; + bool mDrawJointOrientations = true; + bool mDrawJointNames = false; + }; + + /// Draw current pose + void Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset = RMat44::sIdentity()) const; +#endif // JPH_DEBUG_RENDERER + +private: + RefConst mSkeleton; ///< Skeleton definition + RVec3 mRootOffset { RVec3::sZero() }; ///< Extra offset applied to the root (and therefore also to all of its children) + JointStateVector mJoints; ///< Local joint orientations (local to parent Joint) + Mat44Vector mJointMatrices; ///< Local joint matrices (local to world matrix) +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleGrouper/TriangleGrouper.h b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouper.h new file mode 100644 index 000000000..9d75691f6 --- /dev/null +++ b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouper.h @@ -0,0 +1,27 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N (according to closeness) +class JPH_EXPORT TriangleGrouper : public NonCopyable +{ +public: + /// Virtual destructor + virtual ~TriangleGrouper() = default; + + /// Group a batch of indexed triangles + /// @param inVertices The list of vertices + /// @param inTriangles The list of indexed triangles (indexes into inVertices) + /// @param inGroupSize How big each group should be + /// @param outGroupedTriangleIndices An ordered list of indices (indexing into inTriangles), contains groups of inGroupSize large worth of indices to triangles that are grouped together. If the triangle count is not an exact multiple of inGroupSize the last batch will be smaller. + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) = 0; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp new file mode 100644 index 000000000..7231069f9 --- /dev/null +++ b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.cpp @@ -0,0 +1,95 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void TriangleGrouperClosestCentroid::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) +{ + const uint triangle_count = (uint)inTriangles.size(); + const uint num_batches = (triangle_count + inGroupSize - 1) / inGroupSize; + + Array centroids; + centroids.resize(triangle_count); + + outGroupedTriangleIndices.resize(triangle_count); + + for (uint t = 0; t < triangle_count; ++t) + { + // Store centroid + centroids[t] = inTriangles[t].GetCentroid(inVertices); + + // Initialize sort table + outGroupedTriangleIndices[t] = t; + } + + Array::const_iterator triangles_end = outGroupedTriangleIndices.end(); + + // Sort per batch + for (uint b = 0; b < num_batches - 1; ++b) + { + // Get iterators + Array::iterator batch_begin = outGroupedTriangleIndices.begin() + b * inGroupSize; + Array::iterator batch_end = batch_begin + inGroupSize; + Array::iterator batch_begin_plus_1 = batch_begin + 1; + Array::iterator batch_end_minus_1 = batch_end - 1; + + // Find triangle with centroid with lowest X coordinate + Array::iterator lowest_iter = batch_begin; + float lowest_val = centroids[*lowest_iter].GetX(); + for (Array::iterator other = batch_begin; other != triangles_end; ++other) + { + float val = centroids[*other].GetX(); + if (val < lowest_val) + { + lowest_iter = other; + lowest_val = val; + } + } + + // Make this triangle the first in a new batch + swap(*batch_begin, *lowest_iter); + Vec3 first_centroid = centroids[*batch_begin]; + + // Sort remaining triangles in batch on distance to first triangle + QuickSort(batch_begin_plus_1, batch_end, + [&first_centroid, ¢roids](uint inLHS, uint inRHS) + { + return (centroids[inLHS] - first_centroid).LengthSq() < (centroids[inRHS] - first_centroid).LengthSq(); + }); + + // Loop over remaining triangles + float furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq(); + for (Array::iterator other = batch_end; other != triangles_end; ++other) + { + // Check if this triangle is closer than the furthest triangle in the batch + float dist = (centroids[*other] - first_centroid).LengthSq(); + if (dist < furthest_dist) + { + // Replace furthest triangle + uint other_val = *other; + *other = *batch_end_minus_1; + + // Find first element that is bigger than this one and insert the current item before it + Array::iterator upper = std::upper_bound(batch_begin_plus_1, batch_end, dist, + [&first_centroid, ¢roids](float inLHS, uint inRHS) + { + return inLHS < (centroids[inRHS] - first_centroid).LengthSq(); + }); + std::copy_backward(upper, batch_end_minus_1, batch_end); + *upper = other_val; + + // Calculate new furthest distance + furthest_dist = (centroids[*batch_end_minus_1] - first_centroid).LengthSq(); + } + } + } +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h new file mode 100644 index 000000000..583227416 --- /dev/null +++ b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperClosestCentroid.h @@ -0,0 +1,21 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N. +/// Starts with centroid with lowest X coordinate and finds N closest centroids, this repeats until all groups have been found. +/// Time complexity: O(N^2) +class JPH_EXPORT TriangleGrouperClosestCentroid : public TriangleGrouper +{ +public: + // See: TriangleGrouper::Group + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) override; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp new file mode 100644 index 000000000..a6e5a5644 --- /dev/null +++ b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.cpp @@ -0,0 +1,49 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +void TriangleGrouperMorton::Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) +{ + const uint triangle_count = (uint)inTriangles.size(); + + Array centroids; + centroids.resize(triangle_count); + + outGroupedTriangleIndices.resize(triangle_count); + + for (uint t = 0; t < triangle_count; ++t) + { + // Store centroid + centroids[t] = inTriangles[t].GetCentroid(inVertices); + + // Initialize sort table + outGroupedTriangleIndices[t] = t; + } + + // Get bounding box of all centroids + AABox centroid_bounds; + for (uint t = 0; t < triangle_count; ++t) + centroid_bounds.Encapsulate(centroids[t]); + + // Make sure box is not degenerate + centroid_bounds.EnsureMinimalEdgeLength(1.0e-5f); + + // Calculate morton code for each centroid + Array morton_codes; + morton_codes.resize(triangle_count); + for (uint t = 0; t < triangle_count; ++t) + morton_codes[t] = MortonCode::sGetMortonCode(centroids[t], centroid_bounds); + + // Sort triangles based on morton code + QuickSort(outGroupedTriangleIndices.begin(), outGroupedTriangleIndices.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; }); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.h b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.h new file mode 100644 index 000000000..a35f9af00 --- /dev/null +++ b/WickedEngine/Jolt/TriangleGrouper/TriangleGrouperMorton.h @@ -0,0 +1,20 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// A class that groups triangles in batches of N according to morton code of centroid. +/// Time complexity: O(N log(N)) +class JPH_EXPORT TriangleGrouperMorton : public TriangleGrouper +{ +public: + // See: TriangleGrouper::Group + virtual void Group(const VertexList &inVertices, const IndexedTriangleList &inTriangles, int inGroupSize, Array &outGroupedTriangleIndices) override; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.cpp b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.cpp new file mode 100644 index 000000000..b7852a919 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.cpp @@ -0,0 +1,67 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitter::TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + mVertices(inVertices), + mTriangles(inTriangles) +{ + mSortedTriangleIdx.resize(inTriangles.size()); + mCentroids.resize(inTriangles.size()); + + for (uint t = 0; t < inTriangles.size(); ++t) + { + // Initially triangles start unsorted + mSortedTriangleIdx[t] = t; + + // Calculate centroid + inTriangles[t].GetCentroid(inVertices).StoreFloat3(&mCentroids[t]); + } +} + +bool TriangleSplitter::SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight) +{ + // Divide triangles + uint start = inTriangles.mBegin, end = inTriangles.mEnd; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && mCentroids[mSortedTriangleIdx[start]][inDimension] < inSplit) + ++start; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && mCentroids[mSortedTriangleIdx[end - 1]][inDimension] >= inSplit) + --end; + + if (start < end) + { + // Swap the two elements + swap(mSortedTriangleIdx[start], mSortedTriangleIdx[end - 1]); + ++start; + --end; + } + } + JPH_ASSERT(start == end); + +#ifdef JPH_ENABLE_ASSERTS + // Validate division algorithm + JPH_ASSERT(inTriangles.mBegin <= start); + JPH_ASSERT(start <= inTriangles.mEnd); + for (uint i = inTriangles.mBegin; i < start; ++i) + JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] < inSplit); + for (uint i = start; i < inTriangles.mEnd; ++i) + JPH_ASSERT(mCentroids[mSortedTriangleIdx[i]][inDimension] >= inSplit); +#endif + + outLeft = Range(inTriangles.mBegin, start); + outRight = Range(start, inTriangles.mEnd); + return outLeft.Count() > 0 && outRight.Count() > 0; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.h b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.h new file mode 100644 index 000000000..a66672238 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitter.h @@ -0,0 +1,84 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// A class that splits a triangle list into two parts for building a tree +class JPH_EXPORT TriangleSplitter : public NonCopyable +{ +public: + /// Constructor + TriangleSplitter(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + /// Virtual destructor + virtual ~TriangleSplitter() = default; + + struct Stats + { + const char * mSplitterName = nullptr; + int mLeafSize = 0; + }; + + /// Get stats of splitter + virtual void GetStats(Stats &outStats) const = 0; + + /// Helper struct to indicate triangle range before and after the split + struct Range + { + /// Constructor + Range() = default; + Range(uint inBegin, uint inEnd) : mBegin(inBegin), mEnd(inEnd) { } + + /// Get number of triangles in range + uint Count() const + { + return mEnd - mBegin; + } + + /// Start and end index (end = 1 beyond end) + uint mBegin; + uint mEnd; + }; + + /// Range of triangles to start with + Range GetInitialRange() const + { + return Range(0, (uint)mSortedTriangleIdx.size()); + } + + /// Split triangles into two groups left and right, returns false if no split could be made + /// @param inTriangles The range of triangles (in mSortedTriangleIdx) to process + /// @param outLeft On return this will contain the ranges for the left subpart. mSortedTriangleIdx may have been shuffled. + /// @param outRight On return this will contain the ranges for the right subpart. mSortedTriangleIdx may have been shuffled. + /// @return Returns true when a split was found + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) = 0; + + /// Get the list of vertices + const VertexList & GetVertices() const + { + return mVertices; + } + + /// Get triangle by index + const IndexedTriangle & GetTriangle(uint inIdx) const + { + return mTriangles[mSortedTriangleIdx[inIdx]]; + } + +protected: + /// Helper function to split triangles based on dimension and split value + bool SplitInternal(const Range &inTriangles, uint inDimension, float inSplit, Range &outLeft, Range &outRight); + + const VertexList & mVertices; ///< Vertices of the indexed triangles + const IndexedTriangleList & mTriangles; ///< Unsorted triangles + Array mCentroids; ///< Unsorted centroids of triangles + Array mSortedTriangleIdx; ///< Indices to sort triangles +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp new file mode 100644 index 000000000..989520c45 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.cpp @@ -0,0 +1,112 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + + JPH_NAMESPACE_BEGIN + +TriangleSplitterBinning::TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) : + TriangleSplitter(inVertices, inTriangles), + mMinNumBins(inMinNumBins), + mMaxNumBins(inMaxNumBins), + mNumTrianglesPerBin(inNumTrianglesPerBin) +{ + mBins.resize(mMaxNumBins); +} + +bool TriangleSplitterBinning::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate bounds for this range + AABox centroid_bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + centroid_bounds.Encapsulate(Vec3(mCentroids[mSortedTriangleIdx[t]])); + + float best_cp = FLT_MAX; + uint best_dim = 0xffffffff; + float best_split = 0; + + // Bin in all dimensions + uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins); + for (uint dim = 0; dim < 3; ++dim) + { + float bounds_min = centroid_bounds.mMin[dim]; + float bounds_size = centroid_bounds.mMax[dim] - bounds_min; + + // Skip axis if too small + if (bounds_size < 1.0e-5f) + continue; + + // Initialize bins + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = mBins[b]; + bin.mBounds.SetEmpty(); + bin.mMinCentroid = bounds_min + bounds_size * (b + 1) / num_bins; + bin.mNumTriangles = 0; + } + + // Bin all triangles + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + { + float centroid_pos = mCentroids[mSortedTriangleIdx[t]][dim]; + + // Select bin + uint bin_no = min(uint((centroid_pos - bounds_min) / bounds_size * num_bins), num_bins - 1); + Bin &bin = mBins[bin_no]; + + // Accumulate triangle in bin + bin.mBounds.Encapsulate(mVertices, GetTriangle(t)); + bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos); + bin.mNumTriangles++; + } + + // Calculate totals left to right + AABox prev_bounds; + int prev_triangles = 0; + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = mBins[b]; + bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin + bin.mNumTrianglesAccumulatedLeft = prev_triangles; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + } + + // Calculate totals right to left + prev_bounds.SetEmpty(); + prev_triangles = 0; + for (int b = num_bins - 1; b >= 0; --b) + { + Bin &bin = mBins[b]; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + bin.mBoundsAccumulatedRight = prev_bounds; + bin.mNumTrianglesAccumulatedRight = prev_triangles; + } + + // Get best splitting plane + for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side + { + // Calculate surface area heuristic and see if it is better than the current best + const Bin &bin = mBins[b]; + float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight; + if (cp < best_cp) + { + best_cp = cp; + best_dim = dim; + best_split = bin.mMinCentroid; + } + } + } + + // No split found? + if (best_dim == 0xffffffff) + return false; + + return SplitInternal(inTriangles, best_dim, best_split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.h b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.h new file mode 100644 index 000000000..2cb35c9cc --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterBinning.h @@ -0,0 +1,52 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Binning splitter approach taken from: Realtime Ray Tracing on GPU with BVH-based Packet Traversal by Johannes Gunther et al. +class JPH_EXPORT TriangleSplitterBinning : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterBinning(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterBinning"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + // Configuration + const uint mMinNumBins; + const uint mMaxNumBins; + const uint mNumTrianglesPerBin; + + struct Bin + { + // Properties of this bin + AABox mBounds; + float mMinCentroid; + uint mNumTriangles; + + // Accumulated data from left most / right most bin to current (including this bin) + AABox mBoundsAccumulatedLeft; + AABox mBoundsAccumulatedRight; + uint mNumTrianglesAccumulatedLeft; + uint mNumTrianglesAccumulatedRight; + }; + + // Scratch area to store the bins + Array mBins; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp new file mode 100644 index 000000000..2e8ae7371 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.cpp @@ -0,0 +1,170 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterFixedLeafSize::TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins, uint inMaxNumBins, uint inNumTrianglesPerBin) : + TriangleSplitter(inVertices, inTriangles), + mLeafSize(inLeafSize), + mMinNumBins(inMinNumBins), + mMaxNumBins(inMaxNumBins), + mNumTrianglesPerBin(inNumTrianglesPerBin) +{ + // Group the triangles + TriangleGrouperClosestCentroid grouper; + grouper.Group(inVertices, inTriangles, mLeafSize, mSortedTriangleIdx); + + // Pad triangles so that we have a multiple of mLeafSize + const uint num_triangles = (uint)inTriangles.size(); + const uint num_groups = (num_triangles + mLeafSize - 1) / mLeafSize; + const uint last_triangle_idx = mSortedTriangleIdx.back(); + for (uint t = num_triangles, t_end = num_groups * mLeafSize; t < t_end; ++t) + mSortedTriangleIdx.push_back(last_triangle_idx); +} + +Vec3 TriangleSplitterFixedLeafSize::GetCentroidForGroup(uint inFirstTriangleInGroup) +{ + JPH_ASSERT(inFirstTriangleInGroup % mLeafSize == 0); + AABox box; + for (uint g = 0; g < mLeafSize; ++g) + box.Encapsulate(mVertices, GetTriangle(inFirstTriangleInGroup + g)); + return box.GetCenter(); +} + +bool TriangleSplitterFixedLeafSize::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Cannot split anything smaller than leaf size + JPH_ASSERT(inTriangles.Count() > mLeafSize); + JPH_ASSERT(inTriangles.Count() % mLeafSize == 0); + + // Calculate bounds for this range + AABox centroid_bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize) + centroid_bounds.Encapsulate(GetCentroidForGroup(t)); + + float best_cp = FLT_MAX; + uint best_dim = 0xffffffff; + float best_split = 0; + + // Bin in all dimensions + uint num_bins = Clamp(inTriangles.Count() / mNumTrianglesPerBin, mMinNumBins, mMaxNumBins); + Array bins(num_bins); + for (uint dim = 0; dim < 3; ++dim) + { + float bounds_min = centroid_bounds.mMin[dim]; + float bounds_size = centroid_bounds.mMax[dim] - bounds_min; + + // Skip axis if too small + if (bounds_size < 1.0e-5f) + continue; + + // Initialize bins + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins[b]; + bin.mBounds.SetEmpty(); + bin.mMinCentroid = bounds_min + bounds_size * (b + 1) / num_bins; + bin.mNumTriangles = 0; + } + + // Bin all triangles + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; t += mLeafSize) + { + // Calculate average centroid for group + float centroid_pos = GetCentroidForGroup(t)[dim]; + + // Select bin + uint bin_no = min(uint((centroid_pos - bounds_min) / bounds_size * num_bins), num_bins - 1); + Bin &bin = bins[bin_no]; + + // Put all triangles of group in same bin + for (uint g = 0; g < mLeafSize; ++g) + bin.mBounds.Encapsulate(mVertices, GetTriangle(t + g)); + bin.mMinCentroid = min(bin.mMinCentroid, centroid_pos); + bin.mNumTriangles += mLeafSize; + } + + // Calculate totals left to right + AABox prev_bounds; + int prev_triangles = 0; + for (uint b = 0; b < num_bins; ++b) + { + Bin &bin = bins[b]; + bin.mBoundsAccumulatedLeft = prev_bounds; // Don't include this node as we'll take a split on the left side of the bin + bin.mNumTrianglesAccumulatedLeft = prev_triangles; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + } + + // Calculate totals right to left + prev_bounds.SetEmpty(); + prev_triangles = 0; + for (int b = num_bins - 1; b >= 0; --b) + { + Bin &bin = bins[b]; + prev_bounds.Encapsulate(bin.mBounds); + prev_triangles += bin.mNumTriangles; + bin.mBoundsAccumulatedRight = prev_bounds; + bin.mNumTrianglesAccumulatedRight = prev_triangles; + } + + // Get best splitting plane + for (uint b = 1; b < num_bins; ++b) // Start at 1 since selecting bin 0 would result in everything ending up on the right side + { + // Calculate surface area heuristic and see if it is better than the current best + const Bin &bin = bins[b]; + float cp = bin.mBoundsAccumulatedLeft.GetSurfaceArea() * bin.mNumTrianglesAccumulatedLeft + bin.mBoundsAccumulatedRight.GetSurfaceArea() * bin.mNumTrianglesAccumulatedRight; + if (cp < best_cp) + { + best_cp = cp; + best_dim = dim; + best_split = bin.mMinCentroid; + } + } + } + + // No split found? + if (best_dim == 0xffffffff) + return false; + + // Divide triangles + uint start = inTriangles.mBegin, end = inTriangles.mEnd; + while (start < end) + { + // Search for first element that is on the right hand side of the split plane + while (start < end && GetCentroidForGroup(start)[best_dim] < best_split) + start += mLeafSize; + + // Search for the first element that is on the left hand side of the split plane + while (start < end && GetCentroidForGroup(end - mLeafSize)[best_dim] >= best_split) + end -= mLeafSize; + + if (start < end) + { + // Swap the two elements + for (uint g = 0; g < mLeafSize; ++g) + swap(mSortedTriangleIdx[start + g], mSortedTriangleIdx[end - mLeafSize + g]); + start += mLeafSize; + end -= mLeafSize; + } + } + JPH_ASSERT(start == end); + + // No suitable split found, doing random split in half + if (start == inTriangles.mBegin || start == inTriangles.mEnd) + start = inTriangles.mBegin + (inTriangles.Count() / mLeafSize + 1) / 2 * mLeafSize; + + outLeft = Range(inTriangles.mBegin, start); + outRight = Range(start, inTriangles.mEnd); + JPH_ASSERT(outLeft.mEnd > outLeft.mBegin && outRight.mEnd > outRight.mBegin); + JPH_ASSERT(outLeft.Count() % mLeafSize == 0 && outRight.Count() % mLeafSize == 0); + return true; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h new file mode 100644 index 000000000..029121dae --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterFixedLeafSize.h @@ -0,0 +1,55 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +JPH_NAMESPACE_BEGIN + +/// Same as TriangleSplitterBinning, but ensuring that leaves have a fixed amount of triangles +/// The resulting tree should be suitable for processing on GPU where we want all threads to process an equal amount of triangles +class JPH_EXPORT TriangleSplitterFixedLeafSize : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterFixedLeafSize(const VertexList &inVertices, const IndexedTriangleList &inTriangles, uint inLeafSize, uint inMinNumBins = 8, uint inMaxNumBins = 128, uint inNumTrianglesPerBin = 6); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterFixedLeafSize"; + outStats.mLeafSize = mLeafSize; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + /// Get centroid for group + Vec3 GetCentroidForGroup(uint inFirstTriangleInGroup); + + // Configuration + const uint mLeafSize; + const uint mMinNumBins; + const uint mMaxNumBins; + const uint mNumTrianglesPerBin; + + struct Bin + { + // Properties of this bin + AABox mBounds; + float mMinCentroid; + uint mNumTriangles; + + // Accumulated data from left most / right most bin to current (including this bin) + AABox mBoundsAccumulatedLeft; + AABox mBoundsAccumulatedRight; + uint mNumTrianglesAccumulatedLeft; + uint mNumTrianglesAccumulatedRight; + }; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp new file mode 100644 index 000000000..f8115ab89 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.cpp @@ -0,0 +1,31 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterLongestAxis::TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ +} + +bool TriangleSplitterLongestAxis::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate bounding box for triangles + AABox bounds; + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + bounds.Encapsulate(mVertices, GetTriangle(t)); + + // Calculate split plane + uint dimension = bounds.GetExtent().GetHighestComponentIndex(); + float split = bounds.GetCenter()[dimension]; + + return SplitInternal(inTriangles, dimension, split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h new file mode 100644 index 000000000..daf0d4370 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterLongestAxis.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using center of bounding box with longest axis +class JPH_EXPORT TriangleSplitterLongestAxis : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterLongestAxis(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterLongestAxis"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.cpp b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.cpp new file mode 100644 index 000000000..e884246fe --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.cpp @@ -0,0 +1,40 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterMean::TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ +} + +bool TriangleSplitterMean::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + // Calculate mean value for these triangles + Vec3 mean = Vec3::sZero(); + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + mean += Vec3(mCentroids[mSortedTriangleIdx[t]]); + mean *= 1.0f / inTriangles.Count(); + + // Calculate deviation + Vec3 deviation = Vec3::sZero(); + for (uint t = inTriangles.mBegin; t < inTriangles.mEnd; ++t) + { + Vec3 delta = Vec3(mCentroids[mSortedTriangleIdx[t]]) - mean; + deviation += delta * delta; + } + deviation *= 1.0f / inTriangles.Count(); + + // Calculate split plane + uint dimension = deviation.GetHighestComponentIndex(); + float split = mean[dimension]; + + return SplitInternal(inTriangles, dimension, split, outLeft, outRight); +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.h b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.h new file mode 100644 index 000000000..737d76e1c --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMean.h @@ -0,0 +1,28 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using mean of axis with biggest centroid deviation +class JPH_EXPORT TriangleSplitterMean : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterMean(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterMean"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp new file mode 100644 index 000000000..35b0f4212 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.cpp @@ -0,0 +1,63 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#include + +#include +#include +#include + +JPH_NAMESPACE_BEGIN + +TriangleSplitterMorton::TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles) : + TriangleSplitter(inVertices, inTriangles) +{ + // Calculate bounds of centroids + AABox bounds; + for (uint t = 0; t < inTriangles.size(); ++t) + bounds.Encapsulate(Vec3(mCentroids[t])); + + // Make sure box is not degenerate + bounds.EnsureMinimalEdgeLength(1.0e-5f); + + // Calculate morton codes + mMortonCodes.resize(inTriangles.size()); + for (uint t = 0; t < inTriangles.size(); ++t) + mMortonCodes[t] = MortonCode::sGetMortonCode(Vec3(mCentroids[t]), bounds); + + // Sort triangles on morton code + const Array &morton_codes = mMortonCodes; + QuickSort(mSortedTriangleIdx.begin(), mSortedTriangleIdx.end(), [&morton_codes](uint inLHS, uint inRHS) { return morton_codes[inLHS] < morton_codes[inRHS]; }); +} + +bool TriangleSplitterMorton::Split(const Range &inTriangles, Range &outLeft, Range &outRight) +{ + uint32 first_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mBegin]]; + uint32 last_code = mMortonCodes[mSortedTriangleIdx[inTriangles.mEnd - 1]]; + + uint common_prefix = CountLeadingZeros(first_code ^ last_code); + + // Use binary search to find where the next bit differs + uint split = inTriangles.mBegin; // Initial guess + uint step = inTriangles.Count(); + do + { + step = (step + 1) >> 1; // Exponential decrease + uint new_split = split + step; // Proposed new position + if (new_split < inTriangles.mEnd) + { + uint32 split_code = mMortonCodes[mSortedTriangleIdx[new_split]]; + uint split_prefix = CountLeadingZeros(first_code ^ split_code); + if (split_prefix > common_prefix) + split = new_split; // Accept proposal + } + } + while (step > 1); + + outLeft = Range(inTriangles.mBegin, split + 1); + outRight = Range(split + 1, inTriangles.mEnd); + return outLeft.Count() > 0 && outRight.Count() > 0; +} + +JPH_NAMESPACE_END diff --git a/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.h b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.h new file mode 100644 index 000000000..2f48c0ea9 --- /dev/null +++ b/WickedEngine/Jolt/TriangleSplitter/TriangleSplitterMorton.h @@ -0,0 +1,32 @@ +// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics) +// SPDX-FileCopyrightText: 2021 Jorrit Rouwe +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +JPH_NAMESPACE_BEGIN + +/// Splitter using Morton codes, see: http://devblogs.nvidia.com/parallelforall/thinking-parallel-part-iii-tree-construction-gpu/ +class JPH_EXPORT TriangleSplitterMorton : public TriangleSplitter +{ +public: + /// Constructor + TriangleSplitterMorton(const VertexList &inVertices, const IndexedTriangleList &inTriangles); + + // See TriangleSplitter::GetStats + virtual void GetStats(Stats &outStats) const override + { + outStats.mSplitterName = "TriangleSplitterMorton"; + } + + // See TriangleSplitter::Split + virtual bool Split(const Range &inTriangles, Range &outLeft, Range &outRight) override; + +private: + // Precalculated Morton codes + Array mMortonCodes; +}; + +JPH_NAMESPACE_END diff --git a/WickedEngine/WickedEngine_SOURCE.vcxitems b/WickedEngine/WickedEngine_SOURCE.vcxitems index e4dc4b741..f76a12417 100644 --- a/WickedEngine/WickedEngine_SOURCE.vcxitems +++ b/WickedEngine/WickedEngine_SOURCE.vcxitems @@ -15,220 +15,296 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -380,146 +456,143 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -528,6 +601,7 @@ + @@ -697,7 +771,6 @@ - @@ -735,6 +808,21 @@ + + + + + + + + + + + + + + + diff --git a/WickedEngine/WickedEngine_SOURCE.vcxitems.filters b/WickedEngine/WickedEngine_SOURCE.vcxitems.filters index f2fd4d256..bd7e3d385 100644 --- a/WickedEngine/WickedEngine_SOURCE.vcxitems.filters +++ b/WickedEngine/WickedEngine_SOURCE.vcxitems.filters @@ -13,9 +13,6 @@ {3c39405f-ec30-4b72-9c32-68aefb4fb7a5} - - {d6f94528-ce59-4fa3-a0b9-d5c04994ac71} - {3d5f1098-95e6-4baa-9e39-864be28a5b64} @@ -55,6 +52,9 @@ {7cb35feb-169d-4eb9-b90c-61418a8e99df} + + {ea901057-ebb6-4eed-ac17-c98111e6c4bd} + @@ -171,645 +171,6 @@ ENGINE\Scripting\LuaBindings - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - ENGINE\Graphics @@ -1146,6 +507,873 @@ UTILITY + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + @@ -1280,426 +1508,6 @@ ENGINE\Scripting\LuaBindings - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - - - BULLET - ENGINE\Graphics @@ -1772,9 +1580,6 @@ UTILITY - - ENGINE\Physics - ENGINE\Graphics @@ -1919,9 +1724,420 @@ ENGINE\Scripting\LuaBindings - - - + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + ENGINE\Physics + @@ -1942,5 +2158,53 @@ UTILITY + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + JOLT + + + + \ No newline at end of file diff --git a/WickedEngine/WickedEngine_Windows.vcxproj b/WickedEngine/WickedEngine_Windows.vcxproj index b57b7b83c..af3d3c4db 100644 --- a/WickedEngine/WickedEngine_Windows.vcxproj +++ b/WickedEngine/WickedEngine_Windows.vcxproj @@ -62,7 +62,7 @@ Level3 Disabled - _DEBUG;_LIB;%(PreprocessorDefinitions) + _DEBUG;_LIB;%(PreprocessorDefinitions);JPH_DEBUG_RENDERER BULLET;%(AdditionalIncludeDirectories) true false @@ -102,7 +102,7 @@ MaxSpeed true true - NDEBUG;_LIB;%(PreprocessorDefinitions) + NDEBUG;_LIB;%(PreprocessorDefinitions);JPH_DEBUG_RENDERER BULLET;%(AdditionalIncludeDirectories) MultiThreaded true diff --git a/WickedEngine/wiPhysics_Bullet.cpp b/WickedEngine/wiPhysics_Bullet.cpp deleted file mode 100644 index d7c36d515..000000000 --- a/WickedEngine/wiPhysics_Bullet.cpp +++ /dev/null @@ -1,1873 +0,0 @@ -#include "wiPhysics.h" -#include "wiScene.h" -#include "wiProfiler.h" -#include "wiBacklog.h" -#include "wiJobSystem.h" -#include "wiRenderer.h" -#include "wiTimer.h" - -#include "btBulletDynamicsCommon.h" -#include "BulletSoftBody/btSoftBodyHelpers.h" -#include "BulletSoftBody/btDefaultSoftBodySolver.h" -#include "BulletSoftBody/btSoftRigidDynamicsWorld.h" -#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" - -#include -#include - -using namespace wi::ecs; -using namespace wi::scene; - -namespace wi::physics -{ - namespace bullet - { - bool ENABLED = true; - bool SIMULATION_ENABLED = true; - bool DEBUGDRAW_ENABLED = false; - int ACCURACY = 8; - int softbodyIterationCount = 5; - float TIMESTEP = 1.0f / 120.0f; - std::mutex physicsLock; - - class DebugDraw final : public btIDebugDraw - { - void drawLine(const btVector3& from, const btVector3& to, const btVector3& color) override - { - wi::renderer::RenderableLine line; - line.start = XMFLOAT3(from.x(), from.y(), from.z()); - line.end = XMFLOAT3(to.x(), to.y(), to.z()); - line.color_start = line.color_end = XMFLOAT4(color.x(), color.y(), color.z(), 1.0f); - wi::renderer::DrawLine(line); - } - void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color) override - { - } - void reportErrorWarning(const char* warningString) override - { - wi::backlog::post(warningString); - } - void draw3dText(const btVector3& location, const char* textString) override - { - wi::renderer::DebugTextParams params; - params.position.x = location.x(); - params.position.y = location.y(); - params.position.z = location.z(); - params.scaling = 0.6f; - params.flags |= wi::renderer::DebugTextParams::CAMERA_FACING; - params.flags |= wi::renderer::DebugTextParams::CAMERA_SCALING; - wi::renderer::DrawDebugText(textString, params); - } - void setDebugMode(int debugMode) override - { - } - int getDebugMode() const override - { - int retval = 0; - retval |= DBG_DrawWireframe; - retval |= DBG_DrawText; - return retval; - } - }; - DebugDraw debugDraw; - - struct PhysicsScene - { - btSoftBodyRigidBodyCollisionConfiguration collisionConfiguration; - btDbvtBroadphase overlappingPairCache; - btSequentialImpulseConstraintSolver solver; - btCollisionDispatcher dispatcher = btCollisionDispatcher(&collisionConfiguration); - btSoftRigidDynamicsWorld dynamicsWorld = btSoftRigidDynamicsWorld(&dispatcher, &overlappingPairCache, &solver, &collisionConfiguration); - }; - PhysicsScene& GetPhysicsScene(Scene& scene) - { - if (scene.physics_scene == nullptr) - { - auto physics_scene = std::make_shared(); - - btContactSolverInfo& solverInfo = physics_scene->dynamicsWorld.getSolverInfo(); - solverInfo.m_solverMode |= SOLVER_RANDMIZE_ORDER; - solverInfo.m_splitImpulse = true; - - btDispatcherInfo& dispatcherInfo = physics_scene->dynamicsWorld.getDispatchInfo(); - dispatcherInfo.m_enableSatConvex = true; - - btSoftBodyWorldInfo& softWorldInfo = physics_scene->dynamicsWorld.getWorldInfo(); - softWorldInfo.air_density = btScalar(1.2f); - softWorldInfo.water_density = 0; - softWorldInfo.water_offset = 0; - softWorldInfo.water_normal = btVector3(0, 0, 0); - softWorldInfo.m_sparsesdf.Initialize(); - - scene.physics_scene = physics_scene; - } - return *(PhysicsScene*)scene.physics_scene.get(); - } - - struct RigidBody - { - std::shared_ptr physics_scene; - std::unique_ptr shape; - std::unique_ptr rigidBody; - btDefaultMotionState motionState; - btTriangleIndexVertexArray triangles; - Entity entity = INVALID_ENTITY; - - // for trace hit reporting: - wi::ecs::Entity humanoid_ragdoll_entity = wi::ecs::INVALID_ENTITY; - wi::scene::HumanoidComponent::HumanoidBone humanoid_bone = wi::scene::HumanoidComponent::HumanoidBone::Count; - - // These are used to remap default shape orientations into ragdoll and back: - btTransform additionalTransform; - btTransform additionalTransformInverse; - btTransform restBasis; - btTransform restBasisInverse; - - RigidBody() - { - additionalTransform.setIdentity(); - additionalTransformInverse.setIdentity(); - restBasis.setIdentity(); - restBasisInverse.setIdentity(); - } - ~RigidBody() - { - if (physics_scene == nullptr) - return; - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)physics_scene.get())->dynamicsWorld; - dynamicsWorld.removeRigidBody(rigidBody.get()); - } - }; - struct SoftBody - { - std::shared_ptr physics_scene; - std::unique_ptr softBody; - Entity entity = INVALID_ENTITY; - ~SoftBody() - { - if (physics_scene == nullptr) - return; - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)physics_scene.get())->dynamicsWorld; - dynamicsWorld.removeSoftBody(softBody.get()); - } - }; - - RigidBody& GetRigidBody(wi::scene::RigidBodyPhysicsComponent& physicscomponent) - { - if (physicscomponent.physicsobject == nullptr) - { - physicscomponent.physicsobject = std::make_shared(); - } - return *(RigidBody*)physicscomponent.physicsobject.get(); - } - SoftBody& GetSoftBody(wi::scene::SoftBodyPhysicsComponent& physicscomponent) - { - if (physicscomponent.physicsobject == nullptr) - { - physicscomponent.physicsobject = std::make_shared(); - } - return *(SoftBody*)physicscomponent.physicsobject.get(); - } - } - using namespace bullet; - - void Initialize() - { - wi::Timer timer; - - wi::backlog::post("wi::physics Initialized [Bullet] (" + std::to_string((int)std::round(timer.elapsed())) + " ms)"); - } - - bool IsEnabled() { return ENABLED; } - void SetEnabled(bool value) { ENABLED = value; } - - bool IsSimulationEnabled() { return ENABLED && SIMULATION_ENABLED; } - void SetSimulationEnabled(bool value) { SIMULATION_ENABLED = value; } - - bool IsDebugDrawEnabled() { return DEBUGDRAW_ENABLED; } - void SetDebugDrawEnabled(bool value) { DEBUGDRAW_ENABLED = value; } - - int GetAccuracy() { return ACCURACY; } - void SetAccuracy(int value) { ACCURACY = value; } - - float GetFrameRate() { return 1.0f / TIMESTEP; } - void SetFrameRate(float value) { TIMESTEP = 1.0f / value; } - - void AddRigidBody( - wi::scene::Scene& scene, - Entity entity, - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const wi::scene::TransformComponent& transform, - const wi::scene::MeshComponent* mesh - ) - { - RigidBody& physicsobject = GetRigidBody(physicscomponent); - physicsobject.entity = entity; - - switch (physicscomponent.shape) - { - case RigidBodyPhysicsComponent::CollisionShape::BOX: - physicsobject.shape = std::make_unique(btVector3(physicscomponent.box.halfextents.x, physicscomponent.box.halfextents.y, physicscomponent.box.halfextents.z)); - break; - case RigidBodyPhysicsComponent::CollisionShape::SPHERE: - physicsobject.shape = std::make_unique(btScalar(physicscomponent.sphere.radius)); - break; - case RigidBodyPhysicsComponent::CollisionShape::CAPSULE: - physicsobject.shape = std::make_unique(btScalar(physicscomponent.capsule.radius), btScalar(physicscomponent.capsule.height)); - break; - case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: - physicsobject.shape = std::make_unique(btVector3(physicscomponent.box.halfextents.x, physicscomponent.box.halfextents.y, physicscomponent.box.halfextents.z)); - break; - - case RigidBodyPhysicsComponent::CollisionShape::CONVEX_HULL: - if(mesh != nullptr) - { - physicsobject.shape = std::make_unique(); - btConvexHullShape* convexHull = (btConvexHullShape*)physicsobject.shape.get(); - for (auto& pos : mesh->vertex_positions) - { - convexHull->addPoint(btVector3(pos.x, pos.y, pos.z)); - } - } - else - { - wi::backlog::post("Convex Hull physics requested, but no MeshComponent provided!"); - assert(0); - } - break; - - case RigidBodyPhysicsComponent::CollisionShape::TRIANGLE_MESH: - if(mesh != nullptr) - { - int totalTriangles = 0; - int* indices = nullptr; - uint32_t first_subset = 0; - uint32_t last_subset = 0; - mesh->GetLODSubsetRange(physicscomponent.mesh_lod, first_subset, last_subset); - for (uint32_t subsetIndex = first_subset; subsetIndex < last_subset; ++subsetIndex) - { - const MeshComponent::MeshSubset& subset = mesh->subsets[subsetIndex]; - if (indices == nullptr) - { - indices = (int*)(mesh->indices.data() + subset.indexOffset); - } - totalTriangles += int(subset.indexCount / 3); - } - - physicsobject.triangles = btTriangleIndexVertexArray( - totalTriangles, - indices, - 3 * int(sizeof(int)), - int(mesh->vertex_positions.size()), - (btScalar*)mesh->vertex_positions.data(), - int(sizeof(XMFLOAT3)) - ); - - bool useQuantizedAabbCompression = true; - physicsobject.shape = std::make_unique(&physicsobject.triangles, useQuantizedAabbCompression); - } - else - { - wi::backlog::post("Triangle Mesh physics requested, but no MeshComponent provided!"); - assert(0); - } - break; - } - - if (physicsobject.shape == nullptr) - { - physicscomponent.physicsobject = nullptr; - return; - } - else - { - // Use default margin for now - //shape->setMargin(btScalar(0.01)); - - btVector3 S(transform.scale_local.x, transform.scale_local.y, transform.scale_local.z); - physicsobject.shape->setLocalScaling(S); - - btScalar mass = physicscomponent.mass; - - bool isDynamic = (mass != 0.f && !physicscomponent.IsKinematic()); - - if (physicscomponent.shape == RigidBodyPhysicsComponent::CollisionShape::TRIANGLE_MESH) - { - isDynamic = false; - } - - btVector3 localInertia(0, 0, 0); - if (isDynamic) - { - physicsobject.shape->calculateLocalInertia(mass, localInertia); - } - else - { - mass = 0; - } - - XMVECTOR SCA = {}; - XMVECTOR ROT = {}; - XMVECTOR TRA = {}; - XMMatrixDecompose(&SCA, &ROT, &TRA, XMLoadFloat4x4(&transform.world)); - XMFLOAT4 rot = {}; - XMFLOAT3 tra = {}; - XMStoreFloat4(&rot, ROT); - XMStoreFloat3(&tra, TRA); - - tra.x += physicscomponent.local_offset.x; - tra.y += physicscomponent.local_offset.y; - tra.z += physicscomponent.local_offset.z; - - physicsobject.additionalTransform.setOrigin(btVector3(physicscomponent.local_offset.x, physicscomponent.local_offset.y, physicscomponent.local_offset.z)); - physicsobject.additionalTransformInverse = physicsobject.additionalTransform.inverse(); - - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btTransform shapeTransform; - shapeTransform.setIdentity(); - shapeTransform.setOrigin(btVector3(tra.x, tra.y, tra.z)); - shapeTransform.setRotation(btQuaternion(rot.x, rot.y, rot.z, rot.w)); - physicsobject.motionState = btDefaultMotionState(shapeTransform); - - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, &physicsobject.motionState, physicsobject.shape.get(), localInertia); - //rbInfo.m_friction = physicscomponent.friction; - //rbInfo.m_restitution = physicscomponent.restitution; - //rbInfo.m_linearDamping = physicscomponent.damping; - //rbInfo.m_angularDamping = physicscomponent.damping; - - physicsobject.rigidBody = std::make_unique(rbInfo); - physicsobject.rigidBody->setUserPointer(&physicsobject); - physicsobject.rigidBody->setWorldTransform(shapeTransform); // immediate transform on first frame - - if (physicscomponent.IsKinematic()) - { - physicsobject.rigidBody->setCollisionFlags(physicsobject.rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); - } - if (physicscomponent.IsDisableDeactivation()) - { - physicsobject.rigidBody->setActivationState(DISABLE_DEACTIVATION); - } - - physicsobject.physics_scene = scene.physics_scene; - GetPhysicsScene(scene).dynamicsWorld.addRigidBody(physicsobject.rigidBody.get()); - - if (isDynamic) - { - // We must detach dynamic objects, because their physics object is created in world space - // and attachment would apply double transformation to the transform - scene.Component_Detach(entity); - } - } - } - void AddSoftBody( - wi::scene::Scene& scene, - Entity entity, - wi::scene::SoftBodyPhysicsComponent& physicscomponent, - const wi::scene::MeshComponent& mesh - ) - { - SoftBody& physicsobject = GetSoftBody(physicscomponent); - physicsobject.entity = entity; - physicscomponent.CreateFromMesh(mesh); - - XMMATRIX worldMatrix = XMLoadFloat4x4(&physicscomponent.worldMatrix); - - const int vCount = (int)physicscomponent.physicsToGraphicsVertexMapping.size(); - wi::vector btVerts(vCount * 3); - for (int i = 0; i < vCount; ++i) - { - uint32_t graphicsInd = physicscomponent.physicsToGraphicsVertexMapping[i]; - - XMFLOAT3 position = mesh.vertex_positions[graphicsInd]; - XMVECTOR P = XMLoadFloat3(&position); - P = XMVector3Transform(P, worldMatrix); - XMStoreFloat3(&position, P); - - btVerts[i * 3 + 0] = btScalar(position.x); - btVerts[i * 3 + 1] = btScalar(position.y); - btVerts[i * 3 + 2] = btScalar(position.z); - } - - wi::vector btInd; - btInd.reserve(mesh.indices.size()); - uint32_t first_subset = 0; - uint32_t last_subset = 0; - mesh.GetLODSubsetRange(0, first_subset, last_subset); - for (uint32_t subsetIndex = first_subset; subsetIndex < last_subset; ++subsetIndex) - { - const MeshComponent::MeshSubset& subset = mesh.subsets[subsetIndex]; - const uint32_t* indices = mesh.indices.data() + subset.indexOffset; - for (uint32_t i = 0; i < subset.indexCount; ++i) - { - btInd.push_back((int)physicscomponent.graphicsToPhysicsVertexMapping[indices[i]]); - } - } - - //// This function uses new to allocate btSoftbody internally: - //btSoftBody* softbody = btSoftBodyHelpers::CreateFromTriMesh( - // GetPhysicsScene(scene).dynamicsWorld.getWorldInfo(), - // btVerts.data(), - // btInd.data(), - // int(btInd.size() / 3), - // false - //); - - // Modified version of btSoftBodyHelpers::CreateFromTriMesh: - // This version does not allocate btSoftbody with new - btSoftBody* softbody = nullptr; - { - btSoftBodyWorldInfo& worldInfo = GetPhysicsScene(scene).dynamicsWorld.getWorldInfo(); - const btScalar* vertices = btVerts.data(); - const int* triangles = btInd.data(); - int ntriangles = int(btInd.size() / 3); - bool randomizeConstraints = false; - - int maxidx = 0; - int i, j, ni; - - for (i = 0, ni = ntriangles * 3; i < ni; ++i) - { - maxidx = btMax(triangles[i], maxidx); - } - ++maxidx; - btAlignedObjectArray chks; - btAlignedObjectArray vtx; - chks.resize(maxidx * maxidx, false); - vtx.resize(maxidx); - for (i = 0, j = 0, ni = maxidx * 3; i < ni; ++j, i += 3) - { - vtx[j] = btVector3(vertices[i], vertices[i + 1], vertices[i + 2]); - } - //btSoftBody* psb = new btSoftBody(&worldInfo, vtx.size(), &vtx[0], 0); - physicsobject.softBody = std::make_unique(&worldInfo, vtx.size(), &vtx[0], nullptr); - softbody = physicsobject.softBody.get(); - btSoftBody* psb = softbody; - for (i = 0, ni = ntriangles * 3; i < ni; i += 3) - { - const int idx[] = { triangles[i],triangles[i + 1],triangles[i + 2] }; -#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_)) - for (int j = 2, k = 0; k < 3; j = k++) - { - if (!chks[IDX(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]); - } - - if (randomizeConstraints) - { - psb->randomizeConstraints(); - } - } - - if (softbody) - { - softbody->setUserPointer(&physicsobject); - - //btSoftBody::Material* pm = softbody->appendMaterial(); - btSoftBody::Material* pm = softbody->m_materials[0]; - pm->m_kLST = btScalar(0.9f); - pm->m_kVST = btScalar(0.9f); - pm->m_kAST = btScalar(0.9f); - pm->m_flags = 0; - softbody->generateBendingConstraints(2, pm); - softbody->randomizeConstraints(); - - softbody->m_cfg.piterations = softbodyIterationCount; - softbody->m_cfg.aeromodel = btSoftBody::eAeroModel::F_TwoSidedLiftDrag; - - softbody->m_cfg.kAHR = btScalar(.69); //0.69 Anchor hardness [0,1] - softbody->m_cfg.kCHR = btScalar(1.0); //1 Rigid contact hardness [0,1] - softbody->m_cfg.kDF = btScalar(0.2); //0.2 Dynamic friction coefficient [0,1] - softbody->m_cfg.kDG = btScalar(0.01); //0 Drag coefficient [0,+inf] - softbody->m_cfg.kDP = btScalar(0.0); //0 Damping coefficient [0,1] - softbody->m_cfg.kKHR = btScalar(0.1); //0.1 Kinetic contact hardness [0,1] - softbody->m_cfg.kLF = btScalar(0.1); //0 Lift coefficient [0,+inf] - softbody->m_cfg.kMT = btScalar(0.0); //0 Pose matching coefficient [0,1] - softbody->m_cfg.kPR = btScalar(0.0); //0 Pressure coefficient [-1,1] - softbody->m_cfg.kSHR = btScalar(1.0); //1 Soft contacts hardness [0,1] - softbody->m_cfg.kVC = btScalar(0.0); //0 Volume conseration coefficient [0,+inf] - softbody->m_cfg.kVCF = btScalar(1.0); //1 Velocities correction factor (Baumgarte) - - softbody->m_cfg.kSKHR_CL = btScalar(1.0); //1 Soft vs. kinetic hardness [0,1] - softbody->m_cfg.kSK_SPLT_CL = btScalar(0.5); //0.5 Soft vs. rigid impulse split [0,1] - softbody->m_cfg.kSRHR_CL = btScalar(0.1); //0.1 Soft vs. rigid hardness [0,1] - softbody->m_cfg.kSR_SPLT_CL = btScalar(0.5); //0.5 Soft vs. rigid impulse split [0,1] - softbody->m_cfg.kSSHR_CL = btScalar(0.5); //0.5 Soft vs. soft hardness [0,1] - softbody->m_cfg.kSS_SPLT_CL = btScalar(0.5); //0.5 Soft vs. rigid impulse split [0,1] - - for (size_t i = 0; i < physicscomponent.physicsToGraphicsVertexMapping.size(); ++i) - { - float weight = physicscomponent.weights[i]; - softbody->setMass((int)i, weight); - } - softbody->setTotalMass(physicscomponent.mass); // this must be AFTER softbody->setMass(), so that weights will be averaged - - if (physicscomponent.IsDisableDeactivation()) - { - softbody->setActivationState(DISABLE_DEACTIVATION); - } - - softbody->setPose(true, true); - - physicsobject.physics_scene = scene.physics_scene; - GetPhysicsScene(scene).dynamicsWorld.addSoftBody(softbody); - } - } - - struct Ragdoll - { - enum BODYPART - { - BODYPART_PELVIS = 0, - BODYPART_SPINE, - BODYPART_HEAD, - - BODYPART_LEFT_UPPER_LEG, - BODYPART_LEFT_LOWER_LEG, - - BODYPART_RIGHT_UPPER_LEG, - BODYPART_RIGHT_LOWER_LEG, - - BODYPART_LEFT_UPPER_ARM, - BODYPART_LEFT_LOWER_ARM, - - BODYPART_RIGHT_UPPER_ARM, - BODYPART_RIGHT_LOWER_ARM, - - BODYPART_COUNT - }; - enum JOINT - { - JOINT_PELVIS_SPINE = 0, - JOINT_SPINE_HEAD, - - JOINT_LEFT_HIP, - JOINT_LEFT_KNEE, - - JOINT_RIGHT_HIP, - JOINT_RIGHT_KNEE, - - JOINT_LEFT_SHOULDER, - JOINT_LEFT_ELBOW, - - JOINT_RIGHT_SHOULDER, - JOINT_RIGHT_ELBOW, - - JOINT_COUNT - }; - - std::shared_ptr physics_scene; - std::shared_ptr rigidbodies[BODYPART_COUNT]; - btRigidBody* m_bodies[BODYPART_COUNT]; - btTypedConstraint* m_joints[JOINT_COUNT]; - bool state_active = false; - Entity saved_parents[BODYPART_COUNT] = {}; - float scale = 1; - - Ragdoll(Scene& scene, HumanoidComponent& humanoid, Entity humanoidEntity, float scale) - { - physics_scene = scene.physics_scene; - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)physics_scene.get())->dynamicsWorld; - - // https://github.com/bulletphysics/bullet3/blob/39b8de74df93721add193e5b3d9ebee579faebf8/examples/Benchmarks/BenchmarkDemo.cpp#L647 - - btVector3 roots[BODYPART_COUNT] = {}; - btTransform transforms[BODYPART_COUNT] = {}; - -#if 0 - // slow speed and visualizer to aid debugging: - wi::renderer::SetGameSpeed(0.1f); - SetDebugDrawEnabled(true); -#endif - - //Detect which way humanoid is facing in rest pose: - const float facing = scene.GetHumanoidDefaultFacing(humanoid, humanoidEntity); - - // Whole ragdoll will take a uniform scaling: - const XMMATRIX scaleMatrix = XMMatrixScaling(scale, scale, scale); - this->scale = scale; - - // Calculate the bone lengths and radiuses in armature local space and create rigid bodies for bones: - for (int c = 0; c < BODYPART_COUNT; ++c) - { - HumanoidComponent::HumanoidBone humanoid_bone = HumanoidComponent::HumanoidBone::Count; - Entity entityA = INVALID_ENTITY; - Entity entityB = INVALID_ENTITY; - switch (c) - { - case BODYPART_PELVIS: - humanoid_bone = HumanoidComponent::HumanoidBone::Hips; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Hips]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Spine]; - break; - case BODYPART_SPINE: - humanoid_bone = HumanoidComponent::HumanoidBone::Spine; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Spine]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Neck]; // prefer neck instead of head - if (entityB == INVALID_ENTITY) - { - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Head]; // fall back to head if neck not available - } - break; - case BODYPART_HEAD: - humanoid_bone = HumanoidComponent::HumanoidBone::Neck; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Neck]; // prefer neck instead of head - if (entityA == INVALID_ENTITY) - { - humanoid_bone = HumanoidComponent::HumanoidBone::Head; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Head]; // fall back to head if neck not available - } - break; - case BODYPART_LEFT_UPPER_LEG: - humanoid_bone = HumanoidComponent::HumanoidBone::LeftUpperLeg; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftUpperLeg]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerLeg]; - break; - case BODYPART_LEFT_LOWER_LEG: - humanoid_bone = HumanoidComponent::HumanoidBone::LeftLowerLeg; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerLeg]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftFoot]; - break; - case BODYPART_RIGHT_UPPER_LEG: - humanoid_bone = HumanoidComponent::HumanoidBone::RightUpperLeg; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightUpperLeg]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerLeg]; - break; - case BODYPART_RIGHT_LOWER_LEG: - humanoid_bone = HumanoidComponent::HumanoidBone::RightLowerLeg; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerLeg]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightFoot]; - break; - case BODYPART_LEFT_UPPER_ARM: - humanoid_bone = HumanoidComponent::HumanoidBone::LeftUpperArm; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftUpperArm]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerArm]; - break; - case BODYPART_LEFT_LOWER_ARM: - humanoid_bone = HumanoidComponent::HumanoidBone::LeftLowerArm; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerArm]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftHand]; - break; - case BODYPART_RIGHT_UPPER_ARM: - humanoid_bone = HumanoidComponent::HumanoidBone::RightUpperArm; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightUpperArm]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerArm]; - break; - case BODYPART_RIGHT_LOWER_ARM: - humanoid_bone = HumanoidComponent::HumanoidBone::RightLowerArm; - entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerArm]; - entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightHand]; - break; - } - assert(entityA != INVALID_ENTITY); - - // Calculations here will be done in armature local space. - // Unfortunately since humanoid can be separate from armature, we use a "find" utility to find bone rest matrix in armature - // Note that current scaling of character is applied here separately from rest pose - XMMATRIX restA = scene.GetRestPose(entityA) * scaleMatrix; - XMMATRIX restB = scene.GetRestPose(entityB) * scaleMatrix; - XMVECTOR rootA = restA.r[3]; - XMVECTOR rootB = restB.r[3]; - - // Every bone will be a rigid body: - rigidbodies[c] = std::make_unique(); - RigidBody& physicsobject = *rigidbodies[c]; - physicsobject.entity = entityA; - - float mass = scale; - float capsule_height = scale; - float capsule_radius = scale * humanoid.ragdoll_fatness; - - if (c == BODYPART_HEAD) - { - // Head doesn't necessarily have a child, so make up something reasonable: - capsule_height = 0.05f * scale; - capsule_radius = 0.1f * scale * humanoid.ragdoll_headsize; - } - else - { - // bone length: - XMVECTOR len = XMVector3Length(XMVectorSubtract(rootB, rootA)); - capsule_height = XMVectorGetX(len); - - // capsule radius and length is tweaked per body part: - switch (c) - { - case BODYPART_PELVIS: - capsule_radius = 0.1f * scale * humanoid.ragdoll_fatness; - break; - case BODYPART_SPINE: - capsule_radius = 0.1f * scale * humanoid.ragdoll_fatness; - capsule_height -= capsule_radius * 2; - break; - case BODYPART_LEFT_LOWER_ARM: - case BODYPART_RIGHT_LOWER_ARM: - capsule_radius = capsule_height * 0.15f * humanoid.ragdoll_fatness; - capsule_height += capsule_radius; - break; - case BODYPART_LEFT_UPPER_LEG: - case BODYPART_RIGHT_UPPER_LEG: - capsule_radius = capsule_height * 0.15f * humanoid.ragdoll_fatness; - capsule_height -= capsule_radius * 2; - break; - case BODYPART_LEFT_LOWER_LEG: - case BODYPART_RIGHT_LOWER_LEG: - capsule_radius = capsule_height * 0.15f * humanoid.ragdoll_fatness; - capsule_height -= capsule_radius; - break; - default: - capsule_radius = capsule_height * 0.2f * humanoid.ragdoll_fatness; - capsule_height -= capsule_radius * 2; - break; - } - } - - switch (c) - { - case BODYPART_LEFT_UPPER_ARM: - case BODYPART_LEFT_LOWER_ARM: - case BODYPART_RIGHT_UPPER_ARM: - case BODYPART_RIGHT_LOWER_ARM: - // Capsule could be rotated, but it is easier to just make CapsuleX and offset it: - physicsobject.shape = std::make_unique(capsule_radius, capsule_height); - break; - default: - physicsobject.shape = std::make_unique(capsule_radius, capsule_height); - break; - } - - btVector3 localInertia(0, 0, 0); - physicsobject.shape->calculateLocalInertia(mass, localInertia); - - btTransform shapeTransform; - shapeTransform.setIdentity(); - - // Get the translation and rotation part of rest matrix: - XMVECTOR SCA = {}; - XMVECTOR ROT = {}; - XMVECTOR TRA = {}; - XMMatrixDecompose(&SCA, &ROT, &TRA, restA); - XMFLOAT4 rot = {}; - XMFLOAT3 tra = {}; - XMStoreFloat4(&rot, ROT); - XMStoreFloat3(&tra, TRA); - shapeTransform.setOrigin(btVector3(tra.x, tra.y, tra.z)); // bone will be only translated initially - - // Rest basis is saved to make final correction between only-translated rest pose and final pose - btTransform restBasis; - restBasis.setIdentity(); - restBasis.setRotation(btQuaternion(rot.x, rot.y, rot.z, rot.w)); - - // capsule offset on axis is performed because otherwise capsule center would be in the bone root position - // which is not what we want. Instead the bone is moved on its axis so it resides between root and tail - const float offset = capsule_height * 0.5f + capsule_radius; - - btTransform additionalTransform; - additionalTransform.setIdentity(); - - switch (c) - { - case BODYPART_PELVIS: - break; - case BODYPART_SPINE: - case BODYPART_HEAD: - additionalTransform.setOrigin(btVector3(0, offset, 0)); - break; - case BODYPART_LEFT_UPPER_LEG: - case BODYPART_LEFT_LOWER_LEG: - case BODYPART_RIGHT_UPPER_LEG: - case BODYPART_RIGHT_LOWER_LEG: - additionalTransform.setOrigin(btVector3(0, -offset, 0)); - break; - case BODYPART_LEFT_UPPER_ARM: - case BODYPART_LEFT_LOWER_ARM: - additionalTransform.setOrigin(btVector3(-offset * facing, 0, 0)); - break; - case BODYPART_RIGHT_UPPER_ARM: - case BODYPART_RIGHT_LOWER_ARM: - additionalTransform.setOrigin(btVector3(offset * facing, 0, 0)); - break; - } - - shapeTransform.mult(shapeTransform, additionalTransform); - physicsobject.additionalTransform = additionalTransform; - physicsobject.additionalTransformInverse = additionalTransform.inverse(); - physicsobject.restBasis = restBasis; - physicsobject.restBasisInverse = restBasis.inverse(); - - physicsobject.motionState = btDefaultMotionState(shapeTransform); - - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, &physicsobject.motionState, physicsobject.shape.get(), localInertia); - - physicsobject.rigidBody = std::make_unique(rbInfo); - physicsobject.rigidBody->setUserPointer(&physicsobject); - physicsobject.rigidBody->setWorldTransform(shapeTransform); // immediate transform on first frame - - // by default, whole ragdoll is kinematic: - physicsobject.rigidBody->setCollisionFlags(physicsobject.rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); - physicsobject.rigidBody->setActivationState(DISABLE_DEACTIVATION); - - physicsobject.humanoid_ragdoll_entity = humanoidEntity; - physicsobject.humanoid_bone = humanoid_bone; - - physicsobject.physics_scene = scene.physics_scene; - dynamicsWorld.addRigidBody(physicsobject.rigidBody.get()); - - roots[c] = btVector3(XMVectorGetX(rootA), XMVectorGetY(rootA), XMVectorGetZ(rootA)); - m_bodies[c] = physicsobject.rigidBody.get(); - transforms[c] = physicsobject.rigidBody->getWorldTransform(); - } - - // Create all constraints below: - btHingeConstraint* hingeC = nullptr; - btConeTwistConstraint* coneC = nullptr; - - btTransform localA, localB; - static float constraint_dbg = 10; - static bool fixpose = false; // enable to fix the pose to rest pose, useful for debugging - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(0, -XM_PIDIV2 * facing, 0); - localA.setOrigin(roots[BODYPART_SPINE] - transforms[BODYPART_PELVIS].getOrigin()); - localB.getBasis().setEulerZYX(0, -XM_PIDIV2 * facing, 0); - localB.setOrigin(roots[BODYPART_SPINE] - transforms[BODYPART_SPINE].getOrigin()); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB); - if (fixpose) - { - hingeC->setLimit(0, 0); - } - else - { - hingeC->setLimit(0, XM_PIDIV2); - } - hingeC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_PELVIS_SPINE] = hingeC; - dynamicsWorld.addConstraint(m_joints[JOINT_PELVIS_SPINE], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(0, 0, XM_PIDIV2); - localA.setOrigin(roots[BODYPART_HEAD] - transforms[BODYPART_SPINE].getOrigin()); - localB.getBasis().setEulerZYX(0, 0, XM_PIDIV2); - localB.setOrigin(roots[BODYPART_HEAD] - transforms[BODYPART_HEAD].getOrigin()); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); - if (fixpose) - { - coneC->setLimit(0, 0, 0); - } - else - { - coneC->setLimit(XM_PIDIV4, XM_PIDIV2, XM_PIDIV4); - } - coneC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_SPINE_HEAD] = coneC; - dynamicsWorld.addConstraint(m_joints[JOINT_SPINE_HEAD], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(0, 0, XM_PIDIV4); - localA.setOrigin(roots[BODYPART_LEFT_UPPER_LEG] - transforms[BODYPART_PELVIS].getOrigin()); - localB.getBasis().setEulerZYX(0, 0, XM_PIDIV4); - localB.setOrigin(roots[BODYPART_LEFT_UPPER_LEG] - transforms[BODYPART_LEFT_UPPER_LEG].getOrigin()); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); - if (fixpose) - { - coneC->setLimit(0, 0, 0); - } - else - { - coneC->setLimit(XM_PIDIV4, XM_PIDIV4, 0); - } - coneC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_LEFT_HIP] = coneC; - dynamicsWorld.addConstraint(m_joints[JOINT_LEFT_HIP], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(0, -XM_PIDIV2 * facing, 0); - localA.setOrigin(roots[BODYPART_LEFT_LOWER_LEG] - transforms[BODYPART_LEFT_UPPER_LEG].getOrigin()); - localB.getBasis().setEulerZYX(0, -XM_PIDIV2 * facing, 0); - localB.setOrigin(roots[BODYPART_LEFT_LOWER_LEG] - transforms[BODYPART_LEFT_LOWER_LEG].getOrigin()); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB); - if (fixpose) - { - hingeC->setLimit(0, 0); - } - else - { - hingeC->setLimit(0, XM_PI * 0.8f); - } - hingeC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_LEFT_KNEE] = hingeC; - dynamicsWorld.addConstraint(m_joints[JOINT_LEFT_KNEE], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(0, 0, -XM_PIDIV4); - localA.setOrigin(roots[BODYPART_RIGHT_UPPER_LEG] - transforms[BODYPART_PELVIS].getOrigin()); - localB.getBasis().setEulerZYX(0, 0, -XM_PIDIV4); - localB.setOrigin(roots[BODYPART_RIGHT_UPPER_LEG] - transforms[BODYPART_RIGHT_UPPER_LEG].getOrigin()); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); - if (fixpose) - { - coneC->setLimit(0, 0, 0); - } - else - { - coneC->setLimit(XM_PIDIV4, XM_PIDIV4, 0); - } - coneC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_RIGHT_HIP] = coneC; - dynamicsWorld.addConstraint(m_joints[JOINT_RIGHT_HIP], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(0, -XM_PIDIV2 * facing, 0); - localA.setOrigin(roots[BODYPART_RIGHT_LOWER_LEG] - transforms[BODYPART_RIGHT_UPPER_LEG].getOrigin()); - localB.getBasis().setEulerZYX(0, -XM_PIDIV2 * facing, 0); - localB.setOrigin(roots[BODYPART_RIGHT_LOWER_LEG] - transforms[BODYPART_RIGHT_LOWER_LEG].getOrigin()); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB); - if (fixpose) - { - hingeC->setLimit(0, 0); - } - else - { - hingeC->setLimit(0, XM_PI * 0.8f); - } - hingeC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_RIGHT_KNEE] = hingeC; - dynamicsWorld.addConstraint(m_joints[JOINT_RIGHT_KNEE], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.setOrigin(roots[BODYPART_LEFT_UPPER_ARM] - transforms[BODYPART_SPINE].getOrigin()); - localB.setOrigin(roots[BODYPART_LEFT_UPPER_ARM] - transforms[BODYPART_LEFT_UPPER_ARM].getOrigin()); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); - if (fixpose) - { - coneC->setLimit(0, 0, 0); - } - else - { - coneC->setLimit(XM_PIDIV2, XM_PIDIV2, 0); - } - coneC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_LEFT_SHOULDER] = coneC; - dynamicsWorld.addConstraint(m_joints[JOINT_LEFT_SHOULDER], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(-XM_PIDIV2, 0, 0); - localA.setOrigin(roots[BODYPART_LEFT_LOWER_ARM] - transforms[BODYPART_LEFT_UPPER_ARM].getOrigin()); - localB.getBasis().setEulerZYX(-XM_PIDIV2, 0, 0); - localB.setOrigin(roots[BODYPART_LEFT_LOWER_ARM] - transforms[BODYPART_LEFT_LOWER_ARM].getOrigin()); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB); - if (fixpose) - { - hingeC->setLimit(0, 0); - } - else - { - hingeC->setLimit(-XM_PIDIV2, 0); - } - hingeC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_LEFT_ELBOW] = hingeC; - dynamicsWorld.addConstraint(m_joints[JOINT_LEFT_ELBOW], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(0, 0, 0); - localA.setOrigin(roots[BODYPART_RIGHT_UPPER_ARM] - transforms[BODYPART_SPINE].getOrigin()); - localB.setOrigin(roots[BODYPART_RIGHT_UPPER_ARM] - transforms[BODYPART_RIGHT_UPPER_ARM].getOrigin()); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); - if (fixpose) - { - coneC->setLimit(0, 0, 0); - } - else - { - coneC->setLimit(XM_PIDIV2, XM_PIDIV2, 0); - } - coneC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_RIGHT_SHOULDER] = coneC; - dynamicsWorld.addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true); - - localA.setIdentity(); - localB.setIdentity(); - localA.getBasis().setEulerZYX(XM_PIDIV2, 0, 0); - localA.setOrigin(roots[BODYPART_RIGHT_LOWER_ARM] - transforms[BODYPART_RIGHT_UPPER_ARM].getOrigin()); - localB.getBasis().setEulerZYX(XM_PIDIV2, 0, 0); - localB.setOrigin(roots[BODYPART_RIGHT_LOWER_ARM] - transforms[BODYPART_RIGHT_LOWER_ARM].getOrigin()); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB); - if (fixpose) - { - hingeC->setLimit(0, 0); - } - else - { - hingeC->setLimit(-XM_PIDIV2, 0); - } - hingeC->setDbgDrawSize(constraint_dbg); - m_joints[JOINT_RIGHT_ELBOW] = hingeC; - dynamicsWorld.addConstraint(m_joints[JOINT_RIGHT_ELBOW], true); - - // For all body parts, we now apply the current world space pose: - for (auto& x : rigidbodies) - { - RigidBody& physicsobject = *x.get(); - Entity entity = physicsobject.entity; - const TransformComponent* transform = scene.transforms.GetComponent(entity); - if (transform == nullptr) - continue; - - XMVECTOR SCA = {}; - XMVECTOR ROT = {}; - XMVECTOR TRA = {}; - XMMatrixDecompose(&SCA, &ROT, &TRA, XMLoadFloat4x4(&transform->world)); - XMFLOAT4 rot = {}; - XMFLOAT3 tra = {}; - XMStoreFloat4(&rot, ROT); - XMStoreFloat3(&tra, TRA); - - btTransform shapeTransform; - shapeTransform.setIdentity(); - shapeTransform.setOrigin(btVector3(tra.x, tra.y, tra.z)); - shapeTransform.setRotation(btQuaternion(rot.x, rot.y, rot.z, rot.w)); - shapeTransform.mult(shapeTransform, physicsobject.restBasisInverse); - shapeTransform.mult(shapeTransform, physicsobject.additionalTransform); - - btMotionState* ms = physicsobject.rigidBody->getMotionState(); - ms->setWorldTransform(shapeTransform); - physicsobject.rigidBody->setWorldTransform(shapeTransform); // immediate transform on first frame - } - } - ~Ragdoll() - { - if (physics_scene == nullptr) - return; - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)physics_scene.get())->dynamicsWorld; - for (auto& x : m_joints) - { - if (x == nullptr) - continue; - dynamicsWorld.removeConstraint(x); - delete x; - } - for (auto& x : rigidbodies) - { - dynamicsWorld.removeRigidBody(x->rigidBody.get()); - } - } - - // Activates ragdoll as dynamic physics object: - void Activate( - Scene& scene, - Entity humanoidEntity - ) - { - if (state_active) - return; - state_active = true; - - const HumanoidComponent* humanoid = scene.humanoids.GetComponent(humanoidEntity); - if (humanoid == nullptr) - return; - - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)scene.physics_scene.get())->dynamicsWorld; - - int c = 0; - for (auto& x : rigidbodies) - { - // remove kinematic flag from bone: - x->rigidBody->setCollisionFlags(x->rigidBody->getCollisionFlags() ^ btCollisionObject::CF_KINEMATIC_OBJECT); - - x->rigidBody->forceActivationState(ACTIVE_TAG); - x->rigidBody->setDeactivationTime(btScalar(0.8)); - x->rigidBody->setSleepingThresholds(btScalar(1.6), btScalar(2.5)); - x->rigidBody->setDamping(btScalar(0.05), btScalar(0.85)); - - // If we don't remove and re-add rigid body, then it will not correctly switch from kinematic to dynamic it seems: - dynamicsWorld.removeRigidBody(x->rigidBody.get()); - dynamicsWorld.addRigidBody(x->rigidBody.get()); - - // Save parenting information to be able to restore it: - const HierarchyComponent* hier = scene.hierarchy.GetComponent(x->entity); - if (hier != nullptr) - { - saved_parents[c] = hier->parentID; - } - else - { - saved_parents[c] = INVALID_ENTITY; - } - - // detach bone because it will be simulated in world space: - scene.Component_Detach(x->entity); - - c++; - } - - // Stop all anims that are children of humanoid: - for (size_t i = 0; i < scene.animations.GetCount(); ++i) - { - Entity entity = scene.animations.GetEntity(i); - if (!scene.Entity_IsDescendant(entity, humanoidEntity)) - continue; - AnimationComponent& animation = scene.animations[i]; - animation.Stop(); - } - } - - // Disables dynamic ragdoll and reattaches loose parts as they were: - void Deactivate( - Scene& scene - ) - { - if (!state_active) - return; - state_active = false; - - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)scene.physics_scene.get())->dynamicsWorld; - - int c = 0; - for (auto& x : rigidbodies) - { - // add kinematic flag from bone: - x->rigidBody->setCollisionFlags(x->rigidBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); - - x->rigidBody->forceActivationState(DISABLE_DEACTIVATION); - - dynamicsWorld.removeRigidBody(x->rigidBody.get()); - dynamicsWorld.addRigidBody(x->rigidBody.get()); - - if (saved_parents[c] != INVALID_ENTITY) - { - scene.Component_Attach(x->entity, saved_parents[c]); - } - c++; - } - } - }; - - - void RunPhysicsUpdateSystem( - wi::jobsystem::context& ctx, - Scene& scene, - float dt - ) - { - if (!IsEnabled() || dt <= 0) - return; - - auto range = wi::profiler::BeginRangeCPU("Physics"); - - btSoftRigidDynamicsWorld& dynamicsWorld = GetPhysicsScene(scene).dynamicsWorld; - dynamicsWorld.setGravity(btVector3(scene.weather.gravity.x, scene.weather.gravity.y, scene.weather.gravity.z)); - - btVector3 wind = btVector3(scene.weather.windDirection.x, scene.weather.windDirection.y, scene.weather.windDirection.z); - - // Ragdoll management: - for (size_t i = 0; i < scene.humanoids.GetCount(); ++i) - { - HumanoidComponent& humanoid = scene.humanoids[i]; - Entity humanoidEntity = scene.humanoids.GetEntity(i); - float scale = 1; - if (scene.transforms.Contains(humanoidEntity)) - { - scale = scene.transforms.GetComponent(humanoidEntity)->scale_local.x; - } - if (humanoid.ragdoll != nullptr) - { - Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); - if (!wi::math::float_equal(ragdoll.scale, scale)) - { - humanoid.SetRagdollPhysicsEnabled(false); // while scaling ragdoll, it will be kinematic - ragdoll.Deactivate(scene); // recreate attached skeleton hierarchy structure - humanoid.ragdoll = {}; // delete ragdoll if scale changed, it will be recreated - } - } - if (humanoid.ragdoll == nullptr) - { - humanoid.ragdoll = std::make_shared(scene, humanoid, humanoidEntity, scale); - } - Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); - if (humanoid.IsRagdollPhysicsEnabled()) - { - ragdoll.Activate(scene, humanoidEntity); - } - else - { - ragdoll.Deactivate(scene); - } - } - - // System will register rigidbodies to objects: - wi::jobsystem::Dispatch(ctx, (uint32_t)scene.rigidbodies.GetCount(), 256, [&](wi::jobsystem::JobArgs args) { - - RigidBodyPhysicsComponent& physicscomponent = scene.rigidbodies[args.jobIndex]; - Entity entity = scene.rigidbodies.GetEntity(args.jobIndex); - - if (physicscomponent.physicsobject == nullptr && scene.transforms.Contains(entity)) - { - TransformComponent& transform = *scene.transforms.GetComponent(entity); - const ObjectComponent* object = scene.objects.GetComponent(entity); - const MeshComponent* mesh = nullptr; - if (object != nullptr) - { - mesh = scene.meshes.GetComponent(object->meshID); - } - physicsLock.lock(); - AddRigidBody(scene, entity, physicscomponent, transform, mesh); - physicsLock.unlock(); - } - - if (physicscomponent.physicsobject != nullptr) - { - btRigidBody* rigidbody = GetRigidBody(physicscomponent).rigidBody.get(); - - rigidbody->setDamping( - physicscomponent.damping_linear, - physicscomponent.damping_angular - ); - rigidbody->setFriction(physicscomponent.friction); - rigidbody->setRestitution(physicscomponent.restitution); - if (physicscomponent.IsKinematic() && (rigidbody->getCollisionFlags() & btCollisionObject::CF_KINEMATIC_OBJECT) == 0) - { - // It became kinematic when it wasn't before: - rigidbody->setCollisionFlags(rigidbody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); - btVector3 localInertia(0, 0, 0); - rigidbody->setMassProps(0, localInertia); - dynamicsWorld.removeRigidBody(rigidbody); - dynamicsWorld.addRigidBody(rigidbody); - } - if (!physicscomponent.IsKinematic() && (rigidbody->getCollisionFlags() & btCollisionObject::CF_KINEMATIC_OBJECT) != 0) - { - // It became non-kinematic when it was kinematic before: - rigidbody->setCollisionFlags(rigidbody->getCollisionFlags() ^ btCollisionObject::CF_KINEMATIC_OBJECT); - btVector3 localInertia(0, 0, 0); - rigidbody->getCollisionShape()->calculateLocalInertia(physicscomponent.mass, localInertia); - rigidbody->setMassProps(physicscomponent.mass, localInertia); - dynamicsWorld.removeRigidBody(rigidbody); - dynamicsWorld.addRigidBody(rigidbody); - scene.Component_Detach(entity); - } - } - }); - - // System will register softbodies to meshes and update physics engine state: - wi::jobsystem::Dispatch(ctx, (uint32_t)scene.softbodies.GetCount(), 1, [&](wi::jobsystem::JobArgs args) { - - SoftBodyPhysicsComponent& physicscomponent = scene.softbodies[args.jobIndex]; - Entity entity = scene.softbodies.GetEntity(args.jobIndex); - if (!scene.meshes.Contains(entity)) - return; - MeshComponent& mesh = *scene.meshes.GetComponent(entity); - const ArmatureComponent* armature = mesh.IsSkinned() ? scene.armatures.GetComponent(mesh.armatureID) : nullptr; - mesh.SetDynamic(true); - - if (physicscomponent._flags & SoftBodyPhysicsComponent::FORCE_RESET) - { - physicscomponent._flags &= ~SoftBodyPhysicsComponent::FORCE_RESET; - physicscomponent.physicsobject = nullptr; - } - if (physicscomponent._flags & SoftBodyPhysicsComponent::SAFE_TO_REGISTER && physicscomponent.physicsobject == nullptr) - { - physicsLock.lock(); - AddSoftBody(scene, entity, physicscomponent, mesh); - physicsLock.unlock(); - } - - if (physicscomponent.physicsobject != nullptr) - { - btSoftBody* softbody = GetSoftBody(physicscomponent).softBody.get(); - softbody->getWorldInfo()->m_gravity = dynamicsWorld.getGravity(); - softbody->m_cfg.kDF = physicscomponent.friction; - softbody->setWindVelocity(wind); - - softbody->setFriction(physicscomponent.friction); - softbody->setRestitution(physicscomponent.restitution); - - // This is different from rigid bodies, because soft body is a per mesh component (no TransformComponent). World matrix is propagated down from single mesh instance (ObjectUpdateSystem). - XMMATRIX worldMatrix = XMLoadFloat4x4(&physicscomponent.worldMatrix); - - // System controls zero weight soft body nodes: - for (size_t ind = 0; ind < physicscomponent.weights.size(); ++ind) - { - float weight = physicscomponent.weights[ind]; - - if (weight == 0) - { - btSoftBody::Node& node = softbody->m_nodes[(uint32_t)ind]; - uint32_t graphicsInd = physicscomponent.physicsToGraphicsVertexMapping[ind]; - XMFLOAT3 position = mesh.vertex_positions[graphicsInd]; - XMVECTOR P = armature == nullptr ? XMLoadFloat3(&position) : wi::scene::SkinVertex(mesh, *armature, graphicsInd); - P = XMVector3Transform(P, worldMatrix); - XMStoreFloat3(&position, P); - node.m_x = btVector3(position.x, position.y, position.z); - } - } - } - }); - - // Feedback system kinematics to physics engine: - for (int i = 0; i < dynamicsWorld.getCollisionObjectArray().size(); ++i) - { - btCollisionObject* collisionobject = dynamicsWorld.getCollisionObjectArray()[i]; - - btRigidBody* rigidbody = btRigidBody::upcast(collisionobject); - if (rigidbody != nullptr) - { - RigidBody* physicsobject = (RigidBody*)rigidbody->getUserPointer(); - Entity entity = physicsobject->entity; - const bool kinematic = rigidbody->getCollisionFlags() & btCollisionObject::CF_KINEMATIC_OBJECT; - - TransformComponent& transform = *scene.transforms.GetComponent(entity); - - btMotionState* motionState = rigidbody->getMotionState(); - btTransform physicsTransform; - - XMFLOAT3 position = transform.GetPosition(); - XMFLOAT4 rotation = transform.GetRotation(); - btVector3 T(position.x, position.y, position.z); - btQuaternion R(rotation.x, rotation.y, rotation.z, rotation.w); - physicsTransform.setOrigin(T); - physicsTransform.setRotation(R); - physicsTransform.mult(physicsTransform, physicsobject->restBasisInverse); - physicsTransform.mult(physicsTransform, physicsobject->additionalTransform); - motionState->setWorldTransform(physicsTransform); - rigidbody->setWorldTransform(physicsTransform); - - if (physicsobject->humanoid_ragdoll_entity == INVALID_ENTITY) - { - btCollisionShape* shape = rigidbody->getCollisionShape(); - XMFLOAT3 scale = transform.GetScale(); - btVector3 S(scale.x, scale.y, scale.z); - shape->setLocalScaling(S); - } - } - } - - wi::jobsystem::Wait(ctx); - - // Perform internal simulation step: - if (IsSimulationEnabled()) - { - dynamicsWorld.stepSimulation(dt, ACCURACY, TIMESTEP); - } - - // Feedback physics engine state to system: - for (int i = 0; i < dynamicsWorld.getCollisionObjectArray().size(); ++i) - { - btCollisionObject* collisionobject = dynamicsWorld.getCollisionObjectArray()[i]; - - btRigidBody* rigidbody = btRigidBody::upcast(collisionobject); - if (rigidbody != nullptr) - { - RigidBody* physicsobject = (RigidBody*)rigidbody->getUserPointer(); - Entity entity = physicsobject->entity; - const bool kinematic = rigidbody->getCollisionFlags() & btCollisionObject::CF_KINEMATIC_OBJECT; - - // Feedback non-kinematic objects to system: - if (IsSimulationEnabled() && !kinematic && scene.transforms.Contains(entity)) - { - TransformComponent& transform = *scene.transforms.GetComponent(entity); - - btTransform physicsTransform; - rigidbody->getMotionState()->getWorldTransform(physicsTransform); - physicsTransform.mult(physicsTransform, physicsobject->additionalTransformInverse); - physicsTransform.mult(physicsTransform, physicsobject->restBasis); - btVector3 T = physicsTransform.getOrigin(); - btQuaternion R = physicsTransform.getRotation(); - - transform.translation_local = XMFLOAT3(T.x(), T.y(), T.z()); - transform.rotation_local = XMFLOAT4(R.x(), R.y(), R.z(), R.w()); - transform.SetDirty(); - } - } - else - { - btSoftBody* softbody = btSoftBody::upcast(collisionobject); - - if (softbody != nullptr) - { - SoftBody* physicsobject = (SoftBody*)softbody->getUserPointer(); - Entity entity = physicsobject->entity; - SoftBodyPhysicsComponent* physicscomponent = scene.softbodies.GetComponent(entity); - - // If you need it, you can enable soft body node debug strings here: -#if 0 - if (IsDebugDrawEnabled()) - { - btSoftBodyHelpers::DrawInfos( - softbody, - &debugDraw, - false, // masses - true, // areas - false // stress - ); - } -#endif - - MeshComponent& mesh = *scene.meshes.GetComponent(entity); - - // System mesh aabb will be queried from physics engine soft body: - btVector3 aabb_min; - btVector3 aabb_max; - softbody->getAabb(aabb_min, aabb_max); - physicscomponent->aabb = wi::primitive::AABB(XMFLOAT3(aabb_min.x(), aabb_min.y(), aabb_min.z()), XMFLOAT3(aabb_max.x(), aabb_max.y(), aabb_max.z())); - mesh.aabb = physicscomponent->aabb; - - // Soft body simulation nodes will update graphics mesh: - for (size_t ind = 0; ind < mesh.vertex_positions.size(); ++ind) - { - uint32_t physicsInd = physicscomponent->graphicsToPhysicsVertexMapping[ind]; - - btSoftBody::Node& node = softbody->m_nodes[physicsInd]; - - physicscomponent->vertex_positions_simulation[ind].FromFULL(XMFLOAT3(node.m_x.getX(), node.m_x.getY(), node.m_x.getZ())); - physicscomponent->vertex_normals_simulation[ind].FromFULL(XMFLOAT3(-node.m_n.getX(), -node.m_n.getY(), -node.m_n.getZ())); - } - - // Update tangent vectors: - if (!mesh.vertex_uvset_0.empty() && !physicscomponent->vertex_normals_simulation.empty()) - { - uint32_t first_subset = 0; - uint32_t last_subset = 0; - mesh.GetLODSubsetRange(0, first_subset, last_subset); - for (uint32_t subsetIndex = first_subset; subsetIndex < last_subset; ++subsetIndex) - { - const MeshComponent::MeshSubset& subset = mesh.subsets[subsetIndex]; - for (size_t i = 0; i < subset.indexCount; i += 3) - { - const uint32_t i0 = mesh.indices[i + 0]; - const uint32_t i1 = mesh.indices[i + 1]; - const uint32_t i2 = mesh.indices[i + 2]; - - const XMFLOAT3 v0 = physicscomponent->vertex_positions_simulation[i0].GetPOS(); - const XMFLOAT3 v1 = physicscomponent->vertex_positions_simulation[i1].GetPOS(); - const XMFLOAT3 v2 = physicscomponent->vertex_positions_simulation[i2].GetPOS(); - - const XMFLOAT2 u0 = mesh.vertex_uvset_0[i0]; - const XMFLOAT2 u1 = mesh.vertex_uvset_0[i1]; - const XMFLOAT2 u2 = mesh.vertex_uvset_0[i2]; - - const XMVECTOR nor0 = physicscomponent->vertex_normals_simulation[i0].LoadNOR(); - const XMVECTOR nor1 = physicscomponent->vertex_normals_simulation[i1].LoadNOR(); - const XMVECTOR nor2 = physicscomponent->vertex_normals_simulation[i2].LoadNOR(); - - const XMVECTOR facenormal = XMVector3Normalize(XMVectorAdd(XMVectorAdd(nor0, nor1), nor2)); - - const float x1 = v1.x - v0.x; - const float x2 = v2.x - v0.x; - const float y1 = v1.y - v0.y; - const float y2 = v2.y - v0.y; - const float z1 = v1.z - v0.z; - const float z2 = v2.z - v0.z; - - const float s1 = u1.x - u0.x; - const float s2 = u2.x - u0.x; - const float t1 = u1.y - u0.y; - const float t2 = u2.y - u0.y; - - const float r = 1.0f / (s1 * t2 - s2 * t1); - const XMVECTOR sdir = XMVectorSet((t2 * x1 - t1 * x2) * r, (t2 * y1 - t1 * y2) * r, - (t2 * z1 - t1 * z2) * r, 0); - const XMVECTOR tdir = XMVectorSet((s1 * x2 - s2 * x1) * r, (s1 * y2 - s2 * y1) * r, - (s1 * z2 - s2 * z1) * r, 0); - - XMVECTOR tangent; - tangent = XMVector3Normalize(XMVectorSubtract(sdir, XMVectorMultiply(facenormal, XMVector3Dot(facenormal, sdir)))); - float sign = XMVectorGetX(XMVector3Dot(XMVector3Cross(tangent, facenormal), tdir)) < 0.0f ? -1.0f : 1.0f; - - XMFLOAT3 t; - XMStoreFloat3(&t, tangent); - - physicscomponent->vertex_tangents_tmp[i0].x += t.x; - physicscomponent->vertex_tangents_tmp[i0].y += t.y; - physicscomponent->vertex_tangents_tmp[i0].z += t.z; - physicscomponent->vertex_tangents_tmp[i0].w = sign; - - physicscomponent->vertex_tangents_tmp[i1].x += t.x; - physicscomponent->vertex_tangents_tmp[i1].y += t.y; - physicscomponent->vertex_tangents_tmp[i1].z += t.z; - physicscomponent->vertex_tangents_tmp[i1].w = sign; - - physicscomponent->vertex_tangents_tmp[i2].x += t.x; - physicscomponent->vertex_tangents_tmp[i2].y += t.y; - physicscomponent->vertex_tangents_tmp[i2].z += t.z; - physicscomponent->vertex_tangents_tmp[i2].w = sign; - } - } - - for (size_t i = 0; i < physicscomponent->vertex_tangents_simulation.size(); ++i) - { - physicscomponent->vertex_tangents_simulation[i].FromFULL(physicscomponent->vertex_tangents_tmp[i]); - } - } - - } - } - } - - if (IsDebugDrawEnabled()) - { - dynamicsWorld.setDebugDrawer(&debugDraw); - dynamicsWorld.debugDrawWorld(); - } - else - { - // Note: there was an issue of stuck debug draws if debug drawer is not removed completely: - dynamicsWorld.setDebugDrawer(nullptr); - } - - wi::profiler::EndRange(range); // Physics - } - - - - void SetLinearVelocity( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& velocity - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->setLinearVelocity(btVector3(velocity.x, velocity.y, velocity.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - void SetAngularVelocity( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& velocity - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->setAngularVelocity(btVector3(velocity.x, velocity.y, velocity.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - - void ApplyForce( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& force - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->applyCentralForce(btVector3(force.x, force.y, force.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - void ApplyForceAt( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& force, - const XMFLOAT3& at - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->applyForce(btVector3(force.x, force.y, force.z), btVector3(at.x, at.y, at.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - - void ApplyImpulse( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& impulse - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->applyCentralImpulse(btVector3(impulse.x, impulse.y, impulse.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - void ApplyImpulse( - wi::scene::HumanoidComponent& humanoid, - wi::scene::HumanoidComponent::HumanoidBone bone, - const XMFLOAT3& impulse - ) - { - ApplyImpulseAt(humanoid, bone, impulse, XMFLOAT3(0, 0, 0)); - } - void ApplyImpulseAt( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& impulse, - const XMFLOAT3& at - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->applyImpulse(btVector3(impulse.x, impulse.y, impulse.z), btVector3(at.x, at.y, at.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - void ApplyImpulseAt( - wi::scene::HumanoidComponent& humanoid, - wi::scene::HumanoidComponent::HumanoidBone bone, - const XMFLOAT3& impulse, - const XMFLOAT3& at - ) - { - if (humanoid.ragdoll == nullptr) - return; - - Ragdoll::BODYPART bodypart = Ragdoll::BODYPART_COUNT; - switch (bone) - { - case HumanoidComponent::HumanoidBone::Hips: - bodypart = Ragdoll::BODYPART_PELVIS; - break; - case HumanoidComponent::HumanoidBone::Spine: - bodypart = Ragdoll::BODYPART_SPINE; - break; - case HumanoidComponent::HumanoidBone::Head: - case HumanoidComponent::HumanoidBone::Neck: - bodypart = Ragdoll::BODYPART_HEAD; - break; - case HumanoidComponent::HumanoidBone::RightUpperArm: - bodypart = Ragdoll::BODYPART_RIGHT_UPPER_ARM; - break; - case HumanoidComponent::HumanoidBone::RightLowerArm: - bodypart = Ragdoll::BODYPART_RIGHT_LOWER_ARM; - break; - case HumanoidComponent::HumanoidBone::LeftUpperArm: - bodypart = Ragdoll::BODYPART_LEFT_UPPER_ARM; - break; - case HumanoidComponent::HumanoidBone::LeftLowerArm: - bodypart = Ragdoll::BODYPART_LEFT_LOWER_ARM; - break; - case HumanoidComponent::HumanoidBone::RightUpperLeg: - bodypart = Ragdoll::BODYPART_RIGHT_UPPER_LEG; - break; - case HumanoidComponent::HumanoidBone::RightLowerLeg: - bodypart = Ragdoll::BODYPART_RIGHT_LOWER_LEG; - break; - case HumanoidComponent::HumanoidBone::LeftUpperLeg: - bodypart = Ragdoll::BODYPART_LEFT_UPPER_LEG; - break; - case HumanoidComponent::HumanoidBone::LeftLowerLeg: - bodypart = Ragdoll::BODYPART_LEFT_LOWER_LEG; - break; - } - if (bodypart == Ragdoll::BODYPART_COUNT) - return; - - Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); - if (ragdoll.rigidbodies[bodypart] == nullptr || ragdoll.rigidbodies[bodypart]->rigidBody == nullptr) - return; - ragdoll.rigidbodies[bodypart]->rigidBody->applyImpulse(btVector3(impulse.x, impulse.y, impulse.z), btVector3(at.x, at.y, at.z)); - ragdoll.rigidbodies[bodypart]->rigidBody->forceActivationState(ACTIVE_TAG); - } - - void ApplyTorque( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& torque - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->applyTorque(btVector3(torque.x, torque.y, torque.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - void ApplyTorqueImpulse( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - const XMFLOAT3& torque - ) - { - if (physicscomponent.physicsobject != nullptr) - { - GetRigidBody(physicscomponent).rigidBody->applyTorqueImpulse(btVector3(torque.x, torque.y, torque.z)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(ACTIVE_TAG); - } - } - - constexpr int to_internal(ActivationState state) - { - switch (state) - { - default: - case wi::physics::ActivationState::Active: - return ACTIVE_TAG; - case wi::physics::ActivationState::Inactive: - return DISABLE_SIMULATION; - } - } - void SetActivationState( - wi::scene::RigidBodyPhysicsComponent& physicscomponent, - ActivationState state - ) - { - if (physicscomponent.physicsobject != nullptr) - { - //GetRigidBody(physicscomponent).rigidBody->setActivationState(to_internal(state)); - GetRigidBody(physicscomponent).rigidBody->forceActivationState(to_internal(state)); - } - } - void SetActivationState( - wi::scene::SoftBodyPhysicsComponent& physicscomponent, - ActivationState state - ) - { - if (physicscomponent.physicsobject != nullptr) - { - //GetSoftBody(physicscomponent).softBody->setActivationState(to_internal(state)); - GetSoftBody(physicscomponent).softBody->forceActivationState(to_internal(state)); - } - } - - RayIntersectionResult Intersects( - const wi::scene::Scene& scene, - wi::primitive::Ray ray - ) - { - RayIntersectionResult result; - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)scene.physics_scene.get())->dynamicsWorld; - float tmin = wi::math::Clamp(ray.TMin, 0, 1000000); - float tmax = wi::math::Clamp(ray.TMax, 0, 1000000); - btVector3 rayFrom = btVector3( - ray.origin.x + ray.direction.x * tmin, - ray.origin.y + ray.direction.y * tmin, - ray.origin.z + ray.direction.z * tmin - ); - btVector3 rayTo = btVector3( - ray.origin.x + ray.direction.x * tmax, - ray.origin.y + ray.direction.y * tmax, - ray.origin.z + ray.direction.z * tmax - ); - btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom, rayTo); - dynamicsWorld.rayTest(rayFrom, rayTo, rayCallback); - if (rayCallback.hasHit()) - { - result.physicsobject = rayCallback.m_collisionObject; - result.position.x = rayCallback.m_hitPointWorld.getX(); - result.position.y = rayCallback.m_hitPointWorld.getY(); - result.position.z = rayCallback.m_hitPointWorld.getZ(); - result.normal.x = rayCallback.m_hitNormalWorld.getX(); - result.normal.y = rayCallback.m_hitNormalWorld.getY(); - result.normal.z = -rayCallback.m_hitNormalWorld.getZ(); - - btVector3 position_local = rayCallback.m_hitPointWorld; - - const btRigidBody* rigidbody = btRigidBody::upcast(rayCallback.m_collisionObject); - if (rigidbody != nullptr) - { - RigidBody* physicsobject = (RigidBody*)rigidbody->getUserPointer(); - result.entity = physicsobject->entity; - result.humanoid_ragdoll_entity = physicsobject->humanoid_ragdoll_entity; - result.humanoid_bone = physicsobject->humanoid_bone; - position_local = physicsobject->rigidBody->getCenterOfMassTransform().inverse() * position_local; - } - const btSoftBody* softbody = btSoftBody::upcast(rayCallback.m_collisionObject); - if (softbody != nullptr) - { - SoftBody* physicsobject = (SoftBody*)softbody->getUserPointer(); - result.entity = physicsobject->entity; - } - - result.position_local.x = position_local.getX(); - result.position_local.y = position_local.getY(); - result.position_local.z = position_local.getZ(); - } - return result; - } - - struct PickDragOperation_Bullet - { - std::shared_ptr physics_scene; - std::unique_ptr constraint; - float pick_distance = 0; - btRigidBody* rigidbody = nullptr; - ~PickDragOperation_Bullet() - { - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)physics_scene.get())->dynamicsWorld; - dynamicsWorld.removeConstraint(constraint.get()); - } - }; - void PickDrag( - const wi::scene::Scene& scene, - wi::primitive::Ray ray, - PickDragOperation& op - ) - { - if (scene.physics_scene == nullptr) - return; - btSoftRigidDynamicsWorld& dynamicsWorld = ((PhysicsScene*)scene.physics_scene.get())->dynamicsWorld; - float tmin = wi::math::Clamp(ray.TMin, 0, 1000000); - float tmax = wi::math::Clamp(ray.TMax, 0, 1000000); - btVector3 rayFrom = btVector3( - ray.origin.x + ray.direction.x * tmin, - ray.origin.y + ray.direction.y * tmin, - ray.origin.z + ray.direction.z * tmin - ); - btVector3 rayTo = btVector3( - ray.origin.x + ray.direction.x * tmax, - ray.origin.y + ray.direction.y * tmax, - ray.origin.z + ray.direction.z * tmax - ); - if (op.IsValid()) - { - // Continue dragging: - PickDragOperation_Bullet* internal_state = (PickDragOperation_Bullet*)op.internal_state.get(); - btVector3 oldPivotInB = internal_state->constraint->getFrameOffsetA().getOrigin(); - btVector3 newPivotB; - btVector3 dir = (rayTo - rayFrom).normalize(); - newPivotB = rayFrom + dir * internal_state->pick_distance; - internal_state->constraint->getFrameOffsetA().setOrigin(newPivotB); - internal_state->rigidbody->forceActivationState(ACTIVE_TAG); - } - else - { - // Begin picking: - RayIntersectionResult result = Intersects(scene, ray); - if (!result.IsValid()) - return; - btCollisionObject* collisionobject = (btCollisionObject*)result.physicsobject; - btRigidBody* rigidbody = btRigidBody::upcast(collisionobject); - if (rigidbody == nullptr) - return; - - auto internal_state = std::make_shared(); - internal_state->physics_scene = scene.physics_scene; - internal_state->pick_distance = (btVector3(result.position.x, result.position.y, result.position.z) - rayFrom).length(); - internal_state->rigidbody = rigidbody; - - btTransform transform; - transform.setIdentity(); - transform.setOrigin(btVector3(result.position_local.x, result.position_local.y, result.position_local.z)); - - internal_state->constraint = std::make_unique(*rigidbody, transform, false); - internal_state->constraint->setLinearLowerLimit(btVector3(0, 0, 0)); - internal_state->constraint->setLinearUpperLimit(btVector3(0, 0, 0)); - internal_state->constraint->setAngularLowerLimit(btVector3(0, 0, 0)); - internal_state->constraint->setAngularUpperLimit(btVector3(0, 0, 0)); - dynamicsWorld.addConstraint(internal_state->constraint.get()); - - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_CFM, 0.8f, 0); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_CFM, 0.8f, 1); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_CFM, 0.8f, 2); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_CFM, 0.8f, 3); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_CFM, 0.8f, 4); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_CFM, 0.8f, 5); - - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_ERP, 0.1f, 0); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_ERP, 0.1f, 1); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_ERP, 0.1f, 2); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_ERP, 0.1f, 3); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_ERP, 0.1f, 4); - internal_state->constraint->setParam(BT_CONSTRAINT_STOP_ERP, 0.1f, 5); - - op.internal_state = internal_state; - } - } -} diff --git a/WickedEngine/wiPhysics_Jolt.cpp b/WickedEngine/wiPhysics_Jolt.cpp new file mode 100644 index 000000000..8ed8f9588 --- /dev/null +++ b/WickedEngine/wiPhysics_Jolt.cpp @@ -0,0 +1,1950 @@ +#include "wiPhysics.h" + +#include "wiScene.h" +#include "wiProfiler.h" +#include "wiBacklog.h" +#include "wiJobSystem.h" +#include "wiRenderer.h" +#include "wiTimer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef JPH_DEBUG_RENDERER +#include +#endif // JPH_DEBUG_RENDERER + +#include + +// Disable common warnings triggered by Jolt, you can use JPH_SUPPRESS_WARNING_PUSH / JPH_SUPPRESS_WARNING_POP to store and restore the warning state +JPH_SUPPRESS_WARNINGS + +// All Jolt symbols are in the JPH namespace +using namespace JPH; + +using namespace wi::ecs; +using namespace wi::scene; + +namespace wi::physics +{ + namespace jolt + { + bool ENABLED = true; + bool SIMULATION_ENABLED = true; + bool DEBUGDRAW_ENABLED = false; + int ACCURACY = 8; + int softbodyIterationCount = 5; + float TIMESTEP = 1.0f / 120.0f; + + inline Vec3 cast(const XMFLOAT3& v) { return Vec3(v.x, v.y, v.z); } + inline Quat cast(const XMFLOAT4& v) { return Quat(v.x, v.y, v.z, v.w); } + inline Mat44 cast(const XMFLOAT4X4& v) + { + return Mat44( + Vec4(v._11, v._12, v._13, v._14), + Vec4(v._21, v._22, v._23, v._24), + Vec4(v._31, v._32, v._33, v._34), + Vec4(v._41, v._42, v._43, v._44) + ); + } + inline XMFLOAT3 cast(Vec3Arg v) { return XMFLOAT3(v.GetX(), v.GetY(), v.GetZ()); } + inline XMFLOAT4 cast(QuatArg v) { return XMFLOAT4(v.GetX(), v.GetY(), v.GetZ(), v.GetW()); } + inline XMFLOAT4X4 cast(Mat44 v) + { + XMFLOAT4X4 ret; + v.StoreFloat4x4((Float4*)&ret); + return ret; + } + + namespace Layers + { + static constexpr ObjectLayer NON_MOVING = 0; + static constexpr ObjectLayer MOVING = 1; + static constexpr ObjectLayer NUM_LAYERS = 2; + }; + + /// Class that determines if two object layers can collide + class ObjectLayerPairFilterImpl : public ObjectLayerPairFilter + { + public: + bool ShouldCollide(ObjectLayer inObject1, ObjectLayer inObject2) const override + { + switch (inObject1) + { + case Layers::NON_MOVING: + return inObject2 == Layers::MOVING; // Non moving only collides with moving + case Layers::MOVING: + return true; // Moving collides with everything + default: + JPH_ASSERT(false); + return false; + } + } + }; + + // Each broadphase layer results in a separate bounding volume tree in the broad phase. You at least want to have + // a layer for non-moving and moving objects to avoid having to update a tree full of static objects every frame. + // You can have a 1-on-1 mapping between object layers and broadphase layers (like in this case) but if you have + // many object layers you'll be creating many broad phase trees, which is not efficient. If you want to fine tune + // your broadphase layers define JPH_TRACK_BROADPHASE_STATS and look at the stats reported on the TTY. + namespace BroadPhaseLayers + { + static constexpr BroadPhaseLayer NON_MOVING(0); + static constexpr BroadPhaseLayer MOVING(1); + static constexpr uint NUM_LAYERS(2); + }; + + // BroadPhaseLayerInterface implementation + // This defines a mapping between object and broadphase layers. + class BPLayerInterfaceImpl final : public BroadPhaseLayerInterface + { + public: + BPLayerInterfaceImpl() + { + // Create a mapping table from object to broad phase layer + mObjectToBroadPhase[Layers::NON_MOVING] = BroadPhaseLayers::NON_MOVING; + mObjectToBroadPhase[Layers::MOVING] = BroadPhaseLayers::MOVING; + } + + virtual uint GetNumBroadPhaseLayers() const override + { + return BroadPhaseLayers::NUM_LAYERS; + } + + virtual BroadPhaseLayer GetBroadPhaseLayer(ObjectLayer inLayer) const override + { + JPH_ASSERT(inLayer < Layers::NUM_LAYERS); + return mObjectToBroadPhase[inLayer]; + } + + private: + BroadPhaseLayer mObjectToBroadPhase[Layers::NUM_LAYERS]; + }; + + /// Class that determines if an object layer can collide with a broadphase layer + class ObjectVsBroadPhaseLayerFilterImpl : public ObjectVsBroadPhaseLayerFilter + { + public: + virtual bool ShouldCollide(ObjectLayer inLayer1, BroadPhaseLayer inLayer2) const override + { + switch (inLayer1) + { + case Layers::NON_MOVING: + return inLayer2 == BroadPhaseLayers::MOVING; + case Layers::MOVING: + return true; + default: + JPH_ASSERT(false); + return false; + } + } + }; + + struct JoltDestroyer + { + ~JoltDestroyer() + { + UnregisterTypes(); + delete Factory::sInstance; + Factory::sInstance = nullptr; + } + } jolt_destroyer; + + struct PhysicsScene + { + PhysicsSystem physics_system; + BPLayerInterfaceImpl broad_phase_layer_interface; + ObjectVsBroadPhaseLayerFilterImpl object_vs_broadphase_layer_filter; + ObjectLayerPairFilterImpl object_vs_object_layer_filter; + }; + PhysicsScene& GetPhysicsScene(Scene& scene) + { + if (scene.physics_scene == nullptr) + { + auto physics_scene = std::make_shared(); + + const uint cMaxBodies = 65536; + const uint cNumBodyMutexes = 0; + const uint cMaxBodyPairs = 65536; + const uint cMaxContactConstraints = 10240; + physics_scene->physics_system.Init( + cMaxBodies, + cNumBodyMutexes, + cMaxBodyPairs, + cMaxContactConstraints, + physics_scene->broad_phase_layer_interface, + physics_scene->object_vs_broadphase_layer_filter, + physics_scene->object_vs_object_layer_filter + ); + + scene.physics_scene = physics_scene; + } + return *(PhysicsScene*)scene.physics_scene.get(); + } + + struct RigidBody + { + std::shared_ptr physics_scene; + ShapeRefC shape; + BodyID bodyID; + Entity entity = INVALID_ENTITY; + + Mat44 additionalTransform = Mat44::sIdentity(); + Mat44 additionalTransformInverse = Mat44::sIdentity(); + + Mat44 restBasis = Mat44::sIdentity(); + Mat44 restBasisInverse = Mat44::sIdentity(); + + // for trace hit reporting: + wi::ecs::Entity humanoid_ragdoll_entity = wi::ecs::INVALID_ENTITY; + wi::scene::HumanoidComponent::HumanoidBone humanoid_bone = wi::scene::HumanoidComponent::HumanoidBone::Count; + + ~RigidBody() + { + if (physics_scene == nullptr || bodyID.IsInvalid()) + return; + BodyInterface& body_interface = ((PhysicsScene*)physics_scene.get())->physics_system.GetBodyInterface(); // locking version because destructor can be called from any thread + body_interface.RemoveBody(bodyID); + body_interface.DestroyBody(bodyID); + } + }; + struct SoftBody + { + std::shared_ptr physics_scene; + BodyID bodyID; + Entity entity = INVALID_ENTITY; + + SoftBodySharedSettings shared_settings; + Array simulation_normals; + + ~SoftBody() + { + if (physics_scene == nullptr) + return; + BodyInterface& body_interface = ((PhysicsScene*)physics_scene.get())->physics_system.GetBodyInterface(); // locking version because destructor can be called from any thread + body_interface.RemoveBody(bodyID); + body_interface.DestroyBody(bodyID); + } + }; + + RigidBody& GetRigidBody(wi::scene::RigidBodyPhysicsComponent& physicscomponent) + { + if (physicscomponent.physicsobject == nullptr) + { + physicscomponent.physicsobject = std::make_shared(); + } + return *(RigidBody*)physicscomponent.physicsobject.get(); + } + SoftBody& GetSoftBody(wi::scene::SoftBodyPhysicsComponent& physicscomponent) + { + if (physicscomponent.physicsobject == nullptr) + { + physicscomponent.physicsobject = std::make_shared(); + } + return *(SoftBody*)physicscomponent.physicsobject.get(); + } + + void AddRigidBody( + wi::scene::Scene& scene, + Entity entity, + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const wi::scene::TransformComponent& transform, + const wi::scene::MeshComponent* mesh + ) + { + ShapeSettings::ShapeResult shape_result; + + // The default convex radius caused issues when creating small box shape, etc, so I decrease it: + const float convexRadius = 0.001f; + + switch (physicscomponent.shape) + { + case RigidBodyPhysicsComponent::CollisionShape::BOX: + { + BoxShapeSettings settings(Vec3(physicscomponent.box.halfextents.x * transform.scale_local.x, physicscomponent.box.halfextents.y * transform.scale_local.y, physicscomponent.box.halfextents.z * transform.scale_local.z), convexRadius); + settings.SetEmbedded(); + shape_result = settings.Create(); + } + break; + case RigidBodyPhysicsComponent::CollisionShape::SPHERE: + { + SphereShapeSettings settings(physicscomponent.sphere.radius * transform.scale_local.x); + settings.SetEmbedded(); + shape_result = settings.Create(); + } + break; + case RigidBodyPhysicsComponent::CollisionShape::CAPSULE: + { + CapsuleShapeSettings settings(physicscomponent.capsule.height * transform.scale_local.y, physicscomponent.capsule.radius * transform.scale_local.x); + settings.SetEmbedded(); + shape_result = settings.Create(); + } + break; + case RigidBodyPhysicsComponent::CollisionShape::CYLINDER: + { + CylinderShapeSettings settings(physicscomponent.capsule.height * transform.scale_local.y, physicscomponent.capsule.radius * transform.scale_local.x, convexRadius); + settings.SetEmbedded(); + shape_result = settings.Create(); + } + break; + + case RigidBodyPhysicsComponent::CollisionShape::CONVEX_HULL: + if (mesh != nullptr) + { + Array points; + points.reserve(mesh->vertex_positions.size()); + for (auto& pos : mesh->vertex_positions) + { + points.push_back(Vec3(pos.x * transform.scale_local.x, pos.y * transform.scale_local.y, pos.z * transform.scale_local.z)); + } + ConvexHullShapeSettings settings(points, convexRadius); + settings.SetEmbedded(); + shape_result = settings.Create(); + } + else + { + wi::backlog::post("Convex Hull physics requested, but no MeshComponent provided!", wi::backlog::LogLevel::Error); + assert(0); + } + break; + + case RigidBodyPhysicsComponent::CollisionShape::TRIANGLE_MESH: + if (mesh != nullptr) + { + TriangleList trianglelist; + + uint32_t first_subset = 0; + uint32_t last_subset = 0; + mesh->GetLODSubsetRange(physicscomponent.mesh_lod, first_subset, last_subset); + for (uint32_t subsetIndex = first_subset; subsetIndex < last_subset; ++subsetIndex) + { + const MeshComponent::MeshSubset& subset = mesh->subsets[subsetIndex]; + const uint32_t* indices = mesh->indices.data() + subset.indexOffset; + for (uint32_t i = 0; i < subset.indexCount; i += 3) + { + Triangle triangle; + triangle.mMaterialIndex = 0; + triangle.mV[0] = Float3(mesh->vertex_positions[indices[i + 0]].x * transform.scale_local.x, mesh->vertex_positions[indices[i + 0]].y * transform.scale_local.y, mesh->vertex_positions[indices[i + 0]].z * transform.scale_local.z); + triangle.mV[2] = Float3(mesh->vertex_positions[indices[i + 1]].x * transform.scale_local.x, mesh->vertex_positions[indices[i + 1]].y * transform.scale_local.y, mesh->vertex_positions[indices[i + 1]].z * transform.scale_local.z); + triangle.mV[1] = Float3(mesh->vertex_positions[indices[i + 2]].x * transform.scale_local.x, mesh->vertex_positions[indices[i + 2]].y * transform.scale_local.y, mesh->vertex_positions[indices[i + 2]].z * transform.scale_local.z); + trianglelist.push_back(triangle); + } + } + + MeshShapeSettings settings(trianglelist); + settings.SetEmbedded(); + shape_result = settings.Create(); + } + else + { + wi::backlog::post("Triangle Mesh physics requested, but no MeshComponent provided!", wi::backlog::LogLevel::Error); + assert(0); + } + break; + } + + if (!shape_result.IsValid()) + { + physicscomponent.physicsobject = nullptr; + wi::backlog::post("AddRigidBody failed: shape couldn't be created!", wi::backlog::LogLevel::Error); + return; + } + else + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + physicsobject.physics_scene = scene.physics_scene; + physicsobject.entity = entity; + PhysicsScene& physics_scene = GetPhysicsScene(scene); + + physicsobject.shape = shape_result.Get(); + + Mat44 mat = cast(transform.world); + Vec3 local_offset = cast(physicscomponent.local_offset); + + physicsobject.additionalTransform.SetTranslation(local_offset); + physicsobject.additionalTransformInverse = physicsobject.additionalTransform.Inversed(); + + const EMotionType motionType = physicscomponent.IsKinematic() ? EMotionType::Kinematic : (physicscomponent.mass == 0 ? EMotionType::Static : EMotionType::Dynamic); + + BodyCreationSettings settings( + physicsobject.shape.GetPtr(), + local_offset + mat.GetTranslation(), + mat.GetQuaternion().Normalized(), + motionType, + Layers::MOVING + ); + settings.mRestitution = physicscomponent.restitution; + settings.mFriction = physicscomponent.friction; + settings.mLinearDamping = physicscomponent.damping_linear; + settings.mAngularDamping = physicscomponent.damping_angular; + settings.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia; + settings.mMassPropertiesOverride.mMass = physicscomponent.mass; + settings.mAllowSleeping = !physicscomponent.IsDisableDeactivation(); + settings.mUserData = (uint64_t)&physicsobject; + + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterface(); // locking version because this is called from job system! + + physicsobject.bodyID = body_interface.CreateAndAddBody(settings, EActivation::Activate); + if (physicsobject.bodyID.IsInvalid()) + { + physicscomponent.physicsobject = nullptr; + wi::backlog::post("AddRigidBody failed: body couldn't be created!", wi::backlog::LogLevel::Error); + return; + } + + if (motionType == EMotionType::Dynamic) + { + // We must detach dynamic objects, because their physics object is created in world space + // and attachment would apply double transformation to the transform + scene.locker.lock(); + scene.Component_Detach(entity); + scene.locker.unlock(); + } + } + } + void AddSoftBody( + wi::scene::Scene& scene, + Entity entity, + wi::scene::SoftBodyPhysicsComponent& physicscomponent, + const wi::scene::MeshComponent& mesh + ) + { + SoftBody& physicsobject = GetSoftBody(physicscomponent); + physicsobject.physics_scene = scene.physics_scene; + physicsobject.entity = entity; + physicscomponent.CreateFromMesh(mesh); + PhysicsScene& physics_scene = GetPhysicsScene(scene); + + physicsobject.shared_settings.SetEmbedded(); + + XMMATRIX worldMatrix = XMLoadFloat4x4(&physicscomponent.worldMatrix); + + const size_t vertexCount = physicscomponent.physicsToGraphicsVertexMapping.size(); + physicsobject.shared_settings.mVertices.resize(vertexCount); + for (size_t i = 0; i < vertexCount; ++i) + { + uint32_t graphicsInd = physicscomponent.physicsToGraphicsVertexMapping[i]; + + XMFLOAT3 position = mesh.vertex_positions[graphicsInd]; + XMVECTOR P = XMLoadFloat3(&position); + P = XMVector3Transform(P, worldMatrix); + XMStoreFloat3(&position, P); + physicsobject.shared_settings.mVertices[i].mPosition = Float3(position.x, position.y, position.z); + + float weight = physicscomponent.weights[i]; + physicsobject.shared_settings.mVertices[i].mInvMass = weight == 0 ? 0 : 1.0f / weight; + } + + uint32_t first_subset = 0; + uint32_t last_subset = 0; + mesh.GetLODSubsetRange(0, first_subset, last_subset); + for (uint32_t subsetIndex = first_subset; subsetIndex < last_subset; ++subsetIndex) + { + const MeshComponent::MeshSubset& subset = mesh.subsets[subsetIndex]; + const uint32_t* indices = mesh.indices.data() + subset.indexOffset; + for (uint32_t i = 0; i < subset.indexCount; i += 3) + { + SoftBodySharedSettings::Face& face = physicsobject.shared_settings.mFaces.emplace_back(); + face.mVertex[0] = physicscomponent.graphicsToPhysicsVertexMapping[indices[i + 0]]; + face.mVertex[2] = physicscomponent.graphicsToPhysicsVertexMapping[indices[i + 1]]; + face.mVertex[1] = physicscomponent.graphicsToPhysicsVertexMapping[indices[i + 2]]; + } + } + + SoftBodySharedSettings::VertexAttributes vertexAttributes = { 1.0e-5f, 1.0e-5f, 1.0e-5f }; + physicsobject.shared_settings.CreateConstraints(&vertexAttributes, 1); + + physicsobject.shared_settings.Optimize(); + + SoftBodyCreationSettings settings(&physicsobject.shared_settings, Vec3::sZero(), Quat::sIdentity(), Layers::MOVING); + settings.mNumIterations = (uint32)softbodyIterationCount; + settings.mFriction = physicscomponent.friction; + settings.mRestitution = physicscomponent.restitution; + settings.mUpdatePosition = false; + settings.mAllowSleeping = false; + settings.mUserData = (uint64_t)&physicsobject; + + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterface(); // locking version because this is called from job system! + + physicsobject.bodyID = body_interface.CreateAndAddSoftBody(settings, EActivation::Activate); + + if (physicsobject.bodyID.IsInvalid()) + { + physicscomponent.physicsobject = nullptr; + wi::backlog::post("AddSoftBody failed: body couldn't be created!", wi::backlog::LogLevel::Error); + return; + } + + physicsobject.simulation_normals.resize(physicsobject.shared_settings.mVertices.size()); + } + + struct Ragdoll + { + enum BODYPART + { + BODYPART_PELVIS = 0, + BODYPART_SPINE, + BODYPART_HEAD, + + BODYPART_LEFT_UPPER_LEG, + BODYPART_LEFT_LOWER_LEG, + + BODYPART_RIGHT_UPPER_LEG, + BODYPART_RIGHT_LOWER_LEG, + + BODYPART_LEFT_UPPER_ARM, + BODYPART_LEFT_LOWER_ARM, + + BODYPART_RIGHT_UPPER_ARM, + BODYPART_RIGHT_LOWER_ARM, + + BODYPART_COUNT + }; + + std::shared_ptr physics_scene; + std::shared_ptr rigidbodies[BODYPART_COUNT]; + Skeleton skeleton; + RagdollSettings settings; + Ref ragdoll; + bool state_active = false; + Entity saved_parents[BODYPART_COUNT] = {}; + float scale = 1; + + Ragdoll(Scene& scene, HumanoidComponent& humanoid, Entity humanoidEntity, float scale) + { + physics_scene = scene.physics_scene; + PhysicsSystem& physics_system = ((PhysicsScene*)physics_scene.get())->physics_system; + BodyInterface& body_interface = physics_system.GetBodyInterface(); // locking version because this is called from job system! + + float masses[BODYPART_COUNT] = {}; + Vec3 positions[BODYPART_COUNT] = {}; + Vec3 constraint_positions[BODYPART_COUNT] = {}; + Mat44 final_transforms[BODYPART_COUNT] = {}; +#if 0 + // slow speed and visualizer to aid debugging: + wi::renderer::SetGameSpeed(0.1f); + SetDebugDrawEnabled(true); +#endif + + // Detect which way humanoid is facing in rest pose: + const float facing = scene.GetHumanoidDefaultFacing(humanoid, humanoidEntity); + + // Whole ragdoll will take a uniform scaling: + const XMMATRIX scaleMatrix = XMMatrixScaling(scale, scale, scale); + this->scale = scale; + + // Calculate the bone lengths and radiuses in armature local space and create rigid bodies for bones: + for (int c = 0; c < BODYPART_COUNT; ++c) + { + HumanoidComponent::HumanoidBone humanoid_bone = HumanoidComponent::HumanoidBone::Count; + Entity entityA = INVALID_ENTITY; + Entity entityB = INVALID_ENTITY; + switch (c) + { + case BODYPART_PELVIS: + humanoid_bone = HumanoidComponent::HumanoidBone::Hips; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Hips]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Spine]; + break; + case BODYPART_SPINE: + humanoid_bone = HumanoidComponent::HumanoidBone::Spine; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Spine]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Neck]; // prefer neck instead of head + if (entityB == INVALID_ENTITY) + { + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Head]; // fall back to head if neck not available + } + break; + case BODYPART_HEAD: + humanoid_bone = HumanoidComponent::HumanoidBone::Neck; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Neck]; // prefer neck instead of head + if (entityA == INVALID_ENTITY) + { + humanoid_bone = HumanoidComponent::HumanoidBone::Head; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::Head]; // fall back to head if neck not available + } + break; + case BODYPART_LEFT_UPPER_LEG: + humanoid_bone = HumanoidComponent::HumanoidBone::LeftUpperLeg; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftUpperLeg]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerLeg]; + break; + case BODYPART_LEFT_LOWER_LEG: + humanoid_bone = HumanoidComponent::HumanoidBone::LeftLowerLeg; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerLeg]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftFoot]; + break; + case BODYPART_RIGHT_UPPER_LEG: + humanoid_bone = HumanoidComponent::HumanoidBone::RightUpperLeg; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightUpperLeg]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerLeg]; + break; + case BODYPART_RIGHT_LOWER_LEG: + humanoid_bone = HumanoidComponent::HumanoidBone::RightLowerLeg; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerLeg]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightFoot]; + break; + case BODYPART_LEFT_UPPER_ARM: + humanoid_bone = HumanoidComponent::HumanoidBone::LeftUpperArm; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftUpperArm]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerArm]; + break; + case BODYPART_LEFT_LOWER_ARM: + humanoid_bone = HumanoidComponent::HumanoidBone::LeftLowerArm; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftLowerArm]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::LeftHand]; + break; + case BODYPART_RIGHT_UPPER_ARM: + humanoid_bone = HumanoidComponent::HumanoidBone::RightUpperArm; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightUpperArm]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerArm]; + break; + case BODYPART_RIGHT_LOWER_ARM: + humanoid_bone = HumanoidComponent::HumanoidBone::RightLowerArm; + entityA = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightLowerArm]; + entityB = humanoid.bones[(size_t)HumanoidComponent::HumanoidBone::RightHand]; + break; + } + assert(entityA != INVALID_ENTITY); + + // Calculations here will be done in armature local space. + // Unfortunately since humanoid can be separate from armature, we use a "find" utility to find bone rest matrix in armature + // Note that current scaling of character is applied here separately from rest pose + XMMATRIX restA = scene.GetRestPose(entityA) * scaleMatrix; + XMMATRIX restB = scene.GetRestPose(entityB) * scaleMatrix; + XMVECTOR rootA = restA.r[3]; + XMVECTOR rootB = restB.r[3]; + + // Every bone will be a rigid body: + rigidbodies[c] = std::make_unique(); + RigidBody& physicsobject = *rigidbodies[c]; + physicsobject.entity = entityA; + + float mass = scale; + float capsule_height = scale; + float capsule_radius = scale * humanoid.ragdoll_fatness; + + if (c == BODYPART_HEAD) + { + // Head doesn't necessarily have a child, so make up something reasonable: + capsule_height = 0.05f * scale; + capsule_radius = 0.1f * scale * humanoid.ragdoll_headsize; + } + else + { + // bone length: + XMVECTOR len = XMVector3Length(XMVectorSubtract(rootB, rootA)); + capsule_height = XMVectorGetX(len); + + // capsule radius and length is tweaked per body part: + switch (c) + { + case BODYPART_PELVIS: + capsule_radius = 0.1f * scale * humanoid.ragdoll_fatness; + break; + case BODYPART_SPINE: + capsule_radius = 0.1f * scale * humanoid.ragdoll_fatness; + capsule_height -= capsule_radius * 2; + break; + case BODYPART_LEFT_LOWER_ARM: + case BODYPART_RIGHT_LOWER_ARM: + capsule_radius = capsule_height * 0.15f * humanoid.ragdoll_fatness; + capsule_height += capsule_radius; + break; + case BODYPART_LEFT_UPPER_LEG: + case BODYPART_RIGHT_UPPER_LEG: + capsule_radius = capsule_height * 0.15f * humanoid.ragdoll_fatness; + capsule_height -= capsule_radius * 2; + break; + case BODYPART_LEFT_LOWER_LEG: + case BODYPART_RIGHT_LOWER_LEG: + capsule_radius = capsule_height * 0.15f * humanoid.ragdoll_fatness; + capsule_height -= capsule_radius; + break; + default: + capsule_radius = capsule_height * 0.2f * humanoid.ragdoll_fatness; + capsule_height -= capsule_radius * 2; + break; + } + } + + ShapeSettings::ShapeResult shape_result; + CapsuleShapeSettings shape_settings(capsule_height * 0.5f, capsule_radius); + shape_settings.SetEmbedded(); + shape_result = shape_settings.Create(); + + RotatedTranslatedShapeSettings rtshape_settings; + rtshape_settings.SetEmbedded(); + rtshape_settings.mInnerShapePtr = shape_result.Get(); + rtshape_settings.mPosition = Vec3::sZero(); + rtshape_settings.mRotation = Quat::sIdentity(); + + switch (c) + { + case BODYPART_LEFT_UPPER_ARM: + case BODYPART_LEFT_LOWER_ARM: + case BODYPART_RIGHT_UPPER_ARM: + case BODYPART_RIGHT_LOWER_ARM: + rtshape_settings.mRotation = Quat::sRotation(Vec3::sAxisZ(), 0.5f * JPH_PI).Normalized(); + break; + default: + break; + } + + shape_result = rtshape_settings.Create(); + physicsobject.shape = shape_result.Get(); + + // capsule offset on axis is performed because otherwise capsule center would be in the bone root position + // which is not what we want. Instead the bone is moved on its axis so it resides between root and tail + const float offset = capsule_height * 0.5f + capsule_radius; + + Vec3 local_offset = Vec3::sZero(); + switch (c) + { + case BODYPART_PELVIS: + break; + case BODYPART_SPINE: + case BODYPART_HEAD: + local_offset = Vec3(0, offset, 0); + break; + case BODYPART_LEFT_UPPER_LEG: + case BODYPART_LEFT_LOWER_LEG: + case BODYPART_RIGHT_UPPER_LEG: + case BODYPART_RIGHT_LOWER_LEG: + local_offset = Vec3(0, -offset, 0); + break; + case BODYPART_LEFT_UPPER_ARM: + case BODYPART_LEFT_LOWER_ARM: + local_offset = Vec3(-offset * facing, 0, 0); + break; + case BODYPART_RIGHT_UPPER_ARM: + case BODYPART_RIGHT_LOWER_ARM: + local_offset = Vec3(offset * facing, 0, 0); + break; + default: + break; + } + + physicsobject.additionalTransform.SetTranslation(local_offset); + physicsobject.additionalTransformInverse = physicsobject.additionalTransform.Inversed(); + + // Get the translation and rotation part of rest matrix: + XMVECTOR SCA = {}; + XMVECTOR ROT = {}; + XMVECTOR TRA = {}; + XMMatrixDecompose(&SCA, &ROT, &TRA, restA); + XMFLOAT4 rot = {}; + XMFLOAT3 tra = {}; + XMStoreFloat4(&rot, ROT); + XMStoreFloat3(&tra, TRA); + + Vec3 root = cast(tra); + + Mat44 mat = Mat44::sTranslation(root); + mat = mat * physicsobject.additionalTransform; + + physicsobject.restBasis = Mat44::sRotation(cast(rot)); + physicsobject.restBasisInverse = physicsobject.restBasis.Inversed(); + + physicsobject.humanoid_ragdoll_entity = humanoidEntity; + physicsobject.humanoid_bone = humanoid_bone; + + physicsobject.physics_scene = scene.physics_scene; + + masses[c] = mass; + positions[c] = mat.GetTranslation(); + constraint_positions[c] = root; + final_transforms[c] = Mat44::sTranslation(cast(tra)) * Mat44::sRotation(cast(rot)); + final_transforms[c] = final_transforms[c] * physicsobject.restBasisInverse; + final_transforms[c] = final_transforms[c] * physicsobject.additionalTransform; + } + + // For constraint setup, see examples in Jolt/Samples/Utils/RagdollLoader.cpp + + skeleton.SetEmbedded(); + + uint bodyparts[BODYPART_COUNT] = {}; + bodyparts[BODYPART_PELVIS] = skeleton.AddJoint("LowerBody"); + bodyparts[BODYPART_SPINE] = skeleton.AddJoint("UpperBody", bodyparts[BODYPART_PELVIS]); + bodyparts[BODYPART_HEAD] = skeleton.AddJoint("Head", bodyparts[BODYPART_SPINE]); + bodyparts[BODYPART_LEFT_UPPER_LEG] = skeleton.AddJoint("UpperLegL", bodyparts[BODYPART_PELVIS]); + bodyparts[BODYPART_LEFT_LOWER_LEG] = skeleton.AddJoint("LowerLegL", bodyparts[BODYPART_LEFT_UPPER_LEG]); + bodyparts[BODYPART_RIGHT_UPPER_LEG] = skeleton.AddJoint("UpperLegR", bodyparts[BODYPART_PELVIS]); + bodyparts[BODYPART_RIGHT_LOWER_LEG] = skeleton.AddJoint("LowerLegR", bodyparts[BODYPART_RIGHT_UPPER_LEG]); + bodyparts[BODYPART_LEFT_UPPER_ARM] = skeleton.AddJoint("UpperArmL", bodyparts[BODYPART_SPINE]); + bodyparts[BODYPART_LEFT_LOWER_ARM] = skeleton.AddJoint("LowerArmL", bodyparts[BODYPART_LEFT_UPPER_ARM]); + bodyparts[BODYPART_RIGHT_UPPER_ARM] = skeleton.AddJoint("UpperArmR", bodyparts[BODYPART_SPINE]); + bodyparts[BODYPART_RIGHT_LOWER_ARM] = skeleton.AddJoint("LowerArmR", bodyparts[BODYPART_RIGHT_UPPER_ARM]); + + // Constraint limits + const float twist_angle[] = { + 0.0f, // Lower Body (unused, there's no parent) + 5.0f, // Upper Body + 90.0f, // Head + 45.0f, // Upper Leg L + 45.0f, // Lower Leg L + 45.0f, // Upper Leg R + 45.0f, // Lower Leg R + 45.0f, // Upper Arm L + 45.0f, // Lower Arm L + 45.0f, // Upper Arm R + 45.0f, // Lower Arm R + }; + + const float normal_angle[] = { + 0.0f, // Lower Body (unused, there's no parent) + 40.0f, // Upper Body + 45.0f, // Head + 45.0f, // Upper Leg L + 0.0f, // Lower Leg L + 45.0f, // Upper Leg R + 0.0f, // Lower Leg R + 90.0f, // Upper Arm L + 0.0f, // Lower Arm L + 90.0f, // Upper Arm R + 0.0f, // Lower Arm R + }; + + const float plane_angle[] = { + 0.0f, // Lower Body (unused, there's no parent) + 40.0f, // Upper Body + 45.0f, // Head + 45.0f, // Upper Leg L + 60.0f, // Lower Leg L (cheating here, a knee is not symmetric, we should have rotated the twist axis) + 45.0f, // Upper Leg R + 60.0f, // Lower Leg R + 45.0f, // Upper Arm L + 90.0f, // Lower Arm L + 45.0f, // Upper Arm R + 90.0f, // Lower Arm R + }; + + static float constraint_dbg = 0.1f; + static bool fixpose = false; // enable to fix the pose to rest pose, useful for debugging + + settings.SetEmbedded(); + settings.mSkeleton = &skeleton; + settings.mParts.resize(skeleton.GetJointCount()); + for (int p = 0; p < skeleton.GetJointCount(); ++p) + { + RagdollSettings::Part& part = settings.mParts[p]; + part.SetShape(rigidbodies[p]->shape); + part.mPosition = positions[p]; + part.mRotation = Quat::sIdentity(); + part.mMotionType = EMotionType::Kinematic; + part.mObjectLayer = Layers::MOVING; + part.mOverrideMassProperties = EOverrideMassProperties::CalculateInertia; + part.mMassPropertiesOverride.mMass = masses[p]; + + // First part is the root, doesn't have a parent and doesn't have a constraint + if (p > 0) + { + if (p == BODYPART_LEFT_LOWER_LEG || p == BODYPART_RIGHT_LOWER_LEG) + { + Ref constraint = new HingeConstraintSettings; + constraint->mDrawConstraintSize = constraint_dbg; + constraint->mPoint1 = constraint->mPoint2 = constraint_positions[p]; + constraint->mHingeAxis1 = constraint->mHingeAxis2 = Vec3::sAxisX() * facing; + constraint->mNormalAxis1 = constraint->mNormalAxis2 = -Vec3::sAxisY(); + if (fixpose) + { + constraint->mLimitsMin = constraint->mLimitsMax = 0; + } + else + { + constraint->mLimitsMin = 0; + constraint->mLimitsMax = JPH_PI * 0.8f; + } + part.mToParent = constraint; + } + else if (p == BODYPART_LEFT_LOWER_ARM) + { + Ref constraint = new HingeConstraintSettings; + constraint->mDrawConstraintSize = constraint_dbg; + constraint->mPoint1 = constraint->mPoint2 = constraint_positions[p]; + constraint->mHingeAxis1 = constraint->mHingeAxis2 = Vec3::sAxisY(); + constraint->mNormalAxis1 = constraint->mNormalAxis2 = (constraint_positions[p] - constraint_positions[p - 1]).Normalized(); + if (fixpose) + { + constraint->mLimitsMin = constraint->mLimitsMax = 0; + } + else + { + constraint->mLimitsMin = 0; + constraint->mLimitsMax = JPH_PI * 0.6f; + } + part.mToParent = constraint; + } + else if (p == BODYPART_RIGHT_LOWER_ARM) + { + Ref constraint = new HingeConstraintSettings; + constraint->mDrawConstraintSize = constraint_dbg; + constraint->mPoint1 = constraint->mPoint2 = constraint_positions[p]; + constraint->mHingeAxis1 = constraint->mHingeAxis2 = -Vec3::sAxisY(); + constraint->mNormalAxis1 = constraint->mNormalAxis2 = (constraint_positions[p] - constraint_positions[p - 1]).Normalized(); + if (fixpose) + { + constraint->mLimitsMin = constraint->mLimitsMax = 0; + } + else + { + constraint->mLimitsMin = 0; + constraint->mLimitsMax = JPH_PI * 0.6f; + } + part.mToParent = constraint; + } + else + { + Ref constraint = new SwingTwistConstraintSettings; + constraint->mDrawConstraintSize = constraint_dbg; + constraint->mPosition1 = constraint->mPosition2 = constraint_positions[p]; + constraint->mTwistAxis1 = constraint->mTwistAxis2 = (positions[p] - constraint_positions[p]).Normalized(); + constraint->mPlaneAxis1 = constraint->mPlaneAxis2 = Vec3::sAxisZ() * facing; + if (fixpose) + { + constraint->mTwistMinAngle = constraint->mTwistMaxAngle = 0; + constraint->mNormalHalfConeAngle = 0; + constraint->mPlaneHalfConeAngle = 0; + } + else + { + constraint->mTwistMinAngle = -DegreesToRadians(twist_angle[p]); + constraint->mTwistMaxAngle = DegreesToRadians(twist_angle[p]); + constraint->mNormalHalfConeAngle = DegreesToRadians(normal_angle[p]); + constraint->mPlaneHalfConeAngle = DegreesToRadians(plane_angle[p]); + } + part.mToParent = constraint; + } + } + } + + settings.Stabilize(); + settings.DisableParentChildCollisions(); + settings.CalculateBodyIndexToConstraintIndex(); + + ragdoll = settings.CreateRagdoll(0, 0, &physics_system); + ragdoll->SetPose(Vec3::sZero(), final_transforms); + ragdoll->AddToPhysicsSystem(EActivation::Activate); + + const int count = (int)ragdoll->GetBodyCount(); + for (int index = 0; index < count; ++index) + { + rigidbodies[index]->bodyID = ragdoll->GetBodyID(index); + body_interface.SetUserData(rigidbodies[index]->bodyID, (uint64_t)rigidbodies[index].get()); + } + } + ~Ragdoll() + { + if (physics_scene == nullptr) + return; + PhysicsSystem& physics_system = ((PhysicsScene*)physics_scene.get())->physics_system; + + const int count = (int)ragdoll->GetBodyCount(); + for (int index = 0; index < count; ++index) + { + rigidbodies[index]->bodyID = {}; // will be removed by ragdoll + } + + ragdoll->RemoveFromPhysicsSystem(); + } + + // Activates ragdoll as dynamic physics object: + void Activate( + Scene& scene, + Entity humanoidEntity + ) + { + if (state_active) + return; + state_active = true; + + const HumanoidComponent* humanoid = scene.humanoids.GetComponent(humanoidEntity); + if (humanoid == nullptr) + return; + + PhysicsSystem& physics_system = ((PhysicsScene*)physics_scene.get())->physics_system; + BodyInterface& body_interface = physics_system.GetBodyInterface(); // locking version because this is called from job system! + + int c = 0; + for (auto& x : rigidbodies) + { + body_interface.SetMotionType(x->bodyID, EMotionType::Dynamic, EActivation::Activate); + + // Save parenting information to be able to restore it: + const HierarchyComponent* hier = scene.hierarchy.GetComponent(x->entity); + if (hier != nullptr) + { + saved_parents[c] = hier->parentID; + } + else + { + saved_parents[c] = INVALID_ENTITY; + } + + // detach bone because it will be simulated in world space: + scene.Component_Detach(x->entity); + + c++; + } + + // Stop all anims that are children of humanoid: + for (size_t i = 0; i < scene.animations.GetCount(); ++i) + { + Entity entity = scene.animations.GetEntity(i); + if (!scene.Entity_IsDescendant(entity, humanoidEntity)) + continue; + AnimationComponent& animation = scene.animations[i]; + animation.Stop(); + } + } + + // Disables dynamic ragdoll and reattaches loose parts as they were: + void Deactivate( + Scene& scene + ) + { + if (!state_active) + return; + state_active = false; + + PhysicsSystem& physics_system = ((PhysicsScene*)physics_scene.get())->physics_system; + BodyInterface& body_interface = physics_system.GetBodyInterface(); // locking version because this is called from job system! + + int c = 0; + for (auto& x : rigidbodies) + { + body_interface.SetMotionType(x->bodyID, EMotionType::Kinematic, EActivation::Activate); + + if (saved_parents[c] != INVALID_ENTITY) + { + scene.Component_Attach(x->entity, saved_parents[c]); + } + c++; + } + } + }; + + } + using namespace jolt; + + void Initialize() + { + wi::Timer timer; + + RegisterDefaultAllocator(); + + Factory::sInstance = new Factory(); + + RegisterTypes(); + + char text[256] = {}; + snprintf(text, arraysize(text), "wi::physics Initialized [Jolt Physics %d.%d.%d] (%d ms)", JPH_VERSION_MAJOR, JPH_VERSION_MINOR, JPH_VERSION_PATCH, (int)std::round(timer.elapsed())); + wi::backlog::post(text); + } + + bool IsEnabled() { return ENABLED; } + void SetEnabled(bool value) { ENABLED = value; } + + bool IsSimulationEnabled() { return ENABLED && SIMULATION_ENABLED; } + void SetSimulationEnabled(bool value) { SIMULATION_ENABLED = value; } + + bool IsDebugDrawEnabled() { return DEBUGDRAW_ENABLED; } + void SetDebugDrawEnabled(bool value) { DEBUGDRAW_ENABLED = value; } + + int GetAccuracy() { return ACCURACY; } + void SetAccuracy(int value) { ACCURACY = value; } + + float GetFrameRate() { return 1.0f / TIMESTEP; } + void SetFrameRate(float value) { TIMESTEP = 1.0f / value; } + + void RunPhysicsUpdateSystem( + wi::jobsystem::context& ctx, + wi::scene::Scene& scene, + float dt + ) + { + if (!IsEnabled() || dt <= 0) + return; + + wi::jobsystem::Wait(ctx); + + auto range = wi::profiler::BeginRangeCPU("Physics"); + + PhysicsScene& physics_scene = GetPhysicsScene(scene); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + + physics_scene.physics_system.SetGravity(cast(scene.weather.gravity)); + const Vec3 wind = cast(scene.weather.windDirection); + const bool has_wind = !wind.IsNearZero(); + + // System will register rigidbodies to objects: + wi::jobsystem::Dispatch(ctx, (uint32_t)scene.rigidbodies.GetCount(), 64, [&](wi::jobsystem::JobArgs args) { + + RigidBodyPhysicsComponent& physicscomponent = scene.rigidbodies[args.jobIndex]; + Entity entity = scene.rigidbodies.GetEntity(args.jobIndex); + + if (physicscomponent.physicsobject == nullptr && scene.transforms.Contains(entity)) + { + TransformComponent& transform = *scene.transforms.GetComponent(entity); + const ObjectComponent* object = scene.objects.GetComponent(entity); + const MeshComponent* mesh = nullptr; + if (object != nullptr) + { + mesh = scene.meshes.GetComponent(object->meshID); + } + AddRigidBody(scene, entity, physicscomponent, transform, mesh); + } + + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + if (physicsobject.bodyID.IsInvalid()) + return; + + body_interface.SetFriction(physicsobject.bodyID, physicscomponent.friction); + body_interface.SetRestitution(physicsobject.bodyID, physicscomponent.restitution); + + const EMotionType prevMotionType = body_interface.GetMotionType(physicsobject.bodyID); + const EMotionType requiredMotionType = physicscomponent.IsKinematic() ? EMotionType::Kinematic : (physicscomponent.mass == 0 ? EMotionType::Static : EMotionType::Dynamic); + + if (prevMotionType != requiredMotionType) + { + // Changed motion type: + body_interface.SetMotionType(physicsobject.bodyID, requiredMotionType, EActivation::Activate); + + if (requiredMotionType == EMotionType::Dynamic) + { + // We must detach dynamic objects, because their physics object is created in world space + // and attachment would apply double transformation to the transform + scene.locker.lock(); + scene.Component_Detach(entity); + scene.locker.unlock(); + } + } + + if (physicscomponent.IsKinematic()) + { + TransformComponent& transform = *scene.transforms.GetComponent(entity); + + body_interface.MoveKinematic( + physicsobject.bodyID, + cast(transform.GetPosition()), + cast(transform.GetRotation()), + dt + ); + } + } + }); + + // System will register softbodies to meshes and update physics engine state: + wi::jobsystem::Dispatch(ctx, (uint32_t)scene.softbodies.GetCount(), 1, [&](wi::jobsystem::JobArgs args) { + + SoftBodyPhysicsComponent& physicscomponent = scene.softbodies[args.jobIndex]; + Entity entity = scene.softbodies.GetEntity(args.jobIndex); + if (!scene.meshes.Contains(entity)) + return; + MeshComponent& mesh = *scene.meshes.GetComponent(entity); + const ArmatureComponent* armature = mesh.IsSkinned() ? scene.armatures.GetComponent(mesh.armatureID) : nullptr; + mesh.SetDynamic(true); + + if (physicscomponent._flags & SoftBodyPhysicsComponent::FORCE_RESET) + { + physicscomponent._flags &= ~SoftBodyPhysicsComponent::FORCE_RESET; + physicscomponent.physicsobject = nullptr; + } + if (physicscomponent._flags & SoftBodyPhysicsComponent::SAFE_TO_REGISTER && physicscomponent.physicsobject == nullptr) + { + AddSoftBody(scene, entity, physicscomponent, mesh); + } + + if (physicscomponent.physicsobject != nullptr) + { + SoftBody& physicsobject = GetSoftBody(physicscomponent); + if (physicsobject.bodyID.IsInvalid()) + return; + + body_interface.SetFriction(physicsobject.bodyID, physicscomponent.friction); + body_interface.SetRestitution(physicsobject.bodyID, physicscomponent.restitution); + + // Add wind: + if (has_wind) + { + body_interface.AddForce(physicsobject.bodyID, wind, EActivation::Activate); + } + + // This is different from rigid bodies, because soft body is a per mesh component (no TransformComponent). World matrix is propagated down from single mesh instance (ObjectUpdateSystem). + XMMATRIX worldMatrix = XMLoadFloat4x4(&physicscomponent.worldMatrix); + + BodyLockRead lock(physics_scene.physics_system.GetBodyLockInterfaceNoLock(), physicsobject.bodyID); + if (!lock.Succeeded()) + return; + const Body& body = lock.GetBody(); + SoftBodyMotionProperties* motion = (SoftBodyMotionProperties*)body.GetMotionProperties(); + + // System controls zero weight soft body nodes: + for (size_t ind = 0; ind < physicscomponent.weights.size(); ++ind) + { + float weight = physicscomponent.weights[ind]; + + if (weight == 0) + { + uint32_t graphicsInd = physicscomponent.physicsToGraphicsVertexMapping[ind]; + XMFLOAT3 position = mesh.vertex_positions[graphicsInd]; + XMVECTOR P = armature == nullptr ? XMLoadFloat3(&position) : wi::scene::SkinVertex(mesh, *armature, graphicsInd); + P = XMVector3Transform(P, worldMatrix); + XMStoreFloat3(&position, P); + + SoftBodyMotionProperties::Vertex& node = motion->GetVertex((uint)ind); + node.mPosition = cast(position); + } + } + } + }); + + // Ragdoll management: + for (size_t i = 0; i < scene.humanoids.GetCount(); ++i) + { + HumanoidComponent& humanoid = scene.humanoids[i]; + Entity humanoidEntity = scene.humanoids.GetEntity(i); + float scale = 1; + if (scene.transforms.Contains(humanoidEntity)) + { + scale = scene.transforms.GetComponent(humanoidEntity)->scale_local.x; + } + if (humanoid.ragdoll != nullptr) + { + Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); + if (!wi::math::float_equal(ragdoll.scale, scale)) + { + humanoid.SetRagdollPhysicsEnabled(false); // while scaling ragdoll, it will be kinematic + ragdoll.Deactivate(scene); // recreate attached skeleton hierarchy structure + humanoid.ragdoll = {}; // delete ragdoll if scale changed, it will be recreated + } + } + if (humanoid.ragdoll == nullptr) + { + humanoid.ragdoll = std::make_shared(scene, humanoid, humanoidEntity, scale); + } + Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); + if (humanoid.IsRagdollPhysicsEnabled()) + { + ragdoll.Activate(scene, humanoidEntity); + } + else + { + ragdoll.Deactivate(scene); + + for (auto& rb : ragdoll.rigidbodies) + { + TransformComponent& transform = *scene.transforms.GetComponent(rb->entity); + + XMVECTOR SCA = {}; + XMVECTOR ROT = {}; + XMVECTOR TRA = {}; + XMMatrixDecompose(&SCA, &ROT, &TRA, XMLoadFloat4x4(&transform.world)); + XMFLOAT4 rot = {}; + XMFLOAT3 tra = {}; + XMStoreFloat4(&rot, ROT); + XMStoreFloat3(&tra, TRA); + + Mat44 m = Mat44::sTranslation(cast(tra)) * Mat44::sRotation(cast(rot)); + m = m * rb->restBasisInverse; + m = m * rb->additionalTransform; + + body_interface.MoveKinematic( + rb->bodyID, + m.GetTranslation(), + m.GetQuaternion().Normalized(), + dt + ); + } + } + } + + wi::jobsystem::Wait(ctx); + + //physics_scene.physics_system.OptimizeBroadPhase(); + + // Perform internal simulation step: + if (IsSimulationEnabled()) + { + static TempAllocatorImpl temp_allocator(10 * 1024 * 1024); + static JobSystemThreadPool job_system(cMaxPhysicsJobs, cMaxPhysicsBarriers, thread::hardware_concurrency() - 1); + const int steps = ::clamp(int(dt / TIMESTEP), 1, ACCURACY); + physics_scene.physics_system.Update(dt, steps, &temp_allocator, &job_system); + } + + // Feedback physics objects to system: + wi::jobsystem::Dispatch(ctx, (uint32_t)scene.rigidbodies.GetCount(), 64, [&](wi::jobsystem::JobArgs args) { + + RigidBodyPhysicsComponent& physicscomponent = scene.rigidbodies[args.jobIndex]; + if (physicscomponent.physicsobject == nullptr || physicscomponent.IsKinematic()) + return; + + Entity entity = scene.rigidbodies.GetEntity(args.jobIndex); + TransformComponent& transform = *scene.transforms.GetComponent(entity); + + RigidBody& physicsobject = GetRigidBody(physicscomponent); + if (physicsobject.bodyID.IsInvalid()) + return; + + RMat44 world = body_interface.GetWorldTransform(physicsobject.bodyID); + world = world * physicsobject.additionalTransformInverse; + RVec3 position = world.GetTranslation(); + Quat rotation = world.GetQuaternion(); + + transform.translation_local = XMFLOAT3(position.GetX(), position.GetY(), position.GetZ()); + transform.rotation_local = XMFLOAT4(rotation.GetX(), rotation.GetY(), rotation.GetZ(), rotation.GetW()); + transform.SetDirty(); + }); + + wi::jobsystem::Dispatch(ctx, (uint32_t)scene.softbodies.GetCount(), 1, [&](wi::jobsystem::JobArgs args) { + + SoftBodyPhysicsComponent& physicscomponent = scene.softbodies[args.jobIndex]; + if (physicscomponent.physicsobject == nullptr) + return; + + Entity entity = scene.softbodies.GetEntity(args.jobIndex); + + SoftBody& physicsobject = GetSoftBody(physicscomponent); + if (physicsobject.bodyID.IsInvalid()) + return; + + BodyLockRead lock(physics_scene.physics_system.GetBodyLockInterfaceNoLock(), physicsobject.bodyID); + if (!lock.Succeeded()) + return; + + const Body& body = lock.GetBody(); + + MeshComponent& mesh = *scene.meshes.GetComponent(entity); + + physicscomponent.aabb = wi::primitive::AABB(); + + const SoftBodyMotionProperties* motion = (const SoftBodyMotionProperties*)body.GetMotionProperties(); + const Array& soft_vertices = motion->GetVertices(); + const Array& soft_faces = motion->GetFaces(); + + // Recompute normals: (Note: normalization will happen on final storage) + for (auto& n : physicsobject.simulation_normals) + { + n = Vec3::sZero(); + } + for (auto& f : soft_faces) + { + Vec3 x1 = soft_vertices[f.mVertex[0]].mPosition; + Vec3 x2 = soft_vertices[f.mVertex[1]].mPosition; + Vec3 x3 = soft_vertices[f.mVertex[2]].mPosition; + Vec3 n = (x2 - x1).Cross(x3 - x1); + physicsobject.simulation_normals[f.mVertex[0]] += n; + physicsobject.simulation_normals[f.mVertex[1]] += n; + physicsobject.simulation_normals[f.mVertex[2]] += n; + } + + // Soft body simulation nodes will update graphics mesh: + for (size_t ind = 0; ind < mesh.vertex_positions.size(); ++ind) + { + uint32_t physicsInd = physicscomponent.graphicsToPhysicsVertexMapping[ind]; + + const XMFLOAT3 position = cast(soft_vertices[physicsInd].mPosition); + const XMFLOAT3 normal = cast(physicsobject.simulation_normals[physicsInd]); + + physicscomponent.vertex_positions_simulation[ind].FromFULL(position); + physicscomponent.vertex_normals_simulation[ind].FromFULL(normal); // normalizes internally + + physicscomponent.aabb._min = wi::math::Min(physicscomponent.aabb._min, position); + physicscomponent.aabb._max = wi::math::Max(physicscomponent.aabb._max, position); + } + + // Update tangent vectors: + if (!mesh.vertex_uvset_0.empty() && !physicscomponent.vertex_normals_simulation.empty()) + { + uint32_t first_subset = 0; + uint32_t last_subset = 0; + mesh.GetLODSubsetRange(0, first_subset, last_subset); + for (uint32_t subsetIndex = first_subset; subsetIndex < last_subset; ++subsetIndex) + { + const MeshComponent::MeshSubset& subset = mesh.subsets[subsetIndex]; + for (size_t i = 0; i < subset.indexCount; i += 3) + { + const uint32_t i0 = mesh.indices[i + 0]; + const uint32_t i1 = mesh.indices[i + 1]; + const uint32_t i2 = mesh.indices[i + 2]; + + const XMFLOAT3 v0 = physicscomponent.vertex_positions_simulation[i0].GetPOS(); + const XMFLOAT3 v1 = physicscomponent.vertex_positions_simulation[i1].GetPOS(); + const XMFLOAT3 v2 = physicscomponent.vertex_positions_simulation[i2].GetPOS(); + + const XMFLOAT2 u0 = mesh.vertex_uvset_0[i0]; + const XMFLOAT2 u1 = mesh.vertex_uvset_0[i1]; + const XMFLOAT2 u2 = mesh.vertex_uvset_0[i2]; + + const XMVECTOR nor0 = physicscomponent.vertex_normals_simulation[i0].LoadNOR(); + const XMVECTOR nor1 = physicscomponent.vertex_normals_simulation[i1].LoadNOR(); + const XMVECTOR nor2 = physicscomponent.vertex_normals_simulation[i2].LoadNOR(); + + const XMVECTOR facenormal = XMVector3Normalize(XMVectorAdd(XMVectorAdd(nor0, nor1), nor2)); + + const float x1 = v1.x - v0.x; + const float x2 = v2.x - v0.x; + const float y1 = v1.y - v0.y; + const float y2 = v2.y - v0.y; + const float z1 = v1.z - v0.z; + const float z2 = v2.z - v0.z; + + const float s1 = u1.x - u0.x; + const float s2 = u2.x - u0.x; + const float t1 = u1.y - u0.y; + const float t2 = u2.y - u0.y; + + const float r = 1.0f / (s1 * t2 - s2 * t1); + const XMVECTOR sdir = XMVectorSet((t2 * x1 - t1 * x2) * r, (t2 * y1 - t1 * y2) * r, + (t2 * z1 - t1 * z2) * r, 0); + const XMVECTOR tdir = XMVectorSet((s1 * x2 - s2 * x1) * r, (s1 * y2 - s2 * y1) * r, + (s1 * z2 - s2 * z1) * r, 0); + + XMVECTOR tangent; + tangent = XMVector3Normalize(XMVectorSubtract(sdir, XMVectorMultiply(facenormal, XMVector3Dot(facenormal, sdir)))); + float sign = XMVectorGetX(XMVector3Dot(XMVector3Cross(tangent, facenormal), tdir)) < 0.0f ? -1.0f : 1.0f; + + XMFLOAT3 t; + XMStoreFloat3(&t, tangent); + + physicscomponent.vertex_tangents_tmp[i0].x += t.x; + physicscomponent.vertex_tangents_tmp[i0].y += t.y; + physicscomponent.vertex_tangents_tmp[i0].z += t.z; + physicscomponent.vertex_tangents_tmp[i0].w = sign; + + physicscomponent.vertex_tangents_tmp[i1].x += t.x; + physicscomponent.vertex_tangents_tmp[i1].y += t.y; + physicscomponent.vertex_tangents_tmp[i1].z += t.z; + physicscomponent.vertex_tangents_tmp[i1].w = sign; + + physicscomponent.vertex_tangents_tmp[i2].x += t.x; + physicscomponent.vertex_tangents_tmp[i2].y += t.y; + physicscomponent.vertex_tangents_tmp[i2].z += t.z; + physicscomponent.vertex_tangents_tmp[i2].w = sign; + } + } + + for (size_t i = 0; i < physicscomponent.vertex_tangents_simulation.size(); ++i) + { + physicscomponent.vertex_tangents_simulation[i].FromFULL(physicscomponent.vertex_tangents_tmp[i]); + } + } + + mesh.aabb = physicscomponent.aabb; + }); + + for (size_t i = 0; i < scene.humanoids.GetCount(); ++i) + { + HumanoidComponent& humanoid = scene.humanoids[i]; + if (humanoid.ragdoll == nullptr) + continue; + Entity humanoidEntity = scene.humanoids.GetEntity(i); + Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); + if (humanoid.IsRagdollPhysicsEnabled()) + { + for (auto& rb : ragdoll.rigidbodies) + { + TransformComponent* transform = scene.transforms.GetComponent(rb->entity); + if (transform == nullptr) + continue; + Mat44 mat = body_interface.GetWorldTransform(rb->bodyID); + mat = mat * rb->additionalTransformInverse; + mat = mat * rb->restBasis; + transform->translation_local = cast(mat.GetTranslation()); + transform->rotation_local = cast(mat.GetQuaternion().Normalized()); + transform->SetDirty(); + } + } + } + +#ifdef JPH_DEBUG_RENDERER + if (IsDebugDrawEnabled()) + { + class JoltDebugRenderer : public DebugRendererSimple + { + void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor) override + { + wi::renderer::RenderableLine line; + line.start = XMFLOAT3(inFrom.GetX(), inFrom.GetY(), inFrom.GetZ()); + line.end = XMFLOAT3(inTo.GetX(), inTo.GetY(), inTo.GetZ()); + line.color_start = line.color_end = wi::Color(inColor.r, inColor.g, inColor.b, inColor.a); + wi::renderer::DrawLine(line); + } + void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow = ECastShadow::Off) override + { + // Not needed if we only want to draw wireframes + } + void DrawText3D(RVec3Arg inPosition, const string_view& inString, ColorArg inColor = JPH::Color::sWhite, float inHeight = 0.5f) override + { + wi::renderer::DebugTextParams params; + params.position.x = inPosition.GetX(); + params.position.y = inPosition.GetY(); + params.position.z = inPosition.GetZ(); + params.scaling = 0.6f; + params.flags |= wi::renderer::DebugTextParams::CAMERA_FACING; + params.flags |= wi::renderer::DebugTextParams::CAMERA_SCALING; + wi::renderer::DrawDebugText(inString.data(), params); + } + }; + static JoltDebugRenderer debug_renderer; + BodyManager::DrawSettings settings; + settings.mDrawCenterOfMassTransform = false; + settings.mDrawShape = true; + settings.mDrawSoftBodyVertices = true; + settings.mDrawShapeWireframe = true; + settings.mDrawShapeColor = BodyManager::EShapeColor::ShapeTypeColor; + physics_scene.physics_system.DrawBodies(settings, &debug_renderer); + physics_scene.physics_system.DrawConstraints(&debug_renderer); + } +#endif // JPH_DEBUG_RENDERER + + wi::profiler::EndRange(range); // Physics + } + + void SetLinearVelocity( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& velocity + ) + { + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + body_interface.SetLinearVelocity(physicsobject.bodyID, cast(velocity)); + } + } + void SetAngularVelocity( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& velocity + ) + { + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + body_interface.SetAngularVelocity(physicsobject.bodyID, cast(velocity)); + } + } + + void ApplyForce( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& force + ) + { + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + body_interface.AddForce(physicsobject.bodyID, cast(force)); + } + } + void ApplyForceAt( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& force, + const XMFLOAT3& at + ) + { + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + Vec3 at_world = body_interface.GetCenterOfMassTransform(physicsobject.bodyID).Inversed() * cast(at); + body_interface.AddForce(physicsobject.bodyID, cast(force), at_world); + } + } + + void ApplyImpulse( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& impulse + ) + { + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + body_interface.AddImpulse(physicsobject.bodyID, cast(impulse)); + } + } + void ApplyImpulse( + wi::scene::HumanoidComponent& humanoid, + wi::scene::HumanoidComponent::HumanoidBone bone, + const XMFLOAT3& impulse + ) + { + if (humanoid.ragdoll == nullptr) + return; + + Ragdoll::BODYPART bodypart = Ragdoll::BODYPART_COUNT; + switch (bone) + { + case HumanoidComponent::HumanoidBone::Hips: + bodypart = Ragdoll::BODYPART_PELVIS; + break; + case HumanoidComponent::HumanoidBone::Spine: + bodypart = Ragdoll::BODYPART_SPINE; + break; + case HumanoidComponent::HumanoidBone::Head: + case HumanoidComponent::HumanoidBone::Neck: + bodypart = Ragdoll::BODYPART_HEAD; + break; + case HumanoidComponent::HumanoidBone::RightUpperArm: + bodypart = Ragdoll::BODYPART_RIGHT_UPPER_ARM; + break; + case HumanoidComponent::HumanoidBone::RightLowerArm: + bodypart = Ragdoll::BODYPART_RIGHT_LOWER_ARM; + break; + case HumanoidComponent::HumanoidBone::LeftUpperArm: + bodypart = Ragdoll::BODYPART_LEFT_UPPER_ARM; + break; + case HumanoidComponent::HumanoidBone::LeftLowerArm: + bodypart = Ragdoll::BODYPART_LEFT_LOWER_ARM; + break; + case HumanoidComponent::HumanoidBone::RightUpperLeg: + bodypart = Ragdoll::BODYPART_RIGHT_UPPER_LEG; + break; + case HumanoidComponent::HumanoidBone::RightLowerLeg: + bodypart = Ragdoll::BODYPART_RIGHT_LOWER_LEG; + break; + case HumanoidComponent::HumanoidBone::LeftUpperLeg: + bodypart = Ragdoll::BODYPART_LEFT_UPPER_LEG; + break; + case HumanoidComponent::HumanoidBone::LeftLowerLeg: + bodypart = Ragdoll::BODYPART_LEFT_LOWER_LEG; + break; + } + if (bodypart == Ragdoll::BODYPART_COUNT) + return; + + Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); + if (ragdoll.rigidbodies[bodypart] == nullptr || ragdoll.rigidbodies[bodypart]->bodyID.IsInvalid()) + return; + RigidBody& physicsobject = *ragdoll.rigidbodies[bodypart]; + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + body_interface.SetMotionType(physicsobject.bodyID, EMotionType::Dynamic, EActivation::Activate); + body_interface.AddImpulse(physicsobject.bodyID, cast(impulse)); + } + void ApplyImpulseAt( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& impulse, + const XMFLOAT3& at + ) + { + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + Vec3 at_world = body_interface.GetCenterOfMassTransform(physicsobject.bodyID) * cast(at); + body_interface.AddImpulse(physicsobject.bodyID, cast(impulse), at_world); + } + } + void ApplyImpulseAt( + wi::scene::HumanoidComponent& humanoid, + wi::scene::HumanoidComponent::HumanoidBone bone, + const XMFLOAT3& impulse, + const XMFLOAT3& at + ) + { + if (humanoid.ragdoll == nullptr) + return; + + Ragdoll::BODYPART bodypart = Ragdoll::BODYPART_COUNT; + switch (bone) + { + case HumanoidComponent::HumanoidBone::Hips: + bodypart = Ragdoll::BODYPART_PELVIS; + break; + case HumanoidComponent::HumanoidBone::Spine: + bodypart = Ragdoll::BODYPART_SPINE; + break; + case HumanoidComponent::HumanoidBone::Head: + case HumanoidComponent::HumanoidBone::Neck: + bodypart = Ragdoll::BODYPART_HEAD; + break; + case HumanoidComponent::HumanoidBone::RightUpperArm: + bodypart = Ragdoll::BODYPART_RIGHT_UPPER_ARM; + break; + case HumanoidComponent::HumanoidBone::RightLowerArm: + bodypart = Ragdoll::BODYPART_RIGHT_LOWER_ARM; + break; + case HumanoidComponent::HumanoidBone::LeftUpperArm: + bodypart = Ragdoll::BODYPART_LEFT_UPPER_ARM; + break; + case HumanoidComponent::HumanoidBone::LeftLowerArm: + bodypart = Ragdoll::BODYPART_LEFT_LOWER_ARM; + break; + case HumanoidComponent::HumanoidBone::RightUpperLeg: + bodypart = Ragdoll::BODYPART_RIGHT_UPPER_LEG; + break; + case HumanoidComponent::HumanoidBone::RightLowerLeg: + bodypart = Ragdoll::BODYPART_RIGHT_LOWER_LEG; + break; + case HumanoidComponent::HumanoidBone::LeftUpperLeg: + bodypart = Ragdoll::BODYPART_LEFT_UPPER_LEG; + break; + case HumanoidComponent::HumanoidBone::LeftLowerLeg: + bodypart = Ragdoll::BODYPART_LEFT_LOWER_LEG; + break; + } + if (bodypart == Ragdoll::BODYPART_COUNT) + return; + + Ragdoll& ragdoll = *(Ragdoll*)humanoid.ragdoll.get(); + if (ragdoll.rigidbodies[bodypart] == nullptr || ragdoll.rigidbodies[bodypart]->bodyID.IsInvalid()) + return; + RigidBody& physicsobject = *ragdoll.rigidbodies[bodypart]; + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + Vec3 at_world = body_interface.GetCenterOfMassTransform(physicsobject.bodyID) * cast(at); + body_interface.SetMotionType(physicsobject.bodyID, EMotionType::Dynamic, EActivation::Activate); + body_interface.AddImpulse(physicsobject.bodyID, cast(impulse), at_world); + } + + void ApplyTorque( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& torque + ) + { + if (physicscomponent.physicsobject != nullptr) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + body_interface.AddTorque(physicsobject.bodyID, cast(torque), EActivation::Activate); + } + } + void ApplyTorqueImpulse( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + const XMFLOAT3& torque + ) + { + ApplyTorque(physicscomponent, torque); + } + + void SetActivationState( + wi::scene::RigidBodyPhysicsComponent& physicscomponent, + ActivationState state + ) + { + RigidBody& physicsobject = GetRigidBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + switch (state) + { + case wi::physics::ActivationState::Active: + body_interface.ActivateBody(physicsobject.bodyID); + break; + case wi::physics::ActivationState::Inactive: + body_interface.DeactivateBody(physicsobject.bodyID); + break; + default: + break; + } + } + void SetActivationState( + wi::scene::SoftBodyPhysicsComponent& physicscomponent, + ActivationState state + ) + { + SoftBody& physicsobject = GetSoftBody(physicscomponent); + PhysicsScene& physics_scene = *(PhysicsScene*)physicsobject.physics_scene.get(); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + switch (state) + { + case wi::physics::ActivationState::Active: + body_interface.ActivateBody(physicsobject.bodyID); + break; + case wi::physics::ActivationState::Inactive: + body_interface.DeactivateBody(physicsobject.bodyID); + break; + default: + break; + } + } + + RayIntersectionResult Intersects( + const wi::scene::Scene& scene, + wi::primitive::Ray ray + ) + { + RayIntersectionResult result; + if (scene.physics_scene == nullptr) + return result; + + PhysicsScene& physics_scene = *(PhysicsScene*)scene.physics_scene.get(); + + const float tmin = clamp(ray.TMin, 0.0f, 1000000.0f); + const float tmax = clamp(ray.TMax, 0.0f, 1000000.0f); + const float range = tmax - tmin; + + RRayCast inray{ + cast(ray.origin), + cast(ray.direction).Normalized() + }; + + inray.mOrigin = inray.mOrigin + inray.mDirection * tmin; + inray.mDirection = inray.mDirection * range; + + RayCastSettings settings; + settings.mBackFaceMode = EBackFaceMode::IgnoreBackFaces; + settings.mTreatConvexAsSolid = false; + + ClosestHitCollisionCollector collector; + + physics_scene.physics_system.GetNarrowPhaseQuery().CastRay(inray, settings, collector); + if (!collector.HadHit()) + return result; + + if (collector.mHit.mBodyID.IsInvalid()) + return result; + + BodyLockRead lock(physics_scene.physics_system.GetBodyLockInterfaceNoLock(), collector.mHit.mBodyID); + if (!lock.Succeeded()) + return result; + + const Body& body = lock.GetBody(); + const uint64_t userdata = body.GetUserData(); + + const Vec3 position = inray.GetPointOnRay(collector.mHit.mFraction); + const Vec3 position_local = body.GetCenterOfMassTransform().Inversed() * position; + const Vec3 normal = body.GetWorldSpaceSurfaceNormal(collector.mHit.mSubShapeID2, position); + + if (body.IsRigidBody()) + { + const RigidBody* physicsobject = (RigidBody*)userdata; + result.entity = physicsobject->entity; + result.position = cast(position); + result.position_local = cast(position_local); + result.normal = cast(normal); + result.physicsobject = &body; + result.humanoid_ragdoll_entity = physicsobject->humanoid_ragdoll_entity; + result.humanoid_bone = physicsobject->humanoid_bone; + } + else // soft body + { + const SoftBody* physicsobject = (SoftBody*)userdata; + result.entity = physicsobject->entity; + result.position = cast(position); + result.position_local = cast(position_local); + result.normal = cast(normal); + result.physicsobject = &body; + } + + return result; + } + + struct PickDragOperation_Jolt + { + std::shared_ptr physics_scene; + Ref constraint; + float pick_distance = 0; + Body* bodyA = nullptr; + Body* bodyB = nullptr; + ~PickDragOperation_Jolt() + { + if (physics_scene == nullptr) + return; + PhysicsScene& physics_scene = *((PhysicsScene*)this->physics_scene.get()); + physics_scene.physics_system.RemoveConstraint(constraint); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + body_interface.RemoveBody(bodyA->GetID()); + body_interface.DestroyBody(bodyA->GetID()); + } + }; + void PickDrag( + const wi::scene::Scene& scene, + wi::primitive::Ray ray, + PickDragOperation& op + ) + { + if (scene.physics_scene == nullptr) + return; + PhysicsScene& physics_scene = *((PhysicsScene*)scene.physics_scene.get()); + BodyInterface& body_interface = physics_scene.physics_system.GetBodyInterfaceNoLock(); + + if (op.IsValid()) + { + // Continue dragging: + PickDragOperation_Jolt* internal_state = (PickDragOperation_Jolt*)op.internal_state.get(); + const float dist = internal_state->pick_distance; + Vec3 pos = Vec3(ray.origin.x + ray.direction.x * dist, ray.origin.y + ray.direction.y * dist, ray.origin.z + ray.direction.z * dist); + body_interface.MoveKinematic(internal_state->bodyA->GetID(), pos, Quat::sIdentity(), scene.dt); + } + else + { + // Begin picking: + RayIntersectionResult result = Intersects(scene, ray); + if (!result.IsValid()) + return; + Body* body = (Body*)result.physicsobject; + if (!body->IsRigidBody()) + return; + + auto internal_state = std::make_shared(); + internal_state->physics_scene = scene.physics_scene; + internal_state->pick_distance = wi::math::Distance(ray.origin, result.position); + internal_state->bodyB = body; + + Vec3 pos = cast(result.position); + + internal_state->bodyA = body_interface.CreateBody(BodyCreationSettings(new SphereShape(0.01f), pos, Quat::sIdentity(), EMotionType::Kinematic, Layers::MOVING)); + body_interface.AddBody(internal_state->bodyA->GetID(), EActivation::Activate); + +#if 0 + DistanceConstraintSettings settings; + settings.SetEmbedded(); + settings.mPoint1 = settings.mPoint2 = pos; +#else + SixDOFConstraintSettings settings; + settings.SetEmbedded(); + settings.mPosition1 = settings.mPosition2 = pos; + for (int i = 0; i < SixDOFConstraintSettings::EAxis::Num; ++i) + { + settings.mLimitMin[i] = 0; + settings.mLimitMax[i] = 0; + } +#endif + + internal_state->constraint = settings.Create(*internal_state->bodyA, *internal_state->bodyB); + physics_scene.physics_system.AddConstraint(internal_state->constraint); + + op.internal_state = internal_state; + } + } + +} diff --git a/WickedEngine/wiScene.h b/WickedEngine/wiScene.h index d6b180e31..b498f559e 100644 --- a/WickedEngine/wiScene.h +++ b/WickedEngine/wiScene.h @@ -36,7 +36,7 @@ namespace wi::scene wi::ecs::ComponentManager& meshes = componentLibrary.Register("wi::scene::Scene::meshes", 2); // version = 2 wi::ecs::ComponentManager& impostors = componentLibrary.Register("wi::scene::Scene::impostors"); wi::ecs::ComponentManager& objects = componentLibrary.Register("wi::scene::Scene::objects", 3); // version = 3 - wi::ecs::ComponentManager& rigidbodies = componentLibrary.Register("wi::scene::Scene::rigidbodies", 2); // version = 2 + wi::ecs::ComponentManager& rigidbodies = componentLibrary.Register("wi::scene::Scene::rigidbodies", 3); // version = 3 wi::ecs::ComponentManager& softbodies = componentLibrary.Register("wi::scene::Scene::softbodies"); wi::ecs::ComponentManager& armatures = componentLibrary.Register("wi::scene::Scene::armatures"); wi::ecs::ComponentManager& lights = componentLibrary.Register("wi::scene::Scene::lights", 2); // version = 2 diff --git a/WickedEngine/wiScene_Components.h b/WickedEngine/wiScene_Components.h index 71a7a68ee..a83026a38 100644 --- a/WickedEngine/wiScene_Components.h +++ b/WickedEngine/wiScene_Components.h @@ -917,16 +917,16 @@ namespace wi::scene }; CollisionShape shape; float mass = 1.0f; - float friction = 0.5f; - float restitution = 0.0f; - float damping_linear = 0.0f; - float damping_angular = 0.0f; + float friction = 0.2f; + float restitution = 0.1f; + float damping_linear = 0.05f; + float damping_angular = 0.05f; XMFLOAT3 local_offset = XMFLOAT3(0, 0, 0); struct BoxParams { XMFLOAT3 halfextents = XMFLOAT3(1, 1, 1); - } box; // also cylinder params + } box; struct SphereParams { float radius = 1; @@ -935,7 +935,7 @@ namespace wi::scene { float radius = 1; float height = 1; - } capsule; + } capsule; // also cylinder params // This will force LOD level for rigid body if it is a TRIANGLE_MESH shape: // The geometry for LOD level will be taken from MeshComponent. diff --git a/WickedEngine/wiScene_Serializers.cpp b/WickedEngine/wiScene_Serializers.cpp index 4eb2f4ec6..926469c0c 100644 --- a/WickedEngine/wiScene_Serializers.cpp +++ b/WickedEngine/wiScene_Serializers.cpp @@ -706,6 +706,12 @@ namespace wi::scene { archive >> local_offset; } + if (shape == CollisionShape::CYLINDER && seri.GetVersion() < 3) + { + // convert old assumption that was used in Bullet Physics: + capsule.height = box.halfextents.y; + capsule.radius = box.halfextents.x; + } } else { diff --git a/WickedEngine/wiVersion.cpp b/WickedEngine/wiVersion.cpp index ecdd2fd77..81230038d 100644 --- a/WickedEngine/wiVersion.cpp +++ b/WickedEngine/wiVersion.cpp @@ -9,7 +9,7 @@ namespace wi::version // minor features, major updates, breaking compatibility changes const int minor = 71; // minor bug fixes, alterations, refactors, updates - const int revision = 490; + const int revision = 491; const std::string version_string = std::to_string(major) + "." + std::to_string(minor) + "." + std::to_string(revision); diff --git a/third_party_software.txt b/third_party_software.txt index f1a9f76d0..e97e1c144 100644 --- a/third_party_software.txt +++ b/third_party_software.txt @@ -19,23 +19,19 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SO ############################################################################################################################### -BULLET 2.82: +Jolt Physics: -The MIT License (MIT) +Copyright 2021 Jorrit Rouwe -Bullet Collision Detection and Physics Library -Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), +to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, +and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use -this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ###############################################################################################################################