From e4c114762986a24bbe16453ba74bad934fe2cede Mon Sep 17 00:00:00 2001 From: turanszkij Date: Thu, 18 Jun 2015 23:03:51 +0200 Subject: [PATCH] Created --- .gitattributes | 63 + .gitignore | 189 + WickedEngine.sln | 20 + WickedEngine/BULLET.cpp | 862 ++++ WickedEngine/BULLET.h | 78 + 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 + .../BULLET/BulletCollision/CMakeLists.txt | 286 ++ .../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 + .../BULLET/BulletCollision/premake4.lua | 11 + .../BULLET/BulletDynamics/CMakeLists.txt | 153 + .../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 + .../BULLET/BulletDynamics/premake4.lua | 11 + .../BULLET/BulletSoftBody/CMakeLists.txt | 67 + .../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 ++ .../BULLET/BulletSoftBody/premake4.lua | 11 + WickedEngine/BULLET/LinearMath/CMakeLists.txt | 72 + 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 | 731 +++ .../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/LinearMath/premake4.lua | 11 + WickedEngine/BULLET/btBulletCollisionCommon.h | 68 + WickedEngine/BULLET/btBulletDynamicsCommon.h | 51 + WickedEngine/BackLog.cpp | 143 + WickedEngine/BackLog.h | 47 + WickedEngine/Camera.cpp | 115 + WickedEngine/Camera.h | 31 + WickedEngine/Character.cpp | 1744 +++++++ WickedEngine/Character.h | 598 +++ WickedEngine/Client.cpp | 47 + WickedEngine/Client.h | 143 + WickedEngine/CpuInfo.cpp | 92 + WickedEngine/CpuInfo.h | 21 + WickedEngine/Cube.cpp | 89 + WickedEngine/Cube.h | 48 + WickedEngine/DepthTarget.cpp | 119 + WickedEngine/DepthTarget.h | 19 + WickedEngine/DirectInput.cpp | 284 ++ WickedEngine/DirectInput.h | 43 + WickedEngine/EmittedParticle.cpp | 567 +++ WickedEngine/EmittedParticle.h | 115 + WickedEngine/Entity.h | 18 + WickedEngine/Font.cpp | 458 ++ WickedEngine/Font.h | 105 + WickedEngine/FrameRate.cpp | 31 + WickedEngine/FrameRate.h | 19 + WickedEngine/Frustum.cpp | 126 + WickedEngine/Frustum.h | 27 + WickedEngine/HairParticle.cpp | 509 ++ WickedEngine/HairParticle.h | 70 + WickedEngine/Hitbox2D.cpp | 16 + WickedEngine/Hitbox2D.h | 14 + WickedEngine/Image.cpp | 853 ++++ WickedEngine/Image.h | 123 + WickedEngine/ImageEffects.h | 135 + WickedEngine/InputManager.cpp | 154 + WickedEngine/InputManager.h | 52 + WickedEngine/LensFlare.cpp | 227 + WickedEngine/LensFlare.h | 35 + WickedEngine/Level.cpp | 68 + WickedEngine/Level.h | 13 + WickedEngine/Lines.cpp | 88 + WickedEngine/Lines.h | 46 + WickedEngine/Network.cpp | 56 + WickedEngine/Network.h | 86 + WickedEngine/PHYSICS.h | 63 + WickedEngine/Particle.cpp | 5 + WickedEngine/Particle.h | 23 + WickedEngine/RenderTarget.cpp | 205 + WickedEngine/RenderTarget.h | 36 + WickedEngine/Renderer.cpp | 4282 +++++++++++++++++ WickedEngine/Renderer.h | 1028 ++++ WickedEngine/Requirements.cpp | 156 + WickedEngine/Resource.h | 15 + WickedEngine/ResourceManager.cpp | 150 + WickedEngine/ResourceManager.h | 35 + WickedEngine/SPTree.cpp | 249 + WickedEngine/SPTree.h | 82 + WickedEngine/ScriptEngine.cpp | 509 ++ WickedEngine/Server.cpp | 138 + WickedEngine/Server.h | 154 + WickedEngine/Sound.cpp | 314 ++ WickedEngine/Sound.h | 107 + WickedEngine/TaskThread.h | 87 + WickedEngine/Timer.cpp | 43 + WickedEngine/Timer.h | 21 + WickedEngine/Utility/DDSTextureLoader.cpp | 1457 ++++++ WickedEngine/Utility/DDSTextureLoader.h | 90 + WickedEngine/Utility/PlatformHelpers.h | 128 + WickedEngine/Utility/ScreenGrab.cpp | 1076 +++++ WickedEngine/Utility/ScreenGrab.h | 53 + WickedEngine/Utility/WicTextureLoader.cpp | 692 +++ WickedEngine/Utility/WicTextureLoader.h | 55 + WickedEngine/Utility/dds.h | 227 + WickedEngine/Utility/pch.cpp | 14 + WickedEngine/Utility/pch.h | 63 + WickedEngine/VariableManager.cpp | 34 + WickedEngine/VariableManager.h | 27 + WickedEngine/WickedEngine.h | 95 + WickedEngine/WickedEngine.vcxproj | 541 +++ WickedEngine/WickedEngine.vcxproj.filters | 1378 ++++++ WickedEngine/WickedHelper.cpp | 12 + WickedEngine/WickedHelper.h | 15 + WickedEngine/WickedLoader.cpp | 2292 +++++++++ WickedEngine/WickedLoader.h | 888 ++++ WickedEngine/WickedMath.cpp | 109 + WickedEngine/WickedMath.h | 26 + WickedEngine/XInput.cpp | 58 + WickedEngine/XInput.h | 39 + WickedEngine/Xaudio2_7/XAudio2.h | 1282 +++++ WickedEngine/Xaudio2_7/XAudio2fx.h | 431 ++ WickedEngine/Xaudio2_7/audiodefs.h | 263 + WickedEngine/Xaudio2_7/comdecl.h | 59 + WickedEngine/Xaudio2_7/xma2defs.h | 718 +++ WickedEngine/mysql/lib32/libmysql.dll | Bin 0 -> 4468224 bytes WickedEngine/mysql/lib32/libmysql.lib | Bin 0 -> 25958 bytes WickedEngine/mysql/lib64/libmysql.dll | Bin 0 -> 4712448 bytes WickedEngine/mysql/lib64/libmysql.lib | Bin 0 -> 24322 bytes WickedEngine/mysql/my_alloc.h | 60 + WickedEngine/mysql/my_list.h | 45 + WickedEngine/mysql/mysql.h | 739 +++ WickedEngine/mysql/mysql_com.h | 602 +++ WickedEngine/mysql/mysql_time.h | 55 + WickedEngine/mysql/mysql_version.h | 31 + WickedEngine/oImage.cpp | 119 + WickedEngine/oImage.h | 66 + WickedEngine/skinningDEF.h | 6 + WickedEngine/targetver.h | 11 + WickedEngine/typelib.h | 54 + features.txt | 56 + io_export_wicked_wi_bin.py | 651 +++ readme.txt | 28 + 476 files changed, 137108 insertions(+) create mode 100644 .gitattributes create mode 100644 .gitignore create mode 100644 WickedEngine.sln create mode 100644 WickedEngine/BULLET.cpp create mode 100644 WickedEngine/BULLET.h create mode 100644 WickedEngine/BULLET/Bullet-C-Api.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.h create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h create mode 100644 WickedEngine/BULLET/BulletCollision/CMakeLists.txt create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionConfiguration.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionMargin.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMaterial.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleBuffer.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleBuffer.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleInfoMap.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMesh.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMesh.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/Doxyfile create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btBoxCollision.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btClipPolygon.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btCompoundFromGimpact.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactMassUtil.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btGeometryOperations.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btQuantization.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_array.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_basic_geometry_operations.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_bitset.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_collision.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_clip_polygon.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_geom_types.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_geometry.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_hash_table.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_linear_math.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_math.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_radixsort.h create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPointCollector.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp create mode 100644 WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h create mode 100644 WickedEngine/BULLET/BulletCollision/premake4.lua create mode 100644 WickedEngine/BULLET/BulletDynamics/CMakeLists.txt create mode 100644 WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConstraintSolver.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactSolverInfo.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btJacobianEntry.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverBody.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/Bullet-C-API.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btActionInterface.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btDynamicsWorld.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyLink.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.h create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigSolver.h create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolver.h create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btPATHSolver.h create mode 100644 WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btVehicleRaycaster.h create mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.cpp create mode 100644 WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.h create mode 100644 WickedEngine/BULLET/BulletDynamics/premake4.lua create mode 100644 WickedEngine/BULLET/BulletSoftBody/CMakeLists.txt create mode 100644 WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBody.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBody.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyData.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyInternals.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodySolverVertexBuffer.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftBodySolvers.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/btSparseSDF.h create mode 100644 WickedEngine/BULLET/BulletSoftBody/premake4.lua create mode 100644 WickedEngine/BULLET/LinearMath/CMakeLists.txt create mode 100644 WickedEngine/BULLET/LinearMath/btAabbUtil2.h create mode 100644 WickedEngine/BULLET/LinearMath/btAlignedAllocator.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btAlignedAllocator.h create mode 100644 WickedEngine/BULLET/LinearMath/btAlignedObjectArray.h create mode 100644 WickedEngine/BULLET/LinearMath/btConvexHull.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btConvexHull.h create mode 100644 WickedEngine/BULLET/LinearMath/btConvexHullComputer.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btConvexHullComputer.h create mode 100644 WickedEngine/BULLET/LinearMath/btDefaultMotionState.h create mode 100644 WickedEngine/BULLET/LinearMath/btGeometryUtil.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btGeometryUtil.h create mode 100644 WickedEngine/BULLET/LinearMath/btGrahamScan2dConvexHull.h create mode 100644 WickedEngine/BULLET/LinearMath/btHashMap.h create mode 100644 WickedEngine/BULLET/LinearMath/btIDebugDraw.h create mode 100644 WickedEngine/BULLET/LinearMath/btList.h create mode 100644 WickedEngine/BULLET/LinearMath/btMatrix3x3.h create mode 100644 WickedEngine/BULLET/LinearMath/btMatrixX.h create mode 100644 WickedEngine/BULLET/LinearMath/btMinMax.h create mode 100644 WickedEngine/BULLET/LinearMath/btMotionState.h create mode 100644 WickedEngine/BULLET/LinearMath/btPolarDecomposition.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btPolarDecomposition.h create mode 100644 WickedEngine/BULLET/LinearMath/btPoolAllocator.h create mode 100644 WickedEngine/BULLET/LinearMath/btQuadWord.h create mode 100644 WickedEngine/BULLET/LinearMath/btQuaternion.h create mode 100644 WickedEngine/BULLET/LinearMath/btQuickprof.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btQuickprof.h create mode 100644 WickedEngine/BULLET/LinearMath/btRandom.h create mode 100644 WickedEngine/BULLET/LinearMath/btScalar.h create mode 100644 WickedEngine/BULLET/LinearMath/btSerializer.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btSerializer.h create mode 100644 WickedEngine/BULLET/LinearMath/btStackAlloc.h create mode 100644 WickedEngine/BULLET/LinearMath/btTransform.h create mode 100644 WickedEngine/BULLET/LinearMath/btTransformUtil.h create mode 100644 WickedEngine/BULLET/LinearMath/btVector3.cpp create mode 100644 WickedEngine/BULLET/LinearMath/btVector3.h create mode 100644 WickedEngine/BULLET/LinearMath/premake4.lua create mode 100644 WickedEngine/BULLET/btBulletCollisionCommon.h create mode 100644 WickedEngine/BULLET/btBulletDynamicsCommon.h create mode 100644 WickedEngine/BackLog.cpp create mode 100644 WickedEngine/BackLog.h create mode 100644 WickedEngine/Camera.cpp create mode 100644 WickedEngine/Camera.h create mode 100644 WickedEngine/Character.cpp create mode 100644 WickedEngine/Character.h create mode 100644 WickedEngine/Client.cpp create mode 100644 WickedEngine/Client.h create mode 100644 WickedEngine/CpuInfo.cpp create mode 100644 WickedEngine/CpuInfo.h create mode 100644 WickedEngine/Cube.cpp create mode 100644 WickedEngine/Cube.h create mode 100644 WickedEngine/DepthTarget.cpp create mode 100644 WickedEngine/DepthTarget.h create mode 100644 WickedEngine/DirectInput.cpp create mode 100644 WickedEngine/DirectInput.h create mode 100644 WickedEngine/EmittedParticle.cpp create mode 100644 WickedEngine/EmittedParticle.h create mode 100644 WickedEngine/Entity.h create mode 100644 WickedEngine/Font.cpp create mode 100644 WickedEngine/Font.h create mode 100644 WickedEngine/FrameRate.cpp create mode 100644 WickedEngine/FrameRate.h create mode 100644 WickedEngine/Frustum.cpp create mode 100644 WickedEngine/Frustum.h create mode 100644 WickedEngine/HairParticle.cpp create mode 100644 WickedEngine/HairParticle.h create mode 100644 WickedEngine/Hitbox2D.cpp create mode 100644 WickedEngine/Hitbox2D.h create mode 100644 WickedEngine/Image.cpp create mode 100644 WickedEngine/Image.h create mode 100644 WickedEngine/ImageEffects.h create mode 100644 WickedEngine/InputManager.cpp create mode 100644 WickedEngine/InputManager.h create mode 100644 WickedEngine/LensFlare.cpp create mode 100644 WickedEngine/LensFlare.h create mode 100644 WickedEngine/Level.cpp create mode 100644 WickedEngine/Level.h create mode 100644 WickedEngine/Lines.cpp create mode 100644 WickedEngine/Lines.h create mode 100644 WickedEngine/Network.cpp create mode 100644 WickedEngine/Network.h create mode 100644 WickedEngine/PHYSICS.h create mode 100644 WickedEngine/Particle.cpp create mode 100644 WickedEngine/Particle.h create mode 100644 WickedEngine/RenderTarget.cpp create mode 100644 WickedEngine/RenderTarget.h create mode 100644 WickedEngine/Renderer.cpp create mode 100644 WickedEngine/Renderer.h create mode 100644 WickedEngine/Requirements.cpp create mode 100644 WickedEngine/Resource.h create mode 100644 WickedEngine/ResourceManager.cpp create mode 100644 WickedEngine/ResourceManager.h create mode 100644 WickedEngine/SPTree.cpp create mode 100644 WickedEngine/SPTree.h create mode 100644 WickedEngine/ScriptEngine.cpp create mode 100644 WickedEngine/Server.cpp create mode 100644 WickedEngine/Server.h create mode 100644 WickedEngine/Sound.cpp create mode 100644 WickedEngine/Sound.h create mode 100644 WickedEngine/TaskThread.h create mode 100644 WickedEngine/Timer.cpp create mode 100644 WickedEngine/Timer.h create mode 100644 WickedEngine/Utility/DDSTextureLoader.cpp create mode 100644 WickedEngine/Utility/DDSTextureLoader.h create mode 100644 WickedEngine/Utility/PlatformHelpers.h create mode 100644 WickedEngine/Utility/ScreenGrab.cpp create mode 100644 WickedEngine/Utility/ScreenGrab.h create mode 100644 WickedEngine/Utility/WicTextureLoader.cpp create mode 100644 WickedEngine/Utility/WicTextureLoader.h create mode 100644 WickedEngine/Utility/dds.h create mode 100644 WickedEngine/Utility/pch.cpp create mode 100644 WickedEngine/Utility/pch.h create mode 100644 WickedEngine/VariableManager.cpp create mode 100644 WickedEngine/VariableManager.h create mode 100644 WickedEngine/WickedEngine.h create mode 100644 WickedEngine/WickedEngine.vcxproj create mode 100644 WickedEngine/WickedEngine.vcxproj.filters create mode 100644 WickedEngine/WickedHelper.cpp create mode 100644 WickedEngine/WickedHelper.h create mode 100644 WickedEngine/WickedLoader.cpp create mode 100644 WickedEngine/WickedLoader.h create mode 100644 WickedEngine/WickedMath.cpp create mode 100644 WickedEngine/WickedMath.h create mode 100644 WickedEngine/XInput.cpp create mode 100644 WickedEngine/XInput.h create mode 100644 WickedEngine/Xaudio2_7/XAudio2.h create mode 100644 WickedEngine/Xaudio2_7/XAudio2fx.h create mode 100644 WickedEngine/Xaudio2_7/audiodefs.h create mode 100644 WickedEngine/Xaudio2_7/comdecl.h create mode 100644 WickedEngine/Xaudio2_7/xma2defs.h create mode 100644 WickedEngine/mysql/lib32/libmysql.dll create mode 100644 WickedEngine/mysql/lib32/libmysql.lib create mode 100644 WickedEngine/mysql/lib64/libmysql.dll create mode 100644 WickedEngine/mysql/lib64/libmysql.lib create mode 100644 WickedEngine/mysql/my_alloc.h create mode 100644 WickedEngine/mysql/my_list.h create mode 100644 WickedEngine/mysql/mysql.h create mode 100644 WickedEngine/mysql/mysql_com.h create mode 100644 WickedEngine/mysql/mysql_time.h create mode 100644 WickedEngine/mysql/mysql_version.h create mode 100644 WickedEngine/oImage.cpp create mode 100644 WickedEngine/oImage.h create mode 100644 WickedEngine/skinningDEF.h create mode 100644 WickedEngine/targetver.h create mode 100644 WickedEngine/typelib.h create mode 100644 features.txt create mode 100644 io_export_wicked_wi_bin.py create mode 100644 readme.txt diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 000000000..1ff0c4230 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,63 @@ +############################################################################### +# Set default behavior to automatically normalize line endings. +############################################################################### +* text=auto + +############################################################################### +# Set default behavior for command prompt diff. +# +# This is need for earlier builds of msysgit that does not have it on by +# default for csharp files. +# Note: This is only used by command line +############################################################################### +#*.cs diff=csharp + +############################################################################### +# Set the merge driver for project and solution files +# +# Merging from the command prompt will add diff markers to the files if there +# are conflicts (Merging from VS is not affected by the settings below, in VS +# the diff markers are never inserted). Diff markers may cause the following +# file extensions to fail to load in VS. An alternative would be to treat +# these files as binary and thus will always conflict and require user +# intervention with every merge. To do so, just uncomment the entries below +############################################################################### +#*.sln merge=binary +#*.csproj merge=binary +#*.vbproj merge=binary +#*.vcxproj merge=binary +#*.vcproj merge=binary +#*.dbproj merge=binary +#*.fsproj merge=binary +#*.lsproj merge=binary +#*.wixproj merge=binary +#*.modelproj merge=binary +#*.sqlproj merge=binary +#*.wwaproj merge=binary + +############################################################################### +# behavior for image files +# +# image files are treated as binary by default. +############################################################################### +#*.jpg binary +#*.png binary +#*.gif binary + +############################################################################### +# diff behavior for common document formats +# +# Convert binary document formats to text before diffing them. This feature +# is only available from the command line. Turn it on by uncommenting the +# entries below. +############################################################################### +#*.doc diff=astextplain +#*.DOC diff=astextplain +#*.docx diff=astextplain +#*.DOCX diff=astextplain +#*.dot diff=astextplain +#*.DOT diff=astextplain +#*.pdf diff=astextplain +#*.PDF diff=astextplain +#*.rtf diff=astextplain +#*.RTF diff=astextplain diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..796453635 --- /dev/null +++ b/.gitignore @@ -0,0 +1,189 @@ +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. + +# User-specific files +*.suo +*.user +*.sln.docstates + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +x64/ +build/ +bld/ +[Bb]in/ +[Oo]bj/ + +# Roslyn cache directories +*.ide/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +#NUNIT +*.VisualState.xml +TestResult.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +*_i.c +*_p.c +*_i.h +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*.log +*.vspscc +*.vssscc +.builds +*.pidb +*.svclog +*.scc + +# Chutzpah Test files +_Chutzpah* + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opensdf +*.sdf +*.cachefile + +# Visual Studio profiler +*.psess +*.vsp +*.vspx + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# JustCode is a .NET coding addin-in +.JustCode + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# NCrunch +_NCrunch_* +.*crunch*.local.xml + +# MightyMoose +*.mm.* +AutoTest.Net/ + +# Web workbench (sass) +.sass-cache/ + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.[Pp]ublish.xml +*.azurePubxml +## TODO: Comment the next line if you want to checkin your +## web deploy settings but do note that will include unencrypted +## passwords +#*.pubxml + +# NuGet Packages Directory +packages/* +## TODO: If the tool you use requires repositories.config +## uncomment the next line +#!packages/repositories.config + +# Enable "build/" folder in the NuGet Packages folder since +# NuGet packages use it for MSBuild targets. +# This line needs to be after the ignore of the build folder +# (and the packages folder if the line above has been uncommented) +!packages/build/ + +# Windows Azure Build Output +csx/ +*.build.csdef + +# Windows Store app package directory +AppPackages/ + +# Others +sql/ +*.Cache +ClientBin/ +[Ss]tyle[Cc]op.* +~$* +*~ +*.dbmdl +*.dbproj.schemaview +*.pfx +*.publishsettings +node_modules/ + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file +# to a newer Visual Studio version. Backup files are not needed, +# because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm + +# SQL Server files +*.mdf +*.ldf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings + +# Microsoft Fakes +FakesAssemblies/ + +# LightSwitch generated files +GeneratedArtifacts/ +_Pvt_Extensions/ +ModelManifest.xml \ No newline at end of file diff --git a/WickedEngine.sln b/WickedEngine.sln new file mode 100644 index 000000000..938714f19 --- /dev/null +++ b/WickedEngine.sln @@ -0,0 +1,20 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 2012 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "WickedEngine", "WickedEngine\WickedEngine.vcxproj", "{A94A5B28-B5B0-4386-8281-8AE3C6984762}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Release|Win32 = Release|Win32 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {A94A5B28-B5B0-4386-8281-8AE3C6984762}.Debug|Win32.ActiveCfg = Debug|Win32 + {A94A5B28-B5B0-4386-8281-8AE3C6984762}.Debug|Win32.Build.0 = Debug|Win32 + {A94A5B28-B5B0-4386-8281-8AE3C6984762}.Release|Win32.ActiveCfg = Release|Win32 + {A94A5B28-B5B0-4386-8281-8AE3C6984762}.Release|Win32.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/WickedEngine/BULLET.cpp b/WickedEngine/BULLET.cpp new file mode 100644 index 000000000..ac26a622d --- /dev/null +++ b/WickedEngine/BULLET.cpp @@ -0,0 +1,862 @@ +#include "BULLET.h" + +int PHYSICS::softBodyInterationCount=5; +bool BULLET::grab=false; +RAY BULLET::grabRay=RAY(); + +BULLET::BULLET() +{ + registeredObjects=-1; + + ///-----initialization_start----- + //softBodySolver = new btDefaultSoftBodySolver(); + softBodySolver=NULL; + + ///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration. + //collisionConfiguration = new btDefaultCollisionConfiguration(); + collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + dispatcher = new btCollisionDispatcher(collisionConfiguration); + + ///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep. + overlappingPairCache = new btDbvtBroadphase(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + solver = new btSequentialImpulseConstraintSolver; + + //dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration); + dynamicsWorld = new btSoftRigidDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration,softBodySolver); + + + dynamicsWorld->getSolverInfo().m_solverMode|=SOLVER_RANDMIZE_ORDER; + dynamicsWorld->getDispatchInfo().m_enableSatConvex = true; + dynamicsWorld->getSolverInfo().m_splitImpulse=true; + + dynamicsWorld->setGravity(btVector3(0,-11,0)); + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->getWorldInfo().air_density = (btScalar)1.0; + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->getWorldInfo().water_density = 0; + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->getWorldInfo().water_offset = 0; + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->getWorldInfo().water_normal = btVector3(0,0,0); + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->getWorldInfo().m_gravity.setValue(0,-11,0); + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->getWorldInfo().m_sparsesdf.Initialize(); //??? + + dynamicsWorld->setInternalTickCallback(soundTickCallback,this,true); + dynamicsWorld->setInternalTickCallback(pickingPreTickCallback,this,true); + + +#ifdef BACKLOG + BackLog::post("BULLET physics Initialized"); +#endif + + ///-----initialization_end----- +} + + +BULLET::~BULLET() +{ + //cleanup in the reverse order of creation/initialization + + ///-----cleanup_start----- + + ClearWorld(); + + //delete dynamics world + delete dynamicsWorld; + + //delete solver + delete solver; + + //delete broadphase + delete overlappingPairCache; + + //delete dispatcher + delete dispatcher; + + delete collisionConfiguration; + + //next line is optional: it will be cleared by the destructor when the array goes out of scope + collisionShapes.clear(); + + ///-----cleanup_end----- + CleanUp(); +} + + +void BULLET::addWind(const XMFLOAT3& wind){ + this->wind = btVector3(btScalar(wind.x),btScalar(wind.y),btScalar(wind.z)); +} + + +void BULLET::addBox(const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic){ + + btCollisionShape* shape = new btBoxShape(btVector3(sca.x,sca.y,sca.z)); + shape->setMargin(0.05); + collisionShapes.push_back(shape); + + btTransform shapeTransform; + shapeTransform.setIdentity(); + shapeTransform.setOrigin(btVector3(pos.x,pos.y,pos.z)); + shapeTransform.setRotation(btQuaternion(rot.x,rot.y,rot.z,rot.w)); + { + btScalar mass(newMass); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f && !kinematic); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + else + mass=0; + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(shapeTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia); + rbInfo.m_friction=newFriction; + rbInfo.m_restitution=newRestitution; + rbInfo.m_linearDamping=newDamping; + btRigidBody* body = new btRigidBody(rbInfo); + if(kinematic) body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + body->setActivationState(DISABLE_DEACTIVATION); + + //add the body to the dynamics world + dynamicsWorld->addRigidBody(body); + + + if (body && body->getMotionState()) + { + btTransform trans; + body->getMotionState()->getWorldTransform(trans); + btQuaternion nRot = trans.getRotation(); + btVector3 nPos = trans.getOrigin(); + transforms.push_back(new Transform( + XMFLOAT4(nRot.getX(),nRot.getY(),nRot.getZ(),nRot.getW()),XMFLOAT3(nPos.getX(),nPos.getY(),nPos.getZ())) + ); + } + } + + +} + +void BULLET::addSphere(const float& rad, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic){ + ///create a few basic rigid bodies + /*btCollisionShape* groundShape = new btshape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + + collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-56,0)); + + { + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + groundShape->calculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + //add the body to the dynamics world + dynamicsWorld->addRigidBody(body); + } + + + { + //create a dynamic rigidbody + + //btCollisionShape* colShape = new btshape(btVector3(1,1,1)); + btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + colShape->calculateLocalInertia(mass,localInertia); + + startTransform.setOrigin(btVector3(2,10,0)); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + dynamicsWorld->addRigidBody(body); + }*/ + + btCollisionShape* shape = new btSphereShape(btScalar(rad)); + shape->setMargin(0.05); + collisionShapes.push_back(shape); + + btTransform shapeTransform; + shapeTransform.setIdentity(); + shapeTransform.setOrigin(btVector3(pos.x,pos.y,pos.z)); + { + btScalar mass(newMass); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f && !kinematic); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + else + mass=0; + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(shapeTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia); + rbInfo.m_friction=newFriction; + rbInfo.m_restitution=newRestitution; + rbInfo.m_linearDamping=newDamping; + btRigidBody* body = new btRigidBody(rbInfo); + if(kinematic) body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + body->setActivationState(DISABLE_DEACTIVATION); + + //add the body to the dynamics world + dynamicsWorld->addRigidBody(body); + + + if (body && body->getMotionState()) + { + btTransform trans; + body->getMotionState()->getWorldTransform(trans); + btQuaternion nRot = trans.getRotation(); + btVector3 nPos = trans.getOrigin(); + transforms.push_back(new Transform( + XMFLOAT4(nRot.getX(),nRot.getY(),nRot.getZ(),nRot.getW()),XMFLOAT3(nPos.getX(),nPos.getY(),nPos.getZ())) + ); + } + } + + +} + +void BULLET::addCapsule(const float& rad, const float& hei, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic){ + + btCollisionShape* shape = new btCapsuleShape(btScalar(rad),btScalar(hei)); + shape->setMargin(0.05); + collisionShapes.push_back(shape); + + btTransform shapeTransform; + shapeTransform.setIdentity(); + shapeTransform.setOrigin(btVector3(pos.x,pos.y,pos.z)); + shapeTransform.setRotation(btQuaternion(rot.x,rot.y,rot.z,rot.w)); + { + btScalar mass(newMass); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f && !kinematic); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + else + mass=0; + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(shapeTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia); + rbInfo.m_friction=newFriction; + rbInfo.m_restitution=newRestitution; + rbInfo.m_linearDamping=newDamping; + btRigidBody* body = new btRigidBody(rbInfo); + if(kinematic) body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + body->setActivationState(DISABLE_DEACTIVATION); + + //add the body to the dynamics world + dynamicsWorld->addRigidBody(body); + + + if (body && body->getMotionState()) + { + btTransform trans; + body->getMotionState()->getWorldTransform(trans); + btQuaternion nRot = trans.getRotation(); + btVector3 nPos = trans.getOrigin(); + transforms.push_back(new Transform( + XMFLOAT4(nRot.getX(),nRot.getY(),nRot.getZ(),nRot.getW()),XMFLOAT3(nPos.getX(),nPos.getY(),nPos.getZ())) + ); + } + } + + +} + +void BULLET::addConvexHull(const vector& vertices, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic){ + btCollisionShape* shape = new btConvexHullShape(); + for(int i=0;iaddPoint(btVector3(vertices[i].pos.x,vertices[i].pos.y,vertices[i].pos.z)); + shape->setLocalScaling(btVector3(sca.x,sca.y,sca.z)); + shape->setMargin(0.05); + + collisionShapes.push_back(shape); + + btTransform shapeTransform; + shapeTransform.setIdentity(); + shapeTransform.setOrigin(btVector3(pos.x,pos.y,pos.z)); + shapeTransform.setRotation(btQuaternion(rot.x,rot.y,rot.z,rot.w)); + { + btScalar mass(newMass); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f && !kinematic); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + else + mass=0; + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(shapeTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia); + rbInfo.m_friction=newFriction; + rbInfo.m_restitution=newRestitution; + rbInfo.m_linearDamping=newDamping; + btRigidBody* body = new btRigidBody(rbInfo); + if(kinematic) { + body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + } + body->setActivationState(DISABLE_DEACTIVATION); + + //add the body to the dynamics world + dynamicsWorld->addRigidBody(body); + + + if (body && body->getMotionState()) + { + btTransform trans; + body->getMotionState()->getWorldTransform(trans); + btQuaternion nRot = trans.getRotation(); + btVector3 nPos = trans.getOrigin(); + transforms.push_back(new Transform( + XMFLOAT4(nRot.getX(),nRot.getY(),nRot.getZ(),nRot.getW()),XMFLOAT3(nPos.getX(),nPos.getY(),nPos.getZ())) + ); + } + } + + +} + +void BULLET::addTriangleMesh(const vector& vertices, const vector& indices, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic){ + + int totalVerts = vertices.size(); + int totalTriangles = indices.size() / 3; + + btVector3* btVerts = new btVector3[totalVerts]; + for(int i=0;isetMargin(0.05); + shape->setLocalScaling(btVector3(sca.x,sca.y,sca.z)); + + collisionShapes.push_back(shape); + + btTransform shapeTransform; + shapeTransform.setIdentity(); + shapeTransform.setOrigin(btVector3(pos.x,pos.y,pos.z)); + shapeTransform.setRotation(btQuaternion(rot.x,rot.y,rot.z,rot.w)); + { + btScalar mass(newMass); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f && !kinematic); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + else + mass=0; + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(shapeTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia); + rbInfo.m_friction=newFriction; + rbInfo.m_restitution=newRestitution; + rbInfo.m_linearDamping=newDamping; + btRigidBody* body = new btRigidBody(rbInfo); + if(kinematic) body->setCollisionFlags( body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT); + body->setActivationState( DISABLE_DEACTIVATION ); + + //add the body to the dynamics world + dynamicsWorld->addRigidBody(body); + + + if (body && body->getMotionState()) + { + btTransform trans; + body->getMotionState()->getWorldTransform(trans); + btQuaternion nRot = trans.getRotation(); + btVector3 nPos = trans.getOrigin(); + transforms.push_back(new Transform( + XMFLOAT4(nRot.getX(),nRot.getY(),nRot.getZ(),nRot.getW()),XMFLOAT3(nPos.getX(),nPos.getY(),nPos.getZ())) + ); + } + } + + +} + + +void BULLET::addSoftBodyTriangleMesh(const Mesh* mesh, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping){ + + + const int vCount = mesh->physicsverts.size(); + btScalar* btVerts = new btScalar[vCount*3]; + for(int i=0;iphysicsverts[vindex].x); + btVerts[i+1] = btScalar(mesh->physicsverts[vindex].y); + btVerts[i+2] = btScalar(mesh->physicsverts[vindex].z); + } + const int iCount = mesh->physicsindices.size(); + const int tCount = iCount/3; + int* btInd = new int[iCount]; + for(int i=0;iphysicsindices[i]; + } + + + btSoftBody* softBody = btSoftBodyHelpers::CreateFromTriMesh( + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->getWorldInfo() + ,&btVerts[0] + ,&btInd[0] + ,tCount + ,false + ); + + + delete[] btVerts; + delete[] btInd; + + if(softBody){ + btSoftBody::Material* pm=softBody->appendMaterial(); + pm->m_kLST = 0.5; + pm->m_kVST = 0.5; + pm->m_kAST = 0.5; + pm->m_flags = 0; + softBody->generateBendingConstraints(2,pm); + softBody->randomizeConstraints(); + + btTransform shapeTransform; + shapeTransform.setIdentity(); + shapeTransform.setOrigin(btVector3(pos.x,pos.y,pos.z)); + shapeTransform.setRotation(btQuaternion(rot.x,rot.y,rot.z,rot.w)); + softBody->scale(btVector3(sca.x,sca.y,sca.z)); + softBody->transform(shapeTransform); + + + softBody->m_cfg.piterations = softBodyInterationCount; + 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] + + + btScalar mass = btScalar(newMass); + softBody->setTotalMass(mass); + + int mvg = mesh->massVG; + if(mvg>=0){ + for(map::const_iterator it=mesh->vertexGroups[mvg].vertices.begin();it!=mesh->vertexGroups[mvg].vertices.end();++it){ + int vi = (*it).first; + float wei = (*it).second; + int index=mesh->physicalmapGP[vi]; + softBody->setMass(index,softBody->getMass(index)*btScalar(wei)); + } + } + + + int gvg = mesh->goalVG; + if(gvg>=0){ + for(map::const_iterator it=mesh->vertexGroups[gvg].vertices.begin();it!=mesh->vertexGroups[gvg].vertices.end();++it){ + int vi = (*it).first; + int index=mesh->physicalmapGP[vi]; + float weight = (*it).second; + if(weight==1) + softBody->setMass(index,0); + } + } + + + //softBody->m_cfg.collisions = + // btSoftBody::fCollision::CL_RS + // ; + //softBody->generateClusters(tCount/4); + softBody->getCollisionShape()->setMargin(0.2); + + //softBody->m_cfg.collisions = + // btSoftBody::fCollision::SDF_RS+btSoftBody::fCollision::VF_SS + // ; + + //softBody->setWindVelocity(btVector3(-4, -12.0, -20.0)*0.3); + softBody->setWindVelocity(wind); + + softBody->setPose(true,true); + + softBody->setActivationState(DISABLE_DEACTIVATION); + + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->addSoftBody(softBody); + + transforms.push_back(new Transform()); + } + +} + + +void BULLET::connectVerticesToSoftBody(Mesh* const mesh, int objectI){ + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[objectI]; + btSoftBody* softBody = btSoftBody::upcast(obj); + + btVector3 min, max; + softBody->getAabb(min,max); + mesh->aabb.create(XMFLOAT3(min.x(),min.y(),min.z()),XMFLOAT3(max.x(),max.y(),max.z())); + + if(softBody){ + btSoftBody::tNodeArray& nodes(softBody->m_nodes); + + int gvg = mesh->goalVG; + for(int i=0;iskinnedVertices.size();++i) + { + int indexP = mesh->physicalmapGP[i]; + float weight = mesh->vertexGroups[gvg].vertices[indexP]; + mesh->skinnedVertices[i].pre=mesh->skinnedVertices[i].pos; + mesh->skinnedVertices[i].pos=XMFLOAT4(nodes[indexP].m_x.getX(),nodes[indexP].m_x.getY(),nodes[indexP].m_x.getZ(),1); + mesh->skinnedVertices[i].nor=XMFLOAT3(-nodes[indexP].m_n.getX(),-nodes[indexP].m_n.getY(),-nodes[indexP].m_n.getZ()); + mesh->skinnedVertices[i].tex=mesh->vertices[i].tex; + } + } +} +void BULLET::connectSoftBodyToVertices(const Mesh* const mesh, int objectI){ + if(!firstRunWorld){ + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[objectI]; + btSoftBody* softBody = btSoftBody::upcast(obj); + + if(softBody){ + btSoftBody::tNodeArray& nodes(softBody->m_nodes); + + int gvg = mesh->goalVG; + if(gvg>=0){ + int j=0; + for(map::const_iterator it=mesh->vertexGroups[gvg].vertices.begin();it!=mesh->vertexGroups[gvg].vertices.end();++it){ + int vi = (*it).first; + int index=mesh->physicalmapGP[vi]; + float weight = (*it).second; + nodes[index].m_x=nodes[index].m_x.lerp(btVector3(mesh->goalPositions[j].x,mesh->goalPositions[j].y,mesh->goalPositions[j].z),weight); + ++j; + } + } + } + } +} +void BULLET::transformBody(const XMFLOAT4& rot, const XMFLOAT3& pos, int objectI){ + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[objectI]; + btRigidBody* rigidBody = btRigidBody::upcast(obj); + if(rigidBody){ + btTransform transform; + transform.setIdentity(); + transform.setRotation(btQuaternion(rot.x,rot.y,rot.z,rot.w)); + transform.setOrigin(btVector3(pos.x,pos.y,pos.z)); + rigidBody->getMotionState()->setWorldTransform(transform); + } +} + +PHYSICS::Transform* BULLET::getObject(int index){ + + btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[index]; + btTransform trans; + + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + body->getMotionState()->getWorldTransform(trans); + } + + btSoftBody* softBody = btSoftBody::upcast(obj); + if(softBody) + { + trans = softBody->getWorldTransform(); + } + + btQuaternion rot = trans.getRotation(); + btVector3 pos = trans.getOrigin(); + transforms[index]->rotation=XMFLOAT4(rot.getX(),rot.getY(),rot.getZ(),rot.getW()); + transforms[index]->position=XMFLOAT3(pos.getX(),pos.getY(),pos.getZ()); + return transforms[index]; +} + +void BULLET::registerObject(Object* object){ + if(object->rigidBody){ + XMVECTOR s,r,t; + XMMatrixDecompose(&s,&r,&t,XMLoadFloat4x4(&object->world)); + XMFLOAT3 S,T; + XMFLOAT4 R; + XMStoreFloat3(&S,s); + XMStoreFloat4(&R,r); + XMStoreFloat3(&T,t); + + if(!object->collisionShape.compare("BOX")){ + addBox( + S,R,T + ,object->mass,object->friction,object->restitution + ,object->damping,object->kinematic + ); + object->physicsObjectI=++registeredObjects; + } + if(!object->collisionShape.compare("SPHERE")){ + addSphere( + S.x,T + ,object->mass,object->friction,object->restitution + ,object->damping,object->kinematic + ); + object->physicsObjectI=++registeredObjects; + } + if(!object->collisionShape.compare("CAPSULE")){ + addCapsule( + S.x,S.y,R,T + ,object->mass,object->friction,object->restitution + ,object->damping,object->kinematic + ); + object->physicsObjectI=++registeredObjects; + } + if(!object->collisionShape.compare("CONVEX_HULL")){ + addConvexHull( + object->mesh->vertices, + S,R,T + ,object->mass,object->friction,object->restitution + ,object->damping,object->kinematic + ); + object->physicsObjectI=++registeredObjects; + } + if(!object->collisionShape.compare("MESH")){ + addTriangleMesh( + object->mesh->vertices,object->mesh->indices, + S,R,T + ,object->mass,object->friction,object->restitution + ,object->damping,object->kinematic + ); + object->physicsObjectI=++registeredObjects; + } + } + + if(object->mesh->softBody){ + XMFLOAT3 s,t; + XMFLOAT4 r; + if(object->mesh->hasArmature()){ + s=object->mesh->armature->scale; + r=object->mesh->armature->rotation; + t=object->mesh->armature->translation; + } + else{ + s=object->scale; + r=object->rotation; + t=object->translation; + } + addSoftBodyTriangleMesh( + object->mesh + ,s,r,t + ,object->mass,object->mesh->friction,object->restitution,object->damping + ); + object->physicsObjectI=++registeredObjects; + } +} + +void BULLET::Update(){ + dynamicsWorld->stepSimulation((1.f/60.f)); +} +void BULLET::MarkForRead(){ + +} +void BULLET::UnMarkForRead(){ + +} +void BULLET::MarkForWrite(){ + +} +void BULLET::UnMarkForWrite(){ + +} +void BULLET::ClearWorld(){ + for(int i=dynamicsWorld->getNumCollisionObjects()-1;i>=0;i--) + { + btCollisionObject* obj=dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body=btRigidBody::upcast(obj); + if(body&&body->getMotionState()) + { + delete body->getMotionState(); + } + while(dynamicsWorld->getNumConstraints()) + { + btTypedConstraint* pc=dynamicsWorld->getConstraint(0); + dynamicsWorld->removeConstraint(pc); + delete pc; + } + btSoftBody* softBody = btSoftBody::upcast(obj); + if (softBody) + { + ((btSoftRigidDynamicsWorld*)dynamicsWorld)->removeSoftBody(softBody); + } else + { + btRigidBody* body = btRigidBody::upcast(obj); + if (body) + dynamicsWorld->removeRigidBody(body); + else + dynamicsWorld->removeCollisionObject(obj); + } + delete obj; + } + + //delete collision shapes + for (int j=0;jgetDispatcher()->getNumManifolds(); + for (int i=0;igetDispatcher()->getManifoldByIndexInternal(i); + btCollisionObject* obA = (btCollisionObject*)(contactManifold->getBody0()); + btCollisionObject* obB = (btCollisionObject*)(contactManifold->getBody1()); + + int numContacts = contactManifold->getNumContacts(); + for (int j=0;jgetContactPoint(j); + if (pt.getDistance()<0.f) + { + if(pt.getAppliedImpulse()>10.f){ + //((SoundEffect*)ResourceManager::get("sound/select_character.wav")->data)->Play(); + } + const btVector3& ptA = pt.getPositionWorldOnA(); + const btVector3& ptB = pt.getPositionWorldOnB(); + const btVector3& normalOnB = pt.m_normalWorldOnB; + } + } + } +} + +void BULLET::pickingPreTickCallback (btDynamicsWorld *world, btScalar timeStep) +{ +// world->getWorldUserInfo(); +// +// if(grab) +// { +// /*const int x=softDemo->m_lastmousepos[0]; +// const int y=softDemo->m_lastmousepos[1]; +// const btVector3 rayFrom=softDemo->getCameraPosition(); +// const btVector3 rayTo=softDemo->getRayTo(x,y); +// const btVector3 rayDir=(rayTo-rayFrom).normalized(); +// const btVector3 N=(softDemo->getCameraTargetPosition()-softDemo->getCameraPosition()).normalized(); +// const btScalar O=btDot(softDemo->m_impact,N);*/ +// const btVector3 rayFrom=btVector3(grabRay.origin.x,grabRay.origin.y,grabRay.origin.z); +// const btVector3 rayDir=btVector3(grabRay.direction.x,grabRay.direction.y,grabRay.direction.z); +// //const btScalar den=btDot(N,rayDir); +// //if((den*den)>0) +///* { +// const btScalar num=O-btDot(N,rayFrom); +// const btScalar hit=num/den; +// if((hit>0)&&(hit<1500)) +// { +// softDemo->m_goal=rayFrom+rayDir*hit; +// } +// }*/ +// btVector3 delta=softDemo->m_goal-softDemo->m_node->m_x; +// static const btScalar maxdrag=10; +// if(delta.length2()>(maxdrag*maxdrag)) +// { +// delta=delta.normalized()*maxdrag; +// } +// softDemo->m_node->m_v+=delta/timeStep; +// } +// +} +void BULLET::setGrab(bool value, const RAY& ray){ + grab=value; + grabRay=ray; +} diff --git a/WickedEngine/BULLET.h b/WickedEngine/BULLET.h new file mode 100644 index 000000000..7d8a3e061 --- /dev/null +++ b/WickedEngine/BULLET.h @@ -0,0 +1,78 @@ +#pragma once +#include "PHYSICS.h" +#include "btBulletDynamicsCommon.h" + +#include "LinearMath/btHashMap.h" +#include "BulletSoftBody/btSoftRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" + +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "BulletSoftBody/btDefaultSoftBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" + +struct RAY; + +class BULLET:public PHYSICS +{ +private: + btCollisionConfiguration* collisionConfiguration; + btCollisionDispatcher* dispatcher; + btBroadphaseInterface* overlappingPairCache; + btSequentialImpulseConstraintSolver* solver; + btDynamicsWorld* dynamicsWorld; + btAlignedObjectArray collisionShapes; + + btSoftBodySolver* softBodySolver; + btSoftBodySolverOutput* softBodySolverOutput; + + btVector3 wind; + + static bool grab; + static RAY grabRay; +public: + BULLET(); + ~BULLET(); + + void addWind(const XMFLOAT3& wind); + + void addBox(const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic); + void addSphere(const float& rad, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic); + void addCapsule(const float& rad, const float& hei, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic); + void addConvexHull(const vector& vertices, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic); + void addTriangleMesh(const vector& vertices, const vector& indices, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping, bool kinematic); + + void addSoftBodyTriangleMesh(const Mesh* mesh, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass, const float& newFriction, const float& newRestitution, const float& newDamping); + + void addBone(const float& rad, const float& hei); + + void connectVerticesToSoftBody(Mesh* const mesh, int objectI); + void connectSoftBodyToVertices(const Mesh* const mesh, int objectI); + void transformBody(const XMFLOAT4& rot, const XMFLOAT3& pos, int objectI); + + Transform* getObject(int index); + + void registerObject(Object* object); + + void Update(); + void MarkForRead(); + void UnMarkForRead(); + void MarkForWrite(); + void UnMarkForWrite(); + void ClearWorld(); + void CleanUp(); + + + void* operator new(size_t); + void operator delete(void*); + + void setGrab(bool val, const RAY& ray); + static void pickingPreTickCallback (btDynamicsWorld *world, btScalar timeStep); + static void soundTickCallback(btDynamicsWorld *world, btScalar timeStep); +}; + diff --git a/WickedEngine/BULLET/Bullet-C-Api.h b/WickedEngine/BULLET/Bullet-C-Api.h new file mode 100644 index 000000000..f27a17d51 --- /dev/null +++ b/WickedEngine/BULLET/Bullet-C-Api.h @@ -0,0 +1,176 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* + 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 new file mode 100644 index 000000000..77763305b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -0,0 +1,37 @@ + +//Bullet Continuous Collision Detection and Physics Library +//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + + +// +// btAxisSweep3 +// +// Copyright (c) 2006 Simon Hobbs +// +// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +// +// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +// +// 3. This notice may not be removed or altered from any source distribution. +#include "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 new file mode 100644 index 000000000..cd6e1a892 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -0,0 +1,1051 @@ +//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 new file mode 100644 index 000000000..f1bf00594 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -0,0 +1,82 @@ +/* +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 new file mode 100644 index 000000000..f4d7341f8 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp @@ -0,0 +1,17 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btBroadphaseProxy.h" + diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h new file mode 100644 index 000000000..bb58b8289 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -0,0 +1,270 @@ +/* +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 new file mode 100644 index 000000000..c95d1be0f --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp @@ -0,0 +1,23 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..405656236 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -0,0 +1,81 @@ +/* +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 new file mode 100644 index 000000000..95443af50 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -0,0 +1,1295 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvt implementation by Nathanael Presson + +#include "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 new file mode 100644 index 000000000..75cfac643 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -0,0 +1,796 @@ +/* +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 new file mode 100644 index 000000000..18b64ad0e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -0,0 +1,146 @@ +/* +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 new file mode 100644 index 000000000..20768225b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.cpp @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btDispatcher.h" + +btDispatcher::~btDispatcher() +{ + +} + diff --git a/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h new file mode 100644 index 000000000..89c307d14 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -0,0 +1,107 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_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 new file mode 100644 index 000000000..81369fe9b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -0,0 +1,489 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..7bcfe6b13 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -0,0 +1,151 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#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 new file mode 100644 index 000000000..ae22dadc7 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -0,0 +1,633 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "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 new file mode 100644 index 000000000..eee90e473 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -0,0 +1,470 @@ +/* +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 new file mode 100644 index 000000000..9c7b6f813 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h @@ -0,0 +1,40 @@ + +/* +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 new file mode 100644 index 000000000..889216df5 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -0,0 +1,1393 @@ +/* +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 new file mode 100644 index 000000000..78382da79 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -0,0 +1,581 @@ +/* +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 new file mode 100644 index 000000000..752fcd0fe --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -0,0 +1,349 @@ +/* +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 new file mode 100644 index 000000000..7cb3c40a0 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -0,0 +1,171 @@ +/* +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/CMakeLists.txt b/WickedEngine/BULLET/BulletCollision/CMakeLists.txt new file mode 100644 index 000000000..c4723ae25 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CMakeLists.txt @@ -0,0 +1,286 @@ +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ) + +SET(BulletCollision_SRCS + BroadphaseCollision/btAxisSweep3.cpp + BroadphaseCollision/btBroadphaseProxy.cpp + BroadphaseCollision/btCollisionAlgorithm.cpp + BroadphaseCollision/btDbvt.cpp + BroadphaseCollision/btDbvtBroadphase.cpp + BroadphaseCollision/btDispatcher.cpp + BroadphaseCollision/btMultiSapBroadphase.cpp + BroadphaseCollision/btOverlappingPairCache.cpp + BroadphaseCollision/btQuantizedBvh.cpp + BroadphaseCollision/btSimpleBroadphase.cpp + CollisionDispatch/btActivatingCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxDetector.cpp + CollisionDispatch/btCollisionDispatcher.cpp + CollisionDispatch/btCollisionObject.cpp + CollisionDispatch/btCollisionWorld.cpp + CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp + CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btConvexConvexAlgorithm.cpp + CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp + CollisionDispatch/btDefaultCollisionConfiguration.cpp + CollisionDispatch/btEmptyCollisionAlgorithm.cpp + CollisionDispatch/btGhostObject.cpp + CollisionDispatch/btHashedSimplePairCache.cpp + CollisionDispatch/btInternalEdgeUtility.cpp + CollisionDispatch/btInternalEdgeUtility.h + CollisionDispatch/btManifoldResult.cpp + CollisionDispatch/btSimulationIslandManager.cpp + CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp + CollisionDispatch/btUnionFind.cpp + CollisionDispatch/SphereTriangleDetector.cpp + CollisionShapes/btBoxShape.cpp + CollisionShapes/btBox2dShape.cpp + CollisionShapes/btBvhTriangleMeshShape.cpp + CollisionShapes/btCapsuleShape.cpp + CollisionShapes/btCollisionShape.cpp + CollisionShapes/btCompoundShape.cpp + CollisionShapes/btConcaveShape.cpp + CollisionShapes/btConeShape.cpp + CollisionShapes/btConvexHullShape.cpp + CollisionShapes/btConvexInternalShape.cpp + CollisionShapes/btConvexPointCloudShape.cpp + CollisionShapes/btConvexPolyhedron.cpp + CollisionShapes/btConvexShape.cpp + CollisionShapes/btConvex2dShape.cpp + CollisionShapes/btConvexTriangleMeshShape.cpp + CollisionShapes/btCylinderShape.cpp + CollisionShapes/btEmptyShape.cpp + CollisionShapes/btHeightfieldTerrainShape.cpp + CollisionShapes/btMinkowskiSumShape.cpp + CollisionShapes/btMultimaterialTriangleMeshShape.cpp + CollisionShapes/btMultiSphereShape.cpp + CollisionShapes/btOptimizedBvh.cpp + CollisionShapes/btPolyhedralConvexShape.cpp + CollisionShapes/btScaledBvhTriangleMeshShape.cpp + CollisionShapes/btShapeHull.cpp + CollisionShapes/btSphereShape.cpp + CollisionShapes/btStaticPlaneShape.cpp + CollisionShapes/btStridingMeshInterface.cpp + CollisionShapes/btTetrahedronShape.cpp + CollisionShapes/btTriangleBuffer.cpp + CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleIndexVertexArray.cpp + CollisionShapes/btTriangleIndexVertexMaterialArray.cpp + CollisionShapes/btTriangleMesh.cpp + CollisionShapes/btTriangleMeshShape.cpp + CollisionShapes/btUniformScalingShape.cpp + Gimpact/btContactProcessing.cpp + Gimpact/btGenericPoolAllocator.cpp + Gimpact/btGImpactBvh.cpp + Gimpact/btGImpactCollisionAlgorithm.cpp + Gimpact/btGImpactQuantizedBvh.cpp + Gimpact/btGImpactShape.cpp + Gimpact/btTriangleShapeEx.cpp + Gimpact/gim_box_set.cpp + Gimpact/gim_contact.cpp + Gimpact/gim_memory.cpp + Gimpact/gim_tri_collision.cpp + NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btConvexCast.cpp + NarrowPhaseCollision/btGjkConvexCast.cpp + NarrowPhaseCollision/btGjkEpa2.cpp + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp + NarrowPhaseCollision/btGjkPairDetector.cpp + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp + NarrowPhaseCollision/btPersistentManifold.cpp + NarrowPhaseCollision/btRaycastCallback.cpp + NarrowPhaseCollision/btSubSimplexConvexCast.cpp + NarrowPhaseCollision/btVoronoiSimplexSolver.cpp + NarrowPhaseCollision/btPolyhedralContactClipping.cpp +) + +SET(Root_HDRS + ../btBulletCollisionCommon.h +) +SET(BroadphaseCollision_HDRS + BroadphaseCollision/btAxisSweep3.h + BroadphaseCollision/btBroadphaseInterface.h + BroadphaseCollision/btBroadphaseProxy.h + BroadphaseCollision/btCollisionAlgorithm.h + BroadphaseCollision/btDbvt.h + BroadphaseCollision/btDbvtBroadphase.h + BroadphaseCollision/btDispatcher.h + BroadphaseCollision/btMultiSapBroadphase.h + BroadphaseCollision/btOverlappingPairCache.h + BroadphaseCollision/btOverlappingPairCallback.h + BroadphaseCollision/btQuantizedBvh.h + BroadphaseCollision/btSimpleBroadphase.h +) +SET(CollisionDispatch_HDRS + CollisionDispatch/btActivatingCollisionAlgorithm.h + CollisionDispatch/btBoxBoxCollisionAlgorithm.h + CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h + CollisionDispatch/btBoxBoxDetector.h + CollisionDispatch/btCollisionConfiguration.h + CollisionDispatch/btCollisionCreateFunc.h + CollisionDispatch/btCollisionDispatcher.h + CollisionDispatch/btCollisionObject.h + CollisionDispatch/btCollisionObjectWrapper.h + CollisionDispatch/btCollisionWorld.h + CollisionDispatch/btCompoundCollisionAlgorithm.h + CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h + CollisionDispatch/btConvexConcaveCollisionAlgorithm.h + CollisionDispatch/btConvexConvexAlgorithm.h + CollisionDispatch/btConvex2dConvex2dAlgorithm.h + CollisionDispatch/btConvexPlaneCollisionAlgorithm.h + CollisionDispatch/btDefaultCollisionConfiguration.h + CollisionDispatch/btEmptyCollisionAlgorithm.h + CollisionDispatch/btGhostObject.h + CollisionDispatch/btHashedSimplePairCache.h + CollisionDispatch/btManifoldResult.h + CollisionDispatch/btSimulationIslandManager.h + CollisionDispatch/btSphereBoxCollisionAlgorithm.h + CollisionDispatch/btSphereSphereCollisionAlgorithm.h + CollisionDispatch/btSphereTriangleCollisionAlgorithm.h + CollisionDispatch/btUnionFind.h + CollisionDispatch/SphereTriangleDetector.h +) +SET(CollisionShapes_HDRS + CollisionShapes/btBoxShape.h + CollisionShapes/btBox2dShape.h + CollisionShapes/btBvhTriangleMeshShape.h + CollisionShapes/btCapsuleShape.h + CollisionShapes/btCollisionMargin.h + CollisionShapes/btCollisionShape.h + CollisionShapes/btCompoundShape.h + CollisionShapes/btConcaveShape.h + CollisionShapes/btConeShape.h + CollisionShapes/btConvexHullShape.h + CollisionShapes/btConvexInternalShape.h + CollisionShapes/btConvexPointCloudShape.h + CollisionShapes/btConvexPolyhedron.h + CollisionShapes/btConvexShape.h + CollisionShapes/btConvex2dShape.h + CollisionShapes/btConvexTriangleMeshShape.h + CollisionShapes/btCylinderShape.h + CollisionShapes/btEmptyShape.h + CollisionShapes/btHeightfieldTerrainShape.h + CollisionShapes/btMaterial.h + CollisionShapes/btMinkowskiSumShape.h + CollisionShapes/btMultimaterialTriangleMeshShape.h + CollisionShapes/btMultiSphereShape.h + CollisionShapes/btOptimizedBvh.h + CollisionShapes/btPolyhedralConvexShape.h + CollisionShapes/btScaledBvhTriangleMeshShape.h + CollisionShapes/btShapeHull.h + CollisionShapes/btSphereShape.h + CollisionShapes/btStaticPlaneShape.h + CollisionShapes/btStridingMeshInterface.h + CollisionShapes/btTetrahedronShape.h + CollisionShapes/btTriangleBuffer.h + CollisionShapes/btTriangleCallback.h + CollisionShapes/btTriangleIndexVertexArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.h + CollisionShapes/btTriangleInfoMap.h + CollisionShapes/btTriangleMesh.h + CollisionShapes/btTriangleMeshShape.h + CollisionShapes/btTriangleShape.h + CollisionShapes/btUniformScalingShape.h +) +SET(Gimpact_HDRS + Gimpact/btBoxCollision.h + Gimpact/btClipPolygon.h + Gimpact/btContactProcessing.h + Gimpact/btGenericPoolAllocator.h + Gimpact/btGeometryOperations.h + Gimpact/btGImpactBvh.h + Gimpact/btGImpactCollisionAlgorithm.h + Gimpact/btGImpactMassUtil.h + Gimpact/btGImpactQuantizedBvh.h + Gimpact/btGImpactShape.h + Gimpact/btQuantization.h + Gimpact/btTriangleShapeEx.h + Gimpact/gim_array.h + Gimpact/gim_basic_geometry_operations.h + Gimpact/gim_bitset.h + Gimpact/gim_box_collision.h + Gimpact/gim_box_set.h + Gimpact/gim_clip_polygon.h + Gimpact/gim_contact.h + Gimpact/gim_geom_types.h + Gimpact/gim_geometry.h + Gimpact/gim_hash_table.h + Gimpact/gim_linear_math.h + Gimpact/gim_math.h + Gimpact/gim_memory.h + Gimpact/gim_radixsort.h + Gimpact/gim_tri_collision.h +) +SET(NarrowPhaseCollision_HDRS + NarrowPhaseCollision/btContinuousConvexCollision.h + NarrowPhaseCollision/btConvexCast.h + NarrowPhaseCollision/btConvexPenetrationDepthSolver.h + NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h + NarrowPhaseCollision/btGjkConvexCast.h + NarrowPhaseCollision/btGjkEpa2.h + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h + NarrowPhaseCollision/btGjkPairDetector.h + NarrowPhaseCollision/btManifoldPoint.h + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h + NarrowPhaseCollision/btPersistentManifold.h + NarrowPhaseCollision/btPointCollector.h + NarrowPhaseCollision/btRaycastCallback.h + NarrowPhaseCollision/btSimplexSolverInterface.h + NarrowPhaseCollision/btSubSimplexConvexCast.h + NarrowPhaseCollision/btVoronoiSimplexSolver.h + NarrowPhaseCollision/btPolyhedralContactClipping.h +) + +SET(BulletCollision_HDRS + ${Root_HDRS} + ${BroadphaseCollision_HDRS} + ${CollisionDispatch_HDRS} + ${CollisionShapes_HDRS} + ${Gimpact_HDRS} + ${NarrowPhaseCollision_HDRS} +) + + +ADD_LIBRARY(BulletCollision ${BulletCollision_SRCS} ${BulletCollision_HDRS}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletCollision LinearMath) +ENDIF (BUILD_SHARED_LIBS) + + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #INSTALL of other files requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletCollision DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletCollision RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + INSTALL(FILES ../btBulletCollisionCommon.h +DESTINATION ${INCLUDE_INSTALL_DIR}/BulletCollision) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES FRAMEWORK true) + + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${BroadphaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadphaseCollision) + SET_PROPERTY(SOURCE ${CollisionDispatch_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionDispatch) + SET_PROPERTY(SOURCE ${CollisionShapes_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionShapes) + SET_PROPERTY(SOURCE ${Gimpact_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Gimpact) + SET_PROPERTY(SOURCE ${NarrowPhaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/NarrowPhaseCollision) + + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp new file mode 100644 index 000000000..634017809 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -0,0 +1,200 @@ +/* +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 new file mode 100644 index 000000000..22953af43 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -0,0 +1,51 @@ +/* +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 new file mode 100644 index 000000000..57f146493 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btActivatingCollisionAlgorithm.h" +#include "btCollisionDispatcher.h" +#include "btCollisionObject.h" + +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +} +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, 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 new file mode 100644 index 000000000..489812b96 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -0,0 +1,36 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H +#define __BT_ACTIVATING_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +///This class is not enabled yet (work-in-progress) to more aggressively activate objects. +class btActivatingCollisionAlgorithm : public btCollisionAlgorithm +{ +// btCollisionObject* m_colObj0; +// btCollisionObject* m_colObj1; + +public: + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, 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 new file mode 100644 index 000000000..2c3627782 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -0,0 +1,421 @@ +/* +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 new file mode 100644 index 000000000..6ea6e89bd --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_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 new file mode 100644 index 000000000..ac68968f5 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -0,0 +1,84 @@ +/* +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 new file mode 100644 index 000000000..59808df5a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_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 new file mode 100644 index 000000000..7043bde34 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -0,0 +1,718 @@ +/* + * 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 new file mode 100644 index 000000000..392437770 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -0,0 +1,44 @@ +/* + * 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 new file mode 100644 index 000000000..669498494 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -0,0 +1,46 @@ +/* +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 new file mode 100644 index 000000000..62ee66c4e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -0,0 +1,45 @@ +/* +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 new file mode 100644 index 000000000..669d0b6b5 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -0,0 +1,314 @@ +/* +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 new file mode 100644 index 000000000..92696ee54 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -0,0 +1,171 @@ +/* +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 new file mode 100644 index 000000000..d09241000 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -0,0 +1,117 @@ +/* +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 new file mode 100644 index 000000000..89cad1682 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -0,0 +1,565 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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; + int 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; + } + + int 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(int 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 new file mode 100644 index 000000000..952440b7d --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h @@ -0,0 +1,43 @@ +#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 new file mode 100644 index 000000000..093c6f9b2 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -0,0 +1,1552 @@ +/* +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 new file mode 100644 index 000000000..b3fffdecd --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -0,0 +1,526 @@ +/* +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 new file mode 100644 index 000000000..991841ee2 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -0,0 +1,375 @@ +/* +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 new file mode 100644 index 000000000..536751456 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -0,0 +1,99 @@ +/* +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 new file mode 100644 index 000000000..a52dd34fe --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -0,0 +1,421 @@ +/* +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 new file mode 100644 index 000000000..7e2d7ad70 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -0,0 +1,90 @@ +/* +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 new file mode 100644 index 000000000..4ec9ae713 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -0,0 +1,246 @@ +/* +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 new file mode 100644 index 000000000..18d9385a1 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -0,0 +1,95 @@ +/* +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 new file mode 100644 index 000000000..e23f5f7a8 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -0,0 +1,335 @@ +/* +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 new file mode 100644 index 000000000..e90d06eb1 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -0,0 +1,121 @@ +/* +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 new file mode 100644 index 000000000..7f2722aa4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -0,0 +1,783 @@ +/* +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 new file mode 100644 index 000000000..51db0c654 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -0,0 +1,108 @@ +/* +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 new file mode 100644 index 000000000..cce2d95bc --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -0,0 +1,174 @@ +/* +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 new file mode 100644 index 000000000..d28c430c4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -0,0 +1,84 @@ +/* +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 new file mode 100644 index 000000000..c3cacec4a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -0,0 +1,307 @@ +/* +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 new file mode 100644 index 000000000..2078420e1 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -0,0 +1,127 @@ +/* +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 new file mode 100644 index 000000000..5fa1c8be5 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -0,0 +1,34 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..cb0f15218 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -0,0 +1,54 @@ +/* +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 new file mode 100644 index 000000000..86141fa68 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -0,0 +1,171 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btGhostObject.h" +#include "btCollisionWorld.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "LinearMath/btAabbUtil2.h" + +btGhostObject::btGhostObject() +{ + m_internalType = CO_GHOST_OBJECT; +} + +btGhostObject::~btGhostObject() +{ + ///btGhostObject should have been removed from the world, so no overlapping objects + btAssert(!m_overlappingObjects.size()); +} + + +void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + //not found + m_overlappingObjects.push_back(otherObject); + } +} + +void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index~btHashedOverlappingPairCache(); + btAlignedFree( m_hashPairCache ); +} + +void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + m_overlappingObjects.push_back(otherObject); + m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + } +} + +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (indexremoveOverlappingPair(actualThisProxy,otherProxy,dispatcher); + } +} + + +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans = convexFromWorld; + convexToTrans = convexToWorld; + btVector3 castShapeAabbMin, castShapeAabbMax; + /* Compute AABB that encompasses angular movement */ + { + btVector3 linVel, angVel; + btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransform R; + R.setIdentity (); + R.setRotation (convexFromTrans.getRotation()); + castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + } + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;igetBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); + } + } + } + +} + +void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const +{ + btTransform rayFromTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + btTransform rayToTrans; + rayToTrans.setIdentity(); + rayToTrans.setOrigin(rayToWorld); + + + int i; + for (i=0;igetBroadphaseHandle())) + { + btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } + } +} + diff --git a/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h new file mode 100644 index 000000000..8ec861385 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btGhostObject.h @@ -0,0 +1,175 @@ +/* +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 new file mode 100644 index 000000000..cfcca5654 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -0,0 +1,278 @@ +/* +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 new file mode 100644 index 000000000..e88ef97e9 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -0,0 +1,174 @@ +/* +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 new file mode 100644 index 000000000..73fa4e87e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -0,0 +1,842 @@ +#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 new file mode 100644 index 000000000..7d9aafeee --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -0,0 +1,47 @@ + +#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 new file mode 100644 index 000000000..4b2986a00 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -0,0 +1,154 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#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 new file mode 100644 index 000000000..977b9a02f --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -0,0 +1,150 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_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 new file mode 100644 index 000000000..134478225 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -0,0 +1,450 @@ + +/* +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 new file mode 100644 index 000000000..e24c6afec --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -0,0 +1,81 @@ +/* +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 new file mode 100644 index 000000000..e8b567e0e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -0,0 +1,214 @@ +/* +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 new file mode 100644 index 000000000..eefaedc9e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -0,0 +1,75 @@ +/* +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 new file mode 100644 index 000000000..36ba21f5b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -0,0 +1,106 @@ +/* +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 new file mode 100644 index 000000000..3517a568a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_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 new file mode 100644 index 000000000..280a4d355 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -0,0 +1,84 @@ +/* +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 new file mode 100644 index 000000000..6b6e39a72 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -0,0 +1,69 @@ +/* +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 new file mode 100644 index 000000000..522293359 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -0,0 +1,82 @@ +/* +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 new file mode 100644 index 000000000..ecce028c2 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.cpp @@ -0,0 +1,42 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..ce333783e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBox2dShape.h @@ -0,0 +1,371 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..3859138f1 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -0,0 +1,51 @@ +/* +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 new file mode 100644 index 000000000..715e3f2ab --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBoxShape.h @@ -0,0 +1,314 @@ +/* +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 new file mode 100644 index 000000000..ace4cfa26 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -0,0 +1,466 @@ +/* +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 new file mode 100644 index 000000000..493d63553 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -0,0 +1,145 @@ +/* +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 new file mode 100644 index 000000000..864df26e9 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -0,0 +1,171 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..7578bb258 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -0,0 +1,184 @@ +/* +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 new file mode 100644 index 000000000..474bf1fb4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -0,0 +1,27 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..39ee21cad --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -0,0 +1,119 @@ +/* +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 new file mode 100644 index 000000000..ff017a206 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCollisionShape.h @@ -0,0 +1,159 @@ +/* +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 new file mode 100644 index 000000000..0aa75f2bf --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -0,0 +1,356 @@ +/* +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 new file mode 100644 index 000000000..141034a8e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCompoundShape.h @@ -0,0 +1,212 @@ +/* +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 new file mode 100644 index 000000000..58ff84a5b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -0,0 +1,27 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..2917cc5b6 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConcaveShape.h @@ -0,0 +1,62 @@ +/* +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 new file mode 100644 index 000000000..2d83c8bfb --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConeShape.cpp @@ -0,0 +1,147 @@ +/* +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 new file mode 100644 index 000000000..10ea3e981 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.cpp @@ -0,0 +1,92 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..bbd1caf42 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvex2dShape.h @@ -0,0 +1,82 @@ +/* +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 new file mode 100644 index 000000000..0623e351a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -0,0 +1,250 @@ +/* +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 new file mode 100644 index 000000000..3bd598ec4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -0,0 +1,122 @@ +/* +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 new file mode 100644 index 000000000..083d60b1b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.cpp @@ -0,0 +1,151 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..37e04f5fc --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -0,0 +1,224 @@ +/* +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 new file mode 100644 index 000000000..ad1d1bf78 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -0,0 +1,139 @@ +/* +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 new file mode 100644 index 000000000..54b5afac3 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -0,0 +1,105 @@ +/* +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 new file mode 100644 index 000000000..f4324c1f4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -0,0 +1,302 @@ +/* +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 new file mode 100644 index 000000000..d3cd066ac --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -0,0 +1,65 @@ +/* +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 new file mode 100644 index 000000000..f03d0b21e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -0,0 +1,455 @@ +/* +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 new file mode 100644 index 000000000..290cd9fd1 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexShape.h @@ -0,0 +1,84 @@ +/* +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 new file mode 100644 index 000000000..0f9ced554 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -0,0 +1,315 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..f338865ca --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -0,0 +1,77 @@ +/* +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 new file mode 100644 index 000000000..6cfe43be4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -0,0 +1,281 @@ +/* +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 new file mode 100644 index 000000000..a9e6df5c5 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -0,0 +1,50 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..069a79402 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btEmptyShape.h @@ -0,0 +1,72 @@ +/* +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 new file mode 100644 index 000000000..26322791d --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -0,0 +1,410 @@ +/* +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 new file mode 100644 index 000000000..4a7a4a4bd --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -0,0 +1,167 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..866f9b4da --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMaterial.h @@ -0,0 +1,35 @@ +/* +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 new file mode 100644 index 000000000..06707e24e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -0,0 +1,60 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..a3f9a4723 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -0,0 +1,62 @@ +/* +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 new file mode 100644 index 000000000..a7362ea01 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -0,0 +1,182 @@ +/* +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 new file mode 100644 index 000000000..5d3b40268 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -0,0 +1,101 @@ +/* +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 new file mode 100644 index 000000000..58799ac96 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -0,0 +1,45 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// This file was created by Alex Silverman + +#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" +//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" + + +///Obtains the material for a specific triangle +const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) +{ + const unsigned char * materialBase = 0; + int numMaterials; + PHY_ScalarType materialType; + int materialStride; + const unsigned char * triangleMaterialBase = 0; + int numTriangles; + int triangleMaterialStride; + PHY_ScalarType triangleType; + + ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, + &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); + + // return the pointer to the place with the friction for the triangle + // TODO: This depends on whether it's a moving mesh or not + // BUG IN GIMPACT + //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); + int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); + btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); + return (matVal); +} diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h new file mode 100644 index 000000000..2b92ab7d1 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -0,0 +1,120 @@ +/* +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 new file mode 100644 index 000000000..6f36775f7 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -0,0 +1,391 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..715961f55 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -0,0 +1,65 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///Contains contributions from Disney Studio's + +#ifndef 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 new file mode 100644 index 000000000..4854f370f --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -0,0 +1,500 @@ +/* +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 new file mode 100644 index 000000000..961d001a9 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -0,0 +1,116 @@ +/* +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 new file mode 100644 index 000000000..6a337c786 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -0,0 +1,121 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..39049eaf0 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -0,0 +1,95 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..3beaf8658 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -0,0 +1,170 @@ +/* +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 new file mode 100644 index 000000000..e959f198b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btShapeHull.h @@ -0,0 +1,61 @@ +/* +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 new file mode 100644 index 000000000..b9a736c0f --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -0,0 +1,71 @@ +/* +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 new file mode 100644 index 000000000..e6e328839 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -0,0 +1,105 @@ +/* +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 new file mode 100644 index 000000000..b3d449676 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -0,0 +1,381 @@ +/* +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 new file mode 100644 index 000000000..9fbe13976 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -0,0 +1,164 @@ +/* +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 new file mode 100644 index 000000000..52f346bf7 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -0,0 +1,218 @@ +/* +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 new file mode 100644 index 000000000..f558bf6d2 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.cpp @@ -0,0 +1,28 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btTriangleCallback.h" + +btTriangleCallback::~btTriangleCallback() +{ + +} + + +btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback() +{ + +} + diff --git a/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h new file mode 100644 index 000000000..461c57f87 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -0,0 +1,42 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..a665024cb --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -0,0 +1,95 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..9e1544e87 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -0,0 +1,133 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..dc562941a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -0,0 +1,86 @@ +/* +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 new file mode 100644 index 000000000..ba4f7b460 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -0,0 +1,84 @@ +/* +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 new file mode 100644 index 000000000..17deef89d --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -0,0 +1,241 @@ +/* +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 new file mode 100644 index 000000000..0e1795140 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -0,0 +1,207 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..453e58005 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -0,0 +1,90 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..a8a80f82f --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btTriangleShape.h @@ -0,0 +1,184 @@ +/* +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 new file mode 100644 index 000000000..b148bbd99 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.cpp @@ -0,0 +1,160 @@ +/* +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 new file mode 100644 index 000000000..a10f58d24 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/CollisionShapes/btUniformScalingShape.h @@ -0,0 +1,89 @@ +/* +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 new file mode 100644 index 000000000..4ecb6acb6 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Doxyfile @@ -0,0 +1,746 @@ +# 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 new file mode 100644 index 000000000..0a0357e5a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btBoxCollision.h @@ -0,0 +1,645 @@ +#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 new file mode 100644 index 000000000..de0a5231b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btClipPolygon.h @@ -0,0 +1,182 @@ +#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 new file mode 100644 index 000000000..02f8b678a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btCompoundFromGimpact.h @@ -0,0 +1,93 @@ +#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 new file mode 100644 index 000000000..eed31d839 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btContactProcessing.cpp @@ -0,0 +1,181 @@ + +/* +This source file is part of GIMPACT Library. + +For the latest info, see http://gimpact.sourceforge.net/ + +Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371. +email: projectileman@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 new file mode 100644 index 000000000..6174ae97a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactBvh.h @@ -0,0 +1,396 @@ +#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 new file mode 100644 index 000000000..2e87475e3 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp @@ -0,0 +1,932 @@ +/* +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 new file mode 100644 index 000000000..f85a94cb4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -0,0 +1,310 @@ +/*! \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 new file mode 100644 index 000000000..2543aefcf --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactMassUtil.h @@ -0,0 +1,60 @@ +/*! \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 new file mode 100644 index 000000000..4528758c3 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @@ -0,0 +1,528 @@ +/*! \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 new file mode 100644 index 000000000..e6e52fff4 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -0,0 +1,372 @@ +#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 new file mode 100644 index 000000000..ac8efdf38 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.cpp @@ -0,0 +1,238 @@ +/* +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 new file mode 100644 index 000000000..3d1f48d47 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGImpactShape.h @@ -0,0 +1,1184 @@ +/*! \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 new file mode 100644 index 000000000..5d07d1adb --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGenericPoolAllocator.cpp @@ -0,0 +1,283 @@ +/*! \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 new file mode 100644 index 000000000..60f06510a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btGeometryOperations.h @@ -0,0 +1,212 @@ +#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 new file mode 100644 index 000000000..973c2ed12 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -0,0 +1,180 @@ +/*! \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 new file mode 100644 index 000000000..27e6f32fc --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_array.h @@ -0,0 +1,324 @@ +#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 new file mode 100644 index 000000000..7dee48a4c --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_bitset.h @@ -0,0 +1,123 @@ +#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 new file mode 100644 index 000000000..9c572638a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_collision.h @@ -0,0 +1,588 @@ +#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 new file mode 100644 index 000000000..0c3d7ba8d --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.cpp @@ -0,0 +1,182 @@ + +/* +----------------------------------------------------------------------------- +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 new file mode 100644 index 000000000..61d190a7d --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_box_set.h @@ -0,0 +1,674 @@ +#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 new file mode 100644 index 000000000..e342459ce --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_clip_polygon.h @@ -0,0 +1,210 @@ +#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 new file mode 100644 index 000000000..20e41de08 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_contact.cpp @@ -0,0 +1,146 @@ + +/* +----------------------------------------------------------------------------- +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 new file mode 100644 index 000000000..64f11b495 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_linear_math.h @@ -0,0 +1,1573 @@ +#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 new file mode 100644 index 000000000..939079e10 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_math.h @@ -0,0 +1,157 @@ +#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 new file mode 100644 index 000000000..1636eb786 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_memory.cpp @@ -0,0 +1,135 @@ +/* +----------------------------------------------------------------------------- +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 new file mode 100644 index 000000000..c246ef125 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_radixsort.h @@ -0,0 +1,406 @@ +#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 new file mode 100644 index 000000000..f9727e1d5 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -0,0 +1,640 @@ + +/*! \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 new file mode 100644 index 000000000..940282f57 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -0,0 +1,242 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..bdc0572f7 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -0,0 +1,59 @@ +/* +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 new file mode 100644 index 000000000..d2a1310b2 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp @@ -0,0 +1,20 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvexCast.h" + +btConvexCast::~btConvexCast() +{ +} diff --git a/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h new file mode 100644 index 000000000..bfd79d03b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -0,0 +1,73 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef 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 new file mode 100644 index 000000000..29620abff --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -0,0 +1,40 @@ +/* +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 new file mode 100644 index 000000000..46ce1ab75 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -0,0 +1,88 @@ +/* +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 new file mode 100644 index 000000000..bef697a0a --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -0,0 +1,176 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "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 new file mode 100644 index 000000000..6a42ee63b --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -0,0 +1,50 @@ +/* +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 new file mode 100644 index 000000000..3268f06c2 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -0,0 +1,1031 @@ +/* +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 new file mode 100644 index 000000000..ac501d5ec --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -0,0 +1,75 @@ +/* +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 new file mode 100644 index 000000000..572ec36f5 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "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 new file mode 100644 index 000000000..1ed6340af --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h @@ -0,0 +1,43 @@ +/* +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 new file mode 100644 index 000000000..887757949 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -0,0 +1,480 @@ +/* +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 new file mode 100644 index 000000000..feeae6862 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -0,0 +1,103 @@ +/* +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 new file mode 100644 index 000000000..e40fb1d3d --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -0,0 +1,156 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..fa45f4903 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -0,0 +1,361 @@ +/* +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 new file mode 100644 index 000000000..fd533b4fc --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -0,0 +1,40 @@ +/* +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 new file mode 100644 index 000000000..4d92e853d --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -0,0 +1,305 @@ +/* +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 new file mode 100644 index 000000000..2ceaab750 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -0,0 +1,240 @@ +/* +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 new file mode 100644 index 000000000..b87bd4f32 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -0,0 +1,46 @@ +/* +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 new file mode 100644 index 000000000..786efd182 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -0,0 +1,178 @@ +/* +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 new file mode 100644 index 000000000..3999d4005 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -0,0 +1,72 @@ +/* +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 new file mode 100644 index 000000000..da8a13914 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -0,0 +1,63 @@ +/* +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 new file mode 100644 index 000000000..18eb662de --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -0,0 +1,160 @@ +/* +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 new file mode 100644 index 000000000..6c8127983 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -0,0 +1,50 @@ +/* +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 new file mode 100644 index 000000000..a775198ab --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -0,0 +1,609 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + + Elsevier CDROM license agreements grants nonexclusive license to use the software + for any purpose, commercial or non-commercial as long as the following credit is included + identifying the original source of the software: + + Parts of the source are "from the book Real-Time Collision Detection by + Christer Ericson, published by Morgan Kaufmann Publishers, + (c) 2005 Elsevier Inc." + +*/ + + +#include "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 new file mode 100644 index 000000000..2f389e27e --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -0,0 +1,181 @@ +/* +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/BulletCollision/premake4.lua b/WickedEngine/BULLET/BulletCollision/premake4.lua new file mode 100644 index 000000000..9bc0a9e60 --- /dev/null +++ b/WickedEngine/BULLET/BulletCollision/premake4.lua @@ -0,0 +1,11 @@ + project "BulletCollision" + + kind "StaticLib" + targetdir "../../lib" + includedirs { + "..", + } + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/WickedEngine/BULLET/BulletDynamics/CMakeLists.txt b/WickedEngine/BULLET/BulletDynamics/CMakeLists.txt new file mode 100644 index 000000000..cc4727639 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/CMakeLists.txt @@ -0,0 +1,153 @@ +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ) + + + +SET(BulletDynamics_SRCS + Character/btKinematicCharacterController.cpp + ConstraintSolver/btConeTwistConstraint.cpp + ConstraintSolver/btContactConstraint.cpp + ConstraintSolver/btFixedConstraint.cpp + ConstraintSolver/btGearConstraint.cpp + ConstraintSolver/btGeneric6DofConstraint.cpp + ConstraintSolver/btGeneric6DofSpringConstraint.cpp + ConstraintSolver/btHinge2Constraint.cpp + ConstraintSolver/btHingeConstraint.cpp + ConstraintSolver/btPoint2PointConstraint.cpp + ConstraintSolver/btSequentialImpulseConstraintSolver.cpp + ConstraintSolver/btSliderConstraint.cpp + ConstraintSolver/btSolve2LinearConstraint.cpp + ConstraintSolver/btTypedConstraint.cpp + ConstraintSolver/btUniversalConstraint.cpp + Dynamics/btDiscreteDynamicsWorld.cpp + Dynamics/btRigidBody.cpp + Dynamics/btSimpleDynamicsWorld.cpp + Dynamics/Bullet-C-API.cpp + Vehicle/btRaycastVehicle.cpp + Vehicle/btWheelInfo.cpp + Featherstone/btMultiBody.cpp + Featherstone/btMultiBodyConstraintSolver.cpp + Featherstone/btMultiBodyDynamicsWorld.cpp + Featherstone/btMultiBodyJointLimitConstraint.cpp + Featherstone/btMultiBodyConstraint.cpp + Featherstone/btMultiBodyPoint2Point.cpp + Featherstone/btMultiBodyJointMotor.cpp + MLCPSolvers/btDantzigLCP.cpp + MLCPSolvers/btMLCPSolver.cpp +) + +SET(Root_HDRS + ../btBulletDynamicsCommon.h + ../btBulletCollisionCommon.h +) +SET(ConstraintSolver_HDRS + ConstraintSolver/btConeTwistConstraint.h + ConstraintSolver/btConstraintSolver.h + ConstraintSolver/btContactConstraint.h + ConstraintSolver/btContactSolverInfo.h + ConstraintSolver/btFixedConstraint.h + ConstraintSolver/btGearConstraint.h + ConstraintSolver/btGeneric6DofConstraint.h + ConstraintSolver/btGeneric6DofSpringConstraint.h + ConstraintSolver/btHinge2Constraint.h + ConstraintSolver/btHingeConstraint.h + ConstraintSolver/btJacobianEntry.h + ConstraintSolver/btPoint2PointConstraint.h + ConstraintSolver/btSequentialImpulseConstraintSolver.h + ConstraintSolver/btSliderConstraint.h + ConstraintSolver/btSolve2LinearConstraint.h + ConstraintSolver/btSolverBody.h + ConstraintSolver/btSolverConstraint.h + ConstraintSolver/btTypedConstraint.h + ConstraintSolver/btUniversalConstraint.h +) +SET(Dynamics_HDRS + Dynamics/btActionInterface.h + Dynamics/btDiscreteDynamicsWorld.h + Dynamics/btDynamicsWorld.h + Dynamics/btSimpleDynamicsWorld.h + Dynamics/btRigidBody.h +) +SET(Vehicle_HDRS + Vehicle/btRaycastVehicle.h + Vehicle/btVehicleRaycaster.h + Vehicle/btWheelInfo.h +) + +SET(Featherstone_HDRS + Featherstone/btMultiBody.h + Featherstone/btMultiBodyConstraintSolver.h + Featherstone/btMultiBodyDynamicsWorld.h + Featherstone/btMultiBodyLink.h + Featherstone/btMultiBodyLinkCollider.h + Featherstone/btMultiBodySolverConstraint.h + Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyJointLimitConstraint.h + Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyPoint2Point.h + Featherstone/btMultiBodyJointMotor.h +) + +SET(MLCPSolvers_HDRS + MLCPSolvers/btDantzigLCP.h + MLCPSolvers/btDantzigSolver.h + MLCPSolvers/btMLCPSolver.h + MLCPSolvers/btMLCPSolverInterface.h + MLCPSolvers/btPATHSolver.h + MLCPSolvers/btSolveProjectedGaussSeidel.h +) + +SET(Character_HDRS + Character/btCharacterControllerInterface.h + Character/btKinematicCharacterController.h +) + + + +SET(BulletDynamics_HDRS + ${Root_HDRS} + ${ConstraintSolver_HDRS} + ${Dynamics_HDRS} + ${Vehicle_HDRS} + ${Character_HDRS} + ${Featherstone_HDRS} + ${MLCPSolvers_HDRS} +) + + +ADD_LIBRARY(BulletDynamics ${BulletDynamics_SRCS} ${BulletDynamics_HDRS}) +SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletDynamics BulletCollision LinearMath) +ENDIF (BUILD_SHARED_LIBS) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletDynamics DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletDynamics RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + INSTALL(FILES ../btBulletDynamicsCommon.h +DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${ConstraintSolver_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/ConstraintSolver) + SET_PROPERTY(SOURCE ${Dynamics_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Dynamics) + SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle) + SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character) + SET_PROPERTY(SOURCE ${Featherstone_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Featherstone) + SET_PROPERTY(SOURCE ${MLCPSolvers_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/MLCPSolvers) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h b/WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h new file mode 100644 index 000000000..dffb06dfe --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Character/btCharacterControllerInterface.h @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..8f1cd20bf --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -0,0 +1,770 @@ +/* +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 new file mode 100644 index 000000000..add6f30a6 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Character/btKinematicCharacterController.h @@ -0,0 +1,170 @@ +/* +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 new file mode 100644 index 000000000..15a4c92de --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -0,0 +1,1141 @@ +/* +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 new file mode 100644 index 000000000..1735b524d --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -0,0 +1,381 @@ +/* +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 new file mode 100644 index 000000000..1ba1cd1e8 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -0,0 +1,64 @@ +/* +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 new file mode 100644 index 000000000..9d60d9957 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -0,0 +1,178 @@ +/* +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 new file mode 100644 index 000000000..477c79d17 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -0,0 +1,71 @@ +/* +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 new file mode 100644 index 000000000..c07e9bbd8 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -0,0 +1,159 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..f93a3280f --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp @@ -0,0 +1,129 @@ +/* +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 new file mode 100644 index 000000000..697e319e2 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btFixedConstraint.h @@ -0,0 +1,49 @@ +/* +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 new file mode 100644 index 000000000..bcd457b67 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.cpp @@ -0,0 +1,54 @@ +/* +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 new file mode 100644 index 000000000..f9afcb912 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -0,0 +1,152 @@ +/* +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 new file mode 100644 index 000000000..bc2b5a85d --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -0,0 +1,1063 @@ +/* +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 new file mode 100644 index 000000000..431a52416 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -0,0 +1,640 @@ +/* +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 new file mode 100644 index 000000000..6f765884e --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -0,0 +1,185 @@ +/* +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 new file mode 100644 index 000000000..1b2e0f62c --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -0,0 +1,121 @@ +/* +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 new file mode 100644 index 000000000..29123d526 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "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 new file mode 100644 index 000000000..9a0049869 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -0,0 +1,60 @@ +/* +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 new file mode 100644 index 000000000..c18974130 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -0,0 +1,1046 @@ +/* +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 new file mode 100644 index 000000000..7c33ac24e --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -0,0 +1,412 @@ +/* +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 new file mode 100644 index 000000000..125580d19 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -0,0 +1,155 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..3c0430b90 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -0,0 +1,229 @@ +/* +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 new file mode 100644 index 000000000..912189494 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -0,0 +1,175 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..be93e3543 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -0,0 +1,1739 @@ +/* +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 new file mode 100644 index 000000000..180d2a385 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -0,0 +1,148 @@ +/* +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 new file mode 100644 index 000000000..aff9f27f5 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -0,0 +1,864 @@ +/* +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 new file mode 100644 index 000000000..57ebb47d8 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -0,0 +1,361 @@ +/* +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 new file mode 100644 index 000000000..0c7dbd668 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -0,0 +1,255 @@ +/* +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 new file mode 100644 index 000000000..e8bfabf86 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -0,0 +1,107 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_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 new file mode 100644 index 000000000..27ccefe41 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -0,0 +1,306 @@ +/* +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 new file mode 100644 index 000000000..5515e6b31 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -0,0 +1,80 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..27fdd9d3d --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -0,0 +1,222 @@ +/* +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 new file mode 100644 index 000000000..b58f984d0 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -0,0 +1,544 @@ +/* +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 new file mode 100644 index 000000000..b009f41ae --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp @@ -0,0 +1,87 @@ +/* +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 new file mode 100644 index 000000000..9e7084104 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/ConstraintSolver/btUniversalConstraint.h @@ -0,0 +1,65 @@ +/* +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 new file mode 100644 index 000000000..bd8e27483 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -0,0 +1,405 @@ +/* +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 new file mode 100644 index 000000000..e1fea3a49 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btActionInterface.h @@ -0,0 +1,46 @@ +/* +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 new file mode 100644 index 000000000..fb8a4068e --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -0,0 +1,1459 @@ +/* +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 new file mode 100644 index 000000000..d8a34b7da --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -0,0 +1,234 @@ +/* +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 new file mode 100644 index 000000000..35dd1400f --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -0,0 +1,167 @@ +/* +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 new file mode 100644 index 000000000..222f90066 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.cpp @@ -0,0 +1,400 @@ +/* +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 new file mode 100644 index 000000000..ed90fb441 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btRigidBody.h @@ -0,0 +1,604 @@ +/* +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 new file mode 100644 index 000000000..35dd38840 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -0,0 +1,280 @@ +/* +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 new file mode 100644 index 000000000..d48d2e39c --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -0,0 +1,89 @@ +/* +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 new file mode 100644 index 000000000..56a1c55d9 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.cpp @@ -0,0 +1,1009 @@ +/* + * 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 new file mode 100644 index 000000000..7177bebbf --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBody.h @@ -0,0 +1,466 @@ +/* + * 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 new file mode 100644 index 000000000..44e04c3a1 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -0,0 +1,527 @@ +#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 new file mode 100644 index 000000000..9fa317330 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -0,0 +1,166 @@ +/* +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 new file mode 100644 index 000000000..577f84622 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -0,0 +1,795 @@ +/* +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 new file mode 100644 index 000000000..0f4cd69c0 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -0,0 +1,85 @@ +/* +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 new file mode 100644 index 000000000..0910f8f6a --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -0,0 +1,578 @@ +/* +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 new file mode 100644 index 000000000..ad57a346d --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -0,0 +1,56 @@ +/* +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 new file mode 100644 index 000000000..ea309e885 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -0,0 +1,133 @@ +/* +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 new file mode 100644 index 000000000..0c7fc1708 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -0,0 +1,44 @@ +/* +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 new file mode 100644 index 000000000..ab5a43023 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -0,0 +1,89 @@ +/* +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 new file mode 100644 index 000000000..f66900491 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -0,0 +1,143 @@ +/* +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 new file mode 100644 index 000000000..26ca12b40 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -0,0 +1,60 @@ +/* +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 new file mode 100644 index 000000000..cf06dfb9e --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -0,0 +1,82 @@ +/* +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 new file mode 100644 index 000000000..3bf7b5c13 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -0,0 +1,2079 @@ +/************************************************************************* +* * +* 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 new file mode 100644 index 000000000..2a2f2d3d3 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btDantzigSolver.h @@ -0,0 +1,112 @@ +/* +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 new file mode 100644 index 000000000..25bb3f6d3 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h @@ -0,0 +1,33 @@ +/* +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 new file mode 100644 index 000000000..9ec31a6d4 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/MLCPSolvers/btPATHSolver.h @@ -0,0 +1,151 @@ +/* +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 new file mode 100644 index 000000000..77b475b96 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -0,0 +1,771 @@ +/* + * 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 new file mode 100644 index 000000000..f59555f94 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -0,0 +1,236 @@ +/* + * 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 new file mode 100644 index 000000000..3cc909c65 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Vehicle/btVehicleRaycaster.h @@ -0,0 +1,35 @@ +/* + * 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 new file mode 100644 index 000000000..ef93c16ff --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -0,0 +1,56 @@ +/* + * 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 new file mode 100644 index 000000000..f916053ec --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/Vehicle/btWheelInfo.h @@ -0,0 +1,119 @@ +/* + * 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/BulletDynamics/premake4.lua b/WickedEngine/BULLET/BulletDynamics/premake4.lua new file mode 100644 index 000000000..919edaa76 --- /dev/null +++ b/WickedEngine/BULLET/BulletDynamics/premake4.lua @@ -0,0 +1,11 @@ + project "BulletDynamics" + + kind "StaticLib" + targetdir "../../lib" + includedirs { + "..", + } + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/WickedEngine/BULLET/BulletSoftBody/CMakeLists.txt b/WickedEngine/BULLET/BulletSoftBody/CMakeLists.txt new file mode 100644 index 000000000..e66bd02d4 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/CMakeLists.txt @@ -0,0 +1,67 @@ + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src + +) + +#SUBDIRS( Solvers ) + +SET(BulletSoftBody_SRCS + btSoftBody.cpp + btSoftBodyConcaveCollisionAlgorithm.cpp + btSoftBodyHelpers.cpp + btSoftBodyRigidBodyCollisionConfiguration.cpp + btSoftRigidCollisionAlgorithm.cpp + btSoftRigidDynamicsWorld.cpp + btSoftSoftCollisionAlgorithm.cpp + btDefaultSoftBodySolver.cpp + +) + +SET(BulletSoftBody_HDRS + btSoftBody.h + btSoftBodyData.h + btSoftBodyConcaveCollisionAlgorithm.h + btSoftBodyHelpers.h + btSoftBodyRigidBodyCollisionConfiguration.h + btSoftRigidCollisionAlgorithm.h + btSoftRigidDynamicsWorld.h + btSoftSoftCollisionAlgorithm.h + btSparseSDF.h + + btSoftBodySolvers.h + btDefaultSoftBodySolver.h + + btSoftBodySolverVertexBuffer.h +) + + + +ADD_LIBRARY(BulletSoftBody ${BulletSoftBody_SRCS} ${BulletSoftBody_HDRS}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletSoftBody BulletDynamics) +ENDIF (BUILD_SHARED_LIBS) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletSoftBody DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletSoftBody RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES PUBLIC_HEADER "${BulletSoftBody_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp b/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp new file mode 100644 index 000000000..e90d24e6e --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -0,0 +1,151 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..1c17ffcbb --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btDefaultSoftBodySolver.h @@ -0,0 +1,63 @@ +/* +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 new file mode 100644 index 000000000..a0c8cca4a --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBody.cpp @@ -0,0 +1,3655 @@ +/* +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 new file mode 100644 index 000000000..ee1a3d952 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBody.h @@ -0,0 +1,1000 @@ +/* +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 new file mode 100644 index 000000000..9f0d44526 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -0,0 +1,357 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "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 new file mode 100644 index 000000000..11c7b88f9 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -0,0 +1,155 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..87d8841cf --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyData.h @@ -0,0 +1,217 @@ +/* +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 new file mode 100644 index 000000000..36f675a6c --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.cpp @@ -0,0 +1,1055 @@ +/* +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 new file mode 100644 index 000000000..620a52fe3 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyHelpers.h @@ -0,0 +1,143 @@ +/* +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 new file mode 100644 index 000000000..19d0543ef --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyInternals.h @@ -0,0 +1,908 @@ +/* +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 new file mode 100644 index 000000000..f5a67f6d8 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -0,0 +1,134 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "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 new file mode 100644 index 000000000..21addcfe2 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h @@ -0,0 +1,48 @@ +/* +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 new file mode 100644 index 000000000..c4733d640 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolverVertexBuffer.h @@ -0,0 +1,165 @@ +/* +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 new file mode 100644 index 000000000..6947bc27d --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftBodySolvers.h @@ -0,0 +1,154 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_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 new file mode 100644 index 000000000..01c148a2c --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -0,0 +1,86 @@ +/* +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 new file mode 100644 index 000000000..a9b513e36 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -0,0 +1,75 @@ +/* +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 new file mode 100644 index 000000000..5f3593545 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -0,0 +1,367 @@ +/* +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 new file mode 100644 index 000000000..3e0efafd6 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -0,0 +1,107 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_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 new file mode 100644 index 000000000..72043e69e --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp @@ -0,0 +1,48 @@ +/* +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 new file mode 100644 index 000000000..43b1439cc --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -0,0 +1,69 @@ +/* +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 new file mode 100644 index 000000000..bcf0c7982 --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/btSparseSDF.h @@ -0,0 +1,319 @@ +/* +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/BulletSoftBody/premake4.lua b/WickedEngine/BULLET/BulletSoftBody/premake4.lua new file mode 100644 index 000000000..339043f5f --- /dev/null +++ b/WickedEngine/BULLET/BulletSoftBody/premake4.lua @@ -0,0 +1,11 @@ + project "BulletSoftBody" + + kind "StaticLib" + targetdir "../../lib" + includedirs { + "..", + } + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/WickedEngine/BULLET/LinearMath/CMakeLists.txt b/WickedEngine/BULLET/LinearMath/CMakeLists.txt new file mode 100644 index 000000000..8d8a54b9e --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/CMakeLists.txt @@ -0,0 +1,72 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src +) + +SET(LinearMath_SRCS + btAlignedAllocator.cpp + btConvexHull.cpp + btConvexHullComputer.cpp + btGeometryUtil.cpp + btPolarDecomposition.cpp + btQuickprof.cpp + btSerializer.cpp + btVector3.cpp +) + +SET(LinearMath_HDRS + btAabbUtil2.h + btAlignedAllocator.h + btAlignedObjectArray.h + btConvexHull.h + btConvexHullComputer.h + btDefaultMotionState.h + btGeometryUtil.h + btGrahamScan2dConvexHull.h + btHashMap.h + btIDebugDraw.h + btList.h + btMatrix3x3.h + btMinMax.h + btMotionState.h + btPolarDecomposition.h + btPoolAllocator.h + btQuadWord.h + btQuaternion.h + btQuickprof.h + btRandom.h + btScalar.h + btSerializer.h + btStackAlloc.h + btTransform.h + btTransformUtil.h + btVector3.h +) + +ADD_LIBRARY(LinearMath ${LinearMath_SRCS} ${LinearMath_HDRS}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES SOVERSION ${BULLET_VERSION}) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #FILES_MATCHING requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS LinearMath DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS LinearMath + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES PUBLIC_HEADER "${LinearMath_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/WickedEngine/BULLET/LinearMath/btAabbUtil2.h b/WickedEngine/BULLET/LinearMath/btAabbUtil2.h new file mode 100644 index 000000000..d2997b4e6 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btAabbUtil2.h @@ -0,0 +1,232 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef 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 new file mode 100644 index 000000000..a65296c6a --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btAlignedAllocator.cpp @@ -0,0 +1,181 @@ +/* +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 new file mode 100644 index 000000000..f168f3c66 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btAlignedAllocator.h @@ -0,0 +1,107 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_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 new file mode 100644 index 000000000..24e59ab65 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btAlignedObjectArray.h @@ -0,0 +1,511 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef 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 new file mode 100644 index 000000000..69c52bc6f --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btConvexHull.h @@ -0,0 +1,241 @@ + +/* +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 new file mode 100644 index 000000000..d58ac955f --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btConvexHullComputer.cpp @@ -0,0 +1,2755 @@ +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include + +#include "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 new file mode 100644 index 000000000..7240ac4fb --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btConvexHullComputer.h @@ -0,0 +1,103 @@ +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..c90b74923 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btDefaultMotionState.h @@ -0,0 +1,42 @@ +#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 new file mode 100644 index 000000000..5ac230f71 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btGeometryUtil.cpp @@ -0,0 +1,185 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "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 new file mode 100644 index 000000000..a4b13b456 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btGeometryUtil.h @@ -0,0 +1,42 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef 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 new file mode 100644 index 000000000..e658c5cf0 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btGrahamScan2dConvexHull.h @@ -0,0 +1,117 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef 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 new file mode 100644 index 000000000..ce07db3ac --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btHashMap.h @@ -0,0 +1,450 @@ +/* +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 new file mode 100644 index 000000000..de97c3f87 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btIDebugDraw.h @@ -0,0 +1,445 @@ +/* +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 new file mode 100644 index 000000000..eec80a706 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btList.h @@ -0,0 +1,73 @@ +/* +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 new file mode 100644 index 000000000..14fe704f8 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btMatrix3x3.h @@ -0,0 +1,1367 @@ +/* +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 new file mode 100644 index 000000000..1c29632c5 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btMatrixX.h @@ -0,0 +1,504 @@ +/* +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 new file mode 100644 index 000000000..5b436e9ba --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btMinMax.h @@ -0,0 +1,71 @@ +/* +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 new file mode 100644 index 000000000..943181409 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btMotionState.h @@ -0,0 +1,40 @@ +/* +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 new file mode 100644 index 000000000..a4dca7fdd --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btPolarDecomposition.cpp @@ -0,0 +1,99 @@ +#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 new file mode 100644 index 000000000..561566764 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btPolarDecomposition.h @@ -0,0 +1,73 @@ +#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 new file mode 100644 index 000000000..ef2084537 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btPoolAllocator.h @@ -0,0 +1,121 @@ +/* +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 new file mode 100644 index 000000000..11067ef47 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btQuadWord.h @@ -0,0 +1,244 @@ +/* +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 new file mode 100644 index 000000000..665421de1 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btQuaternion.h @@ -0,0 +1,909 @@ +/* +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 new file mode 100644 index 000000000..544aee89d --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btQuickprof.cpp @@ -0,0 +1,566 @@ +/* + +*************************************************************************************************** +** +** 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 new file mode 100644 index 000000000..93f3f4a60 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btQuickprof.h @@ -0,0 +1,203 @@ + +/*************************************************************************************************** +** +** 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 new file mode 100644 index 000000000..4cbfc6bfe --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btRandom.h @@ -0,0 +1,42 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef 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 new file mode 100644 index 000000000..37c6dec19 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btScalar.h @@ -0,0 +1,731 @@ +/* +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 + #ifdef _MSC_VER + #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 + +//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 new file mode 100644 index 000000000..ba3449395 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btSerializer.cpp @@ -0,0 +1,991 @@ +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 new file mode 100644 index 000000000..ff1dc574c --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btSerializer.h @@ -0,0 +1,639 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef 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 new file mode 100644 index 000000000..907627379 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btTransform.h @@ -0,0 +1,305 @@ +/* +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 new file mode 100644 index 000000000..2303c2742 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btTransformUtil.h @@ -0,0 +1,228 @@ +/* +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 new file mode 100644 index 000000000..9389a25ca --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btVector3.cpp @@ -0,0 +1,1664 @@ +/* + 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 new file mode 100644 index 000000000..896859292 --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/btVector3.h @@ -0,0 +1,1352 @@ +/* +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)) +//#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/LinearMath/premake4.lua b/WickedEngine/BULLET/LinearMath/premake4.lua new file mode 100644 index 000000000..0f0a88a4e --- /dev/null +++ b/WickedEngine/BULLET/LinearMath/premake4.lua @@ -0,0 +1,11 @@ + project "LinearMath" + + kind "StaticLib" + targetdir "../../lib" + includedirs { + "..", + } + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/WickedEngine/BULLET/btBulletCollisionCommon.h b/WickedEngine/BULLET/btBulletCollisionCommon.h new file mode 100644 index 000000000..af981b5d3 --- /dev/null +++ b/WickedEngine/BULLET/btBulletCollisionCommon.h @@ -0,0 +1,68 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#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 new file mode 100644 index 000000000..50282bf21 --- /dev/null +++ b/WickedEngine/BULLET/btBulletDynamicsCommon.h @@ -0,0 +1,51 @@ +/* +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/BackLog.cpp b/WickedEngine/BackLog.cpp new file mode 100644 index 000000000..61fcdc9bb --- /dev/null +++ b/WickedEngine/BackLog.cpp @@ -0,0 +1,143 @@ +#include "BackLog.h" +//#include "GameComponents.h" + +//extern ID3D11Device* Renderer::graphicsDevice; + +deque BackLog::stream; +deque BackLog::history; +mutex BackLog::logMutex; +BackLog::State BackLog::state; +const float BackLog::speed=50.0f; +unsigned int BackLog::deletefromline = 100; +float BackLog::pos; +int BackLog::scroll; +int BackLog::RENDERWIDTH,BackLog::RENDERHEIGHT; +stringstream BackLog::inputArea; +int BackLog::historyPos=0; + + +void BackLog::Initialize(int width, int height){ + //stream.resize(0); + ResourceManager::add("images/logBG.png"); + RENDERWIDTH=width; + RENDERHEIGHT=height; + pos=RENDERHEIGHT; + scroll=0; + state=DISABLED; + deletefromline=100; + inputArea=stringstream(""); + //post("BackLog Created"); +} +void BackLog::CleanUp(){ + stream.clear(); +} +void BackLog::Toggle(){ + switch(state){ + case IDLE: + state=DEACTIVATING; + break; + case DISABLED: + state=ACTIVATING; + break; + default:break; + }; +} +void BackLog::Scroll(int dir){ + scroll+=dir; +} +void BackLog::Update(){ + if(state==DEACTIVATING) pos+=speed; + else if(state==ACTIVATING) pos-=speed; + if(pos>=RENDERHEIGHT) {state=DISABLED; pos=RENDERHEIGHT;} + else if(pos<=0) {state=IDLE; pos=0;} +} +void BackLog::Draw(){ + if(state!=DISABLED){ + Image::BatchBegin(); + ImageEffects fx = ImageEffects(RENDERWIDTH,RENDERHEIGHT); + fx.pos=XMFLOAT3(0,pos,0); + fx.opacity=WickedMath::Lerp(0,1,pos/RENDERHEIGHT); + Image::Draw((Renderer::TextureView)(ResourceManager::get("images/logBG.png")->data),fx); + Font::Draw(BackLog::getText(),"01",XMFLOAT4(5,pos-RENDERHEIGHT+75+scroll,0,-8),"left","bottom"); + Font::Draw(inputArea.str().c_str(),"01",XMFLOAT4(5,-RENDERHEIGHT+10,0,-8),"left","bottom"); + } +} + + +string BackLog::getText(){ + logMutex.lock(); + stringstream ss(""); + for(unsigned int i=0;ideletefromline){ + stream.pop_front(); + } + logMutex.unlock(); +} +void BackLog::input(const char& input){ + inputArea<deletefromline){ + history.pop_front(); + } + inputArea.str(""); + +#ifdef GAMECOMPONENTS + vector command(0); + while(!commandStream.eof()){ + string a = ""; + commandStream>>a; + command.push_back(a); + } + command.pop_back(); + GameComponents::consoleCommands.clear(); + GameComponents::consoleCommands=vector(command.begin(),command.end()); + GameComponents::wakeConsoleCommand=true; +#endif +} +void BackLog::deletefromInput(){ + stringstream ss(inputArea.str().substr(0,inputArea.str().length()-1)); + inputArea.str(""); + inputArea<::iterator iter=stream.begin(); iter!=stream.end(); ++iter) + file<c_str(); + file.close(); +} + +void BackLog::historyPrev(){ + if(!history.empty()){ + inputArea.str(""); + inputArea<0) + historyPos--; + inputArea.str(""); + inputArea< stream; + static unsigned int deletefromline; + static mutex logMutex; + static const float speed; + static float pos; + static int scroll; + static stringstream inputArea; + enum State{ + IDLE, + DISABLED, + ACTIVATING, + DEACTIVATING, + }; + static State state; + static int RENDERWIDTH, RENDERHEIGHT; +public: + static void Initialize(int width, int height); + static void CleanUp(); + static void Toggle(); + static void Scroll(int direction); + static void Update(); + static void Draw(); + + static string getText(); + static void clear(); + static void post(const char* input); + static void input(const char& input); + static void acceptInput(); + static void deletefromInput(); + static void save(ofstream& file); + + static deque history; + static int historyPos; + static void historyPrev(); + static void historyNext(); + + static bool isActive(){return state==IDLE;} +}; + +#endif diff --git a/WickedEngine/Camera.cpp b/WickedEngine/Camera.cpp new file mode 100644 index 000000000..022d4b694 --- /dev/null +++ b/WickedEngine/Camera.cpp @@ -0,0 +1,115 @@ +#include "Camera.h" + +Camera::Camera(int width, int height, float newNear, float newFar, const XMVECTOR& newEye) +{ + zNearP=newNear; + zFarP=newFar; + + this->width=width; + this->height=height; + + GetCursorPos(&originalMouse); + + //Initialize the projection matrix + Projection = XMMatrixPerspectiveFovLH( XM_PI / 3.0f, width / (FLOAT)height, zNearP, zFarP ); + //Projection = XMMatrixOrthographicLH(RENDERWIDTH,RENDERHEIGHT,0.1f,10000.0f); + Oprojection = XMMatrixOrthographicLH(width,height,0.1f,5000.0f); + + //View Matrix + defaultEye = Eye = newEye; + At = XMVectorAdd(Eye , XMVectorSet( 0.0f, -1, 0, 0.0f )); + Up = XMVectorAdd(Eye , XMVectorSet( 0.0f, 0, 1, 0.0f )); + defaultView = View = XMMatrixLookAtLH(Eye,At,Up); + refView=prevView=View; + + + leftrightRot=0; + updownRot=0; +} + + +void Camera::ProcessInput(float t, bool enter) +{ + POINT currentMouse; + GetCursorPos(¤tMouse); + if(enter) + { + LONG xDif = currentMouse.x-originalMouse.x; + LONG yDif = currentMouse.y-originalMouse.y; + leftrightRot+=0.1f*xDif*t; + updownRot+=0.1f*yDif*t; + SetCursorPos(originalMouse.x,originalMouse.y); + + } + else + { + GetCursorPos(&originalMouse); + } + + + XMMATRIX cameraRot = XMMatrixRotationX(updownRot-XM_PI/2.0f)*XMMatrixRotationY(leftrightRot); + + XMVECTOR cameraOrigTarg=XMVectorSet(0,-1,0,0); + At=XMVector3Transform(cameraOrigTarg,cameraRot); + XMVECTOR cameraFinalTarg=XMVectorAdd(Eye,At); + + + + XMVECTOR cameraOrigUp=XMVectorSet(0,0,1,0); + Up=XMVector3Transform(cameraOrigUp,cameraRot); + prevView=View; + View = XMMatrixLookAtLH( Eye, cameraFinalTarg, Up ); + + + + XMFLOAT4 refEyeHelper; + XMStoreFloat4(&refEyeHelper,Eye); + refEyeHelper.y*=-1.0f; + refEye = XMVectorSet( refEyeHelper.x, refEyeHelper.y, refEyeHelper.z, refEyeHelper.w ); + + + XMFLOAT4 refTarHelper; + XMStoreFloat4( &refTarHelper, cameraFinalTarg ); + refTarHelper.y*=-1.0f; + XMVECTOR refTarget = XMVectorSet(refTarHelper.x,refTarHelper.y,refTarHelper.z,refTarHelper.w); + + XMVECTOR camRight = XMVector3Transform(XMVectorSet(1,0,0,0),cameraRot); + XMVECTOR invUp = XMVector3Cross(camRight,XMVectorSubtract(refTarget,refEye)); + + + refView = XMMatrixLookAtLH( refEye, refTarget, invUp ); + +} + + +void Camera::AddtoCameraPosition(const XMVECTOR& movevector) +{ + XMMATRIX cameraRot=XMMatrixRotationX(updownRot)*XMMatrixRotationY(leftrightRot); + XMVECTOR rotVect=XMVector3Transform(movevector * 0.1f,cameraRot); + + Eye += rotVect; +} + +void Camera::Reset() +{ + leftrightRot=0; + updownRot=0; + Eye = defaultEye; + XMVECTOR At = XMVectorSet( 0.0f, 270, 0.0f, 0.0f ); + XMVECTOR Up = XMVectorSet( 0.0f, 1.0f, 0.0f, 0.0f ); + View = XMMatrixLookAtLH(Eye,At,Up); +} + +void Camera::CameraRelease() +{ +} + +void* Camera::operator new(size_t size) +{ + void* result = _aligned_malloc(size,16); + return result; +} +void Camera::operator delete(void* p) +{ + if(p) _aligned_free(p); +} diff --git a/WickedEngine/Camera.h b/WickedEngine/Camera.h new file mode 100644 index 000000000..5fbc44ac8 --- /dev/null +++ b/WickedEngine/Camera.h @@ -0,0 +1,31 @@ +#pragma once +#include "WickedEngine.h" + +class Camera +{ +private: + POINT originalMouse; +public: + XMMATRIX View, refView, prevView; + XMMATRIX Projection, Oprojection; + XMVECTOR Eye, refEye; + XMVECTOR Up,At; + float leftrightRot; + float updownRot; + float width,height; + float zNearP, zFarP; + + XMVECTOR defaultEye; + XMMATRIX defaultView; + + Camera(int newWidth,int newHeight, float newNear, float newFar, const XMVECTOR&); + void* operator new(size_t); + void operator delete(void*); + + void ProcessInput(float, bool); + void AddtoCameraPosition(const XMVECTOR&); + void Reset(); + void CameraRelease(); + +}; + diff --git a/WickedEngine/Character.cpp b/WickedEngine/Character.cpp new file mode 100644 index 000000000..13324c2a9 --- /dev/null +++ b/WickedEngine/Character.cpp @@ -0,0 +1,1744 @@ +#include "Renderer.h" +//#include "GameComponents.h" + +extern Camera* cam; + + +extern FLOAT shBias; +extern FLOAT BlackOut, BlackWhite, InvertCol; + + +float Character::hitFreeze; +bool Character::timestop=false; +bool Character::celShaded=true; + +void Character::updateCelShading(){ + for(Object* object : e_objects){ + for(Material* mat : object->mesh->materials){ + mat->toonshading=celShaded; + } + } +} + +Character::Character(const string& name, const string& newIdentifier, short pIndex, float pos, int costume) +{ + identifier=newIdentifier; + Renderer::Renderer(); + + playerIndex=pIndex; + + std::stringstream ss(""); + ss<<"characters/"<mesh->stencilRef=STENCILREF::STENCILREF_CHARACTER; + } + + startPosition=pos; + + + std::stringstream directory(""); + directory<<"characters/"<(armature,Combatant())); + } + + LoadMoveList(); + CreateStateMachine(); + + for(map::iterator iter=combatants.begin();iter!=combatants.end();++iter) + iter->second.ActivateState(iter->second.startState); + + corr=0; + onInputChanged=false; + noInput=true; + ClearBuffer(specialBuffer); + ClearBuffer(inputBuffer); + finalInput=""; + for(short i=0;i<5;i++){ + buttonPress[0][i]=0; + buttonPress[1][i]=0; + if(i<4) buttons[i]=0; + } + directions=0; + inputs.clear(); + commandBuffer.clear(); + currentInput=Input(); + inputTimer=0; + combocount=combodamage=0; + comboContinue=false; + comboShowTime=AFTERCOMBOSHOWTIME; + + ground_height=e_armatures[0]->translation.y; + + //combatants.resize(armatures.size()); for(int i=0;i::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter) + // TurnFacing(iter->first); + + timestopOverride=false; + + //ConstructMoveTree(); + + ResourceManager::add("sound/blockeffect0.wav",ResourceManager::SOUND); + + overrideColor = XMFLOAT4(0,0,0,0); +} +void Character::CompleteLoading(){ + +} + + +void* Character::operator new(size_t size) +{ + void* result = _aligned_malloc(size,16); + return result; +} +void Character::operator delete(void* p) +{ + if(p) _aligned_free(p); +} + +void Character::CleanUp() +{ + Entity::CleanUp(); + + for(map::iterator it=movelist.begin();it!=movelist.end();++it){ + it->second->CleanUp(); + delete it->second; + } + for(map::iterator it=stateCollection.begin();it!=stateCollection.end();++it){ + delete it->second; + } + movelist.clear(); + combatants.clear(); + stateCollection.clear(); + + ResourceManager::del(portraitName); + portrait=nullptr; + inputs.clear(); + commandBuffer.clear(); +} + +void Character::LoadMoveList() +{ + MoveListItem* currentMove=nullptr; + stringstream fname(""); + fname<<"characters/"<>moveName>>armName>>actName; + stringstream identified_armatureP(""); + identified_armatureP<(moveName,currentMove)); + + + currentMove->armature=getArmatureByName(currentMove->armatureP); + currentMove->actionI=getActionByName(currentMove->armature, currentMove->actionP); + + if(actName.back()=='}'){ + actName = actName.substr(0,actName.length()-3); + } + else{ + if(actName.back()=='{'){ + actName = actName.substr(0,actName.length()-2); + } + + string read = ""; + file>>read; + while(read.compare("}")){ + if(!read.compare("{")){ + + } + else if(ParseRequirement(currentMove,currentMove->requirements,read,file)){ + //Could read requirement, move on + } + else if(read.find("active")!=string::npos){ + string sphereP; + string frame; + float radius=1.0; + int damage=0,chip=0; + string prop = "default", height="high"; + int stun = 0; + int blockstun = 0; + int guardeater = 0; + int gain = 0; + int freeze = HITFREEZETIME; + int repeat = 1; + bool attach=false; + file>>sphereP>>frame; + + + string::size_type p = frame.find('['); + if(p!=string::npos){ + string rep = frame.substr(p+1,frame.length()-p-1); + repeat=atoi(rep.c_str()); + frame = frame.substr(0,p); + } + + string r=""; + file>>r; + while(r.back()!='}'){ + if(r.find("radius")!=string::npos){ + file>>radius; + } + else if(r.find("damage")!=string::npos){ + file>>damage; + } + else if(r.find("chip")!=string::npos){ + file>>chip; + } + else if(r.find("prop")!=string::npos){ + file>>prop; + } + else if(r.find("height")!=string::npos){ + file>>height; + } + else if(r.find("blockstun")!=string::npos){ + file>>blockstun; + } + else if(r.find("stun")!=string::npos){ + file>>stun; + } + else if(r.find("guard")!=string::npos){ + file>>guardeater; + } + else if(r.find("gain")!=string::npos){ + file>>gain; + } + else if(r.find("freeze")!=string::npos){ + file>>freeze; + } + else if(r.find("attach")!=string::npos){ + attach=true; + } + + file>>r; + } + + stringstream identified_sphere(""); + identified_sphere<>radius>>damage>>prop>>stun>>blockstun>>guardeater>>gain; + currentMove->aframes.push_back( ActiveFrame(identified_sphere.str(),frame,radius,damage,chip,prop,stun,blockstun,guardeater,gain,freeze,repeat,height,attach) ); + currentMove->aframes.back().sphere=getSphereByName(identified_sphere.str()); + } + else if(read.find("guard")!=string::npos){ + string frame; + stringstream height(""); + int gain = 0; + int freeze = HITFREEZETIME; + file>>frame; + + + string r=""; + file>>r; + while(r.back()!='}'){ + if(r.find("hei")!=string::npos){ + string h; + file>>h; + height<>gain; + } + else if(r.find("fre")!=string::npos){ + file>>freeze; + } + file>>r; + } + //file>>radius>>damage>>prop>>stun>>blockstun>>guardeater>>gain; + currentMove->gframes.push_back(GuardFrame(frame,height.str(),gain,freeze)); + } + else if(read.find("inv")!=string::npos){ + string frames=""; + file>>frames; + currentMove->invincible.push_back(Interval(frames)); + } + else if(read.find("push")!=string::npos || read.find("pull")!=string::npos){ + string frames=""; + float stren=0; + file>>frames>>stren; + currentMove->pushPull.push_back(PushPull(frames,stren)); + } + else if(read.find("propel")!=string::npos){ + string frames=""; + float stren=0; + file>>frames>>stren; + currentMove->propel.push_back(PushPull(frames,stren)); + } + else{ + string script=read; + currentMove->scripts.push_back(ScriptItem(script)); + int paramCount = Script_getParamCount(script.c_str()); + if(paramCount>=0) //params read + for(int i=0;i>s; + currentMove->scripts.back().params.push_back(s); + } + else{ //read block + string read=""; + file>>read; + stack blocks; + blocks.push(read); + while(!blocks.empty()){ + file>>read; + if(!strcmp(read.c_str(),"{")) {blocks.push(read); currentMove->scripts.back().params.push_back(read);} + else if(!strcmp(read.c_str(),"}")) {blocks.pop(); if(!blocks.empty())currentMove->scripts.back().params.push_back(read);} + else if(!strcmp(read.c_str(),"=")) continue; + else { + currentMove->scripts.back().params.push_back(read); + } + } + } + } + + + read.clear(); + file>>read; + } + } + } + + file.close(); + } + + + //if(file){ + // while(!file.eof()){ + // + // std::string voidStr=""; + // file>>voidStr; + + // switch(voidStr[0]){ + // case 'm': + // { + // std::string name=""; + // std::string armatureP=""; + // std::string actionP=""; + // std::string inputMotion=""; + // int repeatableS=-1,repeatableE=-1; //-1 for none + // std::string conditionIn=""; + // std::string typeIn=""; + // file>>name>>armatureP>>actionP>>conditionIn>>typeIn>>repeatableS; + // if(repeatableS>=0) + // file>>repeatableE; + // file>>inputMotion; + // if(!inputMotion.compare("0")) + // inputMotion=""; + + // stringstream identified_armatureP(""); + // identified_armatureP<( + // name,MoveListItem(name,identified_armatureP.str(),actionP,conditionIn,typeIn,repeatableS,repeatableE,inputMotion)) + // ); + // currentMove=&movelist[name]; + // } + // break; + // case 'a': + // { + // std::string sphereP; + // int frame0, frame1; + // float radius; + // int damage; + // std::string prop; + // int stun; + // int blockstun; + // int guardeater; + // int gain; + // file>>sphereP>>frame0>>frame1>>radius>>damage>>prop>>stun>>blockstun>>guardeater>>gain; + // currentMove->aframes.push_back( ActiveFrame(sphereP,frame0,frame1,radius,damage,prop,stun,blockstun,guardeater,gain) ); + // } + // break; + // case 'i': + // currentMove->invincible.push_back(Frames()); + // file>>currentMove->invincible.back().frame[0]>>currentMove->invincible.back().frame[1]; + // break; + // case 'c': + // { + // if(voidStr.length()>1) + // switch(voidStr[1]){ + // case 'm': + // { + // std::string move; + // int frame0,frame1; + // file>>move>>frame0>>frame1; + // currentMove->cancelFrames.push_back( CancelFrame(move,frame0,frame1) ); + // } + // break; + // case 'p': + // { + // std::string type; + // int frame0,frame1; + // file>>type>>frame0>>frame1; + // currentMove->cancelFrames.push_back( CancelFrame(examineType(type),frame0,frame1) ); + // } + // break; + // default: break; + // } + // + // if(voidStr.length()>2) + // switch(voidStr[2]){ + // case 'o': + // currentMove->cancelFrames.back().onHitOnly=true; + // break; + // default:break; + // } + // } + // break; + // case 'p': + // { + // currentMove->pushPull.push_back(PushPull()); + // float frameS = 0; + // file>>frameS; + // currentMove->pushPull.back().frames.frame[0]=frameS; + // if(frameS>=0) + // file>>currentMove->pushPull.back().frames.frame[1]; + // file>>currentMove->pushPull.back().directionStrength; + // } + // case 's': + // { + // std::string script=""; + // file>>script; + // currentMove->scripts.push_back(ScriptItem(script)); + // int paramCount = Script_getParamCount(script.c_str()); + // if(paramCount>=0) //params read + // for(int i=0;i>s; + // currentMove->scripts.back().params.push_back(s); + // } + // else{ //read block + // string read=""; + // file>>read; + // stack blocks; + // blocks.push(read); + // while(!blocks.empty()){ + // file>>read; + // if(!strcmp(read.c_str(),"{")) {blocks.push(read); currentMove->scripts.back().params.push_back(read);} + // else if(!strcmp(read.c_str(),"}")) {blocks.pop(); if(!blocks.empty())currentMove->scripts.back().params.push_back(read);} + // else if(!strcmp(read.c_str(),"=")) continue; + // else { + // currentMove->scripts.back().params.push_back(read); + // } + // } + // } + // } + // break; + // default: break; + // } + // } + //} + //file.close(); + + ////FIXATE THE INDEXERS + //for(map::iterator it=movelist.begin();it!=movelist.end();++it){ + // MoveListItem& move = *it->second; + // //move.armature=getArmatureByName(move.armatureP); + // //it->second.armature = armatures[it->second.armatureI]; + // //move.actionI=getActionByName(move.armature, move.actionP); + // for(int j=0;jactions[move.actionI].frameCount; + //} +} +Character::MoveListItem* Character::getMoveByName(const string& get){ + map::iterator it = movelist.find(get); + if(it==movelist.end()) + return nullptr; + return it->second; +} +Character::State* Character::getStateByName(const string& get){ + map::iterator it = stateCollection.find(get); + if(it==stateCollection.end()) + return nullptr; + return it->second; +} + +void Character::CreateStateMachine(){ + + for(map::iterator iter=movelist.begin();iter!=movelist.end();++iter){ + stateCollection.insert(pair(iter->first,new State(iter->first,iter->second))); + } + + stringstream fname(""); + fname<<"characters/"< > rootState(0); + State* state = nullptr; + while(!file.eof()){ + string read = ""; + file>>read; + + if(!read.compare("start:")){ //start symbol + string s="",a=""; + file>>s>>a; + rootState.push_back(pair(s,a)); + } + else if(read[0]=='-'){ //transition + string transName = ""; + if(read.back()=='{') //just getting the transition name without {} and - + transName = read.substr(1,read.length()-2); + else if(read.back()=='}') + transName = read.substr(1,read.length()-3); + else + transName = read.substr(1,read.length()-1); + + + Requirements requirements(0); + State* addState = getStateByName(transName); + + if(read.back()!='}'){ + string s=""; + file>>s; + while(s.compare("}")){ + ParseRequirement(state!=nullptr?state->move:nullptr,requirements,s,file); + s.clear(); + file>>s; + } + } + + if(addState!=nullptr && state!=nullptr) + state->transitions.push_back(StateTransition( addState,requirements )); + else{ +#ifdef BACKLOG + stringstream ss(""); + ss<<"[CreateStateMachine-Transition] Warning: "<& rs : rootState){ + stringstream identified_aname(""); + identified_aname<::iterator iter=combatants.begin();iter!=combatants.end();++iter){ + Combatant& combatant = iter->second; + + + for(StateTransition& trans : combatant.state->transitions){ + /* SIMPLE VERSION (NO ADDITIVE REQUIREMENTS) + bool valid = false; + for(Requirement* req : trans.requirements){ + valid = req->validate(this); + if(!valid) + break; + } + + if(valid){ + combatant.ActivateState(trans.state); + break; + } + */ + + map > valid; + for(Requirement* req : trans.requirements){ + valid[req->type].push_back( req->validate() ); + } + + bool invalid = false; + for(map >::iterator iter= valid.begin();iter!=valid.end();++iter){ + bool vali = false; + for(bool v : iter->second){ + if(v==true){ + vali=true; + break; + } + } + if(vali==false){ + invalid=true; + break; + } + } + + if(!invalid){ + for(Requirement* req : trans.requirements){ + req->activate(); + } + combatant.ActivateState(trans.state); + break; + } + } + } +} + +bool Character::ParseRequirement(MoveListItem* move, Requirements& requirements, const string& s, ifstream& file) +{ + if(!s.compare("input")){ + string args = ""; + file>>args; + requirements.push_back(new Req_input(this,args)); + return true; + } + else if(!s.compare("frame")){ + string args = ""; + file>>args; + if(move!=nullptr) + requirements.push_back(new Req_frame(this,args,move)); + return true; + } + else if(!s.compare("height")){ + string args = ""; + file>>args; + if(move!=nullptr) + requirements.push_back(new Req_height(this,args,move)); + return true; + } + else if(!s.compare("hit")){ + if(move!=nullptr) + requirements.push_back(new Req_hit(this,combatants[move->armature])); + return true; + } + else if(!s.compare("onstun")){ + if(move!=nullptr) + requirements.push_back(new Req_onstun(this,combatants[move->armature])); + return true; + } + else if(!s.compare("onhit")){ + if(move!=nullptr) + requirements.push_back(new Req_onhit(this,combatants[move->armature])); + return true; + } + else if(!s.compare("nostun")){ + if(move!=nullptr) + requirements.push_back(new Req_nostun(this,combatants[move->armature])); + return true; + } + else if(!s.compare("onblockstun")){ + if(move!=nullptr) + requirements.push_back(new Req_onblockstun(this,combatants[move->armature])); + return true; + } + else if(!s.compare("noblockstun")){ + if(move!=nullptr) + requirements.push_back(new Req_noblockstun(this,combatants[move->armature])); + return true; + } + else if(!s.compare("timeout")){ + string args = ""; + file>>args; + if(move!=nullptr) + requirements.push_back(new Req_timeout(this,args,combatants[move->armature])); + return true; + } + else if(!s.compare("died")){ + requirements.push_back(new Req_died(this)); + return true; + } + else if(!s.compare("health")){ + string args = ""; + file>>args; + requirements.push_back(new Req_health(this,args)); + } + else if(!s.compare("meter")){ + string args = ""; + file>>args; + requirements.push_back(new Req_meter(this,args)); + } + else if(!s.compare("facing")){ + requirements.push_back(new Req_facing(this,combatants[getArmatureByName(move->armatureP)])); + } + else if(!s.compare("canblock")){ + requirements.push_back(new Req_block(this,combatants[getArmatureByName(move->armatureP)])); + } + else if(!s.compare("falling")){ + requirements.push_back(new Req_falling(this,combatants[getArmatureByName(move->armatureP)])); + } + else if(!s.compare("hitproperty")){ + string args = ""; + file>>args; + requirements.push_back(new Req_hitproperty(this,combatants[getArmatureByName(move->armatureP)],args)); + } + + return false; +} + + +void Character::Reset() +{ + for(map::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter){ + iter->second.ActivateState(iter->second.startState); + iter->second.position.x=startPosition; + iter->second.position.y=0; + iter->second.stun=0; + iter->second.hitConfirmed=false; + iter->second.velocity=XMFLOAT2(0,0); + iter->second.hitIn=false; + + iter->first->translation_rest.x=iter->second.position.x; + iter->first->translation_rest.y=iter->second.position.y; + iter->first->translation_rest.z=0; + } + cData.Reset(); + combocount=0; + comboContinue=0; + combodamage=0; + hitFreeze=0; +} +void Character::UpdateC(bool inputAllowed) +{ + //for(map::iterator iter=combatants.begin();iter!=combatants.end();++iter) + // iter->second.hitConfirmed=false; + + updateCelShading(); + + if(cData.hp>0){ + if(inputAllowed) + ReadInput(); + else{ + for(short i=0;i<4;i++) buttons[i]=0; + ClearBuffer(specialBuffer); + } + } + else{ + directions=0; + for(short i=0;i<4;i++) buttons[i]=0; + } + + //SearchForInputAction(); + Action(); + StepStateMachine(); + + if(!hitFreeze && !IsTimeStop()){ + if(inputTimer>=0) inputTimer+=GetGameSpeed(); + + if(!comboContinue){ + comboShowTime-=GetGameSpeed(); + if(comboShowTime<=0) {comboShowTime=0; combocount=0;combodamage=0;} + if(cData.hpcData.hp_prev) cData.hp_prev=cData.hp; + } + if(comboContinue>0) comboContinue-=GetGameSpeed(); + overrideColor = WickedMath::Lerp(XMFLOAT4(0,0,0,0),overrideColor,0.7f); + if(overrideColor.w<0.1f) + overrideColor.w=0; + + VelocityStep(); + + for(map::iterator iter=combatants.begin();iter!=combatants.end();++iter){ + iter->second.stun-=GetGameSpeed(); + iter->second.blockstun-=GetGameSpeed(); + iter->second.state->move->timer+=GetGameSpeed(); + } + + + //Block(opponentShared); + //CheckForOnHit(inputAllowed, opponentShared); + //CheckForHitConfirm(opponentShared); + + + + + + + //Update(); + //UpdateSpheres(); + + //FadeTrails(); + + } + //ShareData(); + +} +void Character::PostUpdateC() +{ + + if(!hitFreeze && !IsTimeStop()){ + + UpdateSpheres(); + + FadeTrails(); + + for(map::iterator iter=combatants.begin();iter!=combatants.end();++iter) + ActivateFrames(iter->first); + + } + +} +void Character::PreUpdateC() +{ + if(!hitFreeze && !IsTimeStop()){ + + for(int i=0;iReset(); + + } +} +void Character::ReadInput() +{ + //corr++; + //if(corr>CORRECTIONTIME) { + // corr=0; + // for(short i=0;i<5;i++) buttons[i]=0; + //} + + ////if(!shared.stunTime){ + // directions = GameComponents::pContext[playerIndex].input.directions; + // if(GameComponents::pContext[playerIndex].input.buttons[0]) buttons[0] = GameComponents::pContext[playerIndex].input.buttons[0]; + // if(GameComponents::pContext[playerIndex].input.buttons[1]) buttons[1] = GameComponents::pContext[playerIndex].input.buttons[1]; + // if(GameComponents::pContext[playerIndex].input.buttons[2]) buttons[2] = GameComponents::pContext[playerIndex].input.buttons[2]; + // if(GameComponents::pContext[playerIndex].input.buttons[3]) buttons[3] = GameComponents::pContext[playerIndex].input.buttons[3]; + ////} + + + Input prevInput = currentInput; + //currentInput = Input(prevInput.frame+1,GameComponents::pContext[playerIndex].input.directions,GameComponents::pContext[playerIndex].input.buttons); + PressChange ch = currentInput.compare(prevInput); + if(ch == PRESSCHANGE_NONE){ + int h = prevInput.held+1; + currentInput.held=h; + if(!commandBuffer.empty()) + commandBuffer.back().held=h; + } + + if(!currentInput.isIdle()){ + if(currentInput.compare(prevInput)!=PRESSCHANGE_NONE ) + inputs.push_back(currentInput); + } + if(ch!=PRESSCHANGE_NONE){ + commandBuffer.push_back(currentInput); + if(ch==PRESSCHANGE_BUTTON) + commandBuffer.back().directions=D_IDLE; + else if(ch==PRESSCHANGE_DIRECTION) + commandBuffer.back().x=commandBuffer.back().y=commandBuffer.back().z=commandBuffer.back().w=false; + } + while(!commandBuffer.empty() && (currentInput.frame-commandBuffer.front().frame-commandBuffer.front().held>INPUTWINDOW)){ + commandBuffer.pop_front(); + } + + + stringstream ss(""); + for(Input& i : commandBuffer){ + //if(i.directions!=D_IDLE) + ss<<(i.held>=CHARGETIME?"h":"")<=CHARGETIME?"h":"")<activeAction; + float cf = armature->currentFrame; + Combatant& combatant = combatants[armature]; + MoveListItem& move = *combatant.state->move; + + combatant.activeSpheres.clear(); + combatant.hitSpheres.clear(); + + for(int i=0;iTYPE!=HitSphere::HitSphereType::ATKTYPE) spheres[j]->TYPE=HitSphere::HitSphereType::INVTYPE; + + for(int i=0;i0 && cf-(int)cf<=GetGameSpeed()){ + HitSphere* hs = move.aframes[i].sphere; + + if(hs!=nullptr && move.aframes[i].interval.contains(cf)){ + hs->TYPE=HitSphere::HitSphereType::ATKTYPE; + float rad = hs->radius*move.aframes[i].radius; + hs->radius = rad; + + combatant.activeSpheres.push_back(ASphere(hs->translation,rad)); + combatant.activeSpheres.back().damage=move.aframes[i].damage; + combatant.activeSpheres.back().chip=move.aframes[i].chip; + combatant.activeSpheres.back().freeze=move.aframes[i].freeze; + combatant.activeSpheres.back().guardeater=move.aframes[i].guardeater; + combatant.activeSpheres.back().stun=move.aframes[i].stun; + combatant.activeSpheres.back().blockstun=move.aframes[i].blockstun; + combatant.activeSpheres.back().prop=move.aframes[i].prop; + combatant.activeSpheres.back().height=move.aframes[i].height; + if(move.aframes[i].attach) + combatant.activeSpheres.back().attach=hs; + else + combatant.activeSpheres.back().attach=nullptr; + } + } + } + + combatant.blocking=false; + for(GuardFrame& g : move.gframes){ + if(g.interval.contains(cf)){ + combatant.blocking=true; + combatant.guard=g; + break; + } + } + + + for(HitSphere* s : spheres){ + //if(s->parentArmature==armature){ + if(s->TYPE!=HitSphere::HitSphereType::ATKTYPE && s->TYPE!=HitSphere::HitSphereType::INVTYPE){ + combatant.hitSpheres.push_back(Sphere(s->translation,s->radius)); + } + //} + } + +} +bool Character::CheckForOnHit(bool inputAllowed, Combatant& opponent){ + for(map::iterator iter=combatants.begin();iter!=combatants.end();++iter){ + Combatant& combatant = iter->second; + Armature* armature = iter->first; + MoveListItem& move = *combatant.state->move; + combatant.hitIn=false; + + for(Sphere& x : combatant.hitSpheres) + for(ASphere& y : opponent.activeSpheres) + //if(x.TYPE!=ATKTYPE && x.TYPE!=INVTYPE) + if(x.intersects(y)) { + combatant.hitIn=true; + + string attackHeight = y.height; + + + if(combatant.blocking && combatant.guard.height.find(attackHeight) != string::npos ){ //BLOCK + //spawn image + images.push_back( new oImage("images/block1.png","","") ); + images.back()->effects.blendFlag=BLENDMODE_ADDITIVE; + images.back()->effects.typeFlag=WORLD; + images.back()->effects.pivotFlag=Pivot::CENTER; + images.back()->effects.sampleFlag=SAMPLEMODE_CLAMP; + images.back()->effects.siz=XMFLOAT2(4,4); + images.back()->effects.pos.x=combatant.clip.pos.x+combatant.clip.siz.x*0.5f*combatant.prevFacing; + images.back()->effects.pos.y=combatant.clip.pos.y+combatant.clip.siz.y*0.5f; + images.back()->anim.fad=0.05f; + + + images.push_back( new oImage("images/block2.png","","") ); + images.back()->effects.blendFlag=BLENDMODE_ADDITIVE; + images.back()->effects.typeFlag=WORLD; + images.back()->effects.pivotFlag=Pivot::CENTER; + images.back()->effects.sampleFlag=SAMPLEMODE_CLAMP; + images.back()->effects.siz=XMFLOAT2(4,4); + images.back()->effects.pos.x=combatant.clip.pos.x+combatant.clip.siz.x*0.5f*combatant.prevFacing; + images.back()->effects.pos.y=combatant.clip.pos.y+combatant.clip.siz.y*0.5f; + images.back()->anim.fad=0.05f; + images.back()->anim.scaleX=-0.08f; + images.back()->anim.scaleY=-0.08f; + + + images.push_back( new oImage("images/block3.png","","") ); + images.back()->effects.blendFlag=BLENDMODE_ADDITIVE; + images.back()->effects.typeFlag=WORLD; + images.back()->effects.pivotFlag=Pivot::CENTER; + images.back()->effects.sampleFlag=SAMPLEMODE_CLAMP; + images.back()->effects.siz=XMFLOAT2(3,3); + images.back()->effects.pos.x=combatant.clip.pos.x+combatant.clip.siz.x*0.5f*combatant.prevFacing; + images.back()->effects.pos.y=combatant.clip.pos.y+combatant.clip.siz.y*0.5f; + images.back()->anim.fad=0.05f; + images.back()->anim.scaleX=0.05f; + images.back()->anim.scaleY=0.05f; + + ((SoundEffect*)ResourceManager::get("sound/blockeffect0.wav")->data)->Play(); + + #ifdef BACKLOG + stringstream ss(""); + ss<move->recordedHit.active=true; + opponent.state->move->recordedHit.pos=x.center; + if(inputAllowed) + cData.hp-=y.damage; + comboShowTime=AFTERCOMBOSHOWTIME; + + if(!comboContinue) {combocount=0;combodamage=0;} + comboContinue=y.stun; + combocount++; + combodamage+=y.damage; + + + hitFreeze=y.freeze; + + + combatant.stun=y.stun; + combatant.hitProperty = y.prop; + + + //ChangeAction(getMoveByName("StaggerLight"),getMainCombatant()); + + if(y.attach!=nullptr){ + armature->attachTo(y.attach,1,0,0); + } + + #ifdef BACKLOG + stringstream ss(""); + ss<<"Hit Confirm: player"<<1-playerIndex+1<<" hits player"<::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter){ + Combatant& combatant = iter->second; + Armature* armature = iter->first; + MoveListItem& move = *combatant.state->move; + + if(combatant.clip.intersects(opponent.block)){ + //int direction = GetDirection(directions); + //if( direction==D_LEFT || direction==D_DOWNLEFT ){ + // MoveListItem* block = getMoveByName(direction==D_LEFT?"StandingBlock":"CrouchingBlock"); + // if(block){ + // //TODO + // } + //} + combatant.canBlock=true; + } + else{ + combatant.canBlock=false; + } + } +} +bool Character::CheckForHitConfirm(Combatant& opponent){ + + for(map::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter){ + Combatant& combatant = iter->second; + Armature* armature = iter->first; + MoveListItem& move = *combatant.state->move; + + if(opponent.hitIn){ + combatant.hitConfirmed=true; + for(int j=0;jcurrentFrame; + if(move.aframes[j].interval.contains(cf)) { + move.aframes[j].active-=1; + + if(opponent.blocking){ + cData.meter+=move.aframes[j].gain/2; + } + else{ + cData.meter+=move.aframes[j].gain; + } + + } + } + } + + combatant.pushPull=0; + for(int j=0;jcurrentFrame)){ + combatant.pushPull+=move.pushPull[j].directionStrength; + if(move.pushPull[j].frames.begin<0) + move.pushPull[j].active=false; + } + } + combatant.propel=0; + for(int j=0;jcurrentFrame)){ + combatant.propel+=move.propel[j].directionStrength; + if(move.propel[j].frames.begin<0) + move.propel[j].active=false; + } + } + } + + + return opponent.hitIn; +} +void Character::Action() +{ + for(map::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter){ + Combatant& combatant = iter->second; + Armature* armature = iter->first; + + combatant.clip=cData.clip; + combatant.clip.pos.x+=combatant.position.x; + combatant.clip.pos.y+=combatant.position.y; + + combatant.block=Hitbox2D(); + + Script_ExecuteAll(*combatant.state->move); + combatant.state->move->recordedHit.active=false; + + //if(armature->translation.y>ground_height) { + // combatant.condition=AIRBORNE; + //} + //else combatant.condition=GROUNDED; + + //if((combatant.facing<0 && opponentShared.pos.x>combatant.position.x + // || combatant.facing>0 && opponentShared.pos.xactiveAction == combatant.idle) //needs chrouching too! + // TurnFacing(armature); + + } +} +void Character::VelocityStep() +{ + static const float GRAVITY = 0.027f; + static const float DECELERATION_GROUND = 0.9f; + static const float DECELERATION_AIR = 0.98f; + + for(map::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter){ + Combatant& combatant = iter->second; + Armature* armature = iter->first; + + XMFLOAT2 newPos = XMFLOAT2( combatant.position.x+combatant.velocity.x,combatant.position.y+combatant.velocity.y ); + XMFLOAT2 newVel = XMFLOAT2(0,0); + //if( + // newX>-XBOUNDS-8 + // && newXground_height) newVel.y = combatant.velocity.y-GRAVITY; + if(newPos.y0) newVel.x = combatant.velocity.x*DECELERATION_AIR; + if(abs(newVel.x)<0.001f) newVel.x=0.f; + + //if(combatant.position.x<-XBOUNDS-6) combatant.position.x=-XBOUNDS-6; + //if(combatant.position.x>XBOUNDS+6) combatant.position.x=XBOUNDS+6; + + combatant.position = WickedMath::Lerp(combatant.position,newPos,GetGameSpeed()); + combatant.velocity = WickedMath::Lerp(combatant.velocity,newVel,GetGameSpeed()); + + + armature->translation_rest.x=combatant.position.x; + armature->translation_rest.y=combatant.position.y; + armature->translation_rest.z=0; + } +} +void Character::ChangeAction(MoveListItem* move, Combatant& combatant, const string& triggeredInput){ + if(move){ + + Armature* armature=move->armature; + int actionI=move->actionI; + + ChangeArmatureAction(armature,actionI); + + combatant.hitConfirmed=false; + +#ifdef BACKLOG + stringstream ss(""); + ss<name<<" start new Action: "<name; + BackLog::post(ss.str().c_str()); +#endif + } +} +void Character::ChangeArmatureAction(Armature* armature, int actionI){ + if(armature!=nullptr + && actionI>=0 && actionIactions.size() ){ + + armature->prevAction=armature->activeAction; + armature->prevActionResolveFrame=armature->currentFrame; + armature->activeAction=actionI; + armature->currentFrame=1; //-5 for SMOOTH ACTION TRANSITION!! + } +} +bool Character::ArmatureInAction(Armature* armature, int action){ + int activeAction = armature->activeAction; + if(activeAction==action) return true; + return false; +} +//void Character::ShareData(){ +// shared.pos=getMainCombatant().position; +// shared.vel=getMainCombatant().velocity; +// shared.facing=getMainCombatant().facing; +// shared.clip=getMainCombatant().clip; +// shared.stunTime-=IsTimeStop()?0:GetGameSpeed(); +// if(shared.stunTime<0) shared.stunTime=0; +// shared.pushPull=0; +// +// for(map::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter){ +// Combatant& combatant = iter->second; +// Armature* armature = iter->first; +// MoveListItem& move = *combatant.state->move; +// +// if(!move.armature) +// break; +// +// float cf=move.armature->currentFrame; +// shared.move[armature]=&move; +// shared.activeSpheres[armature].clear(); +// for(int j=0;jcurrentFrame)){ +// shared.pushPull+=move.pushPull[j].directionStrength; +// } +// } +// } +// +// +//} +Character::Combatant& Character::getMainCombatant() +{ + return combatants[e_armatures[0]]; +} + +void Character::StopTime(){ + timestop=true; + timestopOverride=true; +} +void Character::UnStopTime(){ + timestop=false; + timestopOverride=false; +} +bool Character::IsTimeStop(){ + if(timestop){ + if(timestopOverride) + return false; + return true; + } + return false; +} + +void Character::TurnFacing(Armature* armature){ + //if(!combatants[armature].changeFacing){ + // combatants[armature].changeFacing=true; + // ChangeAction(getMoveByName("Turn"),combatants[armature]); + //} + //else if(!ArmatureInAction(armature,combatants[armature].turn)){ + // combatants[armature].changeFacing=false; + // combatants[armature].facing *= -1; + + XMVECTOR rotO,rotM,rotF; + rotO=XMLoadFloat4(&armature->rotation_rest); + rotM=XMVectorSet(0,1,0,0); + rotF=XMQuaternionNormalize( XMQuaternionMultiply(rotO,rotM) ); + XMStoreFloat4( &armature->rotation_rest,rotF ); + //} +} + +vector Character::getCombatants() +{ + vector c(0); + for(map::iterator iter=combatants.begin();iter!=combatants.end();++iter) + c.push_back(&iter->second); + return c; +} +void Character::Resolve(Character* p1, Character* p2, Level* level) +{ + vector p1c=p1->getCombatants(),p2c=p2->getCombatants(); + +#pragma region BLOCKING + for(Combatant* c : p2c) + p1->Block(*c); + for(Combatant* c : p1c) + p2->Block(*c); +#pragma endregion + + p1->UpdateC(true); + p2->UpdateC(true); + +#pragma region HITTING + if(!hitFreeze){ + for(Combatant* c : p2c) + p1->CheckForOnHit(true,*c); + for(Combatant* c : p1c) + p2->CheckForOnHit(true,*c); + for(Combatant* c : p2c) + p1->CheckForHitConfirm(*c); + for(Combatant* c : p1c) + p2->CheckForHitConfirm(*c); + } +#pragma endregion + +#pragma region CLIPPING +#define REPULSIVENESS 0.35f + + for(Combatant* c1 : p1c){ + for(Combatant* c2 : p2c){ + + if(c1->clip.intersects(c2->clip)) + { + bool p1onleft = (c1->position.x <= c2->position.x); + float curDis = abs(c1->clip.pos.x - c2->clip.pos.x); + float minDis = c1->clip.siz.x*0.5f + c2->clip.siz.x*0.5f; + float overlap = abs(curDis - minDis); + float ppp=overlap*GetGameSpeed() * (p1onleft?1:-1); + + p1->getMainCombatant().position.x-=ppp*REPULSIVENESS; + p2->getMainCombatant().position.x+=ppp*REPULSIVENESS; + } + + + float p1r = c1->clip.pos.x+c1->clip.siz.x*0.5f; + float p2r = c2->clip.pos.x+c2->clip.siz.x*0.5f; + float p1l = c1->clip.pos.x-c1->clip.siz.x*0.5f; + float p2l = c2->clip.pos.x-c2->clip.siz.x*0.5f; + float lr = level->playArea.pos.x+level->playArea.siz.x*0.5f; + float ll = level->playArea.pos.x-level->playArea.siz.x*0.5f; + float p1diff=0,p2diff=0; + + #pragma region PUSH/PULL AND PROPEL + + //if(!hitFreeze) + { + float dif = p1->getMainCombatant().position.x-p2->getMainCombatant().position.x; + float dir = dif/abs(dif); + float p1push = c2->pushPull*dir; + float p2push = c1->pushPull*-dir; + c1->velocity.x = (p1push?p1push:c1->velocity.x); + c2->velocity.x = (p2push?p2push:c2->velocity.x); + if(!c1->blocking) + c1->velocity.y = (c2->propel?c2->propel:c1->velocity.y); + if(!c2->blocking) + c2->velocity.y = (c1->propel?c1->propel:c2->velocity.y); + } + + #pragma endregion + + if(p1r>lr) + p1diff=lr-p1r; + if(p1llr) + p2diff=lr-p2r; + if(p2lposition.x+=p1diff; + if(p2diff) + c2->position.x+=p2diff; + + } + } + + + +#pragma endregion + +#pragma region FACING + if(p1->getMainCombatant().position.x>p2->getMainCombatant().position.x){ + for(Combatant* c1 : p1c){ + c1->facing=-1; + } + for(Combatant* c2 : p2c){ + c2->facing=1; + } + } + else{ + for(Combatant* c1 : p1c){ + c1->facing=1; + } + for(Combatant* c2 : p2c){ + c2->facing=-1; + } + } +#pragma endregion + + + p1->cData.hp=WickedMath::Clamp(p1->cData.hp,0,p1->cData.hp_max); + p1->cData.meter=WickedMath::Clamp(p1->cData.meter,0,100); + p2->cData.hp=WickedMath::Clamp(p2->cData.hp,0,p2->cData.hp_max); + p2->cData.meter=WickedMath::Clamp(p2->cData.meter,0,100); + +} + +void Character::ClearBuffer(DWORD buffer[BUFFERSIZE][5]) +{ + for(DWORD i=0;i=toFind.length()) + return true; + + i++; + } + return false; +} +void Character::StoreInBuffer() +{ + for(short i=0;i<5;i++) buttonPress[0][i]=buttonPress[1][i]; + buttonPress[1][0]=directions; + if(corr==CORRECTIONTIME){ + buttonPress[1][1]=buttons[0]; + buttonPress[1][2]=buttons[1]; + buttonPress[1][3]=buttons[2]; + buttonPress[1][4]=buttons[3]; + } + + //storing in input buffer + PressChange pressCH = checkForPressChange(); + if(pressCH!=PRESSCHANGE_NONE) + { + inputTimer=0; + DWORD NEWBUTTONPRESS[5]; + NEWBUTTONPRESS[0]=NEWBUTTONPRESS[1]=NEWBUTTONPRESS[2]=NEWBUTTONPRESS[3]=NEWBUTTONPRESS[4]=0; + if(pressCH==PRESSCHANGE_BOTH) memcpy(NEWBUTTONPRESS,buttonPress[1],sizeof(buttonPress[1])); + else if(pressCH==PRESSCHANGE_DIRECTION) NEWBUTTONPRESS[0]=buttonPress[1][0]; + else if(pressCH==PRESSCHANGE_BUTTON){ + NEWBUTTONPRESS[1]=buttonPress[1][1]; + NEWBUTTONPRESS[2]=buttonPress[1][2]; + NEWBUTTONPRESS[3]=buttonPress[1][3]; + NEWBUTTONPRESS[4]=buttonPress[1][4]; + } + + for(DWORD i=1;i=0) + memcpy(specialBuffer[gotfreespaceinspecialbuffer],NEWBUTTONPRESS,sizeof(NEWBUTTONPRESS)); + + } + else if((inputTimer>INPUTTIMEMAX && nothingPressed()) || specialBuffer[BUFFERSIZE-1][0]){ + noInput=TRUE; + inputTimer=0; + ClearBuffer(specialBuffer); + } + + + AssembleInputString(); + + if(!hitFreeze) + if(buttonPress[1][1] || buttonPress[1][2] || buttonPress[1][3] || buttonPress[1][4]){ + noInput=TRUE; + inputTimer=0; + ClearBuffer(specialBuffer); + } +} +void Character::AssembleInputString() +{ + //std::stringstream ss(""); + //for(int i=0;i Character::convertInputString(const string& val){ + vector v(0); + for(char x: val){ + switch(x){ + case 'x': + if(v.empty()) + v.push_back(Input()); + v.back().x=true; + break; + case 'y': + if(v.empty()) + v.push_back(Input()); + v.back().y=true; + break; + case 'z': + if(v.empty()) + v.push_back(Input()); + v.back().z=true; + break; + case 'w': + if(v.empty()) + v.push_back(Input()); + v.back().w=true; + break; + default: + v.push_back(Input()); + v.back().directions=atoi(&x); + break; + } + } + + return v; +} +void Character::ClearInput() +{ + ClearBuffer(specialBuffer); + finalInput=""; + + commandBuffer.clear(); + currentInput=Input(); +} +int Character::GetDirection(int inDir){ + if(getMainCombatant().facing<0){ + if(inDir==D_LEFTUP) return D_UPRIGHT; + if(inDir==D_LEFT) return D_RIGHT; + if(inDir==D_DOWNLEFT) return D_RIGHTDOWN; + if(inDir==D_UPRIGHT) return D_LEFTUP; + if(inDir==D_RIGHT) return D_LEFT; + if(inDir==D_RIGHTDOWN) return D_DOWNLEFT; + } + + return inDir; +} +Character::PressChange Character::checkForPressChange() +{ + bool directionCH = false; + bool buttonCH = false; + if(buttonPress[1][0]!=D_IDLE && buttonPress[1][0]!=buttonPress[0][0]) directionCH=true; + for(int i=1;i<5;++i) + if(buttonPress[1][i] && buttonPress[0][i]!=buttonPress[1][i]){buttonCH=true;break;} + + if(buttonCH && directionCH) return PRESSCHANGE_BOTH; + else if(buttonCH) return PRESSCHANGE_BUTTON; + else if(directionCH) return PRESSCHANGE_DIRECTION; + + return PRESSCHANGE_NONE; +} +bool Character::nothingPressed() +{ + short count=0; + if(buttonPress[0][0]==D_IDLE) + count++; + for(short i=1;i<5;i++) + if(!buttonPress[0][i]) count++; + return count==5? true:false; +} + + +void Character::IncreaseTrails(Object* object, const XMFLOAT3& col){ + if(GetGameSpeed() && object!=nullptr && object->mesh!=nullptr) + { + int base = object->mesh->trailInfo.base; + int tip = object->mesh->trailInfo.tip; + + + int x = object->trail.size(); + + if(base>=0 && tip>=0 && xmesh,base).pos; + tipP=TransformVertex(object->mesh,tip).pos; + XMStoreFloat4( &baseP, XMVector3Transform( XMLoadFloat4(&baseP),XMLoadFloat4x4(&object->world) ) ); + XMStoreFloat4( &tipP, XMVector3Transform( XMLoadFloat4(&tipP),XMLoadFloat4x4(&object->world) ) ); + + object->trail.push_back( RibbonVertex( XMFLOAT3(baseP.x,baseP.y,baseP.z),newBTex,XMFLOAT4(0,0,0,1) ) ); + object->trail.push_back( RibbonVertex( XMFLOAT3(tipP.x,tipP.y,tipP.z),newTTex,newCol ) ); + } + } +} +void Character::FadeTrails(){ +#define FADEOUT 0.06f + for(int i=0;itrail.size();++j){ + if(e_objects[i]->trail[j].col.w<=0 /*&& e_objects[i]->trail[j].inc==-1*/) e_objects[i]->trail.pop_front(); + else { + const float fade = /*e_objects[i]->trail[j].inc**/FADEOUT; + if(e_objects[i]->trail[j].col.x>0) e_objects[i]->trail[j].col.x=e_objects[i]->trail[j].col.x-fade*(GetGameSpeed()+GetGameSpeed()*(1-j%2)*2); + else e_objects[i]->trail[j].col.x=0; + if(e_objects[i]->trail[j].col.y>0) e_objects[i]->trail[j].col.y=e_objects[i]->trail[j].col.y-fade*(GetGameSpeed()+GetGameSpeed()*(1-j%2)*2); + else e_objects[i]->trail[j].col.y=0; + if(e_objects[i]->trail[j].col.z>0) e_objects[i]->trail[j].col.z=e_objects[i]->trail[j].col.z-fade*(GetGameSpeed()+GetGameSpeed()*(1-j%2)*2); + else e_objects[i]->trail[j].col.z=0; + if(e_objects[i]->trail[j].col.w>0) + e_objects[i]->trail[j].col.w-=fade*GetGameSpeed(); + else + e_objects[i]->trail[j].col.w=0; + /*if(e_objects[i]->trail[j].col.w>=1) + e_objects[i]->trail[j].inc=-1;*/ + } + } + } +} + +void Character::SetUpStatic(){ + ResourceManager::add("images/clipbox.png"); + ResourceManager::add("images/blockbox.png"); + hitFreeze=0; + timestop=false; +} +void Character::UpdateStatic(){ + if(hitFreeze>0) hitFreeze-=GameSpeed; + else hitFreeze=0; + + if(hitFreeze!=0) overrideGameSpeed=0; + else + overrideGameSpeed=1; +} + +void Character::DrawHitbox2D(ID3D11DeviceContext* context) +{ + Image::BatchBegin(context); + for(map::iterator iter=combatants.begin(); iter!=combatants.end(); ++iter){ + Combatant& combatant = iter->second; + { + ImageEffects fx(combatant.clip.pos.x,combatant.clip.pos.y,combatant.clip.siz.x,combatant.clip.siz.y); + fx.blendFlag=BLENDMODE_ALPHA; + fx.typeFlag=WORLD; + fx.pivotFlag=CENTER; + fx.lookAt=XMFLOAT4(0,0,1,1); + fx.opacity=0.3f; + Image::Draw((ID3D11ShaderResourceView*)ResourceManager::get("images/clipbox.png")->data,fx,context); + } + { + ImageEffects fx(combatant.block.pos.x,combatant.block.pos.y,combatant.block.siz.x,combatant.block.siz.y); + fx.blendFlag=BLENDMODE_ALPHA; + fx.typeFlag=WORLD; + fx.pivotFlag=CENTER; + fx.lookAt=XMFLOAT4(0,0,1,1); + fx.opacity=0.3f; + Image::Draw((ID3D11ShaderResourceView*)ResourceManager::get("images/blockbox.png")->data,fx,context); + } + } + + +} + + + +bool Character::Sphere::intersects(const Sphere& b){ + return WickedMath::Distance(center,b.center)<=radius+b.radius; +} + + +bool Character::Input::isIdle(){return (directions==D_IDLE && !x && !y && !z && !w);} \ No newline at end of file diff --git a/WickedEngine/Character.h b/WickedEngine/Character.h new file mode 100644 index 000000000..586cffc62 --- /dev/null +++ b/WickedEngine/Character.h @@ -0,0 +1,598 @@ +#ifndef CHARACTER +#define CHARACTER + +#include "Hitbox2D.h" +#include "WickedLoader.h" + + +class Character:public Entity{ +private: +#define BUFFERSIZE 24 +#define INPUTTIMEMAX 14 +#define CORRECTIONTIME 2 +#define AFTERCOMBOSHOWTIME 200 + + static const int INPUTWINDOW = 30; + static const int HITFREEZETIME = 12; + static const int CHARGETIME = 50; + + static float hitFreeze; + static bool timestop; + bool timestopOverride; + void StopTime(); + void UnStopTime(); + bool IsTimeStop(); + + static void ClearBuffer(DWORD buffer[BUFFERSIZE][5]); + static bool InputCompare(const string&, const string&); + + void IncreaseTrails(Object* object, const XMFLOAT3& col); + void FadeTrails(); + + + struct State; + struct Requirement; + struct Req_input; + struct Req_frame; + struct Req_height; + typedef vector Requirements; + struct StateTransition; + struct Combatant; + + + template + struct Interval{ + T begin,end; + Interval(){begin=end=(T)0;} + Interval(T a,T b){ + begin=a; + end=b; + } + Interval(T a){ + begin=a; + end=a; + } + Interval(const string& a){ + if(!a.empty()){ + if(a[0]=='('){ + int comma=a.find(','); + begin=(T)atof(a.substr(1,comma).c_str()); + string e=a.substr(comma+1,a.length()-comma-1); + end=(T)atof(e.c_str()); + } + else{ + begin=end=(T)atof(a.c_str()); + } + } + } + bool contains(T value){ + return ((T)begin<=value && value<=(T)end); + } + }; + + + enum PressChange{ + PRESSCHANGE_NONE, + PRESSCHANGE_DIRECTION, + PRESSCHANGE_BUTTON, + PRESSCHANGE_BOTH, + }; + PressChange checkForPressChange(); + struct Input{ + long frame, held; + short directions; + bool x,y,z,w,taunt; + + Input(){ + frame=held=0; + directions=0; + x=y=z=w=taunt=false; + } + Input(long frame,short directions,bool buttons[5]):frame(frame),directions(directions) + ,x(buttons[0]),y(buttons[1]),z(buttons[2]),w(buttons[3]),taunt(buttons[4]),held(0){} + Input(long frame, long held, short directions,bool buttons[5]):frame(frame),directions(directions) + ,x(buttons[0]),y(buttons[1]),z(buttons[2]),w(buttons[3]),taunt(buttons[4]),held(held){} + bool isIdle(); + bool equals(const Input& b){ + return (directions==b.directions && x==b.x && y==b.y && z==b.z && w==b.w && taunt==b.taunt); + } + PressChange compare(const Input& b){ + if(this->equals(b)) + return PRESSCHANGE_NONE; + if(directions==b.directions) + return PRESSCHANGE_BUTTON; + if(x==b.x && y==b.y && z==b.z && w==b.w && taunt==b.taunt) + return PRESSCHANGE_DIRECTION; + return PRESSCHANGE_BOTH; + } + }; + + struct Data + { + float hp_max, hp, hp_prev; + int meter; + Hitbox2D clip; + Data() {hp_max=hp=hp_prev=0;meter=0;clip=Hitbox2D();} + void Load(const string& dir, const string& chname){ + std::stringstream filename(""); + filename<>voidStr>>hp_max; + file>>voidStr>>clip.pos.x>>clip.pos.y>>clip.siz.x>>clip.siz.y; + } + file.close(); + hp=hp_prev=hp_max; + } + void Reset(){hp=hp_prev=hp_max;meter=0;} + }; + struct Sphere{ + float radius; + XMFLOAT3 center; + Sphere(){radius=0;center=XMFLOAT3(0,0,0);} + Sphere(const XMFLOAT3& c, float r):center(c),radius(r){} + bool intersects(const Sphere& b); + }; + struct ASphere : Sphere{ + int damage; + string prop; + string height; + int stun; + int blockstun; + int guardeater; + int chip; + int freeze; + Transform* attach; + + ASphere(){center=XMFLOAT3(0,0,0);radius=0;damage=chip=0;prop="default";height="high";stun=0;blockstun=0;guardeater=0;attach=nullptr;} + ASphere(const XMFLOAT3& c, float r):Sphere(c,r){damage=chip=0;prop="default";height="high";stun=0;blockstun=0;guardeater=0;attach=nullptr;} + }; + + struct ActiveFrame{ + string sphereP; + HitSphere* sphere; + Interval interval; + float radius; + int active,repeat; + int damage,chip,freeze; + string prop; + string height; + int stun; + int blockstun; + int guardeater; + int gain; + bool attach; + + ActiveFrame(){}; + ActiveFrame(const string& newSphereP,const string& frame, float newRadius, int newDamage, int newChip, const string& newProp, int newStun + ,int newBlockStun, int newGuardEater, int newGain, int newFreeze, int newRepeat, const string newHeight, bool newAttach){ + sphereP = newSphereP; + sphere=nullptr; + //frame[0]=frame0; + //frame[1]=frame1; + interval=Interval(frame); + radius=newRadius; + damage=newDamage; + chip=newChip; + prop=newProp; + stun=newStun; + blockstun=newBlockStun; + guardeater=newGuardEater; + gain=newGain; + freeze=newFreeze; + repeat=newRepeat; + height=newHeight; + attach=newAttach; + Reset(); + } + void Reset(){ + active=repeat; + } + }; + struct GuardFrame{ + string height; + int gain; + int freeze; + Interval interval; + + GuardFrame(){ + interval=Interval(0,0); + gain=0; + freeze=0; + height=""; + } + GuardFrame(const string& newFrame, const string& newheight, int newGain, int newFreeze){ + interval=Interval(newFrame); + height=newheight; + gain=newGain; + freeze=newFreeze; + } + }; + struct ScriptItem{ + bool active; + string name; + vector params; + + ScriptItem(){name="";params.resize(0);} + ScriptItem(std::string newName){name=newName;params.resize(0);Reset();} + void Reset(){active=true;} + void Deactivate(){active=false;} + }; + struct RecordedHit{ + bool active; + XMFLOAT3 pos; + RecordedHit(){active=false;pos=XMFLOAT3(0,0,0);} + }; + struct PushPull{ + Interval frames; + float directionStrength; + bool active; + PushPull():frames(Interval()),directionStrength(0),active(true){} + PushPull(const string& frames, float stren):frames(Interval(frames)),directionStrength(stren),active(true){} + void Reset(){active=true;} + }; + struct MoveListItem{ + std::string name; + std::string armatureP; + Armature* armature; + std::string actionP; + int actionI; + Requirements requirements; + string triggeredInput; //stores the input on which this was triggered + RecordedHit recordedHit; + vector > invincible; + vector aframes; + vector gframes; + vector scripts; + vector pushPull,propel; + float timer; + + MoveListItem(){ + name=""; + triggeredInput=""; + armatureP=""; + actionP=""; + actionI=-1; + aframes.resize(0); + gframes.resize(0); + scripts.resize(0); + recordedHit=RecordedHit(); + invincible.resize(0); + pushPull.resize(0); + Reset(); + }; + MoveListItem(const string& newName,const string& newArmatureP,const string& newActionP){ + name=newName; + armatureP=newArmatureP; + actionP=newActionP; + aframes.clear(); + gframes.clear(); + invincible.clear(); + pushPull.clear(); + scripts.clear(); + Reset(); + } + + void CleanUp(){ + aframes.clear(); + scripts.clear(); + pushPull.clear(); + invincible.clear(); + for(int i=0;i movelist; + void LoadMoveList(); + MoveListItem* getMoveByName(const string&); + + static int Script_getParamCount(const char* script); + void Script_Move(const MoveListItem& move, int scriptI, float currentFrame); + void Script_MoveMul(MoveListItem& move, int scriptI, float currentFrame); + void Script_Dash(MoveListItem& move, int scriptI, float currentFrame); + void Script_ToggleAction(MoveListItem& move, int scriptI, float currentFrame); + void Script_OnHitAction(MoveListItem& move, int scriptI, float currentFrame); + void Script_RibbonTrail(MoveListItem& move, int scriptI, float currentFrame); + void Script_Clip(MoveListItem& move, int scriptI, float currentFrame); + void Script_Block(MoveListItem& move, int scriptI, float currentFrame); + void Script_Image(MoveListItem& move, int scriptI, float currentFrame); + void Script_Sound(MoveListItem& move, int scriptI, float currentFrame); + void Script_Cam(MoveListItem& move, int scriptI, float currentFrame); + void Script_Burst(MoveListItem& move, int scriptI, float currentFrame); + void Script_Emit(MoveListItem& move, int scriptI, float currentFrame); + void Script_Opacity(MoveListItem& move, int scriptI, float currentFrame); + void Script_Detach(MoveListItem& move, int scriptI, float currentFrame); + void Script_Decal(MoveListItem& move, int scriptI, float currentFrame); + void Script_ExecuteAll(MoveListItem& move); + void Script_Remove(MoveListItem& move, int scriptI); + + + void ReadInput(); + int GetDirection(int); + bool CheckForOnHit(bool inputAllowed, Combatant& opponent); + void Block(Combatant& opponent); + bool CheckForHitConfirm(Combatant& opponent); + void Action(); + void VelocityStep(); + static void ChangeArmatureAction(Armature* armature, int actionI); + bool ArmatureInAction(Armature* armature, int action); + //void ShareData(); + + bool nothingPressed(); + void StoreInBuffer(); + void AssembleInputString(); + static vector convertInputString(const string& val); + + short playerIndex; + + short corr; + bool buttons[5]; + DWORD directions; + DWORD buttonPress[2][5]; + + float startPosition; + float ground_height; + + void SetUpBaseActions(); + void ActivateFrames(Armature* armature); + void TurnFacing(Armature* armature); + + struct Combatant{ + State* startState,*state; + //Condition condition; + XMFLOAT2 position; + XMFLOAT2 velocity; + float facing,prevFacing; + bool hitConfirmed; + float stun, blockstun; + vector activeSpheres; + vector hitSpheres; + bool hitIn; + bool canBlock, blocking; + GuardFrame guard; + Hitbox2D clip,block; + float pushPull,propel; + string hitProperty; + + Combatant(){ + //condition=GROUNDED; + position=velocity=XMFLOAT2(0,0); + facing=prevFacing=1; + clip=block=Hitbox2D(); + startState=state=nullptr; + hitConfirmed=hitIn=blocking=false; + stun=blockstun=pushPull=0.0f; + hitProperty="default"; + } + void ActivateState(State* newState){ + //if(state!=newState /*|| (state && state->move && !state->move->requirements.empty())*/) + { + if(state && state->move) + state->move->Reset(); + state = newState; + ChangeAction(state->move,*this,"???"); + + //canBlock=blocking=false; + } + } + }; +public: + map combatants; + vector getCombatants(); + Combatant& getMainCombatant(); + XMFLOAT4 overrideColor; + + deque inputs; + Input currentInput; + deque commandBuffer; + +public: + Character(const string& name, const string& newIdentifier, short pIndex, float pos, int costume=1); + void CompleteLoading(); + void CleanUp(); + static void SetUpStatic(); + static void UpdateStatic(); + + void* operator new(size_t); + void operator delete(void*); + + void Reset(); + void ClearInput(); + void UpdateC(bool inputAllowed); + void PreUpdateC(); + void PostUpdateC(); + DWORD inputBuffer[BUFFERSIZE][5]; + DWORD specialBuffer[BUFFERSIZE][5]; + string finalInput,directinput; + float inputTimer; + bool onInputChanged, noInput; + + Data cData; + //SharedData shared; + string Name; + string portraitName; + ID3D11ShaderResourceView *portrait; + + unsigned int combocount,combodamage; + float comboShowTime; + float comboContinue; + + static void Resolve(Character*, Character*, Level*); + + void DrawHitbox2D(ID3D11DeviceContext* context); + + static bool celShaded; + void updateCelShading(); + +private: + static void ChangeAction(MoveListItem* move, Combatant& combatant, const string& triggeredInput=""); + + +#pragma region REQUIREMENTS + struct Requirement{ + string type; + vector data; + Character* c; + + Requirement(Character* c, const string& type):c(c),type(type){} + + virtual bool validate(){ + return true; + } + virtual void activate(){} + }; + struct Req_input : Requirement{ + string input; + bool overrider, special; + + Req_input(Character* c, const string& newData); + bool validate(); + }; + struct Req_timeout : Requirement{ + int timeout; + Combatant& combatant; + + Req_timeout(Character* c, const string& newData, Combatant& newCombatant); + bool validate(); + }; + struct Req_frame : Requirement{ + Interval interval; + MoveListItem* move; + + Req_frame(Character* c, const string& newData, MoveListItem* newMove); + bool validate(); + }; + struct Req_height : Requirement{ + Interval interval; + MoveListItem* move; + + Req_height(Character* c, const string& newData, MoveListItem* newMove); + bool validate(); + }; + struct Req_hit : Requirement{ + Combatant& combatant; + + Req_hit(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_nostun : Requirement{ + Combatant& combatant; + + Req_nostun(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_onstun : Requirement{ + Combatant& combatant; + + Req_onstun(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_onhit : Requirement{ + Combatant& combatant; + + Req_onhit(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_noblockstun : Requirement{ + Combatant& combatant; + + Req_noblockstun(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_onblockstun : Requirement{ + Combatant& combatant; + + Req_onblockstun(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_health : Requirement{ + int health; + + Req_health(Character* c, const string& newData); + bool validate(); + void activate(); + }; + struct Req_meter : Requirement{ + int meter; + + Req_meter(Character* c, const string& newData); + bool validate(); + void activate(); + }; + struct Req_facing : Requirement{ + Combatant& combatant; + + Req_facing(Character* c, Combatant& newCombatant); + bool validate(); + void activate(); + }; + struct Req_died : Requirement{ + Req_died(Character* c); + bool validate(); + }; + struct Req_block : Requirement{ + Combatant& combatant; + + Req_block(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_falling : Requirement{ + Combatant& combatant; + + Req_falling(Character* c, Combatant& newCombatant); + bool validate(); + }; + struct Req_hitproperty : Requirement{ + Combatant& combatant; + string hitproperty; + + Req_hitproperty(Character* c, Combatant& newCombatant, const string& prop); + bool validate(); + }; +#pragma endregion + struct StateTransition{ + State* state; + Requirements requirements; + + StateTransition(State* newState, const Requirements& newRequs){ + state = newState; + requirements.insert(requirements.end(),state->move->requirements.begin(),state->move->requirements.end()); + requirements.insert(requirements.end(),newRequs.begin(),newRequs.end()); + } + }; + struct State{ + string name; + MoveListItem* move; + vector transitions; + + State(const string& newName, MoveListItem* newMove){ + name=newName; + move=newMove; + } + }; + map stateCollection; + State* Character::getStateByName(const string& get); + + void CreateStateMachine(); + void StepStateMachine(); + bool ParseRequirement(MoveListItem* move, Requirements& requirements, const string& s, ifstream& file); +}; + +#endif diff --git a/WickedEngine/Client.cpp b/WickedEngine/Client.cpp new file mode 100644 index 000000000..15cc78132 --- /dev/null +++ b/WickedEngine/Client.cpp @@ -0,0 +1,47 @@ +#include "Client.h" + + +Client::Client(const string& newName, const string& ipaddress, int port) +{ + if(ConnectToHost(port,ipaddress.length()<=1?"127.0.0.1":ipaddress.c_str())){ + success=true; + string welcomeMsg; + if(receiveText(welcomeMsg)){ + stringstream ss(""); + ss<<"Client connected to: "< + bool sendData(const T& value){ + Network::sendData(PACKET_TYPE_OTHER,s); + return Network::sendData(value,s); + } + template + bool receiveData(T& value){ + return Network::receiveData(value,s); + } + + bool changeName(const string& newName); + bool sendMessage(const string& text); + + bool ConnectToHost(int PortNo, const char* IPAddress) + { + WSAData wsadata; + int error = WSAStartup(SCK_VERSION2,&wsadata); + + if(error) + return false; + + if(wsadata.wVersion != SCK_VERSION2){ + WSACleanup(); + return false; + } + + SOCKADDR_IN target; + + target.sin_family = AF_INET; + target.sin_port = htons(PortNo); + target.sin_addr.S_un.S_addr = inet_addr(IPAddress); + + s = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if(s == INVALID_SOCKET){ + return false; + } + + int opt = TRUE; + if( setsockopt(s, SOL_SOCKET, TCP_NODELAY, (char *)&opt, sizeof(opt)) < 0 ) + return false; + + //Try connecting: + + if(connect(s, (SOCKADDR*)&target, sizeof(target)) == SOCKET_ERROR){ + return false; + } + else + return true; + + } + + template + void Poll(T& data) + { + + if(!success) + return; + + //clear the socket set + FD_ZERO(&readfds); + + //add master socket to set + FD_SET(s, &readfds); + + + //wait for an activity on one of the sockets , timeout is NULL , so wait indefinitely + timeval time=timeval(); + time.tv_sec=0; + time.tv_usec=1; + + if ((select( s + 1 , &readfds , NULL , NULL , &time) < 0) && (errno!=EINTR)) + { + BackLog::post("select error"); + } + + //If something happened on the master socket , then its an incoming connection + if (FD_ISSET(s, &readfds)) + { + int command; + bool receiveSuccess = receiveData(command); + + if(!receiveSuccess){ + BackLog::post("Server no longer available. Please disconnect."); + closesocket(s); + success=false; + } + else{ + switch(command){ + case PACKET_TYPE_CHANGENAME: + { + string text; + if(receiveText(text)){ + stringstream ss(""); + ss<<"New server name is: "< + +static class CpuInfo +{ +private: + static bool m_canReadCpu; + static HQUERY m_queryHandle; + static HCOUNTER m_counterHandle; + static unsigned long m_lastSampleTime; + static long m_cpuUsage; +public: + CpuInfo(); + ~CpuInfo(); + + static void Initialize(); + static void Shutdown(); + static void Frame(); + static int GetCpuPercentage(); +}; + diff --git a/WickedEngine/Cube.cpp b/WickedEngine/Cube.cpp new file mode 100644 index 000000000..6d88729f8 --- /dev/null +++ b/WickedEngine/Cube.cpp @@ -0,0 +1,89 @@ +#include "Cube.h" + + +ID3D11Buffer* Cube::vertexBuffer; +ID3D11Buffer* Cube::indexBuffer; + +Cube::Cube(const XMFLOAT3& center, const XMFLOAT3& halfwidth, const XMFLOAT4A& color) +{ + desc=Description(); + desc.center=center; + desc.halfwidth=halfwidth; + desc.color=color; + Transform(XMMatrixScaling(halfwidth.x,halfwidth.y,halfwidth.z)*XMMatrixTranslation(center.x,center.y,center.z)); +} + + +void Cube::CleanUpStatic() +{ + if(vertexBuffer){ + vertexBuffer->Release(); + vertexBuffer = NULL; + } + if(indexBuffer){ + indexBuffer->Release(); + indexBuffer = NULL; + } +} + +void Cube::SetUpVertices() +{ + vertexBuffer=NULL; + indexBuffer=NULL; + + Vertex* verts = new Vertex[8]; + + XMFLOAT3A min = XMFLOAT3A(-1,-1,-1); + XMFLOAT3A max = XMFLOAT3A(1,1,1); + verts[0].pos=min; + verts[1].pos=XMFLOAT3A(min.x,max.y,min.z); + verts[2].pos=XMFLOAT3A(min.x,max.y,max.z); + verts[3].pos=XMFLOAT3A(min.x,min.y,max.z); + verts[4].pos=XMFLOAT3A(max.x,min.y,min.z); + verts[5].pos=XMFLOAT3A(max.x,max.y,min.z); + verts[6].pos=max; + verts[7].pos=XMFLOAT3A(max.x,min.y,max.z); + + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DEFAULT; + bd.ByteWidth = sizeof( Vertex ) * 8; + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = 0; + D3D11_SUBRESOURCE_DATA InitData; + ZeroMemory( &InitData, sizeof(InitData) ); + InitData.pSysMem = verts; + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &vertexBuffer ); + + if(verts){ + delete[](verts); + verts=NULL; + } + + unsigned int indices[]={ + 0,1,1,2,0,3,0,4,1,5,4,5, + 5,6,4,7,2,6,3,7,2,3,6,7 + }; + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DEFAULT; + bd.ByteWidth = sizeof( unsigned int ) * 24; + bd.BindFlags = D3D11_BIND_INDEX_BUFFER; + bd.CPUAccessFlags = 0; + ZeroMemory( &InitData, sizeof(InitData) ); + InitData.pSysMem = indices; + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &indexBuffer ); +} + +void Cube::Transform(const XMFLOAT4X4& mat) +{ + desc.transform=mat; +} +void Cube::Transform(const XMMATRIX& mat) +{ + XMStoreFloat4x4( &desc.transform,mat ); +} + +void Cube::LoadStatic(){ + SetUpVertices(); +} \ No newline at end of file diff --git a/WickedEngine/Cube.h b/WickedEngine/Cube.h new file mode 100644 index 000000000..3f93d14b4 --- /dev/null +++ b/WickedEngine/Cube.h @@ -0,0 +1,48 @@ +#pragma once +#include "WickedEngine.h" + + +class Cube +{ +private: + struct Description{ + XMFLOAT3 center,halfwidth; + XMFLOAT4X4 transform; + XMFLOAT4A color; + + Description(){ + center=XMFLOAT3(0,0,0); + halfwidth=XMFLOAT3(1,1,1); + color=XMFLOAT4A(1,1,1,1); + transform=XMFLOAT4X4( + 1,0,0,0, + 0,1,0,0, + 0,0,1,0, + 0,0,0,1 + ); + }; + }; + struct Vertex + { + XMFLOAT3A pos; + + Vertex(){ pos=XMFLOAT3A(0,0,0); } + Vertex(const XMFLOAT3A& newPos){ pos=newPos; } + }; + static void SetUpVertices(); +public: + Cube(const XMFLOAT3& center=XMFLOAT3(0,0,0), const XMFLOAT3& halfwidth=XMFLOAT3(1,1,1), const XMFLOAT4A& color = XMFLOAT4A(1,1,1,1)); + void* operator new(size_t size){ void* result = _aligned_malloc(size,16); return result; } + void operator delete(void* p){ if(p) _aligned_free(p); } + + void Transform(const XMFLOAT4X4& mat); + void Transform(const XMMATRIX& mat); + + Description desc; + + static ID3D11Buffer* vertexBuffer; + static ID3D11Buffer* indexBuffer; + static void LoadStatic(); + static void CleanUpStatic(); +}; + diff --git a/WickedEngine/DepthTarget.cpp b/WickedEngine/DepthTarget.cpp new file mode 100644 index 000000000..b2cd50adf --- /dev/null +++ b/WickedEngine/DepthTarget.cpp @@ -0,0 +1,119 @@ +#include "DepthTarget.h" + + + +DepthTarget::DepthTarget() +{ + texture2D = NULL; + depthTarget = NULL; + shaderResource = NULL; +} + + +DepthTarget::~DepthTarget() +{ + if(texture2D) texture2D->Release(); texture2D = NULL; + if(depthTarget) depthTarget->Release(); depthTarget = NULL; + if(shaderResource) shaderResource->Release(); shaderResource = NULL; +} + +void DepthTarget::Initialize(int width, int height, UINT MSAAC, UINT MSAAQ) +{ + D3D11_TEXTURE2D_DESC depthBufferDesc; + ZeroMemory(&depthBufferDesc, sizeof(depthBufferDesc)); + + // Set up the description of the depth buffer. + depthBufferDesc.Width = width; + depthBufferDesc.Height = height; + depthBufferDesc.MipLevels = 1; + depthBufferDesc.ArraySize = 1; + depthBufferDesc.Format = DXGI_FORMAT_R24G8_TYPELESS; + depthBufferDesc.SampleDesc.Count = MSAAC; + depthBufferDesc.SampleDesc.Quality = MSAAQ; + depthBufferDesc.Usage = D3D11_USAGE_DEFAULT; + depthBufferDesc.BindFlags = D3D11_BIND_DEPTH_STENCIL | D3D11_BIND_SHADER_RESOURCE; + depthBufferDesc.CPUAccessFlags = 0; + depthBufferDesc.MiscFlags = 0; + + HRESULT hr; + // Create the texture for the depth buffer using the filled out description. + hr=Renderer::graphicsDevice->CreateTexture2D(&depthBufferDesc, NULL, &texture2D); + + D3D11_DEPTH_STENCIL_VIEW_DESC depthStencilViewDesc; + ZeroMemory(&depthStencilViewDesc, sizeof(depthStencilViewDesc)); + depthStencilViewDesc.Format = DXGI_FORMAT_D24_UNORM_S8_UINT; + depthStencilViewDesc.ViewDimension = (MSAAQ==0 ? D3D11_DSV_DIMENSION_TEXTURE2D : D3D11_DSV_DIMENSION_TEXTURE2DMS); + depthStencilViewDesc.Texture2D.MipSlice = 0; + depthStencilViewDesc.Flags = 0; + + // Create the depth stencil view. + hr=Renderer::graphicsDevice->CreateDepthStencilView(texture2D, &depthStencilViewDesc, &depthTarget); + + //Create Depth ShaderResource + D3D11_SHADER_RESOURCE_VIEW_DESC shaderResourceViewDesc; + ZeroMemory(&shaderResourceViewDesc, sizeof(shaderResourceViewDesc)); + shaderResourceViewDesc.Format = DXGI_FORMAT_R24_UNORM_X8_TYPELESS; + shaderResourceViewDesc.ViewDimension = (MSAAQ==0 ? D3D11_SRV_DIMENSION_TEXTURE2D : D3D11_SRV_DIMENSION_TEXTURE2DMS); + shaderResourceViewDesc.Texture2D.MostDetailedMip = 0; + shaderResourceViewDesc.Texture2D.MipLevels = 1; + + Renderer::graphicsDevice->CreateShaderResourceView(texture2D, &shaderResourceViewDesc, &shaderResource); +} +void DepthTarget::InitializeCube(int size) +{ + D3D11_TEXTURE2D_DESC depthBufferDesc; + ZeroMemory(&depthBufferDesc, sizeof(depthBufferDesc)); + + // Set up the description of the depth buffer. + depthBufferDesc.Width = size; + depthBufferDesc.Height = size; + depthBufferDesc.MipLevels = 1; + depthBufferDesc.ArraySize = 6; + depthBufferDesc.Format = DXGI_FORMAT_R32_TYPELESS; + depthBufferDesc.SampleDesc.Count = 1; + depthBufferDesc.SampleDesc.Quality = 0; + depthBufferDesc.Usage = D3D11_USAGE_DEFAULT; + depthBufferDesc.BindFlags = D3D11_BIND_DEPTH_STENCIL | D3D11_BIND_SHADER_RESOURCE; + depthBufferDesc.CPUAccessFlags = 0; + depthBufferDesc.MiscFlags = D3D11_RESOURCE_MISC_TEXTURECUBE; + + HRESULT hr; + // Create the texture for the depth buffer using the filled out description. + hr=Renderer::graphicsDevice->CreateTexture2D(&depthBufferDesc, NULL, &texture2D); + + D3D11_DEPTH_STENCIL_VIEW_DESC depthStencilViewDesc; + ZeroMemory(&depthStencilViewDesc, sizeof(depthStencilViewDesc)); + depthStencilViewDesc.Format = DXGI_FORMAT_D32_FLOAT; + depthStencilViewDesc.ViewDimension = D3D11_DSV_DIMENSION_TEXTURE2DARRAY; + depthStencilViewDesc.Texture2DArray.FirstArraySlice = 0; + depthStencilViewDesc.Texture2DArray.ArraySize = 6; + depthStencilViewDesc.Texture2DArray.MipSlice = 0; + depthStencilViewDesc.Flags = 0; + + // Create the depth stencil view. + hr=Renderer::graphicsDevice->CreateDepthStencilView(texture2D, &depthStencilViewDesc, &depthTarget); + + //Create Depth ShaderResource + D3D11_SHADER_RESOURCE_VIEW_DESC shaderResourceViewDesc; + ZeroMemory(&shaderResourceViewDesc, sizeof(shaderResourceViewDesc)); + shaderResourceViewDesc.Format = DXGI_FORMAT_R32_FLOAT; + shaderResourceViewDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURECUBE; + shaderResourceViewDesc.Texture2D.MostDetailedMip = 0; + shaderResourceViewDesc.Texture2D.MipLevels = 1; + + Renderer::graphicsDevice->CreateShaderResourceView(texture2D, &shaderResourceViewDesc, &shaderResource); +} + +void DepthTarget::Clear(ID3D11DeviceContext* context) +{ + context->ClearDepthStencilView( depthTarget, D3D11_CLEAR_DEPTH | D3D11_CLEAR_STENCIL, 1.0f, 0); +} +void DepthTarget::CopyFrom(const DepthTarget& from,ID3D11DeviceContext* context) +{ + if(shaderResource) shaderResource->Release(); + static D3D11_SHADER_RESOURCE_VIEW_DESC desc; + from.shaderResource->GetDesc(&desc); + context->CopyResource(texture2D,from.texture2D); + HRESULT r = Renderer::graphicsDevice->CreateShaderResourceView(texture2D,&desc,&shaderResource); +} + diff --git a/WickedEngine/DepthTarget.h b/WickedEngine/DepthTarget.h new file mode 100644 index 000000000..8c305a06e --- /dev/null +++ b/WickedEngine/DepthTarget.h @@ -0,0 +1,19 @@ +#pragma once +#include "WickedEngine.h" + +class DepthTarget +{ +public: + ID3D11Texture2D* texture2D; + ID3D11DepthStencilView* depthTarget; + ID3D11ShaderResourceView* shaderResource; + + DepthTarget(); + ~DepthTarget(); + + void Initialize(int width, int height, UINT MSAAC, UINT MSAAQ); + void InitializeCube(int size); + void Clear(ID3D11DeviceContext*); + void CopyFrom(const DepthTarget&, ID3D11DeviceContext*); +}; + diff --git a/WickedEngine/DirectInput.cpp b/WickedEngine/DirectInput.cpp new file mode 100644 index 000000000..d041d11a8 --- /dev/null +++ b/WickedEngine/DirectInput.cpp @@ -0,0 +1,284 @@ +#include "DirectInput.h" + + +DirectInput::DirectInput(HINSTANCE hinstance, HWND hwnd) +{ + for(int i=0;i<256;++i){ + m_keyboardState[i]=0; + } + Initialize(hinstance, hwnd); +} + + +IDirectInputDevice8* m_keyboard = 0; +IDirectInputDevice8* joystick[2] = {0,0}; +IDirectInput8* m_directInput = 0; + short DirectInput::connectedJoys=0; + +BOOL CALLBACK enumCallback(const DIDEVICEINSTANCE* instance, VOID* context) +{ + HRESULT hr; + + // Obtain an interface to the enumerated joystick. + hr = m_directInput->CreateDevice(instance->guidInstance, &joystick[DirectInput::connectedJoys], NULL); + + // If it failed, then we can't use this joystick. (Maybe the user unplugged + // it while we were in the middle of enumerating it.) + if (FAILED(hr)) { + return DIENUM_CONTINUE; + } + + // Stop enumeration. Note: we're just taking the first joystick we get. You + // could store all the enumerated joysticks and let the user pick. + DirectInput::connectedJoys++; + if(DirectInput::connectedJoys<2) + return DIENUM_CONTINUE; + else + return DIENUM_STOP; +} +BOOL CALLBACK enumAxesCallback(const DIDEVICEOBJECTINSTANCE* instance, VOID* context) +{ + HWND hDlg = (HWND)context; + + DIPROPRANGE propRange; + propRange.diph.dwSize = sizeof(DIPROPRANGE); + propRange.diph.dwHeaderSize = sizeof(DIPROPHEADER); + propRange.diph.dwHow = DIPH_BYID; + propRange.diph.dwObj = instance->dwType; + propRange.lMin = -1000; + propRange.lMax = +1000; + + // Set the range for the axis + for(short i=0;iSetProperty(DIPROP_RANGE, &propRange.diph))) { + return DIENUM_STOP; + } + + return DIENUM_CONTINUE; +} + + +HRESULT DirectInput::Initialize(HINSTANCE hinstance, HWND hwnd) +{ + HRESULT result; + + + // Initialize the main direct input interface. + result = DirectInput8Create(hinstance, DIRECTINPUT_VERSION, IID_IDirectInput8, (void**)&m_directInput, NULL); + if(FAILED(result)) + { + return E_FAIL; + } + + // Initialize the direct input interface for the keyboard. + result = m_directInput->CreateDevice(GUID_SysKeyboard, &m_keyboard, NULL); + if(FAILED(result)) + { + return E_FAIL; + } + + // Set the data format. In this case since it is a keyboard we can use the predefined data format. + result = m_keyboard->SetDataFormat(&c_dfDIKeyboard); + if(FAILED(result)) + { + return E_FAIL; + } + + // Set the cooperative level of the keyboard to not share with other programs. + result = m_keyboard->SetCooperativeLevel(hwnd, DISCL_FOREGROUND | DISCL_NONEXCLUSIVE); + if(FAILED(result)) + { + return E_FAIL; + } + + // Now acquire the keyboard. + result = m_keyboard->Acquire(); + if(FAILED(result)) + { + return E_FAIL; + } + m_keyboard->GetDeviceState(sizeof(m_keyboardState), (LPVOID)&m_keyboardState); + + InitJoy(hwnd); + + return S_OK; +} +HRESULT DirectInput::InitJoy(HWND hwnd){ + HRESULT result; + // Look for the first simple joystick we can find. + if (FAILED(result = m_directInput->EnumDevices(DI8DEVCLASS_GAMECTRL, ::enumCallback, NULL, DIEDFL_ATTACHEDONLY))) { + return result; + } + for(short i=0;iSetDataFormat(&c_dfDIJoystick2))) { + return result; + } + + // Set the cooperative level to let DInput know how this device should + // interact with the system and with other DInput applications. + if (FAILED(result = joystick[i]->SetCooperativeLevel(hwnd, DISCL_EXCLUSIVE | DISCL_FOREGROUND))) { + return result; + } + + // Determine how many axis the joystick has (so we don't error out setting + // properties for unavailable axis) + capabilities.dwSize = sizeof(DIDEVCAPS); + if (FAILED(result = joystick[i]->GetCapabilities(&capabilities))) { + return result; + } + + // Enumerate the axes of the joyctick and set the range of each axis. Note: + // we could just use the defaults, but we're just trying to show an example + // of enumerating device objects (axes, buttons, etc.). + if (FAILED(result = joystick[i]->EnumObjects(::enumAxesCallback, NULL, DIDFT_AXIS))) + return result; + } + + + + return S_OK; +} + +void DirectInput::Shutdown() +{ + // Release the keyboard. + if(m_keyboard) + { + m_keyboard->Unacquire(); + m_keyboard->Release(); + m_keyboard = 0; + } + // Release Joypad + if(joystick){ + for(short i=0;iUnacquire(); + joystick[i]->Release(); + joystick[i] = 0; + } + } + // Release the main interface to direct input. + if(m_directInput) + { + m_directInput->Release(); + m_directInput = 0; + } + + + return; +} + +bool DirectInput::Frame() +{ + if(m_keyboard==nullptr) + return false; + // Read the keyboard device. + HRESULT result = m_keyboard->GetDeviceState(sizeof(m_keyboardState), (LPVOID)&m_keyboardState); + if(FAILED(result)) + { + // If the keyboard lost focus or was not acquired then try to get control back. + if((result == DIERR_INPUTLOST) || (result == DIERR_NOTACQUIRED)) + { + m_keyboard->Acquire(); + } + else + { + return false; + } + } + for(short i=0;iPoll(); + if (FAILED(hr)) { + // DInput is telling us that the input stream has been + // interrupted. We aren't tracking any state between polls, so + // we don't have any special reset that needs to be done. We + // just re-acquire and try again. + hr = joy->Acquire(); + while (hr == DIERR_INPUTLOST) { + hr = joy->Acquire(); + } + + // If we encounter a fatal error, return failure. + if ((hr == DIERR_INVALIDPARAM) || (hr == DIERR_NOTINITIALIZED)) { + return E_FAIL; + } + + // If another application has control of this device, return successfully. + // We'll just have to wait our turn to use the joystick. + if (hr == DIERR_OTHERAPPHASPRIO) { + return S_OK; + } + } + + // Get the input's device state + if (FAILED(hr = joy->GetDeviceState(sizeof(DIJOYSTATE2), js))) { + return hr; // The device should have been acquired during the Poll() + } + + return S_OK; +} + +bool DirectInput::isButtonDown(short pIndex, unsigned int buttoncode) +{ + if(connectedJoys) + if(joyState[pIndex].rgbButtons[buttoncode-1]!=0) + return true; + return false; +} +DWORD DirectInput::getDirections(short pIndex) +{ + DWORD buttons=0; + if(connectedJoys) + for(short i=0;i<4;i++){ + //if((LOWORD(joyState[pIndex].rgdwPOV[i]) != 0xFFFF)) + if(buttons+=joyState[pIndex].rgdwPOV[i]!=POV_IDLE) + buttons+=joyState[pIndex].rgdwPOV[i]; + } + return buttons; +} \ No newline at end of file diff --git a/WickedEngine/DirectInput.h b/WickedEngine/DirectInput.h new file mode 100644 index 000000000..71c72cd36 --- /dev/null +++ b/WickedEngine/DirectInput.h @@ -0,0 +1,43 @@ +#include + +#define POV_IDLE 4294967292 +#define POV_UP 1 +#define POV_UPRIGHT 4501 +#define POV_RIGHT 9001 +#define POV_RIGHTDOWN 13501 +#define POV_DOWN 18001 +#define POV_DOWNLEFT 22501 +#define POV_LEFT 27001 +#define POV_LEFTUP 31501 + +class DirectInput +{ + friend BOOL CALLBACK enumCallback(const DIDEVICEINSTANCE* instance, VOID* context); + friend BOOL CALLBACK enumAxesCallback(const DIDEVICEOBJECTINSTANCE* instance, VOID* context); +public: + DirectInput(HINSTANCE, HWND); + + void Shutdown(); + bool Frame(); + + //KEYBOARD + bool IsKeyDown(INT); + int GetPressedKeys(); + + //JOYSTICK + bool isButtonDown(short pIndex, unsigned int buttoncode); + DWORD getDirections(short pIndex); + + DIJOYSTATE2 joyState[2]; + static short connectedJoys; + + + static HRESULT InitJoy(HWND hwnd); + +private: + HRESULT Initialize(HINSTANCE, HWND); + + HRESULT poll(IDirectInputDevice8*joy,DIJOYSTATE2 *js); + unsigned char m_keyboardState[256]; +}; + diff --git a/WickedEngine/EmittedParticle.cpp b/WickedEngine/EmittedParticle.cpp new file mode 100644 index 000000000..62c8d34f1 --- /dev/null +++ b/WickedEngine/EmittedParticle.cpp @@ -0,0 +1,567 @@ +#include "EmittedParticle.h" + +extern Camera* cam; + +//ID3D11Buffer *EmittedParticle::vertexBuffer; +ID3D11InputLayout *EmittedParticle::vertexLayout; +ID3D11VertexShader *EmittedParticle::vertexShader; +ID3D11PixelShader *EmittedParticle::pixelShader,*EmittedParticle::simplestPS; +ID3D11GeometryShader*EmittedParticle::geometryShader; +ID3D11Buffer* EmittedParticle::constantBuffer; +ID3D11BlendState* EmittedParticle::blendStateAlpha,*EmittedParticle::blendStateAdd; +ID3D11SamplerState* EmittedParticle::sampleState; +ID3D11RasterizerState* EmittedParticle::rasterizerState,*EmittedParticle::wireFrameRS; +ID3D11DepthStencilState* EmittedParticle::depthStencilState; +set EmittedParticle::systems; + +EmittedParticle::EmittedParticle(std::string newName, std::string newMat, Object* newObject, float newSize, float newRandomFac, float newNormalFac + ,float newCount, float newLife, float newRandLife, float newScaleX, float newScaleY, float newRot){ + name=newName; + object=newObject; + for(Material*mat : object->mesh->materials) + if(!mat->name.compare(newMat)){ + material=mat; + break; + } + + size=newSize; + random_factor=newRandomFac; + normal_factor=newNormalFac; + + count=newCount; + points.resize(0); + life=newLife; + random_life=newRandLife; + emit=0; + + scaleX=newScaleX; + scaleY=newScaleY; + rotation = newRot; + + bounding_box=new AABB(); + lastSquaredDistMulThousand=0; + light=nullptr; + if(material->blendFlag==BLENDMODE_ADDITIVE){ + light=new Light(); + light->color.x=material->diffuseColor.x; + light->color.y=material->diffuseColor.y; + light->color.z=material->diffuseColor.z; + light->type=Light::POINT; + light->name="particleSystemLight"; + //light->shadowMap.resize(1); + //light->shadowMap[0].InitializeCube(Renderer::POINTLIGHTSHADOWRES,0,true); + } + + LoadVertexBuffer(); + + XMFLOAT4X4 transform = XMFLOAT4X4(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0); + transform4 = transform; + transform3 = XMFLOAT3X3( + transform._11,transform._12,transform._13 + ,transform._21,transform._22,transform._23 + ,transform._31,transform._32,transform._33 + ); + +} +long EmittedParticle::getCount(){return points.size();} + + +int EmittedParticle::getRandomPointOnEmitter(){ return rand()%(object->mesh->indices.size()); } + + +void EmittedParticle::addPoint(const XMMATRIX& t4, const XMMATRIX& t3) +{ + vector& emitterVertexList = object->mesh->vertices; + int gen[3]; + gen[0] = getRandomPointOnEmitter(); + switch(gen[0]%3) + { + case 0: + gen[1]=gen[0]+1; + gen[2]=gen[0]+2; + break; + case 1: + gen[0]=gen[0]-1; + gen[1]=gen[0]+1; + gen[2]=gen[0]+2; + break; + case 2: + gen[0]=gen[0]-2; + gen[1]=gen[0]+1; + gen[2]=gen[0]+2; + break; + default: + break; + } + float f=rand()%1000 * 0.001f,g=rand()%1000 * 0.001f; + if (f + g > 1) + { + f = 1 - f; + g = 1 - g; + } + + XMFLOAT3 pos; + XMFLOAT3 vel; + XMVECTOR& vbar=XMVectorBaryCentric( + XMLoadFloat4(&emitterVertexList[object->mesh->indices[gen[0]]].pos) + , XMLoadFloat4(&emitterVertexList[object->mesh->indices[gen[1]]].pos) + , XMLoadFloat4(&emitterVertexList[object->mesh->indices[gen[2]]].pos) + , f + , g + ); + XMVECTOR& nbar=XMVectorBaryCentric( + XMLoadFloat3(&emitterVertexList[object->mesh->indices[gen[0]]].nor) + , XMLoadFloat3(&emitterVertexList[object->mesh->indices[gen[1]]].nor) + , XMLoadFloat3(&emitterVertexList[object->mesh->indices[gen[2]]].nor) + , f + , g + ); + XMStoreFloat3( &pos, XMVector3Transform( vbar, t4 ) ); + XMStoreFloat3( &vel, XMVector3Normalize( XMVector3Transform( nbar, t3 ) )); + + float vrand = (normal_factor*getNewVelocityModifier())/60.0f; + + vel.x*=vrand; + vel.y*=vrand; + vel.z*=vrand; + + //pos.x+=getNewPositionModifier(); + //pos.y+=getNewPositionModifier(); + //pos.z+=getNewPositionModifier(); + + + points.push_back( Point( pos, XMFLOAT4(size,1,rand()%2,rand()%2), vel/*, XMFLOAT3(1,1,1)*/, getNewLifeSpan() + ,rotation*getNewRotationModifier(),scaleX,scaleY ) ); +} +void EmittedParticle::Update(float gamespeed) +{ + systems.insert(this); + + + XMFLOAT3 minP=XMFLOAT3(D3D11_FLOAT32_MAX,D3D11_FLOAT32_MAX,D3D11_FLOAT32_MAX) + ,maxP=XMFLOAT3(-D3D11_FLOAT32_MAX,-D3D11_FLOAT32_MAX,-D3D11_FLOAT32_MAX); + + for(int i=0;ipoint.maxLife*0.9f) + point.sizOpaMir.y+=0.05f*gamespeed; + if(point.sizOpaMir.y<=0) point.sizOpaMir.y=0; + if(point.sizOpaMir.y>=1) point.sizOpaMir.y=1;*/ + + point.life-=/*1.0f/60.0f**/gamespeed; + point.life=WickedMath::Clamp(point.life,0,point.maxLife); + + float lifeLerp = point.life/point.maxLife; + point.sizOpaMir.x=WickedMath::Lerp(point.sizBeginEnd[1],point.sizBeginEnd[0],lifeLerp); + point.sizOpaMir.y=WickedMath::Lerp(1,0,lifeLerp); + + + minP=WickedMath::Min(XMFLOAT3(point.pos.x-point.sizOpaMir.x,point.pos.y-point.sizOpaMir.x,point.pos.z-point.sizOpaMir.x),minP); + maxP=WickedMath::Max(XMFLOAT3(point.pos.x+point.sizOpaMir.x,point.pos.y+point.sizOpaMir.x,point.pos.z+point.sizOpaMir.x),maxP); + } + bounding_box->create(minP,maxP); + + while(!points.empty() && points.front().life<=0) + points.pop_front(); + + + XMFLOAT4X4& transform = object->world; + transform4 = transform; + transform3 = XMFLOAT3X3( + transform._11,transform._12,transform._13 + ,transform._21,transform._22,transform._23 + ,transform._31,transform._32,transform._33 + ); + XMMATRIX t4=XMLoadFloat4x4(&transform4), t3=XMLoadFloat3x3(&transform3); + + emit += (float)count/60.0f*gamespeed; + + bool clearSpace=false; + if(points.size()+emit>=MAX_PARTICLES) + clearSpace=true; + + for(int i=0;i<(int)emit;++i) + { + if(clearSpace) + points.pop_front(); + + addPoint(t4,t3); + } + if((int)emit>0) + emit=0; + + if(light!=nullptr){ + light->translation_rest=bounding_box->getCenter(); + light->enerDis=XMFLOAT4(5,bounding_box->getRadius()*3,0,0); + } + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //Point* vertexPtr; + //Renderer::immediateContext->Map(vertexBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //vertexPtr = (Point*)mappedResource.pData; + //memcpy(vertexPtr,renderPoints.data(),sizeof(Point)* renderPoints.size()); + //Renderer::immediateContext->Unmap(vertexBuffer,0); +} +void EmittedParticle::Burst(float num) +{ + XMMATRIX t4=XMLoadFloat4x4(&transform4), t3=XMLoadFloat3x3(&transform3); + + static float burst = 0; + burst+=num; + for(int i=0;i<(int)burst;++i) + addPoint(t4,t3); + burst-=(int)burst; +} + + +void EmittedParticle::Draw(const XMVECTOR eye, const XMMATRIX& newView, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, int FLAG) +{ + if(!points.empty()){ + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + if(frustum.CheckBox(bounding_box->corners)){ + + vector renderPoints=vector(points.begin(),points.end()); + Renderer::UpdateBuffer(vertexBuffer,renderPoints.data(),context,sizeof(Point)* renderPoints.size()); + + bool additive = (material->blendFlag==BLENDMODE_ADDITIVE || material->premultipliedTexture); + + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_POINTLIST ); + //context->IASetInputLayout( vertexLayout ); + //context->VSSetShader( vertexShader, NULL, 0 ); + //context->GSSetShader( geometryShader, NULL, 0 ); + //context->PSSetShader( wireRender?simplestPS:pixelShader, NULL, 0 ); + Renderer::BindPrimitiveTopology(Renderer::PRIMITIVETOPOLOGY::POINTLIST,context); + Renderer::BindVertexLayout(vertexLayout,context); + Renderer::BindPS(wireRender?simplestPS:pixelShader,context); + Renderer::BindVS(vertexShader,context); + Renderer::BindGS(geometryShader,context); + + Renderer::BindTexturePS(depth,1,context); + + ConstantBuffer cb; + cb.mView = XMMatrixTranspose(newView); + cb.mProjection = XMMatrixTranspose( cam->Projection ); + cb.mCamPos = eye; + cb.mAdd.x = additive; + cb.mAdd.y = (FLAG==DRAW_DARK?true:false); + + + //context->UpdateSubresource( constantBuffer, 0, NULL, &cb, 0, 0 ); + + Renderer::UpdateBuffer(constantBuffer,&cb,context); + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //ConstantBuffer* dataPtr; + //context->Map(constantBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (ConstantBuffer*)mappedResource.pData; + //memcpy(dataPtr,&cb,sizeof(ConstantBuffer)); + //context->Unmap(constantBuffer,0); + + //context->RSSetState(wireRender?wireFrameRS:rasterizerState); + //context->OMSetDepthStencilState(depthStencilState, 1); + Renderer::BindRasterizerState(wireRender?wireFrameRS:rasterizerState,context); + Renderer::BindDepthStencilState(depthStencilState,1,context); + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState((additive?blendStateAdd:blendStateAlpha), blendFactor, sampleMask); + Renderer::BindBlendState((additive?blendStateAdd:blendStateAlpha),context); + + //context->GSSetConstantBuffers( 0, 1, &constantBuffer ); + //context->PSSetSamplers(0, 1, &sampleState); + Renderer::BindConstantBufferGS(constantBuffer,0,context); + Renderer::BindSamplerPS(sampleState,0,context); + + + //context->UpdateSubresource( vertexBuffer, 0, 0, points.data(), 0, 0 ); + + + //UINT stride = sizeof( Point ); + //UINT offset = 0; + //context->IASetVertexBuffers( 0, 1, &vertexBuffer, &stride, &offset ); + Renderer::BindVertexBuffer(vertexBuffer,0,sizeof(Point),context); + + if(!wireRender && material->texture) Renderer::BindTexturePS(material->texture,0,context); + //context->Draw( renderPoints.size(),0); + Renderer::Draw(renderPoints.size(),context); + + + //context->GSSetShader(0,0,0); + Renderer::BindGS(nullptr,context); + //context->GSSetConstantBuffers(0,0,0); + Renderer::BindConstantBufferGS(nullptr,0,context); + } + } +} +void EmittedParticle::DrawPremul(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, int FLAG){ + if(material->premultipliedTexture) + Draw(eye,view,context,depth,FLAG); +} +void EmittedParticle::DrawNonPremul(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, int FLAG){ + if(!material->premultipliedTexture) + Draw(eye,view,context,depth,FLAG); +} + + +void EmittedParticle::CleanUp() +{ + + points.clear(); + + if(vertexBuffer) vertexBuffer->Release(); vertexBuffer=NULL; + + systems.erase(this); + + delete bounding_box; + bounding_box=nullptr; + + //delete(this); +} + +void* EmittedParticle::operator new(size_t size) +{ + void* result = _aligned_malloc(size,16); + return result; +} +void EmittedParticle::operator delete(void* p) +{ + if(p) _aligned_free(p); +} + + + + + +void EmittedParticle::LoadShaders() +{ + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/pointspriteVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load pointspriteVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vertexShader ); + + + + + // Define the input layout + D3D11_INPUT_ELEMENT_DESC layout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 1, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + //{ "TEXCOORD", 2, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 2, DXGI_FORMAT_R32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + }; + UINT numElements = ARRAYSIZE( layout ); + + // Create the input layout + Renderer::graphicsDevice->CreateInputLayout( layout, numElements, pVSBlob->GetBufferPointer(), + pVSBlob->GetBufferSize(), &vertexLayout ); + + if(pVSBlob){ pVSBlob->Release();pVSBlob=NULL; } + + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/pointspritePS.cso", &pPSBlob))){MessageBox(0,L"Failed To load pointspritePS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &pixelShader ); + + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/pointspritePS_simplest.cso", &pPSBlob))){MessageBox(0,L"Failed To load pointspritePS_simplest.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &simplestPS ); + + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + + ID3DBlob* pGSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/pointspriteGS.cso", &pGSBlob))){MessageBox(0,L"Failed To load pointspriteGS.cso",0,0);} + Renderer::graphicsDevice->CreateGeometryShader( pGSBlob->GetBufferPointer(), pGSBlob->GetBufferSize(), NULL, &geometryShader ); + + if(pGSBlob){ pGSBlob->Release();pGSBlob=NULL; } + + +} +void EmittedParticle::SetUpCB() +{ + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ConstantBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &constantBuffer ); +} +void EmittedParticle::SetUpStates() +{ + D3D11_SAMPLER_DESC samplerDesc; + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_LINEAR; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + Renderer::graphicsDevice->CreateSamplerState(&samplerDesc, &sampleState); + + + + + D3D11_RASTERIZER_DESC rs; + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_BACK; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=false; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + Renderer::graphicsDevice->CreateRasterizerState(&rs,&rasterizerState); + + + rs.FillMode=D3D11_FILL_WIREFRAME; + rs.CullMode=D3D11_CULL_NONE; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=false; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + Renderer::graphicsDevice->CreateRasterizerState(&rs,&wireFrameRS); + + + + + + D3D11_DEPTH_STENCIL_DESC dsd; + dsd.DepthEnable = false; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ZERO; + dsd.DepthFunc = D3D11_COMPARISON_LESS; + + dsd.StencilEnable = false; + dsd.StencilReadMask = 0xFF; + dsd.StencilWriteMask = 0xFF; + + // Stencil operations if pixel is front-facing. + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_INCR; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + + // Stencil operations if pixel is back-facing. + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_DECR; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + + // Create the depth stencil state. + Renderer::graphicsDevice->CreateDepthStencilState(&dsd, &depthStencilState); + + + + D3D11_BLEND_DESC bd; + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_ALPHA; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + bd.IndependentBlendEnable=true; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendStateAlpha); + + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + bd.IndependentBlendEnable=true; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendStateAdd); +} +void EmittedParticle::LoadVertexBuffer() +{ + vertexBuffer=NULL; + + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof( Point ) * MAX_PARTICLES; + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &vertexBuffer ); +} +void EmittedParticle::SetUpStatic() +{ + vertexShader = NULL; + pixelShader = NULL; + geometryShader = NULL; + vertexLayout = NULL; + constantBuffer = NULL; + sampleState = NULL; + rasterizerState = NULL; + blendStateAdd = NULL; + blendStateAlpha = NULL; + depthStencilState = NULL; + + LoadShaders(); + SetUpCB(); + SetUpStates(); + + systems.clear(); +} +void EmittedParticle::CleanUpStatic() +{ + //if(vertexBuffer) vertexBuffer->Release(); vertexBuffer=NULL; + if(vertexShader) vertexShader->Release(); vertexShader = NULL; + if(pixelShader) pixelShader->Release(); pixelShader = NULL; + if(simplestPS) simplestPS->Release(); simplestPS = NULL; + if(geometryShader) geometryShader->Release(); geometryShader = NULL; + if(vertexLayout) vertexLayout->Release(); vertexLayout = NULL; + + if(constantBuffer) constantBuffer->Release(); constantBuffer = NULL; + + if(sampleState) sampleState->Release(); sampleState = NULL; + if(rasterizerState) rasterizerState->Release(); rasterizerState = NULL; + if(wireFrameRS) wireFrameRS->Release(); wireFrameRS = NULL; + if(blendStateAdd) blendStateAdd->Release(); blendStateAdd = NULL; + if(blendStateAlpha) blendStateAlpha->Release(); blendStateAlpha = NULL; + if(depthStencilState) depthStencilState->Release(); depthStencilState = NULL; +} +long EmittedParticle::getNumParticles() +{ + long retval=0; + for(EmittedParticle* e:systems) + if(e) + retval+=e->getCount(); + return retval; +} \ No newline at end of file diff --git a/WickedEngine/EmittedParticle.h b/WickedEngine/EmittedParticle.h new file mode 100644 index 000000000..cff552a14 --- /dev/null +++ b/WickedEngine/EmittedParticle.h @@ -0,0 +1,115 @@ +#pragma once +#include "Particle.h" + +struct Material; +struct AABB; +struct Light; + +class EmittedParticle : + public Particle +{ +private: + struct Point + { + XMFLOAT3 pos; + XMFLOAT4 sizOpaMir; + //XMFLOAT3 col; + float rot,rotVel; + XMFLOAT3 vel; + float life; + float maxLife; + float sizBeginEnd[2]; + + Point(){} + Point(const XMFLOAT3& newPos, const XMFLOAT4& newSizOpaMir, const XMFLOAT3& newVel/*, const XMFLOAT3& newCol*/, float newLife, float newRotVel + ,float scaleX, float scaleY) + { + pos=newPos; + sizOpaMir=newSizOpaMir; + vel=newVel; + //col=newCol; + life=maxLife=newLife; + rot=newRotVel; + rotVel=newRotVel; + sizBeginEnd[0] = sizOpaMir.x; + sizBeginEnd[1] = sizOpaMir.x*scaleX; + } + }; + deque points; + + struct ConstantBuffer + { + XMMATRIX mView; + XMMATRIX mProjection; + XMVECTOR mCamPos; + XMFLOAT4 mAdd; + }; + ID3D11Buffer *vertexBuffer; + static ID3D11InputLayout *vertexLayout; + static ID3D11VertexShader *vertexShader; + static ID3D11PixelShader *pixelShader,*simplestPS; + static ID3D11GeometryShader*geometryShader; + static ID3D11Buffer* constantBuffer; + static ID3D11BlendState* blendStateAlpha,*blendStateAdd; + static ID3D11SamplerState* sampleState; + static ID3D11RasterizerState* rasterizerState,*wireFrameRS; + static ID3D11DepthStencilState* depthStencilState; + + static void LoadShaders(); + static void SetUpCB(); + static void SetUpStates(); + void LoadVertexBuffer(); + + static set systems; + + + //std::vector emitterVertexList; + Object* object; + + float getNewVelocityModifier(){ return 1+((rand()%10001+1)*0.0001)*random_factor;} + float getNewPositionModifier(){ return (rand()%((int)(random_factor*1000)+1))*0.001f - random_factor*0.5f; } + float getNewRotationModifier(){ return (rand()%((int)(random_factor*1000)+1))*0.001f - random_factor*0.5f; } + float getNewLifeSpan(){ return life + (rand()%((int)(random_life*1000)+1))*0.001f - random_life*0.5f; } + int getRandomPointOnEmitter(); + + XMFLOAT4X4 transform4; + XMFLOAT3X3 transform3; + void addPoint(const XMMATRIX& t4, const XMMATRIX& t3); + + float emit; + +public: + EmittedParticle(){}; + EmittedParticle(std::string newName, std::string newMat, Object* newObject, float newSize, float newRandomFac, float newNormalFac + ,float newCount, float newLife, float newRandLife, float newScaleX, float newScaleY, float newRot); + void* operator new(size_t); + void operator delete(void*); + static void SetUpStatic(); + static void CleanUpStatic(); + + long getCount(); + static long getNumParticles(); + + void Update(float gamespeed); + void Burst(float num); + +#define DRAW_DEFAULT 0 +#define DRAW_DARK 1 + void Draw(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, int FLAG = DRAW_DEFAULT); + void DrawPremul(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, int FLAG = DRAW_DEFAULT); + void DrawNonPremul(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, int FLAG = DRAW_DEFAULT); + void CleanUp(); + + std::string name; + //std::string material; + Material* material; + Light* light; + + AABB* bounding_box; + long lastSquaredDistMulThousand; + + float size,random_factor,normal_factor; + float count,life,random_life; + float scaleX,scaleY,rotation; +}; + diff --git a/WickedEngine/Entity.h b/WickedEngine/Entity.h new file mode 100644 index 000000000..40f81be82 --- /dev/null +++ b/WickedEngine/Entity.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Renderer.h" + +class Entity : public Renderer +{ +protected: + string identifier; + vector e_armatures; + vector e_objects; + + void CleanUp(){ + Renderer::CleanUp(); + identifier.clear(); + e_armatures.clear(); + e_objects.clear(); + } +}; diff --git a/WickedEngine/Font.cpp b/WickedEngine/Font.cpp new file mode 100644 index 000000000..96180c1dc --- /dev/null +++ b/WickedEngine/Font.cpp @@ -0,0 +1,458 @@ +#include "Font.h" + +extern Camera* cam; + + +ID3D11Buffer *Font::vertexBuffer,*Font::indexBuffer; +ID3D11InputLayout *Font::vertexLayout; +ID3D11VertexShader *Font::vertexShader; +ID3D11PixelShader *Font::pixelShader; +ID3D11BlendState* Font::blendState; +ID3D11Buffer* Font::constantBuffer; +ID3D11SamplerState* Font::sampleState; +ID3D11RasterizerState* Font::rasterizerState; +ID3D11DepthStencilState* Font::depthStencilState; +int Font::RENDERWIDTH,Font::RENDERHEIGHT; +mutex Font::MUTEX; +UINT Font::textlen; +SHORT Font::line,Font::pos; +BOOL Font::toDraw; +DWORD Font::counter; +vector Font::vertexList; +vector Font::fontStyles; + +void Font::Initialize() +{ + line=pos=counter=0; + toDraw=TRUE; + textlen=0; + line=pos=0; + RENDERWIDTH=1280; + RENDERHEIGHT=720; + + + indexBuffer=NULL; + vertexBuffer=NULL; + vertexLayout=NULL; + vertexShader=NULL; + pixelShader=NULL; + blendState=NULL; + constantBuffer=NULL; + sampleState=NULL; + rasterizerState=NULL; + depthStencilState=NULL; +} + +void Font::SetUpStates() +{ + D3D11_SAMPLER_DESC samplerDesc; + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_LINEAR; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + Renderer::graphicsDevice->CreateSamplerState(&samplerDesc, &sampleState); + + + + + D3D11_RASTERIZER_DESC rs; + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_BACK; + rs.FrontCounterClockwise=TRUE; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=FALSE; + rs.ScissorEnable=FALSE; + rs.MultisampleEnable=FALSE; + rs.AntialiasedLineEnable=FALSE; + Renderer::graphicsDevice->CreateRasterizerState(&rs,&rasterizerState); + + + + + + D3D11_DEPTH_STENCIL_DESC dsd; + dsd.DepthEnable = false; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ZERO; + dsd.DepthFunc = D3D11_COMPARISON_LESS; + + dsd.StencilEnable = false; + dsd.StencilReadMask = 0xFF; + dsd.StencilWriteMask = 0xFF; + + // Stencil operations if pixel is front-facing. + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_INCR; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + + // Stencil operations if pixel is back-facing. + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_DECR; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + + // Create the depth stencil state. + Renderer::graphicsDevice->CreateDepthStencilState(&dsd, &depthStencilState); + + + + D3D11_BLEND_DESC bd; + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=TRUE; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_ALPHA; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendState); +} +void Font::SetUpCB() +{ + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ConstantBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &constantBuffer ); +} +void Font::LoadShaders() +{ + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/fontVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load fontVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vertexShader ); + + + + + // Define the input layout + D3D11_INPUT_ELEMENT_DESC layout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 0, DXGI_FORMAT_R32G32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + }; + UINT numElements = ARRAYSIZE( layout ); + + // Create the input layout + Renderer::graphicsDevice->CreateInputLayout( layout, numElements, pVSBlob->GetBufferPointer(), + pVSBlob->GetBufferSize(), &vertexLayout ); + pVSBlob->Release(); + + + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/fontPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load fontPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &pixelShader ); + pPSBlob->Release(); +} +void Font::SetUpStaticComponents() +{ + SetUpStates(); + SetUpCB(); + LoadShaders(); + LoadVertexBuffer(); + LoadIndices(); +} +void Font::CleanUpStatic() +{ + for(int i=0;iRelease(); vertexBuffer=NULL; + if(indexBuffer) indexBuffer->Release(); indexBuffer=NULL; + if(vertexShader) vertexShader->Release(); vertexShader=NULL; + if(pixelShader) pixelShader->Release(); pixelShader=NULL; + if(vertexLayout) vertexLayout->Release(); vertexLayout=NULL; + if(constantBuffer) constantBuffer->Release(); constantBuffer=NULL; + if(sampleState) sampleState->Release(); sampleState=NULL; + if(rasterizerState) rasterizerState->Release(); rasterizerState=NULL; + if(blendState) blendState->Release(); blendState=NULL; + if(depthStencilState) depthStencilState->Release(); depthStencilState=NULL; +} + + +void Font::ModifyGeo(const wchar_t* text, XMFLOAT2 sizSpa,const int& style, ID3D11DeviceContext* context) +{ + textlen=wcslen(text); + line=0; pos=0; + vertexList.resize(textlen*4); + for(int i=0;iUpdateSubresource( vertexBuffer, 0, NULL, vertexList.data(), 0, 0 ); + + Renderer::UpdateBuffer(vertexBuffer,vertexList.data(),context==nullptr?Renderer::immediateContext:context,sizeof(Vertex) * textlen * 4); + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //Vertex* dataPtr; + //Renderer::immediateContext->Map(vertexBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (Vertex*)mappedResource.pData; + //memcpy(dataPtr,vertexList.data(),sizeof(Vertex) * textlen * 4); + //Renderer::immediateContext->Unmap(vertexBuffer,0); +} + +void Font::LoadVertexBuffer() +{ + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof( Vertex ) * MAX_TEXT * 4; + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &vertexBuffer ); +} +void Font::LoadIndices() +{ + std::vectorindices; + indices.resize(MAX_TEXT*6); + for(unsigned long i=0;iCreateBuffer( &bd, &InitData, &indexBuffer ); +} + + +void Font::DrawBlink(wchar_t* text, XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context) +{ + if(toDraw){ + Draw(text,newPosSizSpa,Halign,Valign,context); + } +} +void Font::DrawBlink(const std::string& text, XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context) +{ + if(toDraw){ + Draw(text,newPosSizSpa,Halign,Valign,context); + } +} +void Font::Draw(const std::string& text, XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context) +{ + std::wstring ws(text.begin(), text.end()); + + Draw(ws.c_str(),newPosSizSpa,Halign,Valign,context); +} +void Font::Draw(const wchar_t* text, XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context) +{ + Draw(text,"",newPosSizSpa,Halign,Valign,context); +} + +void Font::DrawBlink(const string& text,const char* fontStyle,XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context){ + if(toDraw){ + Draw(text,fontStyle,newPosSizSpa,Halign,Valign,context); + } +} +void Font::DrawBlink(const wchar_t* text,const char* fontStyle,XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context){ + if(toDraw){ + Draw(text,fontStyle,newPosSizSpa,Halign,Valign,context); + } +} +void Font::Draw(const string& text,const char* fontStyle,XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context){ + wstring ws(text.begin(),text.end()); + Draw(ws.c_str(),fontStyle,newPosSizSpa,Halign,Valign,context); +} + + +void Font::Draw(const wchar_t* text,const char* fontStyle,XMFLOAT4 newPosSizSpa, const char* Halign, const char* Valign, Renderer::DeviceContext context){ + int fontStyleI = getFontStyleByName(fontStyle); + + + if(!strcmp(Halign,"center") || !strcmp(Valign,"mid")) + newPosSizSpa.x-=textWidth(text,newPosSizSpa.w+newPosSizSpa.z,fontStyleI)/2; + else if(!strcmp(Halign,"right")) + newPosSizSpa.x-=textWidth(text,newPosSizSpa.w+newPosSizSpa.z,fontStyleI); + if(!strcmp(Valign,"center") || !strcmp(Valign,"mid")) + newPosSizSpa.y+=textHeight(text,newPosSizSpa.z,fontStyleI)*0.5f; + else if(!strcmp(Valign,"bottom")) + newPosSizSpa.y+=textHeight(text,newPosSizSpa.z,fontStyleI); + + + ModifyGeo(text,XMFLOAT2(newPosSizSpa.z,newPosSizSpa.w),fontStyleI,context); + + if(textlen){ + + if(context==nullptr) + context=Renderer::immediateContext; + + Renderer::BindPrimitiveTopology(Renderer::PRIMITIVETOPOLOGY::TRIANGLELIST,context); + Renderer::BindVertexLayout(vertexLayout,context); + Renderer::BindVS(vertexShader,context); + Renderer::BindPS(pixelShader,context); + + + ConstantBuffer* cb = new ConstantBuffer(); + cb->mProjection = XMMatrixTranspose( cam->Oprojection ); + cb->mTrans = XMMatrixTranspose( XMMatrixTranslation(newPosSizSpa.x,newPosSizSpa.y,0) ); + cb->mDimensions = XMFLOAT4(RENDERWIDTH,RENDERHEIGHT,0,0); + + Renderer::UpdateBuffer(constantBuffer,cb,context); + delete cb; + + Renderer::BindConstantBufferVS(constantBuffer,0,context); + + Renderer::BindRasterizerState(rasterizerState,context); + Renderer::BindDepthStencilState(depthStencilState,1,context); + + Renderer::BindBlendState(blendState,context); + Renderer::BindVertexBuffer(vertexBuffer,0,sizeof(Vertex),context); + Renderer::BindIndexBuffer(indexBuffer,context); + + Renderer::BindTexturePS(fontStyles[fontStyleI].texture,0,context); + Renderer::BindSamplerPS(sampleState,0,context); + Renderer::DrawIndexed(textlen*6,context); + } +} + +void Font::Blink(DWORD perframe,DWORD invisibleTime) +{ + counter++; + if(toDraw && counter>perframe){ + counter=0; + toDraw=FALSE; + } + else if(!toDraw && counter>invisibleTime){ + counter=0; + toDraw=TRUE; + } +} + + +int Font::textWidth(const wchar_t* text,FLOAT spacing,const int& style) +{ + int i=0; + int max=0,lineW=0; + int len=wcslen(text); + while(i>texWidth>>texHeight>>recSize>>charSize; + int i=0; + while(!file.eof()){ + i++; + int code=0; + file>>code; + lookup[code].code=code; + file>>lookup[code].offX>>lookup[code].offY>>lookup[code].width; + } + file.close(); + } + else {MessageBox(NULL,L"Could not load Font Data!",L"Error!",0); exit(0);} +} +void Font::FontStyle::CleanUp(){ + if(texture) texture->Release(); texture=NULL; +} +void Font::addFontStyle( string toAdd){ + MUTEX.lock(); + fontStyles.push_back(FontStyle(toAdd)); + MUTEX.unlock(); +} +int Font::getFontStyleByName( string get){ + for(int i=0;i vertexList; + struct ConstantBuffer + { + XMMATRIX mProjection; + XMMATRIX mTrans; + XMFLOAT4 mDimensions; + + void* operator new(size_t size) + { + + void* result = _aligned_malloc(size,16); + return result; + } + void operator delete(void* p) + { + if(p) _aligned_free(p); + } + }; + static ID3D11Buffer* vertexBuffer,*indexBuffer; + + static ID3D11InputLayout *vertexLayout; + static ID3D11VertexShader *vertexShader; + static ID3D11PixelShader *pixelShader; + static ID3D11BlendState* blendState; + static ID3D11Buffer* constantBuffer; + static ID3D11SamplerState* sampleState; + static ID3D11RasterizerState* rasterizerState; + static ID3D11DepthStencilState* depthStencilState; + static int RENDERWIDTH,RENDERHEIGHT; + + static void SetUpStates(); + static void SetUpCB(); + static void LoadShaders(); + static void LoadVertexBuffer(); + static void LoadIndices(); + + + + static UINT textlen; + static SHORT line,pos; + static BOOL toDraw; + static DWORD counter; + + + struct FontStyle{ + string name; + ID3D11ShaderResourceView *texture; + + struct LookUp{ + int code; + int offX,offY,width; + }; + LookUp lookup[127]; + int texWidth,texHeight,charSize,recSize; + + FontStyle(){} + FontStyle(const string& newName); + void CleanUp(); + }; + static vector fontStyles; + + + static void ModifyGeo(const wchar_t* text,XMFLOAT2 sizSpacing,const int& style, ID3D11DeviceContext* context = nullptr); +public: + static void Initialize(); + static void SetUpStaticComponents(); + static void CleanUpStatic(); + static void SetScreenResolution(int width, int height){RENDERHEIGHT=height;RENDERWIDTH=width;}; + + static void DrawBlink(wchar_t* text,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + static void Draw(const wchar_t* text,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + static void DrawBlink(const std::string& text,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + static void Draw(const std::string& text,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + + static void DrawBlink(const string& text,const char* fontStyle,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + static void DrawBlink(const wchar_t* text,const char* fontStyle,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + static void Draw(const string& text,const char* fontStyle,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + static void Draw(const wchar_t* text,const char* fontStyle,XMFLOAT4 posSizSpacing=XMFLOAT4(0,0,0,0), const char* Halign="left", const char* Valign="top", ID3D11DeviceContext* context = nullptr); + + static void Blink(DWORD perframe,DWORD invisibleTime); + + static int textWidth(const wchar_t*,FLOAT siz,const int& style); + static int textHeight(const wchar_t*,FLOAT siz,const int& style); + + static void addFontStyle( string toAdd); + static int getFontStyleByName( string get); + + void CleanUp(); +}; diff --git a/WickedEngine/FrameRate.cpp b/WickedEngine/FrameRate.cpp new file mode 100644 index 000000000..2d13eed67 --- /dev/null +++ b/WickedEngine/FrameRate.cpp @@ -0,0 +1,31 @@ +#include "FrameRate.h" + +Timer FrameRate::timer; +double FrameRate::dt=0.0; + +void FrameRate::Initialize() +{ + timer = Timer(); +} + +void FrameRate::Frame(){ + dt=timer.elapsed(); + timer.record(); +} + +double FrameRate::FPS() { + static const int NUM_FPS_SAMPLES = 60; + static double fpsSamples[NUM_FPS_SAMPLES]; + static int currentSample = 0; + + + fpsSamples[currentSample % NUM_FPS_SAMPLES] = 1000.0 / dt; + double fps = 0; + for (int i = 0; i < NUM_FPS_SAMPLES; i++){ + fps += fpsSamples[i]; + } + fps /= NUM_FPS_SAMPLES; + currentSample++; + + return fps; +} diff --git a/WickedEngine/FrameRate.h b/WickedEngine/FrameRate.h new file mode 100644 index 000000000..86e8abef7 --- /dev/null +++ b/WickedEngine/FrameRate.h @@ -0,0 +1,19 @@ +#pragma once +#include "WickedEngine.h" +#include "Timer.h" + +class Timer; + +static class FrameRate +{ +protected: + static Timer timer; + static double dt; +public: + static void Initialize(); + + static void Frame(); + + static double FPS(); +}; + diff --git a/WickedEngine/Frustum.cpp b/WickedEngine/Frustum.cpp new file mode 100644 index 000000000..908912701 --- /dev/null +++ b/WickedEngine/Frustum.cpp @@ -0,0 +1,126 @@ +#include "Frustum.h" + + +Frustum::Frustum() +{ +} +void Frustum::CleanUp() +{ +} + +void Frustum::ConstructFrustum(float screenDepth, XMFLOAT4X4 projectionMatrix, XMFLOAT4X4 viewMatrix) +{ + view=viewMatrix; + + float zMinimum, r; + XMFLOAT4X4 matrix; + + + // Calculate the minimum Z distance in the frustum. + zMinimum = -projectionMatrix._43 / projectionMatrix._33; + r = screenDepth / (screenDepth - zMinimum); + projectionMatrix._33 = r; + projectionMatrix._43 = -r * zMinimum; + + // Create the frustum matrix from the view matrix and updated projection matrix. + XMStoreFloat4x4( &matrix,XMMatrixMultiply(XMLoadFloat4x4(&viewMatrix), XMLoadFloat4x4(&projectionMatrix)) ); + + // Calculate near plane of frustum. + m_planes[0].x = matrix._14 + matrix._13; + m_planes[0].y = matrix._24 + matrix._23; + m_planes[0].z = matrix._34 + matrix._33; + m_planes[0].w = matrix._44 + matrix._43; + XMStoreFloat4( &m_planesNorm[0],XMPlaneNormalize(XMLoadFloat4(&m_planes[0])) ); + + // Calculate far plane of frustum. + m_planes[1].x = matrix._14 - matrix._13; + m_planes[1].y = matrix._24 - matrix._23; + m_planes[1].z = matrix._34 - matrix._33; + m_planes[1].w = matrix._44 - matrix._43; + XMStoreFloat4( &m_planesNorm[1],XMPlaneNormalize(XMLoadFloat4(&m_planes[1])) ); + + // Calculate left plane of frustum. + m_planes[2].x = matrix._14 + matrix._11; + m_planes[2].y = matrix._24 + matrix._21; + m_planes[2].z = matrix._34 + matrix._31; + m_planes[2].w = matrix._44 + matrix._41; + XMStoreFloat4( &m_planesNorm[2],XMPlaneNormalize(XMLoadFloat4(&m_planes[2])) ); + + // Calculate right plane of frustum. + m_planes[3].x = matrix._14 - matrix._11; + m_planes[3].y = matrix._24 - matrix._21; + m_planes[3].z = matrix._34 - matrix._31; + m_planes[3].w = matrix._44 - matrix._41; + XMStoreFloat4( &m_planesNorm[3],XMPlaneNormalize(XMLoadFloat4(&m_planes[3])) ); + + // Calculate top plane of frustum. + m_planes[4].x = matrix._14 - matrix._12; + m_planes[4].y = matrix._24 - matrix._22; + m_planes[4].z = matrix._34 - matrix._32; + m_planes[4].w = matrix._44 - matrix._42; + XMStoreFloat4( &m_planesNorm[4],XMPlaneNormalize(XMLoadFloat4(&m_planes[4])) ); + + // Calculate bottom plane of frustum. + m_planes[5].x = matrix._14 + matrix._12; + m_planes[5].y = matrix._24 + matrix._22; + m_planes[5].z = matrix._34 + matrix._32; + m_planes[5].w = matrix._44 + matrix._42; + XMStoreFloat4( &m_planesNorm[5],XMPlaneNormalize(XMLoadFloat4(&m_planes[5])) ); + +} + +bool Frustum::CheckPoint(const XMFLOAT3& point) +{ + XMVECTOR p = XMLoadFloat3(&point); + for(short i=0; i<6; i++) + { + if(XMVectorGetX( XMPlaneDotCoord(XMLoadFloat4(&m_planesNorm[i]), p) ) < 0.0f) + { + return false; + } + } + + return true; +} +bool Frustum::CheckSphere(const XMFLOAT3& center, const float& radius) +{ + int i; + XMVECTOR c = XMLoadFloat3(¢er); + for(i=0; i<6; i++) + { + if(XMVectorGetX( XMPlaneDotCoord(XMLoadFloat4(&m_planesNorm[i]), c) ) < -radius) + { + return false; + } + } + + return true; +} +int Frustum::CheckBox(XMFLOAT3 corners[8]){ + int iTotalIn = 0; + for(int p = 0; p < 6; ++p) { + + int iInCount = 8; + int iPtIn = 1; + + for(int i = 0; i < 8; ++i) { + if(XMVectorGetX( XMPlaneDotCoord(XMLoadFloat4(&m_planesNorm[p]), XMLoadFloat3(&corners[i])) ) < 0.0f) + { + iPtIn=0; + --iInCount; + } + } + if(iInCount == 0) + return(false); + iTotalIn += iPtIn; + } + if(iTotalIn == 6) + return(BOX_FRUSTUM_INSIDE); + return(BOX_FRUSTUM_INTERSECTS); +} + +XMFLOAT4 Frustum::getFarPlane(){return m_planes[1];} +XMFLOAT4 Frustum::getNearPlane(){return m_planes[0];} +XMFLOAT3 Frustum::getCamPos(){ + return XMFLOAT3(-view._41,-view._42,-view._43); +} diff --git a/WickedEngine/Frustum.h b/WickedEngine/Frustum.h new file mode 100644 index 000000000..69934a132 --- /dev/null +++ b/WickedEngine/Frustum.h @@ -0,0 +1,27 @@ +#pragma once +#include "WickedEngine.h" + +class Frustum +{ +private: + XMFLOAT4 m_planesNorm[6]; + XMFLOAT4 m_planes[6]; + XMFLOAT4X4 view; +public: + Frustum(); + void CleanUp(); + + void ConstructFrustum(float screenDepth, XMFLOAT4X4 projectionMatrix, XMFLOAT4X4 viewMatrix); + + bool CheckPoint(const XMFLOAT3&); + bool CheckSphere(const XMFLOAT3&, const float&); + +#define BOX_FRUSTUM_INTERSECTS 1 +#define BOX_FRUSTUM_INSIDE 2 + int CheckBox(XMFLOAT3[8]); + + XMFLOAT4 getFarPlane(); + XMFLOAT4 getNearPlane(); + XMFLOAT3 getCamPos(); +}; + diff --git a/WickedEngine/HairParticle.cpp b/WickedEngine/HairParticle.cpp new file mode 100644 index 000000000..7616b2f97 --- /dev/null +++ b/WickedEngine/HairParticle.cpp @@ -0,0 +1,509 @@ +#include "HairParticle.h" + +extern Camera* cam; + + + +ID3D11InputLayout* HairParticle::il; +ID3D11VertexShader* HairParticle::vs; +ID3D11PixelShader* HairParticle::ps,*HairParticle::qps; +ID3D11GeometryShader* HairParticle::gs[],*HairParticle::qgs[]; +ID3D11Buffer* HairParticle::cbgs; +ID3D11DepthStencilState* HairParticle::dss; +ID3D11RasterizerState* HairParticle::rs,* HairParticle::ncrs; +ID3D11SamplerState* HairParticle::ss; +ID3D11BlendState* HairParticle::bs; +int HairParticle::LOD[3]; + +HairParticle::HairParticle() +{ +} +HairParticle::HairParticle(const string& newName, float newLen, int newCount + , const string& newMat, Object* newObject, const string& densityGroup, const string& lengthGroup) +{ + name=newName; + densityG=densityGroup; + lenG=lengthGroup; + length=newLen; + count=newCount; + material=nullptr; + object = newObject; + for(Material* m : object->mesh->materials) + if(!newMat.compare(m->name)){ + material = m; + break; + } + + if(material) + SetUpPatches(); +} + + +void HairParticle::CleanUp(){ + for(int i=0;iCleanUp(); + patches.clear(); + Renderer::SafeRelease(vb[0]); + Renderer::SafeRelease(vb[1]); + Renderer::SafeRelease(vb[2]); +} + +void HairParticle::CleanUpStatic(){ + if(il) il->Release(); il=NULL; + if(vs) vs->Release(); vs=NULL; + for(int i=0;i<3;++i){ + if(gs[i]) gs[i]->Release(); gs[i]=NULL;} + for(int i=0;i<2;++i){ + if(qgs[i]) qgs[i]->Release(); qgs[i]=NULL;} + if(ps) ps->Release(); ps=NULL; + if(qps) qps->Release(); qps=NULL; + if(cbgs) cbgs->Release(); cbgs=NULL; + if(ss) ss->Release(); ss=NULL; + if(bs) bs->Release(); bs=NULL; + if(rs) rs->Release(); rs=NULL; + if(ncrs) ncrs->Release(); ncrs=NULL; + +} +void HairParticle::SetUpStatic(){ + Settings(10,25,120); + + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/grassVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load grassVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vs ); + + + // Define the input layout + D3D11_INPUT_ELEMENT_DESC layout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "NORMAL", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TANGENT", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + }; + UINT numElements = ARRAYSIZE( layout ); + + // Create the input layout + Renderer::graphicsDevice->CreateInputLayout( layout, numElements, pVSBlob->GetBufferPointer(), + pVSBlob->GetBufferSize(), &il ); + + if(pVSBlob){ pVSBlob->Release();pVSBlob=NULL; } + + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/grassPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load grassPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &ps ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/qGrassPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load qGrassPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &qps ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + + ID3DBlob* pGSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/grassL0GS.cso", &pGSBlob))){MessageBox(0,L"Failed To load grassL0GS.cso",0,0);} + else Renderer::graphicsDevice->CreateGeometryShader( pGSBlob->GetBufferPointer(), pGSBlob->GetBufferSize(), NULL, &gs[0] ); + if(pGSBlob){ pGSBlob->Release();pGSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/grassL1GS.cso", &pGSBlob))){MessageBox(0,L"Failed To load grassL1GS.cso",0,0);} + else Renderer::graphicsDevice->CreateGeometryShader( pGSBlob->GetBufferPointer(), pGSBlob->GetBufferSize(), NULL, &gs[1] ); + if(pGSBlob){ pGSBlob->Release();pGSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/grassL2GS.cso", &pGSBlob))){MessageBox(0,L"Failed To load grassL2GS.cso",0,0);} + else Renderer::graphicsDevice->CreateGeometryShader( pGSBlob->GetBufferPointer(), pGSBlob->GetBufferSize(), NULL, &gs[2] ); + if(pGSBlob){ pGSBlob->Release();pGSBlob=NULL; } + + + if(FAILED(D3DReadFileToBlob(L"shaders/qGrassLCloseGS.cso", &pGSBlob))){MessageBox(0,L"Failed To load qGrassLCloseGS.cso",0,0);} + else Renderer::graphicsDevice->CreateGeometryShader( pGSBlob->GetBufferPointer(), pGSBlob->GetBufferSize(), NULL, &qgs[0] ); + if(pGSBlob){ pGSBlob->Release();pGSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/qGrassLDistGS.cso", &pGSBlob))){MessageBox(0,L"Failed To load qGrassLDistGS.cso",0,0);} + else Renderer::graphicsDevice->CreateGeometryShader( pGSBlob->GetBufferPointer(), pGSBlob->GetBufferSize(), NULL, &qgs[1] ); + if(pGSBlob){ pGSBlob->Release();pGSBlob=NULL; } + + + + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(CBGS); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &cbgs ); + + + + + + D3D11_RASTERIZER_DESC rsd; + rsd.FillMode=D3D11_FILL_SOLID; + rsd.CullMode=D3D11_CULL_BACK; + rsd.FrontCounterClockwise=true; + rsd.DepthBias=0; + rsd.DepthBiasClamp=0; + rsd.SlopeScaledDepthBias=0; + rsd.DepthClipEnable=true; + rsd.ScissorEnable=false; + rsd.MultisampleEnable=false; + rsd.AntialiasedLineEnable=false; + Renderer::graphicsDevice->CreateRasterizerState(&rsd,&rs); + rsd.FillMode=D3D11_FILL_SOLID; + rsd.CullMode=D3D11_CULL_NONE; + rsd.FrontCounterClockwise=true; + rsd.DepthBias=0; + rsd.DepthBiasClamp=0; + rsd.SlopeScaledDepthBias=0; + rsd.DepthClipEnable=true; + rsd.ScissorEnable=false; + rsd.MultisampleEnable=false; + rsd.AntialiasedLineEnable=false; + Renderer::graphicsDevice->CreateRasterizerState(&rsd,&ncrs); + + + D3D11_DEPTH_STENCIL_DESC dsd; + dsd.DepthEnable = true; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ALL; + dsd.DepthFunc = D3D11_COMPARISON_LESS; + + dsd.StencilEnable = true; + dsd.StencilReadMask = 0xFF; + dsd.StencilWriteMask = 0xFF; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_REPLACE; + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_REPLACE; + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + // Create the depth stencil state. + Renderer::graphicsDevice->CreateDepthStencilState(&dsd, &dss); + + D3D11_SAMPLER_DESC samplerDesc; + samplerDesc.Filter = D3D11_FILTER_ANISOTROPIC; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 16; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + Renderer::graphicsDevice->CreateSamplerState(&samplerDesc, &ss); + + + D3D11_BLEND_DESC bld; + ZeroMemory(&bld, sizeof(bld)); + bld.RenderTarget[0].BlendEnable=false; + bld.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_ALPHA; + bld.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bld.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bld.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bld.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bld.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bld.RenderTarget[0].RenderTargetWriteMask = 0x0f; + bld.AlphaToCoverageEnable=false; + Renderer::graphicsDevice->CreateBlendState(&bld,&bs); +} +void HairParticle::Settings(int l0,int l1,int l2) +{ + LOD[0]=l0; + LOD[1]=l1; + LOD[2]=l2; +} + +struct PatchHolder:public Cullable +{ + HairParticle::Patch* patch; + PatchHolder(){} + PatchHolder(HairParticle::Patch* p){patch=p;} +}; + +void HairParticle::SetUpPatches() +{ + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(Point)*MAX_PARTICLES; + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &vb[0] ); + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &vb[1] ); + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &vb[2] ); + + const int triangleperpatch = 4; + int currentTris=0; + + vector pholder; + patches.push_back(new Patch()); + pholder.push_back(new PatchHolder(patches.back())); + + Mesh* mesh = object->mesh; + XMMATRIX matr = XMLoadFloat4x4(&object->world); + + int dVG=-1,lVG=-1; + if(densityG.compare("")){ + for(int i=0;ivertexGroups.size();++i) + if(!mesh->vertexGroups[i].name.compare(densityG)) + dVG=i; + } + if(lenG.compare("")){ + for(int i=0;ivertexGroups.size();++i) + if(!mesh->vertexGroups[i].name.compare(lenG)) + lVG=i; + } + + float avgPatchSize; + if(dVG>=0) + avgPatchSize = (float)count/((float)mesh->vertexGroups[dVG].vertices.size()/3.0f); + else + avgPatchSize = (float)count/((float)mesh->indices.size()/3.0f); + + for(int i=0;iindices.size()-3;i+=3){ + + int vi[]={mesh->indices[i],mesh->indices[i+1],mesh->indices[i+2]}; + float denMod[]={1,1,1},lenMod[]={1,1,1}; + if(dVG>=0){ + auto found = mesh->vertexGroups[dVG].vertices.find(vi[0]); + if(found!=mesh->vertexGroups[dVG].vertices.end()) + denMod[0]=found->second; + else + continue; + + found = mesh->vertexGroups[dVG].vertices.find(vi[1]); + if(found!=mesh->vertexGroups[dVG].vertices.end()) + denMod[1]=found->second; + else + continue; + + found = mesh->vertexGroups[dVG].vertices.find(vi[2]); + if(found!=mesh->vertexGroups[dVG].vertices.end()) + denMod[2]=found->second; + else + continue; + } + if(lVG>=0){ + auto found = mesh->vertexGroups[lVG].vertices.find(vi[0]); + if(found != mesh->vertexGroups[lVG].vertices.end()) + lenMod[0]=found->second; + else + continue; + + found = mesh->vertexGroups[lVG].vertices.find(vi[1]); + if(found != mesh->vertexGroups[lVG].vertices.end()) + lenMod[1]=found->second; + else + continue; + + found = mesh->vertexGroups[lVG].vertices.find(vi[2]); + if(found != mesh->vertexGroups[lVG].vertices.end()) + lenMod[2]=found->second; + else + continue; + } + for(int m=0;m<3;++m){ + if(denMod[m]<0) denMod[m]=0; + if(lenMod[m]<0) lenMod[m]=0; + } + + SkinnedVertex* v[] = { + &mesh->vertices[vi[0]], + &mesh->vertices[vi[1]], + &mesh->vertices[vi[2]], + }; + if( + (denMod[0] || denMod[1] || denMod[2]) && + (lenMod[0] || lenMod[1] || lenMod[2]) + ) + { + + float density = (float)(denMod[0]+denMod[1]+denMod[2])/3.0f*avgPatchSize; + int rdense = ( density - (int)density ) * 100; + density+=(rand()%100)<=rdense; + int PATCHSIZE = material->texture?(int)density:(int)density*10; + + if(PATCHSIZE){ + + for(int p=0;p 1) + { + f = 1 - f; + g = 1 - g; + } + XMVECTOR pos[] = { + XMVector3Transform(XMLoadFloat4(&v[0]->pos),matr) + , XMVector3Transform(XMLoadFloat4(&v[1]->pos),matr) + , XMVector3Transform(XMLoadFloat4(&v[2]->pos),matr) + }; + XMVECTOR vbar=XMVectorBaryCentric( + pos[0],pos[1],pos[2] + , f + , g + ); + XMVECTOR nbar=XMVectorBaryCentric( + XMLoadFloat3(&v[0]->nor) + , XMLoadFloat3(&v[1]->nor) + , XMLoadFloat3(&v[2]->nor) + , f + , g + ); + int ti=rand()%3; + XMVECTOR tangent = XMVector3Normalize( XMVectorSubtract(pos[ti],pos[(ti+1)%3]) ); + + Point addP; + XMStoreFloat4(&addP.posRand,vbar); + XMStoreFloat4(&addP.normalLen,XMVector3Normalize(nbar)); + XMStoreFloat4(&addP.tangent,tangent); + + float lbar = lenMod[0] + f*(lenMod[1]-lenMod[0]) + g*(lenMod[2]-lenMod[0]); + addP.normalLen.w=length*lbar+(float)(rand()%1001-500)*0.001f*length*lbar; + addP.posRand.w=rand()%1000; + patches.back()->add(addP); + + XMFLOAT3 posN = XMFLOAT3(addP.posRand.x,addP.posRand.y,addP.posRand.z); + patches.back()->min=WickedMath::Min(patches.back()->min,posN); + patches.back()->max=WickedMath::Max(patches.back()->max,posN); + } + /*patches.back()->max.y+=length; + patches.back()->max.x+=length*0.5f; + patches.back()->max.z+=length*0.5f; + patches.back()->min.x-=length*0.5f; + patches.back()->min.z-=length*0.5f;*/ + + if(currentTris>=triangleperpatch){ + pholder.back()->bounds.create(patches.back()->min,patches.back()->max); + currentTris=0; + + //D3D11_BUFFER_DESC bd; + //ZeroMemory( &bd, sizeof(bd) ); + //bd.Usage = D3D11_USAGE_IMMUTABLE; + //bd.ByteWidth = sizeof( Point ) * patches.back()->p.size(); + //bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + //bd.CPUAccessFlags = 0; + //D3D11_SUBRESOURCE_DATA InitData; + //ZeroMemory( &InitData, sizeof(InitData) ); + //InitData.pSysMem = patches.back()->p.data(); + //Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &patches.back()->vb ); + + patches.push_back(new Patch()); + pholder.push_back(new PatchHolder(patches.back())); + } + else + currentTris++; + + } + } + } + + GenerateSPTree(spTree,vector(pholder.begin(),pholder.end()),GENERATE_OCTREE); + return; +} +void HairParticle::Draw(const XMFLOAT3& eye, const XMMATRIX& newView, const XMMATRIX& newProj, ID3D11DeviceContext *context) +{ + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,newProj ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(LOD[2],proj,view); + + + CulledList culledPatches; + if(spTree) + SPTree::getVisible(spTree->root,frustum,culledPatches,SP_TREE_STRICT_CULL); + + if(!culledPatches.empty()) + { + ID3D11ShaderResourceView* texture = material->texture; + + Renderer::BindPrimitiveTopology(Renderer::PRIMITIVETOPOLOGY::POINTLIST,context); + Renderer::BindVertexLayout(il,context); + Renderer::BindPS(texture?qps:ps,context); + Renderer::BindVS(vs,context); + + if(texture){ + Renderer::BindTexturePS(texture,0,context); + Renderer::BindSamplerPS(ss,0,context); + Renderer::BindTextureGS(texture,0,context); + + Renderer::BindBlendState(bs,context); + } + else + Renderer::BindRasterizerState(ncrs,context); + + + CBGS gcb; + gcb.mView = XMMatrixTranspose(newView); + gcb.mProj = XMMatrixTranspose(newProj); + gcb.colTime=XMFLOAT4(material->diffuseColor.x,material->diffuseColor.y,material->diffuseColor.z,Renderer::wind.time); + gcb.eye=eye; + gcb.drawdistance=LOD[2]; + gcb.wind=Renderer::wind.direction; + gcb.windRandomness=Renderer::wind.randomness; + gcb.windWaveSize=Renderer::wind.waveSize; + + Renderer::UpdateBuffer(cbgs,&gcb,context); + + Renderer::BindDepthStencilState(dss,STENCILREF_DEFAULT,context); + Renderer::BindConstantBufferGS(cbgs,0,context); + + + for(int i=0;i<3;++i){ + vector renderPoints; + renderPoints.reserve(MAX_PARTICLES); + + if(texture){ + Renderer::BindGS(i<2?qgs[0]:qgs[1],context); + Renderer::BindRasterizerState(i<2?ncrs:rs,context); + } + else + Renderer::BindGS(gs[i],context); + CulledList::iterator iter = culledPatches.begin(); + while(iter != culledPatches.end()){ + Cullable* culled = *iter; + Patch* patch = ((PatchHolder*)culled)->patch; + + float dis = WickedMath::Distance(eye,((PatchHolder*)culled)->bounds.getCenter()); + + if(disp.begin(),patch->p.end()); + } + else + ++iter; + } + + Renderer::UpdateBuffer(vb[i],renderPoints.data(),context,sizeof(Point)*renderPoints.size()); + Renderer::BindVertexBuffer(vb[i],0,sizeof(Point),context); + Renderer::Draw(renderPoints.size(),context); + } + + Renderer::BindGS(nullptr,context); + Renderer::BindConstantBufferGS(nullptr,0,context); + } +} + +HairParticle::Patch::Patch(){ + p.resize(0); + min=XMFLOAT3(D3D11_FLOAT32_MAX,D3D11_FLOAT32_MAX,D3D11_FLOAT32_MAX); + max=XMFLOAT3(-D3D11_FLOAT32_MAX,-D3D11_FLOAT32_MAX,-D3D11_FLOAT32_MAX); + //vb=NULL; +} +void HairParticle::Patch::add(const Point& pp){ + p.push_back(pp); +} +void HairParticle::Patch::CleanUp(){ + p.clear(); + //if(vb) + // vb->Release(); + //vb=NULL; +} diff --git a/WickedEngine/HairParticle.h b/WickedEngine/HairParticle.h new file mode 100644 index 000000000..7a98da7ce --- /dev/null +++ b/WickedEngine/HairParticle.h @@ -0,0 +1,70 @@ +#pragma once +#include "Particle.h" + +class SPTree; +struct Material; + +class HairParticle : + public Particle +{ +public: + struct Point + { + XMFLOAT4 posRand; + XMFLOAT4 normalLen; + XMFLOAT4 tangent; + }; + struct Patch + { + vector p; + XMFLOAT3 min, max; + //ID3D11Buffer* vb; + Patch(); + void add(const Point&); + void CleanUp(); + }; +private: + float length; + int count; + string name,densityG,lenG; + Material* material; + struct CBGS + { + XMMATRIX mView; + XMMATRIX mProj; + XMFLOAT4 colTime; + XMFLOAT3 eye; float drawdistance; + XMFLOAT3 wind; float pad; + float windRandomness; + float windWaveSize; + float padding[2]; + }; + static ID3D11InputLayout* il; + static ID3D11VertexShader* vs; + static ID3D11PixelShader* ps,*qps; + static ID3D11GeometryShader* gs[3],*qgs[2]; + static ID3D11Buffer* cbgs; + static ID3D11DepthStencilState* dss; + static ID3D11RasterizerState* rs,*ncrs; + static ID3D11SamplerState* ss; + static ID3D11BlendState* bs; + static int LOD[3]; +public: + HairParticle(); + HairParticle(const string& newName, float newLen, int newCount + , const string& newMat, Object* newObject, const string& densityGroup, const string& lengthGroup); + void CleanUp(); + + void SetUpPatches(); + void Draw(const XMFLOAT3& eye, const XMMATRIX& newView, const XMMATRIX& newProj, ID3D11DeviceContext *context); + + static void CleanUpStatic(); + static void SetUpStatic(); + static void Settings(int,int,int); + + Object* object; + vector patches; + SPTree* spTree; + ID3D11Buffer *vb[3]; +}; + diff --git a/WickedEngine/Hitbox2D.cpp b/WickedEngine/Hitbox2D.cpp new file mode 100644 index 000000000..ab037b95b --- /dev/null +++ b/WickedEngine/Hitbox2D.cpp @@ -0,0 +1,16 @@ +#include "WickedEngine.h" +#include "Hitbox2D.h" + + +bool Hitbox2D::intersects(const Hitbox2D& b) +{ + if( (!siz.x && !siz.y) || (!b.siz.x && !b.siz.y) ) + return false; + + if( abs(pos.x-b.pos.x)>(siz.x+b.siz.x)*0.5f ) + return false; + if( abs(pos.y-b.pos.y)>(siz.y+b.siz.y)*0.5f ) + return false; + + return true; +} diff --git a/WickedEngine/Hitbox2D.h b/WickedEngine/Hitbox2D.h new file mode 100644 index 000000000..df7d3350c --- /dev/null +++ b/WickedEngine/Hitbox2D.h @@ -0,0 +1,14 @@ +#pragma once +class Hitbox2D +{ +public: + XMFLOAT2 pos; + XMFLOAT2 siz; + + Hitbox2D():pos(XMFLOAT2(0,0)),siz(XMFLOAT2(0,0)){} + Hitbox2D(const XMFLOAT2& newPos, const XMFLOAT2 newSiz):pos(newPos),siz(newSiz){} + ~Hitbox2D(){}; + + bool intersects(const Hitbox2D& b); +}; + diff --git a/WickedEngine/Image.cpp b/WickedEngine/Image.cpp new file mode 100644 index 000000000..d602436b4 --- /dev/null +++ b/WickedEngine/Image.cpp @@ -0,0 +1,853 @@ +#include "Image.h" + +extern Camera* cam; + +#pragma region STATICS +mutex Image::MUTEX; +ID3D11BlendState* Image::blendState, *Image::blendStateAdd, *Image::blendStateNoBlend, *Image::blendStateAvg; +ID3D11Buffer* Image::constantBuffer,*Image::PSCb,*Image::blurCb,*Image::processCb,*Image::shaftCb,*Image::deferredCb; + +ID3D11VertexShader* Image::vertexShader,*Image::screenVS; +ID3D11PixelShader* Image::pixelShader,*Image::blurHPS,*Image::blurVPS,*Image::shaftPS,*Image::outlinePS + ,*Image::dofPS,*Image::motionBlurPS,*Image::bloomSeparatePS,*Image::fxaaPS,*Image::ssaoPS,*Image::deferredPS + ,*Image::ssssPS,*Image::linDepthPS,*Image::colorGradePS; + + +ID3D11RasterizerState* Image::rasterizerState; +ID3D11DepthStencilState* Image::depthStencilStateGreater,*Image::depthStencilStateLess,*Image::depthNoStencilState; + +int Image::RENDERWIDTH,Image::RENDERHEIGHT; + +//map Image::images; +#pragma endregion + +Image::Image() +{ +} + +void Image::LoadBuffers() +{ + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ConstantBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &constantBuffer ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(PSConstantBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &PSCb ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(BlurBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &blurCb ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ProcessBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &processCb ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(LightShaftBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &shaftCb ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(DeferredBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &deferredCb ); +} + +void Image::LoadShaders() +{ + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/imageVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load imageVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vertexShader ); + if(pVSBlob){ pVSBlob->Release();pVSBlob=NULL; } + + + if(FAILED(D3DReadFileToBlob(L"shaders/screenVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load creenVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &screenVS ); + if(pVSBlob){ pVSBlob->Release();pVSBlob=NULL; } + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/imagePS.cso", &pPSBlob))){MessageBox(0,L"Failed To load imagePS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &pixelShader ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/horizontalBlurPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load horizontalBlurPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &blurHPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/verticalBlurPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load verticalBlurPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &blurVPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/lightShaftPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load lightShaftPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &shaftPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/outlinePS.cso", &pPSBlob))){MessageBox(0,L"Failed To load outlinePS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &outlinePS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/depthofFieldPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load depthofFieldPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &dofPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/motionBlurPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load motionBlurPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &motionBlurPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/bloomSeparatePS.cso", &pPSBlob))){MessageBox(0,L"Failed To load bloomSeparatePS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &bloomSeparatePS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/fxaa.cso", &pPSBlob))){MessageBox(0,L"Failed To load fxaa.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &fxaaPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/ssao.cso", &pPSBlob))){MessageBox(0,L"Failed To load ssao.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &ssaoPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/ssss.cso", &pPSBlob))){MessageBox(0,L"Failed To load ssss.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &ssssPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/linDepth.cso", &pPSBlob))){MessageBox(0,L"Failed To load linDepth.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &linDepthPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/colorGrade.cso", &pPSBlob))){MessageBox(0,L"Failed To load colorGrade.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &colorGradePS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/deferredPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load deferredPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &deferredPS ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } +} +void Image::SetUpStates() +{ + + + D3D11_RASTERIZER_DESC rs; + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_BACK; + rs.FrontCounterClockwise=false; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=FALSE; + rs.ScissorEnable=FALSE; + rs.MultisampleEnable=FALSE; + rs.AntialiasedLineEnable=FALSE; + Renderer::graphicsDevice->CreateRasterizerState(&rs,&rasterizerState); + + + + + + D3D11_DEPTH_STENCIL_DESC dsd; + dsd.DepthEnable = false; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ZERO; + dsd.DepthFunc = D3D11_COMPARISON_LESS; + + dsd.StencilEnable = true; + dsd.StencilReadMask = D3D11_DEFAULT_STENCIL_READ_MASK; + dsd.StencilWriteMask = 0; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_LESS_EQUAL; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_LESS_EQUAL; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + // Create the depth stencil state. + Renderer::graphicsDevice->CreateDepthStencilState(&dsd, &depthStencilStateLess); + + + dsd.DepthEnable = false; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ZERO; + dsd.DepthFunc = D3D11_COMPARISON_LESS; + + dsd.StencilEnable = true; + dsd.StencilReadMask = D3D11_DEFAULT_STENCIL_READ_MASK; + dsd.StencilWriteMask = 0; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_GREATER; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_GREATER; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + // Create the depth stencil state. + Renderer::graphicsDevice->CreateDepthStencilState(&dsd, &depthStencilStateGreater); + + dsd.StencilEnable = false; + Renderer::graphicsDevice->CreateDepthStencilState(&dsd, &depthNoStencilState); + + + D3D11_BLEND_DESC bd; + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_ALPHA; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + bd.IndependentBlendEnable=true; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendState); + + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=false; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_ALPHA; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + //bd.IndependentBlendEnable=true; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendStateNoBlend); + + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + //bd.IndependentBlendEnable=true; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendStateAdd); + + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_COLOR; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_MAX; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_MAX; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + //bd.IndependentBlendEnable=true; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendStateAvg); +} + + +void Image::Draw(Renderer::TextureView texture, const ImageEffects& effects){ + Draw(texture,effects,Renderer::immediateContext); +} +void Image::Draw(Renderer::TextureView texture, const ImageEffects& effects,ID3D11DeviceContext* context){ + if(!context) + return; + if(!effects.blur){ + if(!effects.process.active && !effects.bloom.separate && !effects.sunPos.x && !effects.sunPos.y){ + ConstantBuffer cb; + if(effects.typeFlag==SCREEN){ + cb.mViewProjection = XMMatrixTranspose( cam->Oprojection ); + cb.mTrans = XMMatrixTranspose( XMMatrixTranslation(RENDERWIDTH/2-effects.siz.x/2,-RENDERHEIGHT/2+effects.siz.y/2,0) * XMMatrixRotationZ(effects.rotation) + * XMMatrixTranslation(-RENDERWIDTH/2+effects.pos.x+effects.siz.x*0.5f,RENDERHEIGHT/2 + effects.pos.y-effects.siz.y*0.5f,0) ); //AUTO ORIGIN CORRECTION APPLIED! NO FURTHER TRANSLATIONS NEEDED! + cb.mDimensions = XMFLOAT4(RENDERWIDTH,RENDERHEIGHT,effects.siz.x,effects.siz.y); + } + else if(effects.typeFlag==WORLD){ + cb.mViewProjection = XMMatrixTranspose( cam->View * cam->Projection ); + XMMATRIX faceRot = XMMatrixIdentity(); + if(effects.lookAt.w){ + XMVECTOR vvv = (effects.lookAt.x==1 && !effects.lookAt.y && !effects.lookAt.z)?XMVectorSet(0,1,0,0):XMVectorSet(1,0,0,0); + faceRot = + XMMatrixLookAtLH(XMVectorSet(0,0,0,0) + ,XMLoadFloat4(&effects.lookAt) + ,XMVector3Cross( + XMLoadFloat4(&effects.lookAt),vvv + ) + ) + ; + } + else + faceRot=XMMatrixRotationX(cam->updownRot)*XMMatrixRotationY(cam->leftrightRot); + cb.mTrans = XMMatrixTranspose( + XMMatrixScaling(effects.scale.x,effects.scale.y,1) + *XMMatrixRotationZ(effects.rotation) + *faceRot + //*XMMatrixInverse(0,XMMatrixLookAtLH(XMVectorSet(0,0,0,0),XMLoadFloat3(&effects.pos)-cam->Eye,XMVectorSet(0,1,0,0))) + *XMMatrixTranslation(effects.pos.x,effects.pos.y,effects.pos.z) + ); + cb.mDimensions = XMFLOAT4(0,0,effects.siz.x,effects.siz.y); + } + + cb.mOffsetMirFade = XMFLOAT4(effects.offset.x,effects.offset.y,effects.mirror,effects.fade); + cb.mDrawRec = effects.drawRec; + cb.mBlurOpaPiv = XMFLOAT4(effects.blurDir,effects.blur,effects.opacity,effects.pivotFlag); + cb.mTexOffset=XMFLOAT4(effects.texOffset.x,effects.texOffset.y,0,0); + + Renderer::UpdateBuffer(constantBuffer,&cb,context); + + //context->UpdateSubresource( constantBuffer, 0, NULL, &cb, 0, 0 ); + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //ConstantBuffer* dataPtr; + //context->Map(constantBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (ConstantBuffer*)mappedResource.pData; + //memcpy(dataPtr,&cb,sizeof(ConstantBuffer)); + //context->Unmap(constantBuffer,0); + + //context->VSSetConstantBuffers( 0, 1, &constantBuffer ); + + Renderer::BindConstantBufferVS(constantBuffer,0,context); + } + else if(!effects.sunPos.x && !effects.sunPos.y){ + //context->VSSetShader(screenVS,0,0); + Renderer::BindVS(screenVS,context); + + + if(effects.process.outline) + //context->PSSetShader(outlinePS,0,0); + Renderer::BindPS(outlinePS,context); + else if(effects.process.motionBlur) + //context->PSSetShader(motionBlurPS,0,0); + Renderer::BindPS(motionBlurPS,context); + else if(effects.process.dofStrength) + //context->PSSetShader(dofPS,0,0); + Renderer::BindPS(dofPS,context); + else if(effects.process.fxaa) + //context->PSSetShader(fxaaPS,0,0); + Renderer::BindPS(fxaaPS,context); + else if(effects.process.ssao) { + //context->PSSetShader(ssaoPS,0,0); + Renderer::BindPS(ssaoPS,context); + } + else if(effects.process.linDepth) + //context->PSSetShader(linDepthPS,0,0); + Renderer::BindPS(linDepthPS,context); + else if(effects.process.colorGrade) + Renderer::BindPS(colorGradePS,context); + else if(effects.process.ssss.x||effects.process.ssss.y) + //context->PSSetShader(ssssPS,0,0); + Renderer::BindPS(ssssPS,context); + else if(effects.bloom.separate) + //context->PSSetShader(bloomSeparatePS,0,0); + Renderer::BindPS(bloomSeparatePS,context); + else MessageBoxA(0,"Postprocess branch not implemented!","Warning!",0); + + ProcessBuffer prcb; + prcb.mPostProcess=XMFLOAT4(effects.process.motionBlur,effects.process.outline,effects.process.dofStrength,effects.process.ssss.x); + prcb.mBloom=XMFLOAT4(effects.bloom.separate,effects.bloom.threshold,effects.bloom.saturation,effects.process.ssss.y); + + Renderer::UpdateBuffer(processCb,&prcb,context); + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //ProcessBuffer* dataPtr2; + //context->Map(processCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr2 = (ProcessBuffer*)mappedResource.pData; + //memcpy(dataPtr2,&prcb,sizeof(ProcessBuffer)); + //context->Unmap(processCb,0); + + //context->PSSetConstantBuffers( 1, 1, &processCb ); + + Renderer::BindConstantBufferPS(processCb,1,context); + } + else{ //LIGHTSHAFTS + //context->VSSetShader(screenVS,0,0); + //context->PSSetShader(shaftPS,0,0); + Renderer::BindVS(screenVS,context); + Renderer::BindPS(shaftPS,context); + + LightShaftBuffer scb; + scb.mProperties=XMFLOAT4(0.65f,0.25f,0.945f,0.2f); //Density|Weight|Decay|Exposure + scb.mLightShaft=effects.sunPos; + + Renderer::UpdateBuffer(shaftCb,&scb,context); + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //LightShaftBuffer* dataPtr2; + //context->Map(shaftCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr2 = (LightShaftBuffer*)mappedResource.pData; + //memcpy(dataPtr2,&scb,sizeof(LightShaftBuffer)); + //context->Unmap(shaftCb,0); + + //context->PSSetConstantBuffers( 0, 1, &shaftCb ); + Renderer::BindConstantBufferPS(shaftCb,0,context); + } + + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + PSConstantBuffer pscb; + int normalmapmode=0; + if(effects.normalMap && effects.refractionMap) + normalmapmode=1; + if(effects.extractNormalMap==true) + normalmapmode=2; + pscb.mMaskFadOpaDis=XMFLOAT4(effects.maskMap?1:0,effects.fade,effects.opacity,normalmapmode); + pscb.mDimension=XMFLOAT4(RENDERWIDTH,RENDERHEIGHT,effects.siz.x,effects.siz.y); + + Renderer::UpdateBuffer(PSCb,&pscb,context); + //PSConstantBuffer* dataPtr2; + //context->Map(PSCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr2 = (PSConstantBuffer*)mappedResource.pData; + //memcpy(dataPtr2,&pscb,sizeof(PSConstantBuffer)); + //context->Unmap(PSCb,0); + + //context->PSSetConstantBuffers( 2, 1, &PSCb ); + Renderer::BindConstantBufferPS(PSCb,2,context); + + Renderer::BindTexturePS(effects.depthMap,0,context); + Renderer::BindTexturePS(effects.normalMap,1,context); + Renderer::BindTexturePS(effects.velocityMap,3,context); + Renderer::BindTexturePS(effects.refractionMap,4,context); + Renderer::BindTexturePS(effects.maskMap,5,context); + } + else{ //BLUR + Renderer::BindVS(screenVS,context); + + BlurBuffer cb; + if(effects.blurDir==0){ + Renderer::BindPS(blurHPS,context); + cb.mWeightTexelStren.y=1.0f/RENDERWIDTH; + } + else{ + Renderer::BindPS(blurVPS,context); + cb.mWeightTexelStren.y=1.0f/RENDERHEIGHT; + } + + float weight0 = 1.0f; + float weight1 = 0.9f; + float weight2 = 0.55f; + float weight3 = 0.18f; + float weight4 = 0.1f; + float normalization = (weight0 + 2.0f * (weight1 + weight2 + weight3 + weight4)); + cb.mWeight=XMVectorSet(weight0,weight1,weight2,weight3) / normalization; + cb.mWeightTexelStren.x=weight4 / normalization; + cb.mWeightTexelStren.z=effects.blur; + + Renderer::UpdateBuffer(blurCb,&cb,context); + + Renderer::BindConstantBufferPS(blurCb,0,context); + } + + //if(texture) + Renderer::BindTexturePS(texture,6,context); + + + float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + UINT sampleMask = 0xffffffff; + if(effects.blendFlag==BLENDMODE_ALPHA) + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + Renderer::BindBlendState(blendState,context); + else if(effects.blendFlag==BLENDMODE_ADDITIVE) + //context->OMSetBlendState(blendStateAdd, blendFactor, sampleMask); + Renderer::BindBlendState(blendStateAdd,context); + else if(effects.blendFlag==BLENDMODE_OPAQUE) + //context->OMSetBlendState(blendStateNoBlend, blendFactor, sampleMask); + Renderer::BindBlendState(blendStateNoBlend,context); + else if(effects.blendFlag==BLENDMODE_MAX) + //context->OMSetBlendState(blendStateAvg, blendFactor, sampleMask); + Renderer::BindBlendState(blendStateAvg,context); + + if(effects.quality==QUALITY_NEAREST){ + if(effects.sampleFlag==SAMPLEMODE_MIRROR) + //context->PSSetSamplers(0, 1, &ssMirrorPoi); + Renderer::BindSamplerPS(Renderer::ssMirrorPoi,0,context); + else if(effects.sampleFlag==SAMPLEMODE_WRAP) + //context->PSSetSamplers(0, 1, &ssWrapPoi); + Renderer::BindSamplerPS(Renderer::ssWrapPoi,0,context); + else if(effects.sampleFlag==SAMPLEMODE_CLAMP) + //context->PSSetSamplers(0, 1, &ssClampPoi); + Renderer::BindSamplerPS(Renderer::ssClampPoi,0,context); + } + else if(effects.quality==QUALITY_BILINEAR){ + if(effects.sampleFlag==SAMPLEMODE_MIRROR) + //context->PSSetSamplers(0, 1, &ssMirrorLin); + Renderer::BindSamplerPS(Renderer::ssMirrorLin,0,context); + else if(effects.sampleFlag==SAMPLEMODE_WRAP) + //context->PSSetSamplers(0, 1, &ssWrapLin); + Renderer::BindSamplerPS(Renderer::ssWrapLin,0,context); + else if(effects.sampleFlag==SAMPLEMODE_CLAMP) + //context->PSSetSamplers(0, 1, &ssClampLin); + Renderer::BindSamplerPS(Renderer::ssClampLin,0,context); + } + else if(effects.quality==QUALITY_ANISOTROPIC){ + if(effects.sampleFlag==SAMPLEMODE_MIRROR) + //context->PSSetSamplers(0, 1, &ssMirrorAni); + Renderer::BindSamplerPS(Renderer::ssMirrorAni,0,context); + else if(effects.sampleFlag==SAMPLEMODE_WRAP) + //context->PSSetSamplers(0, 1, &ssWrapAni); + Renderer::BindSamplerPS(Renderer::ssWrapAni,0,context); + else if(effects.sampleFlag==SAMPLEMODE_CLAMP) + //context->PSSetSamplers(0, 1, &ssClampAni); + Renderer::BindSamplerPS(Renderer::ssClampAni,0,context); + } + + + //context->Draw(4,0); + Renderer::Draw(4,context); +} + +void Image::DrawDeferred(Renderer::TextureView texture + , Renderer::TextureView depth, Renderer::TextureView lightmap, Renderer::TextureView normal + , Renderer::TextureView ao, ID3D11DeviceContext* context, int stencilRef){ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLESTRIP ); + + //UINT stride = sizeof( Vertex ); + //UINT offset = 0; + //context->IASetVertexBuffers( 0, 1, &vertexBuffer, &stride, &offset ); + //context->IASetInputLayout( vertexLayout ); + + //context->RSSetState(rasterizerState); + //context->OMSetDepthStencilState(depthStencilStateLess, stencilRef); + + Renderer::BindPrimitiveTopology(Renderer::PRIMITIVETOPOLOGY::TRIANGLESTRIP,context); + Renderer::BindRasterizerState(rasterizerState,context); + Renderer::BindDepthStencilState(depthStencilStateLess,stencilRef,context); + + //context->VSSetShader(screenVS,0,0); + //context->PSSetShader(deferredPS,0,0); + Renderer::BindVS(screenVS,context); + Renderer::BindPS(deferredPS,context); + + Renderer::BindTexturePS(depth,0,context); + Renderer::BindTexturePS(normal,1,context); + Renderer::BindTexturePS(texture,6,context); + Renderer::BindTexturePS(lightmap,7,context); + Renderer::BindTexturePS(ao,8,context); + + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + //context->PSSetSamplers(0, 1, &ssClampLin); + Renderer::BindSamplerPS(Renderer::ssClampLin,0,context); + //context->PSSetSamplers(1, 1, &ssComp); + + Renderer::BindBlendState(blendStateNoBlend,context); + + DeferredBuffer cb; + //cb.mSun=XMVector3Normalize(Renderer::GetSunPosition()); + //cb.mEye=cam->Eye; + cb.mAmbient=Renderer::worldInfo.ambient; + //cb.mBiasResSoftshadow=shadowProps; + cb.mHorizon=Renderer::worldInfo.horizon; + cb.mViewProjInv=XMMatrixInverse( 0,XMMatrixTranspose(cam->View*cam->Projection) ); + cb.mFogSEH=Renderer::worldInfo.fogSEH; + + Renderer::UpdateBuffer(deferredCb,&cb,context); + + /*cb.mShM[0] = shMs[0]; + cb.mShM[1] = shMs[1]; + cb.mShM[2] = shMs[2];*/ + //delete[] shMs; + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //DeferredBuffer* dataPtr; + //context->Map(deferredCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (DeferredBuffer*)mappedResource.pData; + //memcpy(dataPtr,&cb,sizeof(DeferredBuffer)); + //context->Unmap(deferredCb,0); + //context->PSSetConstantBuffers(0,1,&deferredCb); + // + //context->Draw(4,0); + + Renderer::BindConstantBufferPS(deferredCb,0,context); + Renderer::Draw(4,context); +} + + +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz) +{ + Draw(texture,newPosSiz,XMFLOAT4(0,0,0,0),1,0,0,0,0,0,Renderer::immediateContext); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const float&newRot) +{ + Draw(texture,newPosSiz,XMFLOAT4(0,0,0,0),1,0,0,0,0,newRot,Renderer::immediateContext); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const float&newRot, const float&opacity, ID3D11DeviceContext* context) +{ + Draw(texture,newPosSiz,XMFLOAT4(0,0,0,0),1,0,0,0,opacity,newRot,context); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec) +{ + Draw(texture,newPosSiz,newDrawRec,1,0,0,0,0,0,Renderer::immediateContext); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror) +{ + Draw(texture,newPosSiz,newDrawRec,newMirror,0,0,0,0,0,Renderer::immediateContext); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror, const float&newBlur, const float&newBlurStrength, const float&newFade, const float&newOpacity) +{ + Draw(texture,newPosSiz,newDrawRec,newMirror,newBlur,newBlurStrength,newFade,newOpacity,0,Renderer::immediateContext); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror, const float&newBlur, const float&newBlurStrength, const float&newFade, const float&newOpacity, ID3D11DeviceContext* context) +{ + Draw(texture,newPosSiz,newDrawRec,newMirror,newBlur,newBlurStrength,newFade,newOpacity,0,context); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror, const float&newBlur, const float&newBlurStrength, const float&newFade, const float&newOpacity, const float&newRot) +{ + Draw(texture,newPosSiz,newDrawRec,newMirror,newBlur,newBlurStrength,newFade,newOpacity,newRot,Renderer::immediateContext); +} +void Image::Draw(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror, const float&newBlur, const float&newBlurStrength, const float&newFade, const float&newOpacity, const float&newRot, ID3D11DeviceContext* context) +{ + Draw(texture,0,newPosSiz,newDrawRec,newMirror,newBlur,newBlurStrength,newFade,newOpacity,newRot,XMFLOAT2(0,0),BLENDMODE_ALPHA,context); +} +void Image::Draw(Renderer::TextureView texture, Renderer::TextureView mask, const XMFLOAT4& newPosSiz + , const XMFLOAT4& newDrawRec, const float&newMirror, const float&newBlur, const float&newBlurStrength + , const float&newFade, const float&newOpacity, const float&newRot, XMFLOAT2 texOffset + , BLENDMODE blendMode, ID3D11DeviceContext* context) +{ + ImageEffects fx = ImageEffects(); + fx.pos=XMFLOAT3(newPosSiz.x,newPosSiz.y,0); + fx.siz=XMFLOAT2(newPosSiz.z,newPosSiz.w); + fx.drawRec=newDrawRec; + fx.mirror=newMirror; + fx.blur=newBlurStrength; + fx.fade=newFade; + fx.opacity=newOpacity; + fx.rotation=newRot; + fx.texOffset=texOffset; + fx.blendFlag=blendMode; + fx.setMaskMap(mask); + + Draw(texture,fx,context); +} +void Image::DrawModifiedTexCoords(Renderer::TextureView texture, Renderer::TextureView mask, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror, XMFLOAT2 texOffset, const float&newOpacity, BLENDMODE blendMode) +{ + Draw(texture,mask,newPosSiz,newDrawRec,newMirror,0,0,0,newOpacity,0,texOffset,blendMode,Renderer::immediateContext); +} +void Image::DrawModifiedTexCoords(Renderer::TextureView texture, Renderer::TextureView mask, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror, XMFLOAT2 texOffset, const float&newOpacity, BLENDMODE blendMode, ID3D11DeviceContext* context) +{ + Draw(texture,mask,newPosSiz,newDrawRec,newMirror,0,0,0,newOpacity,0,texOffset,blendMode,context); +} +void Image::DrawOffset(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, XMFLOAT2 newOffset) +{ + ConstantBuffer cb; + cb.mViewProjection = XMMatrixTranspose( cam->Oprojection ); + cb.mTrans = XMMatrixTranspose( XMMatrixTranslation(newPosSiz.x,newPosSiz.y,0) ); + cb.mDimensions = XMFLOAT4(RENDERWIDTH,RENDERHEIGHT,newPosSiz.z,newPosSiz.w); + cb.mOffsetMirFade = XMFLOAT4(newOffset.x,newOffset.y,1,0); + cb.mDrawRec = XMFLOAT4(0,0,0,0); + cb.mBlurOpaPiv = XMFLOAT4(0,0,0,0); + cb.mTexOffset=XMFLOAT4(0,0,0,0); + + Renderer::UpdateBuffer(constantBuffer,&cb,Renderer::immediateContext); + + //Renderer::immediateContext->UpdateSubresource( constantBuffer, 0, NULL, &cb, 0, 0 ); + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //ConstantBuffer* dataPtr; + //Renderer::immediateContext->Map(constantBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (ConstantBuffer*)mappedResource.pData; + //memcpy(dataPtr,&cb,sizeof(ConstantBuffer)); + //Renderer::immediateContext->Unmap(constantBuffer,0); + + Renderer::immediateContext->VSSetConstantBuffers( 0, 1, &constantBuffer ); + + + //PSConstantBuffer pscb; + ////pscb.mMaskFxBlSa=XMFLOAT4(0,0,0,0); + ////Renderer::immediateContext->UpdateSubresource( PSCb, 0, NULL, &pscb, 0, 0 ); + //PSConstantBuffer* dataPtr2; + //Renderer::immediateContext->Map(PSCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr2 = (PSConstantBuffer*)mappedResource.pData; + //memcpy(dataPtr2,&pscb,sizeof(PSConstantBuffer)); + //Renderer::immediateContext->Unmap(PSCb,0); + + //Renderer::immediateContext->PSSetConstantBuffers( 0, 1, &PSCb ); + + + Renderer::BindTexturePS(texture,5,Renderer::immediateContext); + Renderer::immediateContext->Draw(4,0); + +} +void Image::DrawOffset(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, XMFLOAT2 newOffset) +{ + ConstantBuffer cb; + cb.mViewProjection = XMMatrixTranspose( cam->Oprojection ); + cb.mTrans = XMMatrixTranspose( XMMatrixTranslation(newPosSiz.x,newPosSiz.y,0) ); + cb.mDimensions = XMFLOAT4(RENDERWIDTH,RENDERHEIGHT,newPosSiz.z,newPosSiz.w); + cb.mOffsetMirFade = XMFLOAT4(newOffset.x,newOffset.y,1,0); + cb.mDrawRec = newDrawRec; + cb.mBlurOpaPiv = XMFLOAT4(0,0,0,0); + cb.mTexOffset=XMFLOAT4(0,0,0,0); + + Renderer::UpdateBuffer(constantBuffer,&cb,Renderer::immediateContext); + + Renderer::BindTexturePS(texture,5,Renderer::immediateContext); + Renderer::immediateContext->Draw(4,0); + +} +void Image::DrawAdditive(Renderer::TextureView texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec) +{ + ConstantBuffer cb; + cb.mViewProjection = XMMatrixTranspose( cam->Oprojection ); + cb.mTrans = XMMatrixTranspose( XMMatrixTranslation(newPosSiz.x,newPosSiz.y,0) ); + cb.mDimensions = XMFLOAT4(RENDERWIDTH,RENDERHEIGHT,newPosSiz.z,newPosSiz.w); + cb.mOffsetMirFade = XMFLOAT4(0,0,1,0); + cb.mDrawRec = newDrawRec; + cb.mBlurOpaPiv = XMFLOAT4(0,0,0,0); + cb.mTexOffset=XMFLOAT4(0,0,0,0); + + Renderer::UpdateBuffer(constantBuffer,&cb,Renderer::immediateContext); + // + + //Renderer::immediateContext->VSSetConstantBuffers( 0, 1, &constantBuffer ); + + //Renderer::BindTexturePS(texture,5,Renderer::immediateContext); + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //Renderer::immediateContext->OMSetBlendState(blendStateAdd, blendFactor, sampleMask); + // Renderer::immediateContext->Draw(4,0); + //Renderer::immediateContext->OMSetBlendState(blendState, blendFactor, sampleMask); + + Renderer::BindConstantBufferVS(constantBuffer,0); + Renderer::BindTexturePS(texture,5); + Renderer::BindBlendState(blendStateAdd); + Renderer::Draw(4); + Renderer::BindBlendState(blendState); + +} + + +void Image::BatchBegin() +{ + BatchBegin(Renderer::immediateContext); +} +void Image::BatchBegin(ID3D11DeviceContext* context) +{ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLESTRIP ); + + //context->RSSetState(rasterizerState); + //context->OMSetDepthStencilState(depthNoStencilState, 0); + + + Renderer::BindPrimitiveTopology(Renderer::PRIMITIVETOPOLOGY::TRIANGLESTRIP,context); + Renderer::BindRasterizerState(rasterizerState,context); + Renderer::BindDepthStencilState(depthNoStencilState, 0, context); + + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState,blendFactor,sampleMask); + Renderer::BindBlendState(blendState,context); + + //context->VSSetShader( vertexShader, NULL, 0 ); + //context->PSSetShader( pixelShader, NULL, 0 ); + Renderer::BindVS(vertexShader,context); + Renderer::BindPS(pixelShader,context); +} +void Image::BatchBegin(ID3D11DeviceContext* context, unsigned int stencilref, bool stencilOpLess) +{ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLESTRIP ); + + //context->RSSetState(rasterizerState); + //context->OMSetDepthStencilState(stencilOpLess?depthStencilStateLess:depthStencilStateGreater, stencilref); + + Renderer::BindPrimitiveTopology(Renderer::PRIMITIVETOPOLOGY::TRIANGLESTRIP,context); + Renderer::BindRasterizerState(rasterizerState,context); + Renderer::BindDepthStencilState(stencilOpLess?depthStencilStateLess:depthStencilStateGreater, stencilref, context); + + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState,blendFactor,sampleMask); + Renderer::BindBlendState(blendState,context); + + //context->VSSetShader( vertexShader, NULL, 0 ); + //context->PSSetShader( pixelShader, NULL, 0 ); + Renderer::BindVS(vertexShader,context); + Renderer::BindPS(pixelShader,context); +} + + +void Image::Load(){ + LoadShaders(); + LoadBuffers(); + SetUpStates(); +} +void Image::CleanUp() +{ +// for(map::iterator iter=images.begin();iter!=images.end();++iter) +// iter->second.CleanUp(); +// images.clear(); + + if(vertexShader) vertexShader->Release(); + if(pixelShader) pixelShader->Release(); + if(screenVS) screenVS->Release(); + if(blurHPS) blurHPS->Release(); + if(blurVPS) blurVPS->Release(); + if(shaftPS) shaftPS->Release(); + if(bloomSeparatePS) bloomSeparatePS->Release(); + if(motionBlurPS) motionBlurPS->Release(); + if(dofPS) dofPS->Release(); + if(outlinePS) outlinePS->Release(); + if(fxaaPS) fxaaPS->Release(); + if(deferredPS) deferredPS->Release(); + Renderer::SafeRelease(colorGradePS); + + if(constantBuffer) constantBuffer->Release(); + if(PSCb) PSCb->Release(); + if(blurCb) blurCb->Release(); + if(processCb) processCb->Release(); + if(shaftCb) shaftCb->Release(); + if(deferredCb) deferredCb->Release(); + + if(rasterizerState) rasterizerState->Release(); + if(blendState) blendState->Release(); + if(blendStateAdd) blendStateAdd->Release(); + if(blendStateNoBlend) blendStateNoBlend->Release(); + if(blendStateAvg) blendStateAvg->Release(); + + + if(depthNoStencilState) depthNoStencilState->Release(); depthNoStencilState=nullptr; + if(depthStencilStateLess) depthStencilStateLess->Release(); depthStencilStateLess=nullptr; + if(depthStencilStateGreater) depthStencilStateGreater->Release(); depthStencilStateGreater=nullptr; +} + +/* +Image::ImageResource::ImageResource(const string& newDirectory, const string& newName){ + name=newName; + wstring wname(name.begin(),name.end()); + wstring wdir(newDirectory.begin(),newDirectory.end()); + wstringstream wss(L""); + wss<::iterator iter = images.find(get); + if(iter!=images.end()) + return iter->second.texture; + return nullptr; +} +void Image::addImageResource(const string& dir, const string& name){ + MUTEX.lock(); + images.insert( pair(name,ImageResource(dir,name)) ); + MUTEX.unlock(); +} +*/ \ No newline at end of file diff --git a/WickedEngine/Image.h b/WickedEngine/Image.h new file mode 100644 index 000000000..ec9afad47 --- /dev/null +++ b/WickedEngine/Image.h @@ -0,0 +1,123 @@ +#pragma once +#include "WickedEngine.h" + +class Image +{ +private: + static mutex MUTEX; +protected: + struct ConstantBuffer + { + XMMATRIX mViewProjection; + XMMATRIX mTrans; + XMFLOAT4 mDimensions; + XMFLOAT4 mOffsetMirFade; + XMFLOAT4 mDrawRec; + XMFLOAT4 mBlurOpaPiv; + XMFLOAT4 mTexOffset; + }; + struct PSConstantBuffer + { + XMFLOAT4 mMaskFadOpaDis; + XMFLOAT4 mDimension; + PSConstantBuffer(){mMaskFadOpaDis=mDimension=XMFLOAT4(0,0,0,0);} + }; + struct ProcessBuffer + { + XMFLOAT4 mPostProcess; + XMFLOAT4 mBloom; + ProcessBuffer(){mPostProcess=mBloom=XMFLOAT4(0,0,0,0);} + }; + struct LightShaftBuffer + { + XMFLOAT4 mProperties; + XMFLOAT2 mLightShaft; XMFLOAT2 mPadding; + LightShaftBuffer(){mProperties=XMFLOAT4(0,0,0,0);mLightShaft=XMFLOAT2(0,0);} + }; + struct DeferredBuffer + { + XMFLOAT3 mAmbient; float pad; + XMFLOAT3 mHorizon; float pad1; + XMMATRIX mViewProjInv; + XMFLOAT3 mFogSEH; float pad2; + }; + struct BlurBuffer + { + XMVECTOR mWeight; + XMFLOAT3 mWeightTexelStren; + }; + + static ID3D11BlendState* blendState, *blendStateAdd, *blendStateNoBlend, *blendStateAvg; + static ID3D11Buffer* constantBuffer,*PSCb,*blurCb,*processCb,*shaftCb,*deferredCb; + + static ID3D11VertexShader* vertexShader,*screenVS; + static ID3D11PixelShader* pixelShader,*blurHPS,*blurVPS,*shaftPS,*outlinePS,*dofPS,*motionBlurPS,*bloomSeparatePS + ,*fxaaPS,*ssaoPS,*ssssPS,*deferredPS,*linDepthPS,*colorGradePS; + + + + static ID3D11RasterizerState* rasterizerState; + static ID3D11DepthStencilState* depthStencilStateGreater,*depthStencilStateLess,*depthNoStencilState; + + static void LoadShaders(); + static void LoadBuffers(); + static void SetUpStates(); + static int RENDERWIDTH,RENDERHEIGHT; + +public: + Image(); + + static void Draw(ID3D11ShaderResourceView* texture, const ImageEffects& effects); + inline static void Draw(ID3D11ShaderResourceView* texture, const ImageEffects& effects,ID3D11DeviceContext* context); + + static void DrawDeferred(ID3D11ShaderResourceView* texture + , ID3D11ShaderResourceView* depth, ID3D11ShaderResourceView* lightmap, ID3D11ShaderResourceView* normal + , ID3D11ShaderResourceView* ao, ID3D11DeviceContext* context, int stencilref = 0); + + + + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT3& pos, const XMFLOAT2& siz, ID3D11DeviceContext* context); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz,const float&rotation); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz,const float&rotation, const float&opacity, ID3D11DeviceContext* context); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz, const XMFLOAT4& drawRec); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, const float&mirror); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, const float&mirror, const float&blur, const float&blurStrength, const float&fade, const float&opacity); + static void Draw(ID3D11ShaderResourceView *texture, const XMFLOAT4& newPosSiz, const XMFLOAT4& newDrawRec, const float&newMirror, const float&newBlur, const float&newBlurStrength, const float&newFade, const float&newOpacity, ID3D11DeviceContext* context); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, const float&mirror, const float&blur, const float&blurStrength, const float&fade, const float&opacity, const float&rotation); + static void Draw(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, const float&mirror, const float&blur, const float&blurStrength, const float&fade, const float&opacity, const float&rotation, ID3D11DeviceContext* context); + static void Draw(ID3D11ShaderResourceView* resourceView, ID3D11ShaderResourceView* mask, const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, const float&mirror, const float&blur, const float&blurStrength, const float&fade, const float&opacity, const float&rotation, XMFLOAT2 texOffset, BLENDMODE blendFlag, ID3D11DeviceContext* context); + + static void DrawModifiedTexCoords(ID3D11ShaderResourceView*, ID3D11ShaderResourceView*,const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, const float&mirror, XMFLOAT2 texOffset, const float&opacity, BLENDMODE blendFlag); + static void DrawModifiedTexCoords(ID3D11ShaderResourceView*, ID3D11ShaderResourceView*, const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, const float&mirror, XMFLOAT2 texOffset, const float&opacity, BLENDMODE blendFlag, ID3D11DeviceContext* context); + static void DrawOffset(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz,XMFLOAT2 offset); + static void DrawOffset(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz, const XMFLOAT4& drawRec, XMFLOAT2 offset); + static void DrawAdditive(ID3D11ShaderResourceView* resourceView,const XMFLOAT4& posSiz,const XMFLOAT4& drawRec=XMFLOAT4(0,0,0,0)); + + static void BatchBegin(); + static void BatchBegin(ID3D11DeviceContext* context); + static void BatchBegin(ID3D11DeviceContext* context, unsigned int stencilref, bool stencilOpLess=true); + + static void Load(); + static void CleanUp(); + + static void SetScreenResolution(int width, int height){RENDERHEIGHT=height;RENDERWIDTH=width;}; + + /* + struct ImageResource{ + string name; + ID3D11ShaderResourceView* texture; + + ImageResource(){name="";texture=NULL;}; + ImageResource(const string& newDirectory, const string& newName); //in Renderer.cpp + void CleanUp(){ + if(texture) texture->Release(); texture=NULL; + } + }; + static map images; + static ID3D11ShaderResourceView* getImageByName(const string& get); + static void addImageResource(const string& dir, const string& name);*/ +}; + + +#include "oImage.h" diff --git a/WickedEngine/ImageEffects.h b/WickedEngine/ImageEffects.h new file mode 100644 index 000000000..3d473c6c4 --- /dev/null +++ b/WickedEngine/ImageEffects.h @@ -0,0 +1,135 @@ +#pragma once +#include "WickedEngine.h" + +enum BLENDMODE{ + BLENDMODE_OPAQUE, + BLENDMODE_ALPHA, + BLENDMODE_ADDITIVE, + BLENDMODE_MAX, +}; +enum SAMPLEMODE{ + SAMPLEMODE_CLAMP, + SAMPLEMODE_WRAP, + SAMPLEMODE_MIRROR, +}; +enum QUALITY{ + QUALITY_NEAREST, + QUALITY_BILINEAR, + QUALITY_ANISOTROPIC, +}; +enum ImageType{ + SCREEN, + WORLD, +}; +enum Pivot{ + UPPERLEFT, + CENTER, +}; + +class ImageEffects{ +public: + XMFLOAT3 pos; + XMFLOAT2 siz; + XMFLOAT2 scale; + XMFLOAT4 drawRec; + XMFLOAT2 texOffset; + XMFLOAT2 offset; + XMFLOAT2 sunPos; + XMFLOAT4 lookAt; //.w is for enabled + float mirror; + float blur,blurDir; + float fade; + float opacity; + float rotation; + bool extractNormalMap; + + BLENDMODE blendFlag; + ImageType typeFlag; + SAMPLEMODE sampleFlag; + QUALITY quality; + Pivot pivotFlag; + + struct Bloom{ + bool separate; + float threshold; + float saturation; + + void clear(){separate=false;threshold=saturation=0;} + Bloom(){clear();} + }; + Bloom bloom; + struct Processing{ + bool active; + bool motionBlur; + bool outline; + float dofStrength; + bool fxaa; + bool ssao; + XMFLOAT2 ssss; + bool linDepth; + bool colorGrade; + + void clear(){active=motionBlur=outline=fxaa=ssao=linDepth=colorGrade=false;dofStrength=0;ssss=XMFLOAT2(0,0);} + void setDOF(float value){dofStrength=value;active=value;} + void setMotionBlur(bool value){motionBlur=value;active=value;} + void setOutline(bool value){outline=value;active=value;} + void setFXAA(bool value){fxaa=value;active=value;} + void setSSAO(bool value){ssao=value;active=value;} + void setLinDepth(bool value){linDepth=value;active=value;} + void setColorGrade(bool value){colorGrade=value;active=value;} + //direction*Properties + void setSSSS(const XMFLOAT2& value){ssss=value;active=value.x||value.y;} + Processing(){clear();} + }; + Processing process; + bool deferred; + ID3D11ShaderResourceView *normalMap,*depthMap,*velocityMap,*refractionMap,*maskMap; + void setMaskMap(ID3D11ShaderResourceView*view){maskMap=view;} + void setRefractionMap(ID3D11ShaderResourceView*view){refractionMap=view;} + void setVelocityMap(ID3D11ShaderResourceView*view){velocityMap=view;} + void setDepthMap(ID3D11ShaderResourceView*view){depthMap=view;} + void setNormalMap(ID3D11ShaderResourceView*view){normalMap=view;} + + void init(){ + pos=XMFLOAT3(0,0,0); + siz=XMFLOAT2(1,1); + scale=XMFLOAT2(1,1); + drawRec=XMFLOAT4(0,0,0,0); + texOffset=XMFLOAT2(0,0); + offset=XMFLOAT2(0,0); + sunPos=XMFLOAT2(0,0); + lookAt=XMFLOAT4(0,0,0,0); + mirror=1.0f; + blur=blurDir=fade=opacity=rotation=0.0f; + extractNormalMap=false; + blendFlag=BLENDMODE_ALPHA; + typeFlag=SCREEN; + sampleFlag=SAMPLEMODE_MIRROR; + quality=QUALITY_BILINEAR; + pivotFlag=UPPERLEFT; + bloom=Bloom(); + process=Processing(); + deferred=false; + normalMap=nullptr; + depthMap=nullptr; + velocityMap=nullptr; + refractionMap=nullptr; + maskMap=nullptr; + } + + + ImageEffects(){ + init(); + } + ImageEffects(float width, float height){ + init(); + siz=XMFLOAT2(width,height); + } + ImageEffects(float posX, float posY, float width, float height){ + init(); + pos.x=posX; + pos.y=posY; + siz=XMFLOAT2(width,height); + } +}; + diff --git a/WickedEngine/InputManager.cpp b/WickedEngine/InputManager.cpp new file mode 100644 index 000000000..53800d354 --- /dev/null +++ b/WickedEngine/InputManager.cpp @@ -0,0 +1,154 @@ +#include "InputManager.h" + +XInput* InputManager::xinput=nullptr; +DirectInput* InputManager::dinput=nullptr; +InputManager::InputCollection InputManager::inputs; + +void InputManager::addXInput(XInput* input){ + xinput=input; +} +void InputManager::addDirectInput(DirectInput* input){ + dinput=input; +} + +void InputManager::CleanUp(){ + xinput->CleanUp(); + xinput=nullptr; + + dinput->Shutdown(); + dinput=nullptr; +} + +void InputManager::Update(){ + if(dinput){ + dinput->Frame(); + } + if(xinput){ + xinput->UpdateControllerState(); + } + + for(InputCollection::iterator iter=inputs.begin();iter!=inputs.end();){ + InputType type = iter->first.type; + DWORD button = iter->first.button; + short playerIndex = iter->first.playerIndex; + + bool todelete = false; + + switch (type) + { + case InputManager::DIRECTINPUT_JOYPAD: + if(dinput!=nullptr && dinput->isButtonDown(playerIndex,button)){ + iter->second++; + } + else{ + todelete=true; + } + break; + case InputManager::XINPUT_JOYPAD: + if(xinput!=nullptr && xinput->isButtonDown(playerIndex,button)){ + iter->second++; + } + else{ + todelete=true; + } + break; + case InputManager::KEYBOARD: + if(dinput!=nullptr && dinput->IsKeyDown(button)){ + iter->second++; + } + else{ + todelete=true; + } + break; + default: + break; + } + + if(todelete){ + inputs.erase(iter++); + } + else{ + ++iter; + } + } +} + +bool InputManager::press(DWORD button){ + return press(button,0,InputType::KEYBOARD); +} +bool InputManager::press(DWORD button, short playerIndex, InputType inputType){ + + switch (inputType) + { + case InputManager::DIRECTINPUT_JOYPAD: + if(dinput!=nullptr && !dinput->isButtonDown(playerIndex,button)){ + return false; + } + break; + case InputManager::XINPUT_JOYPAD: + if(xinput!=nullptr && !xinput->isButtonDown(playerIndex,button)){ + return false; + } + break; + case InputManager::KEYBOARD: + if(dinput!=nullptr && !dinput->IsKeyDown(button)){ + return false; + } + break; + default: + return false; + break; + } + + Input input = Input(); + input.button=button; + input.type=inputType; + input.playerIndex=playerIndex; + InputCollection::iterator iter = inputs.find(input); + if(iter==inputs.end()){ + inputs.insert(InputCollection::value_type(input,0)); + return true; + } + return false; +} +bool InputManager::hold(DWORD button, DWORD frames){ + return hold(button,frames,0,InputType::KEYBOARD); +} +bool InputManager::hold(DWORD button, DWORD frames, short playerIndex, InputType inputType){ + + switch (inputType) + { + case InputManager::DIRECTINPUT_JOYPAD: + if(dinput!=nullptr && !dinput->isButtonDown(playerIndex,button)){ + return false; + } + break; + case InputManager::XINPUT_JOYPAD: + if(xinput!=nullptr && !xinput->isButtonDown(playerIndex,button)){ + return false; + } + break; + case InputManager::KEYBOARD: + if(dinput!=nullptr && !dinput->IsKeyDown(button)){ + return false; + } + break; + default: + return false; + break; + } + + Input input = Input(); + input.button=button; + input.type=inputType; + input.playerIndex=playerIndex; + InputCollection::iterator iter = inputs.find(input); + if(iter==inputs.end()){ + inputs.insert(pair(input,0)); + return false; + } + else if(iter->second==frames){ + return true; + } + return false; +} \ No newline at end of file diff --git a/WickedEngine/InputManager.h b/WickedEngine/InputManager.h new file mode 100644 index 000000000..2dbc72d96 --- /dev/null +++ b/WickedEngine/InputManager.h @@ -0,0 +1,52 @@ +#pragma once +#include "WickedEngine.h" + +class XInput; +class DirectInput; + +class InputManager +{ +public: + static XInput* xinput; + static DirectInput* dinput; + + static void addXInput(XInput* input); + static void addDirectInput(DirectInput* input); + + static void Update(); + static void CleanUp(); + + enum InputType{ + DIRECTINPUT_JOYPAD, + XINPUT_JOYPAD, + KEYBOARD, + }; + struct Input{ + InputType type; + DWORD button; + short playerIndex; + + Input(){ + type=InputType::DIRECTINPUT_JOYPAD; + button=(DWORD)0; + playerIndex=0; + } + bool operator<(const Input other){ + return (button!=other.button || type!=other.type || playerIndex!=other.playerIndex); + } + struct LessComparer{ + bool operator()(Input const& a,Input const& b) const{ + return (a.button InputCollection; + static InputCollection inputs; + + + static bool press(DWORD button); + static bool press(DWORD button, short playerIndex, InputType inputType = InputType::DIRECTINPUT_JOYPAD); + static bool hold(DWORD button, DWORD frames); + static bool hold(DWORD button, DWORD frames, short playerIndex, InputType inputType = InputType::DIRECTINPUT_JOYPAD); +}; + diff --git a/WickedEngine/LensFlare.cpp b/WickedEngine/LensFlare.cpp new file mode 100644 index 000000000..b3687f69b --- /dev/null +++ b/WickedEngine/LensFlare.cpp @@ -0,0 +1,227 @@ +#include "LensFlare.h" + +extern Camera* cam; + +ID3D11Buffer* LensFlare::constantBuffer; +ID3D11PixelShader* LensFlare::pixelShader; +ID3D11GeometryShader* LensFlare::geometryShader; +ID3D11VertexShader* LensFlare::vertexShader; +ID3D11InputLayout* LensFlare::inputLayout; +ID3D11SamplerState* LensFlare::samplerState; +ID3D11RasterizerState* LensFlare::rasterizerState; +ID3D11DepthStencilState* LensFlare::depthStencilState; +ID3D11BlendState* LensFlare::blendState; +float LensFlare::RENDERWIDTH, LensFlare::RENDERHEIGHT; + +void LensFlare::Initialize(float width, float height){ + LoadShaders(); + SetUpCB(); + SetUpStates(); + RENDERWIDTH=width; + RENDERHEIGHT=height; + // + //CreateWICTextureFromFile(0,Renderer::graphicsDevice,0,L"images/flare0.jpg",0,&textures[0],0); + //CreateWICTextureFromFile(0,Renderer::graphicsDevice,0,L"images/flare1.jpg",0,&textures[1],0); + //CreateWICTextureFromFile(0,Renderer::graphicsDevice,0,L"images/flare2.jpg",0,&textures[2],0); + //CreateWICTextureFromFile(0,Renderer::graphicsDevice,0,L"images/flare3.jpg",0,&textures[3],0); + //CreateWICTextureFromFile(0,Renderer::graphicsDevice,0,L"images/flare4.jpg",0,&textures[4],0); + //CreateWICTextureFromFile(0,Renderer::graphicsDevice,0,L"images/flare5.jpg",0,&textures[5],0); + //CreateWICTextureFromFile(0,Renderer::graphicsDevice,0,L"images/flare6.jpg",0,&textures[6],0); +} +void LensFlare::CleanUp(){ + if(vertexShader) vertexShader->Release(); vertexShader = NULL; + if(pixelShader) pixelShader->Release(); pixelShader = NULL; + if(geometryShader) geometryShader->Release(); geometryShader = NULL; + if(inputLayout) inputLayout->Release(); inputLayout = NULL; + + if(constantBuffer) constantBuffer->Release(); constantBuffer = NULL; + + if(samplerState) samplerState->Release(); samplerState = NULL; + if(rasterizerState) rasterizerState->Release(); rasterizerState = NULL; + if(blendState) blendState->Release(); blendState = NULL; + if(depthStencilState) depthStencilState->Release(); depthStencilState = NULL; +} +void LensFlare::Draw(ID3D11ShaderResourceView* depthMap, ID3D11DeviceContext* context, const XMVECTOR& lightPos + , vector& rims){ + + if(!rims.empty()){ + + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_POINTLIST ); + Renderer::BindPrimitiveTopology(Renderer::POINTLIST,context); + Renderer::BindVertexLayout(inputLayout,context); + //context->IASetInputLayout( inputLayout ); + //context->VSSetShader( vertexShader, NULL, 0 ); + //context->GSSetShader( geometryShader, NULL, 0 ); + Renderer::BindPS(pixelShader,context); + Renderer::BindVS(vertexShader,context); + Renderer::BindGS(geometryShader,context); + + ConstantBuffer cb; + cb.mSunPos = lightPos/XMVectorSet(RENDERWIDTH,RENDERHEIGHT,1,1); + cb.mScreen = XMFLOAT4(RENDERWIDTH,RENDERHEIGHT,0,0); + + Renderer::UpdateBuffer(constantBuffer,&cb,context); + + + //context->RSSetState(rasterizerState); + //context->OMSetDepthStencilState(depthStencilState, 1); + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + + //context->GSSetConstantBuffers( 0, 1, &constantBuffer ); + Renderer::BindRasterizerState(rasterizerState,context); + Renderer::BindDepthStencilState(depthStencilState,1,context); + Renderer::BindBlendState(blendState,context); + Renderer::BindConstantBufferGS(constantBuffer,0,context); + + //context->GSSetShaderResources( 0,1,&depthMap ); + Renderer::BindTextureGS(depthMap,0,context); + //context->GSSetSamplers(0, 1, &samplerState); + //context->PSSetSamplers(0, 1, &samplerState); + Renderer::BindSamplerPS(Renderer::ssClampLin,0,context); + Renderer::BindSamplerGS(samplerState,0,context); + + int i=0; + for(Renderer::TextureView x : rims){ + if(x!=nullptr){ + Renderer::BindTexturePS(x,i+1,context); + Renderer::BindTextureGS(x,i+1,context); + i++; + } + } + //context->Draw(i,0); + Renderer::Draw(i,context); + + + + + //context->GSSetShader(0,0,0); + Renderer::BindGS(nullptr,context); + //context->GSSetConstantBuffers(0,0,0); + Renderer::BindConstantBufferGS(nullptr,0,context); + } +} + +void LensFlare::LoadShaders(){ + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/lensFlareVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load lensFlareVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vertexShader ); + + // Define the input layout + D3D11_INPUT_ELEMENT_DESC layout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + }; + UINT numElements = ARRAYSIZE( layout ); + + // Create the input layout + Renderer::graphicsDevice->CreateInputLayout( layout, numElements, pVSBlob->GetBufferPointer(), + pVSBlob->GetBufferSize(), &inputLayout ); + + if(pVSBlob){ pVSBlob->Release();pVSBlob=NULL; } + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/lensFlarePS.cso", &pPSBlob))){MessageBox(0,L"Failed To load lensFlarePS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &pixelShader ); + if(pPSBlob){ pPSBlob->Release();pPSBlob=NULL; } + + + ID3DBlob* pGSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/lensFlareGS.cso", &pGSBlob))){MessageBox(0,L"Failed To load lensFlareGS.cso",0,0);} + Renderer::graphicsDevice->CreateGeometryShader( pGSBlob->GetBufferPointer(), pGSBlob->GetBufferSize(), NULL, &geometryShader ); + if(pGSBlob){ pGSBlob->Release();pGSBlob=NULL; } +} +void LensFlare::SetUpCB() +{ + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ConstantBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &constantBuffer ); + + +} +void LensFlare::SetUpStates() +{ + D3D11_SAMPLER_DESC samplerDesc; + ZeroMemory( &samplerDesc, sizeof(D3D11_SAMPLER_DESC) ); + samplerDesc.Filter = D3D11_FILTER_COMPARISON_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 16; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_LESS_EQUAL; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + Renderer::graphicsDevice->CreateSamplerState(&samplerDesc, &samplerState); + + + + + D3D11_RASTERIZER_DESC rs; + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_NONE; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=false; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + Renderer::graphicsDevice->CreateRasterizerState(&rs,&rasterizerState); + + + + + + D3D11_DEPTH_STENCIL_DESC dsd; + dsd.DepthEnable = false; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ZERO; + dsd.DepthFunc = D3D11_COMPARISON_LESS; + + dsd.StencilEnable = false; + dsd.StencilReadMask = 0xFF; + dsd.StencilWriteMask = 0xFF; + + // Stencil operations if pixel is front-facing. + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_INCR; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + + // Stencil operations if pixel is back-facing. + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_DECR; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + + // Create the depth stencil state. + Renderer::graphicsDevice->CreateDepthStencilState(&dsd, &depthStencilState); + + + + D3D11_BLEND_DESC bd; + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + Renderer::graphicsDevice->CreateBlendState(&bd,&blendState); +} diff --git a/WickedEngine/LensFlare.h b/WickedEngine/LensFlare.h new file mode 100644 index 000000000..590aca062 --- /dev/null +++ b/WickedEngine/LensFlare.h @@ -0,0 +1,35 @@ +#ifndef LENSFLARE +#define LENSFLARE +#include "WickedEngine.h" + +static class LensFlare +{ +private: + static ID3D11Buffer* constantBuffer; + static ID3D11PixelShader* pixelShader; + static ID3D11GeometryShader* geometryShader; + static ID3D11VertexShader* vertexShader; + static ID3D11InputLayout* inputLayout; + static ID3D11SamplerState* samplerState; + static ID3D11RasterizerState* rasterizerState; + static ID3D11DepthStencilState* depthStencilState; + static ID3D11BlendState* blendState; + static float RENDERWIDTH, RENDERHEIGHT; + + struct ConstantBuffer + { + XMVECTOR mSunPos; + XMFLOAT4 mScreen; + }; + + static void LoadShaders(); + static void SetUpStates(); + static void SetUpCB(); +public: + static void Initialize(float width, float height); + static void CleanUp(); + static void Draw(ID3D11ShaderResourceView* depthMap, ID3D11DeviceContext* context, const XMVECTOR& lightPos + , vector& rims); +}; + +#endif diff --git a/WickedEngine/Level.cpp b/WickedEngine/Level.cpp new file mode 100644 index 000000000..275ac333f --- /dev/null +++ b/WickedEngine/Level.cpp @@ -0,0 +1,68 @@ +#include "Renderer.h" + +extern Camera* cam; + + +extern FLOAT shBias; +extern FLOAT BlackOut, BlackWhite, InvertCol; + + +WorldInfo Renderer::worldInfo; +Wind Renderer::wind; + +Level::Level(string name) +{ + identifier="_L"; + + Renderer::Renderer(); + wind=Wind(); + + stringstream ss(""); + ss<<"levels/"<CleanUp(); + spTree_lights=nullptr; + + delete(this); +} diff --git a/WickedEngine/Level.h b/WickedEngine/Level.h new file mode 100644 index 000000000..8f35f0793 --- /dev/null +++ b/WickedEngine/Level.h @@ -0,0 +1,13 @@ +#pragma once +#include "Hitbox2D.h" + +class Level:public Entity{ +public: + Level(string name); + void CleanUp(); + + void* operator new(size_t); + void operator delete(void*); + + Hitbox2D playArea; +}; diff --git a/WickedEngine/Lines.cpp b/WickedEngine/Lines.cpp new file mode 100644 index 000000000..aeb804959 --- /dev/null +++ b/WickedEngine/Lines.cpp @@ -0,0 +1,88 @@ +#include "Lines.h" + + +Lines::Lines() +{ + desc = Description(); + desc.length=1; + parentArmature=0; + parentBone=0; + SetUpVertices(); +} + +Lines::Lines(float newLen, const XMFLOAT4A& newColor, int newParentArmature, int newParentBone) +{ + desc = Description(); + desc.length = newLen; + desc.color = newColor; + parentArmature = newParentArmature; + parentBone = newParentBone; + SetUpVertices(); +} + +Lines::Lines(const XMFLOAT3& a, const XMFLOAT3& b, const XMFLOAT4A& c) +{ + desc = Description(); + desc.length = 0; + desc.color = c; + parentArmature = 0; + parentBone = 0; + + Vertex* verts = new Vertex[2]; + + verts[0].pos = XMFLOAT3A(a.x,a.y,a.z); + verts[1].pos = XMFLOAT3A(b.x,b.y,b.z); + + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DEFAULT; + bd.ByteWidth = sizeof( Vertex ) * 2; + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = 0; + D3D11_SUBRESOURCE_DATA InitData; + ZeroMemory( &InitData, sizeof(InitData) ); + InitData.pSysMem = verts; + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &vertexBuffer ); + + if(verts){ + delete[](verts); + verts=NULL; + } +} + +void Lines::CleanUp() +{ + if(vertexBuffer){ + vertexBuffer->Release(); + vertexBuffer = NULL; + } +} + +void Lines::SetUpVertices() +{ + Vertex* verts = new Vertex[2]; + + verts[0].pos = XMFLOAT3A(0,0,0); + verts[1].pos = XMFLOAT3A(0,0,desc.length); + + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DEFAULT; + bd.ByteWidth = sizeof( Vertex ) * 2; + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = 0; + D3D11_SUBRESOURCE_DATA InitData; + ZeroMemory( &InitData, sizeof(InitData) ); + InitData.pSysMem = verts; + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &vertexBuffer ); + + if(verts){ + delete[](verts); + verts=NULL; + } +} + +void Lines::Transform(const XMFLOAT4X4& mat) +{ + desc.transform=mat; +} \ No newline at end of file diff --git a/WickedEngine/Lines.h b/WickedEngine/Lines.h new file mode 100644 index 000000000..ac3d3b812 --- /dev/null +++ b/WickedEngine/Lines.h @@ -0,0 +1,46 @@ +#pragma once +#include "WickedEngine.h" + + +class Lines +{ +private: + struct Description{ + XMFLOAT4X4 transform; + float length; + XMFLOAT4A color; + + Description(){ + length=0; + color=XMFLOAT4A(1,1,1,1); + transform=XMFLOAT4X4( + 1,0,0,0, + 0,1,0,0, + 0,0,1,0, + 0,0,0,1 + ); + }; + }; + struct Vertex + { + XMFLOAT3A pos; + + Vertex(){ pos=XMFLOAT3A(0,0,0); } + Vertex(const XMFLOAT3A& newPos){ pos=newPos; } + }; + void SetUpVertices(); +public: + Lines(); + Lines(float newLen, const XMFLOAT4A& newColor, int newParentArmature, int newParentBone); + Lines(const XMFLOAT3& a, const XMFLOAT3& b, const XMFLOAT4A& c); + void CleanUp(); + void* operator new(size_t size){ void* result = _aligned_malloc(size,16); return result; } + void operator delete(void* p){ if(p) _aligned_free(p); } + void Transform(const XMFLOAT4X4& mat); + + Description desc; + int parentArmature,parentBone; + + ID3D11Buffer* vertexBuffer; +}; + diff --git a/WickedEngine/Network.cpp b/WickedEngine/Network.cpp new file mode 100644 index 000000000..578b05e2f --- /dev/null +++ b/WickedEngine/Network.cpp @@ -0,0 +1,56 @@ +#include "Network.h" + + +Network::Network(void) +{ + name="UNNAMED_NETWORK"; +} + + +Network::~Network(void) +{ + CloseConnection(); +} + + +bool Network::sendText(const std::string& text, SOCKET socket){ + if(sendData((int)text.length(),socket)){ + int sent = send(socket, text.c_str(), text.length(), 0); + + if(sent==SOCKET_ERROR) { + //cout << "\n[Error] Sending text '"< +#include +#include +#include + +class Network +{ +protected: + +#define SCK_VERSION2 0x0202 + + SOCKET s; + WSADATA w; + fd_set readfds; + + std::string name; + +public: + + static const int PORT = 65000; + + static const int PACKET_TYPE_CHANGENAME = 0; + static const int PACKET_TYPE_TEXTMESSAGE = 1; + static const int PACKET_TYPE_OTHER = 2; + bool success; + + Network(void); + ~Network(void); + + virtual bool changeName(const string& newName){ + name=newName; + return true; + } + + void CloseConnection() + { + if(s) + closesocket(s); + + WSACleanup(); + + + //BackLog::post("\n\nClosed Connection"); + } + + static bool sendText(const std::string& text, SOCKET socket); + template + static bool sendData(const T& value, SOCKET socket){ + int sent = send(socket, (const char*)&value, sizeof(value), 0); + + if(sent==SOCKET_ERROR) { + //stringstream ss(""); + //ss << "\n[Error] Sending data failed with error: " << WSAGetLastError(); + //BackLog::post(ss.str().c_str()); + return false; + } + + return true; + } + + static bool receiveText(std::string& text, SOCKET socket); + template + static bool receiveData(T& value, SOCKET socket){ + char puffer[sizeof(value)]; + int received = recv(socket, puffer, sizeof(puffer), 0); + if(received<=0){ + //cout<< "[Error][receiveNumber] The recv call failed with error!\n"; + } + else if (received <= sizeof(value)) { + value = *((T*)puffer); + return true; + } + else{ + //cout<< "[Error][receiveNumber] Too long request!\n"; + } + return false; + } +}; + +#endif + diff --git a/WickedEngine/PHYSICS.h b/WickedEngine/PHYSICS.h new file mode 100644 index 000000000..3cf355b92 --- /dev/null +++ b/WickedEngine/PHYSICS.h @@ -0,0 +1,63 @@ +#pragma once +#include "WickedEngine.h" + +class PHYSICS +{ +public: + struct Transform{ + XMFLOAT4 rotation; + XMFLOAT3 position; + Transform(){ + rotation=XMFLOAT4(0,0,0,1); + position=XMFLOAT3(0,0,0); + } + Transform(const XMFLOAT4& newRot, const XMFLOAT3& newPos){ + rotation=newRot; + position=newPos; + } + }; + static int softBodyInterationCount; +protected: + vector transforms; + bool firstRunWorld; + int registeredObjects; +public: + void FirstRunWorld(){firstRunWorld=true;} + void NextRunWorld(){firstRunWorld=false;} + int getObjectCount(){return registeredObjects+1;} + + virtual void Update()=0; + virtual void MarkForRead()=0; + virtual void UnMarkForRead()=0; + virtual void MarkForWrite()=0; + virtual void UnMarkForWrite()=0; + virtual void ClearWorld()=0; + virtual void CleanUp()=0; + + virtual void addWind(const XMFLOAT3& wind)=0; + + virtual void addBox(const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass=1, const float& newFriction=1, const float& newRestitution=1, const float& newDamping=1, bool kinematic=false)=0; + virtual void addSphere(const float& rad, const XMFLOAT3& pos + , const float& newMass=1, const float& newFriction=1, const float& newRestitution=1, const float& newDamping=1, bool kinematic=false)=0; + virtual void addCapsule(const float& rad, const float& hei, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass=1, const float& newFriction=1, const float& newRestitution=1, const float& newDamping=1, bool kinematic=false)=0; + virtual void addConvexHull(const vector& vertices, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass=1, const float& newFriction=1, const float& newRestitution=1, const float& newDamping=1, bool kinematic=false)=0; + virtual void addTriangleMesh(const vector& vertices, const vector& indices, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass=1, const float& newFriction=1, const float& newRestitution=1, const float& newDamping=1, bool kinematic=false)=0; + + + virtual void addSoftBodyTriangleMesh(const Mesh* mesh, const XMFLOAT3& sca, const XMFLOAT4& rot, const XMFLOAT3& pos + , const float& newMass=1, const float& newFriction=1, const float& newRestitution=1, const float& newDamping=1)=0; + + virtual void connectVerticesToSoftBody(Mesh* const mesh, int objectI)=0; + virtual void connectSoftBodyToVertices(const Mesh* const mesh, int objectI)=0; + virtual void transformBody(const XMFLOAT4& rot, const XMFLOAT3& pos, int objectI)=0; + + virtual Transform* getObject(int index)=0; + + virtual void registerObject(Object* object)=0; +}; +//#include "HAVOK.h" +#include "BULLET.h" \ No newline at end of file diff --git a/WickedEngine/Particle.cpp b/WickedEngine/Particle.cpp new file mode 100644 index 000000000..4fd50eefe --- /dev/null +++ b/WickedEngine/Particle.cpp @@ -0,0 +1,5 @@ +#include "Particle.h" + +extern Camera* cam; + +bool Particle::wireRender=false; diff --git a/WickedEngine/Particle.h b/WickedEngine/Particle.h new file mode 100644 index 000000000..61225e8b6 --- /dev/null +++ b/WickedEngine/Particle.h @@ -0,0 +1,23 @@ +#pragma once +#include "WickedEngine.h" +#define MAX_PARTICLES 1000 + + +struct SkinnedVertex; +struct Mesh; +struct Object; + +class Particle +{ +protected: + Particle(){}; + +public: + + static bool wireRender; + static void ChangeRasterizer(){wireRender=!wireRender;} +}; + +#include "EmittedParticle.h" +#include "HairParticle.h" + diff --git a/WickedEngine/RenderTarget.cpp b/WickedEngine/RenderTarget.cpp new file mode 100644 index 000000000..f0677aa55 --- /dev/null +++ b/WickedEngine/RenderTarget.cpp @@ -0,0 +1,205 @@ +#include "RenderTarget.h" + + +RenderTarget::RenderTarget() +{ + numViews = 0; + viewPort = D3D11_VIEWPORT(); + depth = NULL; + retargetted=false; +} + + +RenderTarget::~RenderTarget() +{ + for(int i=0;iRelease(); texture2D[i] = NULL; + if(renderTarget[i]) renderTarget[i]->Release(); renderTarget[i] = NULL; + if(SAVEDshaderResource[i]) SAVEDshaderResource[i]->Release(); SAVEDshaderResource[i]=NULL; + if(shaderResource[i] && !retargetted) shaderResource[i]->Release(); shaderResource[i] = NULL; + } + texture2D.clear(); + renderTarget.clear(); + shaderResource.clear(); + if(depth) depth->~DepthTarget(); depth=NULL; +} + +void RenderTarget::Initialize(int width, int height, int numViews, bool hasDepth, UINT MSAAC, UINT MSAAQ, DXGI_FORMAT format) +{ + this->numViews = numViews; + texture2D.resize(numViews); + renderTarget.resize(numViews); + shaderResource.resize(numViews); + SAVEDshaderResource.resize(numViews); + + D3D11_TEXTURE2D_DESC textureDesc; + ZeroMemory(&textureDesc, sizeof(textureDesc)); + textureDesc.Width = width; + textureDesc.Height = height; + textureDesc.MipLevels = 1; + textureDesc.ArraySize = 1; + textureDesc.Format = format; + textureDesc.SampleDesc.Count = MSAAC; + textureDesc.SampleDesc.Quality = MSAAQ; + textureDesc.Usage = D3D11_USAGE_DEFAULT; + textureDesc.BindFlags = D3D11_BIND_RENDER_TARGET | D3D11_BIND_SHADER_RESOURCE; + textureDesc.CPUAccessFlags = 0; + textureDesc.MiscFlags = 0; + + + D3D11_RENDER_TARGET_VIEW_DESC renderTargetViewDesc; + renderTargetViewDesc.Format = format; + renderTargetViewDesc.ViewDimension = (MSAAQ==0 ? D3D11_RTV_DIMENSION_TEXTURE2D : D3D11_RTV_DIMENSION_TEXTURE2DMS); + renderTargetViewDesc.Texture2D.MipSlice = 0; + + + D3D11_SHADER_RESOURCE_VIEW_DESC shaderResourceViewDesc; + shaderResourceViewDesc.Format = format; + shaderResourceViewDesc.ViewDimension = (MSAAQ==0 ? D3D11_SRV_DIMENSION_TEXTURE2D : D3D11_SRV_DIMENSION_TEXTURE2DMS); + shaderResourceViewDesc.Texture2D.MostDetailedMip = 0; + shaderResourceViewDesc.Texture2D.MipLevels = 1; + + for(int i=0;iCreateTexture2D(&textureDesc, NULL, &texture2D[i]); + Renderer::graphicsDevice->CreateRenderTargetView(texture2D[i], &renderTargetViewDesc, &renderTarget[i]); + Renderer::graphicsDevice->CreateShaderResourceView(texture2D[i], &shaderResourceViewDesc, &shaderResource[i]); + } + + viewPort.Width = (FLOAT)width; + viewPort.Height = (FLOAT)height; + viewPort.MinDepth = 0.0f; + viewPort.MaxDepth = 1.0f; + viewPort.TopLeftX = 0; + viewPort.TopLeftY = 0; + + if(hasDepth) { + depth = new DepthTarget(); + depth->Initialize(width,height,MSAAC,MSAAQ); + } +} +void RenderTarget::Initialize(int width, int height, int numViews, bool hasDepth) +{ + Initialize(width,height,numViews,hasDepth,1,0,DXGI_FORMAT_R8G8B8A8_UNORM); +} +void RenderTarget::InitializeCube(int size, int numViews, bool hasDepth, DXGI_FORMAT format) +{ + this->numViews = numViews; + texture2D.resize(numViews); + renderTarget.resize(numViews); + shaderResource.resize(numViews); + SAVEDshaderResource.resize(numViews); + + D3D11_TEXTURE2D_DESC textureDesc; + ZeroMemory(&textureDesc, sizeof(textureDesc)); + textureDesc.Width = size; + textureDesc.Height = size; + textureDesc.MipLevels = 1; + textureDesc.ArraySize = 6; + textureDesc.Format = format; + textureDesc.SampleDesc.Count = 1; + textureDesc.SampleDesc.Quality = 0; + textureDesc.Usage = D3D11_USAGE_DEFAULT; + textureDesc.BindFlags = D3D11_BIND_RENDER_TARGET | D3D11_BIND_SHADER_RESOURCE; + textureDesc.CPUAccessFlags = 0; + textureDesc.MiscFlags = D3D11_RESOURCE_MISC_TEXTURECUBE; + + + D3D11_RENDER_TARGET_VIEW_DESC renderTargetViewDesc; + renderTargetViewDesc.Format = format; + renderTargetViewDesc.ViewDimension = D3D11_RTV_DIMENSION_TEXTURE2DARRAY; + renderTargetViewDesc.Texture2DArray.FirstArraySlice = 0; + renderTargetViewDesc.Texture2DArray.ArraySize = 6; + renderTargetViewDesc.Texture2DArray.MipSlice = 0; + + + D3D11_SHADER_RESOURCE_VIEW_DESC shaderResourceViewDesc; + shaderResourceViewDesc.Format = format; + shaderResourceViewDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURECUBE; + shaderResourceViewDesc.TextureCube.MostDetailedMip = 0; + shaderResourceViewDesc.TextureCube.MipLevels = 1; + + for(int i=0;iCreateTexture2D(&textureDesc, NULL, &texture2D[i]); + Renderer::graphicsDevice->CreateRenderTargetView(texture2D[i], &renderTargetViewDesc, &renderTarget[i]); + Renderer::graphicsDevice->CreateShaderResourceView(texture2D[i], &shaderResourceViewDesc, &shaderResource[0]); + } + + viewPort.Width = (FLOAT)size; + viewPort.Height = (FLOAT)size; + viewPort.MinDepth = 0.0f; + viewPort.MaxDepth = 1.0f; + viewPort.TopLeftX = 0; + viewPort.TopLeftY = 0; + + if(hasDepth) { + depth = new DepthTarget(); + depth->InitializeCube(size); + } +} +void RenderTarget::InitializeCube(int size, int numViews, bool hasDepth) +{ + InitializeCube(size,numViews,hasDepth,DXGI_FORMAT_R8G8B8A8_UNORM); +} + +void RenderTarget::Activate(ID3D11DeviceContext* context) +{ + Activate(context,0,0,0,0); +} +void RenderTarget::Activate(ID3D11DeviceContext* context, float r, float g, float b, float a) +{ + if(context!=nullptr){ + Set(context); + float ClearColor[4] = { r, g, b, a }; // red,green,blue,alpha + for(int i=0;iClearRenderTargetView(renderTarget[i], ClearColor); + if(depth) depth->Clear(context); + } +} +void RenderTarget::Activate(ID3D11DeviceContext* context, DepthTarget* getDepth, float r, float g, float b, float a) +{ + if(context!=nullptr){ + Set(context,getDepth); + float ClearColor[4] = { r, g, b, a }; // red,green,blue,alpha + for(int i=0;iClearRenderTargetView(renderTarget[i], ClearColor); + } +} +void RenderTarget::Activate(ID3D11DeviceContext* context, DepthTarget* getDepth) +{ + Activate(context,getDepth,0,0,0,0); +} +void RenderTarget::Set(ID3D11DeviceContext* context) +{ + //ID3D11ShaderResourceView* t[]={nullptr}; + //context->PSSetShaderResources(6,1,t); + if(context!=nullptr){ + context->RSSetViewports(1, &viewPort); + context->OMSetRenderTargets(numViews, renderTarget.data(),(depth?depth->depthTarget:nullptr)); + } +} +void RenderTarget::Set(ID3D11DeviceContext* context, DepthTarget* getDepth) +{ + //ID3D11ShaderResourceView* t[]={nullptr}; + //context->PSSetShaderResources(6,1,t); + if(context!=nullptr){ + depth = getDepth; + context->RSSetViewports(1, &viewPort); + context->OMSetRenderTargets(numViews, renderTarget.data(),(depth?depth->depthTarget:nullptr)); + } +} +void RenderTarget::Retarget(ID3D11ShaderResourceView* resource) +{ + retargetted=true; + for(int i=0;i SAVEDshaderResource; +public: + D3D11_VIEWPORT viewPort; + vector texture2D; + vector renderTarget; + vector shaderResource; + DepthTarget* depth; + + RenderTarget(); + ~RenderTarget(); + + void Initialize(int width, int height, int numViews, bool hasDepth, UINT MSAAC, UINT MSAAQ, DXGI_FORMAT format); + void Initialize(int width, int height, int numViews, bool hasDepth); + void InitializeCube(int size, int numViews, bool hasDepth, DXGI_FORMAT format); + void InitializeCube(int size, int numViews, bool hasDepth); + void Activate(ID3D11DeviceContext*); + void Activate(ID3D11DeviceContext* context, float r, float g, float b, float a); + void Activate(ID3D11DeviceContext* context, DepthTarget*, float r, float g, float b, float a); + void Activate(ID3D11DeviceContext* context, DepthTarget*); + void Set(ID3D11DeviceContext* context); + void Set(ID3D11DeviceContext* context, DepthTarget*); + void Retarget(ID3D11ShaderResourceView* resource); + void Restore(); +}; + diff --git a/WickedEngine/Renderer.cpp b/WickedEngine/Renderer.cpp new file mode 100644 index 000000000..c8542f60f --- /dev/null +++ b/WickedEngine/Renderer.cpp @@ -0,0 +1,4282 @@ +#include "Renderer.h" +#include "skinningDEF.h" + +extern Camera* cam; + +#pragma region STATICS +D3D_DRIVER_TYPE Renderer::driverType; +D3D_FEATURE_LEVEL Renderer::featureLevel; +Renderer::SwapChain Renderer::swapChain; +Renderer::RenderTargetView Renderer::renderTargetView; +Renderer::ViewPort Renderer::viewPort; +Renderer::GraphicsDevice Renderer::graphicsDevice; +Renderer::DeviceContext Renderer::immediateContext; +bool Renderer::DX11 = false,Renderer::VSYNC=true; +Renderer::DeviceContext Renderer::deferredContexts[]; +Renderer::CommandList Renderer::commandLists[]; +mutex Renderer::graphicsMutex; +Renderer::Sampler Renderer::ssClampLin,Renderer::ssClampPoi,Renderer::ssMirrorLin,Renderer::ssMirrorPoi,Renderer::ssWrapLin,Renderer::ssWrapPoi + ,Renderer::ssClampAni,Renderer::ssWrapAni,Renderer::ssMirrorAni,Renderer::ssComp; + +map Renderer::drawCalls; + +int Renderer::RENDERWIDTH,Renderer::RENDERHEIGHT,Renderer::SCREENWIDTH,Renderer::SCREENHEIGHT,Renderer::SHADOWMAPRES,Renderer::SOFTSHADOW + ,Renderer::POINTLIGHTSHADOW=1,Renderer::POINTLIGHTSHADOWRES=256; +bool Renderer::HAIRPARTICLEENABLED,Renderer::EMITTERSENABLED; +bool Renderer::wireRender,Renderer::debugSpheres,Renderer::debugLines,Renderer::debugBoxes; +ID3D11BlendState *Renderer::blendState, *Renderer::blendStateTransparent, *Renderer::blendStateAdd; +ID3D11Buffer *Renderer::constantBuffer, *Renderer::staticCb, *Renderer::shCb, *Renderer::pixelCB, *Renderer::matCb + , *Renderer::lightCb[3], *Renderer::tessBuf, *Renderer::lineBuffer, *Renderer::skyCb + , *Renderer::trailCB, *Renderer::lightStaticCb, *Renderer::vLightCb,*Renderer::cubeShCb,*Renderer::fxCb + , *Renderer::decalCbVS, *Renderer::decalCbPS, *Renderer::viewPropCB; +ID3D11VertexShader *Renderer::vertexShader10, *Renderer::vertexShader, *Renderer::shVS, *Renderer::lineVS, *Renderer::trailVS + ,*Renderer::waterVS,*Renderer::lightVS[3],*Renderer::vSpotLightVS,*Renderer::vPointLightVS,*Renderer::cubeShVS,*Renderer::sOVS,*Renderer::decalVS; +ID3D11PixelShader *Renderer::pixelShader, *Renderer::shPS, *Renderer::linePS, *Renderer::trailPS, *Renderer::simplestPS,*Renderer::blackoutPS + ,*Renderer::textureonlyPS,*Renderer::waterPS,*Renderer::transparentPS,*Renderer::lightPS[3],*Renderer::vLightPS,*Renderer::cubeShPS + ,*Renderer::decalPS; +ID3D11GeometryShader *Renderer::cubeShGS,*Renderer::sOGS; +ID3D11HullShader *Renderer::hullShader; +ID3D11DomainShader *Renderer::domainShader; +ID3D11InputLayout *Renderer::vertexLayout, *Renderer::lineIL,*Renderer::trailIL, *Renderer::sOIL; +ID3D11SamplerState *Renderer::texSampler, *Renderer::mapSampler, *Renderer::comparisonSampler, *Renderer::mirSampler,*Renderer::pointSampler; +ID3D11RasterizerState *Renderer::rasterizerState, *Renderer::rssh, *Renderer::nonCullRSsh, *Renderer::wireRS, *Renderer::nonCullRS,*Renderer::nonCullWireRS + ,*Renderer::backFaceRS; +ID3D11DepthStencilState *Renderer::depthStencilState,*Renderer::xRayStencilState,*Renderer::depthReadStencilState,*Renderer::stencilReadState + ,*Renderer::stencilReadMatch; +ID3D11PixelShader *Renderer::skyPS; +ID3D11VertexShader *Renderer::skyVS; +ID3D11SamplerState *Renderer::skySampler; +Renderer::TextureView Renderer::noiseTex,Renderer::trailDistortTex,Renderer::enviroMap,Renderer::colorGrading; +float Renderer::GameSpeed=1,Renderer::overrideGameSpeed=1; +int Renderer::visibleCount; +float Renderer::shBias; +RenderTarget Renderer::normalMapRT,Renderer::imagesRT,Renderer::imagesRTAdd; + +vector Renderer::textureSlotsPS(D3D11_COMMONSHADER_INPUT_RESOURCE_SLOT_COUNT); +Renderer::PixelShader Renderer::boundPS=nullptr; +#pragma endregion + +#pragma region STATIC TEMP + +int Renderer::vertexCount; +deque Renderer::images; +deque Renderer::waterRipples; + +SPTree* Renderer::spTree; +SPTree* Renderer::spTree_trans; +SPTree* Renderer::spTree_water; +SPTree* Renderer::spTree_lights; + +vector Renderer::everyObject; +vector Renderer::objects; +vector Renderer::objects_trans; +vector Renderer::objects_water; +MeshCollection Renderer::meshes; +MaterialCollection Renderer::materials; +vector Renderer::armatures; +vector Renderer::boneLines; +vector Renderer::cubes; +vector Renderer::lights; +map> Renderer::emitterSystems; +map Renderer::transforms; +list Renderer::decals; + +#pragma endregion + +Renderer::Renderer() +{ +} + + +HRESULT Renderer::InitDevice(HWND window, int screenW, int screenH, bool windowed, short& requestMultiThreading) +{ + HRESULT hr = S_OK; + + UINT createDeviceFlags = 0; +#ifdef _DEBUG + createDeviceFlags |= D3D11_CREATE_DEVICE_DEBUG; +#endif + + D3D_DRIVER_TYPE driverTypes[] = + { + D3D_DRIVER_TYPE_HARDWARE, + D3D_DRIVER_TYPE_WARP, + D3D_DRIVER_TYPE_REFERENCE, + }; + UINT numDriverTypes = ARRAYSIZE( driverTypes ); + + D3D_FEATURE_LEVEL featureLevels[] = + { + D3D_FEATURE_LEVEL_11_1, + D3D_FEATURE_LEVEL_11_0, + D3D_FEATURE_LEVEL_10_1, + //D3D_FEATURE_LEVEL_10_0, + }; + UINT numFeatureLevels = ARRAYSIZE( featureLevels ); + + + DXGI_SWAP_CHAIN_DESC sd; + ZeroMemory( &sd, sizeof( sd ) ); + sd.BufferCount = 2; + sd.BufferDesc.Width = SCREENWIDTH = screenW; + sd.BufferDesc.Height = SCREENHEIGHT = screenH; + sd.BufferDesc.Format = DXGI_FORMAT_B8G8R8A8_UNORM; + sd.BufferDesc.RefreshRate.Numerator = 60; + sd.BufferDesc.RefreshRate.Denominator = 1; + sd.BufferUsage = DXGI_USAGE_RENDER_TARGET_OUTPUT; + sd.OutputWindow = window; + sd.SampleDesc.Count = 1; + sd.SampleDesc.Quality = 0; + sd.Windowed = windowed; + //sd.SwapEffect = DXGI_SWAP_EFFECT_FLIP_SEQUENTIAL; + + for( UINT driverTypeIndex = 0; driverTypeIndex < numDriverTypes; driverTypeIndex++ ) + { + driverType = driverTypes[driverTypeIndex]; + hr = D3D11CreateDeviceAndSwapChain( NULL, driverType, NULL, createDeviceFlags, featureLevels, numFeatureLevels, + D3D11_SDK_VERSION, &sd, &swapChain, &Renderer::graphicsDevice, &featureLevel, &Renderer::immediateContext ); + if( SUCCEEDED( hr ) ) + break; + } + if(FAILED(hr)){ + MessageBox(window,L"SwapChain Creation Failed!",0,0); +#ifdef BACKLOG + BackLog::post("SwapChain Creation Failed!"); +#endif + exit(0); + } + DX11 = ( ( Renderer::graphicsDevice->GetFeatureLevel() >= D3D_FEATURE_LEVEL_11_0 ) ? true:false ); + drawCalls.insert(pair(immediateContext,0)); + + if(requestMultiThreading){ + D3D11_FEATURE_DATA_THREADING threadingFeature; + Renderer::graphicsDevice->CheckFeatureSupport( D3D11_FEATURE_THREADING,&threadingFeature,sizeof(threadingFeature) ); + if(threadingFeature.DriverConcurrentCreates && threadingFeature.DriverCommandLists){ + for(int i=0;iCreateDeferredContext( 0,&deferredContexts[i] ); + drawCalls.insert(pair(deferredContexts[i],0)); + } +#ifdef BACKLOG + stringstream ss(""); + ss<GetBuffer( 0, __uuidof( ID3D11Texture2D ), ( LPVOID* )&pBackBuffer ); + if( FAILED( hr ) ){ + MessageBox(window,L"BackBuffer Failed!",0,0); + exit(0); + } + + hr = Renderer::graphicsDevice->CreateRenderTargetView( pBackBuffer, NULL, &renderTargetView ); + //pBackBuffer->Release(); + if( FAILED( hr ) ){ + MessageBox(window,L"Main Rendertarget Failed!",0,0); + exit(0); + } + + + // Setup the main viewport + viewPort.Width = (FLOAT)SCREENWIDTH; + viewPort.Height = (FLOAT)SCREENHEIGHT; + viewPort.MinDepth = 0.0f; + viewPort.MaxDepth = 1.0f; + viewPort.TopLeftX = 0; + viewPort.TopLeftY = 0; + + + return S_OK; +} +void Renderer::DestroyDevice() +{ + + if( renderTargetView ) + renderTargetView->Release(); + if( swapChain ) + swapChain->Release(); + if( Renderer::immediateContext ) + Renderer::immediateContext->ClearState(); + if( Renderer::immediateContext ) + Renderer::immediateContext->Release(); + if( graphicsDevice ) + graphicsDevice->Release(); + + for(int i=0;iRelease(); commandLists[i]=0;} + if(deferredContexts[i]) {deferredContexts[i]->Release(); deferredContexts[i]=0;} + } +} +void Renderer::Present(function drawToScreen1,function drawToScreen2,function drawToScreen3) +{ + Renderer::graphicsMutex.lock(); + + Renderer::immediateContext->RSSetViewports( 1, &viewPort ); + Renderer::immediateContext->OMSetRenderTargets( 1, &renderTargetView, 0 ); + float ClearColor[4] = { 0, 0, 0, 1.0f }; // red,green,blue,alpha + Renderer::immediateContext->ClearRenderTargetView( renderTargetView, ClearColor ); + //Renderer::immediateContext->ClearDepthStencilView( g_pDepthStencilView, D3D11_CLEAR_DEPTH, 1.0f, 0); + + + if(drawToScreen1!=nullptr) + drawToScreen1(); + if(drawToScreen2!=nullptr) + drawToScreen2(); + if(drawToScreen3!=nullptr) + drawToScreen3(); + + + + //FrameRate::Count(); + FrameRate::Frame(); + + swapChain->Present( VSYNC, 0 ); + for(auto& d : drawCalls){ + d.second=0; + } + + + Renderer::immediateContext->OMSetRenderTargets(NULL, NULL,NULL); + + Renderer::graphicsMutex.unlock(); +} +void Renderer::ReleaseCommandLists() +{ + for(int i=0;iRelease(); commandLists[i]=NULL; } +} +void Renderer::ExecuteDeferredContexts() +{ + for(int i=0;iExecuteCommandList( commandLists[i], false ); + + ReleaseCommandLists(); +} +void Renderer::FinishCommandList(int THREAD) +{ + if(commandLists[THREAD]) { + commandLists[THREAD]->Release(); + commandLists[THREAD]=0; + } + deferredContexts[THREAD]->FinishCommandList( false,&commandLists[THREAD] ); +} + +long Renderer::getDrawCallCount(){ + long retVal=0; + for(auto d:drawCalls){ + retVal+=d.second; + } + return retVal; +} + +void Renderer::CleanUp() +{ + for(int i=0;ia(0); + //MaterialCollection m; + //LoadWiMeshes("lights/","lights.wi","",lightGRenderer,a,m); + + wind=Wind(); + + Cube::LoadStatic(); + HitSphere::SetUpStatic(); + + spTree_lights=nullptr; + + + waterRipples.resize(0); + + + normalMapRT.Initialize( + SCREENWIDTH + ,SCREENHEIGHT + ,1,false,1,0,DXGI_FORMAT_R8G8B8A8_SNORM + ); + imagesRTAdd.Initialize( + SCREENWIDTH + ,SCREENHEIGHT + ,1,false + ); + imagesRT.Initialize( + SCREENWIDTH + ,SCREENHEIGHT + ,1,false + ); +} +void Renderer::CleanUpStatic() +{ + if(shVS) shVS->Release(); shVS=NULL; + if(shPS) shPS->Release(); shPS=NULL; + if(shCb) shCb->Release(); shCb=NULL; + if(cubeShCb) cubeShCb->Release(); cubeShCb=NULL; + if(lightStaticCb) lightStaticCb->Release(); lightStaticCb=NULL; + if(vLightCb) vLightCb->Release(); vLightCb=NULL; + SafeRelease(vSpotLightVS); + SafeRelease(vPointLightVS); + if(vLightPS) vLightPS->Release(); vLightPS=NULL; + + if(sOVS) sOVS->Release(); sOVS=NULL; + if(sOGS) sOGS->Release(); sOGS=NULL; + if(sOIL) sOIL->Release(); sOIL=NULL; + + if(constantBuffer) constantBuffer->Release(); constantBuffer=NULL; + if(staticCb) staticCb->Release(); staticCb=NULL; + if(vertexShader) vertexShader->Release(); vertexShader=NULL; + if(vertexShader10) vertexShader10->Release(); vertexShader10=NULL; + if(pixelShader) pixelShader->Release(); pixelShader=NULL; + if(simplestPS) simplestPS->Release(); simplestPS=NULL; + if(blackoutPS)blackoutPS->Release(); blackoutPS=NULL; + if(textureonlyPS)textureonlyPS->Release(); textureonlyPS=NULL; + if(waterPS) waterPS->Release(); waterPS=NULL; + if(waterVS) waterVS->Release(); waterVS=NULL; + if(transparentPS) transparentPS->Release(); transparentPS=NULL; + if(vertexLayout) vertexLayout->Release(); vertexLayout=NULL; + + if(cubeShGS) cubeShGS->Release(); cubeShGS=NULL; + if(cubeShVS) cubeShVS->Release(); cubeShVS=NULL; + if(cubeShPS) cubeShPS->Release(); cubeShPS=NULL; + + SafeRelease(decalVS); + SafeRelease(decalPS); + SafeRelease(decalCbVS); + SafeRelease(decalCbPS); + SafeRelease(stencilReadMatch); + + for(int i=0;i<3;++i){ + if(lightPS[i]){ + lightPS[i]->Release(); + lightPS[i]=NULL; + } + if(lightVS[i]){ + lightVS[i]->Release(); + lightVS[i]=NULL; + } + if(lightCb[i]){ + lightCb[i]->Release(); + lightCb[i]=NULL; + } + } + + if(pixelCB) pixelCB->Release(); pixelCB=NULL; + if(fxCb) fxCb->Release(); fxCb=NULL; + + if(texSampler) texSampler->Release(); texSampler=NULL; + if(mapSampler) mapSampler->Release(); mapSampler=NULL; + if(pointSampler) pointSampler->Release(); pointSampler=NULL; + if(comparisonSampler) comparisonSampler->Release(); comparisonSampler=NULL; + if(mirSampler) mirSampler->Release(); mirSampler=NULL; + + if(rasterizerState) rasterizerState->Release(); rasterizerState=NULL; + if(rssh) rssh->Release(); rssh=NULL; + if(nonCullRSsh) nonCullRSsh->Release(); nonCullRSsh=NULL; + if(wireRS) wireRS->Release(); wireRS=NULL; + if(nonCullWireRS) nonCullWireRS->Release(); nonCullWireRS=NULL; + if(nonCullRS) nonCullRS->Release(); nonCullRS=NULL; + if(backFaceRS) backFaceRS->Release(); backFaceRS=NULL; + if(depthStencilState) depthStencilState->Release(); depthStencilState=NULL; + if(stencilReadState) stencilReadState->Release(); stencilReadState=NULL; + if(xRayStencilState) xRayStencilState->Release(); xRayStencilState=NULL; + if(depthReadStencilState) depthReadStencilState->Release(); depthReadStencilState=NULL; + if(blendState) blendState->Release(); blendState=NULL; + if(blendStateTransparent) blendStateTransparent->Release(); blendStateTransparent=NULL; + if(blendStateAdd) blendStateAdd->Release(); blendStateAdd=NULL; + + + if(skyPS) skyPS->Release(); skyPS=NULL; + if(skySampler) skySampler->Release(); skySampler=NULL; + if(skyVS) skyVS->Release(); skyVS=NULL; + + if(matCb) matCb->Release(); matCb=NULL; + + if(hullShader) hullShader->Release(); hullShader=NULL; hullShader=NULL; + if(domainShader) domainShader->Release(); domainShader=NULL; domainShader=NULL; + + if(lineVS){ + lineVS->Release(); + lineVS=NULL; + } + if(linePS){ + linePS->Release(); + linePS=NULL; + } + if(lineIL){ + lineIL->Release(); + lineIL=NULL; + } + if(lineBuffer){ + lineBuffer->Release(); + lineBuffer=NULL; + } + if(noiseTex){ + noiseTex->Release(); + noiseTex=NULL; + } + + if(trailIL) trailIL->Release(); trailIL=NULL; + if(trailPS) trailPS->Release(); trailPS=NULL; + if(trailVS) trailVS->Release(); trailVS=NULL; + if(trailCB) trailCB->Release(); trailCB=NULL; + + SafeRelease(viewPropCB); + + HairParticle::CleanUpStatic(); + EmittedParticle::CleanUpStatic(); + Cube::CleanUpStatic(); + HitSphere::CleanUpStatic(); + + + //ResourceManager::del("images/noise.png"); + //ResourceManager::del("images/normalmap1.jpg"); + ResourceManager::del("images/clipbox.png"); +} +void Renderer::CleanUpStaticTemp(){ + + Renderer::resetVertexCount(); + + for(int i=0;iCleanUp(); + images.clear(); + for(int i=0;iCleanUp(); + waterRipples.clear(); + + + for(Light* l : lights) + l->CleanUp(); + lights.clear(); + + for(Armature* armature : armatures) + armature->CleanUp(); + armatures.clear(); + + for(MeshCollection::iterator iter=meshes.begin(); iter!=meshes.end(); ++iter){ + iter->second->CleanUp(); + } + meshes.clear(); + + for(int i=0;iCleanUp(); + for(int j=0;jeParticleSystems.size();++j) + objects[i]->eParticleSystems[j]->CleanUp(); + objects[i]->eParticleSystems.clear(); + + for(int j=0;jhParticleSystems.size();++j) + objects[i]->hParticleSystems[j]->CleanUp(); + objects[i]->hParticleSystems.clear(); + } + objects.clear(); + + for(int i=0;iCleanUp(); + for(int j=0;jeParticleSystems.size();++j) + objects_trans[i]->eParticleSystems[j]->CleanUp(); + objects_trans[i]->eParticleSystems.clear(); + + for(int j=0;jhParticleSystems.size();++j) + objects_trans[i]->hParticleSystems[j]->CleanUp(); + objects_trans[i]->hParticleSystems.clear(); + } + objects_trans.clear(); + + for(int i=0;iCleanUp(); + for(int j=0;jeParticleSystems.size();++j) + objects_water[i]->eParticleSystems[j]->CleanUp(); + objects_water[i]->eParticleSystems.clear(); + + for(int j=0;jhParticleSystems.size();++j) + objects_water[i]->hParticleSystems[j]->CleanUp(); + objects_water[i]->hParticleSystems.clear(); + } + objects_water.clear(); + everyObject.clear(); + + for(MaterialCollection::iterator iter=materials.begin(); iter!=materials.end();++iter) + iter->second->CleanUp(); + materials.clear(); + + + for(int i=0;iCleanUp(); + spTree = nullptr; + + if(spTree_trans) + spTree_trans->CleanUp(); + spTree_trans = nullptr; + + if(spTree_water) + spTree_water->CleanUp(); + spTree_water = nullptr; + + + if(spTree_lights) + spTree_lights->CleanUp(); + spTree_lights=nullptr; + + for(Decal* decal:decals){ + decal->CleanUp(); + delete decal; + } + decals.clear(); + +} +XMVECTOR Renderer::GetSunPosition() +{ + for(Light* l : lights) + if(l->type==Light::DIRECTIONAL) + return -XMVector3Transform( XMVectorSet(0,-1,0,1), XMMatrixRotationQuaternion( XMLoadFloat4(&l->rotation) ) ); + return XMVectorSet(0.5f,1,-0.5f,0); +} +XMFLOAT4 Renderer::GetSunColor() +{ + for(Light* l : lights) + if(l->type==Light::DIRECTIONAL) + return l->color; + return XMFLOAT4(1,1,1,1); +} +float Renderer::GetGameSpeed(){return GameSpeed*overrideGameSpeed;} + +void Renderer::UpdateSpheres() +{ + //for(int i=0;iparentArmature; + // Bone* bone=(Bone*)spheres[i]->parent; + + // //spheres[i]->world = *bone->boneRelativity; + // //XMStoreFloat3( &spheres[i]->translation, XMVector3Transform( XMLoadFloat3(&spheres[i]->translation_rest),XMLoadFloat4x4(bone->boneRelativity) ) ); + // XMStoreFloat3( &spheres[i]->translation, + // XMVector3Transform( XMLoadFloat3(&spheres[i]->translation_rest),XMLoadFloat4x4(&bone->recursiveRestInv)*XMLoadFloat4x4(&bone->world) ) + // ); + //} + + for(HitSphere* s : spheres){ + s->getTransform(); + s->center=s->translation; + s->radius=s->radius_saved*s->scale.x; + } +} +float Renderer::getSphereRadius(const int& index){ + return spheres[index]->radius; +} +void Renderer::SetUpBoneLines() +{ + boneLines.resize(0); + for(int i=0;iboneCollection.size();j++){ + boneLines.push_back( Lines(armatures[i]->boneCollection[j]->length,XMFLOAT4A(1,1,1,1),i,j) ); + } + } +} +void Renderer::UpdateBoneLines() +{ + if(debugLines) + for(int i=0;iboneCollection[boneI]->recursiveRest) ; + //XMMATRIX pose = XMLoadFloat4x4(&armatures[armatureI]->boneCollection[boneI]->world) ; + //XMFLOAT4X4 finalM; + //XMStoreFloat4x4( &finalM, /*rest**/pose ); + + boneLines[i].Transform(armatures[armatureI]->boneCollection[boneI]->world); + } +} +void iterateSPTree2(SPTree::Node* n, vector& cubes, const XMFLOAT4A& col); +void iterateSPTree(SPTree::Node* n, vector& cubes, const XMFLOAT4A& col){ + if(!n) return; + if(n->count){ + for(int i=0;ichildren.size();++i) + iterateSPTree(n->children[i],cubes,col); + } + if(!n->objects.empty()){ + cubes.push_back(Cube(n->box.getCenter(),n->box.getHalfWidth(),col)); + for(Cullable* object:n->objects){ + cubes.push_back(Cube(object->bounds.getCenter(),object->bounds.getHalfWidth(),XMFLOAT4A(1,0,0,1))); + //Object* o = (Object*)object; + //for(HairParticle& hps : o->hParticleSystems) + // iterateSPTree2(hps.spTree->root,cubes,XMFLOAT4A(0,1,0,1)); + } + } +} +void iterateSPTree2(SPTree::Node* n, vector& cubes, const XMFLOAT4A& col){ + if(!n) return; + if(n->count){ + for(int i=0;ichildren.size();++i) + iterateSPTree2(n->children[i],cubes,col); + } + if(!n->objects.empty()){ + cubes.push_back(Cube(n->box.getCenter(),n->box.getHalfWidth(),col)); + } +} +void Renderer::SetUpCubes(){ + /*if(debugBoxes){ + cubes.resize(0); + iterateSPTree(spTree->root,cubes); + for(Object* object:objects) + cubes.push_back(Cube(XMFLOAT3(0,0,0),XMFLOAT3(1,1,1),XMFLOAT4A(1,0,0,1))); + }*/ + cubes.clear(); +} +void Renderer::UpdateCubes(){ + if(debugBoxes && spTree && spTree->root){ + /*int num=0; + iterateSPTreeUpdate(spTree->root,cubes,num); + for(Object* object:objects){ + AABB b=object->frameBB; + XMFLOAT3 c = b.getCenter(); + XMFLOAT3 hw = b.getHalfWidth(); + cubes[num].Transform( XMMatrixScaling(hw.x,hw.y,hw.z) * XMMatrixTranslation(c.x,c.y,c.z) ); + num+=1; + }*/ + cubes.clear(); + if(spTree) iterateSPTree(spTree->root,cubes,XMFLOAT4A(1,1,0,1)); + if(spTree_trans) iterateSPTree(spTree_trans->root,cubes,XMFLOAT4A(0,1,1,1)); + if(spTree_water) iterateSPTree(spTree_water->root,cubes,XMFLOAT4A(0,0,1,1)); + if(spTree_lights) iterateSPTree(spTree_lights->root,cubes,XMFLOAT4A(1,1,1,1)); + } + if(debugBoxes){ + for(Decal* decal : decals){ + cubes.push_back(Cube(decal->bounds.getCenter(),decal->bounds.getHalfWidth(),XMFLOAT4A(1,0,1,1))); + } + } +} +void Renderer::UpdateSPTree(SPTree*& tree){ + if(tree && tree->root){ + SPTree* newTree = tree->updateTree(tree->root); + if(newTree){ + tree->CleanUp(); + tree=newTree; + } + } +} + +void Renderer::LoadBuffers() +{ + D3D11_BUFFER_DESC bd; + // Create the constant buffer + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ConstantBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &constantBuffer ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(StaticCB); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &staticCb ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(PixelCB); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &pixelCB ); + + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ForShadowMapCB); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &shCb ); + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(CubeShadowCb); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &cubeShCb ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(MaterialCB); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &matCb ); + + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(TessBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &tessBuf ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(LineBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &lineBuffer ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(XMMATRIX); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &trailCB ); + + // + //ZeroMemory( &bd, sizeof(bd) ); + //bd.Usage = D3D11_USAGE_DYNAMIC; + //bd.ByteWidth = sizeof(MatIndexBuf); + //bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + //bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + //Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &matIndexBuf ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(LightStaticCB); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &lightStaticCb ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(dLightBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &lightCb[0] ); + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(pLightBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &lightCb[1] ); + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(sLightBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &lightCb[2] ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(vLightBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &vLightCb ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(FxCB); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &fxCb ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(SkyBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &skyCb ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(DecalCBVS); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &decalCbVS ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(DecalCBPS); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &decalCbPS ); + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(ViewPropCB); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &viewPropCB ); + +} + +void Renderer::LoadBasicShaders() +{ + + ID3DBlob* pVSBlob = NULL; + if(FAILED(D3DReadFileToBlob(L"shaders/effectVS10.cso", &pVSBlob))){MessageBox(0,L"Failed To load effectVS10.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vertexShader10 ); + + D3D11_INPUT_ELEMENT_DESC layout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "NORMAL", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 1, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, +//#ifdef USE_GPU_SKINNING +// { "TEXCOORD", 1, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, +// { "TEXCOORD", 2, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, +//#endif + + { "MATI", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 1, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_INSTANCE_DATA, 1 }, + { "MATI", 1, DXGI_FORMAT_R32G32B32A32_FLOAT, 1, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_INSTANCE_DATA, 1 }, + { "MATI", 2, DXGI_FORMAT_R32G32B32A32_FLOAT, 1, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_INSTANCE_DATA, 1 }, + }; + UINT numElements = ARRAYSIZE( layout ); + + Renderer::graphicsDevice->CreateInputLayout( layout, numElements, pVSBlob->GetBufferPointer(), + pVSBlob->GetBufferSize(), &vertexLayout ); + + + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/sOVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load sOVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &sOVS ); + D3D11_INPUT_ELEMENT_DESC oslayout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "NORMAL", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 0, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 1, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 2, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + }; + numElements = ARRAYSIZE( oslayout ); + + Renderer::graphicsDevice->CreateInputLayout( oslayout, numElements, pVSBlob->GetBufferPointer(), + pVSBlob->GetBufferSize(), &sOIL ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + + if(FAILED(D3DReadFileToBlob(L"shaders/sOGS.cso", &pVSBlob))){MessageBox(0,L"Failed To load sOGS.cso",0,0);} + else Renderer::graphicsDevice->CreateGeometryShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &sOGS ); + D3D11_SO_DECLARATION_ENTRY pDecl[] = + { + // semantic name, semantic index, start component, component count, output slot + { 0, "SV_POSITION", 0, 0, 4, 0 }, // output all components of position + { 0, "NORMAL", 0, 0, 3, 0 }, // output the first 3 of the normal + { 0, "TEXCOORD", 0, 0, 4, 0 }, // output the first 2 texture coordinates + { 0, "TEXCOORD", 1, 0, 4, 0 }, // output the first 2 texture coordinates + }; + HRESULT hr = Renderer::graphicsDevice->CreateGeometryShaderWithStreamOutput( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), pDecl, + 4, NULL, 0, sOGS?0:D3D11_SO_NO_RASTERIZED_STREAM, NULL, &sOGS ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + + if(FAILED(D3DReadFileToBlob(L"shaders/effectVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load effectVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vertexShader ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/dirLightVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load dirlightVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &lightVS[0] ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/pointLightVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load pointlightVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &lightVS[1] ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/spotLightVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load spotlightVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &lightVS[2] ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/vSpotLightVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load vSpotLightVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vSpotLightVS ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/vPointLightVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load vPointLightVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &vPointLightVS ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/decalVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load decalVS.cso",0,0);} + else Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &decalVS ); + if(pVSBlob){ pVSBlob->Release(); pVSBlob=NULL;} + + + ID3DBlob* pPSBlob = NULL; + if(FAILED(D3DReadFileToBlob(L"shaders/effectPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load effectPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &pixelShader ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/effectPS_transparent.cso", &pPSBlob))){MessageBox(0,L"Failed To load effectPS_transparent.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &transparentPS ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/effectPS_simplest.cso", &pPSBlob))){MessageBox(0,L"Failed To load effectPS_simplest.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &simplestPS ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/effectPS_blackout.cso", &pPSBlob))){MessageBox(0,L"Failed To load effectPS_blackout.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &blackoutPS ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/effectPS_textureonly.cso", &pPSBlob))){MessageBox(0,L"Failed To load effectPS_textureonly.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &textureonlyPS ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/dirLightPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load dirLightPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &lightPS[0] ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/pointLightPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load pointLightPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &lightPS[1] ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/spotLightPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load spotLightPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &lightPS[2] ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/volumeLightPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load volumeLightPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &vLightPS ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} + + if(FAILED(D3DReadFileToBlob(L"shaders/decalPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load decalPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &decalPS ); + if(pPSBlob) { pPSBlob->Release(); pPSBlob=NULL;} +} +void Renderer::LoadLineShaders() +{ + ID3DBlob* pSVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/linesVS.cso", &pSVSBlob))){MessageBox(0,L"Failed To load linesVS.cso",0,0);} + else { + Renderer::graphicsDevice->CreateVertexShader( pSVSBlob->GetBufferPointer(), pSVSBlob->GetBufferSize(), NULL, &lineVS ); + + + + + D3D11_INPUT_ELEMENT_DESC layout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + }; + UINT numElements = ARRAYSIZE( layout ); + + Renderer::graphicsDevice->CreateInputLayout( layout, numElements, pSVSBlob->GetBufferPointer(), + pSVSBlob->GetBufferSize(), &lineIL ); + } + + if(pSVSBlob){ pSVSBlob->Release(); pSVSBlob=NULL; } + + ID3DBlob* pSPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/linesPS.cso", &pSPSBlob))){MessageBox(0,L"Failed To load linesPS.cso",0,0);} + else Renderer::graphicsDevice->CreatePixelShader( pSPSBlob->GetBufferPointer(), pSPSBlob->GetBufferSize(), NULL, &linePS ); + + if(pSPSBlob){ pSPSBlob->Release();pSPSBlob=NULL; } +} +void Renderer::LoadTessShaders() +{ + ID3DBlob* pHSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/effectHS.cso", &pHSBlob))){MessageBox(0,L"Failed To load effectHS.cso",0,0);} + else Renderer::graphicsDevice->CreateHullShader( pHSBlob->GetBufferPointer(), pHSBlob->GetBufferSize(), NULL, &hullShader ); + if(pHSBlob) {pHSBlob->Release();pHSBlob=NULL;} + + ID3DBlob* pDSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/effectDS.cso", &pDSBlob))){MessageBox(0,L"Failed To load effectDS.cso",0,0);} + else Renderer::graphicsDevice->CreateDomainShader( pDSBlob->GetBufferPointer(), pDSBlob->GetBufferSize(), NULL, &domainShader ); + if(pDSBlob) {pDSBlob->Release();pDSBlob=NULL;} +} +void Renderer::LoadSkyShaders() +{ + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/skyVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load skyVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &skyVS ); + + if(pVSBlob){ pVSBlob->Release(); + pVSBlob=NULL; } + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/skyPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load skyPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &skyPS ); + + if(pPSBlob){ pPSBlob->Release(); pPSBlob=NULL; } +} +void Renderer::LoadShadowShaders() +{ + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/shadowVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load shadowVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &shVS ); + if(pVSBlob) {pVSBlob->Release(); pVSBlob=NULL;} + + + if(FAILED(D3DReadFileToBlob(L"shaders/cubeShadowVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load cubeShadowVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &cubeShVS ); + if(pVSBlob) {pVSBlob->Release(); pVSBlob=NULL;} + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/shadowPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load shadowPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &shPS ); + if(pPSBlob){ pPSBlob->Release(); pPSBlob=NULL; } + + if(FAILED(D3DReadFileToBlob(L"shaders/cubeShadowPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load cubeShadowPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &cubeShPS ); + if(pPSBlob){ pPSBlob->Release(); pPSBlob=NULL; } + + + + if(FAILED(D3DReadFileToBlob(L"shaders/cubeShadowGS.cso", &pPSBlob))){MessageBox(0,L"Failed To load cubeShadowGS.cso",0,0);} + Renderer::graphicsDevice->CreateGeometryShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &cubeShGS ); + if(pPSBlob){ pPSBlob->Release(); pPSBlob=NULL; } +} +void Renderer::LoadWaterShaders() +{ + ID3DBlob* pVSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/waterVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load waterVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &waterVS ); + if(pVSBlob) {pVSBlob->Release(); pVSBlob=NULL;} + + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/waterPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load waterPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &waterPS ); + if(pPSBlob){ pPSBlob->Release(); pPSBlob=NULL; } +} +void Renderer::LoadTrailShaders(){ + ID3DBlob* pVSBlob = NULL; + if(FAILED(D3DReadFileToBlob(L"shaders/trailVS.cso", &pVSBlob))){MessageBox(0,L"Failed To load trailVS.cso",0,0);} + Renderer::graphicsDevice->CreateVertexShader( pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), NULL, &trailVS ); + + D3D11_INPUT_ELEMENT_DESC layout[] = + { + { "POSITION", 0, DXGI_FORMAT_R32G32B32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 0, DXGI_FORMAT_R32G32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + { "TEXCOORD", 1, DXGI_FORMAT_R32G32B32A32_FLOAT, 0, D3D11_APPEND_ALIGNED_ELEMENT, D3D11_INPUT_PER_VERTEX_DATA, 0 }, + }; + UINT numElements = ARRAYSIZE( layout ); + Renderer::graphicsDevice->CreateInputLayout( layout, numElements, pVSBlob->GetBufferPointer(), pVSBlob->GetBufferSize(), &trailIL ); + + if(pVSBlob) {pVSBlob->Release(); pVSBlob=NULL;} + + + + ID3DBlob* pPSBlob = NULL; + + if(FAILED(D3DReadFileToBlob(L"shaders/trailPS.cso", &pPSBlob))){MessageBox(0,L"Failed To load trailPS.cso",0,0);} + Renderer::graphicsDevice->CreatePixelShader( pPSBlob->GetBufferPointer(), pPSBlob->GetBufferSize(), NULL, &trailPS ); + + if(pPSBlob){ pPSBlob->Release(); pPSBlob=NULL; } +} + + +void Renderer::SetUpStates() +{ + D3D11_SAMPLER_DESC samplerDesc; + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssMirrorLin); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssMirrorPoi); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssWrapLin); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssWrapPoi); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssClampLin); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssClampPoi); + + samplerDesc.Filter = D3D11_FILTER_ANISOTROPIC; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 4; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssClampAni); + + samplerDesc.Filter = D3D11_FILTER_ANISOTROPIC; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 4; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssWrapAni); + + samplerDesc.Filter = D3D11_FILTER_ANISOTROPIC; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 4; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssMirrorAni); + + samplerDesc.Filter = D3D11_FILTER_COMPARISON_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 16; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_LESS_EQUAL; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &ssComp); + + + + samplerDesc.Filter = D3D11_FILTER_ANISOTROPIC; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_WRAP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 16; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &texSampler); + + ZeroMemory( &samplerDesc, sizeof(D3D11_SAMPLER_DESC) ); + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 16; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &mapSampler); + + ZeroMemory( &samplerDesc, sizeof(D3D11_SAMPLER_DESC) ); + samplerDesc.Filter = D3D11_FILTER_COMPARISON_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 16; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_LESS_EQUAL; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &comparisonSampler); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_MIRROR; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 16; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &mirSampler); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &pointSampler); + + samplerDesc.Filter = D3D11_FILTER_MIN_MAG_LINEAR_MIP_POINT; + samplerDesc.AddressU = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressV = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.AddressW = D3D11_TEXTURE_ADDRESS_CLAMP; + samplerDesc.MipLODBias = 0.0f; + samplerDesc.MaxAnisotropy = 0; + samplerDesc.ComparisonFunc = D3D11_COMPARISON_NEVER; + samplerDesc.BorderColor[0] = 0; + samplerDesc.BorderColor[1] = 0; + samplerDesc.BorderColor[2] = 0; + samplerDesc.BorderColor[3] = 0; + samplerDesc.MinLOD = 0; + samplerDesc.MaxLOD = D3D11_FLOAT32_MAX; + graphicsDevice->CreateSamplerState(&samplerDesc, &skySampler); + + + D3D11_RASTERIZER_DESC rs; + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_BACK; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=true; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + graphicsDevice->CreateRasterizerState(&rs,&rasterizerState); + + + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_BACK; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=4; + rs.DepthClipEnable=true; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + graphicsDevice->CreateRasterizerState(&rs,&rssh); + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_NONE; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=5.0f; + rs.DepthClipEnable=true; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + graphicsDevice->CreateRasterizerState(&rs,&nonCullRSsh); + + rs.FillMode=D3D11_FILL_WIREFRAME; + rs.CullMode=D3D11_CULL_BACK; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=true; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + graphicsDevice->CreateRasterizerState(&rs,&wireRS); + + + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_NONE; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=false; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + graphicsDevice->CreateRasterizerState(&rs,&nonCullRS); + + rs.FillMode=D3D11_FILL_WIREFRAME; + rs.CullMode=D3D11_CULL_NONE; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=false; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + graphicsDevice->CreateRasterizerState(&rs,&nonCullWireRS); + + rs.FillMode=D3D11_FILL_SOLID; + rs.CullMode=D3D11_CULL_FRONT; + rs.FrontCounterClockwise=true; + rs.DepthBias=0; + rs.DepthBiasClamp=0; + rs.SlopeScaledDepthBias=0; + rs.DepthClipEnable=false; + rs.ScissorEnable=false; + rs.MultisampleEnable=false; + rs.AntialiasedLineEnable=false; + graphicsDevice->CreateRasterizerState(&rs,&backFaceRS); + + D3D11_DEPTH_STENCIL_DESC dsd; + dsd.DepthEnable = true; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ALL; + dsd.DepthFunc = D3D11_COMPARISON_LESS_EQUAL; + + dsd.StencilEnable = true; + dsd.StencilReadMask = 0xFF; + dsd.StencilWriteMask = 0xFF; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_REPLACE; + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_ALWAYS; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_REPLACE; + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + // Create the depth stencil state. + graphicsDevice->CreateDepthStencilState(&dsd, &depthStencilState); + + + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ZERO; + dsd.DepthEnable = false; + dsd.StencilEnable = true; + dsd.StencilReadMask = 0xFF; + dsd.StencilWriteMask = 0xFF; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_LESS_EQUAL; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_LESS_EQUAL; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + // Create the depth stencil state. + graphicsDevice->CreateDepthStencilState(&dsd, &stencilReadState); + + + dsd.DepthEnable = false; + dsd.StencilEnable = true; + dsd.StencilReadMask = 0xFF; + dsd.StencilWriteMask = 0xFF; + dsd.FrontFace.StencilFunc = D3D11_COMPARISON_EQUAL; + dsd.FrontFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.FrontFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFunc = D3D11_COMPARISON_EQUAL; + dsd.BackFace.StencilPassOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilFailOp = D3D11_STENCIL_OP_KEEP; + dsd.BackFace.StencilDepthFailOp = D3D11_STENCIL_OP_KEEP; + graphicsDevice->CreateDepthStencilState(&dsd, &stencilReadMatch); + + + dsd.DepthEnable = true; + dsd.StencilEnable = false; + dsd.DepthWriteMask = D3D11_DEPTH_WRITE_MASK_ZERO; + dsd.DepthFunc = D3D11_COMPARISON_LESS_EQUAL; + graphicsDevice->CreateDepthStencilState(&dsd, &depthReadStencilState); + + dsd.DepthEnable = false; + dsd.StencilEnable=false; + graphicsDevice->CreateDepthStencilState(&dsd, &xRayStencilState); + + + D3D11_BLEND_DESC bd; + ZeroMemory(&bd, sizeof(bd)); + bd.RenderTarget[0].BlendEnable=false; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_ALPHA; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_MAX; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + bd.AlphaToCoverageEnable=false; + graphicsDevice->CreateBlendState(&bd,&blendState); + + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_SRC_ALPHA; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_INV_SRC_ALPHA; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].RenderTargetWriteMask = 0x0f; + bd.AlphaToCoverageEnable=false; + graphicsDevice->CreateBlendState(&bd,&blendStateTransparent); + + + bd.RenderTarget[0].BlendEnable=true; + bd.RenderTarget[0].SrcBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlend = D3D11_BLEND_ONE; + bd.RenderTarget[0].BlendOp = D3D11_BLEND_OP_ADD; + bd.RenderTarget[0].SrcBlendAlpha = D3D11_BLEND_ONE; + bd.RenderTarget[0].DestBlendAlpha = D3D11_BLEND_ZERO; + bd.RenderTarget[0].BlendOpAlpha = D3D11_BLEND_OP_MAX; + bd.IndependentBlendEnable=true, + bd.AlphaToCoverageEnable=false; + graphicsDevice->CreateBlendState(&bd,&blendStateAdd); +} + + +Armature* Renderer::getArmatureByName(const string& get) +{ + for(Armature* armature : armatures) + if(!armature->name.compare(get)) + return armature; + return nullptr; +} +int Renderer::getActionByName(Armature* armature, const string& get) +{ + if(armature==nullptr) + return (-1); + + stringstream ss(""); + ss<unidentified_name<actions.size();j++) + if(!armature->actions[j].name.compare(ss.str())) + return j; + return (-1); +} +int Renderer::getBoneByName(Armature* armature, const string& get) +{ + for(int j=0;jboneCollection.size();j++) + if(!armature->boneCollection[j]->name.compare(get)) + return j; + return (-1); +} +Material* Renderer::getMaterialByName(const string& get) +{ + MaterialCollection::iterator iter = materials.find(get); + if(iter!=materials.end()) + return iter->second; + return NULL; +} +HitSphere* Renderer::getSphereByName(const string& get){ + for(HitSphere* hs : spheres) + if(!hs->name.compare(get)) + return hs; + return nullptr; +} + +void Renderer::RecursiveBoneTransform(Armature* armature, Bone* bone, const XMMATRIX& parentCombinedMat){ + float cf = armature->currentFrame; + float prevActionResolveFrame = armature->prevActionResolveFrame; + int maxCf = 0; + int activeAction = armature->activeAction; + int prevAction = armature->prevAction; + Bone* parent = (Bone*)bone->parent; + + + XMVECTOR& finalTrans = InterPolateKeyFrames(cf,maxCf,bone->actionFrames[activeAction].keyframesPos,POSITIONKEYFRAMETYPE); + XMVECTOR& finalRotat = InterPolateKeyFrames(cf,maxCf,bone->actionFrames[activeAction].keyframesRot,ROTATIONKEYFRAMETYPE); + XMVECTOR& finalScala = InterPolateKeyFrames(cf,maxCf,bone->actionFrames[activeAction].keyframesSca,SCALARKEYFRAMETYPE); + + bone->worldPrev = bone->world; + bone->translationPrev = bone->translation; + bone->rotationPrev = bone->rotation; + XMStoreFloat3(&bone->translation , finalTrans ); + XMStoreFloat4(&bone->rotation , finalRotat ); + XMStoreFloat3(&bone->scale , finalScala ); + + XMMATRIX& anim = + XMMatrixScalingFromVector( finalScala ) + * XMMatrixRotationQuaternion( finalRotat ) + * XMMatrixTranslationFromVector( finalTrans ); + + XMMATRIX& rest = + XMLoadFloat4x4( &bone->world_rest ); + + XMMATRIX& boneMat = + anim * rest * parentCombinedMat + ; + + XMMATRIX& finalMat = + XMLoadFloat4x4( &bone->recursiveRestInv )* + boneMat + ; + + XMStoreFloat4x4( &bone->world,boneMat ); + + *bone->boneRelativityPrev=*bone->boneRelativity; + XMStoreFloat4x4( bone->boneRelativity,finalMat ); + + for(int i=0;ichildrenI.size();++i){ + RecursiveBoneTransform(armature,bone->childrenI[i],boneMat); + } + +} +XMVECTOR Renderer::InterPolateKeyFrames(const float& cf, const int& maxCf, const vector& keyframeList, KeyFrameType type) +{ + XMVECTOR result = XMVectorSet(0,0,0,0); + + if(type==POSITIONKEYFRAMETYPE) result=XMVectorSet(0,0,0,1); + if(type==ROTATIONKEYFRAMETYPE) result=XMVectorSet(0,0,0,1); + if(type==SCALARKEYFRAMETYPE) result=XMVectorSet(1,1,1,1); + + //SEARCH 2 INTERPOLATABLE FRAMES + int nearest[2] = {0,0}; + int first=0,last=0; + if(keyframeList.size()>1){ + first=keyframeList[0].frameI; + last=keyframeList.back().frameI; + + if(cf<=first){ //BROKEN INTERVAL + nearest[0] = 0; + nearest[1] = 0; + } + else if(cf>=last){ + nearest[0] = keyframeList.size()-1; + nearest[1] = keyframeList.size()-1; + } + else{ //IN BETWEEN TWO KEYFRAMES, DECIDE WHICH + for(int k=keyframeList.size()-1;k>0;k--) + if(keyframeList[k].frameI<=cf){ + nearest[0] = k; + break; + } + for(int k=0;k=cf){ + nearest[1] = k; + break; + } + } + + //INTERPOLATE BETWEEN THE TWO FRAMES + int keyframes[2] = { + keyframeList[nearest[0]].frameI, + keyframeList[nearest[1]].frameI + }; + float interframe=0; + if(cf<=first || cf>=last){ //BROKEN INTERVAL + float intervalBegin = maxCf - keyframes[0]; + float intervalEnd = keyframes[1] + intervalBegin; + float intervalLen = abs(intervalEnd - intervalBegin); + float offsetCf = cf + intervalBegin; + if(intervalLen) interframe = offsetCf / intervalLen; + } + else{ + float intervalBegin = keyframes[0]; + float intervalEnd = keyframes[1]; + float intervalLen = abs(intervalEnd - intervalBegin); + float offsetCf = cf - intervalBegin; + if(intervalLen) interframe = offsetCf / intervalLen; + } + + if(type==ROTATIONKEYFRAMETYPE){ + XMVECTOR quat[2]={ + XMLoadFloat4(&keyframeList[nearest[0]].data), + XMLoadFloat4(&keyframeList[nearest[1]].data) + }; + result = XMQuaternionNormalize( XMQuaternionSlerp(quat[0],quat[1],interframe) ); + } + else{ + XMVECTOR tran[2]={ + XMLoadFloat4(&keyframeList[nearest[0]].data), + XMLoadFloat4(&keyframeList[nearest[1]].data) + }; + result = XMVectorLerp(tran[0],tran[1],interframe); + } + } + else{ + if(!keyframeList.empty()) + result = XMLoadFloat4(&keyframeList.back().data); + } + + return result; +} +Vertex Renderer::TransformVertex(const Mesh* mesh, const int& vertexI, const XMMATRIX& mat){ + XMVECTOR pos = XMLoadFloat4( &mesh->vertices[vertexI].pos ); + XMVECTOR nor = XMLoadFloat3( &mesh->vertices[vertexI].nor ); + float inWei[4]={mesh->vertices[vertexI].wei.x + ,mesh->vertices[vertexI].wei.y + ,mesh->vertices[vertexI].wei.z + ,mesh->vertices[vertexI].wei.w}; + float inBon[4]={mesh->vertices[vertexI].bon.x + ,mesh->vertices[vertexI].bon.y + ,mesh->vertices[vertexI].bon.z + ,mesh->vertices[vertexI].bon.w}; + XMMATRIX sump; + if(inWei[0] || inWei[1] || inWei[2] || inWei[3]){ + sump = XMMATRIX(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0); + float sumw = 0; + for(unsigned int i=0;i<4;i++){ + sumw += inWei[i]; + sump += XMLoadFloat4x4( mesh->armature->boneCollection[int(inBon[i])]->boneRelativity ) * inWei[i]; + } + if(sumw) sump/=sumw; + + //sump = XMMatrixTranspose(sump); + } + else + sump = XMMatrixIdentity(); + + //sump*=mat; + + XMFLOAT3 transformedP,transformedN; + XMStoreFloat3( &transformedP,XMVector3Transform(pos,sump) ); + + sump.r[3]=XMVectorSetX(sump.r[3],0); + sump.r[3]=XMVectorSetY(sump.r[3],0); + sump.r[3]=XMVectorSetZ(sump.r[3],0); + //sump.r[3].m128_f32[0]=sump.r[3].m128_f32[1]=sump.r[3].m128_f32[2]=0; + XMStoreFloat3( &transformedN,XMVector3Normalize(XMVector3Transform(nor,sump))); + + Vertex retV(transformedP); + retV.nor=transformedN; + retV.tex=mesh->vertices[vertexI].tex; + retV.pre=XMFLOAT4(0,0,0,1); + + return retV; +} +XMFLOAT3 Renderer::VertexVelocity(const Mesh* mesh, const int& vertexI){ + XMVECTOR pos = XMLoadFloat4( &mesh->vertices[vertexI].pos ); + float inWei[4]={mesh->vertices[vertexI].wei.x + ,mesh->vertices[vertexI].wei.y + ,mesh->vertices[vertexI].wei.z + ,mesh->vertices[vertexI].wei.w}; + float inBon[4]={mesh->vertices[vertexI].bon.x + ,mesh->vertices[vertexI].bon.y + ,mesh->vertices[vertexI].bon.z + ,mesh->vertices[vertexI].bon.w}; + XMMATRIX sump; + XMMATRIX sumpPrev; + if(inWei[0] || inWei[1] || inWei[2] || inWei[3]){ + sump = sumpPrev = XMMATRIX(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0); + float sumw = 0; + for(unsigned int i=0;i<4;i++){ + sumw += inWei[i]; + sump += XMLoadFloat4x4( mesh->armature->boneCollection[int(inBon[i])]->boneRelativity ) * inWei[i]; + sumpPrev += XMLoadFloat4x4( mesh->armature->boneCollection[int(inBon[i])]->boneRelativityPrev ) * inWei[i]; + } + if(sumw){ + sump/=sumw; + sumpPrev/=sumw; + } + } + else + sump = sumpPrev = XMMatrixIdentity(); + XMFLOAT3 velocity; + XMStoreFloat3( &velocity,GetGameSpeed()*XMVectorSubtract(XMVector3Transform(pos,sump),XMVector3Transform(pos,sumpPrev)) ); + return velocity; +} +void Renderer::Update(float amount) +{ + //if(GetGameSpeed()) + { + for(Armature* armature : armatures){ + if(armature->actions.size()){ + //XMMATRIX world = XMMatrixScalingFromVector(XMLoadFloat3(&armature->scale))*XMMatrixRotationQuaternion(XMLoadFloat4(&armature->rotation))*XMMatrixTranslationFromVector(XMLoadFloat3(&armature->translation)); + //XMStoreFloat4x4( &armature->world,world ); + XMMATRIX world = armature->getTransform(); + + float cf = armature->currentFrame; + float prevActionResolveFrame = armature->prevActionResolveFrame; + int maxCf = 0; + int activeAction = armature->activeAction; + int prevAction = armature->prevAction; + + cf = armature->currentFrame += amount; + maxCf = armature->actions[activeAction].frameCount; + if((int)cf>maxCf) + cf = armature->currentFrame = 1; + + { + for(int j=0;jrootbones.size();++j){ + RecursiveBoneTransform(armature,armature->rootbones[j],world); + } + } + } + } + + + + for(MaterialCollection::iterator iter=materials.begin(); iter!=materials.end();++iter){ + Material* iMat=iter->second; + iMat->framesToWaitForTexCoordOffset-=1.0f; + if(iMat->framesToWaitForTexCoordOffset<=0){ + iMat->texOffset.x+=iMat->movingTex.x*GameSpeed; + iMat->texOffset.y+=iMat->movingTex.y*GameSpeed; + iMat->framesToWaitForTexCoordOffset=iMat->movingTex.z*GameSpeed; + } + } + + for(map>::iterator iter=emitterSystems.begin();iter!=emitterSystems.end();++iter){ + for(EmittedParticle* e:iter->second) + e->Update(GameSpeed); + } + + + list::iterator iter = decals.begin(); + while (iter != decals.end()) + { + Decal* decal = *iter; + decal->getTransform(); + decal->Update(); + if(decal->life>-2){ + decal->life-=GameSpeed; + if(decal->life<=0){ + decal->detach(); + decals.erase(iter++); + continue; + } + } + ++iter; + } + + } +} +void Renderer::UpdateRenderInfo(ID3D11DeviceContext* context) +{ + + UpdatePerWorldCB(context); + + UpdateObjects(); + + //if(GetGameSpeed()) + { + + + for(MeshCollection::iterator iter=meshes.begin(); iter!=meshes.end(); ++iter){ + Mesh* mesh = iter->second; + if(mesh->hasArmature() && !mesh->softBody && mesh->renderable){ +#ifdef USE_GPU_SKINNING + BoneShaderBuffer bonebuf = BoneShaderBuffer(); + for(int k=0;karmature->boneCollection.size();k++){ + bonebuf.pose[k]=XMMatrixTranspose(XMLoadFloat4x4(mesh->armature->boneCollection[k]->boneRelativity)); + bonebuf.prev[k]=XMMatrixTranspose(XMLoadFloat4x4(mesh->armature->boneCollection[k]->boneRelativityPrev)); + } + + + UpdateBuffer(mesh->boneBuffer,&bonebuf,context); +#else + for(int vi=0;viskinnedVertices.size();++vi) + mesh->skinnedVertices[vi]=TransformVertex(mesh,vi); +#endif + } + } +#ifdef USE_GPU_SKINNING + //Renderer::graphicsMutex.lock(); + DrawForSO(context); + //Renderer::graphicsMutex.unlock(); +#endif + + + UpdateSPTree(spTree); + UpdateSPTree(spTree_trans); + UpdateSPTree(spTree_water); + } + + UpdateBoneLines(); + UpdateCubes(); + + wind.time=(float)((Timer::TotalTime())/1000.0*GameSpeed/2.0*3.1415)*XMVectorGetX(XMVector3Length(XMLoadFloat3(&wind.direction)))*0.1f; +} +void Renderer::UpdateObjects(){ + for(int i=0;iarmatureTransform()){ + // world = XMMatrixScalingFromVector(XMLoadFloat3(&everyObject[i]->scale)); + + // if(everyObject[i]->mesh->isBillboarded){ + // XMMATRIX bbMat = XMMatrixIdentity(); + // if(everyObject[i]->mesh->billboardAxis.x || everyObject[i]->mesh->billboardAxis.y || everyObject[i]->mesh->billboardAxis.z){ + // float angle = 0; + // angle = atan2(everyObject[i]->translation.x - XMVectorGetX(cam->Eye), everyObject[i]->translation.z - XMVectorGetZ(cam->Eye)) * (180.0 / XM_PI); + // bbMat = XMMatrixRotationAxis(XMLoadFloat3(&everyObject[i]->mesh->billboardAxis), angle * 0.0174532925f ); + // } + // else + // bbMat = XMMatrixInverse(0,XMMatrixLookAtLH(XMVectorSet(0,0,0,0),XMLoadFloat3(&everyObject[i]->translation)-cam->Eye,XMVectorSet(0,1,0,0))); + // + // world *= bbMat * XMMatrixRotationQuaternion(XMLoadFloat4(&everyObject[i]->rotation)); + // } + // else + // world *= XMMatrixRotationQuaternion(XMLoadFloat4(&everyObject[i]->rotation)); + + // world *= XMMatrixTranslationFromVector(XMLoadFloat3(&everyObject[i]->translation)); + ////} + // if(everyObject[i]->parentbone()){ + // int bi=everyObject[i]->boneIndex; + // world = + // world* + // XMLoadFloat4x4(&everyObject[i]->mesh->armature->boneCollection[bi].poseFrame) + // ; + // } + ////} + ////else world = XMMatrixIdentity(); + + //everyObject[i]->worldPrev=everyObject[i]->world; + //XMStoreFloat4x4(&everyObject[i]->world,world); + + XMMATRIX world = everyObject[i]->getTransform(); + + if(everyObject[i]->mesh->isBillboarded){ + XMMATRIX bbMat = XMMatrixIdentity(); + if(everyObject[i]->mesh->billboardAxis.x || everyObject[i]->mesh->billboardAxis.y || everyObject[i]->mesh->billboardAxis.z){ + float angle = 0; + angle = atan2(everyObject[i]->translation.x - XMVectorGetX(cam->Eye), everyObject[i]->translation.z - XMVectorGetZ(cam->Eye)) * (180.0 / XM_PI); + bbMat = XMMatrixRotationAxis(XMLoadFloat3(&everyObject[i]->mesh->billboardAxis), angle * 0.0174532925f ); + } + else + bbMat = XMMatrixInverse(0,XMMatrixLookAtLH(XMVectorSet(0,0,0,0),XMVectorSubtract(XMLoadFloat3(&everyObject[i]->translation),cam->Eye),XMVectorSet(0,1,0,0))); + + XMMATRIX w = XMMatrixScalingFromVector(XMLoadFloat3(&everyObject[i]->scale)) * + bbMat * + XMMatrixRotationQuaternion(XMLoadFloat4(&everyObject[i]->rotation)) * + XMMatrixTranslationFromVector(XMLoadFloat3(&everyObject[i]->translation) + ); + XMStoreFloat4x4(&everyObject[i]->world,w); + } + + if(everyObject[i]->mesh->softBody) + everyObject[i]->bounds=everyObject[i]->mesh->aabb; + else if(!everyObject[i]->mesh->isBillboarded && everyObject[i]->mesh->renderable){ + //XMMATRIX mw = XMLoadFloat4x4(&everyObject[i]->world); + //if(everyObject[i]->armatureTransform()) + // mw = mw * XMLoadFloat4x4(&everyObject[i]->mesh->armature->world); + everyObject[i]->bounds=everyObject[i]->mesh->aabb.get(world); + } + else if(everyObject[i]->mesh->renderable) + everyObject[i]->bounds.createFromHalfWidth(everyObject[i]->translation,everyObject[i]->scale); + } +} +void Renderer::UpdateSoftBodyPinning(){ + for(MeshCollection::iterator iter=meshes.begin(); iter!=meshes.end(); ++iter){ + Mesh* m = iter->second; + if(m->softBody){ + int gvg = m->goalVG; + if(gvg>=0){ + int j=0; + for(map::iterator it=m->vertexGroups[gvg].vertices.begin();it!=m->vertexGroups[gvg].vertices.end();++it){ + int vi = (*it).first; + Vertex tvert=m->skinnedVertices[vi]; + if(m->hasArmature()) + tvert = TransformVertex(m,vi); + m->goalPositions[j] = XMFLOAT3(tvert.pos.x,tvert.pos.y,tvert.pos.z); + m->goalNormals[j] = tvert.nor; + ++j; + } + } + } + } +} +void Renderer::UpdateSkinnedVB(){ + Renderer::graphicsMutex.lock(); + for(MeshCollection::iterator iter=meshes.begin(); iter!=meshes.end(); ++iter){ + Mesh* m = iter->second; +#ifdef USE_GPU_SKINNING + if(m->softBody) +#else + if(m->softBody || m->hasArmature()) +#endif + { + UpdateBuffer(m->meshVertBuff,m->skinnedVertices.data(),Renderer::immediateContext,sizeof(Vertex)*m->skinnedVertices.size()); + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //void* dataPtr; + //Renderer::immediateContext->Map(m->meshVertBuff,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (void*)mappedResource.pData; + //memcpy(dataPtr,m->skinnedVertices.data(),sizeof(Vertex)*m->skinnedVertices.size()); + //Renderer::immediateContext->Unmap(m->meshVertBuff,0); + } + } + Renderer::graphicsMutex.unlock(); +} +void Renderer::UpdateImages(){ + for(int i=0;iUpdate(GameSpeed); + ManageImages(); +} +void Renderer::ManageImages(){ + while( + !images.empty() && + (images.front()->effects.opacity==1 || images.front()->effects.fade==1) + ) + images.pop_front(); +} +void Renderer::PutWaterRipple(const string& image, const XMFLOAT3& pos, const XMFLOAT4& plane){ + oImage* img=new oImage("","",image); + img->anim.fad=0.01f; + img->anim.scaleX=0.2f; + img->anim.scaleY=0.2f; + img->effects.pos=pos; + img->effects.rotation=(rand()%1000*0.001f)*2*3.1415f; + img->effects.siz=XMFLOAT2(1,1); + img->effects.typeFlag=WORLD; + img->effects.quality=QUALITY_ANISOTROPIC; + img->effects.pivotFlag=Pivot::CENTER; + img->effects.lookAt=plane; + img->effects.lookAt.w=1; + waterRipples.push_back(img); +} +void Renderer::ManageWaterRipples(){ + while( + !waterRipples.empty() && + (waterRipples.front()->effects.opacity==1 || waterRipples.front()->effects.fade==1) + ) + waterRipples.pop_front(); +} +void Renderer::DrawWaterRipples(ID3D11DeviceContext* context){ + Image::BatchBegin(context); + for(oImage* i:waterRipples){ + i->DrawNormal(context); + i->Update(GetGameSpeed()); + } +} + +void Renderer::DrawDebugSpheres(const XMMATRIX& newView, ID3D11DeviceContext* context) +{ + if(debugSpheres){ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLESTRIP ); + //context->IASetInputLayout( lineIL ); + BindPrimitiveTopology(TRIANGLESTRIP,context); + BindVertexLayout(lineIL,context); + + //context->RSSetState(rasterizerState); + //context->OMSetDepthStencilState(xRayStencilState, STENCILREF_EMPTY); + + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendStateTransparent, blendFactor, sampleMask); + BindRasterizerState(rasterizerState,context); + BindDepthStencilState(xRayStencilState,STENCILREF_EMPTY,context); + BindBlendState(blendStateTransparent,context); + + + //context->VSSetShader( lineVS, NULL, 0 ); + //context->PSSetShader( linePS, NULL, 0 ); + BindPS(linePS,context); + BindVS(lineVS,context); + + //context->VSSetConstantBuffers( 0, 1, &lineBuffer ); + // + //UINT stride = sizeof( XMFLOAT3A ); + //UINT offset = 0; + //context->IASetVertexBuffers( 0, 1, &HitSphere::vertexBuffer, &stride, &offset ); + + BindConstantBufferVS(lineBuffer,0,context); + BindVertexBuffer(HitSphere::vertexBuffer,0,sizeof(XMFLOAT3A),context); + + for(int i=0;iupdownRot)*XMMatrixRotationY(cam->leftrightRot)* + XMMatrixScaling( spheres[i]->radius,spheres[i]->radius,spheres[i]->radius ) * + XMMatrixTranslationFromVector( XMLoadFloat3(&spheres[i]->translation) ) + *newView*cam->Projection + ); + + XMFLOAT4A propColor; + if(spheres[i]->TYPE==HitSphere::HitSphereType::HITTYPE) propColor = XMFLOAT4A(0.1098f,0.4196f,1,1); + else if(spheres[i]->TYPE==HitSphere::HitSphereType::INVTYPE) propColor=XMFLOAT4A(0,0,0,1); + else if(spheres[i]->TYPE==HitSphere::HitSphereType::ATKTYPE) propColor=XMFLOAT4A(0.96f,0,0,1); + sb.color=propColor; + + UpdateBuffer(lineBuffer,&sb,context); + //LineBuffer* dataPtr; + //context->Map(lineBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (LineBuffer*)mappedResource.pData; + //memcpy(dataPtr,&sb,sizeof(LineBuffer)); + //context->Unmap(lineBuffer,0); + + + //context->Draw((HitSphere::RESOLUTION+1)*2,0); + Draw((HitSphere::RESOLUTION+1)*2,context); + + //context->Draw(RESOLUTION,0); + //context->Draw(RESOLUTION,RESOLUTION+1); + //context->Draw(RESOLUTION,(RESOLUTION+1)*2); + } + } + +} +void Renderer::DrawDebugLines(const XMMATRIX& newView, ID3D11DeviceContext* context) +{ + if(debugLines){ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_LINELIST ); + //context->IASetInputLayout( lineIL ); + BindPrimitiveTopology(LINELIST,context); + BindVertexLayout(lineIL,context); + + BindRasterizerState(rssh,context); + BindDepthStencilState(xRayStencilState,STENCILREF_EMPTY,context); + BindBlendState(blendState,context); + + + //context->VSSetShader( lineVS, NULL, 0 ); + //context->PSSetShader( linePS, NULL, 0 ); + BindPS(linePS,context); + BindVS(lineVS,context); + + BindConstantBufferVS(lineBuffer,0,context); + + for(int i=0;iProjection + ); + sb.color=boneLines[i].desc.color; + + UpdateBuffer(lineBuffer,&sb,context); + //LineBuffer* dataPtr; + //context->Map(lineBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (LineBuffer*)mappedResource.pData; + //memcpy(dataPtr,&sb,sizeof(LineBuffer)); + //context->Unmap(lineBuffer,0); + + //context->VSSetConstantBuffers( 0, 1, &lineBuffer ); + + //UINT stride = sizeof( XMFLOAT3A ); + //UINT offset = 0; + //context->IASetVertexBuffers( 0, 1, &boneLines[i].vertexBuffer, &stride, &offset ); + + //context->Draw(2,0); + BindVertexBuffer(boneLines[i].vertexBuffer,0,sizeof(XMFLOAT3A),context); + Draw(2,context); + } + } +} +void Renderer::DrawDebugBoxes(const XMMATRIX& newView, ID3D11DeviceContext* context) +{ + if(debugBoxes){ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_LINELIST ); + //context->IASetInputLayout( lineIL ); + BindPrimitiveTopology(LINELIST,context); + BindVertexLayout(lineIL,context); + + //context->RSSetState(nonCullWireRS); + //context->OMSetDepthStencilState(xRayStencilState, STENCILREF_EMPTY); + + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + + BindRasterizerState(nonCullWireRS,context); + BindDepthStencilState(xRayStencilState,STENCILREF_EMPTY,context); + BindBlendState(blendState,context); + + + //context->VSSetShader( lineVS, NULL, 0 ); + //context->PSSetShader( linePS, NULL, 0 ); + BindPS(linePS,context); + BindVS(lineVS,context); + + //vector edges(0); + /*for(Object* object:objects){ + edges.push_back(Lines(object->frameBB.corners[0],object->frameBB.corners[1],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[1],object->frameBB.corners[2],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[0],object->frameBB.corners[3],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[0],object->frameBB.corners[4],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[1],object->frameBB.corners[5],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[4],object->frameBB.corners[5],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[5],object->frameBB.corners[6],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[4],object->frameBB.corners[7],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[2],object->frameBB.corners[6],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[3],object->frameBB.corners[7],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[2],object->frameBB.corners[3],XMFLOAT4A(1,0,0,1))); + edges.push_back(Lines(object->frameBB.corners[6],object->frameBB.corners[7],XMFLOAT4A(1,0,0,1))); + }*/ + + //iterateSPTree(spTree->root,edges); + + //UINT stride = sizeof( XMFLOAT3A ); + //UINT offset = 0; + //context->IASetVertexBuffers( 0, 1, &Cube::vertexBuffer, &stride, &offset ); + //context->IASetIndexBuffer(Cube::indexBuffer,DXGI_FORMAT_R32_UINT,0); + + BindVertexBuffer(Cube::vertexBuffer,0,sizeof(XMFLOAT3A),context); + BindIndexBuffer(Cube::indexBuffer,context); + BindConstantBufferVS(lineBuffer,0,context); + + for(int i=0;iProjection); + sb.color=cubes[i].desc.color; + + UpdateBuffer(lineBuffer,&sb,context); + //LineBuffer* dataPtr; + //context->Map(lineBuffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (LineBuffer*)mappedResource.pData; + //memcpy(dataPtr,&sb,sizeof(LineBuffer)); + //context->Unmap(lineBuffer,0); + + //context->VSSetConstantBuffers( 0, 1, &lineBuffer ); + + + //context->DrawIndexed(24,0,0); + DrawIndexed(24,context); + } + + } +} + +void Renderer::DrawSoftParticles(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, bool dark) +{ + static struct particlesystem_comparator { + bool operator() (const EmittedParticle* a, const EmittedParticle* b) const{ + return a->lastSquaredDistMulThousand>b->lastSquaredDistMulThousand; + } + }; + + set psystems; + for(map>::iterator iter=emitterSystems.begin();iter!=emitterSystems.end();++iter){ + for(EmittedParticle* e:iter->second){ + e->lastSquaredDistMulThousand=(long)(WickedMath::DistanceEstimated(e->bounding_box->getCenter(),XMFLOAT3(XMVectorGetX(eye),XMVectorGetY(eye),XMVectorGetZ(eye)))*1000); + psystems.insert(e); + } + } + + for(EmittedParticle* e:psystems){ + e->DrawNonPremul(eye,view,context,depth,dark); + } + + //for(int i=0;ieParticleSystems.size();j++){ + // Material* mat = objects[i]->eParticleSystems[j]->material; + // if(mat){ + // if(!mat->premultipliedTexture) objects[i]->eParticleSystems[j]->Draw(eye,view,mat->texture,context,depth,dark); + // } + // } + //} +} +void Renderer::DrawSoftPremulParticles(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, bool dark) +{ + for(map>::iterator iter=emitterSystems.begin();iter!=emitterSystems.end();++iter){ + for(EmittedParticle* e:iter->second) + e->DrawPremul(eye,view,context,depth,dark); + } + + //for(int i=0;ieParticleSystems.size();j++){ + // Material* mat = objects[i]->eParticleSystems[j]->material; + // if(mat){ + // if(mat->premultipliedTexture) objects[i]->eParticleSystems[j]->Draw(eye,view,mat->texture,context,depth,dark); + // } + // } + //} +} +void Renderer::DrawTrails(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes){ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLESTRIP ); + //context->IASetInputLayout( trailIL ); + BindPrimitiveTopology(TRIANGLESTRIP,context); + BindVertexLayout(trailIL,context); + + //context->RSSetState(wireRender?nonCullWireRS:nonCullRS); + //context->OMSetDepthStencilState(depthReadStencilState, STENCILREF_EMPTY); + + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + + BindRasterizerState(wireRender?nonCullWireRS:nonCullRS,context); + BindDepthStencilState(depthReadStencilState,STENCILREF_EMPTY,context); + BindBlendState(blendState,context); + + //context->VSSetShader( trailVS, NULL, 0 ); + //context->PSSetShader( trailPS, NULL, 0 ); + BindPS(trailPS,context); + BindVS(trailVS,context); + + BindTexturePS(refracRes,0,context); + BindTexturePS(trailDistortTex,1,context); + //context->PSSetSamplers( 0,1,&mirSampler ); + BindSamplerPS(mirSampler,0,context); + + for(int i=0;itrailBuff && everyObject[i]->trail.size()>=4){ + + + //context->VSSetConstantBuffers( 0, 1, &trailCB ); + BindConstantBufferVS(trailCB,0,context); + + vector trails; + //for(int k=0;ktrail.size();++k) + // trails.push_back(everyObject[i]->trail[k]); + + int bounds = everyObject[i]->trail.size(); + int req = bounds-3; + for(int k=0;ktrail[k?(k-2):0].pos ) + ,XMLoadFloat3( &everyObject[i]->trail[k].pos ) + ,XMLoadFloat3( &everyObject[i]->trail[k+2].pos ) + ,XMLoadFloat3( &everyObject[i]->trail[k+6trail[k?(k-1):1].pos ) + ,XMLoadFloat3( &everyObject[i]->trail[k+1].pos ) + ,XMLoadFloat3( &everyObject[i]->trail[k+3].pos ) + ,XMLoadFloat3( &everyObject[i]->trail[k+5trail[k].tex,everyObject[i]->trail[k+2].tex,r) + ,WickedMath::Lerp(everyObject[i]->trail[k].col,everyObject[i]->trail[k+2].col,r) + )); + trails.push_back(RibbonVertex(xpoint1 + ,WickedMath::Lerp(everyObject[i]->trail[k+1].tex,everyObject[i]->trail[k+3].tex,r) + ,WickedMath::Lerp(everyObject[i]->trail[k+1].col,everyObject[i]->trail[k+3].col,r) + )); + } + } + if(!trails.empty()){ + UpdateBuffer(everyObject[i]->trailBuff,trails.data(),context,sizeof(RibbonVertex)*trails.size()); + //D3D11_MAPPED_SUBRESOURCE mappedResource2; + //RibbonVertex* dataPtrV; + //context->Map(everyObject[i]->trailBuff,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource2); + //dataPtrV = (RibbonVertex*)mappedResource2.pData; + //memcpy(dataPtrV,trails.data(),sizeof(RibbonVertex)*trails.size()); + //context->Unmap(everyObject[i]->trailBuff,0); + + //UINT stride = sizeof( RibbonVertex ); + //UINT offset = 0; + //context->IASetVertexBuffers( 0, 1, &everyObject[i]->trailBuff, &stride, &offset ); + + + + //context->Draw(trails.size(),0); + BindVertexBuffer(everyObject[i]->trailBuff,0,sizeof(RibbonVertex),context); + Draw(trails.size(),context); + + trails.clear(); + } + } + } +} +void Renderer::DrawImagesAdd(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes){ + imagesRTAdd.Activate(context,0,0,0,1); + Image::BatchBegin(context); + for(oImage* x : images){ + if(x->effects.blendFlag==BLENDMODE_ADDITIVE){ + /*ID3D11ShaderResourceView* nor = x->effects.normalMap; + x->effects.setNormalMap(nullptr); + bool changedBlend=false; + if(x->effects.blendFlag==BLENDMODE_OPAQUE && nor){ + x->effects.blendFlag=BLENDMODE_ADDITIVE; + changedBlend=true; + }*/ + x->Draw(refracRes, context); + /*if(changedBlend) + x->effects.blendFlag=BLENDMODE_OPAQUE; + x->effects.setNormalMap(nor);*/ + } + } +} +void Renderer::DrawImages(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes){ + imagesRT.Activate(context,0,0,0,0); + Image::BatchBegin(context); + for(oImage* x : images){ + if(x->effects.blendFlag==BLENDMODE_ALPHA || x->effects.blendFlag==BLENDMODE_OPAQUE){ + /*ID3D11ShaderResourceView* nor = x->effects.normalMap; + x->effects.setNormalMap(nullptr); + bool changedBlend=false; + if(x->effects.blendFlag==BLENDMODE_OPAQUE && nor){ + x->effects.blendFlag=BLENDMODE_ADDITIVE; + changedBlend=true; + }*/ + x->Draw(refracRes, context); + /*if(changedBlend) + x->effects.blendFlag=BLENDMODE_OPAQUE; + x->effects.setNormalMap(nor);*/ + } + } +} +void Renderer::DrawImagesNormals(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes){ + normalMapRT.Activate(context,0,0,0,0); + Image::BatchBegin(context); + for(oImage* x : images){ + x->DrawNormal(context); + } +} +void Renderer::DrawLights(const XMMATRIX& newView, ID3D11DeviceContext* context + , ID3D11ShaderResourceView* depth, ID3D11ShaderResourceView* normal, ID3D11ShaderResourceView* material + , unsigned int stencilRef){ + + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + CulledList culledObjects; + if(spTree_lights) + SPTree::getVisible(spTree_lights->root,frustum,culledObjects); + + if(!culledObjects.empty()) + { + + + BindPrimitiveTopology(TRIANGLELIST,context); + BindConstantBufferVS(staticCb,0,context); + BindConstantBufferPS(lightStaticCb,0,context); + + BindTexturePS(depth,0,context); + BindTexturePS(normal,1,context); + BindTexturePS(material,2,context); + BindSamplerPS(pointSampler,0,context); + BindSamplerPS(comparisonSampler,1,context); + + + BindBlendState(blendStateAdd,context); + BindDepthStencilState(stencilReadState,stencilRef,context); + BindRasterizerState(backFaceRS,context); + + for(int type=0;type<3;++type){ + + + BindVS(lightVS[type],context); + BindPS(lightPS[type],context); + BindConstantBufferPS(lightCb[type],1,context); + BindConstantBufferVS(lightCb[type],1,context); + + + for(Cullable* c : culledObjects){ + Light* l = (Light*)c; + if(l->type==type){ + + switch(type){ + case 0:{ //dir + dLightBuffer lcb; + lcb.direction=XMVector3Normalize( + -XMVector3Transform( XMVectorSet(0,-1,0,1), XMMatrixRotationQuaternion( XMLoadFloat4(&l->rotation) ) ) + ); + lcb.col=XMFLOAT4(l->color.x*l->enerDis.x,l->color.y*l->enerDis.x,l->color.z*l->enerDis.x,1); + lcb.mBiasResSoftshadow=XMFLOAT4(shBias,SHADOWMAPRES,SOFTSHADOW,0); + for(int shmap=0;shmapshadowMap.size();++shmap){ + lcb.mShM[shmap]=l->shadowCam[shmap].getVP(); + BindTexturePS(l->shadowMap[shmap].depth->shaderResource,4+shmap,context); + } + UpdateBuffer(lightCb[type],&lcb,context); + + } + break; + case 1:{ //point + pLightBuffer lcb; + lcb.pos=l->translation; + lcb.col=l->color; + lcb.enerdis=l->enerDis; + lcb.enerdis.w=l->shadowMap.size(); + UpdateBuffer(lightCb[type],&lcb,context); + + if(!l->shadowMap.empty()) + BindTexturePS(l->shadowMap.front().depth->shaderResource,7,context); + } + break; + case 2:{ //spot + sLightBuffer lcb; + const float coneS=l->enerDis.z/0.7853981852531433; + XMMATRIX world,rot; + world = XMMatrixTranspose( + XMMatrixScaling(coneS*l->enerDis.y,l->enerDis.y,coneS*l->enerDis.y)* + XMMatrixRotationQuaternion( XMLoadFloat4( &l->rotation ) )* + XMMatrixTranslationFromVector( XMLoadFloat3(&l->translation) ) + ); + rot=XMMatrixRotationQuaternion( XMLoadFloat4(&l->rotation) ); + lcb.direction=XMVector3Normalize( + -XMVector3Transform( XMVectorSet(0,-1,0,1), rot ) + ); + lcb.world=world; + lcb.mBiasResSoftshadow=XMFLOAT4(shBias,SHADOWMAPRES,SOFTSHADOW,0); + lcb.mShM=l->shadowCam.size()?l->shadowCam[0].getVP():XMMatrixIdentity(); + lcb.col=l->color; + lcb.enerdis=l->enerDis; + lcb.enerdis.z=cos(l->enerDis.z/2.0); + UpdateBuffer(lightCb[type],&lcb,context); + + for(int shmap=0;shmapshadowMap.size();++shmap) + BindTexturePS(l->shadowMap[shmap].depth->shaderResource,4+shmap,context); + } + break; + default: + break; + } + + //context->DrawIndexed(lightGRenderer[Light::getTypeStr(type)]->indices.size(),0,0); + BindVertexBuffer(nullptr,0,0,context); + switch (l->type) + { + case Light::LightType::DIRECTIONAL: + //context->Draw(6,0); + Draw(6,context); + break; + case Light::LightType::SPOT: + //context->Draw(96,0); + Draw(192,context); + break; + case Light::LightType::POINT: + //context->Draw(240,0); + Draw(240,context); + break; + default: + break; + } + + } + } + + + } + + } +} +void Renderer::DrawVolumeLights(const XMMATRIX& newView, ID3D11DeviceContext* context) +{ + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + + CulledList culledObjects; + if(spTree_lights) + SPTree::getVisible(spTree_lights->root,frustum,culledObjects); + + if(!culledObjects.empty()) + { + + BindPrimitiveTopology(TRIANGLELIST,context); + BindVertexLayout(nullptr); + BindConstantBufferVS(staticCb,0,context); + BindBlendState(blendStateAdd,context); + BindDepthStencilState(depthReadStencilState,STENCILREF_DEFAULT,context); + BindRasterizerState(nonCullRS,context); + + + BindPS(vLightPS,context); + BindConstantBufferVS(vLightCb,1,context); + + + for(int type=1;type<3;++type){ + + + if(type<=1){ + BindVS(vPointLightVS,context); + } + else{ + BindVS(vSpotLightVS,context); + } + + for(Cullable* c : culledObjects){ + Light* l = (Light*)c; + if(l->type==type && l->noHalo==false){ + + vLightBuffer lcb; + XMMATRIX world; + float sca=1; + //if(type<1){ //sun + // sca = 10000; + // world = XMMatrixTranspose( + // XMMatrixScaling(sca,sca,sca)* + // XMMatrixRotationX(cam->updownRot)*XMMatrixRotationY(cam->leftrightRot)* + // XMMatrixTranslationFromVector( XMVector3Transform(cam->Eye+XMVectorSet(0,100000,0,0),XMMatrixRotationQuaternion(XMLoadFloat4(&l->rotation))) ) + // ); + //} + if(type==1){ //point + sca = l->enerDis.y*l->enerDis.x*0.01; + world = XMMatrixTranspose( + XMMatrixScaling(sca,sca,sca)* + XMMatrixRotationX(cam->updownRot)*XMMatrixRotationY(cam->leftrightRot)* + XMMatrixTranslationFromVector( XMLoadFloat3(&l->translation) ) + ); + } + else{ //spot + float coneS=l->enerDis.z/0.7853981852531433; + sca = l->enerDis.y*l->enerDis.x*0.03; + world = XMMatrixTranspose( + XMMatrixScaling(coneS*sca,sca,coneS*sca)* + XMMatrixRotationQuaternion( XMLoadFloat4( &l->rotation ) )* + XMMatrixTranslationFromVector( XMLoadFloat3(&l->translation) ) + ); + } + lcb.world=world; + lcb.col=l->color; + lcb.enerdis=l->enerDis; + lcb.enerdis.w=sca; + + UpdateBuffer(vLightCb,&lcb,context); + + if(type<=1) + Draw(108,context); + else + Draw(192,context); + } + } + + } + + } +} + + +void Renderer::DrawLensFlares(ID3D11DeviceContext* context, ID3D11ShaderResourceView* depth, const int& RENDERWIDTH, const int& RENDERHEIGHT){ + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,cam->View ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + + CulledList culledObjects; + if(spTree_lights) + SPTree::getVisible(spTree_lights->root,frustum,culledObjects); + + for(Cullable* c:culledObjects) + { + Light* l = (Light*)c; + + if(!l->lensFlareRimTextures.empty()) + { + + XMVECTOR POS; + + if(l->type==Light::POINT || l->type==Light::SPOT){ + POS=XMLoadFloat3(&l->translation); + } + + else{ + POS=XMVector3Normalize( + -XMVector3Transform( XMVectorSet(0,-1,0,1), XMMatrixRotationQuaternion( XMLoadFloat4(&l->rotation) ) ) + )*100000; + } + + XMVECTOR flarePos = XMVector3Project(POS,0,0,RENDERWIDTH,RENDERHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + + if( XMVectorGetX(XMVector3Dot( XMVectorSubtract(POS,cam->Eye),cam->At ))>0 ) + LensFlare::Draw(depth,context,flarePos,l->lensFlareRimTextures); + + } + + } +} +void Renderer::ClearShadowMaps(ID3D11DeviceContext* context){ + if(GetGameSpeed()) + for(Light* l : lights){ + for(int index=0;indexshadowMap.size();++index){ + l->shadowMap[index].Activate(context); + } + } +} +void Renderer::DrawForShadowMap(ID3D11DeviceContext* context) +{ + if(GameSpeed){ + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,cam->View ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + + CulledList culledLights; + if(spTree_lights) + SPTree::getVisible(spTree_lights->root,frustum,culledLights); + + if(culledLights.size()>0) + { + + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST ); + BindPrimitiveTopology(TRIANGLELIST,context); + BindVertexLayout(vertexLayout,context); + //context->IASetInputLayout( vertexLayout ); + + + //context->OMSetDepthStencilState(depthStencilState, STENCILREF_DEFAULT); + BindDepthStencilState(depthStencilState,STENCILREF_DEFAULT,context); + + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + BindBlendState(blendState,context); + + //context->PSSetShader( shPS, NULL, 0 ); + BindPS(shPS,context); + //context->PSSetSamplers(0, 1, &texSampler); + BindSamplerPS(texSampler,0,context); + + //context->VSSetShader( shVS, NULL, 0 ); + BindVS(shVS,context); + //context->VSSetConstantBuffers( 0, 1, &shCb ); + //context->VSSetConstantBuffers( 1, 1, &matCb ); + //context->PSSetConstantBuffers(1,1,&matCb); + BindConstantBufferVS(shCb,0,context); + BindConstantBufferVS(matCb,1,context); + BindConstantBufferPS(matCb,1,context); + //context->VSSetConstantBuffers(3,1,&matIndexBuf); + //context->PSSetConstantBuffers(3,1,&matIndexBuf); + + vector pointLightsSaved(0); + + //DIRECTIONAL AND SPOTLIGHT 2DSHADOWMAPS + for(Cullable* c : culledLights){ + Light* l=(Light*)c; + if(l->type!=Light::POINT){ + + for(int index=0;indexshadowMap.size();++index){ + l->shadowMap[index].Set(context); + + CulledCollection culledRenderer; + CulledList culledObjects; + + if(l->type==Light::DIRECTIONAL){ + const float siz = l->shadowCam[index].size * 0.5f; + const float f = l->shadowCam[index].farplane; + AABB boundingbox; + boundingbox.createFromHalfWidth(XMFLOAT3(0,0,0),XMFLOAT3(siz,f,siz)); + if(spTree) + SPTree::getVisible(spTree->root,boundingbox.get( + XMMatrixInverse(0,XMLoadFloat4x4(&l->shadowCam[index].View)) + ),culledObjects); + } + else if(l->type==Light::SPOT){ + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,XMLoadFloat4x4(&l->shadowCam[index].Projection) ); + XMStoreFloat4x4( &view,XMLoadFloat4x4(&l->shadowCam[index].View) ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + if(spTree) + SPTree::getVisible(spTree->root,frustum,culledObjects); + } + + if(!culledObjects.empty()){ + + for(Cullable* object : culledObjects){ + culledRenderer[((Object*)object)->mesh].insert((Object*)object); + } + + for (CulledCollection::iterator iter = culledRenderer.begin(); iter != culledRenderer.end(); ++iter) { + Mesh* mesh = iter->first; + CulledObjectList& visibleInstances = iter->second; + + if(visibleInstances.size() && !mesh->isBillboarded){ + + if(!mesh->doubleSided) + //context->RSSetState(rssh); + BindRasterizerState(rssh,context); + else + //context->RSSetState(nonCullRSsh); + BindRasterizerState(nonCullRSsh,context); + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + ForShadowMapCB cb; + cb.mViewProjection = l->shadowCam[index].getVP(); + cb.mWind=wind.direction; + cb.time=wind.time; + cb.windRandomness=wind.randomness; + cb.windWaveSize=wind.waveSize; + UpdateBuffer(shCb,&cb,context); + //ForShadowMapCB* dataPtr; + //context->Map(shCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (ForShadowMapCB*)mappedResource.pData; + //memcpy(dataPtr,&cb,sizeof(ForShadowMapCB)); + //context->Unmap(shCb,0); + +//#ifdef USE_GPU_SKINNING +// context->VSSetConstantBuffers( 1, 1, &mesh->boneBuffer ); +//#endif + + + int k=0; + for(CulledObjectList::iterator viter=visibleInstances.begin();viter!=visibleInstances.end();++viter){ + if((*viter)->particleEmitter!=Object::ParticleEmitter::EMITTER_INVISIBLE){ + if(mesh->softBody || (*viter)->armatureDeform) + mesh->instances[0][k] = Instance( XMMatrixIdentity() ); + else + mesh->instances[0][k]=Instance( + XMMatrixTranspose( XMLoadFloat4x4(&(*viter)->world) ) + ); + ++k; + } + } + if(k<1) + continue; + + UpdateBuffer(mesh->meshInstanceBuffer,mesh->instances[0].data(),context,sizeof(Instance)*visibleInstances.size()); + + + //ID3D11Buffer* vertexBuffers[2] = { + // (mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff) + // ,mesh->meshInstanceBuffer}; + //UINT strides[2] = {sizeof( Vertex ),sizeof(Instance)}; + //UINT offsets[2] = {0,0}; + //context->IASetVertexBuffers( 0, 2, vertexBuffers, strides, offsets ); + //context->IASetIndexBuffer(mesh->meshIndexBuff,DXGI_FORMAT_R32_UINT,0); + + BindVertexBuffer((mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff),0,sizeof(Vertex),context); + BindVertexBuffer(mesh->meshInstanceBuffer,1,sizeof(Instance),context); + BindIndexBuffer(mesh->meshIndexBuff,context); + + + int matsiz = mesh->materialIndices.size(); + int m=0; + for(Material* iMat : mesh->materials){ + + if(!wireRender && !iMat->isSky && !iMat->water && iMat->cast_shadow) { + BindTexturePS(iMat->texture,0,context); + + + //MaterialCB* mcb = (MaterialCB*)_aligned_malloc(sizeof(MaterialCB),16); + //mcb->difColor=XMFLOAT4A(iMat->diffuseColor.x,iMat->diffuseColor.y,iMat->diffuseColor.z,iMat->alpha); + //mcb->hasRefNorTexSpe=XMFLOAT4A(iMat->hasRefMap,iMat->hasNormalMap,iMat->hasTexture,iMat->hasSpecularMap); + //mcb->specular=iMat->specular; + //mcb->refractionIndexMovingTexEnv=XMFLOAT4A(iMat->refraction_index,iMat->texOffset.x,iMat->texOffset.y,iMat->enviroReflection); + //mcb->shadeless=iMat->shadeless; + //mcb->specular_power=iMat->specular_power; + //mcb->toon=iMat->toonshading; + //mcb->matIndex=m; + //mcb->emit=iMat->emit; + + MaterialCB* mcb = new MaterialCB(*iMat,m); + + UpdateBuffer(matCb,mcb,context); + + delete mcb; + + //MaterialCB* MdataPtr; + //context->Map(matCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //MdataPtr = (MaterialCB*)mappedResource.pData; + //memcpy(MdataPtr,&mcb,sizeof(MaterialCB)); + //context->Unmap(matCb,0); + + //MatIndexBuf ib; + //ib.matIndex=m; + //ib.padding=XMFLOAT3(0,0,0); + + //UpdateBuffer(matIndexBuf,&ib,context); + //MatIndexBuf* IdataPtr; + //context->Map(matIndexBuf,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //IdataPtr = (MatIndexBuf*)mappedResource.pData; + //memcpy(IdataPtr,&ib,sizeof(MatIndexBuf)); + //context->Unmap(matIndexBuf,0); + + //context->DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),0,0,0); + DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),context); + + m++; + } + } + } + } + + } + } + } + else{ + pointLightsSaved.push_back(l); + } + } + + if(!pointLightsSaved.empty() && POINTLIGHTSHADOW){ + + + set orderedLights; + for(Light* l : pointLightsSaved){ + l->lastSquaredDistMulThousand=(long)(WickedMath::DistanceEstimated(l->translation,XMFLOAT3(XMVectorGetX(cam->Eye),XMVectorGetY(cam->Eye),XMVectorGetZ(cam->Eye)))*1000); + orderedLights.insert(l); + } + + int cube_shadowrenders_remain = POINTLIGHTSHADOW; + + BindPS(cubeShPS,context); + BindVS(cubeShVS,context); + BindGS(cubeShGS,context); + BindConstantBufferGS(cubeShCb,0,context); + BindConstantBufferPS(lightCb[1],2,context); + for(Light* l : orderedLights){ + + for(int index=0;indexshadowMap.size();++index){ + if(cube_shadowrenders_remain<=0) + break; + cube_shadowrenders_remain-=1; + l->shadowMap[index].Set(context); + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,XMLoadFloat4x4(&l->shadowCam[index].Projection) ); + XMStoreFloat4x4( &view,XMLoadFloat4x4(&l->shadowCam[index].View) ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + pLightBuffer lcb; + lcb.enerdis=l->enerDis; + lcb.pos=l->translation; + UpdateBuffer(lightCb[1],&lcb,context); + //pLightBuffer* dataPtr; + //context->Map(lightCb[1],0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (pLightBuffer*)mappedResource.pData; + //memcpy(dataPtr,&lcb,sizeof(pLightBuffer)); + //context->Unmap(lightCb[1],0); + + CubeShadowCb cb; + for(int shcam=0;shcamshadowCam.size();++shcam) + cb.mViewProjection[shcam] = l->shadowCam[shcam].getVP(); + + UpdateBuffer(cubeShCb,&cb,context); + //CubeShadowCb* dataPtr1; + //context->Map(cubeShCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr1 = (CubeShadowCb*)mappedResource.pData; + //memcpy(dataPtr1,&cb,sizeof(CubeShadowCb)); + //context->Unmap(cubeShCb,0); + + CulledCollection culledRenderer; + CulledList culledObjects; + + if(spTree) + SPTree::getVisible(spTree->root,l->bounds,culledObjects); + + for(Cullable* object : culledObjects) + culledRenderer[((Object*)object)->mesh].insert((Object*)object); + + for (CulledCollection::iterator iter = culledRenderer.begin(); iter != culledRenderer.end(); ++iter) { + Mesh* mesh = iter->first; + CulledObjectList& visibleInstances = iter->second; + + if(!mesh->isBillboarded && !visibleInstances.empty()){ + + if(!mesh->doubleSided) + //context->RSSetState(rssh); + BindRasterizerState(rssh,context); + else + //context->RSSetState(nonCullRSsh); + BindRasterizerState(nonCullRSsh,context); + + + + int k=0; + for(CulledObjectList::iterator viter=visibleInstances.begin();viter!=visibleInstances.end();++viter){ + if((*viter)->particleEmitter!=Object::ParticleEmitter::EMITTER_INVISIBLE){ + if(mesh->softBody || (*viter)->armatureDeform) + mesh->instances[0][k] = Instance( XMMatrixIdentity() ); + else + mesh->instances[0][k]=Instance( + XMMatrixTranspose( XMLoadFloat4x4(&(*viter)->world) ) + ); + ++k; + } + } + if(k<1) + continue; + + UpdateBuffer(mesh->meshInstanceBuffer,mesh->instances[0].data(),context,sizeof(Instance)*visibleInstances.size()); + + BindVertexBuffer((mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff),0,sizeof(Vertex),context); + BindVertexBuffer(mesh->meshInstanceBuffer,1,sizeof(Instance),context); + BindIndexBuffer(mesh->meshIndexBuff,context); + + + int matsiz = mesh->materialIndices.size(); + int m=0; + for(Material* iMat : mesh->materials){ + if(!wireRender && !iMat->isSky && !iMat->water && iMat->cast_shadow) { + BindTexturePS(iMat->texture,0,context); + + //MaterialCB* mcb = (MaterialCB*)_aligned_malloc(sizeof(MaterialCB),16); + //mcb->difColor=XMFLOAT4A(iMat->diffuseColor.x,iMat->diffuseColor.y,iMat->diffuseColor.z,iMat->alpha); + //mcb->hasRefNorTexSpe=XMFLOAT4A(iMat->hasRefMap,iMat->hasNormalMap,iMat->hasTexture,iMat->hasSpecularMap); + //mcb->specular=iMat->specular; + //mcb->refractionIndexMovingTexEnv=XMFLOAT4A(iMat->refraction_index,iMat->texOffset.x,iMat->texOffset.y,iMat->enviroReflection); + //mcb->shadeless=iMat->shadeless; + //mcb->specular_power=iMat->specular_power; + //mcb->toon=iMat->toonshading; + //mcb->matIndex=m; + //mcb->emit=iMat->emit; + MaterialCB* mcb = new MaterialCB(*iMat,m); + + UpdateBuffer(matCb,mcb,context); + delete mcb; + + DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),context); + } + m++; + } + } + visibleInstances.clear(); + } + } + } + } + } + + //context->GSSetShader(0,0,0); + BindGS(nullptr,context); + } +} +void Renderer::DrawForSO(ID3D11DeviceContext* context) +{ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_POINTLIST ); + BindPrimitiveTopology(POINTLIST,context); + BindVertexLayout(sOIL,context); + //context->IASetInputLayout( sOIL ); + //context->VSSetShader( sOVS, NULL, 0 ); + //context->GSSetShader( sOGS, NULL, 0 ); + BindVS(sOVS,context); + BindGS(sOGS,context); + //context->PSSetShader( 0, NULL, 0); + BindPS(nullptr,context); + + for(MeshCollection::iterator iter=meshes.begin(); iter!=meshes.end(); ++iter){ + Mesh* mesh = iter->second; + if(mesh->hasArmature() && !mesh->softBody){ + //UINT stride = sizeof( SkinnedVertex ); + //UINT offset = 0; + //context->IASetVertexBuffers( 0, 1, &mesh->meshVertBuff, &stride, &offset ); + //context->IASetIndexBuffer(mesh->meshIndexBuff,DXGI_FORMAT_R32_UINT,0); + BindVertexBuffer(mesh->meshVertBuff,0,sizeof(SkinnedVertex),context); + //context->VSSetConstantBuffers( 1, 1, &mesh->boneBuffer ); + BindConstantBufferVS(mesh->boneBuffer,1,context); + //UINT offsetSO[1] = {0}; + //context->SOSetTargets(1,&mesh->sOutBuffer,offsetSO); + BindStreamOutTarget(mesh->sOutBuffer,context); + //context->Draw(mesh->vertices.size(),0); + Draw(mesh->vertices.size(),context); + //context->DrawAuto(); + //context->DrawIndexed(mesh->indices.size(),0,0); + } + } + + //context->GSSetShader( 0, NULL, 0 ); + //context->VSSetShader( 0, NULL, 0 ); + BindGS(nullptr,context); + BindVS(nullptr,context); + //UINT offsetSO[1] = {0}; + //context->SOSetTargets(0,nullptr,offsetSO); + BindStreamOutTarget(nullptr,context); +} + +void Renderer::DrawWorld(const XMMATRIX& newView, bool DX11Eff, int tessF, ID3D11DeviceContext* context + , bool BlackOut, bool shaded + , ID3D11ShaderResourceView* refRes, bool grass, int passIdentifier) +{ + + if(objects.empty()) + return; + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + CulledCollection culledRenderer; + CulledList culledObjects; + if(spTree) + SPTree::getVisible(spTree->root,frustum,culledObjects); + //SPTree::getAll(spTree->root,culledObjects); + + if(!culledObjects.empty()) + { + + for(Cullable* object : culledObjects){ + culledRenderer[((Object*)object)->mesh].insert((Object*)object); + if(grass){ + for(HairParticle* hair : ((Object*)object)->hParticleSystems){ + XMFLOAT3 eye; + XMStoreFloat3(&eye,cam->Eye); + hair->Draw(eye,newView,cam->Projection,context); + } + } + } + + if(DX11Eff && tessF) //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_3_CONTROL_POINT_PATCHLIST ); + BindPrimitiveTopology(PATCHLIST,context); + else //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST ); + BindPrimitiveTopology(TRIANGLELIST,context); + //context->IASetInputLayout( vertexLayout ); + BindVertexLayout(vertexLayout,context); + + if(DX11Eff && tessF) //context->VSSetShader( vertexShader, NULL, 0 ); + BindVS(vertexShader,context); + else //context->VSSetShader( vertexShader10, NULL, 0 ); + BindVS(vertexShader10,context); + if(DX11Eff && tessF) //context->HSSetShader( hullShader, NULL, 0 ); + BindHS(hullShader,context); + else //context->HSSetShader( 0, NULL, 0 ); + BindHS(nullptr,context); + if(DX11Eff && tessF) //context->DSSetShader( domainShader, NULL, 0 ); + BindDS(domainShader,context); + else //context->DSSetShader( 0, NULL, 0 ); + BindDS(nullptr,context); + BindPS( wireRender?simplestPS:(BlackOut?blackoutPS:(shaded?pixelShader:textureonlyPS)),context); + + + //context->VSSetSamplers(0, 1, &texSampler); + //context->VSSetShaderResources(0,1,&noiseTex); + BindSamplerVS(texSampler,0,context); + BindTextureVS(noiseTex,0,context); + if(DX11Eff && tessF) //context->DSSetSamplers(0, 1, &texSampler); + BindSamplerDS(texSampler,0,context); + BindSamplerPS(texSampler,0,context); + BindSamplerPS(mapSampler,1,context); + + + BindConstantBufferVS(staticCb,0,context); + if(DX11Eff && tessF){ + BindConstantBufferDS(staticCb,0,context); + BindConstantBufferDS(constantBuffer,3,context); + BindConstantBufferHS(tessBuf,0,context); + } + BindConstantBufferPS(pixelCB,0,context); + BindConstantBufferPS(fxCb,1,context); + BindConstantBufferVS(constantBuffer,3,context); + BindConstantBufferVS(matCb,2,context); + BindConstantBufferPS(matCb,2,context); + + //if(DX11Eff) context->DSSetConstantBuffers(3,1,&matIndexBuf); + + + //float blendFactor[4] = { 1.0f, 1.0f, 1.0f, 1.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + BindBlendState(blendState,context); + + if(!wireRender) { + BindTexturePS(enviroMap,0,context); + if(refRes != nullptr) + BindTexturePS(refRes,1,context); + } + + + for (CulledCollection::iterator iter = culledRenderer.begin(); iter != culledRenderer.end(); ++iter) { + Mesh* mesh = iter->first; + CulledObjectList& visibleInstances = iter->second; + + + if(!mesh->doubleSided) + //context->RSSetState(wireRender?wireRS:rasterizerState); + BindRasterizerState(wireRender?wireRS:rasterizerState,context); + else + //context->RSSetState(wireRender?wireRS:nonCullRS); + BindRasterizerState(wireRender?wireRS:nonCullRS,context); + + int matsiz = mesh->materialIndices.size(); + + if(DX11 && tessF){ + ConstantBuffer cb; + if(matsiz == 1) cb.mDisplace = XMVectorSet(mesh->materials[0]->hasDisplacementMap,0,0,0); + else if(matsiz == 2) cb.mDisplace = XMVectorSet(mesh->materials[0]->hasDisplacementMap,mesh->materials[1]->hasDisplacementMap,0,0); + else if(matsiz == 3) cb.mDisplace = XMVectorSet(mesh->materials[0]->hasDisplacementMap,mesh->materials[1]->hasDisplacementMap,mesh->materials[2]->hasDisplacementMap,0); + else if(matsiz == 4) cb.mDisplace = XMVectorSet(mesh->materials[0]->hasDisplacementMap,mesh->materials[1]->hasDisplacementMap,mesh->materials[2]->hasDisplacementMap,mesh->materials[3]->hasDisplacementMap); + + UpdateBuffer(constantBuffer,&cb,context); + } + + + //Instance* instanceData=new Instance[visibleInstances.size()]; + //int k=0; + //for(CulledObjectList::iterator viter=visibleInstances.begin();viter!=visibleInstances.end();++viter){ + // if((*viter)->particleEmitter!=Object::ParticleEmitter::EMITTER_INVISIBLE){ + // if(mesh->softBody || (*viter)->armatureDeform) + // instanceData[k] = Instance( XMMatrixIdentity() ); + // else + // instanceData[k]=Instance( + // XMMatrixTranspose( XMLoadFloat4x4(&(*viter)->world) ) + // ); + // ++k; + // } + //} + //if(k<1) + // continue; + + //UpdateBuffer(mesh->meshInstanceBuffer,instanceData,context,sizeof(Instance)*visibleInstances.size()); + //delete[visibleInstances.size()] instanceData; + + + + int k=0; + for(CulledObjectList::iterator viter=visibleInstances.begin();viter!=visibleInstances.end();++viter){ + if((*viter)->particleEmitter!=Object::ParticleEmitter::EMITTER_INVISIBLE){ + if(mesh->softBody || (*viter)->armatureDeform) + mesh->instances[passIdentifier][k] = Instance( XMMatrixIdentity() ); + else + mesh->instances[passIdentifier][k]=Instance( + XMMatrixTranspose( XMLoadFloat4x4(&(*viter)->world) ) + ); + ++k; + } + } + if(k<1) + continue; + + UpdateBuffer(mesh->meshInstanceBuffer,mesh->instances[passIdentifier].data(),context,sizeof(Instance)*visibleInstances.size()); + + + + //ID3D11Buffer* vertexBuffers[2] = { + // (mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff) + // ,mesh->meshInstanceBuffer}; + //UINT strides[2] = {sizeof( Vertex ),sizeof(Instance)}; + //UINT offsets[2] = {0,0}; + //context->IASetVertexBuffers( 0, 2, vertexBuffers, strides, offsets ); + //context->IASetIndexBuffer(mesh->meshIndexBuff,DXGI_FORMAT_R32_UINT,0); + BindIndexBuffer(mesh->meshIndexBuff,context); + BindVertexBuffer((mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff),0,sizeof(Vertex),context); + BindVertexBuffer(mesh->meshInstanceBuffer,1,sizeof(Instance),context); + + int m=0; + for(Material* iMat : mesh->materials){ + + if(!iMat->transparent && !iMat->isSky && !iMat->water){ + + if(iMat->shadeless) + //context->OMSetDepthStencilState(depthStencilState, STENCILREF_SHADELESS); + BindDepthStencilState(depthStencilState,STENCILREF_SHADELESS,context); + if(iMat->subsurface_scattering) + //context->OMSetDepthStencilState(depthStencilState, STENCILREF_SKIN); + BindDepthStencilState(depthStencilState,STENCILREF_SKIN,context); + else + //context->OMSetDepthStencilState(depthStencilState, mesh->stencilRef); + BindDepthStencilState(depthStencilState,mesh->stencilRef,context); + + //MaterialCB* mcb = (MaterialCB*)_aligned_malloc(sizeof(MaterialCB),16); + //mcb->difColor=XMFLOAT4A(iMat->diffuseColor.x,iMat->diffuseColor.y,iMat->diffuseColor.z,iMat->alpha); + //mcb->hasRefNorTexSpe=XMFLOAT4A(iMat->hasRefMap && refRes,iMat->hasNormalMap,iMat->hasTexture,iMat->hasSpecularMap); + //mcb->specular=iMat->specular; + //mcb->refractionIndexMovingTexEnv=XMFLOAT4A(iMat->refraction_index,iMat->texOffset.x,iMat->texOffset.y,iMat->enviroReflection); + //mcb->shadeless=iMat->shadeless; + //mcb->specular_power=iMat->specular_power; + //mcb->toon=iMat->toonshading; + //mcb->matIndex=m; + //mcb->emit=iMat->emit; + + MaterialCB* mcb = new MaterialCB(*iMat,m); + + UpdateBuffer(matCb,mcb,context); + _aligned_free(mcb); + + if(!wireRender) BindTexturePS(iMat->texture,3,context); + if(!wireRender) BindTexturePS(iMat->refMap,4,context); + if(!wireRender) BindTexturePS(iMat->normalMap,5,context); + if(!wireRender) BindTexturePS(iMat->specularMap,6,context); + if(DX11Eff) //context->DSSetShaderResources(0,1,&iMat->displacementMap); + BindTextureDS(iMat->displacementMap,0,context); + + //MatIndexBuf ib; + //ib.matIndex=m; + //ib.padding=XMFLOAT3(0,0,0); + + //UpdateBuffer(matIndexBuf,&ib,context); + + //context->DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),0,0,0); + + DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),context); + } + m++; + } + + } + + + //context->VSSetShader( 0, NULL, 0 ); + //context->HSSetShader( 0, NULL, 0 ); + //context->DSSetShader( 0, NULL, 0 ); + //context->PSSetShader( 0, NULL, 0 ); + BindPS(nullptr,context); + BindVS(nullptr,context); + BindDS(nullptr,context); + BindHS(nullptr,context); + + } + +} +void Renderer::DrawWorldWater(const XMMATRIX& newView, ID3D11ShaderResourceView* refracRes, ID3D11ShaderResourceView* refRes + , ID3D11ShaderResourceView* depth, ID3D11ShaderResourceView* nor, ID3D11DeviceContext* context, int passIdentifier){ + + if(objects_water.empty()) + return; + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + CulledCollection culledRenderer; + CulledList culledObjects; + if(spTree_water) + SPTree::getVisible(spTree_water->root,frustum,culledObjects); + + if(!culledObjects.empty()) + { + for(Cullable* object : culledObjects) + culledRenderer[((Object*)object)->mesh].insert((Object*)object); + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST ); + BindPrimitiveTopology(TRIANGLELIST,context); + BindVertexLayout(vertexLayout,context); + //context->IASetInputLayout( vertexLayout ); + //context->PSSetShader( wireRender?simplestPS:waterPS, NULL, 0 ); + BindPS(wireRender?simplestPS:waterPS,context); + //context->VSSetShader( waterVS, NULL, 0 ); + BindVS(vertexShader10,context); + + //context->PSSetSamplers(0, 1, &texSampler); + //context->PSSetSamplers(1, 1, &mapSampler); + BindSamplerPS(texSampler,0,context); + BindSamplerPS(mapSampler,1,context); + + if(!wireRender) BindTexturePS(enviroMap,0,context); + if(!wireRender) BindTexturePS(refRes,1,context); + if(!wireRender) BindTexturePS(refracRes,2,context); + if(!wireRender) BindTexturePS(depth,7,context); + if(!wireRender) BindTexturePS(nor,8,context); + + //context->VSSetConstantBuffers(0,1,&staticCb); + + //context->PSSetConstantBuffers( 0, 1, &pixelCB ); + //context->PSSetConstantBuffers( 1, 1, &fxCb ); + + BindConstantBufferVS(staticCb,0,context); + BindConstantBufferPS(pixelCB,0,context); + BindConstantBufferPS(fxCb,1,context); + BindConstantBufferVS(constantBuffer,3,context); + BindConstantBufferVS(matCb,2,context); + BindConstantBufferPS(matCb,2,context); + + + //float blendFactor[4] = { 1.0f, 1.0f, 1.0f, 1.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + BindBlendState(blendState,context); + //if(opaque) context->OMSetBlendState(blendState, blendFactor, sampleMask); + //if(transparent) context->OMSetBlendState(blendStateTransparent, blendFactor, sampleMask); + //context->OMSetDepthStencilState(depthReadStencilState, STENCILREF_EMPTY); + //context->RSSetState(wireRender?wireRS:rasterizerState); + BindDepthStencilState(depthReadStencilState,STENCILREF_EMPTY,context); + BindRasterizerState(wireRender?wireRS:rasterizerState,context); + + //context->VSSetConstantBuffers( 2, 1, &matCb ); + //context->PSSetConstantBuffers( 2, 1, &matCb ); + //context->VSSetConstantBuffers(3,1,&matIndexBuf); + //context->PSSetConstantBuffers(3,1,&matIndexBuf); + + + for (CulledCollection::iterator iter = culledRenderer.begin(); iter != culledRenderer.end(); ++iter) { + Mesh* mesh = iter->first; + CulledObjectList& visibleInstances = iter->second; + + + int matsiz = mesh->materialIndices.size(); + + + int k=0; + for(CulledObjectList::iterator viter=visibleInstances.begin();viter!=visibleInstances.end();++viter){ + if((*viter)->particleEmitter!=Object::ParticleEmitter::EMITTER_INVISIBLE){ + if(mesh->softBody || (*viter)->armatureDeform) + mesh->instances[passIdentifier][k] = Instance( XMMatrixIdentity() ); + else + mesh->instances[passIdentifier][k]=Instance( + XMMatrixTranspose( XMLoadFloat4x4(&(*viter)->world) ) + ); + ++k; + } + } + if(k<1) + continue; + + UpdateBuffer(mesh->meshInstanceBuffer,mesh->instances[passIdentifier].data(),context,sizeof(Instance)*visibleInstances.size()); + + + //ID3D11Buffer* vertexBuffers[2] = { + // (mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff) + // ,mesh->meshInstanceBuffer}; + //UINT strides[2] = {sizeof( Vertex ),sizeof(Instance)}; + //UINT offsets[2] = {0,0}; + //context->IASetVertexBuffers( 0, 2, vertexBuffers, strides, offsets ); + //context->IASetIndexBuffer(mesh->meshIndexBuff,DXGI_FORMAT_R32_UINT,0); + BindIndexBuffer(mesh->meshIndexBuff,context); + BindVertexBuffer((mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff),0,sizeof(Vertex),context); + BindVertexBuffer(mesh->meshInstanceBuffer,1,sizeof(Instance),context); + + int m=0; + for(Material* iMat : mesh->materials){ + + if(iMat->water){ + + //MaterialCB* mcb = (MaterialCB*)_aligned_malloc(sizeof(MaterialCB),16); + //mcb->difColor=XMFLOAT4(iMat->diffuseColor.x,iMat->diffuseColor.y,iMat->diffuseColor.z,iMat->alpha); + //mcb->hasRefNorTexSpe=XMFLOAT4(iMat->hasRefMap,iMat->hasNormalMap,iMat->hasTexture,iMat->hasSpecularMap); + //mcb->specular=iMat->specular; + //mcb->refractionIndexMovingTexEnv=XMFLOAT4(iMat->refraction_index,iMat->texOffset.x,iMat->texOffset.y,iMat->enviroReflection); + //mcb->shadeless=iMat->shadeless; + //mcb->specular_power=iMat->specular_power; + //mcb->toon=iMat->toonshading; + //mcb->matIndex=m; + //mcb->emit=iMat->emit; + MaterialCB* mcb = new MaterialCB(*iMat,m); + + UpdateBuffer(matCb,mcb,context); + _aligned_free(mcb); + + if(!wireRender) BindTexturePS(iMat->texture,3,context); + if(!wireRender) BindTexturePS(iMat->refMap,4,context); + if(!wireRender) BindTexturePS(iMat->normalMap,5,context); + if(!wireRender) BindTexturePS(iMat->specularMap,6,context); + + //MatIndexBuf ib; + //ib.matIndex=m; + //ib.padding=XMFLOAT3(0,0,0); + + //UpdateBuffer(matIndexBuf,&ib,context); + + //context->DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),0,0,0); + + DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),context); + } + m++; + } + } + + } + +} +void Renderer::DrawWorldTransparent(const XMMATRIX& newView, ID3D11ShaderResourceView* refracRes, ID3D11ShaderResourceView* refRes + , ID3D11ShaderResourceView* depth, ID3D11DeviceContext* context, int passIdentifier){ + + if(objects_trans.empty()) + return; + + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + CulledCollection culledRenderer; + CulledList culledObjects; + if(spTree_trans) + SPTree::getVisible(spTree_trans->root,frustum,culledObjects); + + if(!culledObjects.empty()) + { + for(Cullable* object : culledObjects) + culledRenderer[((Object*)object)->mesh].insert((Object*)object); + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST ); + BindPrimitiveTopology(TRIANGLELIST,context); + BindVertexLayout(vertexLayout,context); + //context->IASetInputLayout( vertexLayout ); + //context->PSSetShader( wireRender?simplestPS:transparentPS, NULL, 0 ); + BindPS(wireRender?simplestPS:transparentPS,context); + //context->VSSetShader( vertexShader10, NULL, 0 ); + BindVS(vertexShader10,context); + + //context->PSSetSamplers(0, 1, &texSampler); + //context->PSSetSamplers(1, 1, &mapSampler); + BindSamplerPS(texSampler,0,context); + BindSamplerPS(mapSampler,1,context); + + if(!wireRender) BindTexturePS(enviroMap,0,context); + if(!wireRender) BindTexturePS(refRes,1,context); + if(!wireRender) BindTexturePS(refracRes,2,context); + if(!wireRender) BindTexturePS(depth,7,context); + + + //context->VSSetConstantBuffers(0,1,&staticCb); + //context->PSSetConstantBuffers( 0, 1, &pixelCB ); + //context->PSSetConstantBuffers( 1, 1, &fxCb ); + + BindConstantBufferVS(staticCb,0,context); + BindConstantBufferPS(pixelCB,0,context); + BindConstantBufferPS(fxCb,1,context); + BindConstantBufferVS(constantBuffer,3,context); + BindConstantBufferVS(matCb,2,context); + BindConstantBufferPS(matCb,2,context); + + + //float blendFactor[4] = { 1.0f, 1.0f, 1.0f, 1.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + BindBlendState(blendState,context); + //if(opaque) context->OMSetBlendState(blendState, blendFactor, sampleMask); + //if(transparent) context->OMSetBlendState(blendStateTransparent, blendFactor, sampleMask); + //context->OMSetDepthStencilState(depthStencilState, STENCILREF_EMPTY); + //context->RSSetState(wireRender?wireRS:rasterizerState); + BindDepthStencilState(depthStencilState,STENCILREF_TRANSPARENT,context); + BindRasterizerState(wireRender?wireRS:rasterizerState,context); + + //context->VSSetConstantBuffers( 2, 1, &matCb ); + //context->PSSetConstantBuffers( 2, 1, &matCb ); + //context->VSSetConstantBuffers(3,1,&matIndexBuf); + //context->PSSetConstantBuffers(3,1,&matIndexBuf); + + for (CulledCollection::iterator iter = culledRenderer.begin(); iter != culledRenderer.end(); ++iter) { + Mesh* mesh = iter->first; + CulledObjectList& visibleInstances = iter->second; + + + + if(!mesh->doubleSided) + context->RSSetState(wireRender?wireRS:rasterizerState); + else + context->RSSetState(wireRender?wireRS:nonCullRS); + + int matsiz = mesh->materialIndices.size(); + + + + int k=0; + for(CulledObjectList::iterator viter=visibleInstances.begin();viter!=visibleInstances.end();++viter){ + if((*viter)->particleEmitter!=Object::ParticleEmitter::EMITTER_INVISIBLE){ + if(mesh->softBody || (*viter)->armatureDeform) + mesh->instances[passIdentifier][k] = Instance( XMMatrixIdentity() ); + else + mesh->instances[passIdentifier][k]=Instance( + XMMatrixTranspose( XMLoadFloat4x4(&(*viter)->world) ) + ); + ++k; + } + } + if(k<1) + continue; + + UpdateBuffer(mesh->meshInstanceBuffer,mesh->instances[passIdentifier].data(),context,sizeof(Instance)*visibleInstances.size()); + + + + //ID3D11Buffer* vertexBuffers[2] = { + // (mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff) + // ,mesh->meshInstanceBuffer}; + //UINT strides[2] = {sizeof( Vertex ),sizeof(Instance)}; + //UINT offsets[2] = {0,0}; + //context->IASetVertexBuffers( 0, 2, vertexBuffers, strides, offsets ); + //context->IASetIndexBuffer(mesh->meshIndexBuff,DXGI_FORMAT_R32_UINT,0); + BindIndexBuffer(mesh->meshIndexBuff,context); + BindVertexBuffer((mesh->sOutBuffer?mesh->sOutBuffer:mesh->meshVertBuff),0,sizeof(Vertex),context); + BindVertexBuffer(mesh->meshInstanceBuffer,1,sizeof(Instance),context); + + //#ifdef USE_GPU_SKINNING + // context->VSSetConstantBuffers( 1, 1, &mesh->boneBuffer ); + //#endif + + int m=0; + for(Material* iMat : mesh->materials){ + + if(iMat->transparent && iMat->alpha>0 && !iMat->water && !iMat->isSky){ + + //MaterialCB* mcb = (MaterialCB*)_aligned_malloc(sizeof(MaterialCB),16); + //mcb->difColor=XMFLOAT4(iMat->diffuseColor.x,iMat->diffuseColor.y,iMat->diffuseColor.z,iMat->alpha); + //mcb->hasRefNorTexSpe=XMFLOAT4(iMat->hasRefMap,iMat->hasNormalMap,iMat->hasTexture,iMat->hasSpecularMap); + //mcb->specular=iMat->specular; + //mcb->refractionIndexMovingTexEnv=XMFLOAT4(iMat->refraction_index,iMat->texOffset.x,iMat->texOffset.y,iMat->enviroReflection); + //mcb->shadeless=iMat->shadeless; + //mcb->specular_power=iMat->specular_power; + //mcb->toon=iMat->toonshading; + //mcb->matIndex=m; + //mcb->emit=iMat->emit; + + MaterialCB* mcb = new MaterialCB(*iMat,m); + + UpdateBuffer(matCb,mcb,context); + _aligned_free(mcb); + + if(!wireRender) BindTexturePS(iMat->texture,3,context); + if(!wireRender) BindTexturePS(iMat->refMap,4,context); + if(!wireRender) BindTexturePS(iMat->normalMap,5,context); + if(!wireRender) BindTexturePS(iMat->specularMap,6,context); + + //MatIndexBuf ib; + //ib.matIndex=m; + //ib.padding=XMFLOAT3(0,0,0); + + //UpdateBuffer(matIndexBuf,&ib,context); + + //context->DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),0,0,0); + + DrawIndexedInstanced(mesh->indices.size(),visibleInstances.size(),context); + } + m++; + } + } + } + +} + + +void Renderer::DrawSky(const XMVECTOR& newCenter, ID3D11DeviceContext* context) +{ + //context->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST ); + BindPrimitiveTopology(TRIANGLELIST,context); + //context->RSSetState(backFaceRS); + //context->OMSetDepthStencilState(depthReadStencilState, STENCILREF_SKY); + //float blendFactor[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + //UINT sampleMask = 0xffffffff; + //context->OMSetBlendState(blendState, blendFactor, sampleMask); + BindRasterizerState(backFaceRS,context); + BindDepthStencilState(depthReadStencilState,STENCILREF_SKY,context); + BindBlendState(blendState,context); + + //context->VSSetShader( skyVS, NULL, 0 ); + //context->PSSetShader( skyPS, NULL, 0 ); + BindVS(skyVS,context); + BindPS(skyPS,context); + + BindTexturePS(enviroMap,0,context); + //context->PSSetSamplers(0, 1, &skySampler); + BindSamplerPS(skySampler,0,context); + + //context->VSSetConstantBuffers( 3, 1, &skyCb ); + //context->PSSetConstantBuffers( 0, 1, &pixelCB ); + + //context->Draw(240,0); + + BindConstantBufferVS(skyCb,3,context); + BindConstantBufferPS(pixelCB,0,context); + BindConstantBufferPS(fxCb,1,context); + + BindVertexBuffer(nullptr,0,0,context); + Draw(240,context); +} + +void Renderer::DrawDecals(const XMMATRIX& newView, DeviceContext context, TextureView depth) +{ + if(!decals.empty()){ + Frustum frustum = Frustum(); + XMFLOAT4X4 proj,view; + XMStoreFloat4x4( &proj,cam->Projection ); + XMStoreFloat4x4( &view,newView ); + frustum.ConstructFrustum(cam->zFarP,proj,view); + + BindTexturePS(depth,1,context); + BindSamplerPS(ssClampAni,0,context); + BindVS(decalVS,context); + BindPS(decalPS,context); + BindRasterizerState(backFaceRS,context); + BindBlendState(blendStateTransparent,context); + BindDepthStencilState(stencilReadMatch,STENCILREF::STENCILREF_DEFAULT,context); + BindVertexLayout(nullptr,context); + BindPrimitiveTopology(PRIMITIVETOPOLOGY::TRIANGLELIST,context); + BindConstantBufferVS(decalCbVS,0,context); + BindConstantBufferPS(lightStaticCb,0,context); + BindConstantBufferPS(decalCbPS,1,context); + + for(Decal* decal : decals){ + + if((decal->texture || decal->normal) && frustum.CheckBox(decal->bounds.corners)){ + + BindTexturePS(decal->texture,2,context); + BindTexturePS(decal->normal,3,context); + + DecalCBVS dcbvs; + dcbvs.mWVP= + XMMatrixTranspose( + XMLoadFloat4x4(&decal->world) + *newView + *cam->Projection + ); + UpdateBuffer(decalCbVS,&dcbvs,context); + + DecalCBPS* dcbps = (DecalCBPS*)_aligned_malloc(sizeof(DecalCBPS),16); + dcbps->mDecalVP= + XMMatrixTranspose( + XMLoadFloat4x4(&decal->view)*XMLoadFloat4x4(&decal->projection) + ); + dcbps->hasTexNor=0; + if(decal->texture!=nullptr) + dcbps->hasTexNor|=0x0000001; + if(decal->normal!=nullptr) + dcbps->hasTexNor|=0x0000010; + XMStoreFloat3(&dcbps->eye,cam->Eye); + dcbps->opacity=WickedMath::Clamp((decal->life<=-2?1:decal->lifefadeStart?decal->life/decal->fadeStart:1),0,1); + dcbps->front=decal->front; + UpdateBuffer(decalCbPS,dcbps,context); + _aligned_free(dcbps); + + Draw(36,context); + + } + + } + + } +} + + +void Renderer::UpdatePerWorldCB(ID3D11DeviceContext* context){ + PixelCB* pcb = (PixelCB*)_aligned_malloc(sizeof(PixelCB),16); + pcb->mSun=XMVector3Normalize( GetSunPosition() ); + pcb->mHorizon=worldInfo.horizon; + pcb->mAmbient=worldInfo.ambient; + pcb->mSunColor=GetSunColor(); + pcb->mFogSEH=worldInfo.fogSEH; + UpdateBuffer(pixelCB,pcb,context); + _aligned_free(pcb); + + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //PixelCB pcb; + //pcb.mSun=XMVector3Normalize( GetSunPosition() ); + //pcb.mHorizon=worldInfo.horizon; + //pcb.mAmbient=worldInfo.ambient; + //pcb.mSunColor=GetSunColor(); + //pcb.mFogSEH=worldInfo.fogSEH; + //PixelCB* dataPtr2; + //context->Map(pixelCB,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr2 = (PixelCB*)mappedResource.pData; + //memcpy(dataPtr2,&pcb,sizeof(PixelCB)); + //context->Unmap(pixelCB,0); +} +void Renderer::UpdatePerFrameCB(ID3D11DeviceContext* context){ + ViewPropCB cb; + cb.mZFarP=cam->zFarP; + cb.mZNearP=cam->zNearP; + UpdateBuffer(viewPropCB,&cb,context); + + BindConstantBufferPS(viewPropCB,10,context); +} +void Renderer::UpdatePerRenderCB(ID3D11DeviceContext* context, int tessF){ + if(tessF){ + TessBuffer tb; + tb.g_f4Eye = cam->Eye; + tb.g_f4TessFactors = XMFLOAT4A( tessF,2,4,6 ); + UpdateBuffer(tessBuf,&tb,context); + } + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //if(tessF){ + // TessBuffer tb; + // tb.g_f4Eye = cam->Eye; + // tb.g_f4TessFactors = XMFLOAT4A( tessF,2,4,6 ); + // TessBuffer* dataPtr; + // context->Map(tessBuf,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + // dataPtr = (TessBuffer*)mappedResource.pData; + // memcpy(dataPtr,&tb,sizeof(TessBuffer)); + // context->Unmap(tessBuf,0); + //} +} +void Renderer::UpdatePerViewCB(ID3D11DeviceContext* context, const XMMATRIX& newView, const XMMATRIX& newRefView, const XMMATRIX& newProjection + , const XMVECTOR& newEye, const XMFLOAT4& newClipPlane){ + + + StaticCB* cb = (StaticCB*)_aligned_malloc(sizeof(StaticCB),16); + cb->mViewProjection = XMMatrixTranspose( newView * newProjection ); + cb->mRefViewProjection = XMMatrixTranspose( newRefView * newProjection); + cb->mCamPos = newEye; + cb->mClipPlane = newClipPlane; + cb->mWind=wind.direction; + cb->time=wind.time; + cb->windRandomness=wind.randomness; + cb->windWaveSize=wind.waveSize; + UpdateBuffer(staticCb,cb,context); + _aligned_free(cb); + + SkyBuffer* scb = (SkyBuffer*)_aligned_malloc(sizeof(SkyBuffer),16); + scb->mV=XMMatrixTranspose(newView); + scb->mP=XMMatrixTranspose(newProjection); + //scb.mV = XMMatrixTranspose( XMMatrixInverse(0, ( newView )) ); + //scb.mP = XMMatrixTranspose( XMMatrixInverse(0, ( newProjection )) ); + UpdateBuffer(skyCb,scb,context); + _aligned_free(scb); + + UpdateBuffer(trailCB,&XMMatrixTranspose( newView * newProjection ),context); + + LightStaticCB* lcb = (LightStaticCB*)_aligned_malloc(sizeof(LightStaticCB),16); + lcb->mProjInv=XMMatrixInverse( 0,XMMatrixTranspose(newView*newProjection) ); + UpdateBuffer(lightStaticCb,lcb,context); + _aligned_free(lcb); + + //D3D11_MAPPED_SUBRESOURCE mapRes; + //StaticCB cb; + //cb.mViewProjection = XMMatrixTranspose( newView * newProjection ); + //cb.mRefViewProjection = XMMatrixTranspose( newRefView * newProjection); + //cb.mCamPos = newEye; + ////cb.mMotionBlur = XMFLOAT4A(vertexBlur,0,0,0); + //cb.mClipPlane = newClipPlane; + //cb.mWind=wind.direction; + //cb.time=wind.time; + //cb.windRandomness=wind.randomness; + //cb.windWaveSize=wind.waveSize; + //StaticCB* dataPtr; + //context->Map(staticCb,0,D3D11_MAP_WRITE_DISCARD,0,&mapRes); + //dataPtr = (StaticCB*)mapRes.pData; + //memcpy(dataPtr,&cb,sizeof(StaticCB)); + //context->Unmap(staticCb,0); + + //D3D11_MAPPED_SUBRESOURCE smapRes; + //SkyBuffer scb; + //scb.mV = XMMatrixTranspose( XMMatrixInverse(0, ( newView )) ); + //scb.mP = XMMatrixTranspose( XMMatrixInverse(0, ( newProjection )) ); + //SkyBuffer* sdataPtr; + //context->Map(skyCb,0,D3D11_MAP_WRITE_DISCARD,0,&smapRes); + //sdataPtr = (SkyBuffer*)smapRes.pData; + //memcpy(sdataPtr,&scb,sizeof(SkyBuffer)); + //context->Unmap(skyCb,0); + + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //XMMATRIX tcb = XMMatrixTranspose( newView * newProjection ); + //XMMATRIX* dataPtr1; + //context->Map(trailCB,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr1 = (XMMATRIX*)mappedResource.pData; + //memcpy(dataPtr1,&tcb,sizeof(XMMATRIX)); + //context->Unmap(trailCB,0); + // + //LightStaticCB lcb; + //lcb.mProjInv=XMMatrixInverse( 0,XMMatrixTranspose(newView*newProjection) ); + //D3D11_MAPPED_SUBRESOURCE mr; + //LightStaticCB* dataPtr2; + //context->Map(lightStaticCb,0,D3D11_MAP_WRITE_DISCARD,0,&mr); + //dataPtr2 = (LightStaticCB*)mr.pData; + //memcpy(dataPtr2,&lcb,sizeof(LightStaticCB)); + //context->Unmap(lightStaticCb,0); +} +void Renderer::UpdatePerEffectCB(ID3D11DeviceContext* context, bool BlackOut, bool BlackWhite, bool InvertCol, const XMFLOAT4 colorMask){ + FxCB* fb = (FxCB*)_aligned_malloc(sizeof(FxCB),16); + fb->mFx=XMFLOAT4(BlackOut,BlackWhite,InvertCol,0); + fb->colorMask=colorMask; + UpdateBuffer(fxCb,fb,context); + _aligned_free(fb); + //D3D11_MAPPED_SUBRESOURCE mappedResource; + //FxCB fb; + //fb.mFx=XMFLOAT4(BlackOut,BlackWhite,InvertCol,0); + //fb.colorMask=colorMask; + //FxCB* dataPtr; + //context->Map(fxCb,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + //dataPtr = (FxCB*)mappedResource.pData; + //memcpy(dataPtr,&fb,sizeof(FxCB)); + //context->Unmap(fxCb,0); +} + +void Renderer::FinishLoading(){ + everyObject.reserve(objects.size()+objects_trans.size()+objects_water.size()); + everyObject.insert(everyObject.end(),objects.begin(),objects.end()); + everyObject.insert(everyObject.end(),objects_trans.begin(),objects_trans.end()); + everyObject.insert(everyObject.end(),objects_water.begin(),objects_water.end()); + + for(MeshCollection::iterator iter=meshes.begin(); iter!=meshes.end(); ++iter) + addVertexCount(iter->second->vertices.size()); + + for(Object* o : everyObject) + for(EmittedParticle* e:o->eParticleSystems){ + emitterSystems[e->name].push_back(e); + if(e->light!=nullptr) + lights.push_back(e->light); + } + + SetUpLights(); + + Update(); + UpdateRenderInfo(deferredContexts[0]); + UpdateLights(); + GenerateSPTree(spTree,vector(objects.begin(),objects.end()),GENERATE_OCTREE); + GenerateSPTree(spTree_trans,vector(objects_trans.begin(),objects_trans.end()),GENERATE_OCTREE); + GenerateSPTree(spTree_water,vector(objects_water.begin(),objects_water.end()),GENERATE_OCTREE); + GenerateSPTree(spTree_lights,vector(lights.begin(),lights.end()),GENERATE_OCTREE); + SetUpCubes(); + SetUpBoneLines(); +} + + +void Renderer::SetUpLights() +{ + for(Light* l:lights){ + if(l->type==Light::DIRECTIONAL){ + + float lerp = 0.5f; + float lerp1 = 0.12f; + float lerp2 = 0.016f; + XMVECTOR a0,a,b0,b; + a0=XMVector3Unproject(XMVectorSet(0,RENDERHEIGHT,0,1),0,0,RENDERWIDTH,RENDERHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + a=XMVector3Unproject(XMVectorSet(0,RENDERHEIGHT,1,1),0,0,RENDERWIDTH,RENDERHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + b0=XMVector3Unproject(XMVectorSet(RENDERWIDTH,RENDERHEIGHT,0,1),0,0,RENDERWIDTH,RENDERHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + b=XMVector3Unproject(XMVectorSet(RENDERWIDTH,RENDERHEIGHT,1,1),0,0,RENDERWIDTH,RENDERHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + float size=XMVectorGetX(XMVector3Length(XMVectorSubtract(XMVectorLerp(b0,b,lerp),XMVectorLerp(a0,a,lerp)))); + float size1=XMVectorGetX(XMVector3Length(XMVectorSubtract(XMVectorLerp(b0,b,lerp1),XMVectorLerp(a0,a,lerp1)))); + float size2=XMVectorGetX(XMVector3Length(XMVectorSubtract(XMVectorLerp(b0,b,lerp2),XMVectorLerp(a0,a,lerp2)))); + XMVECTOR rot = XMQuaternionIdentity(); + + l->shadowCam.push_back(SHCAM(size,rot,0,cam->zFarP)); + l->shadowCam.push_back(SHCAM(size1,rot,0,cam->zFarP)); + l->shadowCam.push_back(SHCAM(size2,rot,0,cam->zFarP)); + + } + else if(l->type==Light::SPOT && l->shadowMap.size()){ + l->shadowCam.push_back( SHCAM(l->rotation,cam->zNearP,l->enerDis.y,l->enerDis.z) ); + } + else if(l->type==Light::POINT && l->shadowMap.size()){ + l->shadowCam.push_back( SHCAM(XMFLOAT4(0.5,-0.5,-0.5,-0.5),cam->zNearP,l->enerDis.y,XM_PI/2.0) ); //+x + l->shadowCam.push_back( SHCAM(XMFLOAT4(0.5,0.5,0.5,-0.5),cam->zNearP,l->enerDis.y,XM_PI/2.0) ); //-x + + l->shadowCam.push_back( SHCAM(XMFLOAT4(1,0,0,-0),cam->zNearP,l->enerDis.y,XM_PI/2.0) ); //+y + l->shadowCam.push_back( SHCAM(XMFLOAT4(0,0,0,-1),cam->zNearP,l->enerDis.y,XM_PI/2.0) ); //-y + + l->shadowCam.push_back( SHCAM(XMFLOAT4(0.707,0,0,-0.707),cam->zNearP,l->enerDis.y,XM_PI/2.0) ); //+z + l->shadowCam.push_back( SHCAM(XMFLOAT4(0,0.707,0.707,0),cam->zNearP,l->enerDis.y,XM_PI/2.0) ); //-z + } + } + + +} +void Renderer::UpdateLights() +{ + if(GetGameSpeed()){ + for(Light* l:lights){ + //Attributes + //if(l->parent!=nullptr){ + // XMVECTOR sca,rot,pos; + // XMMatrixDecompose(&sca,&rot,&pos,XMLoadFloat4x4(&l->parent->world)); + // XMStoreFloat3(&l->translation,pos); + // XMStoreFloat4(&l->rotation,XMQuaternionMultiply(XMLoadFloat4(&l->rotation_rest),rot)); + //} + //else{ + // l->translation=l->translation_rest; + // l->rotation=l->rotation_rest; + //} + l->getTransform(); + + //Shadows + if(l->type==Light::DIRECTIONAL){ + + float lerp = 0.5f;//third slice distance from cam (percentage) + float lerp1 = 0.12f;//second slice distance from cam (percentage) + float lerp2 = 0.016f;//first slice distance from cam (percentage) + XMVECTOR c,d,e,e1,e2; + c=XMVector3Unproject(XMVectorSet(RENDERWIDTH/2,RENDERHEIGHT/2,1,1),0,0,RENDERWIDTH,RENDERHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + d=XMVector3Unproject(XMVectorSet(RENDERWIDTH/2,RENDERHEIGHT/2,0,1),0,0,RENDERWIDTH,RENDERHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + + float f = l->shadowCam[0].size/(float)SHADOWMAPRES; + e= XMVectorFloor( XMVectorLerp(d,c,lerp)/f )*f; + f = l->shadowCam[1].size/(float)SHADOWMAPRES; + e1= XMVectorFloor( XMVectorLerp(d,c,lerp1)/f)*f; + f = l->shadowCam[2].size/(float)SHADOWMAPRES; + e2= XMVectorFloor( XMVectorLerp(d,c,lerp2)/f)*f; + + //static float rot=0.0f; + ////rot+=0.001f; + //XMStoreFloat4(&l->rotation,XMQuaternionMultiply(XMLoadFloat4(&l->rotation_rest),XMQuaternionRotationAxis(XMVectorSet(1,0,0,0),rot))); + + + XMMATRIX rrr = XMMatrixRotationQuaternion(XMLoadFloat4(&l->rotation_rest)); + if(l->shadowCam.size()>0){ + l->shadowCam[0].Update(rrr*XMMatrixTranslationFromVector(e)); + if(l->shadowCam.size()>1){ + l->shadowCam[1].Update(rrr*XMMatrixTranslationFromVector(e1)); + if(l->shadowCam.size()>2) + l->shadowCam[2].Update(rrr*XMMatrixTranslationFromVector(e2)); + } + } + + l->bounds.createFromHalfWidth(XMFLOAT3(XMVectorGetX(cam->Eye),XMVectorGetY(cam->Eye),XMVectorGetZ(cam->Eye)),XMFLOAT3(10000,10000,10000)); + } + else if(l->type==Light::SPOT){ + if(!l->shadowCam.empty()){ + l->shadowCam[0].Update( XMLoadFloat4x4(&l->world) ); + } + //l->shadowCam[0].Update( XMMatrixRotationQuaternion(XMLoadFloat4(&l->frameRot))*XMMatrixTranslationFromVector(XMLoadFloat3(&l->framePos)) ); + //l->bounds=l->mesh->aabb.get( + // XMMatrixScaling(l->enerDis.y,l->enerDis.y,l->enerDis.y) + // *XMMatrixRotationQuaternion(XMLoadFloat4(&l->rotation)) + // *XMMatrixTranslationFromVector(XMLoadFloat3(&l->translation)) + // ); + + l->bounds.createFromHalfWidth(l->translation,XMFLOAT3(l->enerDis.y,l->enerDis.y,l->enerDis.y)); + } + else if(l->type==Light::POINT){ + for(int i=0;ishadowCam.size();++i){ + l->shadowCam[i].Update(XMLoadFloat3(&l->translation)); + } + //l->bounds=l->mesh->aabb.get( + // XMMatrixScaling(l->enerDis.y,l->enerDis.y,l->enerDis.y) + // *XMMatrixRotationQuaternion(XMLoadFloat4(&l->rotation)) + // *XMMatrixTranslationFromVector(XMLoadFloat3(&l->translation)) + // ); + + l->bounds.createFromHalfWidth(l->translation,XMFLOAT3(l->enerDis.y,l->enerDis.y,l->enerDis.y)); + } + } + + + UpdateSPTree(spTree_lights); + + + } +} + +Renderer::Picked Renderer::Pick(long cursorX, long cursorY, PICKTYPE pickType) +{ + XMVECTOR& lineStart = XMVector3Unproject(XMVectorSet(cursorX,cursorY,0,1),0,0 + ,SCREENWIDTH,SCREENHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + XMVECTOR& lineEnd = XMVector3Unproject(XMVectorSet(cursorX,cursorY,1,1),0,0 + ,SCREENWIDTH,SCREENHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + XMVECTOR& rayOrigin = lineStart, rayDirection = XMVector3Normalize(XMVectorSubtract(lineEnd,lineStart)); + RAY ray = RAY(lineStart,rayDirection); + + CulledCollection culledRenderer; + CulledList culledObjects; + SPTree* searchTree = nullptr; + switch (pickType) + { + case Renderer::PICK_OPAQUE: + searchTree=spTree; + break; + case Renderer::PICK_TRANSPARENT: + searchTree=spTree_trans; + break; + case Renderer::PICK_WATER: + searchTree=spTree_water; + break; + default: + break; + } + if(searchTree) + { + SPTree::getVisible(searchTree->root,ray,culledObjects); + + vector pickPoints; + + if(!culledObjects.empty()) + { + for(Cullable* culled : culledObjects){ + Object* object = (Object*)culled; + Mesh* mesh = object->mesh; + XMMATRIX& objectMat = XMLoadFloat4x4(&object->world); + + for(int i=0;iindices.size();i+=3){ + int i0=mesh->indices[i],i1=mesh->indices[i+1],i2=mesh->indices[i+2]; + Vertex& v0=mesh->skinnedVertices[i0],v1=mesh->skinnedVertices[i1],v2=mesh->skinnedVertices[i2]; + XMVECTOR& V0= + XMVector4Transform(XMLoadFloat4(&v0.pos),objectMat) + ,V1=XMVector4Transform(XMLoadFloat4(&v1.pos),objectMat) + ,V2=XMVector4Transform(XMLoadFloat4(&v2.pos),objectMat); + float distance = 0; + if(TriangleTests::Intersects(rayOrigin,rayDirection,V0,V1,V2,distance)){ + XMVECTOR& pos = XMVectorAdd(rayOrigin,rayDirection*distance); + XMVECTOR& nor = XMVector3Normalize( XMVector3Cross( XMVectorSubtract(V1,V0),XMVectorSubtract(V2,V1) ) ); + Picked picked = Picked(); + picked.object = object; + XMStoreFloat3(&picked.position,pos); + XMStoreFloat3(&picked.normal,nor); + pickPoints.push_back(picked); + } + } + + } + } + + if(!pickPoints.empty()){ + XMFLOAT3 eye; + XMStoreFloat3(&eye,rayOrigin); + Picked min = pickPoints.front(); + float mini = WickedMath::DistanceEstimated(min.position,eye); + for(int i=1;iProjection,cam->View,XMMatrixIdentity()); + XMVECTOR& lineEnd = XMVector3Unproject(XMVectorSet(cursorX,cursorY,1,1),0,0 + ,SCREENWIDTH,SCREENHEIGHT,0.1f,1.0f,cam->Projection,cam->View,XMMatrixIdentity()); + XMVECTOR& rayOrigin = lineStart, rayDirection = XMVector3Normalize(XMVectorSubtract(lineEnd,lineStart)); + return RAY(lineStart,rayDirection); +} + +void Renderer::LoadModel(const string& dir, const string& name, const XMMATRIX& transform, const string& ident, PHYSICS* physicsEngine){ + static int unique_identifier = 0; + + vector newObjects(0),newObjects_trans(0),newObjects_water(0); + vector newLights(0); + stringstream idss(""); + idss<<"_"/*<(),WorldInfo(),Wind(),vector() + ,vector(),everyObject,transforms,decals); + + + for(Object* o:newObjects){ + o->transform(transform); + o->bounds=o->bounds.get(transform); + o->getTransform(); + if(physicsEngine!=nullptr){ + physicsEngine->registerObject(o); + } + } + for(Object* o:newObjects_trans){ + o->transform(transform); + o->bounds=o->bounds.get(transform); + o->getTransform(); + if(physicsEngine!=nullptr){ + physicsEngine->registerObject(o); + } + } + for(Object* o:newObjects_water){ + o->transform(transform); + o->bounds=o->bounds.get(transform); + o->getTransform(); + if(physicsEngine!=nullptr){ + physicsEngine->registerObject(o); + } + } + //for(Light* l:newLights){ + // l->transform(transform); + // l->bounds=l->bounds.get(transform); + // l->getTransform(); + //} + + graphicsMutex.lock(); + + if(spTree){ + spTree->AddObjects(spTree->root,vector(newObjects.begin(),newObjects.end())); + } + if(spTree_trans){ + spTree_trans->AddObjects(spTree_trans->root,vector(newObjects_trans.begin(),newObjects_trans.end())); + } + if(spTree_water){ + spTree_water->AddObjects(spTree_water->root,vector(newObjects_water.begin(),newObjects_water.end())); + } + if(spTree_lights){ + spTree_lights->AddObjects(spTree_lights->root,vector(newLights.begin(),newLights.end())); + } + + objects.insert(objects.end(),newObjects.begin(),newObjects.end()); + objects_trans.insert(objects_trans.end(),newObjects_trans.begin(),newObjects_trans.end()); + objects_water.insert(objects_water.end(),newObjects_water.begin(),newObjects_water.end()); + + UpdateLights(); + lights.insert(lights.end(),newLights.begin(),newLights.end()); + + graphicsMutex.unlock(); + + unique_identifier++; +} + + +Renderer::MaterialCB::MaterialCB(const Material& mat,UINT materialIndex){ + difColor=XMFLOAT4(mat.diffuseColor.x,mat.diffuseColor.y,mat.diffuseColor.z,mat.alpha); + hasRef=mat.refMap!=nullptr; + hasNor=mat.normalMap!=nullptr; + hasTex=mat.texture!=nullptr; + hasSpe=mat.specularMap!=nullptr; + specular=mat.specular; + refractionIndex=mat.refraction_index; + movingTex=mat.texOffset; + metallic=mat.enviroReflection; + shadeless=mat.shadeless; + specular_power=mat.specular_power; + toon=mat.toonshading; + matIndex=materialIndex; + emit=mat.emit; +} \ No newline at end of file diff --git a/WickedEngine/Renderer.h b/WickedEngine/Renderer.h new file mode 100644 index 000000000..9e00c94c0 --- /dev/null +++ b/WickedEngine/Renderer.h @@ -0,0 +1,1028 @@ +#pragma once +#include "WickedEngine.h" +#include "PHYSICS.h" +#include "Lines.h" +#include "Cube.h" + +struct Transform; +struct Vertex; +struct Material; +struct Object; +struct BoneShaderBuffer; +struct Mesh; +struct Armature; +struct Bone; +struct KeyFrame; +struct SHCAM; +struct Light; +struct Decal; +struct WorldInfo; +struct Wind; +struct ActionCamera; +struct HitSphere; +struct RAY; + + +class Lines; +class Cube; +class Particle; +class EmittedParticle; +class HairParticle; +class oImage; +class SPTree; +class TaskThread; +struct Cullable; +class PHYSICS; + +typedef map MeshCollection; +typedef map MaterialCollection; + +class Renderer +{ +public: +#define API_CALL_STRICTNESS + typedef IUnknown* APIInterface; + typedef ID3D11DeviceContext* DeviceContext; + typedef ID3D11Device* GraphicsDevice; + typedef D3D11_VIEWPORT ViewPort; + typedef IDXGISwapChain* SwapChain; + typedef ID3D11CommandList* CommandList; + typedef ID3D11RenderTargetView* RenderTargetView; + typedef ID3D11ShaderResourceView* TextureView; + typedef ID3D11SamplerState* Sampler; + typedef ID3D11Resource* APIResource; + typedef ID3D11Buffer* BufferResource; + typedef ID3D11VertexShader* VertexShader; + typedef ID3D11PixelShader* PixelShader; + typedef ID3D11GeometryShader* GeometryShader; + typedef ID3D11HullShader* HullShader; + typedef ID3D11DomainShader* DomainShader; + typedef ID3D11InputLayout* VertexLayout; + typedef ID3D11BlendState* BlendState; + typedef ID3D11DepthStencilState* DepthStencilState; + typedef ID3D11RasterizerState* RasterizerState; + + + enum PRIMITIVETOPOLOGY{ + TRIANGLELIST, + TRIANGLESTRIP, + POINTLIST, + LINELIST, + PATCHLIST, + }; + + static D3D_DRIVER_TYPE driverType; + static D3D_FEATURE_LEVEL featureLevel; + static SwapChain swapChain; + static RenderTargetView renderTargetView; + static ViewPort viewPort; + static GraphicsDevice graphicsDevice; + static DeviceContext immediateContext; + static bool DX11,VSYNC; + static const int NUM_DCONTEXT = 5; + static DeviceContext deferredContexts[NUM_DCONTEXT]; + static CommandList commandLists[NUM_DCONTEXT]; + static mutex graphicsMutex; + static HRESULT InitDevice(HWND window, int screenW, int screenH, bool windowed, short& requestMultiThreading); + static void DestroyDevice(); + static void Present(function drawToScreen1=nullptr,function drawToScreen2=nullptr,function drawToScreen3=nullptr); + static void Renderer::ReleaseCommandLists(); + static void Renderer::ExecuteDeferredContexts(); + static void FinishCommandList(int THREAD); + + static map drawCalls; + static long getDrawCallCount(); + + + static Sampler ssClampLin,ssClampPoi,ssMirrorLin,ssMirrorPoi,ssWrapLin,ssWrapPoi + ,ssClampAni,ssWrapAni,ssMirrorAni,ssComp; + + + static int RENDERWIDTH,RENDERHEIGHT,SCREENWIDTH,SCREENHEIGHT; + static int SHADOWMAPRES,SOFTSHADOW,POINTLIGHTSHADOW,POINTLIGHTSHADOWRES; + static bool HAIRPARTICLEENABLED, EMITTERSENABLED; + +protected: + struct ConstantBuffer + { + XMVECTOR mDisplace; + }; + struct StaticCB + { + XMMATRIX mViewProjection; + XMMATRIX mRefViewProjection; + XMVECTOR mCamPos; + //XMFLOAT4A mMotionBlur; + XMFLOAT4 mClipPlane; + XMFLOAT3 mWind; float time; + float windRandomness; + float windWaveSize; + float padding[2]; + }; + struct PixelCB + { + //XMFLOAT4A mFx; + XMFLOAT3 mHorizon; float pad; + XMVECTOR mSun; + XMFLOAT3 mAmbient; float pad1; + XMFLOAT4 mSunColor; + XMFLOAT3 mFogSEH; float pad2; + }; + struct FxCB{ + XMFLOAT4 mFx; + XMFLOAT4 colorMask; + }; + struct MaterialCB + { + XMFLOAT4 difColor; + UINT hasRef,hasNor,hasTex,hasSpe; + XMFLOAT4 specular; + float refractionIndex; + XMFLOAT2 movingTex; + float metallic; + UINT shadeless; + UINT specular_power; + UINT toon; + UINT matIndex; + float emit; + float padding[3]; + + MaterialCB(const Material& mat,UINT materialIndex); + void* operator new(size_t size){ void* result = _aligned_malloc(size,16); return result; } + void operator delete(void* p){ if(p) _aligned_free(p); } + }; + struct ForShadowMapCB + { + XMMATRIX mViewProjection; + XMFLOAT3 mWind; float time; + float windRandomness; + float windWaveSize; + }; + struct CubeShadowCb{ + XMMATRIX mViewProjection[6]; + }; + struct TessBuffer + { + XMVECTOR g_f4Eye; + XMFLOAT4A g_f4TessFactors; + }; + struct dLightBuffer + { + XMVECTOR direction; + XMFLOAT4 col; + XMFLOAT4 mBiasResSoftshadow; + XMMATRIX mShM[3]; + }; + struct pLightBuffer + { + XMFLOAT3 pos; float pad; + XMFLOAT4 col; + XMFLOAT4 enerdis; + }; + struct sLightBuffer + { + XMMATRIX world; + XMVECTOR direction; + XMFLOAT4 col; + XMFLOAT4 enerdis; + XMFLOAT4 mBiasResSoftshadow; + XMMATRIX mShM; + }; + struct vLightBuffer + { + XMMATRIX world; + XMFLOAT4 col; + XMFLOAT4 enerdis; + }; + struct LightStaticCB{ + XMMATRIX mProjInv; + }; + struct LineBuffer{ + XMMATRIX mWorldViewProjection; + XMFLOAT4A color; + }; + struct SkyBuffer + { + XMMATRIX mV; + XMMATRIX mP; + }; + struct DecalCBVS + { + XMMATRIX mWVP; + }; + struct DecalCBPS + { + XMMATRIX mDecalVP; + int hasTexNor; + XMFLOAT3 eye; + float opacity; + XMFLOAT3 front; + }; + struct ViewPropCB + { + float mZNearP; + float mZFarP; + float pad[2]; + }; + + + void UpdateSpheres(); + static void SetUpBoneLines(); + static void UpdateBoneLines(); + static void SetUpCubes(); + static void UpdateCubes(); + + + + static bool wireRender, debugSpheres, debugLines, debugBoxes; + static ID3D11BlendState* blendState, *blendStateTransparent, *blendStateAdd; + static ID3D11Buffer* constantBuffer, *staticCb, *shCb, *pixelCB, *matCb, *lightCb[3], *tessBuf + , *lineBuffer, *trailCB, *lightStaticCb, *vLightCb,*cubeShCb,*fxCb,*skyCb,*decalCbVS,*decalCbPS,*viewPropCB; + static ID3D11VertexShader* vertexShader10, *vertexShader,*shVS,*lineVS,*trailVS,*waterVS + ,*lightVS[3],*vSpotLightVS,*vPointLightVS,*cubeShVS,*sOVS,*decalVS; + static ID3D11PixelShader* pixelShader,*shPS,*linePS,*trailPS,*simplestPS,*blackoutPS,*textureonlyPS,*waterPS,*transparentPS + ,*lightPS[3],*vLightPS,*cubeShPS,*decalPS; + static ID3D11GeometryShader* cubeShGS,*sOGS; + static ID3D11HullShader* hullShader; + static ID3D11DomainShader* domainShader; + static ID3D11InputLayout* vertexLayout, *lineIL, *trailIL, *sOIL; + static ID3D11SamplerState* texSampler, *mapSampler, *comparisonSampler, *mirSampler, *pointSampler; + static ID3D11RasterizerState* rasterizerState,*rssh,*nonCullRSsh,*wireRS,*nonCullRS,*nonCullWireRS,*backFaceRS; + static ID3D11DepthStencilState* depthStencilState,*xRayStencilState,*depthReadStencilState,*stencilReadState,*stencilReadMatch; + static ID3D11PixelShader* skyPS; + static ID3D11VertexShader* skyVS; + static ID3D11SamplerState* skySampler; + static TextureView noiseTex,trailDistortTex,enviroMap,colorGrading; + static void LoadBuffers(); + static void LoadBasicShaders(); + static void LoadLineShaders(); + static void LoadTessShaders(); + static void LoadSkyShaders(); + static void LoadShadowShaders(); + static void LoadWaterShaders(); + static void LoadTrailShaders(); + static void SetUpStates(); + static int vertexCount; + static int visibleCount; + + + static void addVertexCount(const int& toadd){vertexCount+=toadd;} + + enum KeyFrameType{ + ROTATIONKEYFRAMETYPE, + POSITIONKEYFRAMETYPE, + SCALARKEYFRAMETYPE, + }; + + + static void RecursiveBoneTransform(Armature* armature, Bone* bone, const XMMATRIX& parentCombinedMat); + inline static XMVECTOR InterPolateKeyFrames(const float& currentFrame, const int& frameCount,const std::vector& keyframes, KeyFrameType type); + static Vertex TransformVertex(const Mesh* mesh, const int& vertexI, const XMMATRIX& mat=XMMatrixIdentity()); + XMFLOAT3 VertexVelocity(const Mesh* mesh, const int& vertexI); + + Armature* getArmatureByName(const string& get); + int getActionByName(Armature* armature, const string& get); + int getBoneByName(Armature* armature, const string& get); + Material* getMaterialByName(const string& get); + HitSphere* getSphereByName(const string& get); + + static float GameSpeed,overrideGameSpeed; + +public: + Renderer(); + void CleanUp(); + static void SetUpStaticComponents(); + static void CleanUpStatic(); + + static void Update(float amount = GetGameSpeed()); + static void UpdateRenderInfo(ID3D11DeviceContext* context); + static void UpdateObjects(); + static void UpdateSoftBodyPinning(); + static void UpdateSkinnedVB(); + static void UpdateSPTree(SPTree*& tree); + static void UpdateImages(); + static void ManageImages(); + static void PutWaterRipple(const string& image, const XMFLOAT3& pos, const XMFLOAT4& plane); + static void ManageWaterRipples(); + static void DrawWaterRipples(ID3D11DeviceContext* context); + static void SetGameSpeed(float value){GameSpeed=value; if(GameSpeed<0) GameSpeed=0;}; + static float GetGameSpeed(); + static void ChangeRasterizer(){wireRender=!wireRender;}; + static bool GetRasterizer(){return wireRender;}; + static WorldInfo worldInfo; + static float shBias; + static void ToggleDebugSpheres(){debugSpheres=!debugSpheres;} + static void SetToDrawDebugSpheres(bool param){debugSpheres=param;} + static void SetToDrawDebugLines(bool param){debugLines=param;} + static void SetToDrawDebugBoxes(bool param){debugBoxes=param;} + static bool GetToDrawDebugSpheres(){return debugSpheres;}; + static bool GetToDrawDebugBoxes(){return debugBoxes;}; + static TextureView GetColorGrading(){return colorGrading;}; + static void SetColorGrading(TextureView tex){colorGrading=tex?tex:colorGrading;}; + +private: + static vector textureSlotsPS; + static PixelShader boundPS; +public: + inline static void BindTexturePS(TextureView texture, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && texture!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView t; + context->PSGetShaderResources(slot,1,&t); + if(t!=texture) + #endif + context->PSSetShaderResources(slot,1,&texture); + #ifdef API_CALL_STRICTNESS + if(t!=nullptr) + t->Release(); + #endif + } + } + inline static void BindTexturesPS(TextureView textures[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && textures!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView *t = new TextureView[num]; + context->PSGetShaderResources(slot,num,t); + if(t[0]!=textures[0]) + #endif + context->PSSetShaderResources(slot,num,textures); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindTextureVS(TextureView texture, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && texture!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView t; + context->VSGetShaderResources(slot,1,&t); + if(t!=texture) + #endif + context->VSSetShaderResources(slot,1,&texture); + #ifdef API_CALL_STRICTNESS + if(t!=nullptr) + t->Release(); + #endif + } + } + inline static void BindTexturesVS(TextureView textures[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && textures!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView *t = new TextureView[num]; + context->VSGetShaderResources(slot,num,t); + if(t[0]!=textures[0]) + #endif + context->VSSetShaderResources(slot,num,textures); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindTextureGS(TextureView texture, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && texture!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView t; + context->GSGetShaderResources(slot,1,&t); + if(t!=texture) + #endif + context->GSSetShaderResources(slot,1,&texture); + #ifdef API_CALL_STRICTNESS + if(t!=nullptr) + t->Release(); + #endif + } + } + inline static void BindTexturesGS(TextureView textures[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && textures!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView *t = new TextureView[num]; + context->GSGetShaderResources(slot,num,t); + if(t[0]!=textures[0]) + #endif + context->GSSetShaderResources(slot,num,textures); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindTextureDS(TextureView texture, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && texture!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView t; + context->DSGetShaderResources(slot,1,&t); + if(t!=texture) + #endif + context->DSSetShaderResources(slot,1,&texture); + #ifdef API_CALL_STRICTNESS + if(t!=nullptr) + t->Release(); + #endif + } + } + inline static void BindTexturesDS(TextureView textures[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && textures!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView *t = new TextureView[num]; + context->DSGetShaderResources(slot,num,t); + if(t[0]!=textures[0]) + #endif + context->DSSetShaderResources(slot,num,textures); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindTextureHS(TextureView texture, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && texture!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView t; + context->HSGetShaderResources(slot,1,&t); + if(t!=texture) + #endif + context->HSSetShaderResources(slot,1,&texture); + #ifdef API_CALL_STRICTNESS + if(t!=nullptr) + t->Release(); + #endif + } + } + inline static void BindTexturesHS(TextureView textures[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && textures!=nullptr){ + #ifdef API_CALL_STRICTNESS + TextureView *t = new TextureView[num]; + context->HSGetShaderResources(slot,num,t); + if(t[0]!=textures[0]) + #endif + context->HSSetShaderResources(slot,num,textures); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindSamplerPS(Sampler sampler, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && sampler!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler s; + context->PSGetSamplers(slot,1,&s); + if(s!=sampler) + #endif + context->PSSetSamplers(slot,1,&sampler); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) + s->Release(); + #endif + } + } + inline static void BindSamplersPS(Sampler samplers[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && samplers!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler *s = new Sampler[num]; + context->PSGetSamplers(slot,num,s); + if(s[0]!=samplers[0]) + #endif + context->PSSetSamplers(slot,num,samplers); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindSamplerVS(Sampler sampler, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && sampler!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler s; + context->VSGetSamplers(slot,1,&s); + if(s!=sampler) + #endif + context->VSSetSamplers(slot,1,&sampler); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) + s->Release(); + #endif + } + } + inline static void BindSamplersVS(Sampler samplers[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && samplers!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler *s = new Sampler[num]; + context->VSGetSamplers(slot,num,s); + if(s[0]!=samplers[0]) + #endif + context->VSSetSamplers(slot,num,samplers); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindSamplerGS(Sampler sampler, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && sampler!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler s; + context->GSGetSamplers(slot,1,&s); + if(s!=sampler) + #endif + context->GSSetSamplers(slot,1,&sampler); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) + s->Release(); + #endif + } + } + inline static void BindSamplersGS(Sampler samplers[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && samplers!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler *s = new Sampler[num]; + context->GSGetSamplers(slot,num,s); + if(s[0]!=samplers[0]) + #endif + context->GSSetSamplers(slot,num,samplers); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindSamplerHS(Sampler sampler, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && sampler!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler s; + context->HSGetSamplers(slot,1,&s); + if(s!=sampler) + #endif + context->HSSetSamplers(slot,1,&sampler); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) + s->Release(); + #endif + } + } + inline static void BindSamplersHS(Sampler samplers[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && samplers!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler *s = new Sampler[num]; + context->HSGetSamplers(slot,num,s); + if(s[0]!=samplers[0]) + #endif + context->HSSetSamplers(slot,num,samplers); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindSamplerDS(Sampler sampler, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr && sampler!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler s; + context->DSGetSamplers(slot,1,&s); + if(s!=sampler) + #endif + context->DSSetSamplers(slot,1,&sampler); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) + s->Release(); + #endif + } + } + inline static void BindSamplersDS(Sampler samplers[], int slot, int num, DeviceContext context=immediateContext){ + if(context!=nullptr && samplers!=nullptr){ + #ifdef API_CALL_STRICTNESS + Sampler *s = new Sampler[num]; + context->DSGetSamplers(slot,num,s); + if(s[0]!=samplers[0]) + #endif + context->DSSetSamplers(slot,num,samplers); + #ifdef API_CALL_STRICTNESS + for(int i=0;iRelease(); + #endif + } + } + inline static void BindConstantBufferPS(BufferResource buffer, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + BufferResource b; + context->PSGetConstantBuffers(slot,1,&b); + if(b!=buffer) + #endif + context->PSSetConstantBuffers(slot,1,&buffer); + #ifdef API_CALL_STRICTNESS + if(b!=nullptr) + b->Release(); + #endif + } + } + inline static void BindConstantBufferVS(BufferResource buffer, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + BufferResource b; + context->VSGetConstantBuffers(slot,1,&b); + if(b!=buffer) + #endif + context->VSSetConstantBuffers(slot,1,&buffer); + #ifdef API_CALL_STRICTNESS + if(b!=nullptr) + b->Release(); + #endif + } + } + inline static void BindConstantBufferGS(BufferResource buffer, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + BufferResource b; + context->GSGetConstantBuffers(slot,1,&b); + if(b!=buffer) + #endif + context->GSSetConstantBuffers(slot,1,&buffer); + #ifdef API_CALL_STRICTNESS + if(b!=nullptr) + b->Release(); + #endif + } + } + inline static void BindConstantBufferDS(BufferResource buffer, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + BufferResource b; + context->DSGetConstantBuffers(slot,1,&b); + if(b!=buffer) + #endif + context->DSSetConstantBuffers(slot,1,&buffer); + #ifdef API_CALL_STRICTNESS + if(b!=nullptr) + b->Release(); + #endif + } + } + inline static void BindConstantBufferHS(BufferResource buffer, int slot, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + BufferResource b; + context->HSGetConstantBuffers(slot,1,&b); + if(b!=buffer) + #endif + context->HSSetConstantBuffers(slot,1,&buffer); + #ifdef API_CALL_STRICTNESS + if(b!=nullptr) + b->Release(); + #endif + } + } + inline static void BindVertexBuffer(BufferResource vertexBuffer, int slot, UINT stride, DeviceContext context=immediateContext){ + if(context!=nullptr){ + UINT offset = 0; + context->IASetVertexBuffers(slot,1,&vertexBuffer,&stride,&offset); + } + } + inline static void BindIndexBuffer(BufferResource indexBuffer, DeviceContext context=immediateContext){ + if(context!=nullptr){ + context->IASetIndexBuffer(indexBuffer,DXGI_FORMAT_R32_UINT,0); + } + } + inline static void BindPrimitiveTopology(PRIMITIVETOPOLOGY type, DeviceContext context=immediateContext){ + if(context!=nullptr){ + D3D11_PRIMITIVE_TOPOLOGY topo = D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST; + switch (type) + { + case Renderer::TRIANGLELIST: + topo=D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST; + break; + case Renderer::TRIANGLESTRIP: + topo=D3D11_PRIMITIVE_TOPOLOGY_TRIANGLESTRIP; + break; + case Renderer::POINTLIST: + topo=D3D11_PRIMITIVE_TOPOLOGY_POINTLIST; + break; + case Renderer::LINELIST: + topo=D3D11_PRIMITIVE_TOPOLOGY_LINELIST; + break; + case Renderer::PATCHLIST: + topo=D3D11_PRIMITIVE_TOPOLOGY_3_CONTROL_POINT_PATCHLIST; + break; + default: + break; + } + #ifdef API_CALL_STRICTNESS + D3D11_PRIMITIVE_TOPOLOGY p; + context->IAGetPrimitiveTopology(&p); + if(p!=topo) + #endif + context->IASetPrimitiveTopology(topo); + } + } + inline static void BindVertexLayout(VertexLayout layout, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + VertexLayout v; + context->IAGetInputLayout(&v); + if(v!=layout) + #endif + context->IASetInputLayout(layout); + #ifdef API_CALL_STRICTNESS + if(v!=nullptr) v->Release(); + #endif + } + } + inline static void BindBlendState(BlendState state, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + BlendState b; + context->OMGetBlendState(&b,nullptr,nullptr); + if(b!=state){ + #endif + float blendFactor[4] = { 1.0f, 1.0f, 1.0f, 1.0f }; + UINT sampleMask = 0xffffffff; + context->OMSetBlendState(state,blendFactor,sampleMask); + #ifdef API_CALL_STRICTNESS + } + if(b!=nullptr) b->Release(); + #endif + } + } + inline static void BindDepthStencilState(DepthStencilState state, UINT stencilRef, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + DepthStencilState b; + UINT s; + context->OMGetDepthStencilState(&b,&s); + if(b!=state || s!=stencilRef){ + #endif + context->OMSetDepthStencilState(state,stencilRef); + #ifdef API_CALL_STRICTNESS + } + if(b!=nullptr) b->Release(); + #endif + } + } + inline static void BindRasterizerState(RasterizerState state, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + RasterizerState b; + context->RSGetState(&b); + if(b!=state){ + #endif + context->RSSetState(state); + #ifdef API_CALL_STRICTNESS + } + if(b!=nullptr) b->Release(); + #endif + } + } + inline static void BindStreamOutTarget(BufferResource buffer, DeviceContext context=immediateContext){ + if(context!=nullptr){ + UINT offsetSO[1] = {0}; + context->SOSetTargets(1,&buffer,offsetSO); + } + } + inline static void BindPS(PixelShader shader, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + PixelShader s; + context->PSGetShader(&s,nullptr,0); + if(s!=shader) + #endif + context->PSSetShader(shader, nullptr, 0); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) s->Release(); + #endif + } + } + inline static void BindVS(VertexShader shader, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + VertexShader s; + context->VSGetShader(&s,nullptr,0); + if(s!=shader) + #endif + context->VSSetShader(shader, nullptr, 0); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) s->Release(); + #endif + } + } + inline static void BindGS(GeometryShader shader, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + GeometryShader s; + context->GSGetShader(&s,nullptr,0); + if(s!=shader) + #endif + context->GSSetShader(shader, nullptr, 0); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) s->Release(); + #endif + } + } + inline static void BindHS(HullShader shader, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + HullShader s; + context->HSGetShader(&s,nullptr,0); + if(s!=shader) + #endif + context->HSSetShader(shader, nullptr, 0); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) s->Release(); + #endif + } + } + inline static void BindDS(DomainShader shader, DeviceContext context=immediateContext){ + if(context!=nullptr){ + #ifdef API_CALL_STRICTNESS + DomainShader s; + context->DSGetShader(&s,nullptr,0); + if(s!=shader) + #endif + context->DSSetShader(shader, nullptr, 0); + #ifdef API_CALL_STRICTNESS + if(s!=nullptr) s->Release(); + #endif + } + } + inline static void Draw(int vertexCount, DeviceContext context=immediateContext){ + if(context!=nullptr){ + context->Draw(vertexCount,0); + ++drawCalls[context]; + } + } + inline static void DrawIndexed(int indexCount, DeviceContext context=immediateContext){ + if(context!=nullptr){ + context->DrawIndexed(indexCount,0,0); + ++drawCalls[context]; + } + } + inline static void DrawIndexedInstanced(int indexCount, int instanceCount, DeviceContext context=immediateContext){ + if(context!=nullptr){ + context->DrawIndexedInstanced(indexCount,instanceCount,0,0,0); + ++drawCalls[context]; + } + } + //Comment: specify dataSize param if you are uploading multiple instances of data (eg. sizeof(Vertex)*vertices.count()) + template + inline static void ResizeBuffer(BufferResource& buffer, int dataCount){ + if(buffer!=nullptr){ + D3D11_BUFFER_DESC desc; + buffer->GetDesc(&desc); + int dataSize = sizeof(T)*dataCount; + if((int)desc.ByteWidthRelease(); + desc.ByteWidth=dataSize; + graphicsDevice->CreateBuffer( &desc, nullptr, &buffer ); + } + } + } + template + inline static void UpdateBuffer(BufferResource& buffer, const T* data,DeviceContext context=immediateContext, int dataSize = -1) + { + if(buffer != nullptr && data!=nullptr && context != nullptr){ + D3D11_BUFFER_DESC desc; + buffer->GetDesc(&desc); + HRESULT hr; + if(dataSize>(int)desc.ByteWidth){ //recreate the buffer if new datasize exceeds buffer size [SLOW][TEST!!!!] + buffer->Release(); + desc.ByteWidth=dataSize*2; + hr=graphicsDevice->CreateBuffer( &desc, nullptr, &buffer ); + } + if(desc.Usage == D3D11_USAGE_DYNAMIC){ + D3D11_MAPPED_SUBRESOURCE mappedResource; + void* dataPtr; + context->Map(buffer,0,D3D11_MAP_WRITE_DISCARD,0,&mappedResource); + dataPtr = (void*)mappedResource.pData; + memcpy(dataPtr,data,(dataSize>=0?dataSize:desc.ByteWidth)); + context->Unmap(buffer,0); + } + else{ + context->UpdateSubresource( buffer, 0, nullptr, data, 0, 0 ); + } + } + } + template + inline static void SafeRelease(T& resource){ + if(resource != nullptr){ + resource->Release(); + resource=nullptr; + } + } + + static void UpdatePerWorldCB(ID3D11DeviceContext* context); + static void UpdatePerFrameCB(ID3D11DeviceContext* context); + static void UpdatePerRenderCB(ID3D11DeviceContext* context, int tessF); + static void UpdatePerViewCB(ID3D11DeviceContext* context, const XMMATRIX& newView, const XMMATRIX& newRefView, const XMMATRIX& newProjection + , const XMVECTOR& newEye, const XMFLOAT4& newClipPlane = XMFLOAT4(0,0,0,0)); + static void UpdatePerEffectCB(ID3D11DeviceContext* context, bool BlackOut, bool BlackWhite, bool InvertCol, const XMFLOAT4 colorMask); + + + static void DrawSky(const XMVECTOR&, ID3D11DeviceContext* context); + static void DrawWorld(const XMMATRIX&, bool DX11Eff, int tessF, ID3D11DeviceContext* context + , bool BlackOut, bool shaded, ID3D11ShaderResourceView* refRes, bool grass, int passIdentifier=1); //passidentifier is >1 (0 is for shadows) + static void DrawForSO(ID3D11DeviceContext* context); + static void ClearShadowMaps(ID3D11DeviceContext* context); + static void DrawForShadowMap(ID3D11DeviceContext* context); + static void DrawWorldWater(const XMMATRIX& newView, ID3D11ShaderResourceView* refracRes, ID3D11ShaderResourceView* refRes + , ID3D11ShaderResourceView* depth, ID3D11ShaderResourceView* nor, ID3D11DeviceContext* context, int passIdentifier=1); + static void DrawWorldTransparent(const XMMATRIX& newView, ID3D11ShaderResourceView* refracRes, ID3D11ShaderResourceView* refRes + , ID3D11ShaderResourceView* depth, ID3D11DeviceContext* context, int passIdentifier=1); + void DrawDebugSpheres(const XMMATRIX&, ID3D11DeviceContext* context); + static void DrawDebugLines(const XMMATRIX&, ID3D11DeviceContext* context); + static void DrawDebugBoxes(const XMMATRIX&, ID3D11DeviceContext* context); + static void DrawSoftParticles(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, bool dark = false); + static void DrawSoftPremulParticles(const XMVECTOR eye, const XMMATRIX& view, ID3D11DeviceContext *context, ID3D11ShaderResourceView* depth, bool dark = false); + static void DrawTrails(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes); + static void DrawImagesAdd(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes); + //alpha-opaque + static void DrawImages(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes); + static void DrawImagesNormals(ID3D11DeviceContext* context, ID3D11ShaderResourceView* refracRes); + static void DrawLights(const XMMATRIX&, ID3D11DeviceContext* context + , ID3D11ShaderResourceView* depth, ID3D11ShaderResourceView* normal, ID3D11ShaderResourceView* material + , unsigned int stencilRef = 2); + static void DrawVolumeLights(const XMMATRIX&, ID3D11DeviceContext* context); + static void DrawLensFlares(ID3D11DeviceContext* context, ID3D11ShaderResourceView* depth, const int& RENDERWIDTH, const int& RENDERHEIGHT); + static void DrawDecals(const XMMATRIX& newView, DeviceContext context, TextureView depth); + + inline static XMVECTOR GetSunPosition(); + inline static XMFLOAT4 GetSunColor(); + static Wind wind; + static int getVertexCount(){return vertexCount;} + static void resetVertexCount(){vertexCount=0;} + static int getVisibleObjectCount(){return visibleCount;} + static void resetVisibleObjectCount(){visibleCount=0;} + static void setResolution(int w, int h, int W, int H){RENDERWIDTH=w;RENDERHEIGHT=h;SCREENWIDTH=W;SCREENHEIGHT=H;}; + + static void FinishLoading(); + static SPTree* spTree; + static SPTree* spTree_trans; + static SPTree* spTree_water; + static SPTree* spTree_lights; + + static vector everyObject; + static vector objects; + static vector objects_trans; + static vector objects_water; + static MeshCollection meshes; + static MaterialCollection materials; + static vector armatures; + static vector boneLines; + static vector cubes; + static vector lights; + static map> emitterSystems; + vector cameras; + vector spheres; + static map transforms; + static list decals; + + static deque images; + static deque waterRipples; + static void CleanUpStaticTemp(); + + static void SetUpLights(); + static void UpdateLights(); + + + static RenderTarget normalMapRT,imagesRT,imagesRTAdd; + + + + + float getSphereRadius(const int& index); + + string DIRECTORY; + + enum PICKTYPE + { + PICK_OPAQUE, + PICK_TRANSPARENT, + PICK_WATER, + }; + struct Picked + { + Object* object; + XMFLOAT3 position,normal; + + Picked():object(nullptr){} + }; + static Picked Pick(long cursorX, long cursorY, PICKTYPE pickType = PICKTYPE::PICK_OPAQUE); + static RAY getPickRay(long cursorX, long cursorY); + + static void LoadModel(const string& dir, const string& name, const XMMATRIX& transform = XMMatrixIdentity(), const string& ident = "common", PHYSICS* physicsEngine = nullptr); +}; + + +#include "Entity.h" +#include "Level.h" +#include "Character.h" diff --git a/WickedEngine/Requirements.cpp b/WickedEngine/Requirements.cpp new file mode 100644 index 000000000..c90c68c12 --- /dev/null +++ b/WickedEngine/Requirements.cpp @@ -0,0 +1,156 @@ +#include "Renderer.h" + + +Character::Req_input::Req_input(Character* c, const string& newData):Requirement(c,"input"){ + data.push_back(newData); + input=newData; + overrider = !input.compare("0"); + special = convertInputString(input).size()>1; +} +bool Character::Req_input::validate(){ + if(overrider) + return true; + + vector v = convertInputString(input); + + if(!special) + return InputCompare(input,c->directinput); + return InputCompare(input,c->finalInput); + + //if(input.length()<=1) + // return (!c->directinput.compare(input)); + //return (c->finalInput.find(input)!=string::npos); +} + +Character::Req_timeout::Req_timeout(Character* c, const string& newData, Combatant& newCombatant):Requirement(c,"input"),combatant(newCombatant){ //type must be "input" because it overrides input if no input is captured + data.push_back(newData); + timeout=atoi(newData.c_str()); +} +bool Character::Req_timeout::validate(){ + return combatant.state->move->timer>=timeout; +} + +Character::Req_frame::Req_frame(Character* c, const string& newData, MoveListItem* newMove):Requirement(c,"frame"){ + data.push_back(newData); + move=newMove; + if(!newData.empty()){ + if(!newData.compare("end") && move!=nullptr && move->armature!=nullptr){ + interval=Interval(move->armature->actions[move->actionI].frameCount); + } + else{ + interval=Interval(newData); + } + } +} +bool Character::Req_frame::validate(){ + if(move==nullptr || move->armature==nullptr) + return false; + + return interval.contains(move->armature->currentFrame); +} + +Character::Req_height::Req_height(Character* c, const string& newData, MoveListItem* newMove):Requirement(c,"height"){ + data.push_back(newData); + move=newMove; + interval=Interval(newData); +} +bool Character::Req_height::validate(){ + return interval.contains(move->armature->translation.y); +} + +Character::Req_hit::Req_hit(Character* c, Combatant& newCombatant):Requirement(c,"hit"),combatant(newCombatant){ +} +bool Character::Req_hit::validate(){ + return combatant.hitConfirmed; +} + +Character::Req_nostun::Req_nostun(Character* c, Combatant& newCombatant):Requirement(c,"nostun"),combatant(newCombatant){ +} +bool Character::Req_nostun::validate(){ + return combatant.stun<=0; +} + +Character::Req_onstun::Req_onstun(Character* c, Combatant& newCombatant):Requirement(c,"onstun"),combatant(newCombatant){ +} +bool Character::Req_onstun::validate(){ + return combatant.stun>0; +} + +Character::Req_onhit::Req_onhit(Character* c, Combatant& newCombatant):Requirement(c,"onhit"),combatant(newCombatant){ +} +bool Character::Req_onhit::validate(){ + return combatant.hitIn; +} + +Character::Req_noblockstun::Req_noblockstun(Character* c, Combatant& newCombatant):Requirement(c,"noblockstun"),combatant(newCombatant){ +} +bool Character::Req_noblockstun::validate(){ + return combatant.blockstun<=0; +} + +Character::Req_onblockstun::Req_onblockstun(Character* c, Combatant& newCombatant):Requirement(c,"onblockstun"),combatant(newCombatant){ +} +bool Character::Req_onblockstun::validate(){ + return combatant.blockstun>0; +} + +Character::Req_health::Req_health(Character* c, const string& newData):Requirement(c,"health"){ + data.push_back(newData); + health=atoi(newData.c_str()); +} +bool Character::Req_health::validate(){ + return c->cData.hp>health; +} +void Character::Req_health::activate(){ + c->cData.hp -= health; +} + +Character::Req_meter::Req_meter(Character* c, const string& newData):Requirement(c,"meter"){ + data.push_back(newData); + meter=atoi(newData.c_str()); +} +bool Character::Req_meter::validate(){ + return c->cData.meter>=meter; +} +void Character::Req_meter::activate(){ + c->cData.meter -= meter; +} + + +Character::Req_facing::Req_facing(Character* c, Combatant& newCombatant):Requirement(c,"facing"),combatant(newCombatant){ +} +bool Character::Req_facing::validate(){ + return combatant.facing!=combatant.prevFacing; +} +void Character::Req_facing::activate(){ + combatant.prevFacing=combatant.facing; + c->TurnFacing(combatant.state->move->armature); +} + +Character::Req_died::Req_died(Character* c):Requirement(c,"died"){ +} +bool Character::Req_died::validate(){ + return c->cData.hp<=0; +} + +Character::Req_block::Req_block(Character* c, Combatant& newCombatant):Requirement(c,"block"),combatant(newCombatant){ +} +bool Character::Req_block::validate(){ + return combatant.canBlock; +} + +Character::Req_falling::Req_falling(Character* c, Combatant& newCombatant):Requirement(c,"falling"),combatant(newCombatant){ +} +bool Character::Req_falling::validate(){ + return combatant.velocity.y<=0; +} + +Character::Req_hitproperty::Req_hitproperty(Character* c, Combatant& newCombatant, const string& prop):Requirement(c,"hitproperty"),combatant(newCombatant),hitproperty(prop){ +} +bool Character::Req_hitproperty::validate(){ + return !combatant.hitProperty.compare(hitproperty); +} + + + + diff --git a/WickedEngine/Resource.h b/WickedEngine/Resource.h new file mode 100644 index 000000000..e3f388fb9 --- /dev/null +++ b/WickedEngine/Resource.h @@ -0,0 +1,15 @@ +//{{NO_DEPENDENCIES}} +// Microsoft Visual C++ generated include file. +// Used by DXProject.rc +// + +#define IDS_APP_TITLE 103 +#define IDR_MAINFRAME 128 +#define IDI_DXPROJECT 107 +#define IDI_SMALL 108 +#define IDC_DXPROJECT 109 +#define IDC_MYICON 2 +#ifndef IDC_STATIC +#define IDC_STATIC -1 +#endif + diff --git a/WickedEngine/ResourceManager.cpp b/WickedEngine/ResourceManager.cpp new file mode 100644 index 000000000..3169b5026 --- /dev/null +++ b/WickedEngine/ResourceManager.cpp @@ -0,0 +1,150 @@ +#include "ResourceManager.h" + + +ResourceManager::container ResourceManager::resources; +ResourceManager::filetypes ResourceManager::types; +mutex ResourceManager::MUTEX; + +void ResourceManager::SetUp() +{ + types.insert( pair("jpg",IMAGE) ); + types.insert( pair("JPG",IMAGE) ); + types.insert( pair("png",IMAGE) ); + types.insert( pair("PNG",IMAGE) ); + types.insert( pair("dds",IMAGE) ); + types.insert( pair("wav",SOUND) ); +} + +const ResourceManager::Resource* ResourceManager::get(const string& name) +{ + container::iterator it = resources.find(name); + if(it!=resources.end()) + return it->second; + else return nullptr; +} + +void* ResourceManager::add(const string& name, Data_Type newType) +{ + const Resource* res = get(name); + if(!res){ + string ext = name.substr(name.length()-3,name.length()); + Data_Type type; +#pragma region dynamic type selection + if(newType==Data_Type::DYNAMIC){ + filetypes::iterator it = types.find(ext); + if(it!=types.end()) + type = it->second; + else + return nullptr; + } + else + type = newType; +#pragma endregion + void* success = nullptr; + + MUTEX.lock(); + + switch(type){ + case Data_Type::IMAGE: + { + Renderer::TextureView image=nullptr; + if( + !ext.compare("jpg") + || !ext.compare("JPG") + || !ext.compare("png") + || !ext.compare("PNG") + ) + { + Renderer::graphicsMutex.lock(); + CreateWICTextureFromFile(true,Renderer::graphicsDevice,Renderer::immediateContext,(wchar_t*)(wstring(name.begin(),name.end()).c_str()),nullptr,&image); + Renderer::graphicsMutex.unlock(); + } + else if(!ext.compare("dds")){ + CreateDDSTextureFromFile(Renderer::graphicsDevice,(wchar_t*)(wstring(name.begin(),name.end()).c_str()),nullptr,&image); + } + + if(image) + success=image; + } + break; + case Data_Type::IMAGE_STAGING: + { + Renderer::APIResource image=nullptr; + if(!ext.compare("dds")){ + CreateDDSTextureFromFileEx(Renderer::graphicsDevice,(wchar_t*)(wstring(name.begin(),name.end()).c_str()),0 + ,D3D11_USAGE_STAGING,0,D3D11_CPU_ACCESS_READ | D3D11_CPU_ACCESS_WRITE,0,false + ,&image,nullptr); + } + + if(image) + success=image; + } + break; + case Data_Type::SOUND: + { + success = new SoundEffect(name); + } + break; + case Data_Type::MUSIC: + { + success = new Music(name); + } + break; + default: + success=nullptr; + break; + }; + + if(success) + resources.insert( pair(name,new Resource(success,type)) ); + + MUTEX.unlock(); + + return success; + } + return res->data; +} + +bool ResourceManager::del(const string& name) +{ + const Resource* res = get(name); + + if(res){ + MUTEX.lock(); + bool success = true; + + if(res->data) + switch(res->type){ + case Data_Type::IMAGE: + ((Renderer::TextureView)res->data)->Release(); + break; + case Data_Type::SOUND: + delete ((SoundResource)res->data); + break; + default: + success=false; + break; + }; + + resources.erase(name); + + MUTEX.unlock(); + + return success; + } + return false; +} + +bool ResourceManager::CleanUp() +{ + /*vector names; + names.reserve(resources.size()); + for(container::iterator it=resources.begin();it!=resources.end();++it) + names.push_back(it->first); + for(const string& s:names) + del(s); + names.clear();*/ + resources.clear(); + types.clear(); + return true; +} \ No newline at end of file diff --git a/WickedEngine/ResourceManager.h b/WickedEngine/ResourceManager.h new file mode 100644 index 000000000..25dc0f4af --- /dev/null +++ b/WickedEngine/ResourceManager.h @@ -0,0 +1,35 @@ +#pragma once +#include "WickedEngine.h" + +class ResourceManager +{ +public: + typedef Sound* SoundResource; + enum Data_Type{ + DYNAMIC, + IMAGE,IMAGE_STAGING, + SOUND,MUSIC, + }; +private: +struct Resource +{ + void* data; + Data_Type type; + + Resource(void* newData, Data_Type newType):data(newData),type(newType){}; +}; +typedef map filetypes; +static filetypes types; +typedef unordered_map container; +static container resources; +static mutex MUTEX; +public: + static void SetUp(); + + static const Resource* get(const string& name); + //please specify datatype for shaders + static void* add(const string& name, Data_Type newType = Data_Type::DYNAMIC); + static bool del(const string& name); + static bool CleanUp(); +}; + diff --git a/WickedEngine/SPTree.cpp b/WickedEngine/SPTree.cpp new file mode 100644 index 000000000..8e3e61f22 --- /dev/null +++ b/WickedEngine/SPTree.cpp @@ -0,0 +1,249 @@ +#include "SPTree.h" +#define SP_TREE_MAX_DEPTH 100 +#define SP_TREE_OBJECT_PER_NODE 6 +#define SP_TREE_BOX_CONTAIN + + +SPTree::SPTree() +{ + root=NULL; +} + +SPTree::~SPTree() +{ + //CleanUp(); +} + +void SPTree::initialize(const vector& objects){ + initialize(objects,XMFLOAT3(D3D11_FLOAT32_MAX,D3D11_FLOAT32_MAX,D3D11_FLOAT32_MAX) + ,XMFLOAT3(-D3D11_FLOAT32_MAX,-D3D11_FLOAT32_MAX,-D3D11_FLOAT32_MAX)); +} +void SPTree::initialize(const vector& objects, const XMFLOAT3& newMin, const XMFLOAT3& newMax){ + XMFLOAT3 min = newMin; + XMFLOAT3 max = newMax; + + for(Cullable* object : objects){ + XMFLOAT3 gMin = object->bounds.getMin(); + XMFLOAT3 gMax = object->bounds.getMax(); + min=WickedMath::Min(gMin,min); + max=WickedMath::Max(gMax,max); + } + + this->root = new SPTree::Node(NULL,AABB(min,max)); + AddObjects(this->root,objects); +} + +void SPTree::CleanUp() +{ + if(root) + root->Release(); + root=NULL; + if(this) + delete this; +} + + +void SPTree::AddObjects(Node* node, const vector& newObjects){ + for(Cullable* object : newObjects){ + /*bool default_mesh = false; + bool water_mesh = false; + bool transparent_mesh = false; + + if(object->mesh->renderable) + for(Material* mat : object->mesh->materials){ + if(!mat->water && !mat->isSky && !mat->transparent) + default_mesh=true; + if(mat->water && !mat->isSky) + water_mesh=true; + if(!mat->water && !mat->isSky && mat->transparent) + transparent_mesh=true; + }*/ + + node->objects.push_back(object); + } + + if(newObjects.size()>SP_TREE_OBJECT_PER_NODE && node->depthcount=childCount; + node->children.reserve(node->count); + XMFLOAT3 min = node->box.getMin(); + XMFLOAT3 max = node->box.getMax(); + AABB* boxes = new AABB[node->count]; + if(node->count==8){ + boxes[0] = AABB(XMFLOAT3(min.x,(min.y+max.y)*0.5f,(min.z+max.z)*0.5f),XMFLOAT3((min.x+max.x)*0.5f,max.y,max.z)); + boxes[1] = AABB(XMFLOAT3((min.x+max.x)*0.5f,(min.y+max.y)*0.5f,(min.z+max.z)*0.5f),max); + boxes[2] = AABB(XMFLOAT3(min.x,(min.y+max.y)*0.5f,min.z),XMFLOAT3((min.x+max.x)*0.5f,max.y,(min.z+max.z)*0.5f)); + boxes[3] = AABB(XMFLOAT3((min.x+max.x)*0.5f,(min.y+max.y)*0.5f,min.z),XMFLOAT3(max.x,max.y,(min.z+max.z)*0.5f)); + + boxes[4] = AABB(XMFLOAT3(min.x,min.y,(min.z+max.z)*0.5f),XMFLOAT3((min.x+max.x)*0.5f,(min.y+max.y)*0.5f,max.z)); + boxes[5] = AABB(XMFLOAT3((min.x+max.x)*0.5f,min.y,(min.z+max.z)*0.5f),XMFLOAT3(max.x,(min.y+max.y)*0.5f,max.z)); + boxes[6] = AABB(min,XMFLOAT3((min.x+max.x)*0.5f,(min.y+max.y)*0.5f,(min.z+max.z)*0.5f)); + boxes[7] = AABB(XMFLOAT3((min.x+max.x)*0.5f,min.y,min.z),XMFLOAT3(max.x,(min.y+max.y)*0.5f,(min.z+max.z)*0.5f)); + } + else{ + boxes[0] = AABB(XMFLOAT3(min.x,min.y,(min.z+max.z)*0.5f),XMFLOAT3((min.x+max.x)*0.5f,max.y,max.z)); + boxes[1] = AABB(XMFLOAT3((min.x+max.x)*0.5f,min.y,(min.z+max.z)*0.5f),max); + boxes[2] = AABB(min,XMFLOAT3((min.x+max.x)*0.5f,max.y,(min.z+max.z)*0.5f)); + boxes[3] = AABB(XMFLOAT3((min.x+max.x)*0.5f,min.y,min.z),XMFLOAT3(max.x,max.y,(min.z+max.z)*0.5f)); + } + for(int i=0;icount;++i){ + //children[i] = new Node(this,boxes[i],depth+1); + + vector o(0); + o.reserve(newObjects.size()); + for(Cullable* object : newObjects) +#ifdef SP_TREE_BOX_CONTAIN + if( boxes[i].intersects(object->bounds)==AABB::INSIDE ) +#else + if( (*boxes[i]).intersects(object->translation) ) +#endif + { + o.push_back(object); + node->objects.remove(object); + } + if(!o.empty()){ + node->children.push_back( new Node(node,boxes[i],node->depth+1) ); + AddObjects(node->children.back(),o); + } + } + delete[] boxes; + } + +} + +void SPTree::getVisible(Node* node, Frustum& frustum, CulledList& objects, int type){ + if(!node) return; + int contain_type = frustum.CheckBox(node->box.corners); + if(!contain_type) + return; + else{ + for(Cullable* object : node->objects) + if( + type==SP_TREE_LOOSE_CULL || + (type==SP_TREE_STRICT_CULL && + contain_type==BOX_FRUSTUM_INSIDE || + (contain_type==BOX_FRUSTUM_INTERSECTS && frustum.CheckBox(object->bounds.corners)) + ) + ) + { + //object->lastSquaredDistMulThousand=(long)(WickedMath::DistanceEstimated(object->bounds.getCenter(),frustum.getCamPos())*1000); + objects.insert(object); + } + if(node->count){ + for(int i=0;ichildren.size();++i) + getVisible(node->children[i],frustum,objects,type); + } + } +} +void SPTree::getVisible(Node* node, AABB& frustum, CulledList& objects, int type){ + if(!node) return; + int contain_type = frustum.intersects(node->box); + if(!contain_type) + return; + else{ + for(Cullable* object : node->objects) + if( + type==SP_TREE_LOOSE_CULL || + (type==SP_TREE_STRICT_CULL && + contain_type==AABB::INSIDE || + (contain_type==INTERSECTS && frustum.intersects(object->bounds)) + ) + ){ + //object->lastSquaredDistMulThousand=(long)(WickedMath::DistanceEstimated(object->bounds.getCenter(),frustum.getCenter())*1000); + objects.insert(object); + } + if(node->count){ + for(int i=0;ichildren.size();++i) + getVisible(node->children[i],frustum,objects,type); + } + } +} +void SPTree::getVisible(Node* node, SPHERE& frustum, CulledList& objects, int type){ + if(!node) return; + int contain_type = frustum.intersects(node->box); + if(!contain_type) return; + else { + for(Cullable* object : node->objects) + if(frustum.intersects(object->bounds)){ + //object->lastSquaredDistMulThousand=(long)(WickedMath::DistanceEstimated(object->bounds.getCenter(),frustum.center)*1000); + objects.insert(object); + } + if(node->count){ + for(int i=0;ichildren.size();++i) + getVisible(node->children[i],frustum,objects,type); + } + } +} +void SPTree::getVisible(Node* node, RAY& frustum, CulledList& objects, int type){ + if(!node) return; + int contain_type = frustum.intersects(node->box); + if(!contain_type) return; + else { + for(Cullable* object : node->objects) + if(frustum.intersects(object->bounds)){ + //object->lastSquaredDistMulThousand=(long)(WickedMath::DistanceEstimated(object->bounds.getCenter(),frustum.center)*1000); + objects.insert(object); + } + if(node->count){ + for(int i=0;ichildren.size();++i) + getVisible(node->children[i],frustum,objects,type); + } + } +} +void SPTree::getAll(Node* node, CulledList& objects){ + if(node != nullptr){ + objects.insert(node->objects.begin(),node->objects.end()); + if(node->count){ + for(int i=0;ichildren.size();++i) + getAll(node->children[i],objects); + } + } +} + +SPTree* SPTree::updateTree(Node* node){ + if(this && node) + { + + vector bad(0); + for(Cullable* object : node->objects){ +#ifdef SP_TREE_BOX_CONTAIN + if( node->box.intersects(object->bounds)!=AABB::INSIDE ) +#else + if( !node->box.intersects(object->translation) ) +#endif + bad.push_back(object); + } + + if(!bad.empty()){ + + for(Cullable* object : bad){ + node->objects.remove(object); + } + + if(node->parent){ + AddObjects(node->parent,bad); + } + else{ + CulledList culledItems; + getVisible(node,node->box,culledItems); + for(Cullable* item : culledItems){ + bad.push_back(item); + } + + SPTree* tree; + if(childCount==8) + tree = new Octree(); + else + tree = new QuadTree(); + AABB nbb = node->box*1.5; + tree->initialize(bad,nbb.getMin(),nbb.getMax()); + return tree; + } + } + if(!node->children.empty()){ + for(int i=0;ichildren.size();++i) + updateTree(node->children[i]); + } + + } + return NULL; +} diff --git a/WickedEngine/SPTree.h b/WickedEngine/SPTree.h new file mode 100644 index 000000000..6acc4316f --- /dev/null +++ b/WickedEngine/SPTree.h @@ -0,0 +1,82 @@ +#pragma once +#include "WickedEngine.h" +#include "WickedLoader.h" + +class Frustum; +// +//struct cullable_comparator { +// bool operator() (const Cullable* a, const Cullable* b) const{ +// return a->lastSquaredDistMulThousand<=b->lastSquaredDistMulThousand; +// } +//}; +//typedef set CulledList; +typedef set CulledList; + +typedef set CulledObjectList; +typedef unordered_map CulledCollection; + + +class SPTree +{ +protected: + int childCount; + SPTree(); +public: + ~SPTree(); + void initialize(const vector& objects); + void initialize(const vector& objects, const XMFLOAT3& newMin, const XMFLOAT3& newMax); + + struct Node{ + int depth; + AABB box; + vector children; + Node* parent; + int count; + list objects; + + Node(Node* newParent, const AABB& newBox = AABB(), int newDepth = 0):parent(newParent),box(newBox),depth(newDepth){ + count=0; + objects.resize(0); + } + static void swap(Node*& a, Node*& b){ + Node* c = a; + a=b; + b=c; + } + void Release(){ + if(this){ + for(unsigned int i=0;iRelease(); + children[i]=NULL; + } + children.clear(); + objects.clear(); + delete this; + } + } + }; + Node* root; +#define SP_TREE_STRICT_CULL 0 +#define SP_TREE_LOOSE_CULL 1 + void AddObjects(Node* node, const vector& newObjects); + static void getVisible(Node* node, Frustum& frustum, CulledList& objects, int type = SP_TREE_STRICT_CULL); + static void getVisible(Node* node, AABB& frustum, CulledList& objects, int type = SP_TREE_STRICT_CULL); + static void getVisible(Node* node, SPHERE& frustum, CulledList& objects, int type = SP_TREE_STRICT_CULL); + static void getVisible(Node* node, RAY& frustum, CulledList& objects, int type = SP_TREE_STRICT_CULL); + static void getAll(Node* node, CulledList& objects); + SPTree* updateTree(Node* node); + + void CleanUp(); +}; + +class Octree : public SPTree +{ +public: + Octree(){childCount=8;} +}; +class QuadTree : public SPTree +{ +public: + QuadTree(){childCount=4;} +}; \ No newline at end of file diff --git a/WickedEngine/ScriptEngine.cpp b/WickedEngine/ScriptEngine.cpp new file mode 100644 index 000000000..185a5acb4 --- /dev/null +++ b/WickedEngine/ScriptEngine.cpp @@ -0,0 +1,509 @@ +#include "Renderer.h" + +extern Camera* cam; + +int Character::Script_getParamCount(const char* script){ + if(!strcmp("move",script)) return 6; + else if(!strcmp("movemul",script)) return 3; + else if(!strcmp("dash",script)) return 3; + else if(!strcmp("toggleAction",script)) return 2; + else if(!strcmp("onHitAction",script)) return 1; + else if(!strcmp("ribbonTrail",script)) return 6; + else if(!strcmp("clip",script)) return 6; + else if(!strcmp("block",script)) return 6; + else if(!strcmp("image",script)) return -1; //-1 : read block + else if(!strcmp("sound",script)) return 2; + else if(!strcmp("cam",script)) return 5; + else if(!strcmp("burst",script)) return 3; + else if(!strcmp("emit",script)) return 3; + else if(!strcmp("opacity",script)) return 3; + else if(!strcmp("detach",script)) return 2; + else if(!strcmp("decal",script)) return 8; + return 0; +} +void Character::Script_Remove(MoveListItem& move, int scriptI){ + //move.scripts.erase(move.scripts.begin()+scriptI); + move.scripts[scriptI].Deactivate(); +} + +void Character::Script_ExecuteAll(MoveListItem& move){ + Armature* armature = move.armature; + if(armature!=nullptr){ + float cf = armature->currentFrame; + for(int i=0;i params = move.scripts[i].params; + + if( !script.compare("move")) Script_Move(move,i,cf); + else if(!script.compare("movemul")) Script_MoveMul(move,i,cf); + else if(!script.compare("dash")) Script_Dash(move,i,cf); + else if(!script.compare("toggleAction")) Script_ToggleAction(move,i,cf); + else if(!script.compare("toggleAction")) Script_OnHitAction(move,i,cf); + else if(!script.compare("ribbonTrail")) Script_RibbonTrail(move,i,cf); + else if(!script.compare("clip")) Script_Clip(move,i,cf); + else if(!script.compare("block")) Script_Block(move,i,cf); + else if(!script.compare("image")) Script_Image(move,i,cf); + else if(!script.compare("sound")) Script_Sound(move,i,cf); + else if(!script.compare("cam")) Script_Cam(move,i,cf); + else if(!script.compare("burst")) Script_Burst(move,i,cf); + else if(!script.compare("emit")) Script_Emit(move,i,cf); + else if(!script.compare("opacity")) Script_Opacity(move,i,cf); + else if(!script.compare("detach")) Script_Detach(move,i,cf); + else if(!script.compare("decal")) Script_Decal(move,i,cf); + } + } + } + //move.scripts.clear(); +} + +void Character::Script_Move(const MoveListItem& move, int scriptI, float currentFrame){ + if(currentFrame>=atoi(move.scripts[scriptI].params[4].c_str()) + && currentFrame<=atoi(move.scripts[scriptI].params[5].c_str())){ + XMFLOAT2 vector = XMFLOAT2(atof(move.scripts[scriptI].params[0].c_str()),atof(move.scripts[scriptI].params[1].c_str())); + float mod = (combatants[move.armature].prevFacing<0)?(-1.0f):(1.0f); + + combatants[move.armature].velocity.x+=vector.x*mod; + combatants[move.armature].velocity.y+=vector.y; + + float maxX=atof(move.scripts[scriptI].params[2].c_str()); + float maxY=atof(move.scripts[scriptI].params[3].c_str()); + if( abs(combatants[move.armature].velocity.x)>abs(maxX) ) + combatants[move.armature].velocity.x=maxX*mod; + if( abs(combatants[move.armature].velocity.y)>abs(maxY) ) + combatants[move.armature].velocity.y=maxY; + } +} +void Character::Script_MoveMul(MoveListItem& move, int scriptI, float currentFrame){ + int frame = atoi(move.scripts[scriptI].params[2].c_str()); + if((frame>=0 && currentFrame>=frame) || (frame<0 && move.recordedHit.active) ){ + XMFLOAT2 vector = XMFLOAT2(atof(move.scripts[scriptI].params[0].c_str()),atof(move.scripts[scriptI].params[1].c_str())); + combatants[move.armature].velocity.x*=vector.x; + combatants[move.armature].velocity.y*=vector.y; + Script_Remove(move,scriptI); + } +} +void Character::Script_Dash(MoveListItem& move, int scriptI, float currentFrame){ + if(currentFrame>=atoi(move.scripts[scriptI].params[2].c_str())){ + XMFLOAT2 vector = XMFLOAT2(atof(move.scripts[scriptI].params[0].c_str()),atof(move.scripts[scriptI].params[1].c_str())); + float mod = (combatants[move.armature].prevFacing<0)?(-1.0f):(1.0f); + combatants[move.armature].velocity.x+=vector.x*mod; + combatants[move.armature].velocity.y+=vector.y; + Script_Remove(move,scriptI); + } +} +void Character::Script_ToggleAction(MoveListItem& move, int scriptI, float currentFrame){ + if(atoi(move.scripts[scriptI].params[1].c_str())==(int)currentFrame){ + MoveListItem* a = getMoveByName(move.scripts[scriptI].params[0]); + if(a){ + move = *a; + ChangeArmatureAction(move.armature,move.actionI); + } + //Script_Remove(move,scriptI); + } +} +void Character::Script_OnHitAction(MoveListItem& move, int scriptI, float currentFrame){ + +} +void Character::Script_RibbonTrail(MoveListItem& move, int scriptI, float currentFrame){ + if(currentFrame>=atoi(move.scripts[scriptI].params[1].c_str()) + && currentFrame<=atoi(move.scripts[scriptI].params[2].c_str())){ + stringstream objectName(""); + objectName<name.compare(objectName.str())){ + trailObject=object; + break; + } + } + + if(trailObject!=nullptr){ + IncreaseTrails( + trailObject, + XMFLOAT3( + atof(move.scripts[scriptI].params[3].c_str()) / 256.0f + ,atof(move.scripts[scriptI].params[4].c_str()) / 256.0f + ,atof(move.scripts[scriptI].params[5].c_str()) / 256.0f + ) + ); + } + } + else if(currentFrame > atoi(move.scripts[scriptI].params[2].c_str())) + Script_Remove(move,scriptI); +} +void Character::Script_Clip(MoveListItem& move, int scriptI, float currentFrame){ + int frame0 = atoi(move.scripts[scriptI].params[0].c_str()); + int frame1 = atoi(move.scripts[scriptI].params[1].c_str()); + if(currentFrame>=frame0 + && currentFrame<=frame1){ + XMFLOAT4 box = XMFLOAT4(atof(move.scripts[scriptI].params[2].c_str()),atof(move.scripts[scriptI].params[3].c_str()) + ,atof(move.scripts[scriptI].params[4].c_str()),atof(move.scripts[scriptI].params[5].c_str())); + combatants[move.armature].clip.pos.x+=combatants[move.armature].prevFacing*box.x; + combatants[move.armature].clip.pos.y+=box.y; + combatants[move.armature].clip.siz.x=box.z; + combatants[move.armature].clip.siz.y=box.w; + } + else if(currentFrame > frame1) + Script_Remove(move,scriptI); +} +void Character::Script_Block(MoveListItem& move, int scriptI, float currentFrame){ + if(currentFrame>=atoi(move.scripts[scriptI].params[0].c_str()) + && currentFrame<=atoi(move.scripts[scriptI].params[1].c_str())){ + XMFLOAT4 box = XMFLOAT4(atof(move.scripts[scriptI].params[2].c_str()),atof(move.scripts[scriptI].params[3].c_str()) + ,atof(move.scripts[scriptI].params[4].c_str()),atof(move.scripts[scriptI].params[5].c_str())); + Combatant& combatant = combatants[move.armature]; + combatant.block=Hitbox2D(); + combatant.block.pos.x=combatants[move.armature].position.x; + combatant.block.pos.y=combatants[move.armature].position.y; + combatant.block.pos.x+=combatants[move.armature].prevFacing*box.x; + combatant.block.pos.y+=box.y; + combatant.block.siz.x=box.z; + combatant.block.siz.y=box.w; + } + else if(currentFrame > atoi(move.scripts[scriptI].params[1].c_str())) + Script_Remove(move,scriptI); +} +void Character::Script_Image(MoveListItem& move, int scriptI, float currentFrame){ + Combatant& combatant = combatants[move.armature]; + int paramC = move.scripts[scriptI].params.size(); + bool activate = false; + bool toRemove = false; + for(int i=0;ieffects.pos=move.recordedHit.pos; + if(combatant.facing<0){ + } + string texture="",mask="",normal=""; + + int i=0; + while(ieffects.blendFlag=BLENDMODE::BLENDMODE_ALPHA; + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"additive")) image->effects.blendFlag=BLENDMODE::BLENDMODE_ADDITIVE; + else image->effects.blendFlag=BLENDMODE::BLENDMODE_OPAQUE; + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"size")){ + image->effects.siz=XMFLOAT2(atof(move.scripts[scriptI].params[i+1].c_str()),atof(move.scripts[scriptI].params[i+2].c_str())); + i+=2; + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"scale")){ + image->effects.scale=XMFLOAT2(atof(move.scripts[scriptI].params[i+1].c_str()),atof(move.scripts[scriptI].params[i+2].c_str())); + i+=2; + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"drawRec")){ + image->effects.drawRec=XMFLOAT4( + atof(move.scripts[scriptI].params[i+1].c_str()),atof(move.scripts[scriptI].params[i+2].c_str()) + ,atof(move.scripts[scriptI].params[i+3].c_str()),atof(move.scripts[scriptI].params[i+4].c_str()) + ); + i+=4; + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"position")){ + image->effects.pos=XMFLOAT3( + combatant.position.x+atof(move.scripts[scriptI].params[i+1].c_str()),combatant.position.y+atof(move.scripts[scriptI].params[i+2].c_str()) + ,atof(move.scripts[scriptI].params[i+3].c_str()) + ); + i+=3; + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"rotation")){ + if(!strcmp(move.scripts[scriptI].params[i+1].c_str(),"random")) + image->effects.rotation = rand()%360; + else image->effects.rotation=atoi(move.scripts[scriptI].params[i+1].c_str()); + i+=1; + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"sphere")){ + HitSphere* sph = getSphereByName(move.scripts[scriptI].params[++i].c_str()); + if(sph!=nullptr) + image->effects.pos=sph->translation; + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"animation")){ + stack blocks; + if(!strcmp(move.scripts[scriptI].params[++i].c_str(),"{")) blocks.push('{'); + while(!blocks.empty()){ + ++i; + if(!strcmp(move.scripts[scriptI].params[i].c_str(),"}")){ blocks.pop(); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"opa")){ image->anim.opa=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"fad")){ image->anim.fad=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"repeatable")){ image->anim.repeatable=atoi(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"velocity")){ + image->anim.vel=XMFLOAT3(atof(move.scripts[scriptI].params[++i].c_str()),atof(move.scripts[scriptI].params[++i].c_str()),atof(move.scripts[scriptI].params[++i].c_str())); + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"rot")){ image->anim.rot=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"opa")){ image->anim.opa=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"scaleX")){ image->anim.scaleX=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"scaleY")){ image->anim.scaleY=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"movingTexAnim")){ + stack blockM; + if(!strcmp(move.scripts[scriptI].params[++i].c_str(),"{")) blockM.push('{'); + while(!blockM.empty()){ + ++i; + if(!strcmp(move.scripts[scriptI].params[i].c_str(),"}")) blockM.pop(); + if(!strcmp(move.scripts[scriptI].params[i].c_str(),"speedX")){ image->anim.movingTexAnim.speedX=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"speedY")){ image->anim.movingTexAnim.speedY=atof(move.scripts[scriptI].params[++i].c_str()); } + } + } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"drawRecAnim")){ + stack blockD; + if(!strcmp(move.scripts[scriptI].params[++i].c_str(),"{")) blockD.push('{'); + while(!blockD.empty()){ + ++i; + if(!strcmp(move.scripts[scriptI].params[i].c_str(),"}")) blockD.pop(); + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"frameCount")){ image->anim.drawRecAnim.frameCount=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"jumpX")){ image->anim.drawRecAnim.jumpX=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"jumpY")){ image->anim.drawRecAnim.jumpY=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"onFrameChangeWait")){ image->anim.drawRecAnim.onFrameChangeWait=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"sizX")){ image->anim.drawRecAnim.sizX=atof(move.scripts[scriptI].params[++i].c_str()); } + else if(!strcmp(move.scripts[scriptI].params[i].c_str(),"sizY")){ image->anim.drawRecAnim.sizY=atof(move.scripts[scriptI].params[++i].c_str()); } + } + } + } + } + + ++i; + } + + //if(move.armature) + // image->effects.mirror=combatants[move.armature].facing; + image->effects.typeFlag=WORLD; + image->effects.pivotFlag=Pivot::CENTER; + image->effects.sampleFlag=SAMPLEMODE_WRAP; + stringstream texF,masF,norF; + texF<<"images/"<CreateReference(texF.str(),masF.str(),norF.str()); + images.push_back(image); + if(toRemove) Script_Remove(move,scriptI); + } + else return; +} +void Character::Script_Sound(MoveListItem& move, int scriptI, float currentFrame){ + int trigger = atoi(move.scripts[scriptI].params[0].c_str()); + if(trigger>=0 && currentFrame>=trigger){ + ((Sound*)ResourceManager::add(move.scripts[scriptI].params[1]))->Play(); + Script_Remove(move,scriptI); + } + else if(trigger<0 && move.recordedHit.active){ + ((Sound*)ResourceManager::add(move.scripts[scriptI].params[1]))->Play(); + } +} +void Character::Script_Cam(MoveListItem& move, int scriptI, float currentFrame){ + static Camera* camMemory = new Camera(1280,720,cam->zNearP,cam->zFarP,XMVectorSet(0,0,0,1)); + + string cn = move.scripts[scriptI].params[0]; + int f1s = atoi(move.scripts[scriptI].params[1].c_str()); + int f1e = atoi(move.scripts[scriptI].params[2].c_str()); + int f2s = atoi(move.scripts[scriptI].params[3].c_str()); + int f2e = atoi(move.scripts[scriptI].params[4].c_str()); + + int ai = 0, bi = 0; + int camI = -1; + int i=0; + static ActionCamera cc; + for(ActionCamera& c : cameras) + if(!cn.compare(c.name)){ + cc = c; + camI=1; + } + + if(camI>=0){ + XMVECTOR eye,at,up; + eye=XMVectorSet(0,0,0,1); + at=XMVectorSet(0,-1,0,1); + up=XMVectorSet(0,0,1,1); + if((int)currentFrame<=f1s){ //SAVE CAMERA + camMemory->Eye=cam->Eye; + camMemory->refEye=cam->refEye; + camMemory->leftrightRot=cam->leftrightRot; + camMemory->updownRot=cam->updownRot; + camMemory->Up=cam->Up; + StopTime(); + } + else if(currentFrame>f1s && currentFramef1s && currentFrame<=f1e){ //FOCUS + ip = (float)(currentFrame-f1s)/(float)(f1e-f1s); + MIDPOS = XMVectorLerp(camMemory->Eye,POS,ip); + MIDROT = XMQuaternionSlerp(XMVectorSet(-0.707f,0,0,0.707f),ROT,ip); + } + else if(currentFrame>f1e && currentFrame=f2s && currentFrameEye,ip); + MIDROT = XMQuaternionSlerp(ROT,XMVectorSet(-0.707f,0,0,0.707f),ip); + } + + MR = XMMatrixRotationQuaternion( MIDROT ); + eye=MIDPOS; + up=XMVector3Transform(up, MR ); + at=XMVector3Transform(at, MR ); + at+=eye; + + cam->View = XMMatrixLookAtLH(eye,at,up); + cam->Eye=eye; + XMVECTOR refEye = XMVectorMultiply(eye,XMVectorSet(1,-1,1,1)); + XMVECTOR refAt = XMVectorMultiply(at,XMVectorSet(1,-1,1,1)); + XMVECTOR camRight = XMVector3Transform(XMVectorSet(1,0,0,0),MR); + XMVECTOR invUp = XMVector3Cross(camRight,XMVectorSubtract(refAt,refEye)); + cam->refView = XMMatrixLookAtLH(refEye,refAt,up); + } + else{ //RESTORE CAMERA + cam->Eye=camMemory->Eye; + cam->refEye=camMemory->refEye; + cam->leftrightRot=camMemory->leftrightRot; + cam->updownRot=camMemory->updownRot; + cam->Up=camMemory->Up; + + UnStopTime(); + Script_Remove(move,scriptI); + } + } + else + return; +} +void Character::Script_Burst(MoveListItem& move, int scriptI, float currentFrame){ + stringstream systemName(""); + systemName<=0 && currentFrame>=frame) || (frame<0 && move.recordedHit.active) ){ + + map>::iterator iter=emitterSystems.find(systemName.str()); + if(iter!=emitterSystems.end()){ + for(EmittedParticle* e:iter->second){ + e->Burst(burstNum); + Script_Remove(move,scriptI); + } + } + + //bool found = false; + //for(Object* o : e_objects){ + // for(int j=0;jeParticleSystems.size();++j){ + // if(!o->eParticleSystems[j]->name.compare(particleSystem)){ + // o->eParticleSystems[j]->Burst(burstNum); + // Script_Remove(move,scriptI); + // found = true; + // break; + // } + // } + // if(found) + // break; + //} + } +} +void Character::Script_Emit(MoveListItem& move, int scriptI, float currentFrame){ + stringstream systemName(""); + systemName< interval(move.scripts[scriptI].params[2]); + + if( GetGameSpeed() && currentFrame-(int)currentFrame<=GetGameSpeed() && interval.contains(currentFrame) ){ + + map>::iterator iter=emitterSystems.find(systemName.str()); + if(iter!=emitterSystems.end()){ + for(EmittedParticle* e:iter->second){ + e->Burst(burstNum); + } + } + + //bool found = false; + //for(Object* o : e_objects){ + // for(int j=0;jeParticleSystems.size();++j){ + // if(!o->eParticleSystems[j]->name.compare(particleSystem)){ + // o->eParticleSystems[j]->Burst(burstNum); + // //Script_Remove(move,scriptI); + // found = true; + // break; + // } + // } + // if(found) + // break; + //} + } +} +void Character::Script_Opacity(MoveListItem& move, int scriptI, float currentFrame){ + stringstream materialName(""); + materialName< frames(move.scripts[scriptI].params[1]); + Interval values(move.scripts[scriptI].params[2]); + + if(frames.contains(currentFrame) ){ + + MaterialCollection::iterator iter=materials.find(materialName.str()); + if(iter!=materials.end()){ + iter->second->alpha=values.end; + //iter->second->alpha = WickedMath::Lerp(values.begin,values.end,(currentFrame-frames.begin)/abs(frames.end-frames.begin)); + } + + } +} +void Character::Script_Detach(MoveListItem& move, int scriptI, float currentFrame){ + int frame = 0; + if(!move.scripts[scriptI].params[1].compare("end")) + frame=move.armature->actions[move.armature->activeAction].frameCount-1; + else + frame = atoi( move.scripts[scriptI].params[1].c_str() ); + stringstream node(""); + node<detachChild(); + } + Script_Remove(move,scriptI); + } +} +void Character::Script_Decal(MoveListItem& move, int scriptI, float currentFrame){ + int frame = atoi(move.scripts[scriptI].params[0].c_str()); + + if( (frame>=0 && currentFrame>=frame) || (frame<0 && move.recordedHit.active) ){ + + string& texture = move.scripts[scriptI].params[1]; + XMFLOAT3 armaturePos = move.armature->translation_rest; + XMFLOAT3 translation,scale; + XMFLOAT4 rotation; + + translation=XMFLOAT3(armaturePos.x+(getMainCombatant().facing)*atof(move.scripts[scriptI].params[2].c_str()),armaturePos.y+atof(move.scripts[scriptI].params[3].c_str()),armaturePos.z+atof(move.scripts[scriptI].params[4].c_str())); + scale=XMFLOAT3(atof(move.scripts[scriptI].params[5].c_str()),atof(move.scripts[scriptI].params[6].c_str()),atof(move.scripts[scriptI].params[7].c_str())); + XMStoreFloat4(&rotation,XMQuaternionRotationRollPitchYaw(XM_PI*0.5f,0,XM_PI*((rand()%1000)*0.001f))); + + Decal* decal = new Decal(translation,scale,rotation,texture); + decal->life=800; + decal->fadeStart=40; + decals.push_back(decal); + + Script_Remove(move,scriptI); + } +} + diff --git a/WickedEngine/Server.cpp b/WickedEngine/Server.cpp new file mode 100644 index 000000000..cf668b3dc --- /dev/null +++ b/WickedEngine/Server.cpp @@ -0,0 +1,138 @@ +#include "Server.h" + +using namespace std; + + +Server::Server(const string& newName, int port, const string& ipaddress) +{ + name=newName; + if(ListenOnPort(port,ipaddress.length()<=1?"0.0.0.0":ipaddress.c_str())){ + stringstream ss(""); + ss<<"Listening as "<::iterator it = clients.begin(); it != clients.end(); ++it) { + closesocket(it->first); + } + Network::~Network(); +} + +bool Server::ListenOnPort(int portno, const char* ipaddress) +{ + int error = WSAStartup(SCK_VERSION2,&w); + + if(error) + return false; + + if(w.wVersion != SCK_VERSION2){ + WSACleanup(); + return false; + } + + SOCKADDR_IN addr; + addr.sin_family = AF_INET; + addr.sin_port= htons(portno); + //addr.sin_addr.S_un.S_addr = htonl(INADDR_ANY); //or inet_addr("0.0.0.0"); + addr.sin_addr.S_un.S_addr = inet_addr(ipaddress); + + s = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + + + //set master socket to allow multiple connections , this is just a good habit, it will work without this + int opt = TRUE; + if( setsockopt(s, SOL_SOCKET, SO_REUSEADDR, (char *)&opt, sizeof(opt)) < 0 ) + return false; + + + if( setsockopt(s, SOL_SOCKET, TCP_NODELAY, (char *)&opt, sizeof(opt)) < 0 ) + return false; + + if(s == INVALID_SOCKET) + return false; + + if(::bind(s, (LPSOCKADDR)&addr, sizeof(addr)) == SOCKET_ERROR) + return false; //couldn't bind (for example if you try to bind more than once to the same socket + + //if( listen(s,SOMAXCONN) == SOCKET_ERROR ) + if( listen(s,SOMAXCONN) == SOCKET_ERROR ) + return false; + + + + return true; +} +SOCKET Server::CreateAccepter(){ + struct sockaddr_in caller; + int addrlen = sizeof(caller); + SOCKET newsock = accept(s,(struct sockaddr *) &caller, &addrlen); + + //inform user of socket number - used in send and receive commands + if(newsock != SOCKET_ERROR){ + //printf("\nNew connection , socket fd is %d , ip is : %s , port : %d \n" , newsock , inet_ntoa(caller.sin_addr) , ntohs(caller.sin_port)); + } + else{ + //printf("\nConnection FAILED on socket: fd is %d , ip is : %s , port : %d \n" , newsock , inet_ntoa(caller.sin_addr) , ntohs(caller.sin_port)); + } + + return newsock; +} + + + +vector Server::listClients() +{ + vector ret(0); + for (map::iterator it = clients.begin(); it != clients.end(); ++it) { + stringstream ss(""); + ss<second<<":"<first; + ret.push_back(ss.str()); + } + return ret; +} + +bool Server::sendText(const string& text, int packettype, const string& clientName, int clientID){ + int sentTo=0; + + if(clientName.length()<=0){ //send to everyone + for (map::iterator it = clients.begin(); it != clients.end(); ++it) { + sentTo += Network::sendData(packettype,it->first) && Network::sendText(text,it->first); + } + } + else if(clientID<0){ //send to all of same name + for (map::iterator it = clients.begin(); it != clients.end(); ++it) { + if(!clientName.compare(it->second)){ + sentTo += Network::sendData(packettype,it->first) && Network::sendText(text,it->first); + } + } + } + else{ //send to specific client + if(clients.find(clientID) != clients.end()){ + sentTo += Network::sendData(packettype,clientID) && Network::sendText(text,clientID); + } + } + + if(sentTo<=0) + BackLog::post("No client was found with the specified parameters"); + + return sentTo>0; +} + +bool Server::changeName(const string& newName){ + Network::changeName(newName); + return sendText(newName, Network::PACKET_TYPE_CHANGENAME); +} +bool Server::sendMessage(const string& text, const string& clientName, int clientID){ + return sendText(text, Network::PACKET_TYPE_TEXTMESSAGE, clientName, clientID); +} \ No newline at end of file diff --git a/WickedEngine/Server.h b/WickedEngine/Server.h new file mode 100644 index 000000000..e4602956b --- /dev/null +++ b/WickedEngine/Server.h @@ -0,0 +1,154 @@ +#ifndef SERVER_H +#define SERVER_H + +#include "Network.h" +#include +#include + + +class Server : public Network +{ +private: + map clients; +public: + Server(const string& newName = "SERVER", int port = PORT, const string& ipaddress = "0.0.0.0"); + ~Server(void); + + bool active(){return !clients.empty();} + + + bool sendText(const string& text, int packettype, const string& clientName = "", int clientID = -1); + template + bool sendData(const T& value){ + if(clients.begin()==clients.end()) + return false; + Network::sendData(PACKET_TYPE_OTHER,clients.begin()->first); + return Network::sendData(value,clients.begin()->first); + } + template + bool receiveData(T& value){ + if(clients.begin()==clients.end()) + return false; + return Network::receiveData(value,clients.begin()->first); + } + + + bool changeName(const string& newName); + bool sendMessage(const string& text, const string& clientName = "", int clientID = -1); + + bool ListenOnPort(int portno, const char* ipaddress); + SOCKET CreateAccepter(); + + template + void Poll(T& data) + { + + + //clear the socket set + FD_ZERO(&readfds); + + //add master socket to set + FD_SET(s, &readfds); + SOCKET max_sd = s; + + + for (map::iterator it = clients.begin(); it != clients.end(); ++it) { + if(it->first>0) + FD_SET( it->first, &readfds ); + if(it->first>max_sd) + max_sd=it->first; + } + + + timeval time=timeval(); + time.tv_sec=0; + time.tv_usec=1; + + if ((select( max_sd + 1 , &readfds , NULL , NULL , &time) < 0) && (errno!=EINTR)) + { + BackLog::post("select error"); + } + + //If something happened on the master socket , then its an incoming connection + if (FD_ISSET(s, &readfds)) + { + SOCKET new_socket = CreateAccepter(); + + if(new_socket != SOCKET_ERROR){ + if(Network::sendText(name,new_socket)){ + stringstream ss(""); + ss<<"Name sent, client ["<(new_socket,ss.str())); + + } + else + BackLog::post("Welcome message could NOT be sent, client ignored"); + + } + } + + //else its some IO operation on some other socket :) + for (map::iterator it = clients.begin(); it != clients.end(); ) { + + if (FD_ISSET( it->first , &readfds)) + { + int command; + bool receiveSuccess = Network::receiveData(command,it->first); + + if(!receiveSuccess){ + BackLog::post("Client disconnected."); + closesocket(it->first); + clients.erase(it++); + continue; + } + else{ + + switch(command){ + case PACKET_TYPE_CHANGENAME: + { + string text; + if(receiveText(text,it->first)){ + stringstream ss(""); + ss<<"Client "<second<<" now registered as "<second=text; + } + break; + } + case PACKET_TYPE_TEXTMESSAGE: + { + string text; + if(receiveText(text,it->first)){ + stringstream ss(""); + ss<second<<": "<first); + break; + } + default: + break; + } + } + + } + ++it; + } + + } + + + vector listClients(); + + +}; + +#endif \ No newline at end of file diff --git a/WickedEngine/Sound.cpp b/WickedEngine/Sound.cpp new file mode 100644 index 000000000..bd14e4456 --- /dev/null +++ b/WickedEngine/Sound.cpp @@ -0,0 +1,314 @@ +#include "Sound.h" + +IXAudio2* SoundEffect::pXAudio2; +IXAudio2MasteringVoice* SoundEffect::pMasterVoice; +bool SoundEffect::INITIALIZED=false; + +IXAudio2* Music::pXAudio2; +IXAudio2MasteringVoice* Music::pMasterVoice; +bool Music::INITIALIZED=false; + +using namespace std; + + + + +HRESULT Sound::FindChunk(HANDLE hFile, DWORD fourcc, DWORD & dwChunkSize, DWORD & dwChunkDataPosition) +{ + HRESULT hr = S_OK; + if( INVALID_SET_FILE_POINTER == SetFilePointer( hFile, 0, NULL, FILE_BEGIN ) ) + return HRESULT_FROM_WIN32( GetLastError() ); + + DWORD dwChunkType; + DWORD dwChunkDataSize; + DWORD dwRIFFDataSize = 0; + DWORD dwFileType; + DWORD bytesRead = 0; + DWORD dwOffset = 0; + + while (hr == S_OK) + { + DWORD dwRead; + if( 0 == ReadFile( hFile, &dwChunkType, sizeof(DWORD), &dwRead, NULL ) ) + hr = HRESULT_FROM_WIN32( GetLastError() ); + + if( 0 == ReadFile( hFile, &dwChunkDataSize, sizeof(DWORD), &dwRead, NULL ) ) + hr = HRESULT_FROM_WIN32( GetLastError() ); + + switch (dwChunkType) + { + case fourccRIFF: + dwRIFFDataSize = dwChunkDataSize; + dwChunkDataSize = 4; + if( 0 == ReadFile( hFile, &dwFileType, sizeof(DWORD), &dwRead, NULL ) ) + hr = HRESULT_FROM_WIN32( GetLastError() ); + break; + + default: + if( INVALID_SET_FILE_POINTER == SetFilePointer( hFile, dwChunkDataSize, NULL, FILE_CURRENT ) ) + return HRESULT_FROM_WIN32( GetLastError() ); + } + + dwOffset += sizeof(DWORD) * 2; + + if (dwChunkType == fourcc) + { + dwChunkSize = dwChunkDataSize; + dwChunkDataPosition = dwOffset; + return S_OK; + } + + dwOffset += dwChunkDataSize; + + if (bytesRead >= dwRIFFDataSize) return S_FALSE; + + } + + return S_OK; + +} +HRESULT Sound::ReadChunkData(HANDLE hFile, void * buffer, DWORD buffersize, DWORD bufferoffset) +{ + HRESULT hr = S_OK; + if( INVALID_SET_FILE_POINTER == SetFilePointer( hFile, bufferoffset, NULL, FILE_BEGIN ) ) + return HRESULT_FROM_WIN32( GetLastError() ); + DWORD dwRead; + if( 0 == ReadFile( hFile, buffer, buffersize, &dwRead, NULL ) ) + hr = HRESULT_FROM_WIN32( GetLastError() ); + return hr; +} +HRESULT Sound::OpenFile(TCHAR* strFileName) +{ + HRESULT hr; + + wfx = WAVEFORMATEX(); + buffer = XAUDIO2_BUFFER(); + + // Open the file + HANDLE hFile = CreateFile( + strFileName, + GENERIC_READ, + FILE_SHARE_READ, + NULL, + OPEN_EXISTING, + 0, + NULL ); + + if( INVALID_HANDLE_VALUE == hFile ) + return HRESULT_FROM_WIN32( GetLastError() ); + + if( INVALID_SET_FILE_POINTER == SetFilePointer( hFile, 0, NULL, FILE_BEGIN ) ) + return HRESULT_FROM_WIN32( GetLastError() ); + + + + + DWORD dwChunkSize; + DWORD dwChunkPosition; + //check the file type, should be fourccWAVE or 'XWMA' + FindChunk(hFile,fourccRIFF,dwChunkSize, dwChunkPosition ); + DWORD filetype; + if(FAILED( hr = ReadChunkData(hFile,&filetype,sizeof(DWORD),dwChunkPosition) )) + return hr; + if (filetype != fourccWAVE) + return S_FALSE; + + + + if(FAILED( hr = FindChunk(hFile,fourccFMT, dwChunkSize, dwChunkPosition ) )) + return hr; + if(FAILED( hr = ReadChunkData(hFile, &wfx, dwChunkSize, dwChunkPosition ) )) + return hr; + wfx.wFormatTag=WAVE_FORMAT_PCM; + + + //fill out the audio data buffer with the contents of the fourccDATA chunk + if(FAILED( hr = FindChunk(hFile,fourccDATA,dwChunkSize, dwChunkPosition ) )) + return hr; + BYTE * pDataBuffer = new BYTE[dwChunkSize]; + if(FAILED( hr = ReadChunkData(hFile, pDataBuffer, dwChunkSize, dwChunkPosition) )) + return hr; + + + buffer.AudioBytes = dwChunkSize; //buffer containing audio data + buffer.pAudioData = pDataBuffer; //size of the audio buffer in bytes + buffer.Flags = XAUDIO2_END_OF_STREAM; // tell the source voice not to expect any data after this buffer + + + return EXIT_SUCCESS; +} + + + +HRESULT Sound::Load(wstring filename) +{ + return OpenFile(filename._Myptr()); +} +HRESULT Sound::Load(string filename) +{ + return OpenFile(wstring(filename.begin(),filename.end())._Myptr()); +} +void Sound::DelayHelper(Sound* sound, DWORD delay){ + Sleep(delay); + sound->Play(); +} +HRESULT Sound::Play(DWORD delay) +{ + if(delay>0){ + thread(DelayHelper,this,delay).detach(); + return EXIT_SUCCESS; + } + return PlaySoundEffect(); +} +void Sound::Stop(){ + StopSoundEffect(); +} + + + +void SoundEffect::SetVolume(float vol){ + if(INITIALIZED && pMasterVoice != nullptr) + pMasterVoice->SetVolume(vol); +} +float SoundEffect::GetVolume(){ + float vol; + if(pMasterVoice != nullptr) + pMasterVoice->GetVolume(&vol); + return vol; +} +HRESULT SoundEffect::Initialize() +{ + HRESULT hr; + pXAudio2 = nullptr; + if(FAILED( hr = XAudio2Create( &pXAudio2, 0, XAUDIO2_DEFAULT_PROCESSOR ) )) + return hr; + pMasterVoice = nullptr; + if(FAILED( hr = pXAudio2->CreateMasteringVoice( &pMasterVoice ) )) + return hr; + + INITIALIZED=true; + return EXIT_SUCCESS; +} +void SoundEffect::CleanUp() +{ + if(pMasterVoice!=nullptr) pMasterVoice->DestroyVoice(); + if(pXAudio2!=nullptr) pXAudio2->Release(); + pMasterVoice=nullptr; + pXAudio2=nullptr; + INITIALIZED=false; +} +SoundEffect::SoundEffect() +{ + pSourceVoice=nullptr; + if(!INITIALIZED) Initialize(); +} +SoundEffect::SoundEffect(wstring filename) +{ + if(!INITIALIZED) Initialize(); + Load(filename); +} +SoundEffect::SoundEffect(string filename) +{ + if(!INITIALIZED) Initialize(); + Load(filename); +} +SoundEffect::~SoundEffect() +{ +} +HRESULT SoundEffect::PlaySoundEffect() +{ + + HRESULT hr; + pSourceVoice = nullptr; + + if( FAILED(hr = pXAudio2->CreateSourceVoice( &pSourceVoice, (WAVEFORMATEX*)&wfx ) ) ) + return hr; + + if( FAILED(hr = pSourceVoice->SubmitSourceBuffer( &buffer ) ) ) + return hr; + + if ( FAILED(hr = pSourceVoice->Start( 0 ) ) ) + return hr; + + return EXIT_SUCCESS; +} +void SoundEffect::StopSoundEffect() +{ + if(pSourceVoice!=nullptr){ + pSourceVoice->Stop(); + } +} + + +void Music::SetVolume(float vol){ + if(INITIALIZED && pMasterVoice != nullptr) + pMasterVoice->SetVolume(vol); +} +float Music::GetVolume(){ + float vol; + if(pMasterVoice != nullptr) + pMasterVoice->GetVolume(&vol); + return vol; +} +HRESULT Music::Initialize() +{ + HRESULT hr; + pXAudio2 = nullptr; + if(FAILED( hr = XAudio2Create( &pXAudio2, 0, XAUDIO2_DEFAULT_PROCESSOR ) )) + return hr; + pMasterVoice = nullptr; + if(FAILED( hr = pXAudio2->CreateMasteringVoice( &pMasterVoice ) )) + return hr; + + INITIALIZED=true; + return EXIT_SUCCESS; +} +void Music::CleanUp() +{ + if(pMasterVoice!=nullptr) pMasterVoice->DestroyVoice(); + if(pXAudio2!=nullptr) pXAudio2->Release(); + pMasterVoice=nullptr; + pXAudio2=nullptr; + INITIALIZED=false; +} +Music::Music() +{ + pSourceVoice=nullptr; + if(!INITIALIZED) Initialize(); +} +Music::Music(wstring filename) +{ + if(!INITIALIZED) Initialize(); + Load(filename); +} +Music::Music(string filename) +{ + if(!INITIALIZED) Initialize(); + Load(filename); +} +Music::~Music() +{ +} +HRESULT Music::PlaySoundEffect() +{ + + HRESULT hr; + pSourceVoice = nullptr; + + if( FAILED(hr = pXAudio2->CreateSourceVoice( &pSourceVoice, (WAVEFORMATEX*)&wfx ) ) ) + return hr; + + if( FAILED(hr = pSourceVoice->SubmitSourceBuffer( &buffer ) ) ) + return hr; + + if ( FAILED(hr = pSourceVoice->Start( 0 ) ) ) + return hr; + + return EXIT_SUCCESS; +} +void Music::StopSoundEffect() +{ + if(pSourceVoice!=nullptr){ + pSourceVoice->Stop(); + } +} diff --git a/WickedEngine/Sound.h b/WickedEngine/Sound.h new file mode 100644 index 000000000..fbe6b0b7c --- /dev/null +++ b/WickedEngine/Sound.h @@ -0,0 +1,107 @@ +#ifndef XAUDIO2 +#define XAUDIO2 + +//#include +////#include + + + +#if (_WIN32_WINNT >= 0x0602 /*_WIN32_WINNT_WIN8*/) +#include +#pragma comment(lib,"xaudio2.lib") +#else +#include "Xaudio2_7/comdecl.h" +#include "Xaudio2_7/xaudio2.h" +#endif + +#include +#include + + +#ifdef _XBOX //Big-Endian +#define fourccRIFF 'RIFF' +#define fourccDATA 'data' +#define fourccFMT 'fmt ' +#define fourccWAVE 'WAVE' +#define fourccXWMA 'XWMA' +#define fourccDPDS 'dpds' +#endif + +#ifndef _XBOX //Little-Endian +#define fourccRIFF 'FFIR' +#define fourccDATA 'atad' +#define fourccFMT ' tmf' +#define fourccWAVE 'EVAW' +#define fourccXWMA 'AMWX' +#define fourccDPDS 'sdpd' +#endif + +class Sound +{ +protected: + WAVEFORMATEX wfx; + XAUDIO2_BUFFER buffer; + IXAudio2SourceVoice* pSourceVoice; + + HRESULT FindChunk(HANDLE hFile, DWORD fourcc, DWORD & dwChunkSize, DWORD & dwChunkDataPosition); + HRESULT ReadChunkData(HANDLE hFile, void * buffer, DWORD buffersize, DWORD bufferoffset); + HRESULT OpenFile(TCHAR*); + virtual HRESULT PlaySoundEffect()=0; + virtual void StopSoundEffect()=0; +public: + + HRESULT Load(std::wstring); + HRESULT Load(std::string); + HRESULT Play(DWORD delay = 0); + void Stop(); + +private: + static void DelayHelper(Sound* sound, DWORD delay); +}; + +class SoundEffect : public Sound +{ +private: + static IXAudio2MasteringVoice* pMasterVoice; + static IXAudio2* pXAudio2; + static bool INITIALIZED; + HRESULT PlaySoundEffect(); + void StopSoundEffect(); + +public: + SoundEffect(); + SoundEffect(std::wstring); + SoundEffect(std::string); + ~SoundEffect(); + + static HRESULT Initialize(); + static void CleanUp(); + + static void SetVolume(float); + static float GetVolume(); +}; + +class Music : public Sound +{ +private: + static IXAudio2MasteringVoice* pMasterVoice; + static IXAudio2* pXAudio2; + static bool INITIALIZED; + HRESULT PlaySoundEffect(); + void StopSoundEffect(); + +public: + Music(); + Music(std::wstring); + Music(std::string); + ~Music(); + + static HRESULT Initialize(); + static void CleanUp(); + + static void SetVolume(float); + static float GetVolume(); +}; + +#endif + diff --git a/WickedEngine/TaskThread.h b/WickedEngine/TaskThread.h new file mode 100644 index 000000000..34ba6267f --- /dev/null +++ b/WickedEngine/TaskThread.h @@ -0,0 +1,87 @@ +#include "WickedEngine.h" + +class TaskThread { +public: + TaskThread(std::function task) + : m_task(std::move(task)), + m_wakeup(false), + m_stop(false), + m_thread(&TaskThread::taskFunc, this) + {} + ~TaskThread() { stop(); join(); } + + // wake up the thread and execute the task + void wakeup() { + auto lock = std::unique_lock(m_wakemutex); + //std::cout << "main: sending wakeup signal..." << std::endl; + m_wakeup = true; + m_wakecond.notify_one(); + } + // wait for the task to complete + void wait() { + auto lock = std::unique_lock(m_waitmutex); + //std::cout << "main: waiting for task completion..." << std::endl; + while (m_wakeup) + m_waitcond.wait(lock); + //std::cout << "main: task completed!" << std::endl; + } + + // ask the thread to stop + void stop() { + auto lock = std::unique_lock(m_wakemutex); + //std::cout << "main: sending stop signal..." << std::endl; + m_stop = true; + m_wakecond.notify_one(); + } + // wait for the thread to actually be stopped + void join() { + //std::cout << "main: waiting for join..." << std::endl; + m_thread.join(); + //std::cout << "main: joined!" << std::endl; + } + +private: + std::function m_task; + + // wake up the thread + bool m_wakeup; + bool m_stop; + std::mutex m_wakemutex; + std::condition_variable m_wakecond; + + // wait for the thread to finish its task + std::mutex m_waitmutex; + std::condition_variable m_waitcond; + + std::thread m_thread; + + void taskFunc() { + while (true) { + { + auto lock = std::unique_lock(m_wakemutex); + //std::cout << "thread: waiting for wakeup or stop signal..." << std::endl; + while (!m_wakeup && !m_stop) + m_wakecond.wait(lock); + if (m_stop) { + //std::cout << "thread: got stop signal!" << std::endl; + return; + } + //std::cout << "thread: got wakeup signal!" << std::endl; + } + + //std::cout << "thread: running the task..." << std::endl; + // you should probably do something cleaner than catch (...) + // just ensure that no exception propagates from m_task() to taskFunc() + try { m_task(); } catch (...) {} + //std::cout << "thread: task completed!" << std::endl; + + { + auto lock = std::unique_lock(m_waitmutex); + //std::cout << "thread: sending task completed signal..." << std::endl; + m_wakeup = false; + m_waitcond.notify_all(); + } + } + } + +}; \ No newline at end of file diff --git a/WickedEngine/Timer.cpp b/WickedEngine/Timer.cpp new file mode 100644 index 000000000..ceda881ac --- /dev/null +++ b/WickedEngine/Timer.cpp @@ -0,0 +1,43 @@ +#include "Timer.h" + +double Timer::PCFreq=0; +__int64 Timer::CounterStart=0; + +Timer::Timer() +{ + if(CounterStart==0) + Start(); + record(); +} + + +Timer::~Timer() +{ +} + +void Timer::Start() +{ + LARGE_INTEGER li; + if(!QueryPerformanceFrequency(&li)) + MessageBoxA(0,"QueryPerformanceFrequency failed!\n","Warning!",0); + + PCFreq = double(li.QuadPart)/1000.0; + + QueryPerformanceCounter(&li); + CounterStart = li.QuadPart; +} +double Timer::TotalTime() +{ + LARGE_INTEGER li; + QueryPerformanceCounter(&li); + return double(li.QuadPart-CounterStart)/PCFreq; +} + +void Timer::record() +{ + lastTime = TotalTime(); +} +double Timer::elapsed() +{ + return TotalTime() - lastTime; +} \ No newline at end of file diff --git a/WickedEngine/Timer.h b/WickedEngine/Timer.h new file mode 100644 index 000000000..944aaa92d --- /dev/null +++ b/WickedEngine/Timer.h @@ -0,0 +1,21 @@ +#pragma once +#include "WickedEngine.h" + +class Timer +{ +private: + static double PCFreq; + static __int64 CounterStart; + + double lastTime; +public: + Timer(); + ~Timer(); + + static void Start(); + static double TotalTime(); + + void record(); //start recording + double elapsed(); //elapsed time since record() +}; + diff --git a/WickedEngine/Utility/DDSTextureLoader.cpp b/WickedEngine/Utility/DDSTextureLoader.cpp new file mode 100644 index 000000000..448d7a154 --- /dev/null +++ b/WickedEngine/Utility/DDSTextureLoader.cpp @@ -0,0 +1,1457 @@ +//-------------------------------------------------------------------------------------- +// File: DDSTextureLoader.cpp +// +// Functions for loading a DDS texture and creating a Direct3D 11 runtime resource for it +// +// Note these functions are useful as a light-weight runtime loader for DDS files. For +// a full-featured DDS file reader, writer, and texture processing pipeline see +// the 'Texconv' sample and the 'DirectXTex' library. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248926 +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#include "pch.h" + +#include "DDSTextureLoader.h" + +#include "dds.h" +#include "PlatformHelpers.h" + +using namespace DirectX; + +//-------------------------------------------------------------------------------------- +static HRESULT LoadTextureDataFromFile( _In_z_ const wchar_t* fileName, + std::unique_ptr& ddsData, + DDS_HEADER** header, + uint8_t** bitData, + size_t* bitSize + ) +{ + if (!header || !bitData || !bitSize) + { + return E_POINTER; + } + + // open the file +#if (_WIN32_WINNT >= _WIN32_WINNT_WIN8) + ScopedHandle hFile( safe_handle( CreateFile2( fileName, + GENERIC_READ, + FILE_SHARE_READ, + OPEN_EXISTING, + nullptr ) ) ); +#else + ScopedHandle hFile( safe_handle( CreateFileW( fileName, + GENERIC_READ, + FILE_SHARE_READ, + nullptr, + OPEN_EXISTING, + FILE_ATTRIBUTE_NORMAL, + nullptr ) ) ); +#endif + + if ( !hFile ) + { + return HRESULT_FROM_WIN32( GetLastError() ); + } + + // Get the file size + LARGE_INTEGER FileSize = { 0 }; + +#if (_WIN32_WINNT >= _WIN32_WINNT_VISTA) + FILE_STANDARD_INFO fileInfo; + if ( !GetFileInformationByHandleEx( hFile.get(), FileStandardInfo, &fileInfo, sizeof(fileInfo) ) ) + { + return HRESULT_FROM_WIN32( GetLastError() ); + } + FileSize = fileInfo.EndOfFile; +#else + GetFileSizeEx( hFile.get(), &FileSize ); +#endif + + // File is too big for 32-bit allocation, so reject read + if (FileSize.HighPart > 0) + { + return E_FAIL; + } + + // Need at least enough data to fill the header and magic number to be a valid DDS + if (FileSize.LowPart < ( sizeof(DDS_HEADER) + sizeof(uint32_t) ) ) + { + return E_FAIL; + } + + // create enough space for the file data + ddsData.reset( new (std::nothrow) uint8_t[ FileSize.LowPart ] ); + if (!ddsData) + { + return E_OUTOFMEMORY; + } + + // read the data in + DWORD BytesRead = 0; + if (!ReadFile( hFile.get(), + ddsData.get(), + FileSize.LowPart, + &BytesRead, + nullptr + )) + { + return HRESULT_FROM_WIN32( GetLastError() ); + } + + if (BytesRead < FileSize.LowPart) + { + return E_FAIL; + } + + // DDS files always start with the same magic number ("DDS ") + uint32_t dwMagicNumber = *( const uint32_t* )( ddsData.get() ); + if (dwMagicNumber != DDS_MAGIC) + { + return E_FAIL; + } + + auto hdr = reinterpret_cast( ddsData.get() + sizeof( uint32_t ) ); + + // Verify header to validate DDS file + if (hdr->size != sizeof(DDS_HEADER) || + hdr->ddspf.size != sizeof(DDS_PIXELFORMAT)) + { + return E_FAIL; + } + + // Check for DX10 extension + bool bDXT10Header = false; + if ((hdr->ddspf.flags & DDS_FOURCC) && + (MAKEFOURCC( 'D', 'X', '1', '0' ) == hdr->ddspf.fourCC)) + { + // Must be long enough for both headers and magic value + if (FileSize.LowPart < ( sizeof(DDS_HEADER) + sizeof(uint32_t) + sizeof(DDS_HEADER_DXT10) ) ) + { + return E_FAIL; + } + + bDXT10Header = true; + } + + // setup the pointers in the process request + *header = hdr; + ptrdiff_t offset = sizeof( uint32_t ) + sizeof( DDS_HEADER ) + + (bDXT10Header ? sizeof( DDS_HEADER_DXT10 ) : 0); + *bitData = ddsData.get() + offset; + *bitSize = FileSize.LowPart - offset; + + return S_OK; +} + + +//-------------------------------------------------------------------------------------- +// Return the BPP for a particular format +//-------------------------------------------------------------------------------------- +static size_t BitsPerPixel( _In_ DXGI_FORMAT fmt ) +{ + switch( fmt ) + { + case DXGI_FORMAT_R32G32B32A32_TYPELESS: + case DXGI_FORMAT_R32G32B32A32_FLOAT: + case DXGI_FORMAT_R32G32B32A32_UINT: + case DXGI_FORMAT_R32G32B32A32_SINT: + return 128; + + case DXGI_FORMAT_R32G32B32_TYPELESS: + case DXGI_FORMAT_R32G32B32_FLOAT: + case DXGI_FORMAT_R32G32B32_UINT: + case DXGI_FORMAT_R32G32B32_SINT: + return 96; + + case DXGI_FORMAT_R16G16B16A16_TYPELESS: + case DXGI_FORMAT_R16G16B16A16_FLOAT: + case DXGI_FORMAT_R16G16B16A16_UNORM: + case DXGI_FORMAT_R16G16B16A16_UINT: + case DXGI_FORMAT_R16G16B16A16_SNORM: + case DXGI_FORMAT_R16G16B16A16_SINT: + case DXGI_FORMAT_R32G32_TYPELESS: + case DXGI_FORMAT_R32G32_FLOAT: + case DXGI_FORMAT_R32G32_UINT: + case DXGI_FORMAT_R32G32_SINT: + case DXGI_FORMAT_R32G8X24_TYPELESS: + case DXGI_FORMAT_D32_FLOAT_S8X24_UINT: + case DXGI_FORMAT_R32_FLOAT_X8X24_TYPELESS: + case DXGI_FORMAT_X32_TYPELESS_G8X24_UINT: + return 64; + + case DXGI_FORMAT_R10G10B10A2_TYPELESS: + case DXGI_FORMAT_R10G10B10A2_UNORM: + case DXGI_FORMAT_R10G10B10A2_UINT: + case DXGI_FORMAT_R11G11B10_FLOAT: + case DXGI_FORMAT_R8G8B8A8_TYPELESS: + case DXGI_FORMAT_R8G8B8A8_UNORM: + case DXGI_FORMAT_R8G8B8A8_UNORM_SRGB: + case DXGI_FORMAT_R8G8B8A8_UINT: + case DXGI_FORMAT_R8G8B8A8_SNORM: + case DXGI_FORMAT_R8G8B8A8_SINT: + case DXGI_FORMAT_R16G16_TYPELESS: + case DXGI_FORMAT_R16G16_FLOAT: + case DXGI_FORMAT_R16G16_UNORM: + case DXGI_FORMAT_R16G16_UINT: + case DXGI_FORMAT_R16G16_SNORM: + case DXGI_FORMAT_R16G16_SINT: + case DXGI_FORMAT_R32_TYPELESS: + case DXGI_FORMAT_D32_FLOAT: + case DXGI_FORMAT_R32_FLOAT: + case DXGI_FORMAT_R32_UINT: + case DXGI_FORMAT_R32_SINT: + case DXGI_FORMAT_R24G8_TYPELESS: + case DXGI_FORMAT_D24_UNORM_S8_UINT: + case DXGI_FORMAT_R24_UNORM_X8_TYPELESS: + case DXGI_FORMAT_X24_TYPELESS_G8_UINT: + case DXGI_FORMAT_R9G9B9E5_SHAREDEXP: + case DXGI_FORMAT_R8G8_B8G8_UNORM: + case DXGI_FORMAT_G8R8_G8B8_UNORM: + case DXGI_FORMAT_B8G8R8A8_UNORM: + case DXGI_FORMAT_B8G8R8X8_UNORM: + case DXGI_FORMAT_R10G10B10_XR_BIAS_A2_UNORM: + case DXGI_FORMAT_B8G8R8A8_TYPELESS: + case DXGI_FORMAT_B8G8R8A8_UNORM_SRGB: + case DXGI_FORMAT_B8G8R8X8_TYPELESS: + case DXGI_FORMAT_B8G8R8X8_UNORM_SRGB: + return 32; + + case DXGI_FORMAT_R8G8_TYPELESS: + case DXGI_FORMAT_R8G8_UNORM: + case DXGI_FORMAT_R8G8_UINT: + case DXGI_FORMAT_R8G8_SNORM: + case DXGI_FORMAT_R8G8_SINT: + case DXGI_FORMAT_R16_TYPELESS: + case DXGI_FORMAT_R16_FLOAT: + case DXGI_FORMAT_D16_UNORM: + case DXGI_FORMAT_R16_UNORM: + case DXGI_FORMAT_R16_UINT: + case DXGI_FORMAT_R16_SNORM: + case DXGI_FORMAT_R16_SINT: + case DXGI_FORMAT_B5G6R5_UNORM: + case DXGI_FORMAT_B5G5R5A1_UNORM: + +#ifdef DXGI_1_2_FORMATS + case DXGI_FORMAT_B4G4R4A4_UNORM: +#endif + return 16; + + case DXGI_FORMAT_R8_TYPELESS: + case DXGI_FORMAT_R8_UNORM: + case DXGI_FORMAT_R8_UINT: + case DXGI_FORMAT_R8_SNORM: + case DXGI_FORMAT_R8_SINT: + case DXGI_FORMAT_A8_UNORM: + return 8; + + case DXGI_FORMAT_R1_UNORM: + return 1; + + case DXGI_FORMAT_BC1_TYPELESS: + case DXGI_FORMAT_BC1_UNORM: + case DXGI_FORMAT_BC1_UNORM_SRGB: + case DXGI_FORMAT_BC4_TYPELESS: + case DXGI_FORMAT_BC4_UNORM: + case DXGI_FORMAT_BC4_SNORM: + return 4; + + case DXGI_FORMAT_BC2_TYPELESS: + case DXGI_FORMAT_BC2_UNORM: + case DXGI_FORMAT_BC2_UNORM_SRGB: + case DXGI_FORMAT_BC3_TYPELESS: + case DXGI_FORMAT_BC3_UNORM: + case DXGI_FORMAT_BC3_UNORM_SRGB: + case DXGI_FORMAT_BC5_TYPELESS: + case DXGI_FORMAT_BC5_UNORM: + case DXGI_FORMAT_BC5_SNORM: + case DXGI_FORMAT_BC6H_TYPELESS: + case DXGI_FORMAT_BC6H_UF16: + case DXGI_FORMAT_BC6H_SF16: + case DXGI_FORMAT_BC7_TYPELESS: + case DXGI_FORMAT_BC7_UNORM: + case DXGI_FORMAT_BC7_UNORM_SRGB: + return 8; + + default: + return 0; + } +} + + +//-------------------------------------------------------------------------------------- +// Get surface information for a particular format +//-------------------------------------------------------------------------------------- +static void GetSurfaceInfo( _In_ size_t width, + _In_ size_t height, + _In_ DXGI_FORMAT fmt, + _Out_opt_ size_t* outNumBytes, + _Out_opt_ size_t* outRowBytes, + _Out_opt_ size_t* outNumRows ) +{ + size_t numBytes = 0; + size_t rowBytes = 0; + size_t numRows = 0; + + bool bc = false; + bool packed = false; + size_t bcnumBytesPerBlock = 0; + switch (fmt) + { + case DXGI_FORMAT_BC1_TYPELESS: + case DXGI_FORMAT_BC1_UNORM: + case DXGI_FORMAT_BC1_UNORM_SRGB: + case DXGI_FORMAT_BC4_TYPELESS: + case DXGI_FORMAT_BC4_UNORM: + case DXGI_FORMAT_BC4_SNORM: + bc=true; + bcnumBytesPerBlock = 8; + break; + + case DXGI_FORMAT_BC2_TYPELESS: + case DXGI_FORMAT_BC2_UNORM: + case DXGI_FORMAT_BC2_UNORM_SRGB: + case DXGI_FORMAT_BC3_TYPELESS: + case DXGI_FORMAT_BC3_UNORM: + case DXGI_FORMAT_BC3_UNORM_SRGB: + case DXGI_FORMAT_BC5_TYPELESS: + case DXGI_FORMAT_BC5_UNORM: + case DXGI_FORMAT_BC5_SNORM: + case DXGI_FORMAT_BC6H_TYPELESS: + case DXGI_FORMAT_BC6H_UF16: + case DXGI_FORMAT_BC6H_SF16: + case DXGI_FORMAT_BC7_TYPELESS: + case DXGI_FORMAT_BC7_UNORM: + case DXGI_FORMAT_BC7_UNORM_SRGB: + bc = true; + bcnumBytesPerBlock = 16; + break; + + case DXGI_FORMAT_R8G8_B8G8_UNORM: + case DXGI_FORMAT_G8R8_G8B8_UNORM: + packed = true; + break; + } + + if (bc) + { + size_t numBlocksWide = 0; + if (width > 0) + { + numBlocksWide = std::max( 1, (width + 3) / 4 ); + } + size_t numBlocksHigh = 0; + if (height > 0) + { + numBlocksHigh = std::max( 1, (height + 3) / 4 ); + } + rowBytes = numBlocksWide * bcnumBytesPerBlock; + numRows = numBlocksHigh; + } + else if (packed) + { + rowBytes = ( ( width + 1 ) >> 1 ) * 4; + numRows = height; + } + else + { + size_t bpp = BitsPerPixel( fmt ); + rowBytes = ( width * bpp + 7 ) / 8; // round up to nearest byte + numRows = height; + } + + numBytes = rowBytes * numRows; + if (outNumBytes) + { + *outNumBytes = numBytes; + } + if (outRowBytes) + { + *outRowBytes = rowBytes; + } + if (outNumRows) + { + *outNumRows = numRows; + } +} + + +//-------------------------------------------------------------------------------------- +#define ISBITMASK( r,g,b,a ) ( ddpf.RBitMask == r && ddpf.GBitMask == g && ddpf.BBitMask == b && ddpf.ABitMask == a ) + +static DXGI_FORMAT GetDXGIFormat( const DDS_PIXELFORMAT& ddpf ) +{ + if (ddpf.flags & DDS_RGB) + { + // Note that sRGB formats are written using the "DX10" extended header + + switch (ddpf.RGBBitCount) + { + case 32: + if (ISBITMASK(0x000000ff,0x0000ff00,0x00ff0000,0xff000000)) + { + return DXGI_FORMAT_R8G8B8A8_UNORM; + } + + if (ISBITMASK(0x00ff0000,0x0000ff00,0x000000ff,0xff000000)) + { + return DXGI_FORMAT_B8G8R8A8_UNORM; + } + + if (ISBITMASK(0x00ff0000,0x0000ff00,0x000000ff,0x00000000)) + { + return DXGI_FORMAT_B8G8R8X8_UNORM; + } + + // No DXGI format maps to ISBITMASK(0x000000ff,0x0000ff00,0x00ff0000,0x00000000) aka D3DFMT_X8B8G8R8 + + // Note that many common DDS reader/writers (including D3DX) swap the + // the RED/BLUE masks for 10:10:10:2 formats. We assumme + // below that the 'backwards' header mask is being used since it is most + // likely written by D3DX. The more robust solution is to use the 'DX10' + // header extension and specify the DXGI_FORMAT_R10G10B10A2_UNORM format directly + + // For 'correct' writers, this should be 0x000003ff,0x000ffc00,0x3ff00000 for RGB data + if (ISBITMASK(0x3ff00000,0x000ffc00,0x000003ff,0xc0000000)) + { + return DXGI_FORMAT_R10G10B10A2_UNORM; + } + + // No DXGI format maps to ISBITMASK(0x000003ff,0x000ffc00,0x3ff00000,0xc0000000) aka D3DFMT_A2R10G10B10 + + if (ISBITMASK(0x0000ffff,0xffff0000,0x00000000,0x00000000)) + { + return DXGI_FORMAT_R16G16_UNORM; + } + + if (ISBITMASK(0xffffffff,0x00000000,0x00000000,0x00000000)) + { + // Only 32-bit color channel format in D3D9 was R32F + return DXGI_FORMAT_R32_FLOAT; // D3DX writes this out as a FourCC of 114 + } + break; + + case 24: + // No 24bpp DXGI formats aka D3DFMT_R8G8B8 + break; + + case 16: + if (ISBITMASK(0x7c00,0x03e0,0x001f,0x8000)) + { + return DXGI_FORMAT_B5G5R5A1_UNORM; + } + if (ISBITMASK(0xf800,0x07e0,0x001f,0x0000)) + { + return DXGI_FORMAT_B5G6R5_UNORM; + } + + // No DXGI format maps to ISBITMASK(0x7c00,0x03e0,0x001f,0x0000) aka D3DFMT_X1R5G5B5 + +#ifdef DXGI_1_2_FORMATS + if (ISBITMASK(0x0f00,0x00f0,0x000f,0xf000)) + { + return DXGI_FORMAT_B4G4R4A4_UNORM; + } + + // No DXGI format maps to ISBITMASK(0x0f00,0x00f0,0x000f,0x0000) aka D3DFMT_X4R4G4B4 +#endif + + // No 3:3:2, 3:3:2:8, or paletted DXGI formats aka D3DFMT_A8R3G3B2, D3DFMT_R3G3B2, D3DFMT_P8, D3DFMT_A8P8, etc. + break; + } + } + else if (ddpf.flags & DDS_LUMINANCE) + { + if (8 == ddpf.RGBBitCount) + { + if (ISBITMASK(0x000000ff,0x00000000,0x00000000,0x00000000)) + { + return DXGI_FORMAT_R8_UNORM; // D3DX10/11 writes this out as DX10 extension + } + + // No DXGI format maps to ISBITMASK(0x0f,0x00,0x00,0xf0) aka D3DFMT_A4L4 + } + + if (16 == ddpf.RGBBitCount) + { + if (ISBITMASK(0x0000ffff,0x00000000,0x00000000,0x00000000)) + { + return DXGI_FORMAT_R16_UNORM; // D3DX10/11 writes this out as DX10 extension + } + if (ISBITMASK(0x000000ff,0x00000000,0x00000000,0x0000ff00)) + { + return DXGI_FORMAT_R8G8_UNORM; // D3DX10/11 writes this out as DX10 extension + } + } + } + else if (ddpf.flags & DDS_ALPHA) + { + if (8 == ddpf.RGBBitCount) + { + return DXGI_FORMAT_A8_UNORM; + } + } + else if (ddpf.flags & DDS_FOURCC) + { + if (MAKEFOURCC( 'D', 'X', 'T', '1' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC1_UNORM; + } + if (MAKEFOURCC( 'D', 'X', 'T', '3' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC2_UNORM; + } + if (MAKEFOURCC( 'D', 'X', 'T', '5' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC3_UNORM; + } + + // While pre-mulitplied alpha isn't directly supported by the DXGI formats, + // they are basically the same as these BC formats so they can be mapped + if (MAKEFOURCC( 'D', 'X', 'T', '2' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC2_UNORM; + } + if (MAKEFOURCC( 'D', 'X', 'T', '4' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC3_UNORM; + } + + if (MAKEFOURCC( 'A', 'T', 'I', '1' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC4_UNORM; + } + if (MAKEFOURCC( 'B', 'C', '4', 'U' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC4_UNORM; + } + if (MAKEFOURCC( 'B', 'C', '4', 'S' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC4_SNORM; + } + + if (MAKEFOURCC( 'A', 'T', 'I', '2' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC5_UNORM; + } + if (MAKEFOURCC( 'B', 'C', '5', 'U' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC5_UNORM; + } + if (MAKEFOURCC( 'B', 'C', '5', 'S' ) == ddpf.fourCC) + { + return DXGI_FORMAT_BC5_SNORM; + } + + // BC6H and BC7 are written using the "DX10" extended header + + if (MAKEFOURCC( 'R', 'G', 'B', 'G' ) == ddpf.fourCC) + { + return DXGI_FORMAT_R8G8_B8G8_UNORM; + } + if (MAKEFOURCC( 'G', 'R', 'G', 'B' ) == ddpf.fourCC) + { + return DXGI_FORMAT_G8R8_G8B8_UNORM; + } + + // Check for D3DFORMAT enums being set here + switch( ddpf.fourCC ) + { + case 36: // D3DFMT_A16B16G16R16 + return DXGI_FORMAT_R16G16B16A16_UNORM; + + case 110: // D3DFMT_Q16W16V16U16 + return DXGI_FORMAT_R16G16B16A16_SNORM; + + case 111: // D3DFMT_R16F + return DXGI_FORMAT_R16_FLOAT; + + case 112: // D3DFMT_G16R16F + return DXGI_FORMAT_R16G16_FLOAT; + + case 113: // D3DFMT_A16B16G16R16F + return DXGI_FORMAT_R16G16B16A16_FLOAT; + + case 114: // D3DFMT_R32F + return DXGI_FORMAT_R32_FLOAT; + + case 115: // D3DFMT_G32R32F + return DXGI_FORMAT_R32G32_FLOAT; + + case 116: // D3DFMT_A32B32G32R32F + return DXGI_FORMAT_R32G32B32A32_FLOAT; + } + } + + return DXGI_FORMAT_UNKNOWN; +} + + +//-------------------------------------------------------------------------------------- +static DXGI_FORMAT MakeSRGB( _In_ DXGI_FORMAT format ) +{ + switch( format ) + { + case DXGI_FORMAT_R8G8B8A8_UNORM: + return DXGI_FORMAT_R8G8B8A8_UNORM_SRGB; + + case DXGI_FORMAT_BC1_UNORM: + return DXGI_FORMAT_BC1_UNORM_SRGB; + + case DXGI_FORMAT_BC2_UNORM: + return DXGI_FORMAT_BC2_UNORM_SRGB; + + case DXGI_FORMAT_BC3_UNORM: + return DXGI_FORMAT_BC3_UNORM_SRGB; + + case DXGI_FORMAT_B8G8R8A8_UNORM: + return DXGI_FORMAT_B8G8R8A8_UNORM_SRGB; + + case DXGI_FORMAT_B8G8R8X8_UNORM: + return DXGI_FORMAT_B8G8R8X8_UNORM_SRGB; + + case DXGI_FORMAT_BC7_UNORM: + return DXGI_FORMAT_BC7_UNORM_SRGB; + + default: + return format; + } +} + + +//-------------------------------------------------------------------------------------- +static HRESULT FillInitData( _In_ size_t width, + _In_ size_t height, + _In_ size_t depth, + _In_ size_t mipCount, + _In_ size_t arraySize, + _In_ DXGI_FORMAT format, + _In_ size_t maxsize, + _In_ size_t bitSize, + _In_reads_bytes_(bitSize) const uint8_t* bitData, + _Out_ size_t& twidth, + _Out_ size_t& theight, + _Out_ size_t& tdepth, + _Out_ size_t& skipMip, + _Out_writes_(mipCount*arraySize) D3D11_SUBRESOURCE_DATA* initData ) +{ + if ( !bitData || !initData ) + { + return E_POINTER; + } + + skipMip = 0; + twidth = 0; + theight = 0; + tdepth = 0; + + size_t NumBytes = 0; + size_t RowBytes = 0; + size_t NumRows = 0; + const uint8_t* pSrcBits = bitData; + const uint8_t* pEndBits = bitData + bitSize; + + size_t index = 0; + for( size_t j = 0; j < arraySize; j++ ) + { + size_t w = width; + size_t h = height; + size_t d = depth; + for( size_t i = 0; i < mipCount; i++ ) + { + GetSurfaceInfo( w, + h, + format, + &NumBytes, + &RowBytes, + &NumRows + ); + + if ( (mipCount <= 1) || !maxsize || (w <= maxsize && h <= maxsize && d <= maxsize) ) + { + if ( !twidth ) + { + twidth = w; + theight = h; + tdepth = d; + } + + assert(index < mipCount * arraySize); + _Analysis_assume_(index < mipCount * arraySize); + initData[index].pSysMem = ( const void* )pSrcBits; + initData[index].SysMemPitch = static_cast( RowBytes ); + initData[index].SysMemSlicePitch = static_cast( NumBytes ); + ++index; + } + else if ( !j ) + { + // Count number of skipped mipmaps (first item only) + ++skipMip; + } + + if (pSrcBits + (NumBytes*d) > pEndBits) + { + return HRESULT_FROM_WIN32( ERROR_HANDLE_EOF ); + } + + pSrcBits += NumBytes * d; + + w = w >> 1; + h = h >> 1; + d = d >> 1; + if (w == 0) + { + w = 1; + } + if (h == 0) + { + h = 1; + } + if (d == 0) + { + d = 1; + } + } + } + + return (index > 0) ? S_OK : E_FAIL; +} + + +//-------------------------------------------------------------------------------------- +static HRESULT CreateD3DResources( _In_ ID3D11Device* d3dDevice, + _In_ uint32_t resDim, + _In_ size_t width, + _In_ size_t height, + _In_ size_t depth, + _In_ size_t mipCount, + _In_ size_t arraySize, + _In_ DXGI_FORMAT format, + _In_ D3D11_USAGE usage, + _In_ unsigned int bindFlags, + _In_ unsigned int cpuAccessFlags, + _In_ unsigned int miscFlags, + _In_ bool forceSRGB, + _In_ bool isCubeMap, + _In_reads_(mipCount*arraySize) D3D11_SUBRESOURCE_DATA* initData, + _Outptr_opt_ ID3D11Resource** texture, + _Outptr_opt_ ID3D11ShaderResourceView** textureView ) +{ + if ( !d3dDevice || !initData ) + return E_POINTER; + + HRESULT hr = E_FAIL; + + if ( forceSRGB ) + { + format = MakeSRGB( format ); + } + + switch ( resDim ) + { + case D3D11_RESOURCE_DIMENSION_TEXTURE1D: + { + D3D11_TEXTURE1D_DESC desc; + desc.Width = static_cast( width ); + desc.MipLevels = static_cast( mipCount ); + desc.ArraySize = static_cast( arraySize ); + desc.Format = format; + desc.Usage = usage; + desc.BindFlags = bindFlags; + desc.CPUAccessFlags = cpuAccessFlags; + desc.MiscFlags = miscFlags & ~D3D11_RESOURCE_MISC_TEXTURECUBE; + + ID3D11Texture1D* tex = nullptr; + hr = d3dDevice->CreateTexture1D( &desc, + initData, + &tex + ); + if (SUCCEEDED( hr ) && tex != 0) + { + if (textureView != 0) + { + D3D11_SHADER_RESOURCE_VIEW_DESC SRVDesc; + memset( &SRVDesc, 0, sizeof( SRVDesc ) ); + SRVDesc.Format = format; + + if (arraySize > 1) + { + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURE1DARRAY; + SRVDesc.Texture1DArray.MipLevels = desc.MipLevels; + SRVDesc.Texture1DArray.ArraySize = static_cast( arraySize ); + } + else + { + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURE1D; + SRVDesc.Texture1D.MipLevels = desc.MipLevels; + } + + hr = d3dDevice->CreateShaderResourceView( tex, + &SRVDesc, + textureView + ); + if ( FAILED(hr) ) + { + tex->Release(); + return hr; + } + } + + if (texture != 0) + { + *texture = tex; + } + else + { + SetDebugObjectName(tex, "DDSTextureLoader"); + tex->Release(); + } + } + } + break; + + case D3D11_RESOURCE_DIMENSION_TEXTURE2D: + { + D3D11_TEXTURE2D_DESC desc; + desc.Width = static_cast( width ); + desc.Height = static_cast( height ); + desc.MipLevels = static_cast( mipCount ); + desc.ArraySize = static_cast( arraySize ); + desc.Format = format; + desc.SampleDesc.Count = 1; + desc.SampleDesc.Quality = 0; + desc.Usage = usage; + desc.BindFlags = bindFlags; + desc.CPUAccessFlags = cpuAccessFlags; + if ( isCubeMap ) + { + desc.MiscFlags = miscFlags | D3D11_RESOURCE_MISC_TEXTURECUBE; + } + else + { + desc.MiscFlags = miscFlags & ~D3D11_RESOURCE_MISC_TEXTURECUBE; + } + + ID3D11Texture2D* tex = nullptr; + hr = d3dDevice->CreateTexture2D( &desc, + initData, + &tex + ); + if (SUCCEEDED( hr ) && tex != 0) + { + if (textureView != 0) + { + D3D11_SHADER_RESOURCE_VIEW_DESC SRVDesc; + memset( &SRVDesc, 0, sizeof( SRVDesc ) ); + SRVDesc.Format = format; + + if ( isCubeMap ) + { + if (arraySize > 6) + { + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURECUBEARRAY; + SRVDesc.TextureCubeArray.MipLevels = desc.MipLevels; + + // Earlier we set arraySize to (NumCubes * 6) + SRVDesc.TextureCubeArray.NumCubes = static_cast( arraySize / 6 ); + } + else + { + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURECUBE; + SRVDesc.TextureCube.MipLevels = desc.MipLevels; + } + } + else if (arraySize > 1) + { + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURE2DARRAY; + SRVDesc.Texture2DArray.MipLevels = desc.MipLevels; + SRVDesc.Texture2DArray.ArraySize = static_cast( arraySize ); + } + else + { + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURE2D; + SRVDesc.Texture2D.MipLevels = desc.MipLevels; + } + + hr = d3dDevice->CreateShaderResourceView( tex, + &SRVDesc, + textureView + ); + if ( FAILED(hr) ) + { + tex->Release(); + return hr; + } + } + + if (texture != 0) + { + *texture = tex; + } + else + { + SetDebugObjectName(tex, "DDSTextureLoader"); + tex->Release(); + } + } + } + break; + + case D3D11_RESOURCE_DIMENSION_TEXTURE3D: + { + D3D11_TEXTURE3D_DESC desc; + desc.Width = static_cast( width ); + desc.Height = static_cast( height ); + desc.Depth = static_cast( depth ); + desc.MipLevels = static_cast( mipCount ); + desc.Format = format; + desc.Usage = usage; + desc.BindFlags = bindFlags; + desc.CPUAccessFlags = cpuAccessFlags; + desc.MiscFlags = miscFlags & ~D3D11_RESOURCE_MISC_TEXTURECUBE; + + ID3D11Texture3D* tex = nullptr; + hr = d3dDevice->CreateTexture3D( &desc, + initData, + &tex + ); + if (SUCCEEDED( hr ) && tex != 0) + { + if (textureView != 0) + { + D3D11_SHADER_RESOURCE_VIEW_DESC SRVDesc; + memset( &SRVDesc, 0, sizeof( SRVDesc ) ); + SRVDesc.Format = format; + + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURE3D; + SRVDesc.Texture3D.MipLevels = desc.MipLevels; + + hr = d3dDevice->CreateShaderResourceView( tex, + &SRVDesc, + textureView + ); + if ( FAILED(hr) ) + { + tex->Release(); + return hr; + } + } + + if (texture != 0) + { + *texture = tex; + } + else + { + SetDebugObjectName(tex, "DDSTextureLoader"); + tex->Release(); + } + } + } + break; + } + + return hr; +} + + +//-------------------------------------------------------------------------------------- +static HRESULT CreateTextureFromDDS( _In_ ID3D11Device* d3dDevice, + _In_ const DDS_HEADER* header, + _In_reads_bytes_(bitSize) const uint8_t* bitData, + _In_ size_t bitSize, + _In_ size_t maxsize, + _In_ D3D11_USAGE usage, + _In_ unsigned int bindFlags, + _In_ unsigned int cpuAccessFlags, + _In_ unsigned int miscFlags, + _In_ bool forceSRGB, + _Outptr_opt_ ID3D11Resource** texture, + _Outptr_opt_ ID3D11ShaderResourceView** textureView ) +{ + HRESULT hr = S_OK; + + size_t width = header->width; + size_t height = header->height; + size_t depth = header->depth; + + uint32_t resDim = D3D11_RESOURCE_DIMENSION_UNKNOWN; + size_t arraySize = 1; + DXGI_FORMAT format = DXGI_FORMAT_UNKNOWN; + bool isCubeMap = false; + + size_t mipCount = header->mipMapCount; + if (0 == mipCount) + { + mipCount = 1; + } + + if ((header->ddspf.flags & DDS_FOURCC) && + (MAKEFOURCC( 'D', 'X', '1', '0' ) == header->ddspf.fourCC )) + { + auto d3d10ext = reinterpret_cast( (const char*)header + sizeof(DDS_HEADER) ); + + arraySize = d3d10ext->arraySize; + if (arraySize == 0) + { + return HRESULT_FROM_WIN32( ERROR_INVALID_DATA ); + } + + if (BitsPerPixel( d3d10ext->dxgiFormat ) == 0) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + + format = d3d10ext->dxgiFormat; + + switch ( d3d10ext->resourceDimension ) + { + case D3D11_RESOURCE_DIMENSION_TEXTURE1D: + // D3DX writes 1D textures with a fixed Height of 1 + if ((header->flags & DDS_HEIGHT) && height != 1) + { + return HRESULT_FROM_WIN32( ERROR_INVALID_DATA ); + } + height = depth = 1; + break; + + case D3D11_RESOURCE_DIMENSION_TEXTURE2D: + if (d3d10ext->miscFlag & D3D11_RESOURCE_MISC_TEXTURECUBE) + { + arraySize *= 6; + isCubeMap = true; + } + depth = 1; + break; + + case D3D11_RESOURCE_DIMENSION_TEXTURE3D: + if (!(header->flags & DDS_HEADER_FLAGS_VOLUME)) + { + return HRESULT_FROM_WIN32( ERROR_INVALID_DATA ); + } + + if (arraySize > 1) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + break; + + default: + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + + resDim = d3d10ext->resourceDimension; + } + else + { + format = GetDXGIFormat( header->ddspf ); + + if (format == DXGI_FORMAT_UNKNOWN) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + + if (header->flags & DDS_HEADER_FLAGS_VOLUME) + { + resDim = D3D11_RESOURCE_DIMENSION_TEXTURE3D; + } + else + { + if (header->caps2 & DDS_CUBEMAP) + { + // We require all six faces to be defined + if ((header->caps2 & DDS_CUBEMAP_ALLFACES ) != DDS_CUBEMAP_ALLFACES) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + + arraySize = 6; + isCubeMap = true; + } + + depth = 1; + resDim = D3D11_RESOURCE_DIMENSION_TEXTURE2D; + + // Note there's no way for a legacy Direct3D 9 DDS to express a '1D' texture + } + + assert( BitsPerPixel( format ) != 0 ); + } + + // Bound sizes (for security purposes we don't trust DDS file metadata larger than the D3D 11.x hardware requirements) + if (mipCount > D3D11_REQ_MIP_LEVELS) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + + switch ( resDim ) + { + case D3D11_RESOURCE_DIMENSION_TEXTURE1D: + if ((arraySize > D3D11_REQ_TEXTURE1D_ARRAY_AXIS_DIMENSION) || + (width > D3D11_REQ_TEXTURE1D_U_DIMENSION) ) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + break; + + case D3D11_RESOURCE_DIMENSION_TEXTURE2D: + if ( isCubeMap ) + { + // This is the right bound because we set arraySize to (NumCubes*6) above + if ((arraySize > D3D11_REQ_TEXTURE2D_ARRAY_AXIS_DIMENSION) || + (width > D3D11_REQ_TEXTURECUBE_DIMENSION) || + (height > D3D11_REQ_TEXTURECUBE_DIMENSION)) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + } + else if ((arraySize > D3D11_REQ_TEXTURE2D_ARRAY_AXIS_DIMENSION) || + (width > D3D11_REQ_TEXTURE2D_U_OR_V_DIMENSION) || + (height > D3D11_REQ_TEXTURE2D_U_OR_V_DIMENSION)) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + break; + + case D3D11_RESOURCE_DIMENSION_TEXTURE3D: + if ((arraySize > 1) || + (width > D3D11_REQ_TEXTURE3D_U_V_OR_W_DIMENSION) || + (height > D3D11_REQ_TEXTURE3D_U_V_OR_W_DIMENSION) || + (depth > D3D11_REQ_TEXTURE3D_U_V_OR_W_DIMENSION) ) + { + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + break; + } + + // Create the texture + std::unique_ptr initData( new (std::nothrow) D3D11_SUBRESOURCE_DATA[ mipCount * arraySize ] ); + if ( !initData ) + { + return E_OUTOFMEMORY; + } + + size_t skipMip = 0; + size_t twidth = 0; + size_t theight = 0; + size_t tdepth = 0; + hr = FillInitData( width, height, depth, mipCount, arraySize, format, maxsize, bitSize, bitData, + twidth, theight, tdepth, skipMip, initData.get() ); + + if ( SUCCEEDED(hr) ) + { + hr = CreateD3DResources( d3dDevice, resDim, twidth, theight, tdepth, mipCount - skipMip, arraySize, + format, usage, bindFlags, cpuAccessFlags, miscFlags, forceSRGB, + isCubeMap, initData.get(), texture, textureView ); + + if ( FAILED(hr) && !maxsize && (mipCount > 1) ) + { + // Retry with a maxsize determined by feature level + switch( d3dDevice->GetFeatureLevel() ) + { + case D3D_FEATURE_LEVEL_9_1: + case D3D_FEATURE_LEVEL_9_2: + if ( isCubeMap ) + { + maxsize = 512 /*D3D_FL9_1_REQ_TEXTURECUBE_DIMENSION*/; + } + else + { + maxsize = (resDim == D3D11_RESOURCE_DIMENSION_TEXTURE3D) + ? 256 /*D3D_FL9_1_REQ_TEXTURE3D_U_V_OR_W_DIMENSION*/ + : 2048 /*D3D_FL9_1_REQ_TEXTURE2D_U_OR_V_DIMENSION*/; + } + break; + + case D3D_FEATURE_LEVEL_9_3: + maxsize = (resDim == D3D11_RESOURCE_DIMENSION_TEXTURE3D) + ? 256 /*D3D_FL9_1_REQ_TEXTURE3D_U_V_OR_W_DIMENSION*/ + : 4096 /*D3D_FL9_3_REQ_TEXTURE2D_U_OR_V_DIMENSION*/; + break; + + default: // D3D_FEATURE_LEVEL_10_0 & D3D_FEATURE_LEVEL_10_1 + maxsize = (resDim == D3D11_RESOURCE_DIMENSION_TEXTURE3D) + ? 2048 /*D3D10_REQ_TEXTURE3D_U_V_OR_W_DIMENSION*/ + : 8192 /*D3D10_REQ_TEXTURE2D_U_OR_V_DIMENSION*/; + break; + } + + hr = FillInitData( width, height, depth, mipCount, arraySize, format, maxsize, bitSize, bitData, + twidth, theight, tdepth, skipMip, initData.get() ); + if ( SUCCEEDED(hr) ) + { + hr = CreateD3DResources( d3dDevice, resDim, twidth, theight, tdepth, mipCount - skipMip, arraySize, + format, usage, bindFlags, cpuAccessFlags, miscFlags, forceSRGB, + isCubeMap, initData.get(), texture, textureView ); + } + } + } + + return hr; +} + + +//-------------------------------------------------------------------------------------- +static DDS_ALPHA_MODE GetAlphaMode( _In_ const DDS_HEADER* header ) +{ + if ( header->ddspf.flags & DDS_FOURCC ) + { + if ( MAKEFOURCC( 'D', 'X', '1', '0' ) == header->ddspf.fourCC ) + { + auto d3d10ext = reinterpret_cast( (const char*)header + sizeof(DDS_HEADER) ); + auto mode = static_cast( d3d10ext->miscFlags2 & DDS_MISC_FLAGS2_ALPHA_MODE_MASK ); + switch( mode ) + { + case DDS_ALPHA_MODE_STRAIGHT: + case DDS_ALPHA_MODE_PREMULTIPLIED: + case DDS_ALPHA_MODE_OPAQUE: + case DDS_ALPHA_MODE_CUSTOM: + return mode; + } + } + else if ( ( MAKEFOURCC( 'D', 'X', 'T', '2' ) == header->ddspf.fourCC ) + || ( MAKEFOURCC( 'D', 'X', 'T', '4' ) == header->ddspf.fourCC ) ) + { + return DDS_ALPHA_MODE_PREMULTIPLIED; + } + } + + return DDS_ALPHA_MODE_UNKNOWN; +} + + +//-------------------------------------------------------------------------------------- +_Use_decl_annotations_ +HRESULT DirectX::CreateDDSTextureFromMemory( ID3D11Device* d3dDevice, + const uint8_t* ddsData, + size_t ddsDataSize, + ID3D11Resource** texture, + ID3D11ShaderResourceView** textureView, + size_t maxsize, + DDS_ALPHA_MODE* alphaMode ) +{ + return CreateDDSTextureFromMemoryEx( d3dDevice, ddsData, ddsDataSize, maxsize, + D3D11_USAGE_DEFAULT, D3D11_BIND_SHADER_RESOURCE, 0, 0, false, + texture, textureView, alphaMode ); +} + +_Use_decl_annotations_ +HRESULT DirectX::CreateDDSTextureFromMemoryEx( ID3D11Device* d3dDevice, + const uint8_t* ddsData, + size_t ddsDataSize, + size_t maxsize, + D3D11_USAGE usage, + unsigned int bindFlags, + unsigned int cpuAccessFlags, + unsigned int miscFlags, + bool forceSRGB, + ID3D11Resource** texture, + ID3D11ShaderResourceView** textureView, + DDS_ALPHA_MODE* alphaMode ) +{ + if ( texture ) + { + *texture = nullptr; + } + if ( textureView ) + { + *textureView = nullptr; + } + if ( alphaMode ) + { + *alphaMode = DDS_ALPHA_MODE_UNKNOWN; + } + + if (!d3dDevice || !ddsData || (!texture && !textureView)) + { + return E_INVALIDARG; + } + + // Validate DDS file in memory + if (ddsDataSize < (sizeof(uint32_t) + sizeof(DDS_HEADER))) + { + return E_FAIL; + } + + uint32_t dwMagicNumber = *( const uint32_t* )( ddsData ); + if (dwMagicNumber != DDS_MAGIC) + { + return E_FAIL; + } + + auto header = reinterpret_cast( ddsData + sizeof( uint32_t ) ); + + // Verify header to validate DDS file + if (header->size != sizeof(DDS_HEADER) || + header->ddspf.size != sizeof(DDS_PIXELFORMAT)) + { + return E_FAIL; + } + + // Check for DX10 extension + bool bDXT10Header = false; + if ((header->ddspf.flags & DDS_FOURCC) && + (MAKEFOURCC( 'D', 'X', '1', '0' ) == header->ddspf.fourCC) ) + { + // Must be long enough for both headers and magic value + if (ddsDataSize < (sizeof(DDS_HEADER) + sizeof(uint32_t) + sizeof(DDS_HEADER_DXT10))) + { + return E_FAIL; + } + + bDXT10Header = true; + } + + ptrdiff_t offset = sizeof( uint32_t ) + + sizeof( DDS_HEADER ) + + (bDXT10Header ? sizeof( DDS_HEADER_DXT10 ) : 0); + + HRESULT hr = CreateTextureFromDDS( d3dDevice, header, + ddsData + offset, ddsDataSize - offset, maxsize, + usage, bindFlags, cpuAccessFlags, miscFlags, forceSRGB, + texture, textureView ); + if ( SUCCEEDED(hr) ) + { + if (texture != 0 && *texture != 0) + { + SetDebugObjectName(*texture, "DDSTextureLoader"); + } + + if (textureView != 0 && *textureView != 0) + { + SetDebugObjectName(*textureView, "DDSTextureLoader"); + } + + if ( alphaMode ) + *alphaMode = GetAlphaMode( header ); + } + + return hr; +} + +//-------------------------------------------------------------------------------------- +_Use_decl_annotations_ +HRESULT DirectX::CreateDDSTextureFromFile( ID3D11Device* d3dDevice, + const wchar_t* fileName, + ID3D11Resource** texture, + ID3D11ShaderResourceView** textureView, + size_t maxsize, + DDS_ALPHA_MODE* alphaMode ) +{ + return CreateDDSTextureFromFileEx( d3dDevice, fileName, maxsize, + D3D11_USAGE_DEFAULT, D3D11_BIND_SHADER_RESOURCE, 0, 0, false, + texture, textureView, alphaMode ); +} + +_Use_decl_annotations_ +HRESULT DirectX::CreateDDSTextureFromFileEx( ID3D11Device* d3dDevice, + const wchar_t* fileName, + size_t maxsize, + D3D11_USAGE usage, + unsigned int bindFlags, + unsigned int cpuAccessFlags, + unsigned int miscFlags, + bool forceSRGB, + ID3D11Resource** texture, + ID3D11ShaderResourceView** textureView, + DDS_ALPHA_MODE* alphaMode ) +{ + if ( texture ) + { + *texture = nullptr; + } + if ( textureView ) + { + *textureView = nullptr; + } + if ( alphaMode ) + { + *alphaMode = DDS_ALPHA_MODE_UNKNOWN; + } + + if (!d3dDevice || !fileName || (!texture && !textureView)) + { + return E_INVALIDARG; + } + + DDS_HEADER* header = nullptr; + uint8_t* bitData = nullptr; + size_t bitSize = 0; + + std::unique_ptr ddsData; + HRESULT hr = LoadTextureDataFromFile( fileName, + ddsData, + &header, + &bitData, + &bitSize + ); + if (FAILED(hr)) + { + return hr; + } + + hr = CreateTextureFromDDS( d3dDevice, header, + bitData, bitSize, maxsize, + usage, bindFlags, cpuAccessFlags, miscFlags, forceSRGB, + texture, textureView ); + + if ( SUCCEEDED(hr) ) + { +#if !defined(NO_D3D11_DEBUG_NAME) && ( defined(_DEBUG) || defined(PROFILE) ) + if (texture != 0 || textureView != 0) + { + CHAR strFileA[MAX_PATH]; + int result = WideCharToMultiByte( CP_ACP, + WC_NO_BEST_FIT_CHARS, + fileName, + -1, + strFileA, + MAX_PATH, + nullptr, + FALSE + ); + if ( result > 0 ) + { + const CHAR* pstrName = strrchr( strFileA, '\\' ); + if (!pstrName) + { + pstrName = strFileA; + } + else + { + pstrName++; + } + + if (texture != 0 && *texture != 0) + { + (*texture)->SetPrivateData( WKPDID_D3DDebugObjectName, + static_cast( strnlen_s(pstrName, MAX_PATH) ), + pstrName + ); + } + + if (textureView != 0 && *textureView != 0 ) + { + (*textureView)->SetPrivateData( WKPDID_D3DDebugObjectName, + static_cast( strnlen_s(pstrName, MAX_PATH) ), + pstrName + ); + } + } + } +#endif + + if ( alphaMode ) + *alphaMode = GetAlphaMode( header ); + } + + return hr; +} diff --git a/WickedEngine/Utility/DDSTextureLoader.h b/WickedEngine/Utility/DDSTextureLoader.h new file mode 100644 index 000000000..6b5aa95e5 --- /dev/null +++ b/WickedEngine/Utility/DDSTextureLoader.h @@ -0,0 +1,90 @@ +//-------------------------------------------------------------------------------------- +// File: DDSTextureLoader.h +// +// Functions for loading a DDS texture and creating a Direct3D 11 runtime resource for it +// +// Note these functions are useful as a light-weight runtime loader for DDS files. For +// a full-featured DDS file reader, writer, and texture processing pipeline see +// the 'Texconv' sample and the 'DirectXTex' library. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248926 +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#ifdef _MSC_VER +#pragma once +#endif + +#if defined(_XBOX_ONE) && defined(_TITLE) && MONOLITHIC +#include +#else +#include +#endif + +#pragma warning(push) +#pragma warning(disable : 4005) +#include +#pragma warning(pop) + +namespace DirectX +{ + enum DDS_ALPHA_MODE + { + DDS_ALPHA_MODE_UNKNOWN = 0, + DDS_ALPHA_MODE_STRAIGHT = 1, + DDS_ALPHA_MODE_PREMULTIPLIED = 2, + DDS_ALPHA_MODE_OPAQUE = 3, + DDS_ALPHA_MODE_CUSTOM = 4, + }; + + HRESULT CreateDDSTextureFromMemory( _In_ ID3D11Device* d3dDevice, + _In_reads_bytes_(ddsDataSize) const uint8_t* ddsData, + _In_ size_t ddsDataSize, + _Outptr_opt_ ID3D11Resource** texture, + _Outptr_opt_ ID3D11ShaderResourceView** textureView, + _In_ size_t maxsize = 0, + _Out_opt_ DDS_ALPHA_MODE* alphaMode = nullptr + ); + + HRESULT CreateDDSTextureFromFile( _In_ ID3D11Device* d3dDevice, + _In_z_ const wchar_t* szFileName, + _Outptr_opt_ ID3D11Resource** texture, + _Outptr_opt_ ID3D11ShaderResourceView** textureView, + _In_ size_t maxsize = 0, + _Out_opt_ DDS_ALPHA_MODE* alphaMode = nullptr + ); + + HRESULT CreateDDSTextureFromMemoryEx( _In_ ID3D11Device* d3dDevice, + _In_reads_bytes_(ddsDataSize) const uint8_t* ddsData, + _In_ size_t ddsDataSize, + _In_ size_t maxsize, + _In_ D3D11_USAGE usage, + _In_ unsigned int bindFlags, + _In_ unsigned int cpuAccessFlags, + _In_ unsigned int miscFlags, + _In_ bool forceSRGB, + _Outptr_opt_ ID3D11Resource** texture, + _Outptr_opt_ ID3D11ShaderResourceView** textureView, + _Out_opt_ DDS_ALPHA_MODE* alphaMode = nullptr + ); + + HRESULT CreateDDSTextureFromFileEx( _In_ ID3D11Device* d3dDevice, + _In_z_ const wchar_t* szFileName, + _In_ size_t maxsize, + _In_ D3D11_USAGE usage, + _In_ unsigned int bindFlags, + _In_ unsigned int cpuAccessFlags, + _In_ unsigned int miscFlags, + _In_ bool forceSRGB, + _Outptr_opt_ ID3D11Resource** texture, + _Outptr_opt_ ID3D11ShaderResourceView** textureView, + _Out_opt_ DDS_ALPHA_MODE* alphaMode = nullptr + ); +} \ No newline at end of file diff --git a/WickedEngine/Utility/PlatformHelpers.h b/WickedEngine/Utility/PlatformHelpers.h new file mode 100644 index 000000000..e0be0453c --- /dev/null +++ b/WickedEngine/Utility/PlatformHelpers.h @@ -0,0 +1,128 @@ +//-------------------------------------------------------------------------------------- +// File: PlatformHelpers.h +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#pragma once + +#pragma warning(disable : 4324 4481) + +#include + +#if defined(_DEBUG) || defined(PROFILE) +#pragma comment(lib,"dxguid.lib") +#endif + +namespace DirectX +{ + // Helper utility converts D3D API failures into exceptions. + inline void ThrowIfFailed(HRESULT hr) + { + if (FAILED(hr)) + { + throw std::exception(); + } + } + + + // Helper sets a D3D resource name string (used by PIX and debug layer leak reporting). + template + inline void SetDebugObjectName(_In_ ID3D11DeviceChild* resource, _In_z_ const char (&name)[TNameLength]) + { + #if !defined(NO_D3D11_DEBUG_NAME) && ( defined(_DEBUG) || defined(PROFILE) ) + resource->SetPrivateData(WKPDID_D3DDebugObjectName, TNameLength - 1, name); + #else + UNREFERENCED_PARAMETER(resource); + UNREFERENCED_PARAMETER(name); + #endif + } + + + // Helper for output debug tracing + inline void DebugTrace( _In_z_ _Printf_format_string_ const char* format, ... ) + { +#ifdef _DEBUG + va_list args; + va_start( args, format ); + + char buff[1024]; + vsprintf_s( buff, format, args ); + OutputDebugStringA( buff ); +#else + UNREFERENCED_PARAMETER( format ); +#endif + } + + + // Helper smart-pointers + struct handle_closer { void operator()(HANDLE h) { if (h) CloseHandle(h); } }; + + typedef public std::unique_ptr ScopedHandle; + + inline HANDLE safe_handle( HANDLE h ) { return (h == INVALID_HANDLE_VALUE) ? 0 : h; } + + template class ScopedObject : public Microsoft::WRL::ComPtr {}; +} + + +#if defined(_MSC_VER) && (_MSC_VER < 1610) + +// Emulate the C++0x mutex and lock_guard types when building with Visual Studio versions < 2012. +namespace std +{ + class mutex + { + public: + mutex() { InitializeCriticalSection(&mCriticalSection); } + ~mutex() { DeleteCriticalSection(&mCriticalSection); } + + void lock() { EnterCriticalSection(&mCriticalSection); } + void unlock() { LeaveCriticalSection(&mCriticalSection); } + bool try_lock() { return TryEnterCriticalSection(&mCriticalSection) != 0; } + + private: + CRITICAL_SECTION mCriticalSection; + + mutex(mutex const&); + mutex& operator= (mutex const&); + }; + + + template + class lock_guard + { + public: + typedef Mutex mutex_type; + + explicit lock_guard(mutex_type& mutex) + : mMutex(mutex) + { + mMutex.lock(); + } + + ~lock_guard() + { + mMutex.unlock(); + } + + private: + mutex_type& mMutex; + + lock_guard(lock_guard const&); + lock_guard& operator= (lock_guard const&); + }; +} + +#else // _MSC_VER < 1610 + +#include + +#endif diff --git a/WickedEngine/Utility/ScreenGrab.cpp b/WickedEngine/Utility/ScreenGrab.cpp new file mode 100644 index 000000000..e12433562 --- /dev/null +++ b/WickedEngine/Utility/ScreenGrab.cpp @@ -0,0 +1,1076 @@ +//-------------------------------------------------------------------------------------- +// File: ScreenGrab.cpp +// +// Function for capturing a 2D texture and saving it to a file (aka a 'screenshot' +// when used on a Direct3D 11 Render Target). +// +// Note these functions are useful as a light-weight runtime screen grabber. For +// full-featured texture capture, DDS writer, and texture processing pipeline, +// see the 'Texconv' sample and the 'DirectXTex' library. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248926 +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +// Does not capture 1D textures or 3D textures (volume maps) + +// Does not capture mipmap chains, only the top-most texture level is saved + +// For 2D array textures and cubemaps, it captures only the first image in the array + +#include +#include +#include + +#if !defined(WINAPI_FAMILY) || (WINAPI_FAMILY != WINAPI_FAMILY_PHONE_APP) +// VS 2010's stdint.h conflicts with intsafe.h +#pragma warning(push) +#pragma warning(disable : 4005) +#include +#include +#pragma warning(pop) +#endif + +#include + +#include "ScreenGrab.h" + +using Microsoft::WRL::ComPtr; + +//-------------------------------------------------------------------------------------- +// Macros +//-------------------------------------------------------------------------------------- +#ifndef MAKEFOURCC + #define MAKEFOURCC(ch0, ch1, ch2, ch3) \ + ((uint32_t)(uint8_t)(ch0) | ((uint32_t)(uint8_t)(ch1) << 8) | \ + ((uint32_t)(uint8_t)(ch2) << 16) | ((uint32_t)(uint8_t)(ch3) << 24 )) +#endif /* defined(MAKEFOURCC) */ + +//-------------------------------------------------------------------------------------- +// DDS file structure definitions +// +// See DDS.h in the 'Texconv' sample and the 'DirectXTex' library +//-------------------------------------------------------------------------------------- +#pragma pack(push,1) + +#define DDS_MAGIC 0x20534444 // "DDS " + +struct DDS_PIXELFORMAT +{ + uint32_t size; + uint32_t flags; + uint32_t fourCC; + uint32_t RGBBitCount; + uint32_t RBitMask; + uint32_t GBitMask; + uint32_t BBitMask; + uint32_t ABitMask; +}; + +#define DDS_FOURCC 0x00000004 // DDPF_FOURCC +#define DDS_RGB 0x00000040 // DDPF_RGB +#define DDS_RGBA 0x00000041 // DDPF_RGB | DDPF_ALPHAPIXELS +#define DDS_LUMINANCE 0x00020000 // DDPF_LUMINANCE +#define DDS_LUMINANCEA 0x00020001 // DDPF_LUMINANCE | DDPF_ALPHAPIXELS +#define DDS_ALPHA 0x00000002 // DDPF_ALPHA + +#define DDS_HEADER_FLAGS_TEXTURE 0x00001007 // DDSD_CAPS | DDSD_HEIGHT | DDSD_WIDTH | DDSD_PIXELFORMAT +#define DDS_HEADER_FLAGS_MIPMAP 0x00020000 // DDSD_MIPMAPCOUNT +#define DDS_HEADER_FLAGS_PITCH 0x00000008 // DDSD_PITCH +#define DDS_HEADER_FLAGS_LINEARSIZE 0x00080000 // DDSD_LINEARSIZE + +#define DDS_HEIGHT 0x00000002 // DDSD_HEIGHT +#define DDS_WIDTH 0x00000004 // DDSD_WIDTH + +#define DDS_SURFACE_FLAGS_TEXTURE 0x00001000 // DDSCAPS_TEXTURE + +typedef struct +{ + uint32_t size; + uint32_t flags; + uint32_t height; + uint32_t width; + uint32_t pitchOrLinearSize; + uint32_t depth; // only if DDS_HEADER_FLAGS_VOLUME is set in flags + uint32_t mipMapCount; + uint32_t reserved1[11]; + DDS_PIXELFORMAT ddspf; + uint32_t caps; + uint32_t caps2; + uint32_t caps3; + uint32_t caps4; + uint32_t reserved2; +} DDS_HEADER; + +typedef struct +{ + DXGI_FORMAT dxgiFormat; + uint32_t resourceDimension; + uint32_t miscFlag; // see D3D11_RESOURCE_MISC_FLAG + uint32_t arraySize; + uint32_t reserved; +} DDS_HEADER_DXT10; + +#pragma pack(pop) + +static const DDS_PIXELFORMAT DDSPF_DXT1 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','1'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_DXT3 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','3'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_DXT5 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','5'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_BC4_UNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','4','U'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_BC4_SNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','4','S'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_BC5_UNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','5','U'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_BC5_SNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','5','S'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_R8G8_B8G8 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('R','G','B','G'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_G8R8_G8B8 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('G','R','G','B'), 0, 0, 0, 0, 0 }; + +static const DDS_PIXELFORMAT DDSPF_A8R8G8B8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 32, 0x00ff0000, 0x0000ff00, 0x000000ff, 0xff000000 }; + +static const DDS_PIXELFORMAT DDSPF_X8R8G8B8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 32, 0x00ff0000, 0x0000ff00, 0x000000ff, 0x00000000 }; + +static const DDS_PIXELFORMAT DDSPF_A8B8G8R8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 32, 0x000000ff, 0x0000ff00, 0x00ff0000, 0xff000000 }; + +static const DDS_PIXELFORMAT DDSPF_G16R16 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 32, 0x0000ffff, 0xffff0000, 0x00000000, 0x00000000 }; + +static const DDS_PIXELFORMAT DDSPF_R5G6B5 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 16, 0x0000f800, 0x000007e0, 0x0000001f, 0x00000000 }; + +static const DDS_PIXELFORMAT DDSPF_A1R5G5B5 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 16, 0x00007c00, 0x000003e0, 0x0000001f, 0x00008000 }; + +static const DDS_PIXELFORMAT DDSPF_A4R4G4B4 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 16, 0x00000f00, 0x000000f0, 0x0000000f, 0x0000f000 }; + +static const DDS_PIXELFORMAT DDSPF_L8 = + { sizeof(DDS_PIXELFORMAT), DDS_LUMINANCE, 0, 8, 0xff, 0x00, 0x00, 0x00 }; + +static const DDS_PIXELFORMAT DDSPF_L16 = + { sizeof(DDS_PIXELFORMAT), DDS_LUMINANCE, 0, 16, 0xffff, 0x0000, 0x0000, 0x0000 }; + +static const DDS_PIXELFORMAT DDSPF_A8L8 = + { sizeof(DDS_PIXELFORMAT), DDS_LUMINANCEA, 0, 16, 0x00ff, 0x0000, 0x0000, 0xff00 }; + +static const DDS_PIXELFORMAT DDSPF_A8 = + { sizeof(DDS_PIXELFORMAT), DDS_ALPHA, 0, 8, 0x00, 0x00, 0x00, 0xff }; + +// DXGI_FORMAT_R10G10B10A2_UNORM should be written using DX10 extension to avoid D3DX 10:10:10:2 reversal issue + +// This indicates the DDS_HEADER_DXT10 extension is present (the format is in dxgiFormat) +static const DDS_PIXELFORMAT DDSPF_DX10 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','1','0'), 0, 0, 0, 0, 0 }; + +//--------------------------------------------------------------------------------- +struct handle_closer { void operator()(HANDLE h) { if (h) CloseHandle(h); } }; + +typedef public std::unique_ptr ScopedHandle; + +inline HANDLE safe_handle( HANDLE h ) { return (h == INVALID_HANDLE_VALUE) ? 0 : h; } + + +//-------------------------------------------------------------------------------------- +// Return the BPP for a particular format +//-------------------------------------------------------------------------------------- +static size_t BitsPerPixel( _In_ DXGI_FORMAT fmt ) +{ + switch( fmt ) + { + case DXGI_FORMAT_R32G32B32A32_TYPELESS: + case DXGI_FORMAT_R32G32B32A32_FLOAT: + case DXGI_FORMAT_R32G32B32A32_UINT: + case DXGI_FORMAT_R32G32B32A32_SINT: + return 128; + + case DXGI_FORMAT_R32G32B32_TYPELESS: + case DXGI_FORMAT_R32G32B32_FLOAT: + case DXGI_FORMAT_R32G32B32_UINT: + case DXGI_FORMAT_R32G32B32_SINT: + return 96; + + case DXGI_FORMAT_R16G16B16A16_TYPELESS: + case DXGI_FORMAT_R16G16B16A16_FLOAT: + case DXGI_FORMAT_R16G16B16A16_UNORM: + case DXGI_FORMAT_R16G16B16A16_UINT: + case DXGI_FORMAT_R16G16B16A16_SNORM: + case DXGI_FORMAT_R16G16B16A16_SINT: + case DXGI_FORMAT_R32G32_TYPELESS: + case DXGI_FORMAT_R32G32_FLOAT: + case DXGI_FORMAT_R32G32_UINT: + case DXGI_FORMAT_R32G32_SINT: + case DXGI_FORMAT_R32G8X24_TYPELESS: + case DXGI_FORMAT_D32_FLOAT_S8X24_UINT: + case DXGI_FORMAT_R32_FLOAT_X8X24_TYPELESS: + case DXGI_FORMAT_X32_TYPELESS_G8X24_UINT: + return 64; + + case DXGI_FORMAT_R10G10B10A2_TYPELESS: + case DXGI_FORMAT_R10G10B10A2_UNORM: + case DXGI_FORMAT_R10G10B10A2_UINT: + case DXGI_FORMAT_R11G11B10_FLOAT: + case DXGI_FORMAT_R8G8B8A8_TYPELESS: + case DXGI_FORMAT_R8G8B8A8_UNORM: + case DXGI_FORMAT_R8G8B8A8_UNORM_SRGB: + case DXGI_FORMAT_R8G8B8A8_UINT: + case DXGI_FORMAT_R8G8B8A8_SNORM: + case DXGI_FORMAT_R8G8B8A8_SINT: + case DXGI_FORMAT_R16G16_TYPELESS: + case DXGI_FORMAT_R16G16_FLOAT: + case DXGI_FORMAT_R16G16_UNORM: + case DXGI_FORMAT_R16G16_UINT: + case DXGI_FORMAT_R16G16_SNORM: + case DXGI_FORMAT_R16G16_SINT: + case DXGI_FORMAT_R32_TYPELESS: + case DXGI_FORMAT_D32_FLOAT: + case DXGI_FORMAT_R32_FLOAT: + case DXGI_FORMAT_R32_UINT: + case DXGI_FORMAT_R32_SINT: + case DXGI_FORMAT_R24G8_TYPELESS: + case DXGI_FORMAT_D24_UNORM_S8_UINT: + case DXGI_FORMAT_R24_UNORM_X8_TYPELESS: + case DXGI_FORMAT_X24_TYPELESS_G8_UINT: + case DXGI_FORMAT_R9G9B9E5_SHAREDEXP: + case DXGI_FORMAT_R8G8_B8G8_UNORM: + case DXGI_FORMAT_G8R8_G8B8_UNORM: + case DXGI_FORMAT_B8G8R8A8_UNORM: + case DXGI_FORMAT_B8G8R8X8_UNORM: + case DXGI_FORMAT_R10G10B10_XR_BIAS_A2_UNORM: + case DXGI_FORMAT_B8G8R8A8_TYPELESS: + case DXGI_FORMAT_B8G8R8A8_UNORM_SRGB: + case DXGI_FORMAT_B8G8R8X8_TYPELESS: + case DXGI_FORMAT_B8G8R8X8_UNORM_SRGB: + return 32; + + case DXGI_FORMAT_R8G8_TYPELESS: + case DXGI_FORMAT_R8G8_UNORM: + case DXGI_FORMAT_R8G8_UINT: + case DXGI_FORMAT_R8G8_SNORM: + case DXGI_FORMAT_R8G8_SINT: + case DXGI_FORMAT_R16_TYPELESS: + case DXGI_FORMAT_R16_FLOAT: + case DXGI_FORMAT_D16_UNORM: + case DXGI_FORMAT_R16_UNORM: + case DXGI_FORMAT_R16_UINT: + case DXGI_FORMAT_R16_SNORM: + case DXGI_FORMAT_R16_SINT: + case DXGI_FORMAT_B5G6R5_UNORM: + case DXGI_FORMAT_B5G5R5A1_UNORM: + case DXGI_FORMAT_B4G4R4A4_UNORM: + return 16; + + case DXGI_FORMAT_R8_TYPELESS: + case DXGI_FORMAT_R8_UNORM: + case DXGI_FORMAT_R8_UINT: + case DXGI_FORMAT_R8_SNORM: + case DXGI_FORMAT_R8_SINT: + case DXGI_FORMAT_A8_UNORM: + return 8; + + case DXGI_FORMAT_R1_UNORM: + return 1; + + case DXGI_FORMAT_BC1_TYPELESS: + case DXGI_FORMAT_BC1_UNORM: + case DXGI_FORMAT_BC1_UNORM_SRGB: + case DXGI_FORMAT_BC4_TYPELESS: + case DXGI_FORMAT_BC4_UNORM: + case DXGI_FORMAT_BC4_SNORM: + return 4; + + case DXGI_FORMAT_BC2_TYPELESS: + case DXGI_FORMAT_BC2_UNORM: + case DXGI_FORMAT_BC2_UNORM_SRGB: + case DXGI_FORMAT_BC3_TYPELESS: + case DXGI_FORMAT_BC3_UNORM: + case DXGI_FORMAT_BC3_UNORM_SRGB: + case DXGI_FORMAT_BC5_TYPELESS: + case DXGI_FORMAT_BC5_UNORM: + case DXGI_FORMAT_BC5_SNORM: + case DXGI_FORMAT_BC6H_TYPELESS: + case DXGI_FORMAT_BC6H_UF16: + case DXGI_FORMAT_BC6H_SF16: + case DXGI_FORMAT_BC7_TYPELESS: + case DXGI_FORMAT_BC7_UNORM: + case DXGI_FORMAT_BC7_UNORM_SRGB: + return 8; + + default: + return 0; + } +} + + +//-------------------------------------------------------------------------------------- +// Determines if the format is block compressed +//-------------------------------------------------------------------------------------- +static bool IsCompressed( _In_ DXGI_FORMAT fmt ) +{ + switch ( fmt ) + { + case DXGI_FORMAT_BC1_TYPELESS: + case DXGI_FORMAT_BC1_UNORM: + case DXGI_FORMAT_BC1_UNORM_SRGB: + case DXGI_FORMAT_BC2_TYPELESS: + case DXGI_FORMAT_BC2_UNORM: + case DXGI_FORMAT_BC2_UNORM_SRGB: + case DXGI_FORMAT_BC3_TYPELESS: + case DXGI_FORMAT_BC3_UNORM: + case DXGI_FORMAT_BC3_UNORM_SRGB: + case DXGI_FORMAT_BC4_TYPELESS: + case DXGI_FORMAT_BC4_UNORM: + case DXGI_FORMAT_BC4_SNORM: + case DXGI_FORMAT_BC5_TYPELESS: + case DXGI_FORMAT_BC5_UNORM: + case DXGI_FORMAT_BC5_SNORM: + case DXGI_FORMAT_BC6H_TYPELESS: + case DXGI_FORMAT_BC6H_UF16: + case DXGI_FORMAT_BC6H_SF16: + case DXGI_FORMAT_BC7_TYPELESS: + case DXGI_FORMAT_BC7_UNORM: + case DXGI_FORMAT_BC7_UNORM_SRGB: + return true; + + default: + return false; + } +} + + +//-------------------------------------------------------------------------------------- +// Get surface information for a particular format +//-------------------------------------------------------------------------------------- +static void GetSurfaceInfo( _In_ size_t width, + _In_ size_t height, + _In_ DXGI_FORMAT fmt, + _Out_opt_ size_t* outNumBytes, + _Out_opt_ size_t* outRowBytes, + _Out_opt_ size_t* outNumRows ) +{ + size_t numBytes = 0; + size_t rowBytes = 0; + size_t numRows = 0; + + bool bc = false; + bool packed = false; + size_t bcnumBytesPerBlock = 0; + switch (fmt) + { + case DXGI_FORMAT_BC1_TYPELESS: + case DXGI_FORMAT_BC1_UNORM: + case DXGI_FORMAT_BC1_UNORM_SRGB: + case DXGI_FORMAT_BC4_TYPELESS: + case DXGI_FORMAT_BC4_UNORM: + case DXGI_FORMAT_BC4_SNORM: + bc=true; + bcnumBytesPerBlock = 8; + break; + + case DXGI_FORMAT_BC2_TYPELESS: + case DXGI_FORMAT_BC2_UNORM: + case DXGI_FORMAT_BC2_UNORM_SRGB: + case DXGI_FORMAT_BC3_TYPELESS: + case DXGI_FORMAT_BC3_UNORM: + case DXGI_FORMAT_BC3_UNORM_SRGB: + case DXGI_FORMAT_BC5_TYPELESS: + case DXGI_FORMAT_BC5_UNORM: + case DXGI_FORMAT_BC5_SNORM: + case DXGI_FORMAT_BC6H_TYPELESS: + case DXGI_FORMAT_BC6H_UF16: + case DXGI_FORMAT_BC6H_SF16: + case DXGI_FORMAT_BC7_TYPELESS: + case DXGI_FORMAT_BC7_UNORM: + case DXGI_FORMAT_BC7_UNORM_SRGB: + bc = true; + bcnumBytesPerBlock = 16; + break; + + case DXGI_FORMAT_R8G8_B8G8_UNORM: + case DXGI_FORMAT_G8R8_G8B8_UNORM: + packed = true; + break; + } + + if (bc) + { + size_t numBlocksWide = 0; + if (width > 0) + { + numBlocksWide = std::max( 1, (width + 3) / 4 ); + } + size_t numBlocksHigh = 0; + if (height > 0) + { + numBlocksHigh = std::max( 1, (height + 3) / 4 ); + } + rowBytes = numBlocksWide * bcnumBytesPerBlock; + numRows = numBlocksHigh; + } + else if (packed) + { + rowBytes = ( ( width + 1 ) >> 1 ) * 4; + numRows = height; + } + else + { + size_t bpp = BitsPerPixel( fmt ); + rowBytes = ( width * bpp + 7 ) / 8; // round up to nearest byte + numRows = height; + } + + numBytes = rowBytes * numRows; + if (outNumBytes) + { + *outNumBytes = numBytes; + } + if (outRowBytes) + { + *outRowBytes = rowBytes; + } + if (outNumRows) + { + *outNumRows = numRows; + } +} + + +//-------------------------------------------------------------------------------------- +static DXGI_FORMAT EnsureNotTypeless( DXGI_FORMAT fmt ) +{ + // Assumes UNORM or FLOAT; doesn't use UINT or SINT + switch( fmt ) + { + case DXGI_FORMAT_R32G32B32A32_TYPELESS: return DXGI_FORMAT_R32G32B32A32_FLOAT; + case DXGI_FORMAT_R32G32B32_TYPELESS: return DXGI_FORMAT_R32G32B32_FLOAT; + case DXGI_FORMAT_R16G16B16A16_TYPELESS: return DXGI_FORMAT_R16G16B16A16_UNORM; + case DXGI_FORMAT_R32G32_TYPELESS: return DXGI_FORMAT_R32G32_FLOAT; + case DXGI_FORMAT_R10G10B10A2_TYPELESS: return DXGI_FORMAT_R10G10B10A2_UNORM; + case DXGI_FORMAT_R8G8B8A8_TYPELESS: return DXGI_FORMAT_R8G8B8A8_UNORM; + case DXGI_FORMAT_R16G16_TYPELESS: return DXGI_FORMAT_R16G16_UNORM; + case DXGI_FORMAT_R32_TYPELESS: return DXGI_FORMAT_R32_FLOAT; + case DXGI_FORMAT_R8G8_TYPELESS: return DXGI_FORMAT_R8G8_UNORM; + case DXGI_FORMAT_R16_TYPELESS: return DXGI_FORMAT_R16_UNORM; + case DXGI_FORMAT_R8_TYPELESS: return DXGI_FORMAT_R8_UNORM; + case DXGI_FORMAT_BC1_TYPELESS: return DXGI_FORMAT_BC1_UNORM; + case DXGI_FORMAT_BC2_TYPELESS: return DXGI_FORMAT_BC2_UNORM; + case DXGI_FORMAT_BC3_TYPELESS: return DXGI_FORMAT_BC3_UNORM; + case DXGI_FORMAT_BC4_TYPELESS: return DXGI_FORMAT_BC4_UNORM; + case DXGI_FORMAT_BC5_TYPELESS: return DXGI_FORMAT_BC5_UNORM; + case DXGI_FORMAT_B8G8R8A8_TYPELESS: return DXGI_FORMAT_B8G8R8A8_UNORM; + case DXGI_FORMAT_B8G8R8X8_TYPELESS: return DXGI_FORMAT_B8G8R8X8_UNORM; + case DXGI_FORMAT_BC7_TYPELESS: return DXGI_FORMAT_BC7_UNORM; + default: return fmt; + } +} + + +//-------------------------------------------------------------------------------------- +static HRESULT CaptureTexture( _In_ ID3D11DeviceContext* pContext, + _In_ ID3D11Resource* pSource, + _Inout_ D3D11_TEXTURE2D_DESC& desc, + _Inout_ ComPtr& pStaging ) +{ + if ( !pContext || !pSource ) + return E_INVALIDARG; + + D3D11_RESOURCE_DIMENSION resType = D3D11_RESOURCE_DIMENSION_UNKNOWN; + pSource->GetType( &resType ); + + if ( resType != D3D11_RESOURCE_DIMENSION_TEXTURE2D ) + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + + ComPtr pTexture; + HRESULT hr = pSource->QueryInterface( __uuidof(ID3D11Texture2D), reinterpret_cast( pTexture.GetAddressOf() ) ); + if ( FAILED(hr) ) + return hr; + + assert( pTexture ); + + pTexture->GetDesc( &desc ); + + ComPtr d3dDevice; + pContext->GetDevice( d3dDevice.GetAddressOf() ); + + if ( desc.SampleDesc.Count > 1 ) + { + // MSAA content must be resolved before being copied to a staging texture + desc.SampleDesc.Count = 1; + desc.SampleDesc.Quality = 0; + + ComPtr pTemp; + hr = d3dDevice->CreateTexture2D( &desc, 0, pTemp.GetAddressOf() ); + if ( FAILED(hr) ) + return hr; + + assert( pTemp ); + + DXGI_FORMAT fmt = EnsureNotTypeless( desc.Format ); + + UINT support = 0; + hr = d3dDevice->CheckFormatSupport( fmt, &support ); + if ( FAILED(hr) ) + return hr; + + if ( !(support & D3D11_FORMAT_SUPPORT_MULTISAMPLE_RESOLVE) ) + return E_FAIL; + + for( UINT item = 0; item < desc.ArraySize; ++item ) + { + for( UINT level = 0; level < desc.MipLevels; ++level ) + { + UINT index = D3D11CalcSubresource( level, item, desc.MipLevels ); + pContext->ResolveSubresource( pTemp.Get(), index, pSource, index, fmt ); + } + } + + desc.BindFlags = 0; + desc.MiscFlags &= D3D11_RESOURCE_MISC_TEXTURECUBE; + desc.CPUAccessFlags = D3D11_CPU_ACCESS_READ; + desc.Usage = D3D11_USAGE_STAGING; + + hr = d3dDevice->CreateTexture2D( &desc, 0, pStaging.GetAddressOf() ); + if ( FAILED(hr) ) + return hr; + + assert( pStaging ); + + pContext->CopyResource( pStaging.Get(), pTemp.Get() ); + } + else if ( (desc.Usage == D3D11_USAGE_STAGING) && (desc.CPUAccessFlags & D3D11_CPU_ACCESS_READ) ) + { + // Handle case where the source is already a staging texture we can use directly + pStaging = pTexture; + } + else + { + // Otherwise, create a staging texture from the non-MSAA source + desc.BindFlags = 0; + desc.MiscFlags &= D3D11_RESOURCE_MISC_TEXTURECUBE; + desc.CPUAccessFlags = D3D11_CPU_ACCESS_READ; + desc.Usage = D3D11_USAGE_STAGING; + + hr = d3dDevice->CreateTexture2D( &desc, 0, pStaging.GetAddressOf() ); + if ( FAILED(hr) ) + return hr; + + assert( pStaging ); + + pContext->CopyResource( pStaging.Get(), pSource ); + } + + return S_OK; +} + + +//-------------------------------------------------------------------------------------- +#if !defined(WINAPI_FAMILY) || (WINAPI_FAMILY != WINAPI_FAMILY_PHONE_APP) + +static bool g_WIC2 = false; + +static IWICImagingFactory* _GetWIC() +{ + static IWICImagingFactory* s_Factory = nullptr; + + if ( s_Factory ) + return s_Factory; + +#if(_WIN32_WINNT >= _WIN32_WINNT_WIN8) || defined(_WIN7_PLATFORM_UPDATE) + HRESULT hr = CoCreateInstance( + CLSID_WICImagingFactory2, + nullptr, + CLSCTX_INPROC_SERVER, + __uuidof(IWICImagingFactory2), + (LPVOID*)&s_Factory + ); + + if ( SUCCEEDED(hr) ) + { + // WIC2 is available on Windows 8 and Windows 7 SP1 with KB 2670838 installed + g_WIC2 = true; + } + else + { + hr = CoCreateInstance( + CLSID_WICImagingFactory1, + nullptr, + CLSCTX_INPROC_SERVER, + __uuidof(IWICImagingFactory), + (LPVOID*)&s_Factory + ); + + if ( FAILED(hr) ) + { + s_Factory = nullptr; + return nullptr; + } + } +#else + HRESULT hr = CoCreateInstance( + CLSID_WICImagingFactory, + nullptr, + CLSCTX_INPROC_SERVER, + __uuidof(IWICImagingFactory), + (LPVOID*)&s_Factory + ); + + if ( FAILED(hr) ) + { + s_Factory = nullptr; + return nullptr; + } +#endif + + return s_Factory; +} +#endif + + +//-------------------------------------------------------------------------------------- +HRESULT DirectX::SaveDDSTextureToFile( _In_ ID3D11DeviceContext* pContext, + _In_ ID3D11Resource* pSource, + _In_z_ LPCWSTR fileName ) +{ + if ( !fileName ) + return E_INVALIDARG; + + D3D11_TEXTURE2D_DESC desc = { 0 }; + ComPtr pStaging; + HRESULT hr = CaptureTexture( pContext, pSource, desc, pStaging ); + if ( FAILED(hr) ) + return hr; + + // Create file +#if (_WIN32_WINNT >= _WIN32_WINNT_WIN8) + ScopedHandle hFile( safe_handle( CreateFile2( fileName, GENERIC_WRITE, 0, CREATE_ALWAYS, 0 ) ) ); +#else + ScopedHandle hFile( safe_handle( CreateFileW( fileName, GENERIC_WRITE, 0, 0, CREATE_ALWAYS, 0, 0 ) ) ); +#endif + if ( !hFile ) + return HRESULT_FROM_WIN32( GetLastError() ); + + // Setup header + const size_t MAX_HEADER_SIZE = sizeof(uint32_t) + sizeof(DDS_HEADER) + sizeof(DDS_HEADER_DXT10); + uint8_t fileHeader[ MAX_HEADER_SIZE ]; + + *reinterpret_cast(&fileHeader[0]) = DDS_MAGIC; + + auto header = reinterpret_cast( reinterpret_cast(&fileHeader[0]) + sizeof(uint32_t) ); + size_t headerSize = sizeof(uint32_t) + sizeof(DDS_HEADER); + memset( header, 0, sizeof(DDS_HEADER) ); + header->size = sizeof( DDS_HEADER ); + header->flags = DDS_HEADER_FLAGS_TEXTURE | DDS_HEADER_FLAGS_MIPMAP; + header->height = desc.Height; + header->width = desc.Width; + header->mipMapCount = 1; + header->caps = DDS_SURFACE_FLAGS_TEXTURE; + + // Try to use a legacy .DDS pixel format for better tools support, otherwise fallback to 'DX10' header extension + DDS_HEADER_DXT10* extHeader = nullptr; + switch( desc.Format ) + { + case DXGI_FORMAT_R8G8B8A8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_A8B8G8R8, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_R16G16_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_G16R16, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_R8G8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_A8L8, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_R16_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_L16, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_R8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_L8, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_A8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_A8, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_R8G8_B8G8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_R8G8_B8G8, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_G8R8_G8B8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_G8R8_G8B8, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_BC1_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_DXT1, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_BC2_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_DXT3, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_BC3_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_DXT5, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_BC4_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_BC4_UNORM, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_BC4_SNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_BC4_SNORM, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_BC5_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_BC5_UNORM, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_BC5_SNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_BC5_SNORM, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_B5G6R5_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_R5G6B5, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_B5G5R5A1_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_A1R5G5B5, sizeof(DDS_PIXELFORMAT) ); break; + case DXGI_FORMAT_B8G8R8A8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_A8R8G8B8, sizeof(DDS_PIXELFORMAT) ); break; // DXGI 1.1 + case DXGI_FORMAT_B8G8R8X8_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_X8R8G8B8, sizeof(DDS_PIXELFORMAT) ); break; // DXGI 1.1 + case DXGI_FORMAT_B4G4R4A4_UNORM: memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_A4R4G4B4, sizeof(DDS_PIXELFORMAT) ); break; + + // Legacy D3DX formats using D3DFMT enum value as FourCC + case DXGI_FORMAT_R32G32B32A32_FLOAT: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 116; break; // D3DFMT_A32B32G32R32F + case DXGI_FORMAT_R16G16B16A16_FLOAT: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 113; break; // D3DFMT_A16B16G16R16F + case DXGI_FORMAT_R16G16B16A16_UNORM: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 36; break; // D3DFMT_A16B16G16R16 + case DXGI_FORMAT_R16G16B16A16_SNORM: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 110; break; // D3DFMT_Q16W16V16U16 + case DXGI_FORMAT_R32G32_FLOAT: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 115; break; // D3DFMT_G32R32F + case DXGI_FORMAT_R16G16_FLOAT: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 112; break; // D3DFMT_G16R16F + case DXGI_FORMAT_R32_FLOAT: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 114; break; // D3DFMT_R32F + case DXGI_FORMAT_R16_FLOAT: header->ddspf.size = sizeof(DDS_PIXELFORMAT); header->ddspf.flags = DDS_FOURCC; header->ddspf.fourCC = 111; break; // D3DFMT_R16F + + default: + memcpy_s( &header->ddspf, sizeof(header->ddspf), &DDSPF_DX10, sizeof(DDS_PIXELFORMAT) ); + + headerSize += sizeof(DDS_HEADER_DXT10); + extHeader = reinterpret_cast( reinterpret_cast(&fileHeader[0]) + sizeof(uint32_t) + sizeof(DDS_HEADER) ); + memset( extHeader, 0, sizeof(DDS_HEADER_DXT10) ); + extHeader->dxgiFormat = desc.Format; + extHeader->resourceDimension = D3D11_RESOURCE_DIMENSION_TEXTURE2D; + extHeader->arraySize = 1; + break; + } + + size_t rowPitch, slicePitch, rowCount; + GetSurfaceInfo( desc.Width, desc.Height, desc.Format, &slicePitch, &rowPitch, &rowCount ); + + if ( IsCompressed( desc.Format ) ) + { + header->flags |= DDS_HEADER_FLAGS_LINEARSIZE; + header->pitchOrLinearSize = static_cast( slicePitch ); + } + else + { + header->flags |= DDS_HEADER_FLAGS_PITCH; + header->pitchOrLinearSize = static_cast( rowPitch ); + } + + // Setup pixels + std::unique_ptr pixels( new (std::nothrow) uint8_t[ slicePitch ] ); + if (!pixels) + return E_OUTOFMEMORY; + + D3D11_MAPPED_SUBRESOURCE mapped; + hr = pContext->Map( pStaging.Get(), 0, D3D11_MAP_READ, 0, &mapped ); + if ( FAILED(hr) ) + return hr; + + auto sptr = reinterpret_cast( mapped.pData ); + if ( !sptr ) + { + pContext->Unmap( pStaging.Get(), 0 ); + return E_POINTER; + } + + uint8_t* dptr = pixels.get(); + + for( size_t h = 0; h < rowCount; ++h ) + { + size_t msize = std::min( rowPitch, mapped.RowPitch ); + memcpy_s( dptr, rowPitch, sptr, msize ); + sptr += mapped.RowPitch; + dptr += rowPitch; + } + + pContext->Unmap( pStaging.Get(), 0 ); + + // Write header & pixels + DWORD bytesWritten; + if ( !WriteFile( hFile.get(), fileHeader, static_cast( headerSize ), &bytesWritten, 0 ) ) + return HRESULT_FROM_WIN32( GetLastError() ); + + if ( bytesWritten != headerSize ) + return E_FAIL; + + if ( !WriteFile( hFile.get(), pixels.get(), static_cast( slicePitch ), &bytesWritten, 0 ) ) + return HRESULT_FROM_WIN32( GetLastError() ); + + if ( bytesWritten != slicePitch ) + return E_FAIL; + + return S_OK; +} + +//-------------------------------------------------------------------------------------- +#if !defined(WINAPI_FAMILY) || (WINAPI_FAMILY != WINAPI_FAMILY_PHONE_APP) + +HRESULT DirectX::SaveWICTextureToFile( _In_ ID3D11DeviceContext* pContext, + _In_ ID3D11Resource* pSource, + _In_ REFGUID guidContainerFormat, + _In_z_ LPCWSTR fileName, + _In_opt_ const GUID* targetFormat, + _In_opt_ std::function setCustomProps ) +{ + if ( !fileName ) + return E_INVALIDARG; + + D3D11_TEXTURE2D_DESC desc = { 0 }; + ComPtr pStaging; + HRESULT hr = CaptureTexture( pContext, pSource, desc, pStaging ); + if ( FAILED(hr) ) + return hr; + + // Determine source format's WIC equivalent + WICPixelFormatGUID pfGuid; + bool sRGB = false; + switch ( desc.Format ) + { + case DXGI_FORMAT_R32G32B32A32_FLOAT: pfGuid = GUID_WICPixelFormat128bppRGBAFloat; break; + case DXGI_FORMAT_R16G16B16A16_FLOAT: pfGuid = GUID_WICPixelFormat64bppRGBAHalf; break; + case DXGI_FORMAT_R16G16B16A16_UNORM: pfGuid = GUID_WICPixelFormat64bppRGBA; break; + case DXGI_FORMAT_R10G10B10_XR_BIAS_A2_UNORM: pfGuid = GUID_WICPixelFormat32bppRGBA1010102XR; break; // DXGI 1.1 + case DXGI_FORMAT_R10G10B10A2_UNORM: pfGuid = GUID_WICPixelFormat32bppRGBA1010102; break; + case DXGI_FORMAT_B5G5R5A1_UNORM: pfGuid = GUID_WICPixelFormat16bppBGRA5551; break; + case DXGI_FORMAT_B5G6R5_UNORM: pfGuid = GUID_WICPixelFormat16bppBGR565; break; + case DXGI_FORMAT_R32_FLOAT: pfGuid = GUID_WICPixelFormat32bppGrayFloat; break; + case DXGI_FORMAT_R16_FLOAT: pfGuid = GUID_WICPixelFormat16bppGrayHalf; break; + case DXGI_FORMAT_R16_UNORM: pfGuid = GUID_WICPixelFormat16bppGray; break; + case DXGI_FORMAT_R8_UNORM: pfGuid = GUID_WICPixelFormat8bppGray; break; + case DXGI_FORMAT_A8_UNORM: pfGuid = GUID_WICPixelFormat8bppAlpha; break; + + case DXGI_FORMAT_R8G8B8A8_UNORM: + pfGuid = GUID_WICPixelFormat32bppRGBA; + break; + + case DXGI_FORMAT_R8G8B8A8_UNORM_SRGB: + pfGuid = GUID_WICPixelFormat32bppRGBA; + sRGB = true; + break; + + case DXGI_FORMAT_B8G8R8A8_UNORM: // DXGI 1.1 + pfGuid = GUID_WICPixelFormat32bppBGRA; + break; + + case DXGI_FORMAT_B8G8R8A8_UNORM_SRGB: // DXGI 1.1 + pfGuid = GUID_WICPixelFormat32bppBGRA; + sRGB = true; + break; + + case DXGI_FORMAT_B8G8R8X8_UNORM: // DXGI 1.1 + pfGuid = GUID_WICPixelFormat32bppBGR; + break; + + case DXGI_FORMAT_B8G8R8X8_UNORM_SRGB: // DXGI 1.1 + pfGuid = GUID_WICPixelFormat32bppBGR; + sRGB = true; + break; + + default: + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + + IWICImagingFactory* pWIC = _GetWIC(); + if ( !pWIC ) + return E_NOINTERFACE; + + ComPtr stream; + hr = pWIC->CreateStream( stream.GetAddressOf() ); + if ( FAILED(hr) ) + return hr; + + hr = stream->InitializeFromFilename( fileName, GENERIC_WRITE ); + if ( FAILED(hr) ) + return hr; + + ComPtr encoder; + hr = pWIC->CreateEncoder( guidContainerFormat, 0, encoder.GetAddressOf() ); + if ( FAILED(hr) ) + return hr; + + hr = encoder->Initialize( stream.Get(), WICBitmapEncoderNoCache ); + if ( FAILED(hr) ) + return hr; + + ComPtr frame; + ComPtr props; + hr = encoder->CreateNewFrame( frame.GetAddressOf(), props.GetAddressOf() ); + if ( FAILED(hr) ) + return hr; + + if ( targetFormat && memcmp( &guidContainerFormat, &GUID_ContainerFormatBmp, sizeof(WICPixelFormatGUID) ) == 0 && g_WIC2 ) + { + // Opt-in to the WIC2 support for writing 32-bit Windows BMP files with an alpha channel + PROPBAG2 option = { 0 }; + option.pstrName = L"EnableV5Header32bppBGRA"; + + VARIANT varValue; + varValue.vt = VT_BOOL; + varValue.boolVal = VARIANT_TRUE; + (void)props->Write( 1, &option, &varValue ); + } + + if ( setCustomProps ) + { + setCustomProps( props.Get() ); + } + + hr = frame->Initialize( props.Get() ); + if ( FAILED(hr) ) + return hr; + + hr = frame->SetSize( desc.Width , desc.Height ); + if ( FAILED(hr) ) + return hr; + + hr = frame->SetResolution( 72, 72 ); + if ( FAILED(hr) ) + return hr; + + // Pick a target format + WICPixelFormatGUID targetGuid; + if ( targetFormat ) + { + targetGuid = *targetFormat; + } + else + { + // Screenshots don’t typically include the alpha channel of the render target + switch ( desc.Format ) + { +#if (_WIN32_WINNT >= _WIN32_WINNT_WIN8) || defined(_WIN7_PLATFORM_UPDATE) + case DXGI_FORMAT_R32G32B32A32_FLOAT: + case DXGI_FORMAT_R16G16B16A16_FLOAT: + if ( g_WIC2 ) + { + targetGuid = GUID_WICPixelFormat96bppRGBFloat; + } + else + { + targetGuid = GUID_WICPixelFormat24bppBGR; + } + break; +#endif + + case DXGI_FORMAT_R16G16B16A16_UNORM: targetGuid = GUID_WICPixelFormat48bppBGR; break; + case DXGI_FORMAT_B5G5R5A1_UNORM: targetGuid = GUID_WICPixelFormat16bppBGR555; break; + case DXGI_FORMAT_B5G6R5_UNORM: targetGuid = GUID_WICPixelFormat16bppBGR565; break; + + case DXGI_FORMAT_R32_FLOAT: + case DXGI_FORMAT_R16_FLOAT: + case DXGI_FORMAT_R16_UNORM: + case DXGI_FORMAT_R8_UNORM: + case DXGI_FORMAT_A8_UNORM: + targetGuid = GUID_WICPixelFormat8bppGray; + break; + + default: + targetGuid = GUID_WICPixelFormat24bppBGR; + break; + } + } + + hr = frame->SetPixelFormat( &targetGuid ); + if ( FAILED(hr) ) + return hr; + + if ( targetFormat && memcmp( targetFormat, &targetGuid, sizeof(WICPixelFormatGUID) ) != 0 ) + { + // Requested output pixel format is not supported by the WIC codec + return E_FAIL; + } + + // Encode WIC metadata + ComPtr metawriter; + if ( SUCCEEDED( frame->GetMetadataQueryWriter( metawriter.GetAddressOf() ) ) ) + { + PROPVARIANT value; + PropVariantInit( &value ); + + value.vt = VT_LPSTR; + value.pszVal = "DirectXTK"; + + if ( memcmp( &guidContainerFormat, &GUID_ContainerFormatPng, sizeof(GUID) ) == 0 ) + { + // Set Software name + (void)metawriter->SetMetadataByName( L"/tEXt/{str=Software}", &value ); + + // Set sRGB chunk + if ( sRGB ) + { + value.vt = VT_UI1; + value.bVal = 0; + (void)metawriter->SetMetadataByName( L"/sRGB/RenderingIntent", &value ); + } + } + else + { + // Set Software name + (void)metawriter->SetMetadataByName( L"System.ApplicationName", &value ); + + if ( sRGB ) + { + // Set JPEG EXIF Colorspace of sRGB + value.vt = VT_UI2; + value.uiVal = 1; + (void)metawriter->SetMetadataByName( L"System.Image.ColorSpace", &value ); + } + } + } + + D3D11_MAPPED_SUBRESOURCE mapped; + hr = pContext->Map( pStaging.Get(), 0, D3D11_MAP_READ, 0, &mapped ); + if ( FAILED(hr) ) + return hr; + + if ( memcmp( &targetGuid, &pfGuid, sizeof(WICPixelFormatGUID) ) != 0 ) + { + // Conversion required to write + ComPtr source; + hr = pWIC->CreateBitmapFromMemory( desc.Width, desc.Height, pfGuid, + mapped.RowPitch, mapped.RowPitch * desc.Height, + reinterpret_cast( mapped.pData ), source.GetAddressOf() ); + if ( FAILED(hr) ) + { + pContext->Unmap( pStaging.Get(), 0 ); + return hr; + } + + ComPtr FC; + hr = pWIC->CreateFormatConverter( FC.GetAddressOf() ); + if ( FAILED(hr) ) + { + pContext->Unmap( pStaging.Get(), 0 ); + return hr; + } + + hr = FC->Initialize( source.Get(), targetGuid, WICBitmapDitherTypeNone, 0, 0, WICBitmapPaletteTypeCustom ); + if ( FAILED(hr) ) + { + pContext->Unmap( pStaging.Get(), 0 ); + return hr; + } + + WICRect rect = { 0, 0, desc.Width, desc.Height }; + hr = frame->WriteSource( FC.Get(), &rect ); + if ( FAILED(hr) ) + { + pContext->Unmap( pStaging.Get(), 0 ); + return hr; + } + } + else + { + // No conversion required + hr = frame->WritePixels( desc.Height, mapped.RowPitch, mapped.RowPitch * desc.Height, reinterpret_cast( mapped.pData ) ); + if ( FAILED(hr) ) + return hr; + } + + pContext->Unmap( pStaging.Get(), 0 ); + + hr = frame->Commit(); + if ( FAILED(hr) ) + return hr; + + hr = encoder->Commit(); + if ( FAILED(hr) ) + return hr; + + return S_OK; +} + +#endif // !WINAPI_FAMILY || (WINAPI_FAMILY != WINAPI_FAMILY_PHONE_APP) diff --git a/WickedEngine/Utility/ScreenGrab.h b/WickedEngine/Utility/ScreenGrab.h new file mode 100644 index 000000000..c3510ad61 --- /dev/null +++ b/WickedEngine/Utility/ScreenGrab.h @@ -0,0 +1,53 @@ +//-------------------------------------------------------------------------------------- +// File: ScreenGrab.h +// +// Function for capturing a 2D texture and saving it to a file (aka a 'screenshot' +// when used on a Direct3D 11 Render Target). +// +// Note these functions are useful as a light-weight runtime screen grabber. For +// full-featured texture capture, DDS writer, and texture processing pipeline, +// see the 'Texconv' sample and the 'DirectXTex' library. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248926 +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#ifdef _MSC_VER +#pragma once +#endif + +#include + +#include + +#pragma warning(push) +#pragma warning(disable : 4005) +#include +#pragma warning(pop) + +#include + +namespace DirectX +{ + HRESULT SaveDDSTextureToFile( _In_ ID3D11DeviceContext* pContext, + _In_ ID3D11Resource* pSource, + _In_z_ LPCWSTR fileName ); + +#if !defined(WINAPI_FAMILY) || (WINAPI_FAMILY != WINAPI_FAMILY_PHONE_APP) + + HRESULT SaveWICTextureToFile( _In_ ID3D11DeviceContext* pContext, + _In_ ID3D11Resource* pSource, + _In_ REFGUID guidContainerFormat, + _In_z_ LPCWSTR fileName, + _In_opt_ const GUID* targetFormat = nullptr, + _In_opt_ std::function setCustomProps = nullptr ); + +#endif +} \ No newline at end of file diff --git a/WickedEngine/Utility/WicTextureLoader.cpp b/WickedEngine/Utility/WicTextureLoader.cpp new file mode 100644 index 000000000..a367155b3 --- /dev/null +++ b/WickedEngine/Utility/WicTextureLoader.cpp @@ -0,0 +1,692 @@ +//-------------------------------------------------------------------------------------- +// File: WICTextureLoader.cpp +// +// Function for loading a WIC image and creating a Direct3D 11 runtime texture for it +// (auto-generating mipmaps if possible) +// +// Note: Assumes application has already called CoInitializeEx +// +// Warning: CreateWICTexture* functions are not thread-safe if given a d3dContext instance for +// auto-gen mipmap support. +// +// Note these functions are useful for images created as simple 2D textures. For +// more complex resources, DDSTextureLoader is an excellent light-weight runtime loader. +// For a full-featured DDS file reader, writer, and texture processing pipeline see +// the 'Texconv' sample and the 'DirectXTex' library. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248926 +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +// We could load multi-frame images (TIFF/GIF) into a texture array. +// For now, we just load the first frame (note: DirectXTex supports multi-frame images) + +#include +#include + +#pragma warning(push) +#pragma warning(disable : 4005) +#include +#pragma warning(pop) + +#include + +#include "WICTextureLoader.h" + +#if (_WIN32_WINNT >= 0x0602 /*_WIN32_WINNT_WIN8*/) && !defined(DXGI_1_2_FORMATS) +#define DXGI_1_2_FORMATS +#endif + +//--------------------------------------------------------------------------------- +template class ScopedObject +{ +public: + explicit ScopedObject( T *p = 0 ) : _pointer(p) {} + ~ScopedObject() + { + if ( _pointer ) + { + _pointer->Release(); + _pointer = nullptr; + } + } + + bool IsNull() const { return (!_pointer); } + + T& operator*() { return *_pointer; } + T* operator->() { return _pointer; } + T** operator&() { return &_pointer; } + + void Reset(T *p = 0) { if ( _pointer ) { _pointer->Release(); } _pointer = p; } + + T* Get() const { return _pointer; } + +private: + ScopedObject(const ScopedObject&); + ScopedObject& operator=(const ScopedObject&); + + T* _pointer; +}; + +//------------------------------------------------------------------------------------- +// WIC Pixel Format Translation Data +//------------------------------------------------------------------------------------- +struct WICTranslate +{ + GUID wic; + DXGI_FORMAT format; +}; + +static WICTranslate g_WICFormats[] = +{ + { GUID_WICPixelFormat128bppRGBAFloat, DXGI_FORMAT_R32G32B32A32_FLOAT }, + + { GUID_WICPixelFormat64bppRGBAHalf, DXGI_FORMAT_R16G16B16A16_FLOAT }, + { GUID_WICPixelFormat64bppRGBA, DXGI_FORMAT_R16G16B16A16_UNORM }, + + { GUID_WICPixelFormat32bppRGBA, DXGI_FORMAT_R8G8B8A8_UNORM }, + { GUID_WICPixelFormat32bppBGRA, DXGI_FORMAT_B8G8R8A8_UNORM }, // DXGI 1.1 + { GUID_WICPixelFormat32bppBGR, DXGI_FORMAT_B8G8R8X8_UNORM }, // DXGI 1.1 + + { GUID_WICPixelFormat32bppRGBA1010102XR, DXGI_FORMAT_R10G10B10_XR_BIAS_A2_UNORM }, // DXGI 1.1 + { GUID_WICPixelFormat32bppRGBA1010102, DXGI_FORMAT_R10G10B10A2_UNORM }, + { GUID_WICPixelFormat32bppRGBE, DXGI_FORMAT_R9G9B9E5_SHAREDEXP }, + +#ifdef DXGI_1_2_FORMATS + + { GUID_WICPixelFormat16bppBGRA5551, DXGI_FORMAT_B5G5R5A1_UNORM }, + { GUID_WICPixelFormat16bppBGR565, DXGI_FORMAT_B5G6R5_UNORM }, + +#endif // DXGI_1_2_FORMATS + + { GUID_WICPixelFormat32bppGrayFloat, DXGI_FORMAT_R32_FLOAT }, + { GUID_WICPixelFormat16bppGrayHalf, DXGI_FORMAT_R16_FLOAT }, + { GUID_WICPixelFormat16bppGray, DXGI_FORMAT_R16_UNORM }, + { GUID_WICPixelFormat8bppGray, DXGI_FORMAT_R8_UNORM }, + + { GUID_WICPixelFormat8bppAlpha, DXGI_FORMAT_A8_UNORM }, + +#if (_WIN32_WINNT >= 0x0602 /*_WIN32_WINNT_WIN8*/) + { GUID_WICPixelFormat96bppRGBFloat, DXGI_FORMAT_R32G32B32_FLOAT }, +#endif +}; + +//------------------------------------------------------------------------------------- +// WIC Pixel Format nearest conversion table +//------------------------------------------------------------------------------------- + +struct WICConvert +{ + GUID source; + GUID target; +}; + +static WICConvert g_WICConvert[] = +{ + // Note target GUID in this conversion table must be one of those directly supported formats (above). + + { GUID_WICPixelFormatBlackWhite, GUID_WICPixelFormat8bppGray }, // DXGI_FORMAT_R8_UNORM + + { GUID_WICPixelFormat1bppIndexed, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat2bppIndexed, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat4bppIndexed, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat8bppIndexed, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + + { GUID_WICPixelFormat2bppGray, GUID_WICPixelFormat8bppGray }, // DXGI_FORMAT_R8_UNORM + { GUID_WICPixelFormat4bppGray, GUID_WICPixelFormat8bppGray }, // DXGI_FORMAT_R8_UNORM + + { GUID_WICPixelFormat16bppGrayFixedPoint, GUID_WICPixelFormat16bppGrayHalf }, // DXGI_FORMAT_R16_FLOAT + { GUID_WICPixelFormat32bppGrayFixedPoint, GUID_WICPixelFormat32bppGrayFloat }, // DXGI_FORMAT_R32_FLOAT + +#ifdef DXGI_1_2_FORMATS + + { GUID_WICPixelFormat16bppBGR555, GUID_WICPixelFormat16bppBGRA5551 }, // DXGI_FORMAT_B5G5R5A1_UNORM + +#else + + { GUID_WICPixelFormat16bppBGR555, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat16bppBGRA5551, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat16bppBGR565, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + +#endif // DXGI_1_2_FORMATS + + { GUID_WICPixelFormat32bppBGR101010, GUID_WICPixelFormat32bppRGBA1010102 }, // DXGI_FORMAT_R10G10B10A2_UNORM + + { GUID_WICPixelFormat24bppBGR, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat24bppRGB, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat32bppPBGRA, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat32bppPRGBA, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + + { GUID_WICPixelFormat48bppRGB, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + { GUID_WICPixelFormat48bppBGR, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + { GUID_WICPixelFormat64bppBGRA, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + { GUID_WICPixelFormat64bppPRGBA, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + { GUID_WICPixelFormat64bppPBGRA, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + + { GUID_WICPixelFormat48bppRGBFixedPoint, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT + { GUID_WICPixelFormat48bppBGRFixedPoint, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT + { GUID_WICPixelFormat64bppRGBAFixedPoint, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT + { GUID_WICPixelFormat64bppBGRAFixedPoint, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT + { GUID_WICPixelFormat64bppRGBFixedPoint, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT + { GUID_WICPixelFormat64bppRGBHalf, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT + { GUID_WICPixelFormat48bppRGBHalf, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT + + { GUID_WICPixelFormat96bppRGBFixedPoint, GUID_WICPixelFormat128bppRGBAFloat }, // DXGI_FORMAT_R32G32B32A32_FLOAT + { GUID_WICPixelFormat128bppPRGBAFloat, GUID_WICPixelFormat128bppRGBAFloat }, // DXGI_FORMAT_R32G32B32A32_FLOAT + { GUID_WICPixelFormat128bppRGBFloat, GUID_WICPixelFormat128bppRGBAFloat }, // DXGI_FORMAT_R32G32B32A32_FLOAT + { GUID_WICPixelFormat128bppRGBAFixedPoint, GUID_WICPixelFormat128bppRGBAFloat }, // DXGI_FORMAT_R32G32B32A32_FLOAT + { GUID_WICPixelFormat128bppRGBFixedPoint, GUID_WICPixelFormat128bppRGBAFloat }, // DXGI_FORMAT_R32G32B32A32_FLOAT + + { GUID_WICPixelFormat32bppCMYK, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat64bppCMYK, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + { GUID_WICPixelFormat40bppCMYKAlpha, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + { GUID_WICPixelFormat80bppCMYKAlpha, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + +#if (_WIN32_WINNT >= 0x0602 /*_WIN32_WINNT_WIN8*/) + { GUID_WICPixelFormat32bppRGB, GUID_WICPixelFormat32bppRGBA }, // DXGI_FORMAT_R8G8B8A8_UNORM + { GUID_WICPixelFormat64bppRGB, GUID_WICPixelFormat64bppRGBA }, // DXGI_FORMAT_R16G16B16A16_UNORM + { GUID_WICPixelFormat64bppPRGBAHalf, GUID_WICPixelFormat64bppRGBAHalf }, // DXGI_FORMAT_R16G16B16A16_FLOAT +#endif + + // We don't support n-channel formats +}; + +//-------------------------------------------------------------------------------------- +static IWICImagingFactory* _GetWIC() +{ + static IWICImagingFactory* s_Factory = nullptr; + + if ( s_Factory ) + return s_Factory; + + HRESULT hr = CoCreateInstance( + CLSID_WICImagingFactory, + nullptr, + CLSCTX_INPROC_SERVER, + __uuidof(IWICImagingFactory), + (LPVOID*)&s_Factory + ); + + if ( FAILED(hr) ) + { + s_Factory = nullptr; + return nullptr; + } + + return s_Factory; +} + +//--------------------------------------------------------------------------------- +static DXGI_FORMAT _WICToDXGI( const GUID& guid ) +{ + for( size_t i=0; i < _countof(g_WICFormats); ++i ) + { + if ( memcmp( &g_WICFormats[i].wic, &guid, sizeof(GUID) ) == 0 ) + return g_WICFormats[i].format; + } + + return DXGI_FORMAT_UNKNOWN; +} + +//--------------------------------------------------------------------------------- +static size_t _WICBitsPerPixel( REFGUID targetGuid ) +{ + IWICImagingFactory* pWIC = _GetWIC(); + if ( !pWIC ) + return 0; + + ScopedObject cinfo; + if ( FAILED( pWIC->CreateComponentInfo( targetGuid, &cinfo ) ) ) + return 0; + + WICComponentType type; + if ( FAILED( cinfo->GetComponentType( &type ) ) ) + return 0; + + if ( type != WICPixelFormat ) + return 0; + + ScopedObject pfinfo; + if ( FAILED( cinfo->QueryInterface( __uuidof(IWICPixelFormatInfo), reinterpret_cast( &pfinfo ) ) ) ) + return 0; + + UINT bpp; + if ( FAILED( pfinfo->GetBitsPerPixel( &bpp ) ) ) + return 0; + + return bpp; +} + +//--------------------------------------------------------------------------------- +static HRESULT CreateTextureFromWIC( _In_ BOOL toMipMap, + _In_ ID3D11Device* d3dDevice, + _In_opt_ ID3D11DeviceContext* d3dContext, + _In_ IWICBitmapFrameDecode *frame, + _Out_opt_ ID3D11Resource** texture, + _Out_opt_ ID3D11ShaderResourceView** textureView, + _In_ size_t maxsize ) +{ + UINT width, height; + HRESULT hr = frame->GetSize( &width, &height ); + if ( FAILED(hr) ) + return hr; + + assert( width > 0 && height > 0 ); + + if ( !maxsize ) + { + // This is a bit conservative because the hardware could support larger textures than + // the Feature Level defined minimums, but doing it this way is much easier and more + // performant for WIC than the 'fail and retry' model used by DDSTextureLoader + + switch( d3dDevice->GetFeatureLevel() ) + { + case D3D_FEATURE_LEVEL_9_1: + case D3D_FEATURE_LEVEL_9_2: + maxsize = 2048 /*D3D_FL9_1_REQ_TEXTURE2D_U_OR_V_DIMENSION*/; + break; + + case D3D_FEATURE_LEVEL_9_3: + maxsize = 4096 /*D3D_FL9_3_REQ_TEXTURE2D_U_OR_V_DIMENSION*/; + break; + + case D3D_FEATURE_LEVEL_10_0: + case D3D_FEATURE_LEVEL_10_1: + maxsize = 8192 /*D3D10_REQ_TEXTURE2D_U_OR_V_DIMENSION*/; + break; + + default: + maxsize = D3D11_REQ_TEXTURE2D_U_OR_V_DIMENSION; + break; + } + } + + assert( maxsize > 0 ); + + UINT twidth, theight; + if ( width > maxsize || height > maxsize ) + { + float ar = static_cast(height) / static_cast(width); + if ( width > height ) + { + twidth = static_cast( maxsize ); + theight = static_cast( static_cast(maxsize) * ar ); + } + else + { + theight = static_cast( maxsize ); + twidth = static_cast( static_cast(maxsize) / ar ); + } + assert( twidth <= maxsize && theight <= maxsize ); + } + else + { + twidth = width; + theight = height; + } + + // Determine format + WICPixelFormatGUID pixelFormat; + hr = frame->GetPixelFormat( &pixelFormat ); + if ( FAILED(hr) ) + return hr; + + WICPixelFormatGUID convertGUID; + memcpy( &convertGUID, &pixelFormat, sizeof(WICPixelFormatGUID) ); + + size_t bpp = 0; + + DXGI_FORMAT format = _WICToDXGI( pixelFormat ); + if ( format == DXGI_FORMAT_UNKNOWN ) + { + for( size_t i=0; i < _countof(g_WICConvert); ++i ) + { + if ( memcmp( &g_WICConvert[i].source, &pixelFormat, sizeof(WICPixelFormatGUID) ) == 0 ) + { + memcpy( &convertGUID, &g_WICConvert[i].target, sizeof(WICPixelFormatGUID) ); + + format = _WICToDXGI( g_WICConvert[i].target ); + assert( format != DXGI_FORMAT_UNKNOWN ); + bpp = _WICBitsPerPixel( convertGUID ); + break; + } + } + + if ( format == DXGI_FORMAT_UNKNOWN ) + return HRESULT_FROM_WIN32( ERROR_NOT_SUPPORTED ); + } + else + { + bpp = _WICBitsPerPixel( pixelFormat ); + } + + if ( !bpp ) + return E_FAIL; + + // Verify our target format is supported by the current device + // (handles WDDM 1.0 or WDDM 1.1 device driver cases as well as DirectX 11.0 Runtime without 16bpp format support) + UINT support = 0; + hr = d3dDevice->CheckFormatSupport( format, &support ); + if ( FAILED(hr) || !(support & D3D11_FORMAT_SUPPORT_TEXTURE2D) ) + { + // Fallback to RGBA 32-bit format which is supported by all devices + memcpy( &convertGUID, &GUID_WICPixelFormat32bppRGBA, sizeof(WICPixelFormatGUID) ); + format = DXGI_FORMAT_R8G8B8A8_UNORM; + bpp = 32; + } + + // Allocate temporary memory for image + size_t rowPitch = ( twidth * bpp + 7 ) / 8; + size_t imageSize = rowPitch * theight; + + std::unique_ptr temp( new uint8_t[ imageSize ] ); + + // Load image data + if ( memcmp( &convertGUID, &pixelFormat, sizeof(GUID) ) == 0 + && twidth == width + && theight == height ) + { + // No format conversion or resize needed + hr = frame->CopyPixels( 0, static_cast( rowPitch ), static_cast( imageSize ), temp.get() ); + if ( FAILED(hr) ) + return hr; + } + else if ( twidth != width || theight != height ) + { + // Resize + IWICImagingFactory* pWIC = _GetWIC(); + if ( !pWIC ) + return E_NOINTERFACE; + + ScopedObject scaler; + hr = pWIC->CreateBitmapScaler( &scaler ); + if ( FAILED(hr) ) + return hr; + + hr = scaler->Initialize( frame, twidth, theight, WICBitmapInterpolationModeFant ); + if ( FAILED(hr) ) + return hr; + + WICPixelFormatGUID pfScaler; + hr = scaler->GetPixelFormat( &pfScaler ); + if ( FAILED(hr) ) + return hr; + + if ( memcmp( &convertGUID, &pfScaler, sizeof(GUID) ) == 0 ) + { + // No format conversion needed + hr = scaler->CopyPixels( 0, static_cast( rowPitch ), static_cast( imageSize ), temp.get() ); + if ( FAILED(hr) ) + return hr; + } + else + { + ScopedObject FC; + hr = pWIC->CreateFormatConverter( &FC ); + if ( FAILED(hr) ) + return hr; + + hr = FC->Initialize( scaler.Get(), convertGUID, WICBitmapDitherTypeErrorDiffusion, 0, 0, WICBitmapPaletteTypeCustom ); + if ( FAILED(hr) ) + return hr; + + hr = FC->CopyPixels( 0, static_cast( rowPitch ), static_cast( imageSize ), temp.get() ); + if ( FAILED(hr) ) + return hr; + } + } + else + { + // Format conversion but no resize + IWICImagingFactory* pWIC = _GetWIC(); + if ( !pWIC ) + return E_NOINTERFACE; + + ScopedObject FC; + hr = pWIC->CreateFormatConverter( &FC ); + if ( FAILED(hr) ) + return hr; + + hr = FC->Initialize( frame, convertGUID, WICBitmapDitherTypeErrorDiffusion, 0, 0, WICBitmapPaletteTypeCustom ); + if ( FAILED(hr) ) + return hr; + + hr = FC->CopyPixels( 0, static_cast( rowPitch ), static_cast( imageSize ), temp.get() ); + if ( FAILED(hr) ) + return hr; + } + + // See if format is supported for auto-gen mipmaps (varies by feature level) + bool autogen = false; + if ( d3dContext != 0 && textureView != 0 && toMipMap ) // Must have context and shader-view to auto generate mipmaps + { + UINT fmtSupport = 0; + hr = d3dDevice->CheckFormatSupport( format, &fmtSupport ); + if ( SUCCEEDED(hr) && ( fmtSupport & D3D11_FORMAT_SUPPORT_MIP_AUTOGEN ) ) + { + autogen = true; + } + } + + // Create texture + D3D11_TEXTURE2D_DESC desc; + desc.Width = twidth; + desc.Height = theight; + desc.MipLevels = (autogen) ? 0 : 1; + desc.ArraySize = 1; + desc.Format = format; + desc.SampleDesc.Count = 1; + desc.SampleDesc.Quality = 0; + desc.Usage = D3D11_USAGE_DEFAULT; + desc.BindFlags = (autogen) ? (D3D11_BIND_SHADER_RESOURCE | D3D11_BIND_RENDER_TARGET) : (D3D11_BIND_SHADER_RESOURCE); + desc.CPUAccessFlags = 0; + desc.MiscFlags = (autogen) ? D3D11_RESOURCE_MISC_GENERATE_MIPS : 0; + + D3D11_SUBRESOURCE_DATA initData; + initData.pSysMem = temp.get(); + initData.SysMemPitch = static_cast( rowPitch ); + initData.SysMemSlicePitch = static_cast( imageSize ); + + ID3D11Texture2D* tex = nullptr; + hr = d3dDevice->CreateTexture2D( &desc, (autogen) ? nullptr : &initData, &tex ); + if ( SUCCEEDED(hr) && tex != 0 ) + { + if (textureView != 0) + { + D3D11_SHADER_RESOURCE_VIEW_DESC SRVDesc; + memset( &SRVDesc, 0, sizeof( SRVDesc ) ); + SRVDesc.Format = format; + SRVDesc.ViewDimension = D3D11_SRV_DIMENSION_TEXTURE2D; + SRVDesc.Texture2D.MipLevels = (autogen) ? -1 : 1; + + hr = d3dDevice->CreateShaderResourceView( tex, &SRVDesc, textureView ); + if ( FAILED(hr) ) + { + tex->Release(); + return hr; + } + + if ( autogen ) + { + assert( d3dContext != 0 ); + d3dContext->UpdateSubresource( tex, 0, nullptr, temp.get(), static_cast(rowPitch), static_cast(imageSize) ); + d3dContext->GenerateMips( *textureView ); + } + } + + if (texture != 0) + { + *texture = tex; + } + else + { +#if defined(_DEBUG) || defined(PROFILE) + tex->SetPrivateData( WKPDID_D3DDebugObjectName, + sizeof("WICTextureLoader")-1, + "WICTextureLoader" + ); +#endif + tex->Release(); + } + } + + return hr; +} + +//-------------------------------------------------------------------------------------- +HRESULT CreateWICTextureFromMemory( _In_ ID3D11Device* d3dDevice, + _In_opt_ ID3D11DeviceContext* d3dContext, + _In_bytecount_(wicDataSize) const uint8_t* wicData, + _In_ size_t wicDataSize, + _Out_opt_ ID3D11Resource** texture, + _Out_opt_ ID3D11ShaderResourceView** textureView, + _In_ size_t maxsize + ) +{ + if (!d3dDevice || !wicData || (!texture && !textureView)) + { + return E_INVALIDARG; + } + + if ( !wicDataSize ) + { + return E_FAIL; + } + +#ifdef _M_AMD64 + if ( wicDataSize > 0xFFFFFFFF ) + return HRESULT_FROM_WIN32( ERROR_FILE_TOO_LARGE ); +#endif + + IWICImagingFactory* pWIC = _GetWIC(); + if ( !pWIC ) + return E_NOINTERFACE; + + // Create input stream for memory + ScopedObject stream; + HRESULT hr = pWIC->CreateStream( &stream ); + if ( FAILED(hr) ) + return hr; + + hr = stream->InitializeFromMemory( const_cast( wicData ), static_cast( wicDataSize ) ); + if ( FAILED(hr) ) + return hr; + + // Initialize WIC + ScopedObject decoder; + hr = pWIC->CreateDecoderFromStream( stream.Get(), 0, WICDecodeMetadataCacheOnDemand, &decoder ); + if ( FAILED(hr) ) + return hr; + + ScopedObject frame; + hr = decoder->GetFrame( 0, &frame ); + if ( FAILED(hr) ) + return hr; + + hr = CreateTextureFromWIC( FALSE, d3dDevice, d3dContext, frame.Get(), texture, textureView, maxsize ); + if ( FAILED(hr)) + return hr; + +#if defined(_DEBUG) || defined(PROFILE) + if (texture != 0 && *texture != 0) + { + (*texture)->SetPrivateData( WKPDID_D3DDebugObjectName, + sizeof("WICTextureLoader")-1, + "WICTextureLoader" + ); + } + + if (textureView != 0 && *textureView != 0) + { + (*textureView)->SetPrivateData( WKPDID_D3DDebugObjectName, + sizeof("WICTextureLoader")-1, + "WICTextureLoader" + ); + } +#endif + + return hr; +} + +//-------------------------------------------------------------------------------------- +HRESULT CreateWICTextureFromFile( _In_ BOOL toMipMap, _In_ ID3D11Device* d3dDevice, + _In_opt_ ID3D11DeviceContext* d3dContext, + _In_z_ const wchar_t* fileName, + _Out_opt_ ID3D11Resource** texture, + _Out_opt_ ID3D11ShaderResourceView** textureView, + _In_ size_t maxsize ) +{ + if (!d3dDevice || !fileName || (!texture && !textureView)) + { + return E_INVALIDARG; + } + + IWICImagingFactory* pWIC = _GetWIC(); + if ( !pWIC ) + return E_NOINTERFACE; + + // Initialize WIC + ScopedObject decoder; + HRESULT hr = pWIC->CreateDecoderFromFilename( fileName, 0, GENERIC_READ, WICDecodeMetadataCacheOnDemand, &decoder ); + if ( FAILED(hr) ) + return hr; + + ScopedObject frame; + hr = decoder->GetFrame( 0, &frame ); + if ( FAILED(hr) ) + return hr; + + hr = CreateTextureFromWIC( toMipMap, d3dDevice, d3dContext, frame.Get(), texture, textureView, maxsize ); + if ( FAILED(hr)) + return hr; + +#if defined(_DEBUG) || defined(PROFILE) + if (texture != 0 || textureView != 0) + { + CHAR strFileA[MAX_PATH]; + WideCharToMultiByte( CP_ACP, + WC_NO_BEST_FIT_CHARS, + fileName, + -1, + strFileA, + MAX_PATH, + nullptr, + FALSE + ); + const CHAR* pstrName = strrchr( strFileA, '\\' ); + if (!pstrName) + { + pstrName = strFileA; + } + else + { + pstrName++; + } + + if (texture != 0 && *texture != 0) + { + (*texture)->SetPrivateData( WKPDID_D3DDebugObjectName, + static_cast( strnlen_s(pstrName, MAX_PATH) ), + pstrName + ); + } + + if (textureView != 0 && *textureView != 0 ) + { + (*textureView)->SetPrivateData( WKPDID_D3DDebugObjectName, + static_cast( strnlen_s(pstrName, MAX_PATH) ), + pstrName + ); + } + } +#endif + + return hr; +} \ No newline at end of file diff --git a/WickedEngine/Utility/WicTextureLoader.h b/WickedEngine/Utility/WicTextureLoader.h new file mode 100644 index 000000000..f5d8cc44b --- /dev/null +++ b/WickedEngine/Utility/WicTextureLoader.h @@ -0,0 +1,55 @@ +//-------------------------------------------------------------------------------------- +// File: WICTextureLoader.h +// +// Function for loading a WIC image and creating a Direct3D 11 runtime texture for it +// (auto-generating mipmaps if possible) +// +// Note: Assumes application has already called CoInitializeEx +// +// Warning: CreateWICTexture* functions are not thread-safe if given a d3dContext instance for +// auto-gen support. +// +// Note these functions are useful for images created as simple 2D textures. For +// more complex resources, DDSTextureLoader is an excellent light-weight runtime loader. +// For a full-featured DDS file reader, writer, and texture processing pipeline see +// the 'Texconv' sample and the 'DirectXTex' library. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248926 +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#ifdef _MSC_VER +#pragma once +#endif + +#include + +#pragma warning(push) +#pragma warning(disable : 4005) +#include +#pragma warning(pop) + +HRESULT CreateWICTextureFromMemory( _In_ ID3D11Device* d3dDevice, + _In_opt_ ID3D11DeviceContext* d3dContext, + _In_bytecount_(wicDataSize) const uint8_t* wicData, + _In_ size_t wicDataSize, + _Out_opt_ ID3D11Resource** texture, + _Out_opt_ ID3D11ShaderResourceView** textureView, + _In_ size_t maxsize = 0 + ); + +HRESULT CreateWICTextureFromFile( _In_ BOOL toMipMap, + _In_ ID3D11Device* d3dDevice, + _In_opt_ ID3D11DeviceContext* d3dContext, + _In_z_ const wchar_t* szFileName, + _Out_opt_ ID3D11Resource** texture, + _Out_opt_ ID3D11ShaderResourceView** textureView, + _In_ size_t maxsize = 0 + ); \ No newline at end of file diff --git a/WickedEngine/Utility/dds.h b/WickedEngine/Utility/dds.h new file mode 100644 index 000000000..10308079a --- /dev/null +++ b/WickedEngine/Utility/dds.h @@ -0,0 +1,227 @@ +//-------------------------------------------------------------------------------------- +// dds.h +// +// This header defines constants and structures that are useful when parsing +// DDS files. DDS files were originally designed to use several structures +// and constants that are native to DirectDraw and are defined in ddraw.h, +// such as DDSURFACEDESC2 and DDSCAPS2. This file defines similar +// (compatible) constants and structures so that one can use DDS files +// without needing to include ddraw.h. +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248926 +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#if defined(_MSC_VER) +#pragma once +#endif + +#if defined(_XBOX_ONE) && defined(_TITLE) && MONOLITHIC +#include +#else +#include +#endif + +#pragma warning(push) +#pragma warning(disable : 4005) +#include +#pragma warning(pop) + +namespace DirectX +{ + +#pragma pack(push,1) + +const uint32_t DDS_MAGIC = 0x20534444; // "DDS " + +struct DDS_PIXELFORMAT +{ + uint32_t size; + uint32_t flags; + uint32_t fourCC; + uint32_t RGBBitCount; + uint32_t RBitMask; + uint32_t GBitMask; + uint32_t BBitMask; + uint32_t ABitMask; +}; + +#define DDS_FOURCC 0x00000004 // DDPF_FOURCC +#define DDS_RGB 0x00000040 // DDPF_RGB +#define DDS_RGBA 0x00000041 // DDPF_RGB | DDPF_ALPHAPIXELS +#define DDS_LUMINANCE 0x00020000 // DDPF_LUMINANCE +#define DDS_LUMINANCEA 0x00020001 // DDPF_LUMINANCE | DDPF_ALPHAPIXELS +#define DDS_ALPHA 0x00000002 // DDPF_ALPHA +#define DDS_PAL8 0x00000020 // DDPF_PALETTEINDEXED8 + +#ifndef MAKEFOURCC + #define MAKEFOURCC(ch0, ch1, ch2, ch3) \ + ((uint32_t)(uint8_t)(ch0) | ((uint32_t)(uint8_t)(ch1) << 8) | \ + ((uint32_t)(uint8_t)(ch2) << 16) | ((uint32_t)(uint8_t)(ch3) << 24 )) +#endif /* defined(MAKEFOURCC) */ + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_DXT1 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','1'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_DXT2 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','2'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_DXT3 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','3'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_DXT4 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','4'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_DXT5 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','T','5'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_BC4_UNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','4','U'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_BC4_SNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','4','S'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_BC5_UNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','5','U'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_BC5_SNORM = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('B','C','5','S'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_R8G8_B8G8 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('R','G','B','G'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_G8R8_G8B8 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('G','R','G','B'), 0, 0, 0, 0, 0 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_A8R8G8B8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 32, 0x00ff0000, 0x0000ff00, 0x000000ff, 0xff000000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_X8R8G8B8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 32, 0x00ff0000, 0x0000ff00, 0x000000ff, 0x00000000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_A8B8G8R8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 32, 0x000000ff, 0x0000ff00, 0x00ff0000, 0xff000000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_X8B8G8R8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 32, 0x000000ff, 0x0000ff00, 0x00ff0000, 0x00000000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_G16R16 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 32, 0x0000ffff, 0xffff0000, 0x00000000, 0x00000000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_R5G6B5 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 16, 0x0000f800, 0x000007e0, 0x0000001f, 0x00000000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_A1R5G5B5 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 16, 0x00007c00, 0x000003e0, 0x0000001f, 0x00008000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_A4R4G4B4 = + { sizeof(DDS_PIXELFORMAT), DDS_RGBA, 0, 16, 0x00000f00, 0x000000f0, 0x0000000f, 0x0000f000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_R8G8B8 = + { sizeof(DDS_PIXELFORMAT), DDS_RGB, 0, 24, 0x00ff0000, 0x0000ff00, 0x000000ff, 0x00000000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_L8 = + { sizeof(DDS_PIXELFORMAT), DDS_LUMINANCE, 0, 8, 0xff, 0x00, 0x00, 0x00 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_L16 = + { sizeof(DDS_PIXELFORMAT), DDS_LUMINANCE, 0, 16, 0xffff, 0x0000, 0x0000, 0x0000 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_A8L8 = + { sizeof(DDS_PIXELFORMAT), DDS_LUMINANCEA, 0, 16, 0x00ff, 0x0000, 0x0000, 0xff00 }; + +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_A8 = + { sizeof(DDS_PIXELFORMAT), DDS_ALPHA, 0, 8, 0x00, 0x00, 0x00, 0xff }; + +// D3DFMT_A2R10G10B10/D3DFMT_A2B10G10R10 should be written using DX10 extension to avoid D3DX 10:10:10:2 reversal issue + +// This indicates the DDS_HEADER_DXT10 extension is present (the format is in dxgiFormat) +extern __declspec(selectany) const DDS_PIXELFORMAT DDSPF_DX10 = + { sizeof(DDS_PIXELFORMAT), DDS_FOURCC, MAKEFOURCC('D','X','1','0'), 0, 0, 0, 0, 0 }; + +#define DDS_HEADER_FLAGS_TEXTURE 0x00001007 // DDSD_CAPS | DDSD_HEIGHT | DDSD_WIDTH | DDSD_PIXELFORMAT +#define DDS_HEADER_FLAGS_MIPMAP 0x00020000 // DDSD_MIPMAPCOUNT +#define DDS_HEADER_FLAGS_VOLUME 0x00800000 // DDSD_DEPTH +#define DDS_HEADER_FLAGS_PITCH 0x00000008 // DDSD_PITCH +#define DDS_HEADER_FLAGS_LINEARSIZE 0x00080000 // DDSD_LINEARSIZE + +#define DDS_HEIGHT 0x00000002 // DDSD_HEIGHT +#define DDS_WIDTH 0x00000004 // DDSD_WIDTH + +#define DDS_SURFACE_FLAGS_TEXTURE 0x00001000 // DDSCAPS_TEXTURE +#define DDS_SURFACE_FLAGS_MIPMAP 0x00400008 // DDSCAPS_COMPLEX | DDSCAPS_MIPMAP +#define DDS_SURFACE_FLAGS_CUBEMAP 0x00000008 // DDSCAPS_COMPLEX + +#define DDS_CUBEMAP_POSITIVEX 0x00000600 // DDSCAPS2_CUBEMAP | DDSCAPS2_CUBEMAP_POSITIVEX +#define DDS_CUBEMAP_NEGATIVEX 0x00000a00 // DDSCAPS2_CUBEMAP | DDSCAPS2_CUBEMAP_NEGATIVEX +#define DDS_CUBEMAP_POSITIVEY 0x00001200 // DDSCAPS2_CUBEMAP | DDSCAPS2_CUBEMAP_POSITIVEY +#define DDS_CUBEMAP_NEGATIVEY 0x00002200 // DDSCAPS2_CUBEMAP | DDSCAPS2_CUBEMAP_NEGATIVEY +#define DDS_CUBEMAP_POSITIVEZ 0x00004200 // DDSCAPS2_CUBEMAP | DDSCAPS2_CUBEMAP_POSITIVEZ +#define DDS_CUBEMAP_NEGATIVEZ 0x00008200 // DDSCAPS2_CUBEMAP | DDSCAPS2_CUBEMAP_NEGATIVEZ + +#define DDS_CUBEMAP_ALLFACES ( DDS_CUBEMAP_POSITIVEX | DDS_CUBEMAP_NEGATIVEX |\ + DDS_CUBEMAP_POSITIVEY | DDS_CUBEMAP_NEGATIVEY |\ + DDS_CUBEMAP_POSITIVEZ | DDS_CUBEMAP_NEGATIVEZ ) + +#define DDS_CUBEMAP 0x00000200 // DDSCAPS2_CUBEMAP + +#define DDS_FLAGS_VOLUME 0x00200000 // DDSCAPS2_VOLUME + +// Subset here matches D3D10_RESOURCE_DIMENSION and D3D11_RESOURCE_DIMENSION +enum DDS_RESOURCE_DIMENSION +{ + DDS_DIMENSION_TEXTURE1D = 2, + DDS_DIMENSION_TEXTURE2D = 3, + DDS_DIMENSION_TEXTURE3D = 4, +}; + +// Subset here matches D3D10_RESOURCE_MISC_FLAG and D3D11_RESOURCE_MISC_FLAG +enum DDS_RESOURCE_MISC_FLAG +{ + DDS_RESOURCE_MISC_TEXTURECUBE = 0x4L, +}; + +enum DDS_MISC_FLAGS2 +{ + DDS_MISC_FLAGS2_ALPHA_MODE_MASK = 0x7L, +}; + +struct DDS_HEADER +{ + uint32_t size; + uint32_t flags; + uint32_t height; + uint32_t width; + uint32_t pitchOrLinearSize; + uint32_t depth; // only if DDS_HEADER_FLAGS_VOLUME is set in flags + uint32_t mipMapCount; + uint32_t reserved1[11]; + DDS_PIXELFORMAT ddspf; + uint32_t caps; + uint32_t caps2; + uint32_t caps3; + uint32_t caps4; + uint32_t reserved2; +}; + +struct DDS_HEADER_DXT10 +{ + DXGI_FORMAT dxgiFormat; + uint32_t resourceDimension; + uint32_t miscFlag; // see D3D11_RESOURCE_MISC_FLAG + uint32_t arraySize; + uint32_t miscFlags2; // see DDS_MISC_FLAGS2 +} ; + +#pragma pack(pop) + +static_assert( sizeof(DDS_HEADER) == 124, "DDS Header size mismatch" ); +static_assert( sizeof(DDS_HEADER_DXT10) == 20, "DDS DX10 Extended Header size mismatch"); + +}; // namespace diff --git a/WickedEngine/Utility/pch.cpp b/WickedEngine/Utility/pch.cpp new file mode 100644 index 000000000..a27f00df5 --- /dev/null +++ b/WickedEngine/Utility/pch.cpp @@ -0,0 +1,14 @@ +//-------------------------------------------------------------------------------------- +// File: pch.cpp +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#include "pch.h" diff --git a/WickedEngine/Utility/pch.h b/WickedEngine/Utility/pch.h new file mode 100644 index 000000000..8a197c8c6 --- /dev/null +++ b/WickedEngine/Utility/pch.h @@ -0,0 +1,63 @@ +//-------------------------------------------------------------------------------------- +// File: pch.h +// +// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF +// ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO +// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A +// PARTICULAR PURPOSE. +// +// Copyright (c) Microsoft Corporation. All rights reserved. +// +// http://go.microsoft.com/fwlink/?LinkId=248929 +//-------------------------------------------------------------------------------------- + +#pragma once + +#if !defined(WIN32_LEAN_AND_MEAN) +#define WIN32_LEAN_AND_MEAN +#endif + +#if !defined(NOMINMAX) +#define NOMINMAX +#endif + +#if defined(_XBOX_ONE) && defined(_TITLE) && MONOLITHIC +#include +#define DCOMMON_H_INCLUDED +#define NO_D3D11_DEBUG_NAME +#else +#include +#endif + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#pragma warning(push) +#pragma warning(disable : 4005) +#include +#include +#pragma warning(pop) + +#include + +#if (_WIN32_WINNT >= _WIN32_WINNT_WIN8) && !defined(DXGI_1_2_FORMATS) +#define DXGI_1_2_FORMATS +#endif + +namespace DirectX +{ + #if (DIRECTXMATH_VERSION < 305) && !defined(XM_CALLCONV) + #define XM_CALLCONV __fastcall + typedef const XMVECTOR& HXMVECTOR; + typedef const XMMATRIX& FXMMATRIX; + #endif +} \ No newline at end of file diff --git a/WickedEngine/VariableManager.cpp b/WickedEngine/VariableManager.cpp new file mode 100644 index 000000000..78bb02ec2 --- /dev/null +++ b/WickedEngine/VariableManager.cpp @@ -0,0 +1,34 @@ +#include "VariableManager.h" + +VariableManager::container VariableManager::variables; + +void VariableManager::SetUp() +{ +} + +const VariableManager::Variable* VariableManager::get(const string& name) +{ + container::iterator it = variables.find(name); + if(it!=variables.end()) + return it->second; + else return nullptr; +} +bool VariableManager::add(const string& name, const string& value, Data_Type newType) +{ + if(get(name)) + return false; + variables.insert< pair >( pair(name,new Variable(value,newType)) ); +} +bool VariableManager::del(const string& name) +{ + Variable* var=nullptr; + if(!var) + return false; + delete(var); + variables.erase(name); + return true; +} +bool VariableManager::CleanUp() +{ + return true; +} diff --git a/WickedEngine/VariableManager.h b/WickedEngine/VariableManager.h new file mode 100644 index 000000000..9545b4f1a --- /dev/null +++ b/WickedEngine/VariableManager.h @@ -0,0 +1,27 @@ +#pragma once +#include "WickedEngine.h" + +class VariableManager +{ +public: + enum Data_Type{ + TEXT, + NUMBER, + }; +private: + struct Variable{ + string data; + Data_Type type; + Variable(const string d, Data_Type t):data(d),type(t){} + }; + typedef unordered_map container; + static container variables; +public: + static void SetUp(); + + static const Variable* get(const string& name); + static bool add(const string& name, const string& value, Data_Type newType = Data_Type::TEXT); + static bool del(const string& name); + static bool CleanUp(); +}; + diff --git a/WickedEngine/WickedEngine.h b/WickedEngine/WickedEngine.h new file mode 100644 index 000000000..6535c052f --- /dev/null +++ b/WickedEngine/WickedEngine.h @@ -0,0 +1,95 @@ +#pragma once + +#include "targetver.h" +#include "resource.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 + +using namespace DirectX; +using namespace std; + + +#include "Utility/WicTextureLoader.h" +#include "Utility/DDSTextureLoader.h" +#include "Utility/ScreenGrab.h" +#include "RenderTarget.h" +#include "BackLog.h" +#include "Camera.h" +#include "Frustum.h" +#include "ImageEffects.h" +#include "Image.h" +#include "Font.h" +#include "FrameRate.h" +#include "CpuInfo.h" +#include "WickedLoader.h" +#include "Particle.h" +#include "Renderer.h" +#include "DirectInput.h" +#include "XInput.h" +#include "TaskThread.h" +#include "mysql/mysql.h" +#include "WickedMath.h" +#include "LensFlare.h" +#include "Sound.h" +#include "ResourceManager.h" +#include "Timer.h" +#include "WickedHelper.h" +#include "InputManager.h" + +////#include "ViewProp.h" + + + +#define XBOUNDS 10 +#define CAMERA_POSITIONY 4.0f +#define DEFAULT_CAMERADISTANCE -9.5f +#define MODIFIED_CAMERADISTANCE -11.5f +#define CAMERA_DISTANCE_MODIFIER 10 +#define STARTPOSITION 4.5 + + + + +enum POVENUM{ //NUMPAD SORTING + D_UNUSED, + D_DOWNLEFT, + D_DOWN, + D_RIGHTDOWN, + D_LEFT, + D_IDLE, //5 + D_RIGHT, + D_LEFTUP, + D_UP, + D_UPRIGHT, +}; + + +#define SETTINGSFILE "settings/ProgramSettings.ini" + diff --git a/WickedEngine/WickedEngine.vcxproj b/WickedEngine/WickedEngine.vcxproj new file mode 100644 index 000000000..94e214834 --- /dev/null +++ b/WickedEngine/WickedEngine.vcxproj @@ -0,0 +1,541 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + + {A94A5B28-B5B0-4386-8281-8AE3C6984762} + Win32Proj + WickedEngine + + + + StaticLibrary + true + v120 + Unicode + + + StaticLibrary + false + v120 + true + Unicode + + + + + + + + + + + + + + + + + Level3 + Disabled + WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) + BULLET;%(AdditionalIncludeDirectories) + + + Windows + true + + + + + + + + + Level3 + + + MaxSpeed + true + true + WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + BULLET;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/WickedEngine/WickedEngine.vcxproj.filters b/WickedEngine/WickedEngine.vcxproj.filters new file mode 100644 index 000000000..6da7279e2 --- /dev/null +++ b/WickedEngine/WickedEngine.vcxproj.filters @@ -0,0 +1,1378 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + {df0b43fb-0d9d-434f-a856-fe922f415be9} + + + {4e6f526c-1afd-4811-8f31-aefc151cbf05} + + + {e49e67b6-6f4a-4bd3-82ba-2f3538a1ec2d} + + + {d1a49c6e-b933-41a3-b9c1-7cdfba7a0682} + + + {f6c2535c-8feb-4054-bac2-ff88dce3ccfa} + + + {a12a2d79-a95b-4785-a405-45974671efac} + + + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Utility + + + Utility + + + Utility + + + Utility + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Utility + + + Utility + + + Utility + + + Utility + + + Utility + + + Utility + + + Header Files + + + Header Files + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletCollision + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\BulletSoftBody + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET\LinearMath + + + BULLET + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET\BulletDynamics + + + BULLET + + + BULLET + + + + + + + + + + \ No newline at end of file diff --git a/WickedEngine/WickedHelper.cpp b/WickedEngine/WickedHelper.cpp new file mode 100644 index 000000000..ba378e10c --- /dev/null +++ b/WickedEngine/WickedHelper.cpp @@ -0,0 +1,12 @@ +#include "WickedHelper.h" + +std::string WickedHelper::toUpper(const std::string& s) +{ + std::string result; + std::locale loc; + for (unsigned int i = 0; i < s.length(); ++i) + { + result += std::toupper(s.at(i), loc); + } + return result; +} \ No newline at end of file diff --git a/WickedEngine/WickedHelper.h b/WickedEngine/WickedHelper.h new file mode 100644 index 000000000..4900edec9 --- /dev/null +++ b/WickedEngine/WickedHelper.h @@ -0,0 +1,15 @@ +#ifndef WHELPER +#define WHELPER + +#include "WickedEngine.h" +#include + +static class WickedHelper{ +public: + static string toUpper(const string& s); + + + +}; + +#endif diff --git a/WickedEngine/WickedLoader.cpp b/WickedEngine/WickedLoader.cpp new file mode 100644 index 000000000..6f8cc7e3a --- /dev/null +++ b/WickedEngine/WickedLoader.cpp @@ -0,0 +1,2292 @@ +#include "WickedLoader.h" + + +void Mesh::LoadFromFile(const string& newName, const string& fname + , const MaterialCollection& materialColl, vector armatures, const string& identifier){ + name=newName; + + ifstream file(fname.c_str(),ios::binary|ios::ate); + if(file.is_open()){ + + int fileSize = file.tellg(); + file.seekg (0, file.beg); + char * buffer = new char [fileSize]; + file.read (buffer,fileSize); + file.close(); + + int offset=0; + + int VERSION; + memcpy(&VERSION,buffer,sizeof(int)); + offset+=sizeof(int); + + if(VERSION>=1001){ + int doubleside; + memcpy(&doubleside,buffer+offset,sizeof(int)); + offset+=sizeof(int); + if(doubleside){ + doubleSided=true; + } + } + + int billboard; + memcpy(&billboard,buffer+offset,sizeof(int)); + offset+=sizeof(int); + if(billboard){ + char axis; + memcpy(&axis,buffer+offset,1); + offset+=1; + + if(toupper(axis)=='Y') + billboardAxis=XMFLOAT3(0,1,0); + else if(toupper(axis)=='X') + billboardAxis=XMFLOAT3(1,0,0); + else if(toupper(axis)=='Z') + billboardAxis=XMFLOAT3(0,0,1); + else + billboardAxis=XMFLOAT3(0,0,0); + isBillboarded=true; + } + + int parented; //parentnamelength + memcpy(&parented,buffer+offset,sizeof(int)); + offset+=sizeof(int); + if(parented){ + char* pName = new char[parented+1](); + memcpy(pName,buffer+offset,parented); + offset+=parented; + parent=pName; + delete[] pName; + + stringstream identified_parent(""); + identified_parent<name.compare(identified_parent.str())){ + armature=a; + } + } + } + + int materialCount; + memcpy(&materialCount,buffer+offset,sizeof(int)); + offset+=sizeof(int); + for(int i=0;isecond); + } + + materialNames.push_back(identified_matname.str()); + delete[] matName; + } + int rendermesh,vertexCount; + memcpy(&rendermesh,buffer+offset,sizeof(int)); + offset+=sizeof(int); + memcpy(&vertexCount,buffer+offset,sizeof(int)); + offset+=sizeof(int); + + vertices.reserve(vertexCount); + for(int i=0;iboneCollection.size()){ + if(!armature->boneCollection[b]->name.compare(nameB)){ + gotBone=true; + BONEINDEX=b; //GOT INDEX OF BONE OF THE WEIGHT IN THE PARENT ARMATURE + } + b++; + } + if(gotBone){ //ONLY PROCEED IF CORRESPONDING BONE WAS FOUND + if(!vert.wei.x) { + vert.wei.x=weightValue; + vert.bon.x=BONEINDEX; + } + else if(!vert.wei.y) { + vert.wei.y=weightValue; + vert.bon.y=BONEINDEX; + } + else if(!vert.wei.z) { + vert.wei.z=weightValue; + vert.bon.z=BONEINDEX; + } + else if(!vert.wei.w) { + vert.wei.w=weightValue; + vert.bon.w=BONEINDEX; + } + } + } + + //(+RIBBONTRAIL SETUP)(+VERTEXGROUP SETUP) + + if(nameB.find("trailbase")!=string::npos) + trailInfo.base = vertices.size(); + else if(nameB.find("trailtip")!=string::npos) + trailInfo.tip = vertices.size(); + + bool windAffection=false; + if(nameB.find("wind")!=string::npos) + windAffection=true; + bool gotvg=false; + for(int v=0;vCreateBuffer( &bd, 0, &meshInstanceBuffer ); + + //bool armatureDeformedMesh=false; + //for(int u : usedBy){ + // if(objects[u]->armatureDeform) + // armatureDeformedMesh=true; + //} + //if(!armatureDeformedMesh) + // armature=nullptr; + + + if(goalVG>=0){ + goalPositions.resize(vertexGroups[goalVG].vertices.size()); + goalNormals.resize(vertexGroups[goalVG].vertices.size()); + } + skinnedVertices.resize(vertices.size()); + for(int i=0;iCreateBuffer( &bd, &InitData, &meshVertBuff ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_IMMUTABLE; + bd.ByteWidth = sizeof( unsigned int ) * indices.size(); + bd.BindFlags = D3D11_BIND_INDEX_BUFFER; + bd.CPUAccessFlags = 0; + ZeroMemory( &InitData, sizeof(InitData) ); + InitData.pSysMem = indices.data(); + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &meshIndexBuff ); + + if(renderable) + { + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(BoneShaderBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &boneBuffer ); + + if(hasArmature() && !softBody){ + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DEFAULT; + bd.ByteWidth = sizeof(Vertex) * vertices.size(); + bd.BindFlags = D3D11_BIND_STREAM_OUTPUT | D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = 0; + bd.StructureByteStride=0; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &sOutBuffer ); + } + + //PHYSICALMAPPING + for(int i=0;i(meshInstanceBuffer,usedBy.size()*2); +} + +void LoadWiArmatures(const string& directory, const string& name, const string& identifier, vector& armatures + , map& transforms) +{ + stringstream filename(""); + filename<>line; + if(line[0]=='/' && line.substr(2,8)=="ARMATURE") { + armatures.push_back(new Armature(line.substr(11,strlen(line.c_str())-11),identifier) ); + } + else{ + switch(line[0]){ + case 'r': + file>>trans[0]>>trans[1]>>trans[2]>>trans[3]; + armatures.back()->rotation_rest = XMFLOAT4(trans[0],trans[1],trans[2],trans[3]); + break; + case 's': + file>>trans[0]>>trans[1]>>trans[2]; + armatures.back()->scale_rest = XMFLOAT3(trans[0], trans[1], trans[2]); + break; + case 't': + file>>trans[0]>>trans[1]>>trans[2]; + armatures.back()->translation_rest = XMFLOAT3(trans[0], trans[1], trans[2]); + { + XMMATRIX world = XMMatrixScalingFromVector(XMLoadFloat3(&armatures.back()->scale))*XMMatrixRotationQuaternion(XMLoadFloat4(&armatures.back()->rotation))*XMMatrixTranslationFromVector(XMLoadFloat3(&armatures.back()->translation)); + XMStoreFloat4x4( &armatures.back()->world_rest,world ); + } + break; + case 'b': + { + string boneName; + file>>boneName; + armatures.back()->boneCollection.push_back(new Bone(boneName)); + //stringstream ss(""); + //ss<name<<"_"<boneCollection.push_back(new Bone(ss.str())); + //transforms.insert(pair(armatures.back()->boneCollection.back()->name,armatures.back()->boneCollection.back())); + } + break; + case 'p': + file>>armatures.back()->boneCollection.back()->parentName; + break; + case 'l': + { + float x=0,y=0,z=0,w=0; + file>>x>>y>>z>>w; + XMVECTOR quaternion = XMVectorSet(x,y,z,w); + file>>x>>y>>z; + XMVECTOR translation = XMVectorSet(x,y,z,0); + + XMMATRIX frame; + frame= XMMatrixRotationQuaternion(quaternion) * XMMatrixTranslationFromVector(translation) ; + + XMStoreFloat3(&armatures.back()->boneCollection.back()->translation_rest,translation); + XMStoreFloat4(&armatures.back()->boneCollection.back()->rotation_rest,quaternion); + XMStoreFloat4x4(&armatures.back()->boneCollection.back()->world_rest,frame); + XMStoreFloat4x4(&armatures.back()->boneCollection.back()->restInv,XMMatrixInverse(0,frame)); + + /*XMStoreFloat3( &armatures.back()->boneCollection.back().position,translationInverse ); + XMStoreFloat4( &armatures.back()->boneCollection.back().rotation,quaternionInverse );*/ + + /*XMVECTOR sca,rot,tra; + XMMatrixDecompose(&sca,&rot,&tra,XMMatrixInverse(0,XMMatrixTranspose(frame))*XMLoadFloat4x4(&armatures.back()->world)); + XMStoreFloat3( &armatures.back()->boneCollection.back().position,tra ); + XMStoreFloat4( &armatures.back()->boneCollection.back().rotation,rot );*/ + + } + break; + case 'c': + armatures.back()->boneCollection.back()->connected=true; + break; + case 'h': + file>>armatures.back()->boneCollection.back()->length; + break; + default: break; + } + } + } + } + file.close(); + + + //CREATE FAMILY + for(Armature* armature : armatures){ + //for(int i=0;iboneCollection.size();i++){ + // string parent = armature->boneCollection[i]->parentName; + // if(parent.length()>0){ + // for(int j=0;jboneCollection.size();j++) + // if(!armature->boneCollection[j].name.compare(parent)){ + // armature->boneCollection[i].parentI=j; + // armature->boneCollection[j].children.push_back(armature->boneCollection[i].name); + // armature->boneCollection[j].childrenI.push_back(i); + // } + // } + // else{ + // armature->rootbones.push_back(i); + // } + //} + + for(Bone* i : armature->boneCollection){ + if(i->parentName.length()>0){ + for(Bone* j : armature->boneCollection){ + if(i!=j){ + if(!i->parentName.compare(j->name)){ + i->parent=j; + j->childrenN.push_back(i->name); + j->childrenI.push_back(i); + i->attachTo(j,1,1,1); + } + } + } + } + else{ + armature->rootbones.push_back(i); + } + } + + for(int i=0;irootbones.size();++i){ + RecursiveRest(armature,armature->rootbones[i]); + } + + transforms.insert(pair(armature->name,armature)); + } + +} +void RecursiveRest(Armature* armature, Bone* bone){ + Bone* parent = (Bone*)bone->parent; + + if(parent!=nullptr){ + XMMATRIX recRest = + XMLoadFloat4x4(&bone->world_rest) + * + XMLoadFloat4x4(&parent->recursiveRest) + //* + //XMLoadFloat4x4(&armature->boneCollection[boneI].rest) + ; + XMStoreFloat4x4( &bone->recursiveRest, recRest ); + XMStoreFloat4x4( &bone->recursiveRestInv, XMMatrixInverse(0,recRest) ); + } + else{ + bone->recursiveRest = bone->world_rest ; + XMStoreFloat4x4( &bone->recursiveRestInv, XMMatrixInverse(0,XMLoadFloat4x4(&bone->recursiveRest)) ); + } + + for(int i=0;ichildrenI.size();++i){ + RecursiveRest(armature,bone->childrenI[i]); + } +} +void LoadWiMaterialLibrary(const string& directory, const string& name, const string& identifier, const string& texturesDir,MaterialCollection& materials) +{ + int materialI=materials.size()-1; + + Material* currentMat = NULL; + + stringstream filename(""); + filename<>line; + if(line[0]=='/' && !strcmp(line.substr(2,8).c_str(),"MATERIAL")) { + if(currentMat) + materials.insert(pair(currentMat->name,currentMat)); + + stringstream identified_name(""); + identified_name<>currentMat->diffuseColor.x; + file>>currentMat->diffuseColor.y; + file>>currentMat->diffuseColor.z; + break; + case 'X': + currentMat->cast_shadow=false; + break; + case 'r': + { + string resourceName=""; + file>>resourceName; + stringstream ss(""); + ss<refMapName=ss.str(); + currentMat->refMap=(ID3D11ShaderResourceView*)ResourceManager::add(ss.str()); + } + if(currentMat->refMap!=0) + currentMat->hasRefMap = true; + break; + case 'n': + { + string resourceName=""; + file>>resourceName; + stringstream ss(""); + ss<normalMapName=ss.str(); + currentMat->normalMap=(ID3D11ShaderResourceView*)ResourceManager::add(ss.str()); + } + if(currentMat->normalMap!=0) + currentMat->hasNormalMap = true; + break; + case 't': + { + string resourceName=""; + file>>resourceName; + stringstream ss(""); + ss<textureName=ss.str(); + currentMat->texture=(ID3D11ShaderResourceView*)ResourceManager::add(ss.str()); + } + if(currentMat->texture!=0) + currentMat->hasTexture=true; + file>>currentMat->premultipliedTexture; + break; + case 'D': + { + string resourceName=""; + file>>resourceName; + stringstream ss(""); + ss<displacementMapName=ss.str(); + currentMat->displacementMap=(ID3D11ShaderResourceView*)ResourceManager::add(ss.str()); + } + if(currentMat->displacementMap!=0) + currentMat->hasDisplacementMap=true; + break; + case 'S': + { + string resourceName=""; + file>>resourceName; + stringstream ss(""); + ss<specularMapName=ss.str(); + currentMat->specularMap=(ID3D11ShaderResourceView*)ResourceManager::add(ss.str()); + } + if(currentMat->specularMap!=0) + currentMat->hasSpecularMap=true; + break; + case 'a': + currentMat->transparent=true; + file>>currentMat->alpha; + break; + case 'h': + currentMat->shadeless=true; + break; + case 'R': + file>>currentMat->refraction_index; + break; + case 'e': + file>>currentMat->enviroReflection; + break; + case 's': + file>>currentMat->specular.x; + file>>currentMat->specular.y; + file>>currentMat->specular.z; + file>>currentMat->specular.w; + break; + case 'p': + file>>currentMat->specular_power; + break; + case 'k': + currentMat->isSky=true; + break; + case 'm': + file>>currentMat->movingTex.x; + file>>currentMat->movingTex.y; + file>>currentMat->movingTex.z; + currentMat->framesToWaitForTexCoordOffset=currentMat->movingTex.z; + break; + case 'w': + currentMat->water=true; + break; + case 'u': + currentMat->subsurface_scattering=true; + break; + case 'b': + { + string blend; + file>>blend; + if(!blend.compare("ADD")) + currentMat->blendFlag=BLENDMODE_ADDITIVE; + } + break; + case 'i': + { + file>>currentMat->emit; + } + break; + default:break; + } + } + } + } + file.close(); + + if(currentMat) + materials.insert(pair(currentMat->name,currentMat)); + +} +void LoadWiObjects(const string& directory, const string& name, const string& identifier, vector& objects_norm + , vector& objects_trans, vector& objects_water, vector& armatures + , MeshCollection& meshes, map& transforms, const MaterialCollection& materials) +{ + vector objects(0); + int objectI=objects.size()-1; + + stringstream filename(""); + filename<>line; + if(line[0]=='/' && !strcmp(line.substr(2,6).c_str(),"OBJECT")) { + stringstream identified_name(""); + identified_name<(objects.back()->name,objects.back())); + } + else{ + switch(line[0]){ + case 'm': + { + string meshName=""; + file>>meshName; + stringstream identified_mesh(""); + identified_mesh<LoadFromFile(identified_mesh.str(),meshFileName.str(),materials,armatures,identifier); + objects.back()->mesh=mesh; + meshes.insert(pair(identified_mesh.str(),mesh)); + } + else{ + objects.back()->mesh=iter->second; + objects.back()->mesh->AddInstance(1); + } + } + else{ + if(iter!=meshes.end()) { + objects.back()->mesh=iter->second; + objects.back()->mesh->usedBy.push_back(objects.size()-1); + } + } + } + break; + case 'p': + { + string parentName=""; + file>>parentName; + stringstream identified_parentName(""); + identified_parentName<name.compare(identified_parentName.str())){ + objects.back()->parentName=identified_parentName.str(); + objects.back()->parent=a; + objects.back()->attachTo(a,1,1,1); + objects.back()->armatureDeform=true; + } + } + } + break; + case 'b': + { + string bone=""; + file>>bone; + if(objects.back()->parent!=nullptr){ + for(Bone* b : ((Armature*)objects.back()->parent)->boneCollection){ + if(!bone.compare(b->name)){ + objects.back()->parent=b; + objects.back()->armatureDeform=false; + break; + } + } + } + } + break; + case 'I': + { + XMFLOAT3 s,t; + XMFLOAT4 r; + file>>t.x>>t.y>>t.z>>r.x>>r.y>>r.z>>r.w>>s.x>>s.y>>s.z; + XMStoreFloat4x4(&objects.back()->parent_inv_rest + , XMMatrixScalingFromVector(XMLoadFloat3(&s)) * + XMMatrixRotationQuaternion(XMLoadFloat4(&r)) * + XMMatrixTranslationFromVector(XMLoadFloat3(&t)) + ); + } + break; + case 'r': + file>>trans[0]>>trans[1]>>trans[2]>>trans[3]; + objects.back()->rotation_rest = XMFLOAT4(trans[0],trans[1],trans[2],trans[3]); + break; + case 's': + file>>trans[0]>>trans[1]>>trans[2]; + objects.back()->scale_rest = XMFLOAT3(trans[0], trans[1], trans[2]); + break; + case 't': + file>>trans[0]>>trans[1]>>trans[2]; + objects.back()->translation_rest = XMFLOAT3(trans[0], trans[1], trans[2]); + XMStoreFloat4x4( &objects.back()->world_rest, XMMatrixScalingFromVector(XMLoadFloat3(&objects.back()->scale_rest)) + *XMMatrixRotationQuaternion(XMLoadFloat4(&objects.back()->rotation_rest)) + *XMMatrixTranslationFromVector(XMLoadFloat3(&objects.back()->translation_rest)) + ); + objects.back()->world=objects.back()->world_rest; + break; + case 'E': + { + string systemName,materialName; + bool visibleEmitter; + float size,randfac,norfac; + float count,life,randlife; + float scaleX,scaleY,rot; + file>>systemName>>visibleEmitter>>materialName>>size>>randfac>>norfac>>count>>life>>randlife; + file>>scaleX>>scaleY>>rot; + + if(visibleEmitter) objects.back()->particleEmitter=Object::EMITTER_VISIBLE; + else if(objects.back()->particleEmitter==Object::NO_EMITTER) objects.back()->particleEmitter=Object::EMITTER_INVISIBLE; + + if(Renderer::EMITTERSENABLED){ + stringstream identified_materialName(""); + identified_materialName<mesh){ + objects.back()->eParticleSystems.push_back( + new EmittedParticle(identified_systemName.str(),identified_materialName.str(),objects.back(),size,randfac,norfac,count,life,randlife,scaleX,scaleY,rot) + ); + } + } + } + break; + case 'H': + { + string name,mat,densityG,lenG; + float len; + int count; + file>>name>>mat>>len>>count>>densityG>>lenG; + + if(Renderer::HAIRPARTICLEENABLED){ + stringstream identified_materialName(""); + identified_materialName<hParticleSystems.push_back(new HairParticle(name,len,count,identified_materialName.str(),objects.back(),densityG,lenG) ); + } + } + break; + case 'P': + objects.back()->rigidBody = true; + file>>objects.back()->collisionShape>>objects.back()->mass>> + objects.back()->friction>>objects.back()->restitution>>objects.back()->damping>>objects.back()->physicsType>> + objects.back()->kinematic; + break; + default: break; + } + } + } + } + file.close(); + + for(int i=0;imesh){ + if(objects[i]->mesh->trailInfo.base>=0 && objects[i]->mesh->trailInfo.tip>=0){ + //objects[i]->trail.resize(MAX_RIBBONTRAILS); + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof( RibbonVertex ) * MAX_RIBBONTRAILS; + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &objects[i]->trailBuff ); + } + + /*string parent = objects[i]->parent; + bool childofArmature=true; + if(parent.length()){ + for(int j=0;jname == parent){ + objects[j]->children.push_back(objects[i]->name); + objects[j]->childrenI.push_back(i); + childofArmature=false; + } + }*/ + + bool default_mesh = false; + bool water_mesh = false; + bool transparent_mesh = false; + + //if(objects[i]->mesh->renderable) + for(Material* mat : objects[i]->mesh->materials){ + if(!mat->water && !mat->isSky && !mat->transparent) + default_mesh=true; + if(mat->water && !mat->isSky) + water_mesh=true; + if(!mat->water && !mat->isSky && mat->transparent) + transparent_mesh=true; + } + + if(default_mesh) + objects_norm.push_back(objects[i]); + if(water_mesh) + objects_water.push_back(objects[i]); + if(transparent_mesh) + objects_trans.push_back(objects[i]); + + if(!default_mesh && !water_mesh && !transparent_mesh) + objects_norm.push_back(objects[i]); + } + } + + for(MeshCollection::iterator iter=meshes.begin(); iter!=meshes.end(); ++iter){ + Mesh* iMesh = iter->second; + + if(iMesh->buffersComplete) + continue; + + for(int i=0;i<5;++i){ + iMesh->instances[i].clear(); + iMesh->instances[i].resize(iMesh->usedBy.size()); + } + + if(iMesh->meshInstanceBuffer!=nullptr) + continue; + + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof( Instance )*iMesh->usedBy.size(); + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, 0, &iMesh->meshInstanceBuffer ); + + bool armatureDeformedMesh=false; + for(int u : iMesh->usedBy){ + if(objects[u]->armatureDeform) + armatureDeformedMesh=true; + } + if(!armatureDeformedMesh) + iMesh->armature=nullptr; + + + if(iMesh->goalVG>=0){ + iMesh->goalPositions.resize(iMesh->vertexGroups[iMesh->goalVG].vertices.size()); + iMesh->goalNormals.resize(iMesh->vertexGroups[iMesh->goalVG].vertices.size()); + } + iMesh->skinnedVertices.resize(iMesh->vertices.size()); + for(int i=0;ivertices.size();++i){ + iMesh->skinnedVertices[i].pos=iMesh->vertices[i].pos; + iMesh->skinnedVertices[i].nor=iMesh->vertices[i].nor; + iMesh->skinnedVertices[i].tex=iMesh->vertices[i].tex; + } + + + ZeroMemory( &bd, sizeof(bd) ); + #ifdef USE_GPU_SKINNING + bd.Usage = (iMesh->softBody?D3D11_USAGE_DYNAMIC:D3D11_USAGE_IMMUTABLE); + bd.CPUAccessFlags = (iMesh->softBody?D3D11_CPU_ACCESS_WRITE:0); + if(iMesh->hasArmature() && !iMesh->softBody) + bd.ByteWidth = sizeof( SkinnedVertex ) * iMesh->vertices.size(); + else + bd.ByteWidth = sizeof( Vertex ) * iMesh->vertices.size(); + #else + bd.Usage = ((iMesh->softBody || iMesh->hasArmature())?D3D11_USAGE_DYNAMIC:D3D11_USAGE_IMMUTABLE); + bd.CPUAccessFlags = ((iMesh->softBody || iMesh->hasArmature())?D3D11_CPU_ACCESS_WRITE:0); + bd.ByteWidth = sizeof( Vertex ) * iMesh->vertices.size(); + #endif + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + D3D11_SUBRESOURCE_DATA InitData; + ZeroMemory( &InitData, sizeof(InitData) ); + if(iMesh->hasArmature() && !iMesh->softBody) + InitData.pSysMem = iMesh->vertices.data(); + else + InitData.pSysMem = iMesh->skinnedVertices.data(); + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &iMesh->meshVertBuff ); + + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_IMMUTABLE; + bd.ByteWidth = sizeof( unsigned int ) * iMesh->indices.size(); + bd.BindFlags = D3D11_BIND_INDEX_BUFFER; + bd.CPUAccessFlags = 0; + ZeroMemory( &InitData, sizeof(InitData) ); + InitData.pSysMem = iMesh->indices.data(); + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &iMesh->meshIndexBuff ); + + if(iMesh->renderable) + { + + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DYNAMIC; + bd.ByteWidth = sizeof(BoneShaderBuffer); + bd.BindFlags = D3D11_BIND_CONSTANT_BUFFER; + bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &iMesh->boneBuffer ); + + if(iMesh->hasArmature() && !iMesh->softBody){ + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_DEFAULT; + bd.ByteWidth = sizeof(Vertex) * iMesh->vertices.size(); + bd.BindFlags = D3D11_BIND_STREAM_OUTPUT | D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = 0; + bd.StructureByteStride=0; + Renderer::graphicsDevice->CreateBuffer( &bd, NULL, &iMesh->sOutBuffer ); + } + + //PHYSICALMAPPING + for(int i=0;ivertices.size();++i){ + for(int j=0;jphysicsverts.size();++j){ + if( fabs( iMesh->vertices[i].pos.x-iMesh->physicsverts[j].x ) < DBL_EPSILON + && fabs( iMesh->vertices[i].pos.y-iMesh->physicsverts[j].y ) < DBL_EPSILON + && fabs( iMesh->vertices[i].pos.z-iMesh->physicsverts[j].z ) < DBL_EPSILON + ) + { + iMesh->physicalmapGP.push_back(j); + break; + } + } + } + + } + } + objects.clear(); +} +void LoadWiMeshes(const string& directory, const string& name, const string& identifier, MeshCollection& meshes, const vector& armatures, const MaterialCollection& materials) +{ + int meshI=meshes.size()-1; + Mesh* currentMesh = NULL; + + stringstream filename(""); + filename<>line; + if(line[0]=='/' && !line.substr(2,4).compare("MESH")) { + stringstream identified_name(""); + identified_name<(currentMesh->name,currentMesh) ); + meshI++; + } + else{ + switch(line[0]){ + case 'p': + { + string parentArmature=""; + file>>parentArmature; + + stringstream identified_parentArmature(""); + identified_parentArmature<parent=identified_parentArmature.str(); + for(int i=0;iname.c_str(),currentMesh->parent.c_str())){ + currentMesh->armatureIndex=i; + currentMesh->armature=armatures[i]; + } + } + break; + case 'v': + currentMesh->vertices.push_back(SkinnedVertex()); + file >> currentMesh->vertices.back().pos.x; + file >> currentMesh->vertices.back().pos.y; + file >> currentMesh->vertices.back().pos.z; + break; + case 'n': + if(currentMesh->isBillboarded){ + currentMesh->vertices.back().nor=currentMesh->billboardAxis; + } + else{ + file >> currentMesh->vertices.back().nor.x; + file >> currentMesh->vertices.back().nor.y; + file >> currentMesh->vertices.back().nor.z; + } + break; + case 'u': + file >> currentMesh->vertices.back().tex.x; + file >> currentMesh->vertices.back().tex.y; + //texCoordFill++; + break; + case 'w': + { + string nameB; + float weight=0; + int BONEINDEX=0; + file>>nameB>>weight; + bool gotArmature=false; + bool gotBone=false; + int i=0; + while(!gotArmature && iname.c_str(),currentMesh->parent.c_str())) + gotArmature=true; + else i++; + } + if(gotArmature){ + int j=0; + while(!gotBone && jboneCollection.size()){ + if(!armatures[i]->boneCollection[j]->name.compare(nameB)){ + gotBone=true; + BONEINDEX=j; //GOT INDEX OF BONE OF THE WEIGHT IN THE PARENT ARMATURE + } + j++; + } + } + if(gotBone){ //ONLY PROCEED IF CORRESPONDING BONE WAS FOUND + if(!currentMesh->vertices.back().wei.x) { + currentMesh->vertices.back().wei.x=weight; + currentMesh->vertices.back().bon.x=BONEINDEX; + } + else if(!currentMesh->vertices.back().wei.y) { + currentMesh->vertices.back().wei.y=weight; + currentMesh->vertices.back().bon.y=BONEINDEX; + } + else if(!currentMesh->vertices.back().wei.z) { + currentMesh->vertices.back().wei.z=weight; + currentMesh->vertices.back().bon.z=BONEINDEX; + } + else if(!currentMesh->vertices.back().wei.w) { + currentMesh->vertices.back().wei.w=weight; + currentMesh->vertices.back().bon.w=BONEINDEX; + } + } + + //(+RIBBONTRAIL SETUP)(+VERTEXGROUP SETUP) + + if(nameB.find("trailbase")!=string::npos) + currentMesh->trailInfo.base = currentMesh->vertices.size()-1; + else if(nameB.find("trailtip")!=string::npos) + currentMesh->trailInfo.tip = currentMesh->vertices.size()-1; + + bool windAffection=false; + if(nameB.find("wind")!=string::npos) + windAffection=true; + bool gotvg=false; + for(int v=0;vvertexGroups.size();++v) + if(!nameB.compare(currentMesh->vertexGroups[v].name)){ + gotvg=true; + currentMesh->vertexGroups[v].addVertex( VertexRef(currentMesh->vertices.size()-1,weight) ); + if(windAffection) + currentMesh->vertices.back().tex.w=weight; + } + if(!gotvg){ + currentMesh->vertexGroups.push_back(VertexGroup(nameB)); + currentMesh->vertexGroups.back().addVertex( VertexRef(currentMesh->vertices.size()-1,weight) ); + if(windAffection) + currentMesh->vertices.back().tex.w=weight; + } + } + break; + case 'i': + { + int count; + file>>count; + for(int i=0;i>index; + currentMesh->indices.push_back(index); + } + break; + } + case 'V': + { + XMFLOAT3 pos; + file >> pos.x>>pos.y>>pos.z; + currentMesh->physicsverts.push_back(pos); + } + break; + case 'I': + { + int count; + file>>count; + for(int i=0;i>index; + currentMesh->physicsindices.push_back(index); + } + break; + } + case 'm': + { + string mName=""; + file>>mName; + stringstream identified_material(""); + identified_material<materialNames.push_back(identified_material.str()); + MaterialCollection::const_iterator iter = materials.find(identified_material.str()); + if(iter!=materials.end()) { + currentMesh->renderable=true; + currentMesh->materials.push_back(iter->second); + currentMesh->materialIndices.push_back(currentMesh->materials.size()); //CONNECT meshes WITH MATERIALS + } + } + break; + case 'a': + file>>currentMesh->vertices.back().tex.z; + break; + case 'B': + for(int corner=0;corner<8;++corner){ + file>>currentMesh->aabb.corners[corner].x; + file>>currentMesh->aabb.corners[corner].y; + file>>currentMesh->aabb.corners[corner].z; + } + break; + case 'b': + { + currentMesh->isBillboarded=true; + string read = ""; + file>>read; + transform(read.begin(), read.end(), read.begin(), toupper); + if(read.find(toupper('y'))!=string::npos) currentMesh->billboardAxis=XMFLOAT3(0,1,0); + else if(read.find(toupper('x'))!=string::npos) currentMesh->billboardAxis=XMFLOAT3(1,0,0); + else if(read.find(toupper('z'))!=string::npos) currentMesh->billboardAxis=XMFLOAT3(0,0,1); + else currentMesh->billboardAxis=XMFLOAT3(0,0,0); + } + break; + case 'S': + { + currentMesh->softBody=true; + string mvgi="",gvgi="",svgi=""; + file>>currentMesh->mass>>currentMesh->friction>>gvgi>>mvgi>>svgi; + for(int v=0;vvertexGroups.size();++v){ + if(!strcmp(mvgi.c_str(),currentMesh->vertexGroups[v].name.c_str())) + currentMesh->massVG=v; + if(!strcmp(gvgi.c_str(),currentMesh->vertexGroups[v].name.c_str())) + currentMesh->goalVG=v; + if(!strcmp(svgi.c_str(),currentMesh->vertexGroups[v].name.c_str())) + currentMesh->softVG=v; + } + } + break; + default: break; + } + } + } + } + file.close(); + + if(currentMesh) + meshes.insert( pair(currentMesh->name,currentMesh) ); + +} +void LoadWiActions(const string& directory, const string& name, const string& identifier, vector& armatures) +{ + int armatureI=0; + int boneI=0; + int firstFrame=INT_MAX; + + stringstream filename(""); + filename<>line; + if(line[0]=='/' && !strcmp(line.substr(2,8).c_str(),"ARMATURE")) { + stringstream identified_name(""); + identified_name<name.compare(armaturename)){ + armatureI=i; + break; + } + } + else{ + switch(line[0]){ + case 'C': + armatures[armatureI]->actions.push_back(Action()); + file>>armatures[armatureI]->actions.back().name; + break; + case 'A': + file>>armatures[armatureI]->actions.back().frameCount; + break; + case 'b': + { + string boneName; + file>>boneName; + for(int i=0;iboneCollection.size();i++) + if(!armatures[armatureI]->boneCollection[i]->name.compare(boneName)){ + boneI=i; + break; + } //GOT BONE INDEX + armatures[armatureI]->boneCollection[boneI]->actionFrames.resize(armatures[armatureI]->actions.size()); + } + break; + case 'r': + { + float f=0,x=0,y=0,z=0,w=0; + file>>f>>x>>y>>z>>w; + armatures[armatureI]->boneCollection[boneI]->actionFrames.back().keyframesRot.push_back(KeyFrame(f,x,y,z,w)); + } + break; + case 't': + { + float f=0,x=0,y=0,z=0; + file>>f>>x>>y>>z; + armatures[armatureI]->boneCollection[boneI]->actionFrames.back().keyframesPos.push_back(KeyFrame(f,x,y,z,0)); + } + break; + case 's': + { + float f=0,x=0,y=0,z=0; + file>>f>>x>>y>>z; + armatures[armatureI]->boneCollection[boneI]->actionFrames.back().keyframesSca.push_back(KeyFrame(f,x,y,z,0)); + } + break; + default: break; + } + } + } + } + file.close(); +} +void LoadWiLights(const string& directory, const string& name, const string& identifier, vector& lights + , const vector& armatures + , map& transforms) +{ + + stringstream filename(""); + filename<>line; + switch(line[0]){ + case 'P': + { + lights.push_back(new Light()); + lights.back()->type=Light::POINT; + bool shadow; + string lname = ""; + file>>lname>>shadow; + stringstream identified_name(""); + identified_name<name=identified_name.str(); + if(shadow){ + lights.back()->shadowMap.resize(1); + lights.back()->shadowMap[0].InitializeCube(Renderer::POINTLIGHTSHADOWRES,0,true); + } + //lights.back()->mesh=lightGRenderer[Light::getTypeStr(Light::POINT)]; + + transforms.insert(pair(lights.back()->name,lights.back())); + } + break; + case 'D': + { + lights.push_back(new Light()); + lights.back()->type=Light::DIRECTIONAL; + file>>lights.back()->name; + lights.back()->shadowMap.resize(3); + for(int i=0;i<3;++i) + lights.back()->shadowMap[i].Initialize( + Renderer::SHADOWMAPRES,Renderer::SHADOWMAPRES + ,0,true + ); + //lightGRenderer[Light::getTypeStr(Light::DIRECTIONAL)]->usedBy.push_back(lights.size()-1); + //lights.back()->mesh=lightGRenderer[Light::getTypeStr(Light::DIRECTIONAL)]; + } + break; + case 'S': + { + lights.push_back(new Light()); + lights.back()->type=Light::SPOT; + file>>lights.back()->name; + bool shadow; + file>>shadow>>lights.back()->enerDis.z; + if(shadow){ + lights.back()->shadowMap.resize(1); + lights.back()->shadowMap[0].Initialize( + Renderer::SHADOWMAPRES,Renderer::SHADOWMAPRES + ,0,true + ); + } + //lightGRenderer[Light::getTypeStr(Light::SPOT)]->usedBy.push_back(lights.size()-1); + //lights.back()->mesh=lightGRenderer[Light::getTypeStr(Light::SPOT)]; + } + break; + case 'p': + { + string parentName=""; + file>>parentName; + + stringstream identified_parentName(""); + identified_parentName<parentName=identified_parentName.str(); + for(map::iterator it=transforms.begin();it!=transforms.end();++it){ + if(!it->second->name.compare(lights.back()->parentName)){ + lights.back()->parent=it->second; + lights.back()->attachTo(it->second,1,1,1); + break; + } + } + } + break; + case 'b': + { + string parentBone=""; + file>>parentBone; + + for(Bone* b : ((Armature*)lights.back()->parent)->boneCollection){ + if(!b->name.compare(parentBone)){ + lights.back()->parent=b; + lights.back()->attachTo(b,1,1,1); + } + } + } + break; + case 'I': + { + XMFLOAT3 s,t; + XMFLOAT4 r; + file>>t.x>>t.y>>t.z>>r.x>>r.y>>r.z>>r.w>>s.x>>s.y>>s.z; + XMStoreFloat4x4(&lights.back()->parent_inv_rest + , XMMatrixScalingFromVector(XMLoadFloat3(&s)) * + XMMatrixRotationQuaternion(XMLoadFloat4(&r)) * + XMMatrixTranslationFromVector(XMLoadFloat3(&t)) + ); + } + break; + case 't': + { + float x,y,z; + file>>x>>y>>z; + lights.back()->translation_rest=XMFLOAT3(x,y,z); + break; + } + case 'r': + { + float x,y,z,w; + file>>x>>y>>z>>w; + lights.back()->rotation_rest=XMFLOAT4(x,y,z,w); + break; + } + case 'c': + { + float r,g,b; + file>>r>>g>>b; + lights.back()->color=XMFLOAT4(r,g,b,0); + break; + } + case 'e': + file>>lights.back()->enerDis.x; + break; + case 'd': + file>>lights.back()->enerDis.y; + //lights.back()->enerDis.y *= XMVectorGetX( world.r[0] )*0.1f; + break; + case 'n': + lights.back()->noHalo=true; + break; + case 'l': + { + string t=""; + file>>t; + stringstream rim(""); + rim<lensFlareRimTextures.push_back(tex); + lights.back()->lensFlareNames.push_back(rim.str()); + } + } + break; + default: break; + } + } + + //for(MeshCollection::iterator iter=lightGRenderer.begin(); iter!=lightGRenderer.end(); ++iter){ + // Mesh* iMesh = iter->second; + // D3D11_BUFFER_DESC bd; + // ZeroMemory( &bd, sizeof(bd) ); + // bd.Usage = D3D11_USAGE_DYNAMIC; + // bd.ByteWidth = sizeof( Instance )*iMesh->usedBy.size(); + // bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + // bd.CPUAccessFlags = D3D11_CPU_ACCESS_WRITE; + // Renderer::graphicsDevice->CreateBuffer( &bd, 0, &iMesh->meshInstanceBuffer ); + //} + } + file.close(); +} +void LoadWiHitSpheres(const string& directory, const string& name, const string& identifier, vector& spheres + ,const vector& armatures, map& transforms) +{ + stringstream filename(""); + filename<>voidStr; + while(!file.eof()){ + string line=""; + file>>line; + switch(line[0]){ + case 'H': + { + string name; + float scale; + XMFLOAT3 loc; + string parentStr; + string prop; + file>>name>>scale>>loc.x>>loc.y>>loc.z>>parentStr>>prop; + + stringstream identified_parent(""),identified_name(""); + identified_parent<name)){ + // for(Bone* b:a->boneCollection){ + // if(!parentBone.compare(b->name)){ + // parentA=a; + // parent=b; + // } + // } + // } + //} + //spheres.push_back(new HitSphere(identified_name.str(),scale,loc,parentA,parent,prop)); + + Transform* parent = nullptr; + if(transforms.find(identified_parent.str())!=transforms.end()) + { + parent = transforms[identified_parent.str()]; + spheres.push_back(new HitSphere(identified_name.str(),scale,loc,parent,prop)); + spheres.back()->attachTo(parent,1,1,1); + transforms.insert(pair(spheres.back()->name,spheres.back())); + } + + } + break; + case 'I': + { + XMFLOAT3 s,t; + XMFLOAT4 r; + file>>t.x>>t.y>>t.z>>r.x>>r.y>>r.z>>r.w>>s.x>>s.y>>s.z; + XMStoreFloat4x4(&spheres.back()->parent_inv_rest + , XMMatrixScalingFromVector(XMLoadFloat3(&s)) * + XMMatrixRotationQuaternion(XMLoadFloat4(&r)) * + XMMatrixTranslationFromVector(XMLoadFloat3(&t)) + ); + } + break; + case 'b': + { + string parentBone = ""; + file>>parentBone; + Armature* parentA = (Armature*)spheres.back()->parent; + if(parentA!=nullptr){ + for(Bone* b:parentA->boneCollection){ + if(!parentBone.compare(b->name)){ + spheres.back()->attachTo(b,1,1,1); + } + } + } + } + break; + default: break; + }; + } + } + file.close(); + + + ////SET UP SPHERE INDEXERS + //for(int i=0;iname.compare(spheres[i]->pA)){ + // spheres[i]->parentArmature=armatures[j]; + // for(int k=0;kboneCollection.size();k++) + // if(!armatures[j]->boneCollection[k]->name.compare(spheres[i]->pB)){ + // spheres[i]->parentBone=k; + // break; + // } + // break; + // } + // } + //} +} +void LoadWiWorldInfo(const string&directory, const string& name, WorldInfo& worldInfo, Wind& wind){ + worldInfo=WorldInfo(); + stringstream filename(""); + filename<>read; + switch(read[0]){ + case 'h': + file>>worldInfo.horizon.x>>worldInfo.horizon.y>>worldInfo.horizon.z; + break; + case 'z': + file>>worldInfo.zenith.x>>worldInfo.zenith.y>>worldInfo.zenith.z; + break; + case 'a': + file>>worldInfo.ambient.x>>worldInfo.ambient.y>>worldInfo.ambient.z; + break; + case 'W': + { + XMFLOAT4 r; + float s; + file>>r.x>>r.y>>r.z>>r.w>>s; + XMStoreFloat3(&wind.direction, XMVector3Transform( XMVectorSet(0,s,0,0),XMMatrixRotationQuaternion(XMLoadFloat4(&r)) )); + } + break; + case 'm': + { + float s,e,h; + file>>s>>e>>h; + worldInfo.fogSEH=XMFLOAT3(s,e,h); + } + break; + default:break; + } + } + } + file.close(); +} +void LoadWiCameras(const string&directory, const string& name, const string& identifier, vector& cameras + ,const vector& armatures, map& transforms){ + stringstream filename(""); + filename<>voidStr; + while(!file.eof()){ + string line=""; + file>>line; + switch(line[0]){ + + case 'c': + { + XMFLOAT3 trans; + XMFLOAT4 rot; + string name(""),parentA(""),parentB(""); + file>>name>>parentA>>parentB>>trans.x>>trans.y>>trans.z>>rot.x>>rot.y>>rot.z>>rot.w; + + + stringstream identified_parentArmature(""); + identified_parentArmature<name.compare(identified_parentArmature.str())){ + for(int j=0;jboneCollection.size();++j){ + if(!armatures[i]->boneCollection[j]->name.compare(parentB.c_str())) + cameras.back().attachTo(armatures[i]->boneCollection[j]); + } + } + } + + } + break; + case 'I': + { + XMFLOAT3 s,t; + XMFLOAT4 r; + file>>t.x>>t.y>>t.z>>r.x>>r.y>>r.z>>r.w>>s.x>>s.y>>s.z; + XMStoreFloat4x4(&cameras.back().parent_inv_rest + , XMMatrixScalingFromVector(XMLoadFloat3(&s)) * + XMMatrixRotationQuaternion(XMLoadFloat4(&r)) * + XMMatrixTranslationFromVector(XMLoadFloat3(&t)) + ); + } + break; + default:break; + } + } + } + file.close(); +} +void LoadWiDecals(const string&directory, const string& name, const string& texturesDir, list& decals){ + stringstream filename(""); + filename<>voidStr; + while(!file.eof()){ + string line=""; + file>>line; + switch(line[0]){ + case 'd': + { + string name; + XMFLOAT3 loc,scale; + XMFLOAT4 rot; + file>>name>>scale.x>>scale.y>>scale.z>>loc.x>>loc.y>>loc.z>>rot.x>>rot.y>>rot.z>>rot.w; + Decal* decal = new Decal(); + decal->name=name; + decal->translation_rest=loc; + decal->scale_rest=scale; + decal->rotation_rest=rot; + decals.push_back(new Decal(loc,scale,rot)); + } + break; + case 't': + { + string tex=""; + file>>tex; + stringstream ss(""); + ss<addTexture(ss.str()); + } + break; + case 'n': + { + string tex=""; + file>>tex; + stringstream ss(""); + ss<addNormal(ss.str()); + } + break; + default:break; + }; + } + } + file.close(); +} + +void LoadFromDisk(const string& dir, const string& name, const string& identifier + , vector& armatures + , MaterialCollection& materials + , vector& objects_norm, vector& objects_trans, vector& objects_water + , MeshCollection& meshes + , vector& lights + , vector& spheres + , WorldInfo& worldInfo, Wind& wind + , vector& cameras + , vector& l_armatures + , vector& l_objects + , map& transforms + , list& decals + ) +{ + MaterialCollection l_materials; + vector l_objects_norm; + vector l_objects_trans; + vector l_objects_water; + MeshCollection l_meshes; + vector l_lights; + vector l_spheres; + WorldInfo l_worldInfo; + Wind l_wind; + vector l_cameras; + map l_transforms; + list l_decals; + + stringstream directory(""),armatureFilePath(""),materialLibFilePath(""),meshesFilePath(""),objectsFilePath("") + ,actionsFilePath(""),lightsFilePath(""),worldInfoFilePath(""),enviroMapFilePath(""),hitSpheresFilePath("") + ,camerasFilePath(""),decalsFilePath(""); + + directory<Release(); + if(texture) texture->Release(); + if(normalMap) normalMap->Release(); + if(displacementMap) displacementMap->Release(); + if(specularMap) specularMap->Release();*/ + ResourceManager::del(refMapName); + ResourceManager::del(textureName); + ResourceManager::del(normalMapName); + ResourceManager::del(displacementMapName); + ResourceManager::del(specularMapName); + refMap=nullptr; + texture=nullptr; + normalMap=nullptr; + displacementMap=nullptr; + specularMap=nullptr; +} +void Light::CleanUp(){ + shadowCam.clear(); + shadowMap.clear(); + lensFlareRimTextures.clear(); + for(string x:lensFlareNames) + ResourceManager::del(x); + lensFlareNames.clear(); +} + +void GenerateSPTree(SPTree*& tree, vector& objects, int type){ + if(type==GENERATE_QUADTREE) + tree = new QuadTree(); + else if(type==GENERATE_OCTREE) + tree = new Octree(); + tree->initialize(objects); +} + +Cullable::Cullable():bounds(AABB())/*,lastSquaredDistMulThousand(0)*/{} +Streamable::Streamable():directory(""),meshfile(""),materialfile(""),loaded(false){} + +#pragma region AABB +AABB::AABB(){ + for(int i=0;i<8;++i) corners[i]=XMFLOAT3(0,0,0); +} +AABB::AABB(const XMFLOAT3& min, const XMFLOAT3& max){ + create(min,max); +} +void AABB::createFromHalfWidth(const XMFLOAT3& center, const XMFLOAT3& halfwidth){ + XMFLOAT3 min = XMFLOAT3(center.x-halfwidth.x,center.y-halfwidth.y,center.z-halfwidth.z); + XMFLOAT3 max = XMFLOAT3(center.x+halfwidth.x,center.y+halfwidth.y,center.z+halfwidth.z); + create(min,max); +} +void AABB::create(const XMFLOAT3& min, const XMFLOAT3& max){ + corners[0]=min; + corners[1]=XMFLOAT3(min.x,max.y,min.z); + corners[2]=XMFLOAT3(min.x,max.y,max.z); + corners[3]=XMFLOAT3(min.x,min.y,max.z); + corners[4]=XMFLOAT3(max.x,min.y,min.z); + corners[5]=XMFLOAT3(max.x,max.y,min.z); + corners[6]=max; + corners[7]=XMFLOAT3(max.x,min.y,max.z); +} +AABB AABB::get(const XMMATRIX& mat){ + AABB ret; + XMFLOAT3 min,max; + for(int i=0;i<8;++i){ + XMVECTOR point = XMVector3Transform( XMLoadFloat3(&corners[i]), mat ); + XMStoreFloat3(&ret.corners[i],point); + } + min=ret.corners[0]; + max=ret.corners[6]; + for(int i=0;i<8;++i){ + XMFLOAT3& p=ret.corners[i]; + + if(p.xmax.x) max.x=p.x; + if(p.y>max.y) max.y=p.y; + if(p.z>max.z) max.z=p.z; + } + + ret.create(min,max); + + return ret; +} +AABB AABB::get(const XMFLOAT4X4& mat){ + return get(XMLoadFloat4x4(&mat)); +} +XMFLOAT3 AABB::getMin()const {return corners[0];} +XMFLOAT3 AABB::getMax() const{return corners[6];} +XMFLOAT3 AABB::getCenter() const{ + XMFLOAT3 min=getMin(),max=getMax(); + return XMFLOAT3((min.x+max.x)*0.5f,(min.y+max.y)*0.5f,(min.z+max.z)*0.5f); +} +XMFLOAT3 AABB::getHalfWidth() const{ + XMFLOAT3 max=getMax(),center=getCenter(); + return XMFLOAT3(abs(max.x-center.x),abs(max.y-center.y),abs(max.z-center.z)); +} +float AABB::getArea() const{ + XMFLOAT3 _min = getMin(); + XMFLOAT3 _max = getMax(); + return (_max.x-_min.x)*(_max.y-_min.y)*(_max.z-_min.z); +} +float AABB::getRadius() const{ + XMFLOAT3& abc=getHalfWidth(); + return max(max(abc.x,abc.y),abc.z); +} +AABB::INTERSECTION_TYPE AABB::intersects(const AABB& b) const{ + + XMFLOAT3 aMin = getMin(), aMax=getMax(); + XMFLOAT3 bMin = b.getMin(), bMax=b.getMax(); + + if( bMin.x >= aMin.x && bMax.x <= aMax.x && + bMin.y >= aMin.y && bMax.y <= aMax.y && + bMin.z >= aMin.z && bMax.z <= aMax.z ) + { + return INSIDE; + } + + if( aMax.x < bMin.x || aMin.x > bMax.x ) + return OUTSIDE; + if( aMax.y < bMin.y || aMin.y > bMax.y ) + return OUTSIDE; + if( aMax.z < bMin.z || aMin.z > bMax.z ) + return OUTSIDE; + + return INTERSECTS; +} +bool AABB::intersects(const XMFLOAT3& p) const{ + XMFLOAT3 max=getMax(); + XMFLOAT3 min=getMin(); + if (p.x>max.x) return false; + if (p.xmax.y) return false; + if (p.ymax.z) return false; + if (p.z= tmin; +} +AABB AABB::operator* (float a) +{ + XMFLOAT3 min = getMin(); + XMFLOAT3 max = getMax(); + min.x*=a; + min.y*=a; + min.z*=a; + max.x*=a; + max.y*=a; + max.z*=a; + return AABB(min,max); +} +#pragma endregion + +bool SPHERE::intersects(const AABB& b){ + XMFLOAT3 min = b.getMin(); + XMFLOAT3 max = b.getMax(); + XMFLOAT3 closestPointInAabb = WickedMath::Min(WickedMath::Max(center, min), max); + double distanceSquared = WickedMath::Distance(closestPointInAabb,center); + return distanceSquared < radius; +} +bool SPHERE::intersects(const SPHERE& b){ + return WickedMath::Distance(center,b.center)<=radius+b.radius; +} + +bool RAY::intersects(const AABB& box) const{ + return box.intersects(*this); +} + +ID3D11Buffer *HitSphere::vertexBuffer=nullptr; +void HitSphere::SetUpStatic() +{ + const int numVert = (RESOLUTION+1)*2; + vector verts(0); + + for(int i=0;i<=RESOLUTION;++i){ + float alpha = (float)i/(float)RESOLUTION*2*3.14159265359f; + verts.push_back(XMFLOAT3A(XMFLOAT3A(sin(alpha),cos(alpha),0))); + verts.push_back(XMFLOAT3A(XMFLOAT3A(0,0,0))); + } + + D3D11_BUFFER_DESC bd; + ZeroMemory( &bd, sizeof(bd) ); + bd.Usage = D3D11_USAGE_IMMUTABLE; + bd.ByteWidth = sizeof( XMFLOAT3A )*verts.size(); + bd.BindFlags = D3D11_BIND_VERTEX_BUFFER; + bd.CPUAccessFlags = 0; + D3D11_SUBRESOURCE_DATA InitData; + ZeroMemory( &InitData, sizeof(InitData) ); + InitData.pSysMem = verts.data(); + Renderer::graphicsDevice->CreateBuffer( &bd, &InitData, &vertexBuffer ); +} +void HitSphere::CleanUpStatic() +{ + if(vertexBuffer){ + vertexBuffer->Release(); + vertexBuffer=NULL; + } +} + +#pragma region BONE +XMMATRIX Bone::getTransform(int getTranslation, int getRotation, int getScale){ + //XMMATRIX& w = XMMatrixTranslation(0,0,length)*XMLoadFloat4x4(&world); + + //if(getTranslation&&getRotation&&getScale){ + // return w; + //} + //XMVECTOR v[3]; + //XMMATRIX& identity = XMMatrixIdentity(); + //XMMatrixDecompose(&v[0],&v[1],&v[2],w); + + //return (getScale?XMMatrixScalingFromVector(v[0]):identity)* + // (getRotation?XMMatrixRotationQuaternion(v[1]):identity)* + // (getTranslation?XMMatrixTranslationFromVector(v[2]):identity) + // ; + + return XMMatrixTranslation(0,0,length)*XMLoadFloat4x4(&world); +} +#pragma endregion + +#pragma region TRANSFORM + + +XMMATRIX Transform::getTransform(int getTranslation, int getRotation, int getScale){ + worldPrev=world; + translationPrev=translation; + scalePrev=scale; + rotationPrev=rotation; + + XMVECTOR s = XMLoadFloat3(&scale_rest); + XMVECTOR r = XMLoadFloat4(&rotation_rest); + XMVECTOR t = XMLoadFloat3(&translation_rest); + XMMATRIX& w = + XMMatrixScalingFromVector(s)* + XMMatrixRotationQuaternion(r)* + XMMatrixTranslationFromVector(t) + ; + XMStoreFloat4x4( &world_rest,w ); + + if(parent!=nullptr){ + w = w * XMLoadFloat4x4(&parent_inv_rest) * parent->getTransform(); + XMVECTOR v[3]; + XMMatrixDecompose(&v[0],&v[1],&v[2],w); + XMStoreFloat3( &scale,v[0] ); + XMStoreFloat4( &rotation,v[1] ); + XMStoreFloat3( &translation,v[2] ); + XMStoreFloat4x4( &world, w ); + } + else{ + world = world_rest; + translation=translation_rest; + rotation=rotation_rest; + scale=scale_rest; + } + + return w; + + //XMVECTOR s = XMVectorSet(1,1,1,1); + //XMVECTOR r = XMVectorSet(0,0,0,1); + //XMVECTOR t = XMVectorSet(0,0,0,1); + //if(getScale) + // s = XMLoadFloat3(&scale_rest); + //if(getRotation) + // r = XMLoadFloat4(&rotation_rest); + //if(getTranslation) + // t = XMLoadFloat3(&translation_rest); + //XMMATRIX& w = + // XMMatrixScalingFromVector(s)* + // XMMatrixRotationQuaternion(r)* + // XMMatrixTranslationFromVector(t) + // ; + //XMStoreFloat4x4( &world_rest,w ); + + //if(parent!=nullptr){ + // w = w * XMLoadFloat4x4(&parent_inv_rest) * parent->getTransform(copyParentT,copyParentR,copyParentS); + // XMVECTOR v[3]; + // XMMatrixDecompose(&v[0],&v[1],&v[2],w); + // XMStoreFloat3( &scale,v[0] ); + // XMStoreFloat4( &rotation,v[1] ); + // XMStoreFloat3( &translation,v[2] ); + // XMStoreFloat4x4( &world, w ); + //} + //else{ + // world = world_rest; + // translation=translation_rest; + // rotation=rotation_rest; + // scale=scale_rest; + //} + + // + //return w; +} +//attach to parent +void Transform::attachTo(Transform* newParent, int copyTranslation, int copyRotation, int copyScale){ + if(newParent!=nullptr){ + parent=newParent; + copyParentT=copyTranslation; + copyParentR=copyRotation; + copyParentS=copyScale; + XMStoreFloat4x4( &parent_inv_rest, XMMatrixInverse( nullptr,parent->getTransform(copyParentT,copyParentR,copyParentS) ) ); + parent->children.insert(this); + } +} +//detach child - detach all if no parameters +void Transform::detachChild(Transform* child){ + if(child==nullptr){ + for(Transform* c : children){ + if(c!=nullptr){ + c->detach(0); + } + } + children.clear(); + } + else{ + if( children.find(child)!=children.end() ){ + child->detach(); + } + } +} +//detach from parent +void Transform::detach(int eraseFromParent){ + if(parent!=nullptr){ + if(eraseFromParent && parent->children.find(this)!=parent->children.end()){ + parent->children.erase(this); + } + applyTransform(copyParentT,copyParentR,copyParentS); + } + parent=nullptr; +} +void Transform::applyTransform(int t, int r, int s){ + if(t) + translation_rest=translation; + if(r) + rotation_rest=rotation; + if(s) + scale_rest=scale; +} +void Transform::transform(const XMFLOAT3& t, const XMFLOAT4& r, const XMFLOAT3& s){ + translation_rest.x+=t.x; + translation_rest.y+=t.y; + translation_rest.z+=t.z; + + XMStoreFloat4(&rotation_rest,XMQuaternionMultiply(XMLoadFloat4(&rotation_rest),XMLoadFloat4(&r))); + + scale_rest.x*=s.x; + scale_rest.y*=s.y; + scale_rest.z*=s.z; +} +void Transform::transform(const XMMATRIX& m){ + XMVECTOR v[3]; + if(XMMatrixDecompose(&v[0],&v[1],&v[2],m)){ + XMFLOAT3 t,s; + XMFLOAT4 r; + XMStoreFloat3(&s,v[0]); + XMStoreFloat4(&r,v[1]); + XMStoreFloat3(&t,v[2]); + transform(t,r,s); + } +} + +#pragma endregion + +#pragma region Decals +Decal::Decal(const XMFLOAT3& tra, const XMFLOAT3& sca, const XMFLOAT4& rot, const string& tex, const string& nor):Cullable(),Transform(){ + scale_rest=scale=sca; + rotation_rest=rotation=rot; + translation_rest=translation=tra; + + Update(); + + texture=normal=nullptr; + addTexture(tex); + addNormal(nor); + + life = -2; //persistent + fadeStart=0; +} +void Decal::Update(){ + XMMATRIX rotMat = XMMatrixRotationQuaternion(XMLoadFloat4(&rotation)); + XMVECTOR eye = XMLoadFloat3(&translation); + XMVECTOR frontV = XMVector3Transform( XMVectorSet(0,0,1,0),rotMat ); + XMStoreFloat3(&front,frontV); + XMVECTOR at = XMVectorAdd(eye,frontV); + XMVECTOR up = XMVector3Transform( XMVectorSet(0,1,0,0),rotMat ); + XMStoreFloat4x4(&view, XMMatrixLookAtLH(eye,at,up)); + XMStoreFloat4x4(&projection, XMMatrixOrthographicLH(scale.x,scale.y,-scale.z,scale.z)); + XMStoreFloat4x4(&world_rest, XMMatrixScalingFromVector(XMLoadFloat3(&scale))*rotMat*XMMatrixTranslationFromVector(eye)); + + bounds.createFromHalfWidth(XMFLOAT3(0,0,0),XMFLOAT3(scale.x,scale.y,scale.z)); + bounds = bounds.get(XMLoadFloat4x4(&world_rest)); +} +void Decal::addTexture(const string& tex){ + texName=tex; + if(!tex.empty()){ + texture=(ID3D11ShaderResourceView*)ResourceManager::add(tex); + } +} +void Decal::addNormal(const string& nor){ + norName=nor; + if(!nor.empty()){ + normal=(ID3D11ShaderResourceView*)ResourceManager::add(nor); + } +} +void Decal::CleanUp(){ + ResourceManager::del(texName); + ResourceManager::del(norName); +} +#pragma endregion \ No newline at end of file diff --git a/WickedEngine/WickedLoader.h b/WickedEngine/WickedLoader.h new file mode 100644 index 000000000..a542cafef --- /dev/null +++ b/WickedEngine/WickedLoader.h @@ -0,0 +1,888 @@ +#pragma once +#include "WickedEngine.h" +//#include "ViewProp.h" +#include "skinningDEF.h" + +class HitSphere; +class Particle; +class EmittedParticle; +class HairParticle; +class RenderTarget; + +struct Armature; +struct Bone; +struct Mesh; +struct Material; + +typedef map MeshCollection; +typedef map MaterialCollection; + +struct SkinnedVertex +{ + XMFLOAT4 pos; + XMFLOAT3 nor; + XMFLOAT4 tex; + XMFLOAT4 bon; + XMFLOAT4 wei; + + + SkinnedVertex(){ + pos=XMFLOAT4(0,0,0,1); + nor=XMFLOAT3(0,0,0); + tex=XMFLOAT4(0,0,0,0); + bon=XMFLOAT4(0,0,0,0); + wei=XMFLOAT4(0,0,0,0); + }; + SkinnedVertex(const XMFLOAT3& newPos){ + pos=XMFLOAT4(newPos.x,newPos.y,newPos.z,1); + nor=XMFLOAT3(0,0,0); + tex=XMFLOAT4(0,0,0,0); + bon=XMFLOAT4(0,0,0,0); + wei=XMFLOAT4(0,0,0,0); + } +}; +struct Vertex +{ + XMFLOAT4 pos; + XMFLOAT3 nor; + XMFLOAT4 tex; + XMFLOAT4 pre; + //XMFLOAT4 bon; + //XMFLOAT4 wei; + + + Vertex(){ + pos=XMFLOAT4(0,0,0,1); + nor=XMFLOAT3(0,0,0); + tex=XMFLOAT4(0,0,0,0); + pre=XMFLOAT4(0,0,0,0); + //bon=XMFLOAT4(0,0,0,0); + //wei=XMFLOAT4(0,0,0,0); + }; + Vertex(const XMFLOAT3& newPos){ + pos=XMFLOAT4(newPos.x,newPos.y,newPos.z,1); + nor=XMFLOAT3(0,0,0); + tex=XMFLOAT4(0,0,0,0); + pre=XMFLOAT4(0,0,0,0); + //bon=XMFLOAT4(0,0,0,0); + //wei=XMFLOAT4(0,0,0,0); + } +}; +struct Instance +{ + XMFLOAT4A mat0; + XMFLOAT4A mat1; + XMFLOAT4A mat2; + + Instance(){ + mat0=XMFLOAT4A(1,0,0,0); + mat0=XMFLOAT4A(0,1,0,0); + mat0=XMFLOAT4A(0,0,1,0); + } + Instance(const XMMATRIX& matIn){ + XMStoreFloat4A(&mat0, matIn.r[0]); + XMStoreFloat4A(&mat1, matIn.r[1]); + XMStoreFloat4A(&mat2, matIn.r[2]); + } +}; +struct Material +{ + string name; + XMFLOAT3 diffuseColor; + + bool hasRefMap; + string refMapName; + ID3D11ShaderResourceView* refMap; + + bool hasTexture; + string textureName; + ID3D11ShaderResourceView* texture; + bool premultipliedTexture; + BLENDMODE blendFlag; + + bool hasNormalMap; + string normalMapName; + ID3D11ShaderResourceView* normalMap; + + bool hasDisplacementMap; + string displacementMapName; + ID3D11ShaderResourceView* displacementMap; + + bool hasSpecularMap; + string specularMapName; + ID3D11ShaderResourceView* specularMap; + + bool subsurface_scattering; + bool toonshading; + bool transparent; + bool water,shadeless; + float alpha; + float refraction_index; + float enviroReflection; + float emit; + + XMFLOAT4 specular; + int specular_power; + XMFLOAT3 movingTex; + float framesToWaitForTexCoordOffset; + XMFLOAT2 texOffset; + bool isSky; + + bool cast_shadow; + + + Material(){} + Material(string newName){ + name=newName; + diffuseColor = XMFLOAT3(0,0,0); + + hasRefMap=false; + refMapName=""; + refMap=0; + + hasTexture=false; + textureName=""; + texture=0; + premultipliedTexture=false; + blendFlag=BLENDMODE::BLENDMODE_ALPHA; + + hasNormalMap=false; + normalMapName=""; + normalMap=0; + + hasDisplacementMap=false; + displacementMapName=""; + displacementMap=0; + + + hasSpecularMap=false; + specularMapName=""; + specularMap=0; + + subsurface_scattering=toonshading=transparent=water=false; + alpha=1.0f; + refraction_index=0.0f; + enviroReflection=0.0f; + emit=0.0f; + + specular=XMFLOAT4(0,0,0,0); + specular_power=50; + movingTex=XMFLOAT3(0,0,0); + framesToWaitForTexCoordOffset=0; + texOffset=XMFLOAT2(0,0); + isSky=water=shadeless=false; + cast_shadow=true; + } + void CleanUp(); +}; +struct RibbonVertex +{ + XMFLOAT3 pos; + XMFLOAT2 tex; + XMFLOAT4 col; + float inc; + //XMFLOAT3 vel; + + RibbonVertex(){pos=/*vel=*/XMFLOAT3(0,0,0);tex=XMFLOAT2(0,0);col=XMFLOAT4(0,0,0,0);inc=1;} + RibbonVertex(const XMFLOAT3& newPos, const XMFLOAT2& newTex, const XMFLOAT4& newCol){ + pos=newPos; + tex=newTex; + col=newCol; + inc=1; + //vel=XMFLOAT3(0,0,0); + } +}; +struct RibbonTrail +{ + int base, tip; + RibbonTrail(){base=tip=-1;} +}; +struct SPHERE; +struct RAY; +struct AABB{ + enum INTERSECTION_TYPE{ + OUTSIDE, + INTERSECTS, + INSIDE, + }; + + XMFLOAT3 corners[8]; + + AABB(); + AABB(const XMFLOAT3& min, const XMFLOAT3& max); + void create(const XMFLOAT3& min, const XMFLOAT3& max); + void createFromHalfWidth(const XMFLOAT3& center, const XMFLOAT3& halfwidth); + AABB get(const XMMATRIX& mat); + AABB get(const XMFLOAT4X4& mat); + XMFLOAT3 getMin() const; + XMFLOAT3 getMax() const; + XMFLOAT3 getCenter() const; + XMFLOAT3 getHalfWidth() const; + float getArea() const; + float getRadius() const; + INTERSECTION_TYPE intersects(const AABB& b) const; + bool intersects(const XMFLOAT3& p) const; + bool intersects(const RAY& ray) const; + AABB operator* (float a); +}; +struct SPHERE{ + float radius; + XMFLOAT3 center; + SPHERE():center(XMFLOAT3(0,0,0)),radius(0){} + SPHERE(const XMFLOAT3& c, float r):center(c),radius(r){} + bool intersects(const AABB& b); + bool intersects(const SPHERE& b); +}; +struct RAY{ + XMFLOAT3 origin,direction,direction_inverse; + + RAY(const XMFLOAT3& newOrigin=XMFLOAT3(0,0,0), const XMFLOAT3& newDirection=XMFLOAT3(0,0,1)):origin(newOrigin),direction(newDirection){} + RAY(const XMVECTOR& newOrigin, const XMVECTOR& newDirection){ + XMStoreFloat3(&origin,newOrigin); + XMStoreFloat3(&direction,newDirection); + XMStoreFloat3(&direction_inverse,XMVectorDivide(XMVectorSet(1,1,1,1),newDirection)); + } + bool intersects(const AABB& box) const; +}; +struct BoneShaderBuffer +{ +#ifdef USE_GPU_SKINNING + XMMATRIX pose[MAXBONECOUNT]; + XMMATRIX prev[MAXBONECOUNT]; +#endif +}; +struct VertexRef{ + int index; + float weight; + VertexRef(){index=0;weight=0;} + VertexRef(int i, float w){index=i;weight=w;} +}; +struct VertexGroup{ + string name; + //vector vertices; + //index,weight + map vertices; + VertexGroup(){name="";} + VertexGroup(string n){name=n;} + void addVertex(const VertexRef& vRef){vertices.insert(pair(vRef.index,vRef.weight));} +}; +enum STENCILREF{ + STENCILREF_EMPTY, + STENCILREF_SKY, + STENCILREF_DEFAULT, + STENCILREF_TRANSPARENT, + STENCILREF_CHARACTER, + STENCILREF_SHADELESS, + STENCILREF_SKIN, +}; +struct Mesh{ + string name; + string parent; + vector vertices; + vector skinnedVertices; + vector indices; + vector physicsverts; + vector physicsindices; + vector physicalmapGP; + vector instances[5]; + + ID3D11Buffer* meshVertBuff; + ID3D11Buffer* meshInstanceBuffer; + ID3D11Buffer* meshIndexBuff; + ID3D11Buffer* boneBuffer; + ID3D11Buffer* sOutBuffer; + + vector materialNames; + vector materialIndices; + vector materials; + bool renderable,doubleSided; + STENCILREF stencilRef; + vector usedBy; + + int armatureIndex; + Armature* armature; + + AABB aabb; + + //RIBBONTRAIL VERTS + RibbonTrail trailInfo; + + bool isBillboarded; + XMFLOAT3 billboardAxis; + + vector vertexGroups; + bool softBody; + float mass, friction; + int massVG,goalVG,softVG; //vertexGroupID + vector goalPositions,goalNormals; + + Mesh(){ + init(); + } + Mesh(string newName){ + name=newName; + init(); + } + void CleanUp(){ + if(meshVertBuff) {meshVertBuff->Release(); meshVertBuff=NULL; } + if(meshInstanceBuffer) {meshInstanceBuffer->Release(); meshInstanceBuffer=NULL;} + if(meshIndexBuff) {meshIndexBuff->Release(); meshIndexBuff=NULL; } + if(sOutBuffer) {sOutBuffer->Release(); sOutBuffer=NULL; } + vertices.clear(); + skinnedVertices.clear(); + indices.clear(); + vertexGroups.clear(); + goalPositions.clear(); + goalNormals.clear(); + for(int i=0;i<5;++i){ + instances[i].clear(); + } + } + void LoadFromFile(const string& newName, const string& fname + , const MaterialCollection& materialColl, vector armatures, const string& identifier=""); + bool buffersComplete; + void CreateBuffers(); + void AddInstance(int count); + void init(){ + parent=""; + vertices.resize(0); + skinnedVertices.resize(0); + indices.resize(0); + meshVertBuff=NULL; + meshInstanceBuffer=NULL; + meshIndexBuff=NULL; + boneBuffer=NULL; + sOutBuffer=NULL; + materialNames.resize(0); + materialIndices.resize(0); + renderable=false; + doubleSided=false; + stencilRef = STENCILREF::STENCILREF_DEFAULT; + usedBy.resize(0); + aabb=AABB(); + trailInfo=RibbonTrail(); + armatureIndex=0; + armature=NULL; + isBillboarded=false; + billboardAxis=XMFLOAT3(0,0,0); + vertexGroups.resize(0); + softBody=false; + mass=friction=1; + massVG=-1; + goalVG=-1; + softVG=-1; + goalPositions.resize(0); + goalNormals.resize(0); + buffersComplete=false; + } + + bool hasArmature()const{return armature!=nullptr;} +}; +struct Node +{ + string name; + + Node(){ + name=""; + } +}; +struct Load_Debug_Properties +{ + string parentName; + + Load_Debug_Properties(){ + parentName=""; + } +}; +struct Transform : public Node, public Load_Debug_Properties +{ + Transform* parent; + set children; + + XMFLOAT3 translation_rest, translation, translationPrev; + XMFLOAT4 rotation_rest, rotation, rotationPrev; + XMFLOAT3 scale_rest, scale, scalePrev; + XMFLOAT4X4 world_rest, world, worldPrev; + XMFLOAT4X4 *boneRelativity, *boneRelativityPrev; //for bones only (calculating efficiency) + + XMFLOAT4X4 parent_inv_rest; + int copyParentT,copyParentR,copyParentS; + + Transform():Node(){ + parent=nullptr; + copyParentT=copyParentR=copyParentS=1; + boneRelativity=boneRelativityPrev=nullptr; + translation_rest=translation=translationPrev=XMFLOAT3(0,0,0); + scale_rest=scale=scalePrev=XMFLOAT3(1,1,1); + rotation_rest=rotation=rotationPrev=XMFLOAT4(0,0,0,1); + world_rest=world=worldPrev=parent_inv_rest=XMFLOAT4X4(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); + } + ~Transform(){ + if(boneRelativity!=nullptr){ + delete boneRelativity; + boneRelativity=nullptr; + } + if(boneRelativityPrev!=nullptr){ + delete boneRelativityPrev; + boneRelativityPrev=nullptr; + } + }; + + virtual XMMATRIX getTransform(int getTranslation = 1, int getRotation = 1, int getScale = 1); + //attach to parent + void attachTo(Transform* newParent, int copyTranslation=1, int copyRotation=1, int copyScale=1); + //detach child - detach all if no parameters + void detachChild(Transform* child = nullptr); + //detach from parent + void detach(int erasefromParent = 1); + void applyTransform(int t=1, int r=1, int s=1); + void transform(const XMFLOAT3& t = XMFLOAT3(0,0,0), const XMFLOAT4& r = XMFLOAT4(0,0,0,0), const XMFLOAT3& s = XMFLOAT3(0,0,0)); + void transform(const XMMATRIX& m = XMMatrixIdentity()); +}; +struct Cullable +{ +public: + Cullable(); + AABB bounds; + long lastSquaredDistMulThousand; //for early depth sort + bool operator() (const Cullable* a, const Cullable* b) const{ + return a->lastSquaredDistMulThousandlastSquaredDistMulThousand; + } +}; +struct Streamable : public Cullable +{ +public: + Streamable(); + string directory; + string meshfile; + string materialfile; + bool loaded; + Mesh* mesh; + + void StreamIn(); + void StreamOut(); +}; +struct Object : public Streamable, public Transform +{ + bool armatureDeform; + + //PARTICLE + enum ParticleEmitter{ + NO_EMITTER, + EMITTER_VISIBLE, + EMITTER_INVISIBLE, + }; + ParticleEmitter particleEmitter; + vector< EmittedParticle* > eParticleSystems; + vector< HairParticle* > hParticleSystems; + + + //PHYSICS + bool rigidBody, kinematic; + string collisionShape, physicsType; + float mass, friction, restitution, damping; + + + //RIBBON TRAIL +#define MAX_RIBBONTRAILS 10000 + deque trail; + ID3D11Buffer* trailBuff; + + int physicsObjectI; + + Object(){}; + Object(string newName):Transform(){ + armatureDeform=false; + name=newName; + XMStoreFloat4x4( &world,XMMatrixIdentity() ); + XMStoreFloat4x4( &worldPrev,XMMatrixIdentity() ); + trail.resize(0); + trailBuff=NULL; + particleEmitter = NO_EMITTER; + eParticleSystems.resize(0); + hParticleSystems.resize(0); + mesh=nullptr; + rigidBody=kinematic=false; + collisionShape=""; + mass = friction = restitution = damping = 1.0f; + physicsType="ACTIVE"; + physicsObjectI=-1; + } + + void CleanUp(){ + trail.clear(); + if(trailBuff) {trailBuff->Release(); trailBuff=NULL;} + } +}; +struct KeyFrame +{ + XMFLOAT4 data; + int frameI; + + KeyFrame(){ + frameI=0; + data=XMFLOAT4(0,0,0,1); + } + KeyFrame(int newFrameI, float x, float y, float z, float w){ + frameI=newFrameI; + data=XMFLOAT4(x,y,z,w); + } +}; +struct Action +{ + string name; + int frameCount; + + Action(){ + name=""; + frameCount=0; + } +}; +struct ActionFrames +{ + vector< KeyFrame > keyframesRot; + vector< KeyFrame > keyframesPos; + vector< KeyFrame > keyframesSca; + + ActionFrames(){ + keyframesRot.resize(0); + keyframesPos.resize(0); + keyframesSca.resize(0); + } + void CleanUp(){ + keyframesRot.clear(); + keyframesPos.clear(); + keyframesSca.clear(); + } +}; +struct Bone : public Transform +{ + vector< string > childrenN; + vector< Bone* > childrenI; + + XMFLOAT4X4 restInv; + vector actionFrames; + + XMFLOAT4X4 recursivePose, recursiveRest, recursiveRestInv; + + float length; + bool connected; + + Bone(){}; + Bone(string newName):Transform(){ + name=newName; + childrenN.resize(0); + childrenI.resize(0); + actionFrames.resize(0); + length=1.0f; + //XMStoreFloat4x4( &world,XMMatrixIdentity() ); + //XMStoreFloat4x4( &worldPrev,XMMatrixIdentity() ); + //XMStoreFloat4x4( &recursivePose,XMMatrixIdentity() ); + //XMStoreFloat4x4( &recursiveRest,XMMatrixIdentity() ); + //XMStoreFloat4x4( &recursiveRestInv,XMMatrixIdentity() ); + boneRelativity=new XMFLOAT4X4(); + boneRelativityPrev=new XMFLOAT4X4(); + connected = false; + } + //XMFLOAT3 getTailPos(const XMMATRIX& world){ + // XMFLOAT3 pos = XMFLOAT3( 0,0,length ); + // XMVECTOR _pos = XMLoadFloat3( &pos ); + // XMVECTOR restedPos = XMVector3Transform( _pos,XMMatrixInverse( 0,XMMatrixTranspose(XMLoadFloat4x4(&rest) )) ); + // XMVECTOR posedPos = XMVector3Transform( restedPos,XMMatrixTranspose(XMLoadFloat4x4(&poseFrame)) ); + // XMVECTOR worldPos = XMVector3Transform( posedPos,world ); + // XMFLOAT3 finalPos; + // XMStoreFloat3( &finalPos, worldPos ); + + // return finalPos; + //} + void CleanUp(){ + childrenN.clear(); + childrenI.clear(); + + for(unsigned int i=0;i boneCollection; + vector rootbones; + + int activeAction, prevAction; + float currentFrame, prevActionResolveFrame; + vector actions; + + + Armature(){} + Armature(string newName,string identifier):Transform(){ + unidentified_name=newName; + stringstream ss(""); + ss<CleanUp(); + boneCollection.clear(); + rootbones.clear(); + } +}; +struct SHCAM{ + XMFLOAT4X4 View,Projection; + XMFLOAT3 Eye,At,Up; + float nearplane,farplane,size; + + SHCAM(){ + Init(XMQuaternionIdentity()); + Create_Perspective(XM_PI/2.0); + nearplane=0.1;farplane=200,size=0; + } + //orthographic + SHCAM(const float& size, const XMVECTOR& dir, const float& newNear, const float& newFar){ + nearplane=newNear; + farplane=newFar; + Init(dir); + Create_Ortho(size); + }; + //perspective + SHCAM(const XMFLOAT4& dir, const float& newNear, const float& newFar, const float& newFov){ + nearplane=newNear; + farplane=newFar; + Init(XMLoadFloat4(&dir)); + Create_Perspective(newFov); + }; + void Init(const XMVECTOR& dir){ + XMMATRIX rot = XMMatrixIdentity(); + rot = XMMatrixRotationQuaternion( dir ); + XMVECTOR rEye = XMVectorSet(0,0,0,0); + XMVECTOR rAt = XMVector3Transform( XMVectorSet( 0.0f, -1.0f, 0.0f, 0.0f ), rot); + XMVECTOR rUp = XMVector3Transform( XMVectorSet( 0.0f, 0.0f, 1.0f, 0.0f ), rot); + XMMATRIX rView = XMMatrixLookAtLH(rEye,rAt,rUp); + + XMStoreFloat3( &Eye, rEye ); + XMStoreFloat3( &At, rAt ); + XMStoreFloat3( &Up, rUp ); + XMStoreFloat4x4( &View, rView); + } + void Create_Ortho(const float& size){ + XMMATRIX rProjection = XMMatrixOrthographicLH(size,size,-farplane,farplane); + XMStoreFloat4x4( &Projection, rProjection); + this->size=size; + } + void Create_Perspective(const float& fov){ + XMMATRIX rProjection = XMMatrixPerspectiveFovLH(fov,1,nearplane,farplane); + XMStoreFloat4x4( &Projection, rProjection); + } + void Update(const XMVECTOR& pos){ + XMStoreFloat4x4( &View , XMMatrixTranslationFromVector(-pos) + * XMMatrixLookAtLH(XMLoadFloat3(&Eye),XMLoadFloat3(&At),XMLoadFloat3(&Up)) + ); + } + void Update(const XMMATRIX& mat){ + XMVECTOR sca,rot,tra; + XMMatrixDecompose(&sca,&rot,&tra,mat); + + XMMATRIX mRot = XMMatrixRotationQuaternion( rot ); + + XMVECTOR rEye = XMVectorAdd( XMLoadFloat3(&Eye),tra ); + XMVECTOR rAt = XMVectorAdd( XMVector3Transform( XMLoadFloat3(&At),mRot ),tra ); + XMVECTOR rUp = XMVector3Transform( XMLoadFloat3(&Up),mRot ); + + XMStoreFloat4x4( &View , + XMMatrixLookAtLH(rEye,rAt,rUp) + ); + } + XMMATRIX getVP(){ + return XMMatrixTranspose(XMLoadFloat4x4(&View)*XMLoadFloat4x4(&Projection)); + } +}; +struct Light : public Cullable , public Transform +{ + XMFLOAT4 color; + XMFLOAT4 enerDis; + bool noHalo; + vector lensFlareRimTextures; + vector lensFlareNames; + + vector shadowMap; + vector shadowCam; + + enum LightType{ + DIRECTIONAL, + POINT, + SPOT, + }; + LightType type; + + Light():Transform(){ + color=XMFLOAT4(0,0,0,0); + enerDis=XMFLOAT4(0,0,0,0); + type=LightType::POINT; + shadowMap.resize(0); + shadowCam.resize(0); + noHalo=false; + lensFlareRimTextures.resize(0); + lensFlareNames.resize(0); + } + void CleanUp(); + static string getTypeStr(int type){ + if(type==Light::DIRECTIONAL) return "dirLightMesh"; + else if(type==Light::POINT) return "pointLightMesh"; + else if(type==Light::SPOT) return "spotLightMesh"; + else if(type==3) return "pointLightHaloMesh"; + return ""; + } +}; +struct Decal : public Cullable, public Transform +{ + string texName,norName; + ID3D11ShaderResourceView* texture,*normal; + XMFLOAT4X4 view,projection; + XMFLOAT3 front; + float life,fadeStart; + + Decal(const XMFLOAT3& tra=XMFLOAT3(0,0,0), const XMFLOAT3& sca=XMFLOAT3(1,1,1), const XMFLOAT4& rot=XMFLOAT4(0,0,0,1), const string& tex="", const string& nor=""); + void Update(); + void addTexture(const string& tex); + void addNormal(const string& nor); + void CleanUp(); +}; +struct WorldInfo{ + XMFLOAT3 horizon; + XMFLOAT3 zenith; + XMFLOAT3 ambient; + XMFLOAT3 fogSEH; + + WorldInfo(){ + horizon=zenith=ambient=XMFLOAT3(0,0,0); + fogSEH=XMFLOAT3(100,1000,0); + } +}; +struct Wind{ + XMFLOAT3 direction; + float time; + float randomness; + float waveSize; + Wind():direction(XMFLOAT3(0,0,0)),time(0),randomness(5),waveSize(1){} +}; +struct ActionCamera:public Transform{ + + ActionCamera():Transform(){ + } + ActionCamera(const XMFLOAT3& newPos, const XMFLOAT4& newRot, + const string& newName):Transform() + { + translation_rest=newPos; + rotation_rest=newRot; + name=newName; + } + //ActionCamera transformed(const XMFLOAT3& translation, const XMFLOAT4& rotation, const XMFLOAT3& scale){ + // XMVECTOR t = XMVector3Transform( XMLoadFloat3( &pos ), XMMatrixTranslationFromVector( XMLoadFloat3(&translation) ) ); + // XMVECTOR r = XMQuaternionMultiply( XMLoadFloat4( &rot ), XMLoadFloat4( &rotation ) ); + + // XMFLOAT3 newPos; + // XMStoreFloat3( &newPos,t ); + // XMFLOAT4 newRot; + // XMStoreFloat4( &newRot,r ); + + // return ActionCamera( + // newPos, newRot + // ,name,parentA,parentB,armatureI,boneI + // ); + //} +}; +struct HitSphere:public SPHERE, public Transform{ + float radius_saved, radius; + static ID3D11Buffer *vertexBuffer; + enum HitSphereType{ + HITTYPE, + ATKTYPE, + INVTYPE, + }; + HitSphereType TYPE_SAVED,TYPE; + + HitSphere():Transform(){} + HitSphere(string newName, float newRadius, const XMFLOAT3& location, + Transform* newParent, string newProperty):Transform(){ + + name=newName; + radius_saved=newRadius; + translation_rest=location; + if(newParent!=nullptr){ + parentName=newParent->name; + parent=newParent; + } + TYPE=HitSphereType::HITTYPE; + if(newProperty.find("inv")!=string::npos) TYPE=HitSphereType::INVTYPE; + else if(newProperty.find("atk")!=string::npos) TYPE=HitSphereType::ATKTYPE; + TYPE_SAVED=TYPE; + } + void Reset(){ + TYPE=TYPE_SAVED; + radius=radius_saved; + } + +static const int RESOLUTION = 36; + static void SetUpStatic(); + static void CleanUpStatic(); + +}; + + + + +void RecursiveRest(Armature* armature, Bone* bone); + +void LoadWiArmatures(const string& directory, const string& filename, const string& identifier, vector& armatures + , map& transforms); +void LoadWiMaterialLibrary(const string& directory, const string& filename, const string& identifier, const string& texturesDir, MaterialCollection& materials); +void LoadWiObjects(const string& directory, const string& filename, const string& identifier, vector& objects_norm + , vector& objects_trans, vector& objects_water, vector& armatures, MeshCollection& meshes + , map& transforms, const MaterialCollection& materials); +void LoadWiMeshes(const string& directory, const string& filename, const string& identifier, MeshCollection& meshes, const vector& armatures, const MaterialCollection& materials); +void LoadWiActions(const string& directory, const string& filename, const string& identifier, vector& armatures); +void LoadWiLights(const string& directory, const string& filename, const string& identifier, vector& lights + , const vector& armatures + , map& transforms); +void LoadWiHitSpheres(const string& directory, const string& name, const string& identifier, vector& spheres + ,const vector& armatures, map& transforms); +void LoadWiWorldInfo(const string&directory, const string& name, WorldInfo& worldInfo, Wind& wind); +void LoadWiCameras(const string&directory, const string& name, const string& identifier, vector& cameras + ,const vector& armatures, map& transforms); +void LoadWiDecals(const string&directory, const string& name, const string& texturesDir, list& decals); + +void LoadFromDisk(const string& dir, const string& name, const string& identifier + , vector& armatures + , MaterialCollection& materials + , vector& objects_norm, vector& objects_trans, vector& objects_water + , MeshCollection& meshes + , vector& lights + , vector& spheres + , WorldInfo& worldInfo, Wind& wind + , vector& cameras + , vector& l_armatures + , vector& l_objects + , map& transforms + , list& decals + ); + + + +#include "SPTree.h" +class SPTree; +#define GENERATE_QUADTREE 0 +#define GENERATE_OCTREE 1 +void GenerateSPTree(SPTree*& tree, vector& objects, int type=GENERATE_QUADTREE); \ No newline at end of file diff --git a/WickedEngine/WickedMath.cpp b/WickedEngine/WickedMath.cpp new file mode 100644 index 000000000..6e2565252 --- /dev/null +++ b/WickedEngine/WickedMath.cpp @@ -0,0 +1,109 @@ +#include "WickedMath.h" + + +float WickedMath::Distance(const XMFLOAT3& v1,const XMFLOAT3& v2) +{ + XMVECTOR vector1 = XMLoadFloat3(&v1); + XMVECTOR vector2 = XMLoadFloat3(&v2); + XMVECTOR vectorSub = XMVectorSubtract(vector1,vector2); + XMVECTOR length = XMVector3Length(vectorSub); + + float Distance = 0.0f; + XMStoreFloat(&Distance,length); + return Distance; +} +float WickedMath::DistanceSqaured(const XMFLOAT3& v1,const XMFLOAT3& v2) +{ + XMVECTOR vector1 = XMLoadFloat3(&v1); + XMVECTOR vector2 = XMLoadFloat3(&v2); + XMVECTOR vectorSub = XMVectorSubtract(vector1,vector2); + XMVECTOR length = XMVector3LengthSq(vectorSub); + + float Distance = 0.0f; + XMStoreFloat(&Distance,length); + return Distance; +} +float WickedMath::DistanceEstimated(const XMFLOAT3& v1,const XMFLOAT3& v2) +{ + XMVECTOR vector1 = XMLoadFloat3(&v1); + XMVECTOR vector2 = XMLoadFloat3(&v2); + XMVECTOR vectorSub = XMVectorSubtract(vector1,vector2); + XMVECTOR length = XMVector3LengthEst(vectorSub); + + float Distance = 0.0f; + XMStoreFloat(&Distance,length); + return Distance; +} +XMFLOAT3 WickedMath::getVectorHalfWayPoint(const XMFLOAT3& a, const XMFLOAT3& b) +{ + return XMFLOAT3( (a.x+b.x)*0.5f,(a.y+b.y)*0.5f,(a.z+b.z)*0.5f ); +} + +bool WickedMath::Collision(const XMFLOAT2& hitBox1Pos, const XMFLOAT2& hitBox1Siz, const XMFLOAT2& hitBox2Pos, const XMFLOAT2& hitBox2Siz) +{ + if( hitBox1Pos.x+hitBox1Siz.xhitBox2Pos.x+hitBox2Siz.x ) + return false; + else if( hitBox1Pos.y-hitBox1Siz.y>hitBox2Pos.y ) + return false; + else if( hitBox1Pos.ymax) return max; + return val; +} + +XMFLOAT3 WickedMath::getCubicHermiteSplinePos(const XMFLOAT3& startPos, const XMFLOAT3& endPos + , const XMFLOAT3& startTangent, const XMFLOAT3& endTangent + , const float& atInterval){ + float x,y,z,t; float r1=1.0f,r4=1.0f; + t=atInterval; + x=(2*t*t*t-3*t*t+1)*startPos.x+(-2*t*t*t+3*t*t)*endPos.x+(t*t*t-2*t*t+t)*startTangent.x+(t*t*t-t*t)*endTangent.x; + y=(2*t*t*t-3*t*t+1)*startPos.y+(-2*t*t*t+3*t*t)*endPos.y+(t*t*t-2*t*t+1)*startTangent.y+(t*t*t-t*t)*endTangent.y; + z=(2*t*t*t-3*t*t+1)*startPos.z+(-2*t*t*t+3*t*t)*endPos.z+(t*t*t-2*t*t+1)*startTangent.z+(t*t*t-t*t)*endTangent.z; + return XMFLOAT3(x,y,z); +} +XMFLOAT3 WickedMath::getQuadraticBezierPos(const XMFLOAT3& a,const XMFLOAT3&b, const XMFLOAT3& c, const float& t){ + float param0,param1,param2; + param0=pow(1-t,2); + param1=2*(1-t)*t; + param2=pow(t,2); + float x = param0*a.x+param1*b.x+param2*c.x; + float y = param0*a.y+param1*b.y+param2*c.y; + float z = param0*a.z+param1*b.z+param2*c.z; + return XMFLOAT3(x,y,z); +} +XMFLOAT3 WickedMath::getQuadraticBezierPos(const XMFLOAT4& a,const XMFLOAT4&b, const XMFLOAT4& c, const float& t){ + return getQuadraticBezierPos(XMFLOAT3(a.x,a.y,a.z),XMFLOAT3(b.x,b.y,b.z),XMFLOAT3(c.x,c.y,c.z),t); +} \ No newline at end of file diff --git a/WickedEngine/WickedMath.h b/WickedEngine/WickedMath.h new file mode 100644 index 000000000..10d860169 --- /dev/null +++ b/WickedEngine/WickedMath.h @@ -0,0 +1,26 @@ +#pragma once +#include "WickedEngine.h" + +static class WickedMath +{ +public: + static float Distance(const XMFLOAT3& v1,const XMFLOAT3& v2); + static float DistanceSqaured(const XMFLOAT3& v1,const XMFLOAT3& v2); + static float DistanceEstimated(const XMFLOAT3& v1,const XMFLOAT3& v2); + static XMFLOAT3 getVectorHalfWayPoint(const XMFLOAT3& a, const XMFLOAT3& b); + static bool Collision(const XMFLOAT2& hitBox1Pos, const XMFLOAT2& hitBox1Siz, const XMFLOAT2& hitBox2Pos, const XMFLOAT2& hitBox2Siz); + static float Lerp(const float& value1,const float& value2,const float& amount); + static XMFLOAT2 Lerp(const XMFLOAT2&,const XMFLOAT2&,const float&); + static XMFLOAT3 Lerp(const XMFLOAT3&,const XMFLOAT3&,const float&); + static XMFLOAT4 Lerp(const XMFLOAT4&,const XMFLOAT4&,const float&); + static XMFLOAT3 Max(const XMFLOAT3& a, const XMFLOAT3& b); + static XMFLOAT3 Min(const XMFLOAT3& a, const XMFLOAT3& b); + static float Clamp(float val, float min, float max); + + static XMFLOAT3 getCubicHermiteSplinePos(const XMFLOAT3& startPos, const XMFLOAT3& endPos + , const XMFLOAT3& startTangent, const XMFLOAT3& endTangent + , const float& atInterval); + static XMFLOAT3 getQuadraticBezierPos(const XMFLOAT3& a,const XMFLOAT3&b, const XMFLOAT3& c, const float& t); + static XMFLOAT3 getQuadraticBezierPos(const XMFLOAT4& a,const XMFLOAT4&b, const XMFLOAT4& c, const float& t); +}; + diff --git a/WickedEngine/XInput.cpp b/WickedEngine/XInput.cpp new file mode 100644 index 000000000..e32781da7 --- /dev/null +++ b/WickedEngine/XInput.cpp @@ -0,0 +1,58 @@ +#include "XInput.h" + + +XInput::XInput(void) +{ + g_bDeadZoneOn = true; + for(int i=0;iGamepad.wButtons; + return 0; +} +DWORD XInput::GetDirections(short newPlayerIndex){ + if(controllers[newPlayerIndex].bConnected) + return controllers[newPlayerIndex].state->Gamepad.wButtons; + return 0; + + /*if(dir == (XINPUT_GAMEPAD_DPAD_LEFT || XINPUT_GAMEPAD_DPAD_RIGHT || XINPUT_GAMEPAD_DPAD_UP || XINPUT_GAMEPAD_DPAD_DOWN || + XINPUT_GAMEPAD_DPAD_LEFT+XINPUT_GAMEPAD_DPAD_UP || XINPUT_GAMEPAD_DPAD_RIGHT+XINPUT_GAMEPAD_DPAD_DOWN || XINPUT_GAMEPAD_DPAD_UP+XINPUT_GAMEPAD_DPAD_RIGHT || XINPUT_GAMEPAD_DPAD_DOWN+XINPUT_GAMEPAD_DPAD_LEFT) + ) + return dir;*/ + //TODO!!! + //return dir; +} +bool XInput::isButtonDown(short newPlayerIndex,DWORD checkforButton){ + if(controllers[newPlayerIndex].bConnected) + return controllers[newPlayerIndex].state->Gamepad.wButtons & checkforButton; + return false; +} + +void XInput::CleanUp() +{ + delete(this); +} \ No newline at end of file diff --git a/WickedEngine/XInput.h b/WickedEngine/XInput.h new file mode 100644 index 000000000..184e0ad93 --- /dev/null +++ b/WickedEngine/XInput.h @@ -0,0 +1,39 @@ +#pragma once +#include "WickedEngine.h" + + + +#if (_WIN32_WINNT >= 0x0602 /*_WIN32_WINNT_WIN8*/) +#pragma comment(lib,"xinput.lib") +#else +#pragma comment(lib,"xinput9_1_0.lib") +#endif + + +#define MAX_CONTROLLERS 4 // XInput handles up to 4 controllers +#define INPUT_DEADZONE ( 0.24f * FLOAT(0x7FFF) ) // Default to 24% of the +/- 32767 range. This is a reasonable default value but can be altered if needed. + + +class XInput +{ +private: + + struct CONTROLLER_STATE + { + XINPUT_STATE* state; + bool bConnected; + }; + +public: + XInput(void); + HRESULT UpdateControllerState(); + DWORD GetButtons(SHORT); + DWORD GetDirections(short); + bool isButtonDown(short,DWORD); + void CleanUp(); + + CONTROLLER_STATE controllers[MAX_CONTROLLERS]; + WCHAR g_szMessage[4][1024]; + bool g_bDeadZoneOn; +}; + diff --git a/WickedEngine/Xaudio2_7/XAudio2.h b/WickedEngine/Xaudio2_7/XAudio2.h new file mode 100644 index 000000000..db0ebd8f8 --- /dev/null +++ b/WickedEngine/Xaudio2_7/XAudio2.h @@ -0,0 +1,1282 @@ +/************************************************************************** + * + * Copyright (c) Microsoft Corporation. All rights reserved. + * + * File: xaudio2.h + * Content: Declarations for the XAudio2 game audio API. + * + **************************************************************************/ + +#ifndef __XAUDIO2_INCLUDED__ +#define __XAUDIO2_INCLUDED__ + + +/************************************************************************** + * + * XAudio2 COM object class and interface IDs. + * + **************************************************************************/ + +#include "comdecl.h" // For DEFINE_CLSID and DEFINE_IID + +// XAudio 2.0 (March 2008 SDK) +//DEFINE_CLSID(XAudio2, fac23f48, 31f5, 45a8, b4, 9b, 52, 25, d6, 14, 01, aa); +//DEFINE_CLSID(XAudio2_Debug, fac23f48, 31f5, 45a8, b4, 9b, 52, 25, d6, 14, 01, db); + +// XAudio 2.1 (June 2008 SDK) +//DEFINE_CLSID(XAudio2, e21a7345, eb21, 468e, be, 50, 80, 4d, b9, 7c, f7, 08); +//DEFINE_CLSID(XAudio2_Debug, f7a76c21, 53d4, 46bb, ac, 53, 8b, 45, 9c, ae, 46, bd); + +// XAudio 2.2 (August 2008 SDK) +//DEFINE_CLSID(XAudio2, b802058a, 464a, 42db, bc, 10, b6, 50, d6, f2, 58, 6a); +//DEFINE_CLSID(XAudio2_Debug, 97dfb7e7, 5161, 4015, 87, a9, c7, 9e, 6a, 19, 52, cc); + +// XAudio 2.3 (November 2008 SDK) +//DEFINE_CLSID(XAudio2, 4c5e637a, 16c7, 4de3, 9c, 46, 5e, d2, 21, 81, 96, 2d); +//DEFINE_CLSID(XAudio2_Debug, ef0aa05d, 8075, 4e5d, be, ad, 45, be, 0c, 3c, cb, b3); + +// XAudio 2.4 (March 2009 SDK) +//DEFINE_CLSID(XAudio2, 03219e78, 5bc3, 44d1, b9, 2e, f6, 3d, 89, cc, 65, 26); +//DEFINE_CLSID(XAudio2_Debug, 4256535c, 1ea4, 4d4b, 8a, d5, f9, db, 76, 2e, ca, 9e); + +// XAudio 2.5 (August 2009 SDK) +//DEFINE_CLSID(XAudio2, 4c9b6dde, 6809, 46e6, a2, 78, 9b, 6a, 97, 58, 86, 70); +//DEFINE_CLSID(XAudio2_Debug, 715bdd1a, aa82, 436b, b0, fa, 6a, ce, a3, 9b, d0, a1); + +// XAudio 2.6 (February 2010 SDK) +//DEFINE_CLSID(XAudio2, 3eda9b49, 2085, 498b, 9b, b2, 39, a6, 77, 84, 93, de); +//DEFINE_CLSID(XAudio2_Debug, 47199894, 7cc2, 444d, 98, 73, ce, d2, 56, 2c, c6, 0e); + +// XAudio 2.7 (June 2010 SDK) +DEFINE_CLSID(XAudio2, 5a508685, a254, 4fba, 9b, 82, 9a, 24, b0, 03, 06, af); +DEFINE_CLSID(XAudio2_Debug, db05ea35, 0329, 4d4b, a5, 3a, 6d, ea, d0, 3d, 38, 52); +DEFINE_IID(IXAudio2, 8bcf1f58, 9fe7, 4583, 8a, c6, e2, ad, c4, 65, c8, bb); + + +// Ignore the rest of this header if only the GUID definitions were requested +#ifndef GUID_DEFS_ONLY + +#ifdef _XBOX + #include // Xbox COM declarations (IUnknown, etc) +#else + #include // Windows COM declarations +#endif + +#include // Markers for documenting API semantics +#include "audiodefs.h" // Basic audio data types and constants +#include "xma2defs.h" // Data types and constants for XMA2 audio + +// All structures defined in this file use tight field packing +#pragma pack(push, 1) + + +/************************************************************************** + * + * XAudio2 constants, flags and error codes. + * + **************************************************************************/ + +// Numeric boundary values +#define XAUDIO2_MAX_BUFFER_BYTES 0x80000000 // Maximum bytes allowed in a source buffer +#define XAUDIO2_MAX_QUEUED_BUFFERS 64 // Maximum buffers allowed in a voice queue +#define XAUDIO2_MAX_BUFFERS_SYSTEM 2 // Maximum buffers allowed for system threads (Xbox 360 only) +#define XAUDIO2_MAX_AUDIO_CHANNELS 64 // Maximum channels in an audio stream +#define XAUDIO2_MIN_SAMPLE_RATE 1000 // Minimum audio sample rate supported +#define XAUDIO2_MAX_SAMPLE_RATE 200000 // Maximum audio sample rate supported +#define XAUDIO2_MAX_VOLUME_LEVEL 16777216.0f // Maximum acceptable volume level (2^24) +#define XAUDIO2_MIN_FREQ_RATIO (1/1024.0f) // Minimum SetFrequencyRatio argument +#define XAUDIO2_MAX_FREQ_RATIO 1024.0f // Maximum MaxFrequencyRatio argument +#define XAUDIO2_DEFAULT_FREQ_RATIO 2.0f // Default MaxFrequencyRatio argument +#define XAUDIO2_MAX_FILTER_ONEOVERQ 1.5f // Maximum XAUDIO2_FILTER_PARAMETERS.OneOverQ +#define XAUDIO2_MAX_FILTER_FREQUENCY 1.0f // Maximum XAUDIO2_FILTER_PARAMETERS.Frequency +#define XAUDIO2_MAX_LOOP_COUNT 254 // Maximum non-infinite XAUDIO2_BUFFER.LoopCount +#define XAUDIO2_MAX_INSTANCES 8 // Maximum simultaneous XAudio2 objects on Xbox 360 + +// For XMA voices on Xbox 360 there is an additional restriction on the MaxFrequencyRatio +// argument and the voice's sample rate: the product of these numbers cannot exceed 600000 +// for one-channel voices or 300000 for voices with more than one channel. +#define XAUDIO2_MAX_RATIO_TIMES_RATE_XMA_MONO 600000 +#define XAUDIO2_MAX_RATIO_TIMES_RATE_XMA_MULTICHANNEL 300000 + +// Numeric values with special meanings +#define XAUDIO2_COMMIT_NOW 0 // Used as an OperationSet argument +#define XAUDIO2_COMMIT_ALL 0 // Used in IXAudio2::CommitChanges +#define XAUDIO2_INVALID_OPSET (UINT32)(-1) // Not allowed for OperationSet arguments +#define XAUDIO2_NO_LOOP_REGION 0 // Used in XAUDIO2_BUFFER.LoopCount +#define XAUDIO2_LOOP_INFINITE 255 // Used in XAUDIO2_BUFFER.LoopCount +#define XAUDIO2_DEFAULT_CHANNELS 0 // Used in CreateMasteringVoice +#define XAUDIO2_DEFAULT_SAMPLERATE 0 // Used in CreateMasteringVoice + +// Flags +#define XAUDIO2_DEBUG_ENGINE 0x0001 // Used in XAudio2Create on Windows only +#define XAUDIO2_VOICE_NOPITCH 0x0002 // Used in IXAudio2::CreateSourceVoice +#define XAUDIO2_VOICE_NOSRC 0x0004 // Used in IXAudio2::CreateSourceVoice +#define XAUDIO2_VOICE_USEFILTER 0x0008 // Used in IXAudio2::CreateSource/SubmixVoice +#define XAUDIO2_VOICE_MUSIC 0x0010 // Used in IXAudio2::CreateSourceVoice +#define XAUDIO2_PLAY_TAILS 0x0020 // Used in IXAudio2SourceVoice::Stop +#define XAUDIO2_END_OF_STREAM 0x0040 // Used in XAUDIO2_BUFFER.Flags +#define XAUDIO2_SEND_USEFILTER 0x0080 // Used in XAUDIO2_SEND_DESCRIPTOR.Flags + +// Default parameters for the built-in filter +#define XAUDIO2_DEFAULT_FILTER_TYPE LowPassFilter +#define XAUDIO2_DEFAULT_FILTER_FREQUENCY XAUDIO2_MAX_FILTER_FREQUENCY +#define XAUDIO2_DEFAULT_FILTER_ONEOVERQ 1.0f + +// Internal XAudio2 constants +#ifdef _XBOX + #define XAUDIO2_QUANTUM_NUMERATOR 2 // On Xbox 360, XAudio2 processes audio + #define XAUDIO2_QUANTUM_DENOMINATOR 375 // in 5.333ms chunks (= 2/375 seconds) +#else + #define XAUDIO2_QUANTUM_NUMERATOR 1 // On Windows, XAudio2 processes audio + #define XAUDIO2_QUANTUM_DENOMINATOR 100 // in 10ms chunks (= 1/100 seconds) +#endif +#define XAUDIO2_QUANTUM_MS (1000.0f * XAUDIO2_QUANTUM_NUMERATOR / XAUDIO2_QUANTUM_DENOMINATOR) + +// XAudio2 error codes +#define FACILITY_XAUDIO2 0x896 +#define XAUDIO2_E_INVALID_CALL 0x88960001 // An API call or one of its arguments was illegal +#define XAUDIO2_E_XMA_DECODER_ERROR 0x88960002 // The XMA hardware suffered an unrecoverable error +#define XAUDIO2_E_XAPO_CREATION_FAILED 0x88960003 // XAudio2 failed to initialize an XAPO effect +#define XAUDIO2_E_DEVICE_INVALIDATED 0x88960004 // An audio device became unusable (unplugged, etc) + + +/************************************************************************** + * + * Forward declarations for the XAudio2 interfaces. + * + **************************************************************************/ + +#ifdef __cplusplus + #define FWD_DECLARE(x) interface x +#else + #define FWD_DECLARE(x) typedef interface x x +#endif + +FWD_DECLARE(IXAudio2); +FWD_DECLARE(IXAudio2Voice); +FWD_DECLARE(IXAudio2SourceVoice); +FWD_DECLARE(IXAudio2SubmixVoice); +FWD_DECLARE(IXAudio2MasteringVoice); +FWD_DECLARE(IXAudio2EngineCallback); +FWD_DECLARE(IXAudio2VoiceCallback); + + +/************************************************************************** + * + * XAudio2 structures and enumerations. + * + **************************************************************************/ + +// Used in IXAudio2::Initialize +#ifdef _XBOX + typedef enum XAUDIO2_XBOX_HWTHREAD_SPECIFIER + { + XboxThread0 = 0x01, + XboxThread1 = 0x02, + XboxThread2 = 0x04, + XboxThread3 = 0x08, + XboxThread4 = 0x10, + XboxThread5 = 0x20, + XAUDIO2_ANY_PROCESSOR = XboxThread4, + XAUDIO2_DEFAULT_PROCESSOR = XAUDIO2_ANY_PROCESSOR + } XAUDIO2_XBOX_HWTHREAD_SPECIFIER, XAUDIO2_PROCESSOR; +#else + typedef enum XAUDIO2_WINDOWS_PROCESSOR_SPECIFIER + { + Processor1 = 0x00000001, + Processor2 = 0x00000002, + Processor3 = 0x00000004, + Processor4 = 0x00000008, + Processor5 = 0x00000010, + Processor6 = 0x00000020, + Processor7 = 0x00000040, + Processor8 = 0x00000080, + Processor9 = 0x00000100, + Processor10 = 0x00000200, + Processor11 = 0x00000400, + Processor12 = 0x00000800, + Processor13 = 0x00001000, + Processor14 = 0x00002000, + Processor15 = 0x00004000, + Processor16 = 0x00008000, + Processor17 = 0x00010000, + Processor18 = 0x00020000, + Processor19 = 0x00040000, + Processor20 = 0x00080000, + Processor21 = 0x00100000, + Processor22 = 0x00200000, + Processor23 = 0x00400000, + Processor24 = 0x00800000, + Processor25 = 0x01000000, + Processor26 = 0x02000000, + Processor27 = 0x04000000, + Processor28 = 0x08000000, + Processor29 = 0x10000000, + Processor30 = 0x20000000, + Processor31 = 0x40000000, + Processor32 = 0x80000000, + XAUDIO2_ANY_PROCESSOR = 0xffffffff, + XAUDIO2_DEFAULT_PROCESSOR = XAUDIO2_ANY_PROCESSOR + } XAUDIO2_WINDOWS_PROCESSOR_SPECIFIER, XAUDIO2_PROCESSOR; +#endif + +// Used in XAUDIO2_DEVICE_DETAILS below to describe the types of applications +// that the user has specified each device as a default for. 0 means that the +// device isn't the default for any role. +typedef enum XAUDIO2_DEVICE_ROLE +{ + NotDefaultDevice = 0x0, + DefaultConsoleDevice = 0x1, + DefaultMultimediaDevice = 0x2, + DefaultCommunicationsDevice = 0x4, + DefaultGameDevice = 0x8, + GlobalDefaultDevice = 0xf, + InvalidDeviceRole = ~GlobalDefaultDevice +} XAUDIO2_DEVICE_ROLE; + +// Returned by IXAudio2::GetDeviceDetails +typedef struct XAUDIO2_DEVICE_DETAILS +{ + WCHAR DeviceID[256]; // String identifier for the audio device. + WCHAR DisplayName[256]; // Friendly name suitable for display to a human. + XAUDIO2_DEVICE_ROLE Role; // Roles that the device should be used for. + WAVEFORMATEXTENSIBLE OutputFormat; // The device's native PCM audio output format. +} XAUDIO2_DEVICE_DETAILS; + +// Returned by IXAudio2Voice::GetVoiceDetails +typedef struct XAUDIO2_VOICE_DETAILS +{ + UINT32 CreationFlags; // Flags the voice was created with. + UINT32 InputChannels; // Channels in the voice's input audio. + UINT32 InputSampleRate; // Sample rate of the voice's input audio. +} XAUDIO2_VOICE_DETAILS; + +// Used in XAUDIO2_VOICE_SENDS below +typedef struct XAUDIO2_SEND_DESCRIPTOR +{ + UINT32 Flags; // Either 0 or XAUDIO2_SEND_USEFILTER. + IXAudio2Voice* pOutputVoice; // This send's destination voice. +} XAUDIO2_SEND_DESCRIPTOR; + +// Used in the voice creation functions and in IXAudio2Voice::SetOutputVoices +typedef struct XAUDIO2_VOICE_SENDS +{ + UINT32 SendCount; // Number of sends from this voice. + XAUDIO2_SEND_DESCRIPTOR* pSends; // Array of SendCount send descriptors. +} XAUDIO2_VOICE_SENDS; + +// Used in XAUDIO2_EFFECT_CHAIN below +typedef struct XAUDIO2_EFFECT_DESCRIPTOR +{ + IUnknown* pEffect; // Pointer to the effect object's IUnknown interface. + BOOL InitialState; // TRUE if the effect should begin in the enabled state. + UINT32 OutputChannels; // How many output channels the effect should produce. +} XAUDIO2_EFFECT_DESCRIPTOR; + +// Used in the voice creation functions and in IXAudio2Voice::SetEffectChain +typedef struct XAUDIO2_EFFECT_CHAIN +{ + UINT32 EffectCount; // Number of effects in this voice's effect chain. + XAUDIO2_EFFECT_DESCRIPTOR* pEffectDescriptors; // Array of effect descriptors. +} XAUDIO2_EFFECT_CHAIN; + +// Used in XAUDIO2_FILTER_PARAMETERS below +typedef enum XAUDIO2_FILTER_TYPE +{ + LowPassFilter, // Attenuates frequencies above the cutoff frequency. + BandPassFilter, // Attenuates frequencies outside a given range. + HighPassFilter, // Attenuates frequencies below the cutoff frequency. + NotchFilter // Attenuates frequencies inside a given range. +} XAUDIO2_FILTER_TYPE; + +// Used in IXAudio2Voice::Set/GetFilterParameters and Set/GetOutputFilterParameters +typedef struct XAUDIO2_FILTER_PARAMETERS +{ + XAUDIO2_FILTER_TYPE Type; // Low-pass, band-pass or high-pass. + float Frequency; // Radian frequency (2 * sin(pi*CutoffFrequency/SampleRate)); + // must be >= 0 and <= XAUDIO2_MAX_FILTER_FREQUENCY + // (giving a maximum CutoffFrequency of SampleRate/6). + float OneOverQ; // Reciprocal of the filter's quality factor Q; + // must be > 0 and <= XAUDIO2_MAX_FILTER_ONEOVERQ. +} XAUDIO2_FILTER_PARAMETERS; + +// Used in IXAudio2SourceVoice::SubmitSourceBuffer +typedef struct XAUDIO2_BUFFER +{ + UINT32 Flags; // Either 0 or XAUDIO2_END_OF_STREAM. + UINT32 AudioBytes; // Size of the audio data buffer in bytes. + const BYTE* pAudioData; // Pointer to the audio data buffer. + UINT32 PlayBegin; // First sample in this buffer to be played. + UINT32 PlayLength; // Length of the region to be played in samples, + // or 0 to play the whole buffer. + UINT32 LoopBegin; // First sample of the region to be looped. + UINT32 LoopLength; // Length of the desired loop region in samples, + // or 0 to loop the entire buffer. + UINT32 LoopCount; // Number of times to repeat the loop region, + // or XAUDIO2_LOOP_INFINITE to loop forever. + void* pContext; // Context value to be passed back in callbacks. +} XAUDIO2_BUFFER; + +// Used in IXAudio2SourceVoice::SubmitSourceBuffer when submitting XWMA data. +// NOTE: If an XWMA sound is submitted in more than one buffer, each buffer's +// pDecodedPacketCumulativeBytes[PacketCount-1] value must be subtracted from +// all the entries in the next buffer's pDecodedPacketCumulativeBytes array. +// And whether a sound is submitted in more than one buffer or not, the final +// buffer of the sound should use the XAUDIO2_END_OF_STREAM flag, or else the +// client must call IXAudio2SourceVoice::Discontinuity after submitting it. +typedef struct XAUDIO2_BUFFER_WMA +{ + const UINT32* pDecodedPacketCumulativeBytes; // Decoded packet's cumulative size array. + // Each element is the number of bytes accumulated + // when the corresponding XWMA packet is decoded in + // order. The array must have PacketCount elements. + UINT32 PacketCount; // Number of XWMA packets submitted. Must be >= 1 and + // divide evenly into XAUDIO2_BUFFER.AudioBytes. +} XAUDIO2_BUFFER_WMA; + +// Returned by IXAudio2SourceVoice::GetState +typedef struct XAUDIO2_VOICE_STATE +{ + void* pCurrentBufferContext; // The pContext value provided in the XAUDIO2_BUFFER + // that is currently being processed, or NULL if + // there are no buffers in the queue. + UINT32 BuffersQueued; // Number of buffers currently queued on the voice + // (including the one that is being processed). + UINT64 SamplesPlayed; // Total number of samples produced by the voice since + // it began processing the current audio stream. +} XAUDIO2_VOICE_STATE; + +// Returned by IXAudio2::GetPerformanceData +typedef struct XAUDIO2_PERFORMANCE_DATA +{ + // CPU usage information + UINT64 AudioCyclesSinceLastQuery; // CPU cycles spent on audio processing since the + // last call to StartEngine or GetPerformanceData. + UINT64 TotalCyclesSinceLastQuery; // Total CPU cycles elapsed since the last call + // (only counts the CPU XAudio2 is running on). + UINT32 MinimumCyclesPerQuantum; // Fewest CPU cycles spent processing any one + // audio quantum since the last call. + UINT32 MaximumCyclesPerQuantum; // Most CPU cycles spent processing any one + // audio quantum since the last call. + + // Memory usage information + UINT32 MemoryUsageInBytes; // Total heap space currently in use. + + // Audio latency and glitching information + UINT32 CurrentLatencyInSamples; // Minimum delay from when a sample is read from a + // source buffer to when it reaches the speakers. + UINT32 GlitchesSinceEngineStarted; // Audio dropouts since the engine was started. + + // Data about XAudio2's current workload + UINT32 ActiveSourceVoiceCount; // Source voices currently playing. + UINT32 TotalSourceVoiceCount; // Source voices currently existing. + UINT32 ActiveSubmixVoiceCount; // Submix voices currently playing/existing. + + UINT32 ActiveResamplerCount; // Resample xAPOs currently active. + UINT32 ActiveMatrixMixCount; // MatrixMix xAPOs currently active. + + // Usage of the hardware XMA decoder (Xbox 360 only) + UINT32 ActiveXmaSourceVoices; // Number of source voices decoding XMA data. + UINT32 ActiveXmaStreams; // A voice can use more than one XMA stream. +} XAUDIO2_PERFORMANCE_DATA; + +// Used in IXAudio2::SetDebugConfiguration +typedef struct XAUDIO2_DEBUG_CONFIGURATION +{ + UINT32 TraceMask; // Bitmap of enabled debug message types. + UINT32 BreakMask; // Message types that will break into the debugger. + BOOL LogThreadID; // Whether to log the thread ID with each message. + BOOL LogFileline; // Whether to log the source file and line number. + BOOL LogFunctionName; // Whether to log the function name. + BOOL LogTiming; // Whether to log message timestamps. +} XAUDIO2_DEBUG_CONFIGURATION; + +// Values for the TraceMask and BreakMask bitmaps. Only ERRORS and WARNINGS +// are valid in BreakMask. WARNINGS implies ERRORS, DETAIL implies INFO, and +// FUNC_CALLS implies API_CALLS. By default, TraceMask is ERRORS and WARNINGS +// and all the other settings are zero. +#define XAUDIO2_LOG_ERRORS 0x0001 // For handled errors with serious effects. +#define XAUDIO2_LOG_WARNINGS 0x0002 // For handled errors that may be recoverable. +#define XAUDIO2_LOG_INFO 0x0004 // Informational chit-chat (e.g. state changes). +#define XAUDIO2_LOG_DETAIL 0x0008 // More detailed chit-chat. +#define XAUDIO2_LOG_API_CALLS 0x0010 // Public API function entries and exits. +#define XAUDIO2_LOG_FUNC_CALLS 0x0020 // Internal function entries and exits. +#define XAUDIO2_LOG_TIMING 0x0040 // Delays detected and other timing data. +#define XAUDIO2_LOG_LOCKS 0x0080 // Usage of critical sections and mutexes. +#define XAUDIO2_LOG_MEMORY 0x0100 // Memory heap usage information. +#define XAUDIO2_LOG_STREAMING 0x1000 // Audio streaming information. + + +/************************************************************************** + * + * IXAudio2: Top-level XAudio2 COM interface. + * + **************************************************************************/ + +// Use default arguments if compiling as C++ +#ifdef __cplusplus + #define X2DEFAULT(x) =x +#else + #define X2DEFAULT(x) +#endif + +#undef INTERFACE +#define INTERFACE IXAudio2 +DECLARE_INTERFACE_(IXAudio2, IUnknown) +{ + // NAME: IXAudio2::QueryInterface + // DESCRIPTION: Queries for a given COM interface on the XAudio2 object. + // Only IID_IUnknown and IID_IXAudio2 are supported. + // + // ARGUMENTS: + // riid - IID of the interface to be obtained. + // ppvInterface - Returns a pointer to the requested interface. + // + STDMETHOD(QueryInterface) (THIS_ REFIID riid, __deref_out void** ppvInterface) PURE; + + // NAME: IXAudio2::AddRef + // DESCRIPTION: Adds a reference to the XAudio2 object. + // + STDMETHOD_(ULONG, AddRef) (THIS) PURE; + + // NAME: IXAudio2::Release + // DESCRIPTION: Releases a reference to the XAudio2 object. + // + STDMETHOD_(ULONG, Release) (THIS) PURE; + + // NAME: IXAudio2::GetDeviceCount + // DESCRIPTION: Returns the number of audio output devices available. + // + // ARGUMENTS: + // pCount - Returns the device count. + // + STDMETHOD(GetDeviceCount) (THIS_ __out UINT32* pCount) PURE; + + // NAME: IXAudio2::GetDeviceDetails + // DESCRIPTION: Returns information about the device with the given index. + // + // ARGUMENTS: + // Index - Index of the device to be queried. + // pDeviceDetails - Returns the device details. + // + STDMETHOD(GetDeviceDetails) (THIS_ UINT32 Index, __out XAUDIO2_DEVICE_DETAILS* pDeviceDetails) PURE; + + // NAME: IXAudio2::Initialize + // DESCRIPTION: Sets global XAudio2 parameters and prepares it for use. + // + // ARGUMENTS: + // Flags - Flags specifying the XAudio2 object's behavior. Currently unused. + // XAudio2Processor - An XAUDIO2_PROCESSOR enumeration value that specifies + // the hardware thread (Xbox) or processor (Windows) that XAudio2 will use. + // The enumeration values are platform-specific; platform-independent code + // can use XAUDIO2_DEFAULT_PROCESSOR to use the default on each platform. + // + STDMETHOD(Initialize) (THIS_ UINT32 Flags X2DEFAULT(0), + XAUDIO2_PROCESSOR XAudio2Processor X2DEFAULT(XAUDIO2_DEFAULT_PROCESSOR)) PURE; + + // NAME: IXAudio2::RegisterForCallbacks + // DESCRIPTION: Adds a new client to receive XAudio2's engine callbacks. + // + // ARGUMENTS: + // pCallback - Callback interface to be called during each processing pass. + // + STDMETHOD(RegisterForCallbacks) (__in IXAudio2EngineCallback* pCallback) PURE; + + // NAME: IXAudio2::UnregisterForCallbacks + // DESCRIPTION: Removes an existing receiver of XAudio2 engine callbacks. + // + // ARGUMENTS: + // pCallback - Previously registered callback interface to be removed. + // + STDMETHOD_(void, UnregisterForCallbacks) (__in IXAudio2EngineCallback* pCallback) PURE; + + // NAME: IXAudio2::CreateSourceVoice + // DESCRIPTION: Creates and configures a source voice. + // + // ARGUMENTS: + // ppSourceVoice - Returns the new object's IXAudio2SourceVoice interface. + // pSourceFormat - Format of the audio that will be fed to the voice. + // Flags - XAUDIO2_VOICE flags specifying the source voice's behavior. + // MaxFrequencyRatio - Maximum SetFrequencyRatio argument to be allowed. + // pCallback - Optional pointer to a client-provided callback interface. + // pSendList - Optional list of voices this voice should send audio to. + // pEffectChain - Optional list of effects to apply to the audio data. + // + STDMETHOD(CreateSourceVoice) (THIS_ __deref_out IXAudio2SourceVoice** ppSourceVoice, + __in const WAVEFORMATEX* pSourceFormat, + UINT32 Flags X2DEFAULT(0), + float MaxFrequencyRatio X2DEFAULT(XAUDIO2_DEFAULT_FREQ_RATIO), + __in_opt IXAudio2VoiceCallback* pCallback X2DEFAULT(NULL), + __in_opt const XAUDIO2_VOICE_SENDS* pSendList X2DEFAULT(NULL), + __in_opt const XAUDIO2_EFFECT_CHAIN* pEffectChain X2DEFAULT(NULL)) PURE; + + // NAME: IXAudio2::CreateSubmixVoice + // DESCRIPTION: Creates and configures a submix voice. + // + // ARGUMENTS: + // ppSubmixVoice - Returns the new object's IXAudio2SubmixVoice interface. + // InputChannels - Number of channels in this voice's input audio data. + // InputSampleRate - Sample rate of this voice's input audio data. + // Flags - XAUDIO2_VOICE flags specifying the submix voice's behavior. + // ProcessingStage - Arbitrary number that determines the processing order. + // pSendList - Optional list of voices this voice should send audio to. + // pEffectChain - Optional list of effects to apply to the audio data. + // + STDMETHOD(CreateSubmixVoice) (THIS_ __deref_out IXAudio2SubmixVoice** ppSubmixVoice, + UINT32 InputChannels, UINT32 InputSampleRate, + UINT32 Flags X2DEFAULT(0), UINT32 ProcessingStage X2DEFAULT(0), + __in_opt const XAUDIO2_VOICE_SENDS* pSendList X2DEFAULT(NULL), + __in_opt const XAUDIO2_EFFECT_CHAIN* pEffectChain X2DEFAULT(NULL)) PURE; + + + // NAME: IXAudio2::CreateMasteringVoice + // DESCRIPTION: Creates and configures a mastering voice. + // + // ARGUMENTS: + // ppMasteringVoice - Returns the new object's IXAudio2MasteringVoice interface. + // InputChannels - Number of channels in this voice's input audio data. + // InputSampleRate - Sample rate of this voice's input audio data. + // Flags - XAUDIO2_VOICE flags specifying the mastering voice's behavior. + // DeviceIndex - Identifier of the device to receive the output audio. + // pEffectChain - Optional list of effects to apply to the audio data. + // + STDMETHOD(CreateMasteringVoice) (THIS_ __deref_out IXAudio2MasteringVoice** ppMasteringVoice, + UINT32 InputChannels X2DEFAULT(XAUDIO2_DEFAULT_CHANNELS), + UINT32 InputSampleRate X2DEFAULT(XAUDIO2_DEFAULT_SAMPLERATE), + UINT32 Flags X2DEFAULT(0), UINT32 DeviceIndex X2DEFAULT(0), + __in_opt const XAUDIO2_EFFECT_CHAIN* pEffectChain X2DEFAULT(NULL)) PURE; + + // NAME: IXAudio2::StartEngine + // DESCRIPTION: Creates and starts the audio processing thread. + // + STDMETHOD(StartEngine) (THIS) PURE; + + // NAME: IXAudio2::StopEngine + // DESCRIPTION: Stops and destroys the audio processing thread. + // + STDMETHOD_(void, StopEngine) (THIS) PURE; + + // NAME: IXAudio2::CommitChanges + // DESCRIPTION: Atomically applies a set of operations previously tagged + // with a given identifier. + // + // ARGUMENTS: + // OperationSet - Identifier of the set of operations to be applied. + // + STDMETHOD(CommitChanges) (THIS_ UINT32 OperationSet) PURE; + + // NAME: IXAudio2::GetPerformanceData + // DESCRIPTION: Returns current resource usage details: memory, CPU, etc. + // + // ARGUMENTS: + // pPerfData - Returns the performance data structure. + // + STDMETHOD_(void, GetPerformanceData) (THIS_ __out XAUDIO2_PERFORMANCE_DATA* pPerfData) PURE; + + // NAME: IXAudio2::SetDebugConfiguration + // DESCRIPTION: Configures XAudio2's debug output (in debug builds only). + // + // ARGUMENTS: + // pDebugConfiguration - Structure describing the debug output behavior. + // pReserved - Optional parameter; must be NULL. + // + STDMETHOD_(void, SetDebugConfiguration) (THIS_ __in_opt const XAUDIO2_DEBUG_CONFIGURATION* pDebugConfiguration, + __in_opt __reserved void* pReserved X2DEFAULT(NULL)) PURE; +}; + + +/************************************************************************** + * + * IXAudio2Voice: Base voice management interface. + * + **************************************************************************/ + +#undef INTERFACE +#define INTERFACE IXAudio2Voice +DECLARE_INTERFACE(IXAudio2Voice) +{ + // These methods are declared in a macro so that the same declarations + // can be used in the derived voice types (IXAudio2SourceVoice, etc). + + #define Declare_IXAudio2Voice_Methods() \ + \ + /* NAME: IXAudio2Voice::GetVoiceDetails + // DESCRIPTION: Returns the basic characteristics of this voice. + // + // ARGUMENTS: + // pVoiceDetails - Returns the voice's details. + */\ + STDMETHOD_(void, GetVoiceDetails) (THIS_ __out XAUDIO2_VOICE_DETAILS* pVoiceDetails) PURE; \ + \ + /* NAME: IXAudio2Voice::SetOutputVoices + // DESCRIPTION: Replaces the set of submix/mastering voices that receive + // this voice's output. + // + // ARGUMENTS: + // pSendList - Optional list of voices this voice should send audio to. + */\ + STDMETHOD(SetOutputVoices) (THIS_ __in_opt const XAUDIO2_VOICE_SENDS* pSendList) PURE; \ + \ + /* NAME: IXAudio2Voice::SetEffectChain + // DESCRIPTION: Replaces this voice's current effect chain with a new one. + // + // ARGUMENTS: + // pEffectChain - Structure describing the new effect chain to be used. + */\ + STDMETHOD(SetEffectChain) (THIS_ __in_opt const XAUDIO2_EFFECT_CHAIN* pEffectChain) PURE; \ + \ + /* NAME: IXAudio2Voice::EnableEffect + // DESCRIPTION: Enables an effect in this voice's effect chain. + // + // ARGUMENTS: + // EffectIndex - Index of an effect within this voice's effect chain. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(EnableEffect) (THIS_ UINT32 EffectIndex, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::DisableEffect + // DESCRIPTION: Disables an effect in this voice's effect chain. + // + // ARGUMENTS: + // EffectIndex - Index of an effect within this voice's effect chain. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(DisableEffect) (THIS_ UINT32 EffectIndex, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::GetEffectState + // DESCRIPTION: Returns the running state of an effect. + // + // ARGUMENTS: + // EffectIndex - Index of an effect within this voice's effect chain. + // pEnabled - Returns the enabled/disabled state of the given effect. + */\ + STDMETHOD_(void, GetEffectState) (THIS_ UINT32 EffectIndex, __out BOOL* pEnabled) PURE; \ + \ + /* NAME: IXAudio2Voice::SetEffectParameters + // DESCRIPTION: Sets effect-specific parameters. + // + // REMARKS: Unlike IXAPOParameters::SetParameters, this method may + // be called from any thread. XAudio2 implements + // appropriate synchronization to copy the parameters to the + // realtime audio processing thread. + // + // ARGUMENTS: + // EffectIndex - Index of an effect within this voice's effect chain. + // pParameters - Pointer to an effect-specific parameters block. + // ParametersByteSize - Size of the pParameters array in bytes. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(SetEffectParameters) (THIS_ UINT32 EffectIndex, \ + __in_bcount(ParametersByteSize) const void* pParameters, \ + UINT32 ParametersByteSize, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::GetEffectParameters + // DESCRIPTION: Obtains the current effect-specific parameters. + // + // ARGUMENTS: + // EffectIndex - Index of an effect within this voice's effect chain. + // pParameters - Returns the current values of the effect-specific parameters. + // ParametersByteSize - Size of the pParameters array in bytes. + */\ + STDMETHOD(GetEffectParameters) (THIS_ UINT32 EffectIndex, \ + __out_bcount(ParametersByteSize) void* pParameters, \ + UINT32 ParametersByteSize) PURE; \ + \ + /* NAME: IXAudio2Voice::SetFilterParameters + // DESCRIPTION: Sets this voice's filter parameters. + // + // ARGUMENTS: + // pParameters - Pointer to the filter's parameter structure. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(SetFilterParameters) (THIS_ __in const XAUDIO2_FILTER_PARAMETERS* pParameters, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::GetFilterParameters + // DESCRIPTION: Returns this voice's current filter parameters. + // + // ARGUMENTS: + // pParameters - Returns the filter parameters. + */\ + STDMETHOD_(void, GetFilterParameters) (THIS_ __out XAUDIO2_FILTER_PARAMETERS* pParameters) PURE; \ + \ + /* NAME: IXAudio2Voice::SetOutputFilterParameters + // DESCRIPTION: Sets the filter parameters on one of this voice's sends. + // + // ARGUMENTS: + // pDestinationVoice - Destination voice of the send whose filter parameters will be set. + // pParameters - Pointer to the filter's parameter structure. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(SetOutputFilterParameters) (THIS_ __in_opt IXAudio2Voice* pDestinationVoice, \ + __in const XAUDIO2_FILTER_PARAMETERS* pParameters, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::GetOutputFilterParameters + // DESCRIPTION: Returns the filter parameters from one of this voice's sends. + // + // ARGUMENTS: + // pDestinationVoice - Destination voice of the send whose filter parameters will be read. + // pParameters - Returns the filter parameters. + */\ + STDMETHOD_(void, GetOutputFilterParameters) (THIS_ __in_opt IXAudio2Voice* pDestinationVoice, \ + __out XAUDIO2_FILTER_PARAMETERS* pParameters) PURE; \ + \ + /* NAME: IXAudio2Voice::SetVolume + // DESCRIPTION: Sets this voice's overall volume level. + // + // ARGUMENTS: + // Volume - New overall volume level to be used, as an amplitude factor. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(SetVolume) (THIS_ float Volume, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::GetVolume + // DESCRIPTION: Obtains this voice's current overall volume level. + // + // ARGUMENTS: + // pVolume: Returns the voice's current overall volume level. + */\ + STDMETHOD_(void, GetVolume) (THIS_ __out float* pVolume) PURE; \ + \ + /* NAME: IXAudio2Voice::SetChannelVolumes + // DESCRIPTION: Sets this voice's per-channel volume levels. + // + // ARGUMENTS: + // Channels - Used to confirm the voice's channel count. + // pVolumes - Array of per-channel volume levels to be used. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(SetChannelVolumes) (THIS_ UINT32 Channels, __in_ecount(Channels) const float* pVolumes, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::GetChannelVolumes + // DESCRIPTION: Returns this voice's current per-channel volume levels. + // + // ARGUMENTS: + // Channels - Used to confirm the voice's channel count. + // pVolumes - Returns an array of the current per-channel volume levels. + */\ + STDMETHOD_(void, GetChannelVolumes) (THIS_ UINT32 Channels, __out_ecount(Channels) float* pVolumes) PURE; \ + \ + /* NAME: IXAudio2Voice::SetOutputMatrix + // DESCRIPTION: Sets the volume levels used to mix from each channel of this + // voice's output audio to each channel of a given destination + // voice's input audio. + // + // ARGUMENTS: + // pDestinationVoice - The destination voice whose mix matrix to change. + // SourceChannels - Used to confirm this voice's output channel count + // (the number of channels produced by the last effect in the chain). + // DestinationChannels - Confirms the destination voice's input channels. + // pLevelMatrix - Array of [SourceChannels * DestinationChannels] send + // levels. The level used to send from source channel S to destination + // channel D should be in pLevelMatrix[S + SourceChannels * D]. + // OperationSet - Used to identify this call as part of a deferred batch. + */\ + STDMETHOD(SetOutputMatrix) (THIS_ __in_opt IXAudio2Voice* pDestinationVoice, \ + UINT32 SourceChannels, UINT32 DestinationChannels, \ + __in_ecount(SourceChannels * DestinationChannels) const float* pLevelMatrix, \ + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; \ + \ + /* NAME: IXAudio2Voice::GetOutputMatrix + // DESCRIPTION: Obtains the volume levels used to send each channel of this + // voice's output audio to each channel of a given destination + // voice's input audio. + // + // ARGUMENTS: + // pDestinationVoice - The destination voice whose mix matrix to obtain. + // SourceChannels - Used to confirm this voice's output channel count + // (the number of channels produced by the last effect in the chain). + // DestinationChannels - Confirms the destination voice's input channels. + // pLevelMatrix - Array of send levels, as above. + */\ + STDMETHOD_(void, GetOutputMatrix) (THIS_ __in_opt IXAudio2Voice* pDestinationVoice, \ + UINT32 SourceChannels, UINT32 DestinationChannels, \ + __out_ecount(SourceChannels * DestinationChannels) float* pLevelMatrix) PURE; \ + \ + /* NAME: IXAudio2Voice::DestroyVoice + // DESCRIPTION: Destroys this voice, stopping it if necessary and removing + // it from the XAudio2 graph. + */\ + STDMETHOD_(void, DestroyVoice) (THIS) PURE + + Declare_IXAudio2Voice_Methods(); +}; + + +/************************************************************************** + * + * IXAudio2SourceVoice: Source voice management interface. + * + **************************************************************************/ + +#undef INTERFACE +#define INTERFACE IXAudio2SourceVoice +DECLARE_INTERFACE_(IXAudio2SourceVoice, IXAudio2Voice) +{ + // Methods from IXAudio2Voice base interface + Declare_IXAudio2Voice_Methods(); + + // NAME: IXAudio2SourceVoice::Start + // DESCRIPTION: Makes this voice start consuming and processing audio. + // + // ARGUMENTS: + // Flags - Flags controlling how the voice should be started. + // OperationSet - Used to identify this call as part of a deferred batch. + // + STDMETHOD(Start) (THIS_ UINT32 Flags X2DEFAULT(0), UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; + + // NAME: IXAudio2SourceVoice::Stop + // DESCRIPTION: Makes this voice stop consuming audio. + // + // ARGUMENTS: + // Flags - Flags controlling how the voice should be stopped. + // OperationSet - Used to identify this call as part of a deferred batch. + // + STDMETHOD(Stop) (THIS_ UINT32 Flags X2DEFAULT(0), UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; + + // NAME: IXAudio2SourceVoice::SubmitSourceBuffer + // DESCRIPTION: Adds a new audio buffer to this voice's input queue. + // + // ARGUMENTS: + // pBuffer - Pointer to the buffer structure to be queued. + // pBufferWMA - Additional structure used only when submitting XWMA data. + // + STDMETHOD(SubmitSourceBuffer) (THIS_ __in const XAUDIO2_BUFFER* pBuffer, __in_opt const XAUDIO2_BUFFER_WMA* pBufferWMA X2DEFAULT(NULL)) PURE; + + // NAME: IXAudio2SourceVoice::FlushSourceBuffers + // DESCRIPTION: Removes all pending audio buffers from this voice's queue. + // + STDMETHOD(FlushSourceBuffers) (THIS) PURE; + + // NAME: IXAudio2SourceVoice::Discontinuity + // DESCRIPTION: Notifies the voice of an intentional break in the stream of + // audio buffers (e.g. the end of a sound), to prevent XAudio2 + // from interpreting an empty buffer queue as a glitch. + // + STDMETHOD(Discontinuity) (THIS) PURE; + + // NAME: IXAudio2SourceVoice::ExitLoop + // DESCRIPTION: Breaks out of the current loop when its end is reached. + // + // ARGUMENTS: + // OperationSet - Used to identify this call as part of a deferred batch. + // + STDMETHOD(ExitLoop) (THIS_ UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; + + // NAME: IXAudio2SourceVoice::GetState + // DESCRIPTION: Returns the number of buffers currently queued on this voice, + // the pContext value associated with the currently processing + // buffer (if any), and other voice state information. + // + // ARGUMENTS: + // pVoiceState - Returns the state information. + // + STDMETHOD_(void, GetState) (THIS_ __out XAUDIO2_VOICE_STATE* pVoiceState) PURE; + + // NAME: IXAudio2SourceVoice::SetFrequencyRatio + // DESCRIPTION: Sets this voice's frequency adjustment, i.e. its pitch. + // + // ARGUMENTS: + // Ratio - Frequency change, expressed as source frequency / target frequency. + // OperationSet - Used to identify this call as part of a deferred batch. + // + STDMETHOD(SetFrequencyRatio) (THIS_ float Ratio, + UINT32 OperationSet X2DEFAULT(XAUDIO2_COMMIT_NOW)) PURE; + + // NAME: IXAudio2SourceVoice::GetFrequencyRatio + // DESCRIPTION: Returns this voice's current frequency adjustment ratio. + // + // ARGUMENTS: + // pRatio - Returns the frequency adjustment. + // + STDMETHOD_(void, GetFrequencyRatio) (THIS_ __out float* pRatio) PURE; + + // NAME: IXAudio2SourceVoice::SetSourceSampleRate + // DESCRIPTION: Reconfigures this voice to treat its source data as being + // at a different sample rate than the original one specified + // in CreateSourceVoice's pSourceFormat argument. + // + // ARGUMENTS: + // UINT32 - The intended sample rate of further submitted source data. + // + STDMETHOD(SetSourceSampleRate) (THIS_ UINT32 NewSourceSampleRate) PURE; +}; + + +/************************************************************************** + * + * IXAudio2SubmixVoice: Submixing voice management interface. + * + **************************************************************************/ + +#undef INTERFACE +#define INTERFACE IXAudio2SubmixVoice +DECLARE_INTERFACE_(IXAudio2SubmixVoice, IXAudio2Voice) +{ + // Methods from IXAudio2Voice base interface + Declare_IXAudio2Voice_Methods(); + + // There are currently no methods specific to submix voices. +}; + + +/************************************************************************** + * + * IXAudio2MasteringVoice: Mastering voice management interface. + * + **************************************************************************/ + +#undef INTERFACE +#define INTERFACE IXAudio2MasteringVoice +DECLARE_INTERFACE_(IXAudio2MasteringVoice, IXAudio2Voice) +{ + // Methods from IXAudio2Voice base interface + Declare_IXAudio2Voice_Methods(); + + // There are currently no methods specific to mastering voices. +}; + + +/************************************************************************** + * + * IXAudio2EngineCallback: Client notification interface for engine events. + * + * REMARKS: Contains methods to notify the client when certain events happen + * in the XAudio2 engine. This interface should be implemented by + * the client. XAudio2 will call these methods via the interface + * pointer provided by the client when it calls XAudio2Create or + * IXAudio2::Initialize. + * + **************************************************************************/ + +#undef INTERFACE +#define INTERFACE IXAudio2EngineCallback +DECLARE_INTERFACE(IXAudio2EngineCallback) +{ + // Called by XAudio2 just before an audio processing pass begins. + STDMETHOD_(void, OnProcessingPassStart) (THIS) PURE; + + // Called just after an audio processing pass ends. + STDMETHOD_(void, OnProcessingPassEnd) (THIS) PURE; + + // Called in the event of a critical system error which requires XAudio2 + // to be closed down and restarted. The error code is given in Error. + STDMETHOD_(void, OnCriticalError) (THIS_ HRESULT Error) PURE; +}; + + +/************************************************************************** + * + * IXAudio2VoiceCallback: Client notification interface for voice events. + * + * REMARKS: Contains methods to notify the client when certain events happen + * in an XAudio2 voice. This interface should be implemented by the + * client. XAudio2 will call these methods via an interface pointer + * provided by the client in the IXAudio2::CreateSourceVoice call. + * + **************************************************************************/ + +#undef INTERFACE +#define INTERFACE IXAudio2VoiceCallback +DECLARE_INTERFACE(IXAudio2VoiceCallback) +{ + // Called just before this voice's processing pass begins. + STDMETHOD_(void, OnVoiceProcessingPassStart) (THIS_ UINT32 BytesRequired) PURE; + + // Called just after this voice's processing pass ends. + STDMETHOD_(void, OnVoiceProcessingPassEnd) (THIS) PURE; + + // Called when this voice has just finished playing a buffer stream + // (as marked with the XAUDIO2_END_OF_STREAM flag on the last buffer). + STDMETHOD_(void, OnStreamEnd) (THIS) PURE; + + // Called when this voice is about to start processing a new buffer. + STDMETHOD_(void, OnBufferStart) (THIS_ void* pBufferContext) PURE; + + // Called when this voice has just finished processing a buffer. + // The buffer can now be reused or destroyed. + STDMETHOD_(void, OnBufferEnd) (THIS_ void* pBufferContext) PURE; + + // Called when this voice has just reached the end position of a loop. + STDMETHOD_(void, OnLoopEnd) (THIS_ void* pBufferContext) PURE; + + // Called in the event of a critical error during voice processing, + // such as a failing xAPO or an error from the hardware XMA decoder. + // The voice may have to be destroyed and re-created to recover from + // the error. The callback arguments report which buffer was being + // processed when the error occurred, and its HRESULT code. + STDMETHOD_(void, OnVoiceError) (THIS_ void* pBufferContext, HRESULT Error) PURE; +}; + + +/************************************************************************** + * + * Macros to make it easier to use the XAudio2 COM interfaces in C code. + * + **************************************************************************/ + +#ifndef __cplusplus + +// IXAudio2 +#define IXAudio2_QueryInterface(This,riid,ppvInterface) ((This)->lpVtbl->QueryInterface(This,riid,ppvInterface)) +#define IXAudio2_AddRef(This) ((This)->lpVtbl->AddRef(This)) +#define IXAudio2_Release(This) ((This)->lpVtbl->Release(This)) +#define IXAudio2_GetDeviceCount(This,puCount) ((This)->lpVtbl->GetDeviceCount(This,puCount)) +#define IXAudio2_GetDeviceDetails(This,Index,pDeviceDetails) ((This)->lpVtbl->GetDeviceDetails(This,Index,pDeviceDetails)) +#define IXAudio2_Initialize(This,Flags,XAudio2Processor) ((This)->lpVtbl->Initialize(This,Flags,XAudio2Processor)) +#define IXAudio2_CreateSourceVoice(This,ppSourceVoice,pSourceFormat,Flags,MaxFrequencyRatio,pCallback,pSendList,pEffectChain) ((This)->lpVtbl->CreateSourceVoice(This,ppSourceVoice,pSourceFormat,Flags,MaxFrequencyRatio,pCallback,pSendList,pEffectChain)) +#define IXAudio2_CreateSubmixVoice(This,ppSubmixVoice,InputChannels,InputSampleRate,Flags,ProcessingStage,pSendList,pEffectChain) ((This)->lpVtbl->CreateSubmixVoice(This,ppSubmixVoice,InputChannels,InputSampleRate,Flags,ProcessingStage,pSendList,pEffectChain)) +#define IXAudio2_CreateMasteringVoice(This,ppMasteringVoice,InputChannels,InputSampleRate,Flags,DeviceIndex,pEffectChain) ((This)->lpVtbl->CreateMasteringVoice(This,ppMasteringVoice,InputChannels,InputSampleRate,Flags,DeviceIndex,pEffectChain)) +#define IXAudio2_StartEngine(This) ((This)->lpVtbl->StartEngine(This)) +#define IXAudio2_StopEngine(This) ((This)->lpVtbl->StopEngine(This)) +#define IXAudio2_CommitChanges(This,OperationSet) ((This)->lpVtbl->CommitChanges(This,OperationSet)) +#define IXAudio2_GetPerformanceData(This,pPerfData) ((This)->lpVtbl->GetPerformanceData(This,pPerfData)) +#define IXAudio2_SetDebugConfiguration(This,pDebugConfiguration,pReserved) ((This)->lpVtbl->SetDebugConfiguration(This,pDebugConfiguration,pReserved)) + +// IXAudio2Voice +#define IXAudio2Voice_GetVoiceDetails(This,pVoiceDetails) ((This)->lpVtbl->GetVoiceDetails(This,pVoiceDetails)) +#define IXAudio2Voice_SetOutputVoices(This,pSendList) ((This)->lpVtbl->SetOutputVoices(This,pSendList)) +#define IXAudio2Voice_SetEffectChain(This,pEffectChain) ((This)->lpVtbl->SetEffectChain(This,pEffectChain)) +#define IXAudio2Voice_EnableEffect(This,EffectIndex,OperationSet) ((This)->lpVtbl->EnableEffect(This,EffectIndex,OperationSet)) +#define IXAudio2Voice_DisableEffect(This,EffectIndex,OperationSet) ((This)->lpVtbl->DisableEffect(This,EffectIndex,OperationSet)) +#define IXAudio2Voice_GetEffectState(This,EffectIndex,pEnabled) ((This)->lpVtbl->GetEffectState(This,EffectIndex,pEnabled)) +#define IXAudio2Voice_SetEffectParameters(This,EffectIndex,pParameters,ParametersByteSize, OperationSet) ((This)->lpVtbl->SetEffectParameters(This,EffectIndex,pParameters,ParametersByteSize,OperationSet)) +#define IXAudio2Voice_GetEffectParameters(This,EffectIndex,pParameters,ParametersByteSize) ((This)->lpVtbl->GetEffectParameters(This,EffectIndex,pParameters,ParametersByteSize)) +#define IXAudio2Voice_SetFilterParameters(This,pParameters,OperationSet) ((This)->lpVtbl->SetFilterParameters(This,pParameters,OperationSet)) +#define IXAudio2Voice_GetFilterParameters(This,pParameters) ((This)->lpVtbl->GetFilterParameters(This,pParameters)) +#define IXAudio2Voice_SetOutputFilterParameters(This,pDestinationVoice,pParameters,OperationSet) ((This)->lpVtbl->SetOutputFilterParameters(This,pDestinationVoice,pParameters,OperationSet)) +#define IXAudio2Voice_GetOutputFilterParameters(This,pDestinationVoice,pParameters) ((This)->lpVtbl->GetOutputFilterParameters(This,pDestinationVoice,pParameters)) +#define IXAudio2Voice_SetVolume(This,Volume,OperationSet) ((This)->lpVtbl->SetVolume(This,Volume,OperationSet)) +#define IXAudio2Voice_GetVolume(This,pVolume) ((This)->lpVtbl->GetVolume(This,pVolume)) +#define IXAudio2Voice_SetChannelVolumes(This,Channels,pVolumes,OperationSet) ((This)->lpVtbl->SetChannelVolumes(This,Channels,pVolumes,OperationSet)) +#define IXAudio2Voice_GetChannelVolumes(This,Channels,pVolumes) ((This)->lpVtbl->GetChannelVolumes(This,Channels,pVolumes)) +#define IXAudio2Voice_SetOutputMatrix(This,pDestinationVoice,SourceChannels,DestinationChannels,pLevelMatrix,OperationSet) ((This)->lpVtbl->SetOutputMatrix(This,pDestinationVoice,SourceChannels,DestinationChannels,pLevelMatrix,OperationSet)) +#define IXAudio2Voice_GetOutputMatrix(This,pDestinationVoice,SourceChannels,DestinationChannels,pLevelMatrix) ((This)->lpVtbl->GetOutputMatrix(This,pDestinationVoice,SourceChannels,DestinationChannels,pLevelMatrix)) +#define IXAudio2Voice_DestroyVoice(This) ((This)->lpVtbl->DestroyVoice(This)) + +// IXAudio2SourceVoice +#define IXAudio2SourceVoice_GetVoiceDetails IXAudio2Voice_GetVoiceDetails +#define IXAudio2SourceVoice_SetOutputVoices IXAudio2Voice_SetOutputVoices +#define IXAudio2SourceVoice_SetEffectChain IXAudio2Voice_SetEffectChain +#define IXAudio2SourceVoice_EnableEffect IXAudio2Voice_EnableEffect +#define IXAudio2SourceVoice_DisableEffect IXAudio2Voice_DisableEffect +#define IXAudio2SourceVoice_GetEffectState IXAudio2Voice_GetEffectState +#define IXAudio2SourceVoice_SetEffectParameters IXAudio2Voice_SetEffectParameters +#define IXAudio2SourceVoice_GetEffectParameters IXAudio2Voice_GetEffectParameters +#define IXAudio2SourceVoice_SetFilterParameters IXAudio2Voice_SetFilterParameters +#define IXAudio2SourceVoice_GetFilterParameters IXAudio2Voice_GetFilterParameters +#define IXAudio2SourceVoice_SetOutputFilterParameters IXAudio2Voice_SetOutputFilterParameters +#define IXAudio2SourceVoice_GetOutputFilterParameters IXAudio2Voice_GetOutputFilterParameters +#define IXAudio2SourceVoice_SetVolume IXAudio2Voice_SetVolume +#define IXAudio2SourceVoice_GetVolume IXAudio2Voice_GetVolume +#define IXAudio2SourceVoice_SetChannelVolumes IXAudio2Voice_SetChannelVolumes +#define IXAudio2SourceVoice_GetChannelVolumes IXAudio2Voice_GetChannelVolumes +#define IXAudio2SourceVoice_SetOutputMatrix IXAudio2Voice_SetOutputMatrix +#define IXAudio2SourceVoice_GetOutputMatrix IXAudio2Voice_GetOutputMatrix +#define IXAudio2SourceVoice_DestroyVoice IXAudio2Voice_DestroyVoice +#define IXAudio2SourceVoice_Start(This,Flags,OperationSet) ((This)->lpVtbl->Start(This,Flags,OperationSet)) +#define IXAudio2SourceVoice_Stop(This,Flags,OperationSet) ((This)->lpVtbl->Stop(This,Flags,OperationSet)) +#define IXAudio2SourceVoice_SubmitSourceBuffer(This,pBuffer,pBufferWMA) ((This)->lpVtbl->SubmitSourceBuffer(This,pBuffer,pBufferWMA)) +#define IXAudio2SourceVoice_FlushSourceBuffers(This) ((This)->lpVtbl->FlushSourceBuffers(This)) +#define IXAudio2SourceVoice_Discontinuity(This) ((This)->lpVtbl->Discontinuity(This)) +#define IXAudio2SourceVoice_ExitLoop(This,OperationSet) ((This)->lpVtbl->ExitLoop(This,OperationSet)) +#define IXAudio2SourceVoice_GetState(This,pVoiceState) ((This)->lpVtbl->GetState(This,pVoiceState)) +#define IXAudio2SourceVoice_SetFrequencyRatio(This,Ratio,OperationSet) ((This)->lpVtbl->SetFrequencyRatio(This,Ratio,OperationSet)) +#define IXAudio2SourceVoice_GetFrequencyRatio(This,pRatio) ((This)->lpVtbl->GetFrequencyRatio(This,pRatio)) +#define IXAudio2SourceVoice_SetSourceSampleRate(This,NewSourceSampleRate) ((This)->lpVtbl->SetSourceSampleRate(This,NewSourceSampleRate)) + +// IXAudio2SubmixVoice +#define IXAudio2SubmixVoice_GetVoiceDetails IXAudio2Voice_GetVoiceDetails +#define IXAudio2SubmixVoice_SetOutputVoices IXAudio2Voice_SetOutputVoices +#define IXAudio2SubmixVoice_SetEffectChain IXAudio2Voice_SetEffectChain +#define IXAudio2SubmixVoice_EnableEffect IXAudio2Voice_EnableEffect +#define IXAudio2SubmixVoice_DisableEffect IXAudio2Voice_DisableEffect +#define IXAudio2SubmixVoice_GetEffectState IXAudio2Voice_GetEffectState +#define IXAudio2SubmixVoice_SetEffectParameters IXAudio2Voice_SetEffectParameters +#define IXAudio2SubmixVoice_GetEffectParameters IXAudio2Voice_GetEffectParameters +#define IXAudio2SubmixVoice_SetFilterParameters IXAudio2Voice_SetFilterParameters +#define IXAudio2SubmixVoice_GetFilterParameters IXAudio2Voice_GetFilterParameters +#define IXAudio2SubmixVoice_SetOutputFilterParameters IXAudio2Voice_SetOutputFilterParameters +#define IXAudio2SubmixVoice_GetOutputFilterParameters IXAudio2Voice_GetOutputFilterParameters +#define IXAudio2SubmixVoice_SetVolume IXAudio2Voice_SetVolume +#define IXAudio2SubmixVoice_GetVolume IXAudio2Voice_GetVolume +#define IXAudio2SubmixVoice_SetChannelVolumes IXAudio2Voice_SetChannelVolumes +#define IXAudio2SubmixVoice_GetChannelVolumes IXAudio2Voice_GetChannelVolumes +#define IXAudio2SubmixVoice_SetOutputMatrix IXAudio2Voice_SetOutputMatrix +#define IXAudio2SubmixVoice_GetOutputMatrix IXAudio2Voice_GetOutputMatrix +#define IXAudio2SubmixVoice_DestroyVoice IXAudio2Voice_DestroyVoice + +// IXAudio2MasteringVoice +#define IXAudio2MasteringVoice_GetVoiceDetails IXAudio2Voice_GetVoiceDetails +#define IXAudio2MasteringVoice_SetOutputVoices IXAudio2Voice_SetOutputVoices +#define IXAudio2MasteringVoice_SetEffectChain IXAudio2Voice_SetEffectChain +#define IXAudio2MasteringVoice_EnableEffect IXAudio2Voice_EnableEffect +#define IXAudio2MasteringVoice_DisableEffect IXAudio2Voice_DisableEffect +#define IXAudio2MasteringVoice_GetEffectState IXAudio2Voice_GetEffectState +#define IXAudio2MasteringVoice_SetEffectParameters IXAudio2Voice_SetEffectParameters +#define IXAudio2MasteringVoice_GetEffectParameters IXAudio2Voice_GetEffectParameters +#define IXAudio2MasteringVoice_SetFilterParameters IXAudio2Voice_SetFilterParameters +#define IXAudio2MasteringVoice_GetFilterParameters IXAudio2Voice_GetFilterParameters +#define IXAudio2MasteringVoice_SetOutputFilterParameters IXAudio2Voice_SetOutputFilterParameters +#define IXAudio2MasteringVoice_GetOutputFilterParameters IXAudio2Voice_GetOutputFilterParameters +#define IXAudio2MasteringVoice_SetVolume IXAudio2Voice_SetVolume +#define IXAudio2MasteringVoice_GetVolume IXAudio2Voice_GetVolume +#define IXAudio2MasteringVoice_SetChannelVolumes IXAudio2Voice_SetChannelVolumes +#define IXAudio2MasteringVoice_GetChannelVolumes IXAudio2Voice_GetChannelVolumes +#define IXAudio2MasteringVoice_SetOutputMatrix IXAudio2Voice_SetOutputMatrix +#define IXAudio2MasteringVoice_GetOutputMatrix IXAudio2Voice_GetOutputMatrix +#define IXAudio2MasteringVoice_DestroyVoice IXAudio2Voice_DestroyVoice + +#endif // #ifndef __cplusplus + + +/************************************************************************** + * + * Utility functions used to convert from pitch in semitones and volume + * in decibels to the frequency and amplitude ratio units used by XAudio2. + * These are only defined if the client #defines XAUDIO2_HELPER_FUNCTIONS + * prior to #including xaudio2.h. + * + **************************************************************************/ + +#ifdef XAUDIO2_HELPER_FUNCTIONS + +#define _USE_MATH_DEFINES // Make math.h define M_PI +#include // For powf, log10f, sinf and asinf + +// Calculate the argument to SetVolume from a decibel value +__inline float XAudio2DecibelsToAmplitudeRatio(float Decibels) +{ + return powf(10.0f, Decibels / 20.0f); +} + +// Recover a volume in decibels from an amplitude factor +__inline float XAudio2AmplitudeRatioToDecibels(float Volume) +{ + if (Volume == 0) + { + return -3.402823466e+38f; // Smallest float value (-FLT_MAX) + } + return 20.0f * log10f(Volume); +} + +// Calculate the argument to SetFrequencyRatio from a semitone value +__inline float XAudio2SemitonesToFrequencyRatio(float Semitones) +{ + // FrequencyRatio = 2 ^ Octaves + // = 2 ^ (Semitones / 12) + return powf(2.0f, Semitones / 12.0f); +} + +// Recover a pitch in semitones from a frequency ratio +__inline float XAudio2FrequencyRatioToSemitones(float FrequencyRatio) +{ + // Semitones = 12 * log2(FrequencyRatio) + // = 12 * log2(10) * log10(FrequencyRatio) + return 39.86313713864835f * log10f(FrequencyRatio); +} + +// Convert from filter cutoff frequencies expressed in Hertz to the radian +// frequency values used in XAUDIO2_FILTER_PARAMETERS.Frequency. Note that +// the highest CutoffFrequency supported is SampleRate/6. Higher values of +// CutoffFrequency will return XAUDIO2_MAX_FILTER_FREQUENCY. +__inline float XAudio2CutoffFrequencyToRadians(float CutoffFrequency, UINT32 SampleRate) +{ + if ((UINT32)(CutoffFrequency * 6.0f) >= SampleRate) + { + return XAUDIO2_MAX_FILTER_FREQUENCY; + } + return 2.0f * sinf((float)M_PI * CutoffFrequency / SampleRate); +} + +// Convert from radian frequencies back to absolute frequencies in Hertz +__inline float XAudio2RadiansToCutoffFrequency(float Radians, float SampleRate) +{ + return SampleRate * asinf(Radians / 2.0f) / (float)M_PI; +} +#endif // #ifdef XAUDIO2_HELPER_FUNCTIONS + + +/************************************************************************** + * + * XAudio2Create: Top-level function that creates an XAudio2 instance. + * + * On Windows this is just an inline function that calls CoCreateInstance + * and Initialize. The arguments are described above, under Initialize, + * except that the XAUDIO2_DEBUG_ENGINE flag can be used here to select + * the debug version of XAudio2. + * + * On Xbox, this function is implemented in the XAudio2 library, and the + * XAUDIO2_DEBUG_ENGINE flag has no effect; the client must explicitly + * link with the debug version of the library to obtain debug behavior. + * + **************************************************************************/ + +#ifdef _XBOX + +STDAPI XAudio2Create(__deref_out IXAudio2** ppXAudio2, UINT32 Flags X2DEFAULT(0), + XAUDIO2_PROCESSOR XAudio2Processor X2DEFAULT(XAUDIO2_DEFAULT_PROCESSOR)); + +#else // Windows + +__inline HRESULT XAudio2Create(__deref_out IXAudio2** ppXAudio2, UINT32 Flags X2DEFAULT(0), + XAUDIO2_PROCESSOR XAudio2Processor X2DEFAULT(XAUDIO2_DEFAULT_PROCESSOR)) +{ + // Instantiate the appropriate XAudio2 engine + IXAudio2* pXAudio2; + + #ifdef __cplusplus + + HRESULT hr = CoCreateInstance((Flags & XAUDIO2_DEBUG_ENGINE) ? __uuidof(XAudio2_Debug) : __uuidof(XAudio2), + NULL, CLSCTX_INPROC_SERVER, __uuidof(IXAudio2), (void**)&pXAudio2); + if (SUCCEEDED(hr)) + { + hr = pXAudio2->Initialize(Flags, XAudio2Processor); + + if (SUCCEEDED(hr)) + { + *ppXAudio2 = pXAudio2; + } + else + { + pXAudio2->Release(); + } + } + + #else + + HRESULT hr = CoCreateInstance((Flags & XAUDIO2_DEBUG_ENGINE) ? &CLSID_XAudio2_Debug : &CLSID_XAudio2, + NULL, CLSCTX_INPROC_SERVER, &IID_IXAudio2, (void**)&pXAudio2); + if (SUCCEEDED(hr)) + { + hr = pXAudio2->lpVtbl->Initialize(pXAudio2, Flags, XAudio2Processor); + + if (SUCCEEDED(hr)) + { + *ppXAudio2 = pXAudio2; + } + else + { + pXAudio2->lpVtbl->Release(pXAudio2); + } + } + + #endif // #ifdef __cplusplus + + return hr; +} + +#endif // #ifdef _XBOX + + +// Undo the #pragma pack(push, 1) directive at the top of this file +#pragma pack(pop) + +#endif // #ifndef GUID_DEFS_ONLY +#endif // #ifndef __XAUDIO2_INCLUDED__ diff --git a/WickedEngine/Xaudio2_7/XAudio2fx.h b/WickedEngine/Xaudio2_7/XAudio2fx.h new file mode 100644 index 000000000..4284bd249 --- /dev/null +++ b/WickedEngine/Xaudio2_7/XAudio2fx.h @@ -0,0 +1,431 @@ +/************************************************************************** + * + * Copyright (c) Microsoft Corporation. All rights reserved. + * + * File: xaudio2fx.h + * Content: Declarations for the audio effects included with XAudio2. + * + **************************************************************************/ + +#ifndef __XAUDIO2FX_INCLUDED__ +#define __XAUDIO2FX_INCLUDED__ + + +/************************************************************************** + * + * XAudio2 effect class IDs. + * + **************************************************************************/ + +#include "comdecl.h" // For DEFINE_CLSID and DEFINE_IID + +// XAudio 2.0 (March 2008 SDK) +//DEFINE_CLSID(AudioVolumeMeter, C0C56F46, 29B1, 44E9, 99, 39, A3, 2C, E8, 68, 67, E2); +//DEFINE_CLSID(AudioVolumeMeter_Debug, C0C56F46, 29B1, 44E9, 99, 39, A3, 2C, E8, 68, 67, DB); +//DEFINE_CLSID(AudioReverb, 6F6EA3A9, 2CF5, 41CF, 91, C1, 21, 70, B1, 54, 00, 63); +//DEFINE_CLSID(AudioReverb_Debug, 6F6EA3A9, 2CF5, 41CF, 91, C1, 21, 70, B1, 54, 00, DB); + +// XAudio 2.1 (June 2008 SDK) +//DEFINE_CLSID(AudioVolumeMeter, c1e3f122, a2ea, 442c, 85, 4f, 20, d9, 8f, 83, 57, a1); +//DEFINE_CLSID(AudioVolumeMeter_Debug, 6d97a461, b02d, 48ae, b5, 43, 82, bc, 35, fd, fa, e2); +//DEFINE_CLSID(AudioReverb, f4769300, b949, 4df9, b3, 33, 00, d3, 39, 32, e9, a6); +//DEFINE_CLSID(AudioReverb_Debug, aea2cabc, 8c7c, 46aa, ba, 44, 0e, 6d, 75, 88, a1, f2); + +// XAudio 2.2 (August 2008 SDK) +//DEFINE_CLSID(AudioVolumeMeter, f5ca7b34, 8055, 42c0, b8, 36, 21, 61, 29, eb, 7e, 30); +//DEFINE_CLSID(AudioVolumeMeter_Debug, f796f5f7, 6059, 4a9f, 98, 2d, 61, ee, c2, ed, 67, ca); +//DEFINE_CLSID(AudioReverb, 629cf0de, 3ecc, 41e7, 99, 26, f7, e4, 3e, eb, ec, 51); +//DEFINE_CLSID(AudioReverb_Debug, 4aae4299, 3260, 46d4, 97, cc, 6c, c7, 60, c8, 53, 29); + +// XAudio 2.3 (November 2008 SDK) +//DEFINE_CLSID(AudioVolumeMeter, e180344b, ac83, 4483, 95, 9e, 18, a5, c5, 6a, 5e, 19); +//DEFINE_CLSID(AudioVolumeMeter_Debug, 922a0a56, 7d13, 40ae, a4, 81, 3c, 6c, 60, f1, 14, 01); +//DEFINE_CLSID(AudioReverb, 9cab402c, 1d37, 44b4, 88, 6d, fa, 4f, 36, 17, 0a, 4c); +//DEFINE_CLSID(AudioReverb_Debug, eadda998, 3be6, 4505, 84, be, ea, 06, 36, 5d, b9, 6b); + +// XAudio 2.4 (March 2009 SDK) +//DEFINE_CLSID(AudioVolumeMeter, c7338b95, 52b8, 4542, aa, 79, 42, eb, 01, 6c, 8c, 1c); +//DEFINE_CLSID(AudioVolumeMeter_Debug, 524bd872, 5c0b, 4217, bd, b8, 0a, 86, 81, 83, 0b, a5); +//DEFINE_CLSID(AudioReverb, 8bb7778b, 645b, 4475, 9a, 73, 1d, e3, 17, 0b, d3, af); +//DEFINE_CLSID(AudioReverb_Debug, da7738a2, cd0c, 4367, 9a, ac, d7, ea, d7, c6, 4f, 98); + +// XAudio 2.5 (March 2009 SDK) +//DEFINE_CLSID(AudioVolumeMeter, 2139e6da, c341, 4774, 9a, c3, b4, e0, 26, 34, 7f, 64); +//DEFINE_CLSID(AudioVolumeMeter_Debug, a5cc4e13, ca00, 416b, a6, ee, 49, fe, e7, b5, 43, d0); +//DEFINE_CLSID(AudioReverb, d06df0d0, 8518, 441e, 82, 2f, 54, 51, d5, c5, 95, b8); +//DEFINE_CLSID(AudioReverb_Debug, 613604ec, 304c, 45ec, a4, ed, 7a, 1c, 61, 2e, 9e, 72); + +// XAudio 2.6 (February 2010 SDK) +//DEFINE_CLSID(AudioVolumeMeter, e48c5a3f, 93ef, 43bb, a0, 92, 2c, 7c, eb, 94, 6f, 27); +//DEFINE_CLSID(AudioVolumeMeter_Debug, 9a9eaef7, a9e0, 4088, 9b, 1b, 9c, a0, 3a, 1a, ec, d4); +//DEFINE_CLSID(AudioReverb, cecec95a, d894, 491a, be, e3, 5e, 10, 6f, b5, 9f, 2d); +//DEFINE_CLSID(AudioReverb_Debug, 99a1c72e, 364c, 4c1b, 96, 23, fd, 5c, 8a, bd, 90, c7); + +// XAudio 2.7 (June 2010 SDK) +DEFINE_CLSID(AudioVolumeMeter, cac1105f, 619b, 4d04, 83, 1a, 44, e1, cb, f1, 2d, 57); +DEFINE_CLSID(AudioVolumeMeter_Debug, 2d9a0f9c, e67b, 4b24, ab, 44, 92, b3, e7, 70, c0, 20); +DEFINE_CLSID(AudioReverb, 6a93130e, 1d53, 41d1, a9, cf, e7, 58, 80, 0b, b1, 79); +DEFINE_CLSID(AudioReverb_Debug, c4f82dd4, cb4e, 4ce1, 8b, db, ee, 32, d4, 19, 82, 69); + +// Ignore the rest of this header if only the GUID definitions were requested +#ifndef GUID_DEFS_ONLY + +#ifdef _XBOX + #include // Xbox COM declarations (IUnknown, etc) +#else + #include // Windows COM declarations +#endif +#include // For log10() + + +// All structures defined in this file should use tight packing +#pragma pack(push, 1) + + +/************************************************************************** + * + * Effect creation functions. On Windows, these are just inline functions + * that call CoCreateInstance and Initialize; the XAUDIO2FX_DEBUG flag can + * be used to select the debug version of the effects. On Xbox, these map + * to real functions included in xaudio2.lib, and the XAUDIO2FX_DEBUG flag + * is ignored; the application must link with the debug library to use the + * debug functionality. + * + **************************************************************************/ + +// Use default values for some parameters if building C++ code +#ifdef __cplusplus + #define DEFAULT(x) =x +#else + #define DEFAULT(x) +#endif + +#define XAUDIO2FX_DEBUG 1 // To select the debug version of an effect + +#ifdef _XBOX + + STDAPI CreateAudioVolumeMeter(__deref_out IUnknown** ppApo); + STDAPI CreateAudioReverb(__deref_out IUnknown** ppApo); + + __inline HRESULT XAudio2CreateVolumeMeter(__deref_out IUnknown** ppApo, UINT32 /*Flags*/ DEFAULT(0)) + { + return CreateAudioVolumeMeter(ppApo); + } + + __inline HRESULT XAudio2CreateReverb(__deref_out IUnknown** ppApo, UINT32 /*Flags*/ DEFAULT(0)) + { + return CreateAudioReverb(ppApo); + } + +#else // Windows + + __inline HRESULT XAudio2CreateVolumeMeter(__deref_out IUnknown** ppApo, UINT32 Flags DEFAULT(0)) + { + #ifdef __cplusplus + return CoCreateInstance((Flags & XAUDIO2FX_DEBUG) ? __uuidof(AudioVolumeMeter_Debug) + : __uuidof(AudioVolumeMeter), + NULL, CLSCTX_INPROC_SERVER, __uuidof(IUnknown), (void**)ppApo); + #else + return CoCreateInstance((Flags & XAUDIO2FX_DEBUG) ? &CLSID_AudioVolumeMeter_Debug + : &CLSID_AudioVolumeMeter, + NULL, CLSCTX_INPROC_SERVER, &IID_IUnknown, (void**)ppApo); + #endif + } + + __inline HRESULT XAudio2CreateReverb(__deref_out IUnknown** ppApo, UINT32 Flags DEFAULT(0)) + { + #ifdef __cplusplus + return CoCreateInstance((Flags & XAUDIO2FX_DEBUG) ? __uuidof(AudioReverb_Debug) + : __uuidof(AudioReverb), + NULL, CLSCTX_INPROC_SERVER, __uuidof(IUnknown), (void**)ppApo); + #else + return CoCreateInstance((Flags & XAUDIO2FX_DEBUG) ? &CLSID_AudioReverb_Debug + : &CLSID_AudioReverb, + NULL, CLSCTX_INPROC_SERVER, &IID_IUnknown, (void**)ppApo); + #endif + } + +#endif // #ifdef _XBOX + + + +/************************************************************************** + * + * Volume meter parameters. + * The volume meter supports FLOAT32 audio formats and must be used in-place. + * + **************************************************************************/ + +// XAUDIO2FX_VOLUMEMETER_LEVELS: Receives results from GetEffectParameters(). +// The user is responsible for allocating pPeakLevels, pRMSLevels, and +// initializing ChannelCount accordingly. +// The volume meter does not support SetEffectParameters(). +typedef struct XAUDIO2FX_VOLUMEMETER_LEVELS +{ + float* pPeakLevels; // Peak levels table: receives maximum absolute level for each channel + // over a processing pass; may be NULL if pRMSLevls != NULL, + // otherwise must have at least ChannelCount elements. + float* pRMSLevels; // Root mean square levels table: receives RMS level for each channel + // over a processing pass; may be NULL if pPeakLevels != NULL, + // otherwise must have at least ChannelCount elements. + UINT32 ChannelCount; // Number of channels being processed by the volume meter APO +} XAUDIO2FX_VOLUMEMETER_LEVELS; + + + +/************************************************************************** + * + * Reverb parameters. + * The reverb supports only FLOAT32 audio with the following channel + * configurations: + * Input: Mono Output: Mono + * Input: Mono Output: 5.1 + * Input: Stereo Output: Stereo + * Input: Stereo Output: 5.1 + * The framerate must be within [20000, 48000] Hz. + * + * When using mono input, delay filters associated with the right channel + * are not executed. In this case, parameters such as PositionRight and + * PositionMatrixRight have no effect. This also means the reverb uses + * less CPU when hosted in a mono submix. + * + **************************************************************************/ + +#define XAUDIO2FX_REVERB_MIN_FRAMERATE 20000 +#define XAUDIO2FX_REVERB_MAX_FRAMERATE 48000 + +// XAUDIO2FX_REVERB_PARAMETERS: Native parameter set for the reverb effect + +typedef struct XAUDIO2FX_REVERB_PARAMETERS +{ + // ratio of wet (processed) signal to dry (original) signal + float WetDryMix; // [0, 100] (percentage) + + // Delay times + UINT32 ReflectionsDelay; // [0, 300] in ms + BYTE ReverbDelay; // [0, 85] in ms + BYTE RearDelay; // [0, 5] in ms + + // Indexed parameters + BYTE PositionLeft; // [0, 30] no units + BYTE PositionRight; // [0, 30] no units, ignored when configured to mono + BYTE PositionMatrixLeft; // [0, 30] no units + BYTE PositionMatrixRight; // [0, 30] no units, ignored when configured to mono + BYTE EarlyDiffusion; // [0, 15] no units + BYTE LateDiffusion; // [0, 15] no units + BYTE LowEQGain; // [0, 12] no units + BYTE LowEQCutoff; // [0, 9] no units + BYTE HighEQGain; // [0, 8] no units + BYTE HighEQCutoff; // [0, 14] no units + + // Direct parameters + float RoomFilterFreq; // [20, 20000] in Hz + float RoomFilterMain; // [-100, 0] in dB + float RoomFilterHF; // [-100, 0] in dB + float ReflectionsGain; // [-100, 20] in dB + float ReverbGain; // [-100, 20] in dB + float DecayTime; // [0.1, inf] in seconds + float Density; // [0, 100] (percentage) + float RoomSize; // [1, 100] in feet +} XAUDIO2FX_REVERB_PARAMETERS; + + +// Maximum, minimum and default values for the parameters above +#define XAUDIO2FX_REVERB_MIN_WET_DRY_MIX 0.0f +#define XAUDIO2FX_REVERB_MIN_REFLECTIONS_DELAY 0 +#define XAUDIO2FX_REVERB_MIN_REVERB_DELAY 0 +#define XAUDIO2FX_REVERB_MIN_REAR_DELAY 0 +#define XAUDIO2FX_REVERB_MIN_POSITION 0 +#define XAUDIO2FX_REVERB_MIN_DIFFUSION 0 +#define XAUDIO2FX_REVERB_MIN_LOW_EQ_GAIN 0 +#define XAUDIO2FX_REVERB_MIN_LOW_EQ_CUTOFF 0 +#define XAUDIO2FX_REVERB_MIN_HIGH_EQ_GAIN 0 +#define XAUDIO2FX_REVERB_MIN_HIGH_EQ_CUTOFF 0 +#define XAUDIO2FX_REVERB_MIN_ROOM_FILTER_FREQ 20.0f +#define XAUDIO2FX_REVERB_MIN_ROOM_FILTER_MAIN -100.0f +#define XAUDIO2FX_REVERB_MIN_ROOM_FILTER_HF -100.0f +#define XAUDIO2FX_REVERB_MIN_REFLECTIONS_GAIN -100.0f +#define XAUDIO2FX_REVERB_MIN_REVERB_GAIN -100.0f +#define XAUDIO2FX_REVERB_MIN_DECAY_TIME 0.1f +#define XAUDIO2FX_REVERB_MIN_DENSITY 0.0f +#define XAUDIO2FX_REVERB_MIN_ROOM_SIZE 0.0f + +#define XAUDIO2FX_REVERB_MAX_WET_DRY_MIX 100.0f +#define XAUDIO2FX_REVERB_MAX_REFLECTIONS_DELAY 300 +#define XAUDIO2FX_REVERB_MAX_REVERB_DELAY 85 +#define XAUDIO2FX_REVERB_MAX_REAR_DELAY 5 +#define XAUDIO2FX_REVERB_MAX_POSITION 30 +#define XAUDIO2FX_REVERB_MAX_DIFFUSION 15 +#define XAUDIO2FX_REVERB_MAX_LOW_EQ_GAIN 12 +#define XAUDIO2FX_REVERB_MAX_LOW_EQ_CUTOFF 9 +#define XAUDIO2FX_REVERB_MAX_HIGH_EQ_GAIN 8 +#define XAUDIO2FX_REVERB_MAX_HIGH_EQ_CUTOFF 14 +#define XAUDIO2FX_REVERB_MAX_ROOM_FILTER_FREQ 20000.0f +#define XAUDIO2FX_REVERB_MAX_ROOM_FILTER_MAIN 0.0f +#define XAUDIO2FX_REVERB_MAX_ROOM_FILTER_HF 0.0f +#define XAUDIO2FX_REVERB_MAX_REFLECTIONS_GAIN 20.0f +#define XAUDIO2FX_REVERB_MAX_REVERB_GAIN 20.0f +#define XAUDIO2FX_REVERB_MAX_DENSITY 100.0f +#define XAUDIO2FX_REVERB_MAX_ROOM_SIZE 100.0f + +#define XAUDIO2FX_REVERB_DEFAULT_WET_DRY_MIX 100.0f +#define XAUDIO2FX_REVERB_DEFAULT_REFLECTIONS_DELAY 5 +#define XAUDIO2FX_REVERB_DEFAULT_REVERB_DELAY 5 +#define XAUDIO2FX_REVERB_DEFAULT_REAR_DELAY 5 +#define XAUDIO2FX_REVERB_DEFAULT_POSITION 6 +#define XAUDIO2FX_REVERB_DEFAULT_POSITION_MATRIX 27 +#define XAUDIO2FX_REVERB_DEFAULT_EARLY_DIFFUSION 8 +#define XAUDIO2FX_REVERB_DEFAULT_LATE_DIFFUSION 8 +#define XAUDIO2FX_REVERB_DEFAULT_LOW_EQ_GAIN 8 +#define XAUDIO2FX_REVERB_DEFAULT_LOW_EQ_CUTOFF 4 +#define XAUDIO2FX_REVERB_DEFAULT_HIGH_EQ_GAIN 8 +#define XAUDIO2FX_REVERB_DEFAULT_HIGH_EQ_CUTOFF 4 +#define XAUDIO2FX_REVERB_DEFAULT_ROOM_FILTER_FREQ 5000.0f +#define XAUDIO2FX_REVERB_DEFAULT_ROOM_FILTER_MAIN 0.0f +#define XAUDIO2FX_REVERB_DEFAULT_ROOM_FILTER_HF 0.0f +#define XAUDIO2FX_REVERB_DEFAULT_REFLECTIONS_GAIN 0.0f +#define XAUDIO2FX_REVERB_DEFAULT_REVERB_GAIN 0.0f +#define XAUDIO2FX_REVERB_DEFAULT_DECAY_TIME 1.0f +#define XAUDIO2FX_REVERB_DEFAULT_DENSITY 100.0f +#define XAUDIO2FX_REVERB_DEFAULT_ROOM_SIZE 100.0f + + +// XAUDIO2FX_REVERB_I3DL2_PARAMETERS: Parameter set compliant with the I3DL2 standard + +typedef struct XAUDIO2FX_REVERB_I3DL2_PARAMETERS +{ + // ratio of wet (processed) signal to dry (original) signal + float WetDryMix; // [0, 100] (percentage) + + // Standard I3DL2 parameters + INT32 Room; // [-10000, 0] in mB (hundredths of decibels) + INT32 RoomHF; // [-10000, 0] in mB (hundredths of decibels) + float RoomRolloffFactor; // [0.0, 10.0] + float DecayTime; // [0.1, 20.0] in seconds + float DecayHFRatio; // [0.1, 2.0] + INT32 Reflections; // [-10000, 1000] in mB (hundredths of decibels) + float ReflectionsDelay; // [0.0, 0.3] in seconds + INT32 Reverb; // [-10000, 2000] in mB (hundredths of decibels) + float ReverbDelay; // [0.0, 0.1] in seconds + float Diffusion; // [0.0, 100.0] (percentage) + float Density; // [0.0, 100.0] (percentage) + float HFReference; // [20.0, 20000.0] in Hz +} XAUDIO2FX_REVERB_I3DL2_PARAMETERS; + + +// ReverbConvertI3DL2ToNative: Utility function to map from I3DL2 to native parameters + +__inline void ReverbConvertI3DL2ToNative +( + __in const XAUDIO2FX_REVERB_I3DL2_PARAMETERS* pI3DL2, + __out XAUDIO2FX_REVERB_PARAMETERS* pNative +) +{ + float reflectionsDelay; + float reverbDelay; + + // RoomRolloffFactor is ignored + + // These parameters have no equivalent in I3DL2 + pNative->RearDelay = XAUDIO2FX_REVERB_DEFAULT_REAR_DELAY; // 5 + pNative->PositionLeft = XAUDIO2FX_REVERB_DEFAULT_POSITION; // 6 + pNative->PositionRight = XAUDIO2FX_REVERB_DEFAULT_POSITION; // 6 + pNative->PositionMatrixLeft = XAUDIO2FX_REVERB_DEFAULT_POSITION_MATRIX; // 27 + pNative->PositionMatrixRight = XAUDIO2FX_REVERB_DEFAULT_POSITION_MATRIX; // 27 + pNative->RoomSize = XAUDIO2FX_REVERB_DEFAULT_ROOM_SIZE; // 100 + pNative->LowEQCutoff = 4; + pNative->HighEQCutoff = 6; + + // The rest of the I3DL2 parameters map to the native property set + pNative->RoomFilterMain = (float)pI3DL2->Room / 100.0f; + pNative->RoomFilterHF = (float)pI3DL2->RoomHF / 100.0f; + + if (pI3DL2->DecayHFRatio >= 1.0f) + { + INT32 index = (INT32)(-4.0 * log10(pI3DL2->DecayHFRatio)); + if (index < -8) index = -8; + pNative->LowEQGain = (BYTE)((index < 0) ? index + 8 : 8); + pNative->HighEQGain = 8; + pNative->DecayTime = pI3DL2->DecayTime * pI3DL2->DecayHFRatio; + } + else + { + INT32 index = (INT32)(4.0 * log10(pI3DL2->DecayHFRatio)); + if (index < -8) index = -8; + pNative->LowEQGain = 8; + pNative->HighEQGain = (BYTE)((index < 0) ? index + 8 : 8); + pNative->DecayTime = pI3DL2->DecayTime; + } + + reflectionsDelay = pI3DL2->ReflectionsDelay * 1000.0f; + if (reflectionsDelay >= XAUDIO2FX_REVERB_MAX_REFLECTIONS_DELAY) // 300 + { + reflectionsDelay = (float)(XAUDIO2FX_REVERB_MAX_REFLECTIONS_DELAY - 1); + } + else if (reflectionsDelay <= 1) + { + reflectionsDelay = 1; + } + pNative->ReflectionsDelay = (UINT32)reflectionsDelay; + + reverbDelay = pI3DL2->ReverbDelay * 1000.0f; + if (reverbDelay >= XAUDIO2FX_REVERB_MAX_REVERB_DELAY) // 85 + { + reverbDelay = (float)(XAUDIO2FX_REVERB_MAX_REVERB_DELAY - 1); + } + pNative->ReverbDelay = (BYTE)reverbDelay; + + pNative->ReflectionsGain = pI3DL2->Reflections / 100.0f; + pNative->ReverbGain = pI3DL2->Reverb / 100.0f; + pNative->EarlyDiffusion = (BYTE)(15.0f * pI3DL2->Diffusion / 100.0f); + pNative->LateDiffusion = pNative->EarlyDiffusion; + pNative->Density = pI3DL2->Density; + pNative->RoomFilterFreq = pI3DL2->HFReference; + + pNative->WetDryMix = pI3DL2->WetDryMix; +} + + +/************************************************************************** + * + * Standard I3DL2 reverb presets (100% wet). + * + **************************************************************************/ + +#define XAUDIO2FX_I3DL2_PRESET_DEFAULT {100,-10000, 0,0.0f, 1.00f,0.50f,-10000,0.020f,-10000,0.040f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_GENERIC {100, -1000, -100,0.0f, 1.49f,0.83f, -2602,0.007f, 200,0.011f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_PADDEDCELL {100, -1000,-6000,0.0f, 0.17f,0.10f, -1204,0.001f, 207,0.002f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_ROOM {100, -1000, -454,0.0f, 0.40f,0.83f, -1646,0.002f, 53,0.003f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_BATHROOM {100, -1000,-1200,0.0f, 1.49f,0.54f, -370,0.007f, 1030,0.011f,100.0f, 60.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_LIVINGROOM {100, -1000,-6000,0.0f, 0.50f,0.10f, -1376,0.003f, -1104,0.004f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_STONEROOM {100, -1000, -300,0.0f, 2.31f,0.64f, -711,0.012f, 83,0.017f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_AUDITORIUM {100, -1000, -476,0.0f, 4.32f,0.59f, -789,0.020f, -289,0.030f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_CONCERTHALL {100, -1000, -500,0.0f, 3.92f,0.70f, -1230,0.020f, -2,0.029f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_CAVE {100, -1000, 0,0.0f, 2.91f,1.30f, -602,0.015f, -302,0.022f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_ARENA {100, -1000, -698,0.0f, 7.24f,0.33f, -1166,0.020f, 16,0.030f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_HANGAR {100, -1000,-1000,0.0f,10.05f,0.23f, -602,0.020f, 198,0.030f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_CARPETEDHALLWAY {100, -1000,-4000,0.0f, 0.30f,0.10f, -1831,0.002f, -1630,0.030f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_HALLWAY {100, -1000, -300,0.0f, 1.49f,0.59f, -1219,0.007f, 441,0.011f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_STONECORRIDOR {100, -1000, -237,0.0f, 2.70f,0.79f, -1214,0.013f, 395,0.020f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_ALLEY {100, -1000, -270,0.0f, 1.49f,0.86f, -1204,0.007f, -4,0.011f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_FOREST {100, -1000,-3300,0.0f, 1.49f,0.54f, -2560,0.162f, -613,0.088f, 79.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_CITY {100, -1000, -800,0.0f, 1.49f,0.67f, -2273,0.007f, -2217,0.011f, 50.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_MOUNTAINS {100, -1000,-2500,0.0f, 1.49f,0.21f, -2780,0.300f, -2014,0.100f, 27.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_QUARRY {100, -1000,-1000,0.0f, 1.49f,0.83f,-10000,0.061f, 500,0.025f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_PLAIN {100, -1000,-2000,0.0f, 1.49f,0.50f, -2466,0.179f, -2514,0.100f, 21.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_PARKINGLOT {100, -1000, 0,0.0f, 1.65f,1.50f, -1363,0.008f, -1153,0.012f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_SEWERPIPE {100, -1000,-1000,0.0f, 2.81f,0.14f, 429,0.014f, 648,0.021f, 80.0f, 60.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_UNDERWATER {100, -1000,-4000,0.0f, 1.49f,0.10f, -449,0.007f, 1700,0.011f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_SMALLROOM {100, -1000, -600,0.0f, 1.10f,0.83f, -400,0.005f, 500,0.010f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_MEDIUMROOM {100, -1000, -600,0.0f, 1.30f,0.83f, -1000,0.010f, -200,0.020f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_LARGEROOM {100, -1000, -600,0.0f, 1.50f,0.83f, -1600,0.020f, -1000,0.040f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_MEDIUMHALL {100, -1000, -600,0.0f, 1.80f,0.70f, -1300,0.015f, -800,0.030f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_LARGEHALL {100, -1000, -600,0.0f, 1.80f,0.70f, -2000,0.030f, -1400,0.060f,100.0f,100.0f,5000.0f} +#define XAUDIO2FX_I3DL2_PRESET_PLATE {100, -1000, -200,0.0f, 1.30f,0.90f, 0,0.002f, 0,0.010f,100.0f, 75.0f,5000.0f} + + +// Undo the #pragma pack(push, 1) at the top of this file +#pragma pack(pop) + +#endif // #ifndef GUID_DEFS_ONLY +#endif // #ifndef __XAUDIO2FX_INCLUDED__ diff --git a/WickedEngine/Xaudio2_7/audiodefs.h b/WickedEngine/Xaudio2_7/audiodefs.h new file mode 100644 index 000000000..ff995ecc7 --- /dev/null +++ b/WickedEngine/Xaudio2_7/audiodefs.h @@ -0,0 +1,263 @@ +/*************************************************************************** + * + * Copyright (c) Microsoft Corporation. All rights reserved. + * + * File: audiodefs.h + * Content: Basic constants and data types for audio work. + * + * Remarks: This header file defines all of the audio format constants and + * structures required for XAudio2 and XACT work. Providing these + * in a single location avoids certain dependency problems in the + * legacy audio headers (mmreg.h, mmsystem.h, ksmedia.h). + * + * NOTE: Including the legacy headers after this one may cause a + * compilation error, because they define some of the same types + * defined here without preprocessor guards to avoid multiple + * definitions. If a source file needs one of the old headers, + * it must include it before including audiodefs.h. + * + ***************************************************************************/ + +#ifndef __AUDIODEFS_INCLUDED__ +#define __AUDIODEFS_INCLUDED__ + +#include // For WORD, DWORD, etc. + +#pragma pack(push, 1) // Pack structures to 1-byte boundaries + + +/************************************************************************** + * + * WAVEFORMATEX: Base structure for many audio formats. Format-specific + * extensions can be defined for particular formats by using a non-zero + * cbSize value and adding extra fields to the end of this structure. + * + ***************************************************************************/ + +#ifndef _WAVEFORMATEX_ + + #define _WAVEFORMATEX_ + typedef struct tWAVEFORMATEX + { + WORD wFormatTag; // Integer identifier of the format + WORD nChannels; // Number of audio channels + DWORD nSamplesPerSec; // Audio sample rate + DWORD nAvgBytesPerSec; // Bytes per second (possibly approximate) + WORD nBlockAlign; // Size in bytes of a sample block (all channels) + WORD wBitsPerSample; // Size in bits of a single per-channel sample + WORD cbSize; // Bytes of extra data appended to this struct + } WAVEFORMATEX; + +#endif + +// Defining pointer types outside of the #if block to make sure they are +// defined even if mmreg.h or mmsystem.h is #included before this file + +typedef WAVEFORMATEX *PWAVEFORMATEX, *NPWAVEFORMATEX, *LPWAVEFORMATEX; +typedef const WAVEFORMATEX *PCWAVEFORMATEX, *LPCWAVEFORMATEX; + + +/************************************************************************** + * + * WAVEFORMATEXTENSIBLE: Extended version of WAVEFORMATEX that should be + * used as a basis for all new audio formats. The format tag is replaced + * with a GUID, allowing new formats to be defined without registering a + * format tag with Microsoft. There are also new fields that can be used + * to specify the spatial positions for each channel and the bit packing + * used for wide samples (e.g. 24-bit PCM samples in 32-bit containers). + * + ***************************************************************************/ + +#ifndef _WAVEFORMATEXTENSIBLE_ + + #define _WAVEFORMATEXTENSIBLE_ + typedef struct + { + WAVEFORMATEX Format; // Base WAVEFORMATEX data + union + { + WORD wValidBitsPerSample; // Valid bits in each sample container + WORD wSamplesPerBlock; // Samples per block of audio data; valid + // if wBitsPerSample=0 (but rarely used). + WORD wReserved; // Zero if neither case above applies. + } Samples; + DWORD dwChannelMask; // Positions of the audio channels + GUID SubFormat; // Format identifier GUID + } WAVEFORMATEXTENSIBLE; + +#endif + +typedef WAVEFORMATEXTENSIBLE *PWAVEFORMATEXTENSIBLE, *LPWAVEFORMATEXTENSIBLE; +typedef const WAVEFORMATEXTENSIBLE *PCWAVEFORMATEXTENSIBLE, *LPCWAVEFORMATEXTENSIBLE; + + + +/************************************************************************** + * + * Define the most common wave format tags used in WAVEFORMATEX formats. + * + ***************************************************************************/ + +#ifndef WAVE_FORMAT_PCM // Pulse Code Modulation + + // If WAVE_FORMAT_PCM is not defined, we need to define some legacy types + // for compatibility with the Windows mmreg.h / mmsystem.h header files. + + // Old general format structure (information common to all formats) + typedef struct waveformat_tag + { + WORD wFormatTag; + WORD nChannels; + DWORD nSamplesPerSec; + DWORD nAvgBytesPerSec; + WORD nBlockAlign; + } WAVEFORMAT, *PWAVEFORMAT, NEAR *NPWAVEFORMAT, FAR *LPWAVEFORMAT; + + // Specific format structure for PCM data + typedef struct pcmwaveformat_tag + { + WAVEFORMAT wf; + WORD wBitsPerSample; + } PCMWAVEFORMAT, *PPCMWAVEFORMAT, NEAR *NPPCMWAVEFORMAT, FAR *LPPCMWAVEFORMAT; + + #define WAVE_FORMAT_PCM 0x0001 + +#endif + +#ifndef WAVE_FORMAT_ADPCM // Microsoft Adaptive Differental PCM + + // Replicate the Microsoft ADPCM type definitions from mmreg.h. + + typedef struct adpcmcoef_tag + { + short iCoef1; + short iCoef2; + } ADPCMCOEFSET; + + #pragma warning(push) + #pragma warning(disable:4200) // Disable zero-sized array warnings + + typedef struct adpcmwaveformat_tag { + WAVEFORMATEX wfx; + WORD wSamplesPerBlock; + WORD wNumCoef; + ADPCMCOEFSET aCoef[]; // Always 7 coefficient pairs for MS ADPCM + } ADPCMWAVEFORMAT; + + #pragma warning(pop) + + #define WAVE_FORMAT_ADPCM 0x0002 + +#endif + +// Other frequently used format tags + +#ifndef WAVE_FORMAT_UNKNOWN + #define WAVE_FORMAT_UNKNOWN 0x0000 // Unknown or invalid format tag +#endif + +#ifndef WAVE_FORMAT_IEEE_FLOAT + #define WAVE_FORMAT_IEEE_FLOAT 0x0003 // 32-bit floating-point +#endif + +#ifndef WAVE_FORMAT_MPEGLAYER3 + #define WAVE_FORMAT_MPEGLAYER3 0x0055 // ISO/MPEG Layer3 +#endif + +#ifndef WAVE_FORMAT_DOLBY_AC3_SPDIF + #define WAVE_FORMAT_DOLBY_AC3_SPDIF 0x0092 // Dolby Audio Codec 3 over S/PDIF +#endif + +#ifndef WAVE_FORMAT_WMAUDIO2 + #define WAVE_FORMAT_WMAUDIO2 0x0161 // Windows Media Audio +#endif + +#ifndef WAVE_FORMAT_WMAUDIO3 + #define WAVE_FORMAT_WMAUDIO3 0x0162 // Windows Media Audio Pro +#endif + +#ifndef WAVE_FORMAT_WMASPDIF + #define WAVE_FORMAT_WMASPDIF 0x0164 // Windows Media Audio over S/PDIF +#endif + +#ifndef WAVE_FORMAT_EXTENSIBLE + #define WAVE_FORMAT_EXTENSIBLE 0xFFFE // All WAVEFORMATEXTENSIBLE formats +#endif + + +/************************************************************************** + * + * Define the most common wave format GUIDs used in WAVEFORMATEXTENSIBLE + * formats. Note that including the Windows ksmedia.h header after this + * one will cause build problems; this cannot be avoided, since ksmedia.h + * defines these macros without preprocessor guards. + * + ***************************************************************************/ + +#ifdef __cplusplus // uuid() and __uuidof() are only available in C++ + + #ifndef KSDATAFORMAT_SUBTYPE_PCM + struct __declspec(uuid("00000001-0000-0010-8000-00aa00389b71")) KSDATAFORMAT_SUBTYPE_PCM_STRUCT; + #define KSDATAFORMAT_SUBTYPE_PCM __uuidof(KSDATAFORMAT_SUBTYPE_PCM_STRUCT) + #endif + + #ifndef KSDATAFORMAT_SUBTYPE_ADPCM + struct __declspec(uuid("00000002-0000-0010-8000-00aa00389b71")) KSDATAFORMAT_SUBTYPE_ADPCM_STRUCT; + #define KSDATAFORMAT_SUBTYPE_ADPCM __uuidof(KSDATAFORMAT_SUBTYPE_ADPCM_STRUCT) + #endif + + #ifndef KSDATAFORMAT_SUBTYPE_IEEE_FLOAT + struct __declspec(uuid("00000003-0000-0010-8000-00aa00389b71")) KSDATAFORMAT_SUBTYPE_IEEE_FLOAT_STRUCT; + #define KSDATAFORMAT_SUBTYPE_IEEE_FLOAT __uuidof(KSDATAFORMAT_SUBTYPE_IEEE_FLOAT_STRUCT) + #endif + +#endif + + +/************************************************************************** + * + * Speaker positions used in the WAVEFORMATEXTENSIBLE dwChannelMask field. + * + ***************************************************************************/ + +#ifndef SPEAKER_FRONT_LEFT + #define SPEAKER_FRONT_LEFT 0x00000001 + #define SPEAKER_FRONT_RIGHT 0x00000002 + #define SPEAKER_FRONT_CENTER 0x00000004 + #define SPEAKER_LOW_FREQUENCY 0x00000008 + #define SPEAKER_BACK_LEFT 0x00000010 + #define SPEAKER_BACK_RIGHT 0x00000020 + #define SPEAKER_FRONT_LEFT_OF_CENTER 0x00000040 + #define SPEAKER_FRONT_RIGHT_OF_CENTER 0x00000080 + #define SPEAKER_BACK_CENTER 0x00000100 + #define SPEAKER_SIDE_LEFT 0x00000200 + #define SPEAKER_SIDE_RIGHT 0x00000400 + #define SPEAKER_TOP_CENTER 0x00000800 + #define SPEAKER_TOP_FRONT_LEFT 0x00001000 + #define SPEAKER_TOP_FRONT_CENTER 0x00002000 + #define SPEAKER_TOP_FRONT_RIGHT 0x00004000 + #define SPEAKER_TOP_BACK_LEFT 0x00008000 + #define SPEAKER_TOP_BACK_CENTER 0x00010000 + #define SPEAKER_TOP_BACK_RIGHT 0x00020000 + #define SPEAKER_RESERVED 0x7FFC0000 + #define SPEAKER_ALL 0x80000000 + #define _SPEAKER_POSITIONS_ +#endif + +#ifndef SPEAKER_STEREO + #define SPEAKER_MONO (SPEAKER_FRONT_CENTER) + #define SPEAKER_STEREO (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT) + #define SPEAKER_2POINT1 (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_LOW_FREQUENCY) + #define SPEAKER_SURROUND (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_FRONT_CENTER | SPEAKER_BACK_CENTER) + #define SPEAKER_QUAD (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_BACK_LEFT | SPEAKER_BACK_RIGHT) + #define SPEAKER_4POINT1 (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_LOW_FREQUENCY | SPEAKER_BACK_LEFT | SPEAKER_BACK_RIGHT) + #define SPEAKER_5POINT1 (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_FRONT_CENTER | SPEAKER_LOW_FREQUENCY | SPEAKER_BACK_LEFT | SPEAKER_BACK_RIGHT) + #define SPEAKER_7POINT1 (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_FRONT_CENTER | SPEAKER_LOW_FREQUENCY | SPEAKER_BACK_LEFT | SPEAKER_BACK_RIGHT | SPEAKER_FRONT_LEFT_OF_CENTER | SPEAKER_FRONT_RIGHT_OF_CENTER) + #define SPEAKER_5POINT1_SURROUND (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_FRONT_CENTER | SPEAKER_LOW_FREQUENCY | SPEAKER_SIDE_LEFT | SPEAKER_SIDE_RIGHT) + #define SPEAKER_7POINT1_SURROUND (SPEAKER_FRONT_LEFT | SPEAKER_FRONT_RIGHT | SPEAKER_FRONT_CENTER | SPEAKER_LOW_FREQUENCY | SPEAKER_BACK_LEFT | SPEAKER_BACK_RIGHT | SPEAKER_SIDE_LEFT | SPEAKER_SIDE_RIGHT) +#endif + + +#pragma pack(pop) + +#endif // #ifndef __AUDIODEFS_INCLUDED__ diff --git a/WickedEngine/Xaudio2_7/comdecl.h b/WickedEngine/Xaudio2_7/comdecl.h new file mode 100644 index 000000000..2ae9a961e --- /dev/null +++ b/WickedEngine/Xaudio2_7/comdecl.h @@ -0,0 +1,59 @@ +// comdecl.h: Macros to facilitate COM interface and GUID declarations. +// Copyright (c) Microsoft Corporation. All rights reserved. + +#ifndef _COMDECL_H_ +#define _COMDECL_H_ + +#ifndef _XBOX + #include // For standard COM interface macros +#else + #pragma warning(push) + #pragma warning(disable:4061) + #include // Required by xobjbase.h + #include // Special definitions for Xbox build + #pragma warning(pop) +#endif + +// The DEFINE_CLSID() and DEFINE_IID() macros defined below allow COM GUIDs to +// be declared and defined in such a way that clients can obtain the GUIDs using +// either the __uuidof() extension or the old-style CLSID_Foo / IID_IFoo names. +// If using the latter approach, the client can also choose whether to get the +// GUID definitions by defining the INITGUID preprocessor constant or by linking +// to a GUID library. This works in either C or C++. + +#ifdef __cplusplus + + #define DECLSPEC_UUID_WRAPPER(x) __declspec(uuid(#x)) + #ifdef INITGUID + + #define DEFINE_CLSID(className, l, w1, w2, b1, b2, b3, b4, b5, b6, b7, b8) \ + class DECLSPEC_UUID_WRAPPER(l##-##w1##-##w2##-##b1##b2##-##b3##b4##b5##b6##b7##b8) className; \ + EXTERN_C const GUID DECLSPEC_SELECTANY CLSID_##className = __uuidof(className) + + #define DEFINE_IID(interfaceName, l, w1, w2, b1, b2, b3, b4, b5, b6, b7, b8) \ + interface DECLSPEC_UUID_WRAPPER(l##-##w1##-##w2##-##b1##b2##-##b3##b4##b5##b6##b7##b8) interfaceName; \ + EXTERN_C const GUID DECLSPEC_SELECTANY IID_##interfaceName = __uuidof(interfaceName) + + #else // INITGUID + + #define DEFINE_CLSID(className, l, w1, w2, b1, b2, b3, b4, b5, b6, b7, b8) \ + class DECLSPEC_UUID_WRAPPER(l##-##w1##-##w2##-##b1##b2##-##b3##b4##b5##b6##b7##b8) className; \ + EXTERN_C const GUID CLSID_##className + + #define DEFINE_IID(interfaceName, l, w1, w2, b1, b2, b3, b4, b5, b6, b7, b8) \ + interface DECLSPEC_UUID_WRAPPER(l##-##w1##-##w2##-##b1##b2##-##b3##b4##b5##b6##b7##b8) interfaceName; \ + EXTERN_C const GUID IID_##interfaceName + + #endif // INITGUID + +#else // __cplusplus + + #define DEFINE_CLSID(className, l, w1, w2, b1, b2, b3, b4, b5, b6, b7, b8) \ + DEFINE_GUID(CLSID_##className, 0x##l, 0x##w1, 0x##w2, 0x##b1, 0x##b2, 0x##b3, 0x##b4, 0x##b5, 0x##b6, 0x##b7, 0x##b8) + + #define DEFINE_IID(interfaceName, l, w1, w2, b1, b2, b3, b4, b5, b6, b7, b8) \ + DEFINE_GUID(IID_##interfaceName, 0x##l, 0x##w1, 0x##w2, 0x##b1, 0x##b2, 0x##b3, 0x##b4, 0x##b5, 0x##b6, 0x##b7, 0x##b8) + +#endif // __cplusplus + +#endif // #ifndef _COMDECL_H_ diff --git a/WickedEngine/Xaudio2_7/xma2defs.h b/WickedEngine/Xaudio2_7/xma2defs.h new file mode 100644 index 000000000..ce64a61af --- /dev/null +++ b/WickedEngine/Xaudio2_7/xma2defs.h @@ -0,0 +1,718 @@ +/*************************************************************************** + * + * Copyright (c) Microsoft Corporation. All rights reserved. + * + * File: xma2defs.h + * Content: Constants, data types and functions for XMA2 compressed audio. + * + ***************************************************************************/ + +#ifndef __XMA2DEFS_INCLUDED__ +#define __XMA2DEFS_INCLUDED__ + +#include // Markers for documenting API semantics +#include // For S_OK, E_FAIL +#include "audiodefs.h" // Basic data types and constants for audio work + + +/*************************************************************************** + * Overview + ***************************************************************************/ + +// A typical XMA2 file contains these RIFF chunks: +// +// 'fmt' or 'XMA2' chunk (or both): A description of the XMA data's structure +// and characteristics (length, channels, sample rate, loops, block size, etc). +// +// 'seek' chunk: A seek table to help navigate the XMA data. +// +// 'data' chunk: The encoded XMA2 data. +// +// The encoded XMA2 data is structured as a set of BLOCKS, which contain PACKETS, +// which contain FRAMES, which contain SUBFRAMES (roughly speaking). The frames +// in a file may also be divided into several subsets, called STREAMS. +// +// FRAME: A variable-sized segment of XMA data that decodes to exactly 512 mono +// or stereo PCM samples. This is the smallest unit of XMA data that can +// be decoded in isolation. Frames are an arbitrary number of bits in +// length, and need not be byte-aligned. See "XMA frame structure" below. +// +// SUBFRAME: A region of bits in an XMA frame that decodes to 128 mono or stereo +// samples. The XMA decoder cannot decode a subframe in isolation; it needs +// a whole frame to work with. However, it can begin emitting the frame's +// decoded samples at any one of the four subframe boundaries. Subframes +// can be addressed for seeking and looping purposes. +// +// PACKET: A 2Kb region containing a 32-bit header and some XMA frames. Frames +// can (and usually do) span packets. A packet's header includes the offset +// in bits of the first frame that begins within that packet. All of the +// frames that begin in a given packet belong to the same "stream" (see the +// Multichannel Audio section below). +// +// STREAM: A set of packets within an XMA file that all contain data for the +// same mono or stereo component of a PCM file with more than two channels. +// The packets comprising a given stream may be interleaved with each other +// more or less arbitrarily; see Multichannel Audio. +// +// BLOCK: An array of XMA packets; or, to break it down differently, a series of +// consecutive XMA frames, padded at the end with reserved data. A block +// must contain at least one 2Kb packet per stream, and it can hold up to +// 4095 packets (8190Kb), but its size is typically in the 32Kb-128Kb range. +// (The size chosen involves a trade-off between memory use and efficiency +// of reading from permanent storage.) +// +// XMA frames do not span blocks, so a block is guaranteed to begin with a +// set of complete frames, one per stream. Also, a block in a multi-stream +// XMA2 file always contains the same number of samples for each stream; +// see Multichannel Audio. +// +// The 'data' chunk in an XMA2 file is an array of XMA2WAVEFORMAT.BlockCount XMA +// blocks, all the same size (as specified in XMA2WAVEFORMAT.BlockSizeInBytes) +// except for the last one, which may be shorter. + + +// MULTICHANNEL AUDIO: the XMA decoder can only decode raw XMA data into either +// mono or stereo PCM data. In order to encode a 6-channel file (say), the file +// must be deinterleaved into 3 stereo streams that are encoded independently, +// producing 3 encoded XMA data streams. Then the packets in these 3 streams +// are interleaved to produce a single XMA2 file, and some information is added +// to the file so that the original 6-channel audio can be reconstructed at +// decode time. This works using the concept of an XMA stream (see above). +// +// The frames for all the streams in an XMA file are interleaved in an arbitrary +// order. To locate a frame that belongs to a given stream in a given XMA block, +// you must examine the first few packets in the block. Here (and only here) the +// packets are guaranteed to be presented in stream order, so that all frames +// beginning in packet 0 belong to stream 0 (the first stereo pair), etc. +// +// (This means that when decoding multi-stream XMA files, only entire XMA blocks +// should be submitted to the decoder; otherwise it cannot know which frames +// belong to which stream.) +// +// Once you have one frame that belongs to a given stream, you can find the next +// one by looking at the frame's 'NextFrameOffsetBits' value (which is stored in +// its first 15 bits; see XMAFRAME below). The GetXmaFrameBitPosition function +// uses this technique. + + +// SEEKING IN XMA2 FILES: Here is some pseudocode to find the byte position and +// subframe in an XMA2 file which will contain sample S when decoded. +// +// 1. Traverse the seek table to find the XMA2 block containing sample S. The +// seek table is an array of big-endian DWORDs, one per block in the file. +// The Nth DWORD is the total number of PCM samples that would be obtained +// by decoding the entire XMA file up to the end of block N. Hence, the +// block we want is the first one whose seek table entry is greater than S. +// (See the GetXmaBlockContainingSample helper function.) +// +// 2. Calculate which frame F within the block found above contains sample S. +// Since each frame decodes to 512 samples, this is straightforward. The +// first frame in the block produces samples X to X + 512, where X is the +// seek table entry for the prior block. So F is (S - X) / 512. +// +// 3. Find the bit offset within the block where frame F starts. Since frames +// are variable-sized, this can only be done by traversing all the frames in +// the block until we reach frame F. (See GetXmaFrameBitPosition.) +// +// 4. Frame F has four 128-sample subframes. To find the subframe containing S, +// we can use the formula (S % 512) / 128. +// +// In the case of multi-stream XMA files, sample S is a multichannel sample with +// parts coming from several frames, one per stream. To find all these frames, +// steps 2-4 need to be repeated for each stream N, using the knowledge that the +// first packets in a block are presented in stream order. The frame traversal +// in step 3 must be started at the first frame in the Nth packet of the block, +// which will be the first frame for stream N. (And the packet header will tell +// you the first frame's start position within the packet.) +// +// Step 1 can be performed using the GetXmaBlockContainingSample function below, +// and steps 2-4 by calling GetXmaDecodePositionForSample once for each stream. + + + +/*************************************************************************** + * XMA constants + ***************************************************************************/ + +// Size of the PCM samples produced by the XMA decoder +#define XMA_OUTPUT_SAMPLE_BYTES 2u +#define XMA_OUTPUT_SAMPLE_BITS (XMA_OUTPUT_SAMPLE_BYTES * 8u) + +// Size of an XMA packet +#define XMA_BYTES_PER_PACKET 2048u +#define XMA_BITS_PER_PACKET (XMA_BYTES_PER_PACKET * 8u) + +// Size of an XMA packet header +#define XMA_PACKET_HEADER_BYTES 4u +#define XMA_PACKET_HEADER_BITS (XMA_PACKET_HEADER_BYTES * 8u) + +// Sample blocks in a decoded XMA frame +#define XMA_SAMPLES_PER_FRAME 512u + +// Sample blocks in a decoded XMA subframe +#define XMA_SAMPLES_PER_SUBFRAME 128u + +// Maximum encoded data that can be submitted to the XMA decoder at a time +#define XMA_READBUFFER_MAX_PACKETS 4095u +#define XMA_READBUFFER_MAX_BYTES (XMA_READBUFFER_MAX_PACKETS * XMA_BYTES_PER_PACKET) + +// Maximum size allowed for the XMA decoder's output buffers +#define XMA_WRITEBUFFER_MAX_BYTES (31u * 256u) + +// Required byte alignment of the XMA decoder's output buffers +#define XMA_WRITEBUFFER_BYTE_ALIGNMENT 256u + +// Decode chunk sizes for the XMA_PLAYBACK_INIT.subframesToDecode field +#define XMA_MIN_SUBFRAMES_TO_DECODE 1u +#define XMA_MAX_SUBFRAMES_TO_DECODE 8u +#define XMA_OPTIMAL_SUBFRAMES_TO_DECODE 4u + +// LoopCount<255 means finite repetitions; LoopCount=255 means infinite looping +#define XMA_MAX_LOOPCOUNT 254u +#define XMA_INFINITE_LOOP 255u + + + +/*************************************************************************** + * XMA format structures + ***************************************************************************/ + +// The currently recommended way to express format information for XMA2 files +// is the XMA2WAVEFORMATEX structure. This structure is fully compliant with +// the WAVEFORMATEX standard and contains all the information needed to parse +// and manage XMA2 files in a compact way. + +#define WAVE_FORMAT_XMA2 0x166 + +typedef struct XMA2WAVEFORMATEX +{ + WAVEFORMATEX wfx; + // Meaning of the WAVEFORMATEX fields here: + // wFormatTag; // Audio format type; always WAVE_FORMAT_XMA2 + // nChannels; // Channel count of the decoded audio + // nSamplesPerSec; // Sample rate of the decoded audio + // nAvgBytesPerSec; // Used internally by the XMA encoder + // nBlockAlign; // Decoded sample size; channels * wBitsPerSample / 8 + // wBitsPerSample; // Bits per decoded mono sample; always 16 for XMA + // cbSize; // Size in bytes of the rest of this structure (34) + + WORD NumStreams; // Number of audio streams (1 or 2 channels each) + DWORD ChannelMask; // Spatial positions of the channels in this file, + // stored as SPEAKER_xxx values (see audiodefs.h) + DWORD SamplesEncoded; // Total number of PCM samples the file decodes to + DWORD BytesPerBlock; // XMA block size (but the last one may be shorter) + DWORD PlayBegin; // First valid sample in the decoded audio + DWORD PlayLength; // Length of the valid part of the decoded audio + DWORD LoopBegin; // Beginning of the loop region in decoded sample terms + DWORD LoopLength; // Length of the loop region in decoded sample terms + BYTE LoopCount; // Number of loop repetitions; 255 = infinite + BYTE EncoderVersion; // Version of XMA encoder that generated the file + WORD BlockCount; // XMA blocks in file (and entries in its seek table) +} XMA2WAVEFORMATEX, *PXMA2WAVEFORMATEX; + + +// The legacy XMA format structures are described here for reference, but they +// should not be used in new content. XMAWAVEFORMAT was the structure used in +// XMA version 1 files. XMA2WAVEFORMAT was used in early XMA2 files; it is not +// placed in the usual 'fmt' RIFF chunk but in its own 'XMA2' chunk. + +#ifndef WAVE_FORMAT_XMA +#define WAVE_FORMAT_XMA 0x0165 + +// Values used in the ChannelMask fields below. Similar to the SPEAKER_xxx +// values defined in audiodefs.h, but modified to fit in a single byte. +#ifndef XMA_SPEAKER_LEFT + #define XMA_SPEAKER_LEFT 0x01 + #define XMA_SPEAKER_RIGHT 0x02 + #define XMA_SPEAKER_CENTER 0x04 + #define XMA_SPEAKER_LFE 0x08 + #define XMA_SPEAKER_LEFT_SURROUND 0x10 + #define XMA_SPEAKER_RIGHT_SURROUND 0x20 + #define XMA_SPEAKER_LEFT_BACK 0x40 + #define XMA_SPEAKER_RIGHT_BACK 0x80 +#endif + + +// Used in XMAWAVEFORMAT for per-stream data +typedef struct XMASTREAMFORMAT +{ + DWORD PsuedoBytesPerSec; // Used by the XMA encoder (typo preserved for legacy reasons) + DWORD SampleRate; // The stream's decoded sample rate (in XMA2 files, + // this is the same for all streams in the file). + DWORD LoopStart; // Bit offset of the frame containing the loop start + // point, relative to the beginning of the stream. + DWORD LoopEnd; // Bit offset of the frame containing the loop end. + BYTE SubframeData; // Two 4-bit numbers specifying the exact location of + // the loop points within the frames that contain them. + // SubframeEnd: Subframe of the loop end frame where + // the loop ends. Ranges from 0 to 3. + // SubframeSkip: Subframes to skip in the start frame to + // reach the loop. Ranges from 0 to 4. + BYTE Channels; // Number of channels in the stream (1 or 2) + WORD ChannelMask; // Spatial positions of the channels in the stream +} XMASTREAMFORMAT; + +// Legacy XMA1 format structure +typedef struct XMAWAVEFORMAT +{ + WORD FormatTag; // Audio format type (always WAVE_FORMAT_XMA) + WORD BitsPerSample; // Bit depth (currently required to be 16) + WORD EncodeOptions; // Options for XMA encoder/decoder + WORD LargestSkip; // Largest skip used in interleaving streams + WORD NumStreams; // Number of interleaved audio streams + BYTE LoopCount; // Number of loop repetitions; 255 = infinite + BYTE Version; // XMA encoder version that generated the file. + // Always 3 or higher for XMA2 files. + XMASTREAMFORMAT XmaStreams[1]; // Per-stream format information; the actual + // array length is in the NumStreams field. +} XMAWAVEFORMAT; + + +// Used in XMA2WAVEFORMAT for per-stream data +typedef struct XMA2STREAMFORMAT +{ + BYTE Channels; // Number of channels in the stream (1 or 2) + BYTE RESERVED; // Reserved for future use + WORD ChannelMask; // Spatial positions of the channels in the stream +} XMA2STREAMFORMAT; + +// Legacy XMA2 format structure (big-endian byte ordering) +typedef struct XMA2WAVEFORMAT +{ + BYTE Version; // XMA encoder version that generated the file. + // Always 3 or higher for XMA2 files. + BYTE NumStreams; // Number of interleaved audio streams + BYTE RESERVED; // Reserved for future use + BYTE LoopCount; // Number of loop repetitions; 255 = infinite + DWORD LoopBegin; // Loop begin point, in samples + DWORD LoopEnd; // Loop end point, in samples + DWORD SampleRate; // The file's decoded sample rate + DWORD EncodeOptions; // Options for the XMA encoder/decoder + DWORD PsuedoBytesPerSec; // Used internally by the XMA encoder + DWORD BlockSizeInBytes; // Size in bytes of this file's XMA blocks (except + // possibly the last one). Always a multiple of + // 2Kb, since XMA blocks are arrays of 2Kb packets. + DWORD SamplesEncoded; // Total number of PCM samples encoded in this file + DWORD SamplesInSource; // Actual number of PCM samples in the source + // material used to generate this file + DWORD BlockCount; // Number of XMA blocks in this file (and hence + // also the number of entries in its seek table) + XMA2STREAMFORMAT Streams[1]; // Per-stream format information; the actual + // array length is in the NumStreams field. +} XMA2WAVEFORMAT; + +#endif // #ifndef WAVE_FORMAT_XMA + + + +/*************************************************************************** + * XMA packet structure (in big-endian form) + ***************************************************************************/ + +typedef struct XMA2PACKET +{ + int FrameCount : 6; // Number of XMA frames that begin in this packet + int FrameOffsetInBits : 15; // Bit of XmaData where the first complete frame begins + int PacketMetaData : 3; // Metadata stored in the packet (always 1 for XMA2) + int PacketSkipCount : 8; // How many packets belonging to other streams must be + // skipped to find the next packet belonging to this one + BYTE XmaData[XMA_BYTES_PER_PACKET - sizeof(DWORD)]; // XMA encoded data +} XMA2PACKET; + +// E.g. if the first DWORD of a packet is 0x30107902: +// +// 001100 000001000001111 001 00000010 +// | | | |____ Skip 2 packets to find the next one for this stream +// | | |___________ XMA2 signature (always 001) +// | |_____________________ First frame starts 527 bits into packet +// |________________________________ Packet contains 12 frames + + +// Helper functions to extract the fields above from an XMA packet. (Note that +// the bitfields cannot be read directly on little-endian architectures such as +// the Intel x86, as they are laid out in big-endian form.) + +__inline DWORD GetXmaPacketFrameCount(__in_bcount(1) const BYTE* pPacket) +{ + return (DWORD)(pPacket[0] >> 2); +} + +__inline DWORD GetXmaPacketFirstFrameOffsetInBits(__in_bcount(3) const BYTE* pPacket) +{ + return ((DWORD)(pPacket[0] & 0x3) << 13) | + ((DWORD)(pPacket[1]) << 5) | + ((DWORD)(pPacket[2]) >> 3); +} + +__inline DWORD GetXmaPacketMetadata(__in_bcount(3) const BYTE* pPacket) +{ + return (DWORD)(pPacket[2] & 0x7); +} + +__inline DWORD GetXmaPacketSkipCount(__in_bcount(4) const BYTE* pPacket) +{ + return (DWORD)(pPacket[3]); +} + + + +/*************************************************************************** + * XMA frame structure + ***************************************************************************/ + +// There is no way to represent the XMA frame as a C struct, since it is a +// variable-sized string of bits that need not be stored at a byte-aligned +// position in memory. This is the layout: +// +// XMAFRAME +// { +// LengthInBits: A 15-bit number representing the length of this frame. +// XmaData: Encoded XMA data; its size in bits is (LengthInBits - 15). +// } + +// Size in bits of the frame's initial LengthInBits field +#define XMA_BITS_IN_FRAME_LENGTH_FIELD 15 + +// Special LengthInBits value that marks an invalid final frame +#define XMA_FINAL_FRAME_MARKER 0x7FFF + + + +/*************************************************************************** + * XMA helper functions + ***************************************************************************/ + +// We define a local ASSERT macro to equal the global one if it exists. +// You can define XMA2DEFS_ASSERT in advance to override this default. +#ifndef XMA2DEFS_ASSERT + #ifdef ASSERT + #define XMA2DEFS_ASSERT ASSERT + #else + #define XMA2DEFS_ASSERT(a) /* No-op by default */ + #endif +#endif + + +// GetXmaBlockContainingSample: Use a given seek table to find the XMA block +// containing a given decoded sample. Note that the seek table entries in an +// XMA file are stored in big-endian form and may need to be converted prior +// to calling this function. + +__inline HRESULT GetXmaBlockContainingSample +( + DWORD nBlockCount, // Blocks in the file (= seek table entries) + __in_ecount(nBlockCount) const DWORD* pSeekTable, // Pointer to the seek table data + DWORD nDesiredSample, // Decoded sample to locate + __out DWORD* pnBlockContainingSample, // Index of the block containing the sample + __out DWORD* pnSampleOffsetWithinBlock // Position of the sample in this block +) +{ + DWORD nPreviousTotalSamples = 0; + DWORD nBlock; + DWORD nTotalSamplesSoFar; + + XMA2DEFS_ASSERT(pSeekTable); + XMA2DEFS_ASSERT(pnBlockContainingSample); + XMA2DEFS_ASSERT(pnSampleOffsetWithinBlock); + + for (nBlock = 0; nBlock < nBlockCount; ++nBlock) + { + nTotalSamplesSoFar = pSeekTable[nBlock]; + if (nTotalSamplesSoFar > nDesiredSample) + { + *pnBlockContainingSample = nBlock; + *pnSampleOffsetWithinBlock = nDesiredSample - nPreviousTotalSamples; + return S_OK; + } + nPreviousTotalSamples = nTotalSamplesSoFar; + } + + return E_FAIL; +} + + +// GetXmaFrameLengthInBits: Reads a given frame's LengthInBits field. + +__inline DWORD GetXmaFrameLengthInBits +( + __in_bcount(nBitPosition / 8 + 3) + __in const BYTE* pPacket, // Pointer to XMA packet[s] containing the frame + DWORD nBitPosition // Bit offset of the frame within this packet +) +{ + DWORD nRegion; + DWORD nBytePosition = nBitPosition / 8; + DWORD nBitOffset = nBitPosition % 8; + + if (nBitOffset < 2) // Only need to read 2 bytes (and might not be safe to read more) + { + nRegion = (DWORD)(pPacket[nBytePosition+0]) << 8 | + (DWORD)(pPacket[nBytePosition+1]); + return (nRegion >> (1 - nBitOffset)) & 0x7FFF; // Last 15 bits + } + else // Need to read 3 bytes + { + nRegion = (DWORD)(pPacket[nBytePosition+0]) << 16 | + (DWORD)(pPacket[nBytePosition+1]) << 8 | + (DWORD)(pPacket[nBytePosition+2]); + return (nRegion >> (9 - nBitOffset)) & 0x7FFF; // Last 15 bits + } +} + + +// GetXmaFrameBitPosition: Calculates the bit offset of a given frame within +// an XMA block or set of blocks. Returns 0 on failure. + +__inline DWORD GetXmaFrameBitPosition +( + __in_bcount(nXmaDataBytes) const BYTE* pXmaData, // Pointer to XMA block[s] + DWORD nXmaDataBytes, // Size of pXmaData in bytes + DWORD nStreamIndex, // Stream within which to seek + DWORD nDesiredFrame // Frame sought +) +{ + const BYTE* pCurrentPacket; + DWORD nPacketsExamined = 0; + DWORD nFrameCountSoFar = 0; + DWORD nFramesToSkip; + DWORD nFrameBitOffset; + + XMA2DEFS_ASSERT(pXmaData); + XMA2DEFS_ASSERT(nXmaDataBytes % XMA_BYTES_PER_PACKET == 0); + + // Get the first XMA packet belonging to the desired stream, relying on the + // fact that the first packets for each stream are in consecutive order at + // the beginning of an XMA block. + + pCurrentPacket = pXmaData + nStreamIndex * XMA_BYTES_PER_PACKET; + for (;;) + { + // If we have exceeded the size of the XMA data, return failure + if (pCurrentPacket + XMA_BYTES_PER_PACKET > pXmaData + nXmaDataBytes) + { + return 0; + } + + // If the current packet contains the frame we are looking for... + if (nFrameCountSoFar + GetXmaPacketFrameCount(pCurrentPacket) > nDesiredFrame) + { + // See how many frames in this packet we need to skip to get to it + XMA2DEFS_ASSERT(nDesiredFrame >= nFrameCountSoFar); + nFramesToSkip = nDesiredFrame - nFrameCountSoFar; + + // Get the bit offset of the first frame in this packet + nFrameBitOffset = XMA_PACKET_HEADER_BITS + GetXmaPacketFirstFrameOffsetInBits(pCurrentPacket); + + // Advance nFrameBitOffset to the frame of interest + while (nFramesToSkip--) + { + nFrameBitOffset += GetXmaFrameLengthInBits(pCurrentPacket, nFrameBitOffset); + } + + // The bit offset to return is the number of bits from pXmaData to + // pCurrentPacket plus the bit offset of the frame of interest + return (DWORD)(pCurrentPacket - pXmaData) * 8 + nFrameBitOffset; + } + + // If we haven't found the right packet yet, advance our counters + ++nPacketsExamined; + nFrameCountSoFar += GetXmaPacketFrameCount(pCurrentPacket); + + // And skip to the next packet belonging to the same stream + pCurrentPacket += XMA_BYTES_PER_PACKET * (GetXmaPacketSkipCount(pCurrentPacket) + 1); + } +} + + +// GetLastXmaFrameBitPosition: Calculates the bit offset of the last complete +// frame in an XMA block or set of blocks. + +__inline DWORD GetLastXmaFrameBitPosition +( + __in_bcount(nXmaDataBytes) const BYTE* pXmaData, // Pointer to XMA block[s] + DWORD nXmaDataBytes, // Size of pXmaData in bytes + DWORD nStreamIndex // Stream within which to seek +) +{ + const BYTE* pLastPacket; + DWORD nBytesToNextPacket; + DWORD nFrameBitOffset; + DWORD nFramesInLastPacket; + + XMA2DEFS_ASSERT(pXmaData); + XMA2DEFS_ASSERT(nXmaDataBytes % XMA_BYTES_PER_PACKET == 0); + XMA2DEFS_ASSERT(nXmaDataBytes >= XMA_BYTES_PER_PACKET * (nStreamIndex + 1)); + + // Get the first XMA packet belonging to the desired stream, relying on the + // fact that the first packets for each stream are in consecutive order at + // the beginning of an XMA block. + pLastPacket = pXmaData + nStreamIndex * XMA_BYTES_PER_PACKET; + + // Search for the last packet belonging to the desired stream + for (;;) + { + nBytesToNextPacket = XMA_BYTES_PER_PACKET * (GetXmaPacketSkipCount(pLastPacket) + 1); + XMA2DEFS_ASSERT(nBytesToNextPacket); + if (pLastPacket + nBytesToNextPacket + XMA_BYTES_PER_PACKET > pXmaData + nXmaDataBytes) + { + break; // The next packet would extend beyond the end of pXmaData + } + pLastPacket += nBytesToNextPacket; + } + + // The last packet can sometimes have no seekable frames, in which case we + // have to use the previous one + if (GetXmaPacketFrameCount(pLastPacket) == 0) + { + pLastPacket -= nBytesToNextPacket; + } + + // Found the last packet. Get the bit offset of its first frame. + nFrameBitOffset = XMA_PACKET_HEADER_BITS + GetXmaPacketFirstFrameOffsetInBits(pLastPacket); + + // Traverse frames until we reach the last one + nFramesInLastPacket = GetXmaPacketFrameCount(pLastPacket); + while (--nFramesInLastPacket) + { + nFrameBitOffset += GetXmaFrameLengthInBits(pLastPacket, nFrameBitOffset); + } + + // The bit offset to return is the number of bits from pXmaData to + // pLastPacket plus the offset of the last frame in this packet. + return (DWORD)(pLastPacket - pXmaData) * 8 + nFrameBitOffset; +} + + +// GetXmaDecodePositionForSample: Obtains the information needed to make the +// decoder generate audio starting at a given sample position relative to the +// beginning of the given XMA block: the bit offset of the appropriate frame, +// and the right subframe within that frame. This data can be passed directly +// to the XMAPlaybackSetDecodePosition function. + +__inline HRESULT GetXmaDecodePositionForSample +( + __in_bcount(nXmaDataBytes) const BYTE* pXmaData, // Pointer to XMA block[s] + DWORD nXmaDataBytes, // Size of pXmaData in bytes + DWORD nStreamIndex, // Stream within which to seek + DWORD nDesiredSample, // Sample sought + __out DWORD* pnBitOffset, // Returns the bit offset within pXmaData of + // the frame containing the sample sought + __out DWORD* pnSubFrame // Returns the subframe containing the sample +) +{ + DWORD nDesiredFrame = nDesiredSample / XMA_SAMPLES_PER_FRAME; + DWORD nSubFrame = (nDesiredSample % XMA_SAMPLES_PER_FRAME) / XMA_SAMPLES_PER_SUBFRAME; + DWORD nBitOffset = GetXmaFrameBitPosition(pXmaData, nXmaDataBytes, nStreamIndex, nDesiredFrame); + + XMA2DEFS_ASSERT(pnBitOffset); + XMA2DEFS_ASSERT(pnSubFrame); + + if (nBitOffset) + { + *pnBitOffset = nBitOffset; + *pnSubFrame = nSubFrame; + return S_OK; + } + else + { + return E_FAIL; + } +} + + +// GetXmaSampleRate: Obtains the legal XMA sample rate (24, 32, 44.1 or 48Khz) +// corresponding to a generic sample rate. + +__inline DWORD GetXmaSampleRate(DWORD dwGeneralRate) +{ + DWORD dwXmaRate = 48000; // Default XMA rate for all rates above 44100Hz + + if (dwGeneralRate <= 24000) dwXmaRate = 24000; + else if (dwGeneralRate <= 32000) dwXmaRate = 32000; + else if (dwGeneralRate <= 44100) dwXmaRate = 44100; + + return dwXmaRate; +} + + +// Functions to convert between WAVEFORMATEXTENSIBLE channel masks (combinations +// of the SPEAKER_xxx flags defined in audiodefs.h) and XMA channel masks (which +// are limited to eight possible speaker positions: left, right, center, low +// frequency, side left, side right, back left and back right). + +__inline DWORD GetStandardChannelMaskFromXmaMask(BYTE bXmaMask) +{ + DWORD dwStandardMask = 0; + + if (bXmaMask & XMA_SPEAKER_LEFT) dwStandardMask |= SPEAKER_FRONT_LEFT; + if (bXmaMask & XMA_SPEAKER_RIGHT) dwStandardMask |= SPEAKER_FRONT_RIGHT; + if (bXmaMask & XMA_SPEAKER_CENTER) dwStandardMask |= SPEAKER_FRONT_CENTER; + if (bXmaMask & XMA_SPEAKER_LFE) dwStandardMask |= SPEAKER_LOW_FREQUENCY; + if (bXmaMask & XMA_SPEAKER_LEFT_SURROUND) dwStandardMask |= SPEAKER_SIDE_LEFT; + if (bXmaMask & XMA_SPEAKER_RIGHT_SURROUND) dwStandardMask |= SPEAKER_SIDE_RIGHT; + if (bXmaMask & XMA_SPEAKER_LEFT_BACK) dwStandardMask |= SPEAKER_BACK_LEFT; + if (bXmaMask & XMA_SPEAKER_RIGHT_BACK) dwStandardMask |= SPEAKER_BACK_RIGHT; + + return dwStandardMask; +} + +__inline BYTE GetXmaChannelMaskFromStandardMask(DWORD dwStandardMask) +{ + BYTE bXmaMask = 0; + + if (dwStandardMask & SPEAKER_FRONT_LEFT) bXmaMask |= XMA_SPEAKER_LEFT; + if (dwStandardMask & SPEAKER_FRONT_RIGHT) bXmaMask |= XMA_SPEAKER_RIGHT; + if (dwStandardMask & SPEAKER_FRONT_CENTER) bXmaMask |= XMA_SPEAKER_CENTER; + if (dwStandardMask & SPEAKER_LOW_FREQUENCY) bXmaMask |= XMA_SPEAKER_LFE; + if (dwStandardMask & SPEAKER_SIDE_LEFT) bXmaMask |= XMA_SPEAKER_LEFT_SURROUND; + if (dwStandardMask & SPEAKER_SIDE_RIGHT) bXmaMask |= XMA_SPEAKER_RIGHT_SURROUND; + if (dwStandardMask & SPEAKER_BACK_LEFT) bXmaMask |= XMA_SPEAKER_LEFT_BACK; + if (dwStandardMask & SPEAKER_BACK_RIGHT) bXmaMask |= XMA_SPEAKER_RIGHT_BACK; + + return bXmaMask; +} + + +// LocalizeXma2Format: Modifies a XMA2WAVEFORMATEX structure in place to comply +// with the current platform's byte-ordering rules (little- or big-endian). + +__inline HRESULT LocalizeXma2Format(__inout XMA2WAVEFORMATEX* pXma2Format) +{ + #define XMASWAP2BYTES(n) ((WORD)(((n) >> 8) | (((n) & 0xff) << 8))) + #define XMASWAP4BYTES(n) ((DWORD)((n) >> 24 | (n) << 24 | ((n) & 0xff00) << 8 | ((n) & 0xff0000) >> 8)) + + if (pXma2Format->wfx.wFormatTag == WAVE_FORMAT_XMA2) + { + return S_OK; + } + else if (XMASWAP2BYTES(pXma2Format->wfx.wFormatTag) == WAVE_FORMAT_XMA2) + { + pXma2Format->wfx.wFormatTag = XMASWAP2BYTES(pXma2Format->wfx.wFormatTag); + pXma2Format->wfx.nChannels = XMASWAP2BYTES(pXma2Format->wfx.nChannels); + pXma2Format->wfx.nSamplesPerSec = XMASWAP4BYTES(pXma2Format->wfx.nSamplesPerSec); + pXma2Format->wfx.nAvgBytesPerSec = XMASWAP4BYTES(pXma2Format->wfx.nAvgBytesPerSec); + pXma2Format->wfx.nBlockAlign = XMASWAP2BYTES(pXma2Format->wfx.nBlockAlign); + pXma2Format->wfx.wBitsPerSample = XMASWAP2BYTES(pXma2Format->wfx.wBitsPerSample); + pXma2Format->wfx.cbSize = XMASWAP2BYTES(pXma2Format->wfx.cbSize); + pXma2Format->NumStreams = XMASWAP2BYTES(pXma2Format->NumStreams); + pXma2Format->ChannelMask = XMASWAP4BYTES(pXma2Format->ChannelMask); + pXma2Format->SamplesEncoded = XMASWAP4BYTES(pXma2Format->SamplesEncoded); + pXma2Format->BytesPerBlock = XMASWAP4BYTES(pXma2Format->BytesPerBlock); + pXma2Format->PlayBegin = XMASWAP4BYTES(pXma2Format->PlayBegin); + pXma2Format->PlayLength = XMASWAP4BYTES(pXma2Format->PlayLength); + pXma2Format->LoopBegin = XMASWAP4BYTES(pXma2Format->LoopBegin); + pXma2Format->LoopLength = XMASWAP4BYTES(pXma2Format->LoopLength); + pXma2Format->BlockCount = XMASWAP2BYTES(pXma2Format->BlockCount); + return S_OK; + } + else + { + return E_FAIL; // Not a recognizable XMA2 format + } + + #undef XMASWAP2BYTES + #undef XMASWAP4BYTES +} + + +#endif // #ifndef __XMA2DEFS_INCLUDED__ diff --git a/WickedEngine/mysql/lib32/libmysql.dll b/WickedEngine/mysql/lib32/libmysql.dll new file mode 100644 index 0000000000000000000000000000000000000000..c6caefc747cf3a93b17313ce96218c189019fa60 GIT binary patch literal 4468224 zcmeFaeSB2awLg3&Gf9RpVFpYfXpn$GQBYHdC~*i5LPAi2CmAwBgamqpbULMqVGdv= zA@L-blkHf%RqOp#TPfJuTl*LZmn6x7KV6rkU3k+!zj*%lk3J-in)t#f z>6sC4Oxk5z_{OBg;UBKeSyOw*4{E>vRfBWrsM05UaRZgvPd(IDU&zW~iAm_(- ztXy?fdU{%}Njmw*J=e~0wXU-M4&FFsRWahXK9I4hM!YAjnuK>rbo{CkfyY+$33$$` zmFa{xdX-osbHNlLJ6YugL-WDd>R!!t0Nm^)2k+!^DU1q_$rKCwCZE2F^OqZl% zvjq6oF?f0Kmjf7uD4Zlo$&!uYLvJbPCZq8HvrB#?0+Dcs-bQqb>XtUS0nS?>t)ygm zf#gl0z!X5^~qstr+uA4@@fh(D?; zb$Y_T929$1?aJ>*zb{GdwWuuBJ&LzWw80RjAH(A+15J906-d(KsMtZTBsEY4hyVE{ zsqLyC8i*Um0*<;%S$LPv9Y#XPPk6O!Yi~nXv7M}mA+EuIX0OH&ikdC&J zUc$TfTX;7D`v3p?U$j8P8GdrI+r#%>AW5vqr4HDXpYXd7X>Lu z9trVYo{pEEx$oKqo^6j$Tj1d}5SDH8?wsr4x48)Ou2jAq!zTU~e_%AlH{@_Th6V!T zA`+J%X_X*1Qcw0HPwzpn$?q5i{QX|t*QG2iC?t4&qn=K0t!}WO@IeHWWWH~tBo)+o z6gR&C`1}SG!aqgfT}6XubzSGFAk}*h9$uZ$MyZ7un;t!y%rk)65s#he;JC@Ghas~^#jdq(Q%<5cOgqQTPG#=_FrW_DmDCVl3c0HEw)y!pU~P{}inzl2sEt=3%1*N{S(Ov1T2W8)NySx= zNtJXry#lV)RfCeRFPf~LO;WDfx^a5C2L(Gr!GL}M*lJym1DDCmSJ`@e5HvWn~9j#m#hY8-EGC-of+=c3iD<+0btn0ZCV!tklCQ zT>Lm#sjQ-g!6@2`w(QU|cjPbd>~TmU6EO9B2X$QLc7XK*RY=)Ny#k`>pW!6dhbFv8 z&tEdsSFCRc;c?_;oz(^-y5bUK6MMC4kJBKl4(QRbvYx+Uh>U(fO&7ypD-}lRqhvkv zQBYLVMe)5?qDl}RP91wM6l%Bu-KA$fNQElSuHt3RE7Lt{jT1w0zE+>1)vxK?ybm2U zw{!E&7=ClRio^T1xjm6|J`Jj|lDb}B7uGW$0Rd|`&jxP%B6NFub9_=q4cwWCn)NtCn&^;vFoLy&JmL82UI z#KXr!#aB`an$dF!n$Zs(0iK&rx|;@24_1s(98HzEM8sI}bR+g92_Y+xgMidqTXQj;buL0A*!$!jfGZsH|o@5 zQAqghglx5W$mpe&lo2p=F7mEM-b+G!G)afAkh&|H(WrO-8gG|+gG6fLeoC{5T#Q^n zK9=b6I_Nf%1<`%k(6taRgk9WYgoR+4n(bz_&M7IEnS-9?3xPR|I$mJuF)>^ZlLm2C zL*TwcyFoDc22u~!SRWCQFxp9esyg&3r0w)6`_KqX4QEhI!KnywZS zk`#5I^>epU)wWT}R+7W6WXiaCjO5XTsup<-J+jTLhr6jAt!iioSx2L|n|1)ee~(ty z^C>&aC-tKrHgHU4b%UP4tf3UP8O(|@crU~Y4SX9bq{$)Nlp#TsL_Oct4{Rf?LHc5z zB}s!c9t=s!J6Do`4muZ+6#e$b}edUvk#|EovT<;0Fn;fuiSsOYtuW6vIa_zKxjx-J2QS zhbY8}gah$pv?XRVYgTgdeb9-O1k5}FjMpPhejx@JG`5rHKp{f*^&meuK#DWS-ItS^ z;Fr?#9r2u;OX15W(sK-+>fS7K$_ui-_(+z5GLtZ!bvk^Cv$k)jLogt`ZE-gw%fm)B z?+4D?0>@Q*+`_?dXx^`6@-HvM3QNK?Bk|&?n32XVsp{BzA4UjBf2`TOZNtMy;O`m6 zA3KEK@@;&zu+KjV;ypf0Z~mt%P+O2p6OBQjO8!3T8_t`O?g?jHlJ4PisI6%2xHuRJ zuf9a`@F#$<3mX;BZqMX&PbC}hb*^=@RI}1u4&T|@FVw>A3+V=O-IlS+a5%)waD}9EqG&zBe|O& z2UyR45Okmhg-&5dw5kKmN26gqe+$6+VK$)E9nz|fX>~``&m9Q+&eYz_2TtmGL2C4$ zh|Jcn1J)7#cYw51{T+iIfh-aQRmYU%f}$hQQADQ-I{k>C%Bl{;f8SNqGvm^9Ph(wA z$>r&uwWG06W2C64_ltTQ@lU#nx))wS2;K5D1EG5up<5sr%jg_bMy>Nm%H>_6)-9c>J1G%79j_#ZK)K*-I(0jQ0j|6xHRHbUf@5$`}0 z0R9s*Di(tL>S_%AiqyN` z*RD9BiF)18^|39qo7~oL1y=3(v+(roS$nP8maLYX(QCT=x|F7s*b$g0rFHrzA>wT* zxtiq@zS`!G$!8iniGc5L?HhqmO+o1=?xs;_5>~{0?d~Q!1+&yPhgLLb@7PV4TG4>- zgN;e-gMtZET-}deZo-Q=Vq$~3!Ydj4 z;hB>L=B}2Q8!Xx11XI4ZYY(bj4x@;TzmBIcQJ7b6K3ZSPiUycO)6g{N>e9P{;}LEo8;_j`}B6GTEZ8%v?GM^f1-4T~HN@jE+<9gWRLmE_3D zCGoV_T>aFMi`vw-uRyaKG}(Zv>(LvrW}1!lmdTrU3U#GWl>*J7uOmcfo_-- zTjNse2c>8#=C?lLE>-RHCsQ}Y7WMn~)qbpB$5%rktCvALbU7OJ2FUlu=a-v$msVua z3|-0Al`LIBfF>9I_+&|{>QD)DYn*>rZ(l%h+}?;@1SX5pt4dc+<<&&uGb6-fw(uww<1Hs?83^}5?gd~#1`Q9 z02{beyBmT5nO_J5nswWh`C!|AqG7hA?9&Ac{LxdW&lKWDLbbY*sY)dF-nPo6sF>rhmL+k|Y^I$%>rYNY$HCUxqDTz}ptoQnBr=aCO;;WdvYOxig02J4BGQ_dLirUaIXZ9D#ReO-b~p8c zjnz5YYM7oR8CJFu4z1--)y4Ti}h3_Rk(_KgBa?_p4yot zQHxOqPwyxIQIqPWAZJ%|B;B^((9-I0SHOET7%G@RBN7AhDOe}M4)o9N$I*YmK+A;f zVujfc3D`ZuE z!Jt;uufIz=x-&Xc=HtLlC2EA&6~Umc49L3D4=nv%2nvh_gS?@iFqFZ7o;g730Hbq+ z8HPSF*b44?9OJk-98 zmiw$7f@rM!ok&urP+02NO0SOB#lP^1T*Re9@f77}H!O+%*|8#r4 zS2XB-Mb=hvR25Y*H=t;aSW(}~ZIrKrmG-UFgw3~dIqJv$UR7q2e;v9IN;EsWhmzs7n|=C@!pk)8=M$VbEeJ;uqs!Ot#x|yiH8tN=`7M=w(BQ-5W5>YCL+4X zVSmf(n#m^;(M2v+>@?zTiWfOg`W-dy=JCjaksYhxT40bZb!kOhD?P5BIU8LbTOh?8 zOWDHB8L@erJz7y8qp27|n8DuVhd=~{+F*tBZ<0zJRD3^{$c+WJt^IMga=BVKy@Xb?IbnM9zlpUtcCJx3t8hka z<$5N;)xEHSxd1U*uEW?G&oU%;!K_lDExJ z5JCR-KS@x@9CHn|`J!cDo3)250?!KqT^51i{HSv(lby{6g$4O55LdrWR$S@|7{l89 zy*&b8bBm=k8Y94}S7?h?uo8!Q+UaiIiR4IbC97Yit}Jj$YP+lPG?ldRpA2@Dm4)}? zv_KirRlHIKFkWAXG$YyN_^85XD4vS)!3a!tDde9<6kg6z$VagD!7|6huCbaUoKGV9kytCNPj29 z$A69`$fa0Pbvhf*uG#ALY&<3~my12@2*OnCvi{C+`X|*(jj;;lpwD=P$m*Hds4L7? z(Od_q&>X~!jRR~WI@v+POK6982rPcphctj}B$hs(Ruhwb^ml1T!hgvI&j=6>Q@8kHKpPsE&pJlQ{r(byPSQGit12 z6?cKmFQG)tSkB03{_>Xy>(}a;xv0Ihn-5^MNyg<6t8>CW$L`$$^0X>|X^h>wD>3p+ zVx(<25?d~J6**G?XuNxXdI-xnyGcU58*8C7HkULWkyJZ8dZ|+Z=|jLzCX#?cf0{x; zY&7e};RUfqM%Mq=*uoswwcKP3%;2PAGi`CaM?+Zd+)?-lq#+t+o2kVBs4mD-Ze^RP z$q*Zd7}|g7adf~=%Ei~^q4z7j=NxfXB2hng30I4w$k3LQLP;h8TYpL3OW(ZKn1w+(5l`QNbg` z6R|r5>qmVjCGo95tgMXH5B2RPqi;vERB%5MOu#(8k&p_O4@SqZ`Ho@mDgvurqN}@t zJV?=V`WQC8s1KtRqhdyo<@ruUr-rndJc5-Toh@7xjz6l;yMXO)Y)k|~JZz&xIZ zAw08cFj|C#vHJZr-^6FBXTMR#B%(|QO8lsLc2F5ZYhu4+I5EDL@}|<>5`xS+hj)@W z2{I$LD2g-0SAq{V;616;Iell9QLKY|QE7NXYy~rDUzjR(4WP~4O=Pdpabn(s72LgtGmw7=p_uY{>!_yv?k0>w$-T1}0=vKWcL*nt`2)zX z(jJic(f&M5>5l!$`6R}Q(i5N7T5N-?U+&&H+r6_bjwb7FZRkg(ob!AWK+QJaH?Zzg zOwwv+5*dmPVEpJs-B^67b=?k7sO@E4;H_(}6P!!@V~pHLiZ<63o9l^bgCKNk4f|e= zmZ!ESVKaV{@&!A=WhmC(XD~n-7TPB`{J!>eCy9K$4hwo78a+1m;^%3mYj8UK3zXAX zP4HIG1ywEJjN3Kr9}U%MXw17wW5$15kWb^p-MkotvF;{utR{Ipl11`pEv_RH4Y4HX z3ao_H4Y1LTX@LC(<0P784yyef1Q{A9-Zn^cMtB!V^LDHR)zWUA=YK?FL#sQ+FTj{! z%fnbLa4)FheAPGC;cg01N9I6t@xMT#D%p|dqwc0+0H1F`%^>>h%!wz|OW=TkjO&n* z=Adp2foAGnrX17B4Zsxj(X2RJXb8K^2mS_RtvVcFg(TkkyC;j+B)OYEMYfa4B%n5T z^ZSTE%-)7T&>#(`bZ5iV2+Lnd47Y3>xbs)LUQ^OX{_0(vtA_u_!I2S2} zH1pJ@w=T@eZfW69xRKCnB+xWc2d$e$YOCh8i7r&n5Zf0Bl{Cco4TA9rq%H#b zyXQ=|d0@7H>?gmzzM@7;6ENjC)id4e z?+Av5DK#6E-ojCVv31H8lwj6$u#Py^Naqt{dp{tlgHin-E8pI7;vK9$a$j3ty3It(*H2@Txl8e=LOo z>I*cnt=QY9p6yYt%ww2J6Wnr~MppbC4C$L1&6pWIP?#C)kuyx@^FZ ztgaD$7$eEj|1QeK_8uMTLTT9ZJNfKxfUq|#4aM^W2e30H9OBPHBv6mfL5q2?eEp)J${9t)FtVqE1$KmZ=S~_`IEldWKEwCHTF<8ItQ*jM@W6rK7oII6nXp`P^q!P-RVFa9)|t|*D)SI3A`S-9sCV} zyb;JazwCVm#evoU(Hf6*JwF@T%5cy7Z|Kzl^Y!rZnefmMj%M2^*N|@5mV?-~TzX8U z$8x#DOm_Y{64Y)kP~8^Q<<=vvgrBZDSQLP_Muzs94^{Tr1X68Oi08* zj|2k_`Bp|#x$`{?>=a7}qEq3}*rSZX;JA=VyOK689dPvnuC8?J6+Oa}Fal<*1rwx` zrEiDZQm{aFz`FRwk50PmIPGk6!J2pcNBdp&QYTsSk|`M*C>TTXseVRlb?~)+#Z=a| zvV%uNU)aAP`Af?)%W))CHc^GnwwD-6Am7 z-Sn&oq_~@&5&^fniR{Sg8E15qdd3kQp`ID(Zr%KJ#PI5P01qhB} z;}bMk6Wd%YliKM=2VM5yve%GeqwONp1U4wbucwuc6tR1zJ- zmkzV@M{IH8&+n#DzLg&Fu|gKNibhM@O--|v9&nMMZj!AA?8Zz`*9}wjVL%Itx|P(3 zotJ(H6M9h(IR$Q^7NK>|APwd;jAI%j4&@?t%L=XZkT!n>;L2=v%Sx^EK-c`0_ffL~ zGM%MAf`O-%9%Jc`B48KMg3@Dk<5}r3h$ss!)8>tK>P5%m@AIz?C8`8c7er&CzJ6su zujsCfk4IxNIE}#n7akW99{~ke*&))-Wec5Y@r2X+1KRwRaK^p*6LgVfG~6}=tzgdM zW$;F$sL>d#&zSYcayx7$k+Id7eV4$7!8&_kf|k@ADTI@NW-@|@UEd=Ir-hITv<#-m zvHUH#tQdK6jXawPZy;~T^GSGpPUXJ^9?Pf6iVf(8WnO_ePN?~3Fso3!h~n{u0dvg4 zIrYdpFbRZ-Pq@KkmY_^u$I|EsoXhE%^AL+TFddO|pOV2BAh$6)^4V07B|BeqB{3!t<; zVkXAgMY&)&&oh(Uf zot1CHd0ecSz*_SWJY&sy&@HsTZayY}-H2+jWAuFJ2t6M=M9*gr(DN^^<5_dxLj&R! zJ1$<2?Gvx&z2fymn|S@%W5tjG~7b!f4Egu6s}Hf*1?W=?4f2DMlZ1xj67 zjQGN`5)i@5aXj_RWi-6ubE#)8K`18JhU!`@T26uc2hP-tNmU3vF#q`klKQgB>n)VS+oyCaV1Jb!7K0@a;G15ZK37h z9`I{8fFu&cxVKm7-HCoMg!~X8TdpHrh(MZf5*9RkNp=E|z{Nlz)`CN`1DDD96`L& zcVunJE^;$MAc=L@JA56rZb-`@cl;NLA8A@xR{)(Ypr2vhAZYaJc0Eas&DdpjE_oqR zXBP_R=6CU>N~Z(v4BN;J5t3vmMdNHQ;()RIRzZipoG=Clcb9C6aCLWgoX=%C$Wr2kCM$V(UOi4C{C`P24V-l2AF130{1!e8F_!5I#O9eGZD@g zb^W;|*iakAYxhzZ4oa#%we}MjxD(w8OedpK=dRLQC8?&s7KQ5vsLYvVZ*QDQANMJr zdk;Q860>|wK}nYT!98qGRU!pzw6Ky{_I>W>=41qd?&t1I@g5At<~g(IM1s}1Sc%Jb zx^@Ix;PBLkfzQi9XxdgaLidCcRR0p*4&ZKLD;VNW=b9`jPw3B5l>z(J5bS}lC&wm*mmouk zXI~;v8O7I>$ok7gpWha%KCX0t0}!_?w@3TPUZ znu`#kFSx|Ha)mP(A}2CP0p^zr_(`mQjfLIKFvPOXxL24%6h})^{A2dzw7ynniz~(7 zv_ERUMLB^iqv3wW+bGxMO3lf>hm zqdA9O%=UtQ_H4cVfZC^f_o(fT-$^!GW0bQwgUESJQ%^J(CU1D~T!5FPN0Z`j#OCN! z%Cz>5*qpONp>KvlgJwun7^e?Vm1G&_m@nKri{N?F-&&E;wAeO5xmY+kkAdVcZ6C z*~8<8vpw`L(vZz?_8zL0JxsQ9v$X2-vzckgMxQ<=FqssgE~MtZ;=2-%Va&O3attvt zoW}Yqfs&Dc3?H2fCsUn~fmVD88H@yE*l`YA3@R55s-NKq0LKj&Emt&daBRo8S0~z2g~NpU#mB; zSQlINw1^IOWH-MqN4(2oa}P_&TRe8 zEd9=@JkvykO{^jq3b0Sf@z#6LdlGAjx8bl10s54E!P@bG@73T+r0z$$u783#4~-O& z%EKw8lBbOCz%(GH^$NB+*I8Z$&!G(dtQava`a;xH6Rv>!xg|Hkg6w{-WS&+!h_#XU zz~D$Kv^-q4Lhve$6+7g}_X4q^{)kQH_kKZhW31RIN3OA8YXKVxm`jd$E!c7s=8+>a zEZEI}se5HY&5$Gc7I=njKY#Bo8f1)go9&XXhaE!Y(%tVE8KSg^4sY@r-kV8N1xNy>8MCJP*g znUyNOSdQFm!QM4tOXSEx3)W%6mdTM)3-(9A%)VJ6M;2M&M@-a}a^w~Z)?~uMa%83j zTVujj%aH;LMh99Z$u)B1dJ9}+qDJM2&w_o=gl&)`vn<#I6Sh%~6k4zmLnJphK!oJT zY%Aq+SUgep%}oM)g9Scff?EXGZ-LuQFqyXG$Q%p&p8y*iZzijX9J$d-*z~k0AFQ-j|i|K;{cBvCVxz% zOtUh4gXI`izFU9|Nk@iHOmL3?8^R9oTPBzbup#dN?*iE5{?)>G&7JlnI-iA z-)n;V1lSOKfY+GdegQUQAK=Pi@&h8p5PziHU}m6YtEB+|o@#<&Q32Rc0sxOT!Sp4A zr3V218a7jtd-7MXQ~^@DkwT3_2LqNi0C>L%CjS#lAprbm6PzKyhE4$Z7sKSUM2evn zkkV*o$QIyR0x*Y>=8c$OY!^V@P!7NjnBZIiHuM9)vxdn}6)A>_K+0q@!*l^Qv;@E- zO>mw78;SzpejFoFU1tcep(_A>A7Ha_=)Mn2S0JUs%rHxU4P627QzqCiz=o~>_#qQK zPk;?w0dV~=`4W+0=nAB)G&3v|U_(~`Txx=40XB36z}K2!Ix3JOm!m0{qAjsmn=o9! z5|L(V3nV3(d6o&dsVo5h6lYzHohtwhTABh4BdEPa$6$MBjieJy%Z?@v=iN=WVKo9z zHhw$VyVf~*jd;6wwRn4YP`oqvBJs}R#q_?G-yotn{5tW@ZWlpP>3pD# zvb{vXe~Mrm1wR%+(!l&(5hSh4Ul&1Av%DQa!7^!A{_iIxdSK={H^^f~3&+^&&`0j9)E+q_}vl z2$Hhm<3*6P6CZ`3V40K>mqY?7BL3CO)C8n__@^RB+J=89f}~;iArU04!uN|HX%fC$ z1W9%9r$vy|1%FZmNk#C-M3B@1|9252J;3i3L6UvGQ3Ofe`JEz2GR|v6kc65qMNqIz zvdl|G0?9AGQ3Oe5`E?>lBFd+WAW0>kB!VQ0d@O?W0lT^X6|UH7J&vAU6#mOMhF1a$ zYkK0`@IFw;2peC(I@w#iyk8u(!1OX7_VWS(#-NH{j9xpiLE8e zKhvX}jQ>My5ex^OXeCSWB@2gfoF5hw2f|hIQS8>}L~(`zar%-UHzAV^NS-fwp9%2@ z$hsN6{MQ8}$j|--bgWh(he=_6!&$4+r`7cr)MqQ11@&19-Ib)(4ZKLj_pYL*f)6gg z8|3KJ$@d|B^6&5cH4>N*qew<2I}>r7c!G}s4i1>;qqr_*M1aSr!1&uxd)iEpcQ?I< z)`3y|Q-b5r4_Fs^PZkIGN&xU78DB;TFs1j>xX@<0pIbLO5E+HfRQhNuw2`*K1F^Z- z9=R=87%-^Zx!4|gEZAZbhJA*S7h2yBm@zrc#XiHx4)EmyoH4R1>JIR`&!Ti(WzqX1 zaD%OAd`zpe;dI`{F%72+Hl;W8=CwHe4KpRBH$J=Jv`;y~`dSy+T1FLxt<*KzT<2Em5N&SWR_X?7+PO2}D2O&eD{3~_-RRl`1cakMtnE0}B(spL zziWIdfXGBdV0_Cmef_U}3H%uvY#M^(X*H-B*^34=u{hO!NeWI3#Qh!r4+f&U=~K{Q z#V%j+cCb%lZ`X?`k`;S=$xoV)-x!b#U-DBX zWkRYANUksWB@?pHfXwtIA21=;8<1JPu?A$GFZq}WaTt&iUvjqz z`SJy-#kz&Q{2l=b@@|5Bmvo@ZOaaLv0m(jBKn}oaECl3Ml#wVPZp`u+>t~7VkVM=~ zv3NI$LzcT~2IY0|qXr6z!{`8Cf*8qyEKm;$DVQ0Xi@koPHNe+?WC(&sj$~M{<$#eO zWXO@R7VKsdhK>F>3w9k~rXXN*on?V@Ow=4XB3m$*3Bz8$%z_Pc8^vI+f1!Y3=hFk2 zN%C?zG9E5m^t~8=!$iG8jx4rNpEqICv{)p8_Y!OBe-Oa(@xLDb26&g2r-0;AmkzRX1RTkQte1ttup0;Amk(?2vS z4pV{AK!C*oGfBc)V6+jyhXk0EN^XGv5tB%OtGkR&y<5Bo`C7bT0qlL}8`3n{|60@J ze3zzy#@$WIhJ1i=+QV8%*x3keq#0V^AeDQw!-^toAuY($Zq8#3B7>h45!4YB5hej! zLET_98TyEH5v^?CZI+7jT2e@cCIa9R6JTf~qQak~qd4Ouq{k36wGk13qx^ShBVORhtg{3^gR?De0_D__U`Q5-ev*xFA|o=&*?*qqhS65F7EJS_7#5}M1_;9K))z-A4! zP)j_UqjNPF9$jQJ#2Lg7aWn#3AwDn}+xT?@aBFd&UoP&akfO=Gcf-*%#7?VUJ8Hjo z>OJ)%d(<`U?Uq!v%cl0)l#^^=Ym7u!=14*}dPrmhv`viuD=@xkW@RZ|0S8p(1$)COOSnBx^%+~7<&7st$S zs_Mg6iFA2mIl%RE4X4;sxJIM)y4buMoQ0A@lGb0qN23YZUMz&NyYR_27DuD;b*sU0JA0>@2mO?s`{f(h45*-ahe~&WiBXJQdko>cJQPML7g~3V8~O2HZd0W-y+en6dYMqF9G-E9-AeVE$TS-q*fUYj(o(*?UNw*ypwL_>By3q17aBU?-eD{7yh3nMYm+Tw)D(*!9WavB-@*N^iZ#eJZTN&2Q z-nMu8&aQL%2Hf{N4)JR|-GISdn{8>X3qnElA8z#%x9@EAawWNU7hJYZbz#9FDH+gE z&{OybzV`AHPw2T1?WmDFtm_3zRC^t@F16ck->2s`?_C9_)ZADblDFVJkKTs>lMu+o z-z9IirnIEu`<|Bhw$|b#kZqZtq;}fXEz0%VF*`X+3+=v+Lc5s{ALJii-Dtd z+K53YSfsWLrGmz9QRajAyAOX&_}hrTDE?OCZzcZNy!(5!c|YyJo8!&#K92Wsy!-I( z!@D2ve!K_p9>9AL@4=XZybUFLpMAh;4V@~z4}XX7$B8!XHd(&EV+++dF>KV{gn-9$ zS_I8u;l`rh8;or%7&QAJceoGIzd7;5Aj(~c>o}Nx>OHjd;147ZXjy%7+US#M)F(2M zfI-MxRv_Gjzo+omT5P}M?Uwm=^)u0WpNrP}a{_7N&BPh*tIv|u&qXh^Bw%hyz(WmC zL$l95v#Xy|&${mkLY5M8fHqww;>4c^e_8m;!QWK;<>7BG{?OLhXzOgev+&NsI|J_w zyghhRx(ja?-cG!ocsuZRoZB}>8!ZF9mH3O|uZ3t^eN${l3ncV~h@-X%{eZdw$AF#F zch&@KTvk(+p}_x1@f1|KlriRR9n%%LEBMY}PMZ(aYq%>s`W_TeXZ-yFWO1})#SgLy zkMA83d{3>*z}iH)fb~uLvNf&6HTK5qUyDwL{{k+yC6|VbQ7xnAT>nngc?+)VE6q?o zBO*yHkb-}k1Nwl`zY*B6=Jve2|$5Sur`oVLgETXEpXj`==_{#Tb>Onx0= z^7}q&UWq$whG%Ye1e4!UAU;JG+K|&F6$CYy6ATt8S&AbVq-l>Pric!YTe?JoWzjdjLW@geQqiao`*wD;4mkoL|J+FSU9(ghc1h2G9MPj9PJ@@VY_Z*V^G&!BEB zoz&yG=QL6P-@@UZTg-d7_~6)Yp_5k~m(;?I^i>XSdQs}aO>-^hW8>30vWwv=PB0$c zfFyiiMvIcK(T2wDp15@&9HxYAL_~j=;)_MRVJJRR#G9=6w)+r5LwUkE21VrA)&kg8 zdazP%`1$$Tqw@>W2{(EJF!X#1l3z0z)1%WdcTN@UJw8@?tSmzQ0A(v06eZzZlM1)hME0!N4AB2mv5^&uE~7XSCq z_ac-?{wYIVLGOGx)NR6O1Q$2cxhIp4-)~9F?T1NP#JM?r+(g5$2Hz3A&kon9*JD|$ z?n$~u=|`>pG3{;lbG91Ik%sR&Yfv`ae&8<#$DcjOz`OPhOTm8{qFdpFdu|52ijKl% z^&mvIVpwz&J&o?8titX5BlruF=pu*UBPo7Bd~#*Q4zWG(dsVU^t2m7(W=5IJTn1+1 zli3&3zH~q5N~xhWJeb6{TDL^)H!E?vfX=%gU|c-WO#WDK{|6!TD5SYYn|IUMCA}*F zuF;bCH*icf<+(Qkn5Bnmm`POmYFd%=?E!0*|E%0=K9;x~YRW^@lpBW`h`(f%PWN$cxELS0)Z=RxyOObWAsp#&7bm^6bR)v& zA)on`#v@qvh&7SL{5JBuGdC@U6ZN)7?xwsC(lc`lJty2p&-@m87B=Bob6@5T@k-w= zUK1L`D|w@MP1zt`S4Q!A^ry*B;>C)jZI6n#W833+$KbvF6vc)6byqUSh$ctqt(3U! zQN+5Ex8OMfj>lce4~kUN+0gLS-b&Ze7=GG@SM;`SAdwyT3=|2RbA*F*_)!R};U29P z5`u)4pp7>XPMBz)AQf~KWjN~&%;E5{`6V_hRiyX+j;1=_f8`o%7Bg7M zApBQ0925R49wtvMXL4>i&w$cV_d37jzCucWUaqG?hNEZ>3Ni2JGJSF67`C6r&0D~l zO1?Pcg`-EWS2yNP*P|)EXzuiNSHcU2xt`V*=DNrYXXDfG!x?Az;mmv5$uZNzlLvw2 z2*jTww#_TNOh+7V+e$|6_fq3s4&ni$zRbnST?J)MC3zQgsMHZCd(nfnZG5B|01+4L z53q=8KXO%+$xAA$Dwo3JMnBL| z)1;_V8y{QXxCz(0b=#vUE#v2uC|@sK(!q|LI*R*drs3A1a`q0^&`7h*xk>rDJx+N* z$#JtXaBFV`MI5H!_- zIsgr^PEmh%b31Cm-c~=_)c$E|bX7bh_6q<|i91s8w+M}JM=%GEYHEa3{M`c3rVv1z zmIHK0jUSKOWjxlcz@vT*5`F;KraJ)Jv>vcco7Tj{ky}h${Bo1B4fkH@i{j$>3zSn!s&UK8 z3z&#r?Qb_R%f5Q-q89lpW|0ead$6d|W&q70*Y+^Fi_4 zBA(*n`YB}DGOi@or_ju=PoaCTL0>%S9%X$BUH7R^q1#3EDLvvz_loLM=(1RS3SC5{ zPocXF^(l0zpFV|_KKc|%l;RLir+CucS^5-@cxH%amUw21XO4L0isw}Eq-!YjDS6^K zLp*1S=PdE`i|0J?ED=xoj7py(i|1nTTq2&!#B+sst`yI(c#_AnK4pz~M#Xc3cy1KW z2Jvjd(-_#o{qGM{Kmh0<~aj4{N6QmWK?qyeA1;p)sy%D7n3 zQ}{^=PxvVy^Akt}?xHvl(xTo5d?0vL13neJ+`ap?GXCY66iNl{=VKLTd|yWY9Db5= z{l&=Da&d9|agbXIdXp^~aqq_0N_^^Rky9PWQJjln6;JuTR6YrBN8Zj9d^3vd$QE(R z{Ps(daGeGGbX?vys`&sdTA-s7MVRV~FOW8-Z%QE>L2dGfKZX?#k7LPM^T?e&5gFlK zQ=#z^fW0HHN=m%Iuy^V7bK-CS48?wO2CJw=I6R6j2)cDGV0B@Rvo+?6@-A#eg!7^S;frqJ>h^L%yN6ai#P>-My zw*UqbMsZkcPKe+rm-3_H%QaDaYmrOL%U~b7zi+{CoJJ>f6+jmaGK8SrPYt6zOf6;+ z5bcci49`Oy_#tDEiWZp#KqOBV{4d4vKgOFiaDd-MM_+4N!OWS3E@Htcjlm` z_(agDU59b9DdGrI6N^Wzc_VCpfmppu?o5@s=vOwtR<)BKx;6<-9g9(NL@MKp5m)!- zXt=)pt^oh!3Q&~f*h3qkbHfeDQ;u77_zhPBoLQ1R&LiWHSvGQ)m4|hjocKrhz5-Up z<<(+#Hw~71VIvjm8sm}_Z`)G-Clh3~N8|Ap6fj&rGHqc;Mk8)4hD4cM&VMr#1>>Su ze1mpZD1d69+7kB@mq&O3UdHE?%uYO6ESK@0LbljLv|(n8oH9#dna3^y?O(n?A;HFLt13fxLCgA7JDwb>Zi176Oi*+eL1@L+QRZ%hsz|=+8sj~| z*cw}GWvAoJ0!6!~Qz*1DtZ~(bdCr<0Kg@08u`xUaH-dtIhE& zr{CM?%N9hX%J42isTp_*jI4!1z#~bx8#EC8xgCQZj z5V9llXSzkdu(vR?@bzEg`^^}omIe8%ctO8qEXNmW7&9=+cf%@7D$a?YL^!sZddwI} z`-b2~*(U%0L;1@@`7;vBKXp2x{F}a2{)(Y;f_zy*`6WZ}bIKQzwYeK2G)2Ng+>G9h zJz)y0?m>R^9-)7rm+W-J{V?INlBwkpTbXu0Ma*&-Be~a7h0Pog#~8S*)3U(?{mt|4@?r5D=M%5DMT*n>!YwY_b0_n6pB4eH7rMGU9ebm zmbRH{g?@0-_$6r#c5Q9|QFNK4?WX#3{LT`-P%tDJ@`5=~@NA1!xXU9_OOcy2R10we zIP~fq2bG+I>%@oU&}xJKF@Ng+m>-jYDBlsQaG`um{)y$g{>S_^7J(##K+Sm+F8c2% zkUp_$%gWB}zI0hXXw}=&dcZgO`w&`^E%J2Yc1HAO!}XGK;Z_-IKRNs!R6Tx8WsZ=u zSh29v?6HJsDF1hjmk~qb#VPWk_t?V%@x~dV_c0TFTmSy+@iz+e&}*ne;rn1pxx8U_ zGkR5u=2&FUtuXM-jvgw@&g=)PynZv$inp8>C+8rG9~y78e8d!Ikr`sh%x|QRhF;Z1 zvn>L-+@+N{tguHbb6VjH4TcvJCkvCM6_&NKEGxWNE6ZjLdFXsn(OXOEVT_xT@vYXz z{)O~JLjPTJo<#%vZOuXbY0!lmydT+uTJcm~^M9~(7cqV}f?dUaA|nu4dM?nhw8Q0m zDO&M#2!ghVo<^0azG5*1DZ~xLqv^8yH`nIHQ%%E}9Lx+m%^;^-K zuBFR|MvQlVLCfQ4wCE&Fyjxm!AjZ-cgJ@au4ncFK$nO++sJ)HNfVP?PF-J2MB**Tj zd}Z84x!@`l&&vLu7O|c2d29 zja@`8#Q$JGNkb_Ou)(=5$ZgwR;8z`JB3c=CNEo?K5wkMO6 z5$ZmhCL(aQg2dwEMk5>65;Ov`jkN-%%0LJc|AkmYVU?!Mp9#?(v3#k;D?CAe^&~jc z?%j^(@}<{cIeVUQ164-*(=T_xpxnoHlX_&;)3L6A7cg%u!tQD_k$`TNvHzP(Lmod9 ziGtis>5y=CKc%*Rlu{Pr9U|4K(GMx&o-e?SyzCdWAOQtdKhxfU<4HKa(mmbS2IDH- zE%51Jb%VYmZk4VeeV#5WVkfXt3(RD5Ggw`JISL*iMwCIVID_3hg9SWg zsE07^^@4mdcHU?%Gu4tmv^NbebvLz~6rCa#ua|ku#jBDnn~Mm8#s~0v0S$8{#}-Xz z573Cv zcKzSeuEAnoyU76}&LOe0nn&Pc0a)UEkM2@pvg^GntPWgS8qkTV7_T5kDDRsn?^g|w z`(hffv0OJvn`2$zXC!V`T+Hum*pMyZ7bWmJC@rP`bnh&2wUmB|m#3xl1YQ{}rTwfV zOBltpiqEvtuV5TcebI#ui%(QHz#x^OZop~KD0M>y&V+CX>F^$nC%@k#YVU-fn6{Sj_K0dzqOyku;4dWsO&Y)(DNfeAXaw_0!v=(bS@45 zKKxJ$4x4eh&M$^|K!+|L);!2>bRJ1T8UF@5F(3?&14<|R1MMeW%#JJGVIm5a^Jom5 z4!{*Z2myWfuf(JiTF-t~yUPU;J7>I=OsApachGT$-M&ZtDD}^_gC9GEu@<+@V_v{7 z$4rm9gRF~_YaJMXa_nGkHi91;&}oJqg%$Ksfa!KR?m*H?`!I;hTF zpHqOUcYuwg9X9rJTDocvHxSvMl7`mqZ=&$eX}ZuJepJ9|`KsODBH*-$)gHbNVNjT) z)=$UpC}7`7Klv!CvmE^kt43Oz(Xr~wVtc5h;}>ze8YqnaY2WL8yQw7H?6n@5 zPm0+Nzts{!1b6qlo9Lq-x~gmfb-PLhscot4+>Q#wN^CGjb1ytfnsAZJWME(6$Ax%Tc9``o;ivG}F0y{?8A353n2Gb%Gi1z?7;@8`!( zivG`Ho%pfqU!e??IArkz-)wCje)n?ic;(|>?BDiA48U+popHTKx-x@mnF>N5WwE>R<6C|#J6V=Twy zaxtj{_#0pk6GCjh&74;-ai8GpK^05Ok)5ij6c2%ddw0=D(6G<~{GiC{Oz4B5PWnJ} z(wzqF1O(cagM5~l$(+^LN{Tb=^Tt2P>m0g;HG^cX!lfl4%CJM(AnyGsa&@6~ab3eo zJci`dq5=6SLmlAixPCfrB0^_MxC!B6!CFub)3z1z{*|cpa{Mxy&?Ls-u?Og!=;zO) zADNhYX`YKn>c_cdjeDp{nDiera>ffto0zMq>F`a0e31(_{DSxk0>v43saT17!2|JW z*h~-$D2L;2(c)15ll=25ofHTzZ@Q6?59G*%yC5EA&tHMRH%qchg8HL`_CQk zLQj1BqRF9=wQQD9gbpk=-(~j-9VB>Vi~ND0wrB(Y6r%-o6LLcn$gj=(Mvse8kf1@) zUZSrth)7)|V-79j|2QG~TkJ*88!^Dg{GoCwT2&Igs6@HcxJ|hLw&hZ%)=1U9rj2%W z2K+Mm)k@pC$%Ys~7=`Sx5w^IsN^2yQotS-$dpC9;&X#-V$G**k3QTl~ z0Ba;0?_vICltd?#L{kjB14CMu(ZE+C8r_*W6&xu>MKCk`b6#HQ)CEu5B^#nk%;v8*> zYp%MXN9y!nh|eIp{5>z!!Lg?#qwzG|+wZ>j1A6Y%w*O4H!$$D}&I~NHdmlpzF3J^# z%`Sicj(a6fuV%u~fD&K_0E=%RB!>a$Bft*`fFVE_o)!Rt2^f9?6eVD+AixZuZb6_+ zN=Lr<6hylJDJ9Wmd49Fck1F5efnZnHblpoz+GZPS zFLT3vudzA4Xdf86_i@neM=P^MGgzkdf8n0{vU2QM0S)m-KB2?ytm@@v!*6k*Rc`-x z$#W0>eulqi@z;sJf5YEb_}h-ZPw@9H{;mP8WJ|I;l2cNhBS)mAjvOhC{Qty1R1?jI zf5XCo5XSqt-_u}cfosaKl)#$Jx0wcRt!Wb>)$t|zB*^EByHGr|(-tc7<()!B;t(0; ztM-4S*z5DyEi)QVpGI!?edK;?4ChVtIP43+9Wr_5E!Qi{*u!G2SWPQLSeW|CG%OEO zNK?avJn_KtHkIOLtSGkkpvsgr5Fe%Bi9ZwvJ}J11Wp|q*e<$MQBvt64X2YV-IsLjVM!-F`ul*?dj0Wip~>Pex6Anar~IYlA-@UvD$tws#- z3mfT|MSOdrOPD**I=-bkn^oFy1nu4lYe=;VuN18shP%pCtvX|=I^bNFjO`$NO7~+= zjdNQq=+MUcUX8kAt_Ak}^q*~!OA9)?igQSeZK?3^4e}nSDoiOIA^x8YW_h=vyp~|L z`{nT(e$2HpNvnpPu+omAaTDgcaR@t%^1%j`4>mI5BeYSGq51+&1O7R<4wF6}4}@=1wwto9S* zFDr3>O3oNxu;2_Orab;QIfnbvB-+OzU4$ z>jtDvufX2XIb6U^2?b0w3IOuDYZD5n(X0}leR{YA2}Kws91Y&xt8B;#jc*S{%f?*r}q!?f>9pgozbbHP4YIzapW zQPrAV3*gs1g#>>C4{}<|!H&?pf74VYt@+@_$(yjqsm&Id&O?9GLSN?+Y@V0=M>9EA z%0(}QYKosU9zb1hw(2^1 zh)iEOEQtJfzoB^s`+fZMd>>_LYq8EjKlziTC!BRK{SYoK9cQE|IqKax zSloh!s73Wq_TQS>kpdIe5O|9P4(8eq_}*SSi52HedzHP11$mZnRjpjkDhA5==daQV z!zDgDsLolN24i8T!!G%%b5Kk7uD+D!DIFs&4rWVig_=lG@ve^)#rdD8y_?y>k= z3!yoT_h!JRo8jLhPB2Rr=`G}O#{W&;*#|~dTn&FWy9o;{?4kjpMvaOsHKa|wY0c+kaE8d+r#>~7$&&=e3hh4#jZ!-}`Hy>Gi)q-;ykIS^`#bkXZbHSDP6K9*n zpITt1=;E{0b%3SSttvQSww~z>^1=-cCSGTtd2)QE6Q8c3Z~})Af8>>XA8j z4)v(2{hU%--Scm?QWcFDs7PY<&<#n@^Mq-Mu4yDKFIa%}{{G(6=?=YVFx{E3K&Gqi zOuCYE9Y1x7a9WV6BOTw8iZ9!5ond{;JMEf+*oDnUmYsK}xB0b&XOYdcYXEoVq;e27 z7^kju!r34L@NPnT`}_CJlRJ&xtT(>Ci`RXjy>`_bS=d63V}vj^FyU0y0o}a4HGn#_ zf4&uTMxy6ervDV_r^xqT)rX^A59Pf+Z_n$YJ$B|NmREK_B%#apw4xNYUsdg`nQ!^5 zaBBA5>y(P!{$!|Iq#WE4O8<~M7+ev-g4J`Y+f~B~Fu!$-)0TNouE5rAIprqa%1&%M zQjrWD43l@=Zt@;Zid(K={(DDe6&1y%s#&@4ATCbyja@h~nw)d_!HVR}%!6Tj(#xb> zmaAUMf03o>VBO>jdmk)GDpf~S*#RjLHpQ))_2p$|8P8XLdE7zDe)kVt!LRbX!qe~& zzdU~c?LWiwIo@lWclnlw`P6vmHa6FRDYft2=>-&y8 zm-+3{`wqOGJHfu-FIldA2YP+-KAv|qW9sXx?;~9)gUZ$&SpGhxqA54wQCE=i>}y`n zay5LXz}1>1e`90tLS{8TK!(@!-`+rA>kcb95Rh7W&s3FWrq#4&I26-A*E`*4+qtUm`4?$l%j$WTAA#a!q?X#OEa)=b$g|bx*Y_fi6R{7&RC~Qy#(*K%x3lMI z-rl&R=eLq|FPZnPmS*oW{T(*|-|JKGO_8bh{L+E%^%}mZ;3cWxDq-+j;}A)#d3~=l zCO4)cXU%*aN89Hv;GQRMdKEaZD6>s|I5E=(k$HTKRv=XfW(o3)_?OqJiH z7|LF$ekUzIQc_f;`eIkBlH9_+h0~^AN2v=xTb;abx|CRvY|511ux0dFt>7KM`J{rj}kxMPAQkMX-h&A@a}fbfw6K zC!BaJf56|M`?xC)+rQoop5#tYFWP@cg@%1SUEZ>E`iDs`iD9e0V&87K#_42pQ`MHw zWbN8dk?-CE6M;(yZU$bkf33M!-1R$))@PjLtEK~))H8~AdG6xrX5~nQFXjDbJQDT~ zp41p2>Rmj4!vVM#jh7(p3h$KN zz3zbwSEMp+CO@q z8q4;;cxUJ{HOqORig`nlYD>5;a%A2^(m(4v(t)Ix4N1D|JyYtRP~_1TSY$G$IyuZq z8nvqyFr_K3Pi2MeT3X`1%9*&DwEW9Y9cc6q|1mhja~n@L&k3I2G2AP79(KZ~@?PVF zN!k=H$uIaM_g{!Wk6CeArJ7p=XE3U6_jUAUHoWa_9XD-m!TcLzr#F1GY!SXq{ZE5X zuGV0`o1;mQf68w2I;cRWys$<`H5&Cr_@Eu{WLB#?d<}1*%Q$RWq#$}e$ zTH=+?ho#fBk@igs7sSRjQe6MC1)~>dqNTQInC>$)|86zc-+DT?wbo>7i8tXE#5su@ z?7%aqEZ-Rq(W;)Eq zW}%jTc!9^F-TWU=mB=_$*l%nB4Y>pJ7sP|sWayT__c#N&KR*aCx;@x)}@dPlm{_`<&ci5nd-9-?4bqwZ zs?y04jV27=vJfJFN^S0}!JtWvzls0g8xjw5cz!`~-a?O6oTs5k{)l+OgHJB!@5;^7 z;O$1m)&1F}yv2&u6qR+^&dYWu`m$o5QaIA`1@7BE;sxws+}DmD5)R2(M8*G(RQuUY zyW z;XF65=SPk1lIiH`d4fM`Tvtg>-cR8AW=LRq$upt9n;hD*^pYO%|p3ieDPZduq&pL1*%{%wNJI`?WNBaMN z_~-Kb<&MDRTD2A!90V5E^7}7-v-lm)vyb;AzhC6JljmzZ@-68k&mh=kX8r&0FEjmb z$O^-HXYSXuY+hGLCb$bb0+;GrP$bdJU0Y?~;g0aNi8tSA?C_FA`Chs|_W*VaKumNTzmLmU?l`JXbNF~=&naT2r z*674iM47UZ(P?nQ*^v>>#Y`A{cUeJP_WKZeEE1Q<{nhq4g>nCaO{#iQ=W-E8aW_1D zi->Q;hM8~A{M2oh?uH0mk}6`CKh6SJ?@xsvitdrq|d?K55 z!zQWN)fMWK*2Pgx()`-Sb$lUofWg1phj+Z|LH^S7C-tqVBJ9x#5j4%J^8Xf7XCDHF zMabL2IK-h0TYb}+xH^q`yGnvmUT228_cQ2ki3S0(#BzsOl2grncCf!2?qy&rqS#Oo z@Kycps=f^DKJ@MTxFC?9}Uo_Z%C0M9Z0H#5mft-9oW?hk=DMu+^CT67kXx zX6uuYh?n{d+Z6h#B-k+R<2Vdk=qSt0X386;K}G)0*{GcSs{BsT9!Uu@#4iNOTrs?; z7k80tfGKxNJkX`B&$g#*o}fN@jaDlRDdZjNPcn#`M3ey1bxPxhXY*QM(v+fMBH4dTzf8wY2Dh^Ho|D(%hGgKP4soO5CBbFEUNU*CTpV zsxjyhi;5;d3F8JKhiLl*#GeWE$f%9sC0Bi3ea^ZpiA*poiFuIfjET{83A%J>qHAKr zDzL8^8U^jgS;^DM#j6&lL{?k=(Ot#~| z0ENSi>Y!B)m+iyktp0E@q*v4%sj#s4c|`6x5a3a8NKk-|pPJ@6(avRYw(QOffze8OI(XIiED!nE{E>r2hF>g1JK++B8t`n`om7hXdBE2{dN#>7Xf@?5b_Zv<}q z-M1i0-472x)URg_tIE*@yX7=CK!=(72A#JB6_H6sy%2lD7whGv+Qr8pgyZ123w$oYy(X@p+O81d+IMS-L8(*iiA+qzVnlc_~!!yMLfb##UEA)5m&=3 z>lcq`X_ul{#N6I;bz5DHcn!U*%dG?mEKqWsv8Q}R=C@qfPA*tFDK>)p+OV!`V5oF+ z6Y07VeV#z`Jn}Yd6$ubm?6!ujdd-XfkCE|(HkG9w(*9GzkJZB0682`6XNQpT9ShHY zT8O3I5svZ%>zx%wc0pfmCJ;jCKt8Q}DkA@JPU9jy*TI6(+9I{2)axc1MI)Aw%ai5E z7e(wl#%t+hYiW_SwANaBo3&YBkwy~Ti!Jx3AeL%;j#^rT&UHX_!SLDmZ=$b8p6$IU z{VG~yMVBb#lv)~StSZ0F5oqkT?$r%xX(HH4h^S*%$(@IjEKi$7>dMsXuy;fwuQXlh zz?LEF7b~^shb}<^?Gz0ci5KBeV$%9UtTf`w9Xu;3s;1+osbeEQilN1t=;%-EMj15z zE_V7#mc+*rImT&|SxaRT9HMsO)L&eyBDXp6IT3?oI14N3_np!VKgQ+96A~xX450#zF9m zrL&SNwZPgv0Vl7wn8eCC0PXX!*V zz#W@uM956?C|>94P+B|@<^-*-^VXfIjM-@) z_yd-N^o}IxEfm9`iSG;{@RO0K8*Ohh2Nh>uN{uk_`n)Xif>T~g&MHNTVsCZ^su1?e z{1^$@%830OA6#n4V*3b{sh0KhFJ*$4#yH7yB9hrtOAwY8E;-J?Z1w9;NR{@JvSr-)HnASEC>!)W*gw(bRL zBalA45|~hkx?j+}!hZOG8wsEL`75Br4%TaOAB$YoN&1H(0}?-5uZAKvCt{LOdjrDE z>`(}5Ncy^w_A;g&yVGD0I{4opKymPs`JfeZbR{M}Q{+Deeb;5xHfXiwhc7)zV9cbi z^VPQM`1((XZ@`2@+d87L4RIeoXxLL_*pBG5r|NIZ{M4FP$ooz}Ixp3AjRMps_~D2T z$Bdw8*jdbLpi3Cd6hVUStG!-=b_f(4A!AFgc2-O4lHhwcA_fXK@P~h=RWa23iH+O;89RMN=K;u&7Lnl_nu=a1t(iHQkE$Z%!q> zB5GewW59XxU5rVM3v?v|jX;Tyx@bYKBVS(8>j#Xk-fVMxbJmk!rY=!u7NAP|LVaPo z?=`mzJw;iehR|*@a56vZCfBwzyX-LO(j>2(Ay9V8v#%hOtY!v^uGsg8sjd}?O#LTn z&-G{U%DIf~QUBt(iXeEI$@09ek~v^L>JRIsMHTjUg6y5kq2+e9;D@*OtWp!Eh%lZ~Z z#_VNwm|Ly2qETV#o!eR~bb$TOQ9xm(w3;36E&0$AgwUXDG-bXB%83GI*?WyD2?VL! z$kd%tEjovjS!Tw8R69DQO=&t*OH0F}hh{SRCq-TnNUUJDAE$-H+2P|m9Le!WUR0Om zBkW0|;pfp(k|OULwYO*>!b+HoRaNI4Seep@3+j5)#|vHW8tItG1$Wn2(WwK}ft*q# z0r({RdvnZ>OyX>)k090R>H7eKr^{grATNo1k#9}q^ajf_bb>-%>euV|TlgEBgt39W zayX+Mv70b7Du-8tz+CrE=WHeXfg4J#>q}L2u2q?jRQkU;T3bk}@YiUhNZ`$Z$yG(`@D@b=q(pP0WO-h#K zz&|LNivqOXZNm1ynO@xbAkk|Yv;|Wmki}_slC0N_hjLll29DfpG-z@(f5;`hTt0Da zS70?CE-Ef}a{TdVn!`)1KL*pcYmHuNV_QliQZlR)pPGMBJ!J zwr5~ZmCKof7?96At39z-&Q!s!W+|v!GU*~wJFeZqSG4vy{w9+rh^_5sa3Y7z|pV@>Ag)(k4(Vy6HBN^GKtS$^pzbmMwJ!x)s} z^Lw3G_)v5LTe!1zW1Etl1CaPJZ30%w*)->5pMGNR5Cn5NjI5c-C zvZ)7VGs;m|JXUC(+HRGtx9Z!i`W;sN2CIITRlnD&Z?o#3vFf|6`sb|r14vtlH7mJs z8?a}UXwMUie*1SjG)LjlK+|wpG3xjC`g?s2M`RWJhpPSRuA3nSs%vj9!;Et{YK5M( zpX91b_0`THg!9)LnI*X>6V_%xhxCu#1&pjvI}ke)nZ86j4CVbuvM=~`SLnbIVAJy> z7M0qWI`bEGX3<0(l6eQ2{Ukps$rnoUsXBRnI(c12|JWV6q76>kK1n-^v~|W#ONRJd z(J6L0DUL~s3rV3u?L@W{>BM|2G3>^gmtD03s&ni!IyNS)__xIV142QS?NVdvMu^;B zh28(C1UyO01u_z5Km^0+zCRMxS360!c`?F@kP~!qpw0WK_kSq)BT82Do}n$wR4cTH z#eG<%!7>$UQ)dgh$&nem)hhH{`mH?j+;GrTCtMktVn@oKn+$&HB6BVWFQ=_Mz%IKGN8+2e+#( zlga5t8FiBV7Zj02v>3TeZ0@;C5xDyUiOyEhNlEN@R@i57kfCHq!qlZ6EJpiYN}^dB zAcNS1nqCpPgF#yeXBR_~RhzBO();Vv8vwkOlpRTgLS@U1Sb8h8Rv+Laxmrkwieyqg zBG0n4xWd*J3G31u7~d|t{t;SjZ{zTt|!1C7smx!=*yRferXxhbR zwSD?;G>Td!{+`VQNCYOp;Wbad^Ybo7k%j99eg*kUCiL7-T%tTXud`&HyJv-y;V#Kg zo)@wI_JYZ9PAWrQ&#lC9xeC7Nva9*5w%7jJRGQT@%PHb&DI%I%VgJ}E;x&GCow+?1 z=se#yOw69KPL5HMqa-J6f6K}7^Hh$Up7*FudeXzMDxVm!%bYaLsWcON{-D#uGoO7x zujQ{H8`-moFnX8Kvq3UOi!iS`>{c6r&H&*z;+{;Cuz{ZS9Aoy%xWC75wLQ%w z77iPXbU7AGWKvYy-I_T;EkUMiv-mixu3aL_^;;V#y}c-G&(JyjSoy!JjD4zPPGROv zP>QkPVH>qXfbEr@Ow+G0Ip*{

    8hFmEr1%rQSWRo?6~JOKx-b)bK(V#$#{D>~VAA-=+X!3SH&42Wh^j>68bU9v+r-ocwTZimx&V-7-f!HE535*d_{$c&!Hfb1Xy zDe~fpMdwW6-|d6Tp_AhTIA=*I+CqtxQta*hlKDcQfsaZRt*x-1BBP}Jr!Ig2sqMWY zu~m@B6*s6o_N1amRoh>A+(ChuaW7zsZ7rT{mdl|Fa&rvn6?4w2uxncY$$%LCnC}=8 zWYrH}a0T0V_Ve`cjN0W2X7fzq3G>Y7xr^tgJWum%=XrzYIM4V$kp4w1{(?a5F5SuPmXjh_>``2Bn z8rg?rXm8j)20^0A_7b2nB3NNL#_WcqMRZAmuJKCPUv=04?0HF$Pl9&qpo3OPpAN&? zPNlt7huK!i2^|)cuwUq~LsrRQ9hQylFA2U^2k+%{Xq9}ZLIRhoe{JnS=}JaZ*iGN=|$=4-erxf14P zqHp*z#Bzqk*_B7dhH0tC_^`(E$*u;dg8Ce+fnl8}foUo(elgV%&V`7!k25M(i6eUi zO(hbYa4_}eDnIJ#MCTj;+uR1&dcG*2F545a55i|l8NKMPsO^PN58GG&YEd>#%@Wi{ z%NpD-pA1Qa4l%0S40W~!Z{e{(vxnv@*x)iLmVdyzSx_AI#+F3}j&dH+hf|lGfkSCF zZpVhU;TXmXxEtBQzHU$5=1ewb+F5#nr7oeq-z$TJ0GMWicD6K;+}#`0*<^ZMq;$L* z^(A&DV~1XimCKE^&BndWnx<@A=_e~4#ypPfV%ADwvvjoxJrX3evqc+&y|t_5W~svM zTzm{YFR7MkL?W`d^)@DM*gnxgyH{RG82VuHzD7ux3j3hEx6I_7`*@wERHxac!|&vM z%4-nPtYv3mc&);IG98Y}W5oUmpFkhanj_Wt3OlR7Bb%*iF_kV)CnnHZD=o6tN*k=T zQoFTQ>b2HNja1;b@OZOQsJU2Pa9R~&d#@= zcVdR(OJe$}>qZ4#(XH|mo3Kh{sZqC!?t3H`u`R9r?pVK^&XOXssIRkW)IJ8BY9n4I z#&Lac57su!LQ=q08%syLWg^-n0*h=Vz63^qf~5#;#tx%KTYDubdpGkr#6X+g|7H&C zf7Nh&VVZ<%pOcXZJyTZ{wtXuBbNw@R5mQ{wsdG6DicQEIXsr7>td^;uP7SU&_4vNB zz?tc$->PLYv6bpWKZcxgX`OF3WMTVd=te+@AU!3~_^>lswQyS!zrJ;22#@WdbofvDG2^ zq&GxdZ-&uy>f>*jPHj3syZ&2d#c=zFU?xTV&OX9RLi+=81)7!21q~K@J57yxW)Ndp z!v$5A+PB$|qV`pPm$fWsa8njKO)>Ot*nZ({S!ATqk4izHJey8LX@)vXE~lV>{Y?tW z9bC{wPC>edtfuxEPQYOB$$BkB3!c8G%M3=7rw?K4MJsf>lHSkb$LQqr0^g(BzP_xZ zfhJa4`YY>=jK3->m9rO|D7j{^pShAqy%-4&;2~vatSWH_d;JZSiH_iu*N_V;_=0~? zQFZSyv%l_u3HE77d{of_YO&}`MeJ(Xd#Vw6@>kf`^KJ~B&){aMaISyBF4EN82&aqR zPvUnahWLvX71?l7yt1E1(~@O}^9^FtJp1T3hv2q!9Qeq6keVkl!Q#Os{{w*aw?eXB zIhj0#_5KCJw1o4#?NL~MS6Tu&zrq-^CPel8^9>-UzFfi|*#H2m)jp&IPljKzDd!nh zKl_>cXwu3U2(F&1n;Z#q(QD_R%SIhAOT+$$UftB$tpCy8u!u>xatVd*7~Fovdm zqV^27V=`$BN#1fuE^PID_Bef}7~oeKuD0*s?u3+{k~`QcabVkHmp_h^^k?kB?C z&4fdb{Q#-#tL~+~H9|%-KA-uct6am^2$+=yw%Tt!e2_#x7lDJ=$Wd|zwscHltL)IO z_*IFmvPrw*mnXK$KJAK6N^G5}UoJ>&mE9Vn`RS1#ug#N=U=EEEE2gLxw9FxiyJ4&R zxHgHDMl5UiO5Fn86)|uc8QWG6S=i9k#D^hJ0U= zoRQgDDZVF2r23k5)qy5C`D0Pxg$P2vUn|zwA?&}a369#oU}0h3mViqn-2kSSie;0X zX3@|L8^nwRJYm%$z)7yQsineDScC~INg=Qm?*4*DWkJsaJtruW((Yf&TIpOh?q#uL zd3=X;wg(Xz79qL#POjdo%N!S}2wR%EvPY1Km4s%@P)Uicl~bFu{6_4v@;t6br7%;l z!)98l?+1 zmz!(~(%7i|8C_g<&;L-I-8qM3NI3d$z*fQK0zW-KN~0cvRTMA0(b2e9s;23~)B$q8)EYH_Yan{K+m_mtGX#{VI92P(JI6?01Q!6I}62 z6H6zuM+Rf3vr@8vv)fLZ2EP`BOUi(_z zYCo+WS8d;YMXJ@qr?itzE>s}D82Mclw85vwO3R%)qH=$&^oEOf=|RNq|BLPc*<`=b zJGI_#2HDe^(oXaJR+cbOIAY34dU-bGVb?Le`u0&PkG?;`eL?v%I;ee^@rTmf%~>y zV~w+yo>ErKDXS|>z^0ZzLaS&DI>~yss!B&gDUPI={ep2tV;g`l|G)&*6WjL=;jSh8_)v#o? zD}K?Vhly8XZ(?L>r!g^&Tm-{1k~KPF*S@ZY1MGN_e7-Xr#B<$X;K?-lli^4^lG)10Bxu0*U6`$ zKsz`l-grC}TVY?Svj=Ni!KFG^W2fv8QG4gNW%&F?3myhAfnnRLOOfR^V)x0tvUYt} zVIPurXH@ik^U7zaIAZT19Ajp(z)64^A>)b9xas58g2J?aGU&@{)wZ5w*cc6x{ zubh~Q-9Ss35pM4J#ZMCdd4_s65?3aY9-%Fx&k~MA?aiDQ*px;6PD`h>)Fb4!)I<8ONDNr&G5xnqBM^L})OhLrQp*bn{Is2OH-6WR zb10*Aw((U5er#GwdtIv>fS#T6D_|zJ%Mh~od~(KI3r`q&akQ?-_lLkb>0K@Bzc4F$ zAJ=GY%@9g3P-)*tW?*rn@DC|F)ywOB##LB#@;ejN`>B4mN@2*m!gigJgRHA{{RC@7 zvNBR*wMs@@xahyS)2-6oE^Q#57sOhn;|Ob-eK&=&;E(iQl}wOxlab>@%^2I@(NeSl z)wS&2swCS;(et8Hq%!lX@mKtjB>FB4EKe}W*`N=;hjjw58oe&0maP@CPC4?ygM&|e za{2)!-wD{|C|wU3`}KP#8yw#z!RYGG>%H z@gkorp}NEdat(wcl7M>yv!dF2HJO6hnN(fCXN}oY9$(K;heRdDev4O-y2fgesKIGh zP9h*;H%i(n(pK0T(`hAYa9YiG3ENeYwv4n9dqp~}Y*&NRYOYF!{TWGX+Au$zcIx1? zB4B~}6hOa905qr@C+$an`K3lar{i zh5{Mh*y(0~8Q-Gx5E@JX5uQy43D&3f;Zce~difU`7|x~Z4E@Drz22BZf>*hE{t}*I z5J|_=2&uPh(N(s@zBZUPsC-s8Z-ssPkP6=Vxl}i#0vz6k?G-0gQ0z@XW1xm&??4sP z00NEU$~$Pv2TXh;ToAUe(joe+YUzssk$w3i8ETxvdK!At*elZ|b?Uak%u1K^p=qtK z$}A^gph^9wl8VwL-FZq$zZhJSNf>A<{!b;%OqX=oDJ6Yra7iX%py?=!$|v@&G+mMn z70!}7sCVNAmt+zKnjZg8B?(3VVFs0pksp}C9IP3ozlIE|9*EzWa4qGbhtVnqwWXlM zw?)dM`2Sf~5zNU;ht&1x&rU%gUDvgz*5yytB_bbxlq1R~jeojdMF`Do*QKXK2QELg zE{&|O6Fzu06wmwwWSyBp*2Gq+A|*5MranxiV5R3-Vo44SoJs=|?VxYXVM;ep5-VU` z*OZcb(Bd$(yzdSvZ>J%>_F$#`?old) z!;dQu#$VEMomm6qZ+Qn=PY;~>R%!kR`HQ`k`G@Tr2+cn%Ywaqj&00{Z8g#YoK?FbE z<4geMIC#Ms+Ta5}89aby{nB7Crv1XRQ?NXBX)7AoyHZQrvjdylVC^Mn+%LR?O&6Xv zo~&M1Z~{-5=T4q=JTDTyhu>bF+j(Ek^GCMu^A8O?Q+c1m^EI9po}D}&@qFsFrKeb_G3IkMris0PgdFy$q03d!x7@qC&H`v zd9jh2B!kHe@-XnLrEDQux`HHwlcw9on zhmcNEM4BHE^0gy3dyeu)+_Za)9^oyx@%{{X{xZlpU84yIGGZe ztExY>?r!5ws~eu=3AiMjJU(wq?!7db?50Ze{kq)d0x`wLmS9Y2q%IJjd|ogrSFP3% zfSqQI)fc%XbG7=)>%xZC?K%~ni9>kbHZ_Wdv};RxW+qx_oOwFlXc8mxk6sTK5NZN9pblEeou#xW(_Z$ zE#K#_OKn^NC~+hjn;Zoe2g7#pR|u{4)@5TEI-->}a~ROeg`_mhL|>}HSz5hB>w3zV zpEi)>ufjxz5o{QYi1E+2cRN7_5PCfwolS*Wa786_kKz8`+_3p( zfkEuk&M8~$=2hC^9XkCW^cpu%vWPGpuCQ;q5g?U&@yf=PsMduPAi;)auQX74OZd$NS%<-(^M9%$qB25*({*_-O(;I)Zwah9+IA6Me6J4%3}8Em-MIoAlXp48eF7a zZ}f;%hp|3kKO|g46awqc1_OiAUK*5Gn#U3%`%u(gr*Hiw@vs^} zDPZf^BgqkSBzO=jnWUBltt(apSY8^AeH`0M+Tjhy+;Q(L>xxn-In_t%udZFk%20Pc z_JFXiS^Jpzpm!VO^8xco7ao<*Rq4;~$!BZ&Ga;W#(w{N;yp2y&_;~_@b`-R0Yf2%n73A3Vh80}}Qn3DcG9uqMJhef?vzMfe0fsJ)4JB+o8cArd;lO4G3OqV|nW z!ae9EYO+ESB43x8N~oJm!pkIK*Oc8H!G5@{Mr|{AdJrQR}tgUdBEUt0P=%v}iX&uh>UWc}}ib&m605qd67!AE8u1z{J)k z^}OT$Uh@=7CmVI*hOJFH9NP;o{6Mziv<(v4r?(m!7HE2j7DnxB&u6NzMR;*8fP5%n z&99-1z^^u@g=piW(v#z!Mc=N5(hfW|EYX#b=uKU+GtrRrj-A_Z^#$=W>oSu!xN&eJ z+OGJIm@gZYKhc%dJJzfW$(0g={=bg#Au7PUT9CO}ekzmEn(7MsX^HyjA(9;>uTeyr zx7}G-x>=>7_R;P1KAl8sH_h8VA6^(T5cU<=BUaJ5W}j(%rx8Y-G1-3%w{!=|kDa!S zQUH3vvUZ$0|CvwtkJtAy;(j#F&@`0_d3~P-v z8^=HMwu=!MSy+I# ztRNW@@itaw!6>39P7C$NFEIPR$uJK8Kc@9W4|g)gy9U>j+Loy=aVSSv*p>Dwjl{^X z;*uH5W3r;tFsQ1@b<^zMJ|IW4DMzT&sy|ft@eorWm3Fpvioo!vawTW^YF&A96|!k3 zIm?0|N7t-uKd5QdAGQ`xv=&dYZonu#N{@YF#SiL~dVPVrGQq9HtcmVA|Cq&-l8YJJ z`=2*N5e#1A>Jo~V6G(FQU0yi$cMvPBn&B1h%EjbXRfnn2I#bQySaTv7YxO2)j_bGP z9mdD1W9*=L=b}3p$V5jTUjCBeJms$OWb8{|DJ~%S%1(57TWPGT_bOFpE94+-t^hqSH+HNSHU23fqHce(53+{!`8m5hDf9UG4E!-8oGGjO*;w?ZGz!Bpgl z!ccI*y{euY3It`OiVACLt9^zs$ZfzLo6k^8InpEJhl?xgv*M0#JcPTpO?R)s+#-*a z5X{w5Lx$Wq6tT}^`yOJhwgo|X? z3D1rxNAS;EFwehej&q}xtU6I){~hzUf|92(=w03oGAdmx42;d02n6Mqoha;BEN*RJ zGFblKcy>^f#iA!Vo1RVtN9Ua=5qq4!6`ZMbTiF{3OnadmIFQO>6|t{-R^Wq=cG+sB zd8w(AnxFG}{+EmV!SI-D?Ct2&kQhY!EQY(en7hbXdXU74ol}>AD8f}`44R?t;};G( zOKl%mq&6>+yX4Hx8|2FPJ4lkfXg{C@JY*%m>j z*1QAaOF>g)copeiih@e8;fjxArOoY2&vuTCtWKF*H4q1PF|l;6D?TO;fk$3iGRR?+iAVrIGb!eotGy^xH!F$S}0#h79YF&xV; z^W}FW>dH?zYs(NY^dqAvH zKrL{OHayz*ajX!3^w{)}&76K;>~t^C9Ab61KblMzIfoC(q9BGV)J;Xtc;eaMrtF+1 z-24uN^EwQwGCvy+?O_cZIAUo=0i1vwtGS8xs8F6vU#q!+k16dOzJk`4IlNoR+vV@- zo8)hESpU75zXe%H{W#2k5tGcEx-8N#v6x=hfax!|aO|XvHe4*$F16yh%>gEWl%GWxyI#!MK za*v-(%Ai&_SkzHl$v}s@G#b$~L9)pTfOY9c4MGs6r z)=#t{ldk`={jvOp>0er_t->9rxD@opiXzWD1fU=cW+*H(2q5 zFPd!r65-)wQi{=TYp~m}M~j2SF0%ig&n#g{Y!U%WBL|76*V;d_Cm>(fCdix96XMGV z6#<+`GAZD|@sJdV90<~7PH~?bQjQ~rlrC-eQ&goJLz!girVTys&|vkjw9tC^HvS4h z8lXEk4#MXIL>+RXm_H$@AtlFPqJ$4K7(ILFt7_wX(Xsm4%BZlqFXRBEdiDpmuq;}ny zn+8X-up!lI0g*aeU{-=LI0J=N9NJ7w1BPBQ?tujq2d=yGy;J}2b}RK}!A>zC5T5e%jB0w)r7 z<)(I(6`QD53Ev2-l!k9K1d5maPM{d<`8#WxYW))F@F0$1!4?stpaftug7OrSjC`pU z7xU2xyFV2cHentZHi@mWud&!4k*vToVC!gjtY`vMkF2-1sImjx+ z&UUEo1_;VADZ*4(p*=!JF;8r(AutZ-V(V9Y$Nxwc7EeuzQ_DZzgy3u9l zomszQcB&6*B!css^k)O*gZ^OjY1|x0~4A+e;VRqn);8gkgx0nhv+|W2(aJ1 zB2|n&f9#@?f%Atjml`(fD~LeTasjz!UO=*Zn~%h1!8!;~b|Y+>C7Y+h4hh=_;rOY0 z1*=p-a3%R$SuQu7PtxR;No)ik{6wk!1U2rIMn#m8QL9k*F5xGv5`z5%%Dh4OBe_~i zopPkY`rVhvYyIvf{@UGO0^s}~EzcWGHcPQ#`=fH*_heGCa6c{9D_7smd$Rdn{)Us+ z*F>z!x%Q8)r75fL1*#Q|+l5P7Y2QN-bHv!g)$sjefdF0 zdz2<5RwSB$nNsWd+Zt+1CZxCF&d?^eRd&GotpwX&HVv!n)M;IIF)gceZ+c%q@;hm) zI$rs~PwX>6L8choiy9-#X-vaww@$iE#ZH?NKb9r*i~Eo>`?ex_NQbw!G3UT4!eJ#`E)(+ z{B)`z&}7qoxOAX*(a%8Nz}7x{m{YP}bGY}DKot!^jNcer z)pFZzXI0^npKbmQ9GpX6Z}m34yCtBwBV|Z#Hy@u zpVJ4t9INF%GLq;ab9DjOvBM=YtTm|?F`jxjr@U5x@A>9tQdCcyAC6zImgX3Pq8YK$ zz}E7JULTI`J8Ng+%?aVg4w1`6@8@jO1ucMF9x;~i3+@p~*OV!JYd$;}El7jFax_ns zlxl(2YWp|6y7ypzM*`DY+)<60kG{zS^dJ!ffc7YSAwJTYKOvpD*sI2fEBaKD6hAJa zLK7-COW&fc^O(>vf58JX9)Z=NPor%V`m_i<$o2V|(G%Slu4jxFZtnYLPLPfA+(Il=E_7w!o=_ z2Q7BxD4_)Ihmjw-gRMlaIB=I@MDJ_miX&?8`9*)fwh;{r-LT#5^H?FG_AfsYF!^Bq z&8M~TFL2$cNvLpKmDr|Dm*8v zWe;r++!vDCovKr0vXQ;Mf`V8M=C9cP3GN4WwcQROIEY1BI|}-#R><|*_Hpi@eWzy6 z_|qu{;_WwelrJ5X8c%NV6!cNUN5>b6%z}kyVr^SLJ_6T141%CX!$&9Lqmp3{kxvzA zoA(VL{X0G;i8swLs>sa--LLZ`!^3qzT3`fxms@4uiZN3DNa6Bf6T|)ByzTK3#g?p^ zQ+ChWF2(zk5f5>&u8EY=gUN{33aVmLI)TZ^aQ&?!nQ1Pul&>|nns+ari$`(cR^W)e z%-8Bp7Ng)pa52FmoU|}NaHspktyVE{9vyd~j&r#eV$sB>x;x$fZY_3qyyNSzgRRA$ z)<{OO7(b0V-^XOLBALnJ;pxhOAoXkJJop;5uCnXDDogQ149AE#>OYqnMSU)ey@eg| zESzJ>WpZ?)r`wA9TFY;;%1f>Csg~IEV4nF~%WGQ8!>#4Bt@0eJe1cUz*D9Y=cq}%y z+Wt_O7A4tg`OQoiEeYYnK}G78Ikj7)+k=?_>~B#$Sl3ko{_9I_(_Z+K(&WODNecoS z+^r=yb(QETB26pzRwvu~#fJUg#fCjXRI~8xXfIkOC9(X)@!n8lvW%frve0aL=jpPR zVie`nHkt7kd%;t!(#aw(QtBTJKQ#>D0s4mU08M39jlm+r4Ml>7?K zL^yE%)D`kew_wplqXYI+Wf+I4RdXq^GNRob2m>m9RWk0kSAnI>>(7(rd^N0d)=e4I z9n*D-IR$EmwcFUyDx261$kpK8LIeu90nOj*4sjV4e{^u5ZWR5Cs9{lOz(2kdM(JUe z4^&Aq^<@T1@;Lv(a?t!?mG-mOLDjHpA#pe76@h5Zjr2HW4?M83>_jW>ma0yGfUVI? zwn9O}lB~eSKC~KcnVGtgdB%3No%F#XE{11eX^`V^l+e zn)_K_YiJ~epuze*`U=oighoo%*@6oOzf%4X7$a3P!BfMQ(4X>)Cinh#F=~LU65tE( zTC2*)7=@B}mO8|pD9Fh$6|+DhRaY(J#AhkP+A4#k??MDqKG!vC?s6K7US0DM?S@2H z^6W=v#?;>MTdZn0Nj3I-_F>ovh$pq5=NM${g_-Uzlci8?{_0V`k zEBu9H@QK6GdY-YAznQyn6RS4c`P@Ii~UN(hRHK7b{fT z3kw%`aI2wQo{qwji3>*4-L^D|5Ri#6T(?ay(qE$|0-Kt&v&($(ngYHVKkQv-5>Ogb z9j}oEfl_8rCrCD3s*Yc*=1-W?FZMLuB@-aPJ$=Gm&l{8^_BWqfTPaD;Aggr!~H%K~nW$plLS|iEZA(mluCcI~6=vg${eVBFRz=pV@!7 zHOu9hCvFH6U-xl^@9Q0=syRNF6alA|R@h(+VnG zUXNG~{lhLj&%lLQoEF?{79n4c;2uYJam>(1tEAi5_sZX%|3?12zU1sH_+mQ_u9$ho ziPn-bV9SduIpc(;&zx1Wnf@^D4n;RG;Ldt1t{{N_37u04>d{(;?S5Q$I*(<0L~Ged``^qhT_&P!H|iB0F9%`{oxqx=_8Uc0=~R3jBtNOjptOT4#Gxh2 z5FN?2$__&yCvOV4xvXvD%;ajV>T`N`=d(;aSN<{6vT(BM!wUKcB6Nn1^tYBBP3hyy z`Jh7}FGTzy(4Z>S&Or0eCA{h|l21hgf%g&Uu0L#`@T@?mE0j%`S-T70SeR7DS40II z__S}G_aXl8I(l$o9t^jZ{i}7}k=ClC_Iv4Rd=vGuR3pGGb?-}u&ouS!NC!+o{zQK; z+vA@yA|3jSKkqZFiR?auSP|DbL2@cZ4AWeT+f$v;6o%j+5j1AvJBx%+N26QA04 zZgX$s=$kk;|Jv@GRY(4|_?hXTVPD<7igiPWj>HDgur_WtP)=wQF416wFy(Rk>&9Fr zNp50w zu1;?ft|;?&73Il!&&l6fnw!dO)>HA{6|D|5B}fCYf^f6yxDZwcb^=Fk;|jPValp-s zKe5c~idBdm>DcVXH%W|c*T^05+t7Jo@NWrhY(47xfyX-B>aeHXbNKXQ;>j@Z z*ba|pr)TGse*RwPKOM6ZuqH3P)7N-RV88f6^BX?{Qhsckj7q^Z!T896YqH~6iEpEr z;T4MrVj00fP}9D~X@O6Zdq!f3*S~l=hH%bZ{CPb_w0ZHxw0ZHwRGZILrFn^-4B=cf zzWFi>R%3_MTu|!&r0&T1DRC$^z`O)m zTdSUKt!hIj!{UTErm!=1PVX@1em<}iFZd5k4X zG$aa>5^n6zgt5%5rG))tlq}xZ?#|F=m)eoK@8>l9(UFQm9_;o z795#5-tCJIqqGI@^`1f0c;hd^iJ~?gMP=8sx+dO4n`h*UXv2gyV2K{=lMc=AD%L$p z_vh6KraxVwM>2_5G%S9|irZHFpcOxWL=l!Vb;B|5k(_f{hE?@g?*)l|Z!D1L_r*qt z;~rO{fAoUPO|m;%We-_pj{yz$icw>vw^esf8zuc&jH#iQ0~<%3#(Uw*5UKJpj*qR< zs#pH@to*3wU-^4i=mFkbBabHjc7qkaPaRJj9~MZU>w@D0frH$~#-A+g3^XkyYz0S} zm-QF~n*N7xE403$XL3X6H*JN_6c*U{-jSK-Fo0(_ z)Nf*{_Ln^tJBb;! z=4=r--7j6H2?L;#b`B>^s^`bzBO9j8iDwn{{E|)*I#%FuES}pi?Gi)g6m^9jlN6N#dtL_UlJ;7IJsLfr}8G70c>UKMwrr45M z4b#U{Lx1ebWH_VnPm3>KQR;3uR)*}$v9HG`uJAP+yItPHu)4N&g;vSw82A7--;tST zFc&gqF1$yVW+nQE1@4mbTtofSSO9CNZ*#?i?16~`*n1k;!TmJ!lbW|)b>)X1u|ki4 z9Sif5bGAHVRo(YA{3ciatDfDS-O1RN{p$IS9x>(p@dv+D_4nzo4}PhKtocm(qC$78 z+0D;@O3gb1O;<^=bDH<7_&RG`U}L7ns@cu^aWc$|5_D2CS7YB6L>P6te9cMjS2$UT zVvaR4TLN@q=FeDxwMm@KmmnWs&sf-}?aNwtM$-R#TIp(g1Th-y$N2(J%%)r(HHY75 zjswX+Qzxl7>m>U8i{^C=lcDGdFvO~Ag9-J4`Uj=tw%5sUhJ!@xikX>j&}6<@I}dq}K4g!WkD#GtR3%sEUt zEWApG#wD|Lhc@XBJ+HHF$EQ-}Mh+vG#$^62zWzFujc6N}{hs*vB_F zcH9Q-fSBfJ6?#NW$-4@7oVD}lJ}d=`Pv5)VCM0T~mLhBd%|hZEzGrn9Ez;H70voqQ zIQZPtCwpPGP-{ITVMEQr9ZP(k3au~f2sFv<$_h-Jm?iG;k(uL|jV0hwU~6~coykxg zGpuS~;l9{cOd2`g1U7n(%nXw5oEe4t;_n$gxn7({aNLo_*`y`cq(SL@v7D3B|M^^# z-VEcQf;yat=H2m&ROrcq(0bTEqn!08u(9yS%mA}v*o?xx@%MT&&3vri;K;-$w)uvS zm))WrX*zA6Ni)3Q)`^K_3@@WYBImMxNIceIdyZ3CioRoi6G9pLej0XiO3b4y$?DVl zujc%TgRHI+O&%qD8G|Wsv;Stw08al6e-?TY?Kh`)XQNLlrx*-3#zp2L1Ax8!)!jn2 zd8)99c(~(lnfZsSld*AcLeebuT-$qQGP@bkYggNY{5$$i$441zTZetFXPf7+AqRUp zKFZWV$(b4Mq_^n7#9N-^m9Qe@#j~xWKck~BBfm>4@QHn##G#u9?cZXHLGu@@ne>~6 zQ&AEJNufu$KMd_#A}8Ecbo9Qop7NaI~JQhTQUOw|FlHdS??vVt?|K0Qefs(M?^J4Any-VW8>@s>Bg zJL$b&?dW*Zo4+I3q+Cs^@-J;Bj(+w>%kpTfzq^@+c@ zB32xNWS#wu=j1q`fhdq!0qEPOdWqOiU#o>@5W$yhzpJ3rLU8MJ~!k z##C)cRz1lbbnkYztAx{A;^U0KU2zc6G(|p}*8=Opmjg|>aAH2ET9cV9>Hkk{*`ea= zH9(xI+h~h{mD4`jGF@7NQNOB(+JtP)A84&-XZ{PG_gD}UFS|N3+`XUXb^77=w21$! z0`XUq-h1;8C-$+xJ|qcPJ~mJzYptq!#Dc5&*jL*dB5v1%(6i6q*t~Pu*gH>e6ryd} zVooK?{C7ftyBB66wH}8KI>EjAlt0Y351eJNLFSyyx1{$Ew9B8O2Vwp;$bB8os`gbO2Xlt#Ut|6 zeE&%aN1l?<-&s8B3N=4?Qo_-vBn)&G2T7QHQo=DOB}~qGqNeo=r(sch=s$rs{slDi z!hS+lhIxn%awA|yQp8yu#4#gTne+ynk!+u(vBnw6PHzA<+fe2nRq675syI(nk@~!g zufaS}+huMRMd)XLxFTYI8+3-f1umfZ9kGu?6ES`HbPNz;pH2r!iZXwvm?@jLebwWX zA&=r&!t;F|>6zfKsI+oW;^HH$MMW#^u{2dtN}#dQyA@mVO>F4evQl z-wu|TzIC45CCZ=DZaAuO;alYzMUeln8NxQBSL%h4I|5DrU<;a_!v+wz=Ph3EgyU7T zees+?Vh>+j+236#wDdi4bDTJSOW>|ad=h+*+~jj!J$~-pBV3CwXx#3)__OXrHxiV! zMUd&JB<_pSxxH4EUv2Aoh{RIXKPhwB&5g%@|Em$Mz+G?Z8kWtGV*1bJAnj}1-u}*c z?%t<33|}Roe_iu{Yhh5ItlY2(p6A?V^5osQrt$c*TmO~0Xr+{UzmzL2zaTR!a2K*% z_zcS9uWR}v{0ZD6^rQ4_@el7m99Z7Q7Y#1@>qBEFpC$(w|3Ir3R?%w2sJ^;ex}jdk zf4=Q;^;+%Ge|CICNAw14Wqq~%@LtiwG?Sbtvk`l?L3(_x{pS2WlZPETou|a_WZH4k z?k$;ul4(cOT8bI5Y1Y#P$pbau@A$2*XXW^I*YD)P3t|kaMp`kp*R`F4<~SqG&TXH) zawbgkOs*4QUqe_r;vHowb{YhEe9xnEcONFJuw7(w)5CLAPyW~SJ(t&a8}-XoaBXw^6ZGdM z=ug%iRyMtlm;z>>NT|AavzR}{221_SX_8UAIFGXOvGE!HqSZZv@5I>UoAzZap;|lc z^LFx3IEi#0b>@oD$+nlHlr4rSNvzKq`!FRvmY(m)8Qb;2bKb!mquforrcgs{-raAfQBE%t`J)h6@AAyWadF+WpurbAt!BlxZ2hBNB= z`EW*Toc%a9?nHIN`DVQ(TJJf$S``+Gut_Y@mO#-=*l%u*7r6;@vVw~`bI zW10aD3zAMEg}~GA$UUrwny?r!=HKrrkoa%OuI;BN3xJ9>W9{W+O`7GJRb^KHEQ|3O=J)zkBqIRG9=)p9kvx^Xa`xN zB>7Qm^n|fSnjFD_|RXopw`Db&i1jihkC4D*0q zIUzr5!mF`{ahp4#VOTbGcFg$^`Xs^;01Da#%g)LluB3Ry-w-z{Hd{%@xu7lr8oHh4q&fj7YI!Y*SXHA`jZt^i1+}3wsH;o8BycVy>4j zt)R6+7ugBcuodtkWWOo&L_S&OXBFS_nF=u(zoBdt?$bfcsWu@)2H9F|GsxEJB)&&7 zgy_XHLQR%!hwFf)7Hh=+)>9B0xu$WoRfhjG!w=kT#RWiN6rs|8{lHWbeCU2ff|G@< zpDgoJRYsB4f2IHiIRwKL2cf4)QG@fj2w#CHzrtuJA|KDebc2k9HfPP+^svLznVLODQX#3_jQM zxs}gl;}2b$&rX6fOL7uN&Ji%!6Ln@I1Pxc%!*j!x$0TQX;i<;bOy3#IGn7!$wQJ7c zz0;6LUYTFu3!V!8VT!_PaGD|M5SekXk2)XM-F-_iEx&a|rj;87+&PRayXuP)oZLq0 zaQ#Q8sPR}9@(O$vFN|*?b1}g*68bN*x$sH{66A0y1v>+CO_@+{lKmo8M#ieu(1gTY`v>hc80gSq_3d;&;`4zzJ*r;2yZLm z*DcparmLRL2+osU{;dFd5GkuYkR}cEe-jAR)qG$HE~%?IX$dZhI$uJAiht6>FX=?J z8!pnF3-QX3C5faBh8u@XUA&N9Mk-)ccbH)&JP)f zG2tf6hv{6T+*;ntYc4#z^Vme z*3e!1h-UBIfJz84Tl-F&ll12idq}^^4hRd;9xfXu4w%2AE!Qs9T--_=kjSNYaUA>e9AXU>=160k4x20fvXq*_#X`ALKKW?1d6hCe{1kTfS$fnW2T z6%F4EQ^S|{?f~aIS&SNgnhfE=We@r$BU=4A&j$sq@BNA(|ypW(lR=f$)Wzv)kMN_7zf_9nJ zLoP{qCuTW|njX`CY~D5ZVppwJ_ZGd7?zdu5F}LNkm35pxU)$qgcn zdURUdc=ddmTW>AaKk1PXk@nM(7&=C+pT#C)8f+Ye6KwMOweZO4D~u&g{v3kGmpS#H zaRYHv`}|RaC*!`ht$w8FBg?Aqrf;0ODimY$Zxr$!-+D8Tn1t&~%guwZNRDdl7`Crf z4z66_T>{KRdh=&wfb?B-VroLsWuX)^j|OU+Wd9AFeq>T#glpw!9*njBfaF$<79N0Q z@6hKE@>(n7QO#)~NrLb)u>%YD<L!cn|?_nL*#4n3p_*;N0>P03=f5m1X)71T2oBs8zxumwqBIb%j4=LxQ zfde~H(Tt?`$on~Z&Mx{}Rjgpa_+g|cW*E8h@!12569vsih2L12K?p`_EiWf8oYX7P zzV{Zr0VyQrqP)jbp$&}BUchTGd7fo7-Y8wh`AA;j!0=p@q4<%?`Vf1IF*`LOj zsiL|>Uos~;J!OFO2cs0?TUwCbaA*^8?Q}_$Vz|ewJB{k02(-YrQj(<<>AuZsXGvHg zi4qa%`QnCwpRJI--i`Q2%XMgn68oq7&Y8h0>yHGkh!k1xEVc$`h#)+cg!Anw0^%gk zZwhsa&(JjOi zD-k1f@wIhCUy_^gudH`Q8&319Xx%XoN-uj>*^P<=1o^gpwSbpW`mXVg_)@&cJyC%M zz9~C~siMs&3ER~j{oqCco?UA-8Llr{4UGHM7+@C2O#wZp(gvwM0r{A!oPp(QD${;1 zv#Br_-!A=A#w6%);D9-quKMpWy7G#5r6bxMOw5hO{;H;DA$?6Ea$&w0x}_2@K{X~5 z&_~p!ZR2B9+OjvN$r$WUq>Z1Pmp{Y&`8&ZD-BU+=Qm4d4mGJj8GcU1H7AOR^wFN9*7xBLFv~(ep7)UvO)y-o0WdUK_i0J%8FJsEuoxNX|rX8tP^jsb?X2#i$r@? zR!g+7uUMwZquSSqRj8KBc;H&IC*NzL6mEHH-Nw_+Zb%_a*|iiskJs{s8~#?Iy9#u$ zu822CkX6C;QvD(Hr@|=@#%Hd#l#<(eT>rY!S8YJ`a1HgZFnUO@)MBGYcJo!T;Bdsa z#BEFA^Adc`Rz#BzG!yxcbU7!cG zCX7n{6h_f@>P-dHBvtWT3MLgN)S;=D!Yj>1u5nSnnitC3a%fpoz0$UCL7>V@=a6Iq z3t3D2`YC{o|Jypp6NKp5ezUS+WqBm;Ii@CU-2uMJ*R}JvD5{c=skCiH`ac%3(Z81| zyJl3S94j85-6MeO#J}dwqICrCvP5?bGYpCB(F8p$DqSb}fX8QdQjDIYM_-r6{eQ)D zzavqA`osMC+l8$BB4U>upbA>A!QhP2clPjl1KZ>4w6_Ja4da{BeRl^^-+6^cqk$|_LiLpke;)#0e)b(G^ z`WHNyT>n)H8>d(q4~<x2h_kwR)ZZ==|?1|#E zSW#%Sq=|hfv87SIxP0Bif`myorC8Qw^Gh4h2R|nWDeYvs-XT5TLMiaXZHb$yt;v!) z%nF@cS+U+t{ImiX4gv}iYjJG7uR_0_qNXDkA?*}LH7?^BuHg4PQpA74o57ZcP@dZDsuTC(ux6=(-X4W0KWqgNNmAupjDWQ&^C!&tO3= zC8@VRf+Om$uT`BxfaDak?LO%WLAD^5XXAqBaB5U)@iC)i9Pc20`4dB z#d({FtX$t6T%=83JHcGB+}5q5sdWp8q`@*5*In{2bLeN&nQW0z@!O=Y#KtZsAh$%M zVZ}tD5AbS@@}yY*x9L*JjQ(n1o&f>1ew@9@FY=!9oFnhB!tNQuJP8vqcUzu#y4=(I zs5?9p1AxKRm`_hEJQIKdFP5CzoTAHS?76)nyND2;FyNLqpDtf~MYzW48>(JfDhJ9e zqJ)j4VU#?Kg3VSlhNZ3R7bA#bKX%wIdU!CMYb0CP9mO1t1z{k_AYC19%S5vu5qcP8POGv zu)kA_G20c|#c)|4a>j@mR|Y=5n1GH&%3px+Duq4dNmlkHWqmni$2ha(_&bQMN2qgM zg-Y=g$zcJ}N3y*w1g0uGRie+lGRe|2{Z&R++?%;TZ-#{RB)`HXYe%v;4jzEn)giTq9bs{q*4EoKNn^F-|AtY^y z7QM@_y3u)?>>>S*mXKbN*w{=U%la&nc&<)Uvfd^KWzt8u>VGe|CSj*f@}soEpr<)| zqt7HoQlgJ^)Lls1gsMC%){a2wf2(xT|5j<+|3>Lh ztNkIe471|G)9@ISv8k9ZDjE0`cz|7kKgOw3R(Fy9@O84f-KM{+VNXhe5=hUD!aJAh zr{>7SHmTKN?@8Xn554btwML*bA3UZ|pG7Y4hM@Z0<g>lURd~u3a-@ zpP&i(%Vn^SjVMuOhCV`|W?U9o^EkcYgVEKRP|cNVXDYPOs3*mDwz{m8{0=E52G|#7 z$7e(f-mM#9?ibz>Pg+=k1TOk$V&3+K`eoi8W#0DIsd?*{dF!ue>a$OaHKrmcUFkbp z^^3>Wte%Q8^uHpyjDVEY^B|q&+F5VAcOc@iE^_BKKAeI$r|RtLsX5>k{mh(PzhIhT z-k=)mXBX+c$Yo&u;JqP4B^B!*D%am+iGbOm_p(%q*1tM?$u>)SqNTins8-sRNa{bHf?&`e@#D+7| zd@b$$yY&0H&!XQ8C&w%FTgI%#gk_CBG7?|DY2&=Ke16V4W{*ERASbV_f zhiez62e)}!gV(usEysr7X)C{Ru2|0Pr<3?Sjo(-%tg?AyvEgfobQ9k-h2Ycw%Kke5 z>Xh{;8E+hUdv2mTmHp`u@=MFJida(XJ*}3Ti*t+gAHu4!qnaD4Ie&hKx5J31bWfSa^<5Cbb`&!lt#yWlnG43ME{Jj)Dn3oD6jm2Dzv){WyzgL-4|+e zT<`1^r=ACjJHj)>^c@I?((HJ|80B)6D-mVMVrurx7{s48cHKl{57XG3&uHT0n#mQ} z1`iLHN3@i$2dC#QaR+DQ8jc@?EctdYl?!=3S)Tq2KT`%*(J2c9qw~e-72)78cG=$z zNmGTMMh9sFd?%|{3qN6FBwF#>Mn9^CCXANva}e{8eN=^%fT@g&33SjywIzk)1Wj3K zCCYjA(ngmy9QEAoD|1)hQjSmWDL)PXCz%&**jG7Rg|CT|%mcncclFGO-_w}Nc_pR< z6+FPB*+_mwx9P9M<9B+6Yz*fU?kV}*c8Dt!l+@VyUr;9~-@qmtM&#Q4YsI;m!IjwB zNcHNeP`TC0cu_)~2(-!4raY$?7Qr14tQmH@#GB?8J(KzD6Io z!|H3_nCxwfZWu2|Nxzwc9i^i7J64Oo0Ynh_dN5U~cH+}Q!`Dr zsH<^5)l2FjOLfG(}Dvx9xYFTeJZ$5vA+{{on4g65^HI*ME`LEUmRVnwj zKv6u6W!>7ju>V+S*ez}eWDd1OgFSVlYXe`Z8~tjatZp_kEx9FVfyE(V^XtCbas}~pf6W% z6*26bP`dR#JkmZ)4$oQ65xup3y2y6dw$)F!K(nOq(jD@eO(|jZof~)P&ryQ1xbOuZ zFq$6=F3Z*bO&Tm63#BI+Eq+E@(fh37(y^scvqklt8=s*^s7h#o6}*x*-j>m7LwMyx zpSxYUq5q8MLQggXQ%vGgV9jkT9ccXi?b6-wcg~Y(0pfUtqNjBuHw4x|qlBcMBYLGk z$?!k$9wv@9mzHq-j_>pB`tENTA=(&C-HvV;Bj#f?Ds4ur;*r(-N@z68wf%KU!SM8b z#p)hku;Tz<9FhYskE%T|7^3|!oReZ$bWxXpLp%zE)qZy%AM|>sD>ymI+;`cha&Ao83xRA$hS++JXZ`oG^ zvx&eUG6QNCb81zN{c+^w=FOeqV%*K$p;#8|Wj0Wq>{ayjIyw|tJ?46+CDinaB>UR_ z7m#GvuDZ;%GQb8qUx@6>9pHslKi$q0;NUSBOA1WGnoaxuY^U6LNsyH2pn{<#+73Zp z0(E|%WNhyK*|do8??FebFL^NEnWTL`S=ylm+I()hYN0`@5{8q%`saYT8H`x2ed?L3 z{&UjI9NrpvPQ8_lvj3lD&gLK|=e9K6TR&T*G&J2ucav4qs#iv*N=-ucTh%jXyox$j zgk}%gr7Lf#u3(5!Yu9FQBlMPm`2ja0S519?x|`PHSJhou8*tZM_-Y{Ce3Em7ATUOu zoxlfBNY4R<46CT3Byv`gK97L+mgK}g>L%N4 z$;J%}Uv~K|S*6teAiE(cpoPJFJXMsn5#)vg;Yufq@(OX99cWDz=MZ(?u5`+pb^0y@ ze%t6;*)N_7+U>&2x1OZD0G{?&x>Ggd{F`}MT29y)i6D(Te2YR_XO8SlAKx&5^LqWKEN$TPOXvZ;0>?=P<8T(%1gAz z#*kOFh~!xRsTzm7e*o038~HL}6=qS8a@s*k>eHBTwK(cVo-*C%NB)cHK0mTHaDUy% zR|ENVBf&sk-N@#^vbvE+0(Xcp1Ao31xTS97E5Uib8mFti9%PB6(`=)QW>@wLbEO`9 zjR&o%H>`{Ys204E_#-@_Cu;*&OWX4_8N(t(V22{tu7)!rED`KzbeZ#I z*!!u-g`9e1VkMlm1LV;Eio)% zoC(*w+U9>v4!Ufk{@2#>17@?K`a-V2$Nb86+55{k9ckPPd101mvVP;5chN;hFb^C5Wd|vpPHX8A` z9)x#ooPz}O_xLJ%ha&d78&g*jK4ow<$@A8jiqWf18EYEWLWQ_}OiwrOBp3_xTdlu> zspFG`oepC))=Z=GDbo3&F2g@eOj!)iFtgjyOm|ZE&HL+O*@U0b_R-&)_xzWqo_cDE zBP;_GKe`8}6rCX@gYSEXUYzW{a6t?}Nzzt-Y;^y?fzqfm^_RF+dp4HiFpD2RlK9|$ zl4Kjy+IHFoLrpgP7UZ_AN$xiJhBhwr9o#r=$)n_yTID;qX;6O6^R-sbh6kScW)}aJ z7LNr}L$NVm`=;}&3CgSrC$I7y+B66gKY7*=5CI?X!&m63O5O8+ffto6VoSQ)?*G9r znQg35l9-9KCDuSPeyD${PEqyC+`h8O`e64qlr|R2M#EqrhV?6o_8+_{i9`ZOiqCBO zop1!MUG|jKlJL1v!nI+fAMChXS!4E3gu59^C@&nftu>nITC^AYnQ*0h(GlN3m2f)+ zz6*RG3COW9CDOHMZyec7N~Cdf%;k>!U~`kpozK&hNb{m2@+39#qIyy%PgdU98C=Y2 z1?}Un(%yW?@?6?W#|VLgk5XJE@}}i9E%9d>Hjx6V4%aSX_;{Nq6jBB}ReVw9HR!2` zX3jw%Lx-ieFCu`E?mq52r@EWZZ%3r7)$g*z2~2>zZm0CCvC8gXTc13B{rgdosM1HAtUUdt(#BQ}Yd^keaYVGcBC`JmOkcmWkMD zrq<~@RDFlf@2p;gmgiQaV{nSnTO}!r{H1hXhBQNpq;W9aXm51qKA88Q?@-lVtuPCO z!#Jv(_*P@gB1o`E>@}!9%o)HA^;#Z;=L)=Yj;KA~x8GI&3O7k@1XlZ<6P0Xv@pGsX=!T(2itdz!Rk!pOKtSv- zs#t=b&ubHlm2s}s9PbZ2fg$1Wnyty~T>t#nL$llb15z@6Lz|SRwpW71><=uZ*uc9N zr%~+jFZ{tvG2(bycz1R%Bj<=I@|1#QbxX4YgVCqc;@2d0^vlSimFm1@o)fzCsgCGgk4&5;GH!?ovU{$Kt!KO zlMG|0(W?3`Y_w??fS;<*gi(L-uISRVjyp^u1S)QT)( z*75L+r8N#$y%@3&b&8|H&hnd&KmRyA72R}A%aijX+`4& zs~SBi?669H9y)yqKajzg-C_T0ZT?pitiY=VDxIXDcz{*7P2?5Gq8a|qyS?XF+!_p+#7jW}mKgh*>Z_T5dr1nFf#Y~6$!?KSNn zl;}G4)}adz!Gp96eq!<QW02n6)7?lEyR(w-4RzKBd%O*ZxBFa>r z+4z*fruh_yXClwbw0!31W#~BuyMhaFB5p?nhySD_4@EPR zgtLLbN4}?&8FRIE_WBO3@-^>;i2a&I+X~^jgu^2F`?x3o|KUV}ZJ8i|p>fyNOzqw` zo#3WNbA%sw(}@bI$fJU>Im7N2Z8?fS?T{JjDe8P(Vu^%49E)Z;d|z^GN)CU?9#1RQ zo8P-Aa+L4}M|d-CqN{1*v57V&-ge-CFRAKE-*QLwY*av@5!Y7mFA#Btv%~jBoU2=k z^z(mzas1IKEDo=aK4{~8wszlrj!pLPeRgu1K5V>=a)`WAZ2S0|TBPr}tggIP@sDn{ zBdtIb1=*~Hok}u`R%a(?G1F#SjNXY`Nqj2*c_Q(Up@%T8CHm?N!dtdLk8ms+&dnJ| zLZO|&_sr)X+wvP6>HH2(zNAMiaBR7ooP?wJ5AJYgBoByv)_P~54}@ZlJxibec*#)z;Zd!*0ta%a zWwXe5W3{Qay(@jOts~k|eWA1|^5=K)#J7ZJw&l8ISAm??UTYh|p8cH}rD&##$tt7w zQFbKH6T|=DD3L0DM)|Ov)T4G?A<&gSKZ6SkSvROE!wofSZd(9 z!I(8*!?r`oX%6=*&L}*sh1gSjM1RW9a!g5|61rl%x8DuZr{395^h1~N!hX91Lga1| z6=^c~R_@{c;BMnh22z3_6)Jw2eim*-Am<3r3~zQPv$p5uX*@gJ_*l*xyp{*T|k=7qNj~hv;LP(Zld9-iz@x)7}#+ z3C|tS)ME0*{9-m6R<|uh+du25&EGTYFziyR9XWcqtOt~>I)ZPbq(}AZ5_Rf4lwU+{ zyv={A>c479c%Mk)wivDx9+Q6%r4*_5lJ9Tm-g<7$>hPD`TC>`!%r&jnm(p*;miF1q z#&`z($g{{BL6gUI zNvR@>m6)%DAJ&^hJX+~ET;mL^5_6rvH59_eOxagRq71oWCiK6ipmr|)%WW4sXt_E$ z)ar@<_u$J$>6;Z6+qD5YX^p?Y!hQ_emrDCjNHxD>s(Fe*KV&;Vc&V&pVi%L650Fn` z1^SZjFf*LeSbt3`eeJBjGPlYnz+(2FZy2mi*G9F2GT%|pzlNT4UW2INerQ4?^BMT! zyIBm~TjL~{5Z=|!qaSOBm}=C2=_kWq8q+q69sShjAFld{SBHfgpEd3ui_g%q!;6e! z(7dyVDYW4E`^efL(wG+gZx9ek@|npQ60CjDtt}@k8pcUiXkU!p8ySl}@cfvTjn46k z7k$lmU2GVWY3{fCZVUWDv-qs5vD)stn2VA2CA#SedYUQBau>3W7VWJ5L$+%6o%z~v z%xC|JlcVM^iB5Q_Nz9&cLEz45go7FrVkCigl?0#ps`C3J5Z->di$7$Qy^ zSYH5RW7sSi_|E1f5k&{Kia&X-ZE_#!^$%Zq$WY}uNmgh}v|Iw*Z6{sw_AzarvfpHC z-^gM--u7^MXgIy5+g+UhaJ!%@9Eq^Glh0(s_JK6Jveyjcc~;Gx~NOAF7FdB=%bo&lp7YCB$MvCx=3(qh-a*y203_ot=VJkeTLb7^wV_6^enXFg9NJ!SKF6M z=qVg7jq7xEXEwS@10TVySt`t1s3}X_kSg4?eyzyVo^$+KXv#EJ&l~6qnFukU4Np_E z&7w>`D37h$1AjH9UqMe0(tA6Eop!6hPM1!#qKIm~GsJa|4SpP%UT@`qWYmF7i8!#g z5(84VSCaRLGQsN~v&radiJ;-J`ho92Pf(X;`SsN%Ks;{spEhqL+TV8i`*fAw(Q6@0 z;MUTdL44O2kusIV7;gAFe{FtGbjN0Xl#?#%Ke3aq zcD-W_H4TM^7i_wXqj%AFmEt6quRPB6AN6+0rdZ~|y@vM^=(E2A`*yBmEmNZUBFANRiho)Q+y8rJ5PUmo^U3*ww{nN%XB}q2VVb zA1Tg+A`M{dGnNL=2h)}YPr2$#+1Xe+dEsD(6mKy`p>>2l@dlisPm-`f^LNM`DbPug zA9G=h(zRWXNI3zmK?G5(zc7|#?ArU=68yKM-1%O+U-xR)6^AB|U z2gpm9(v!Ra7lGcgVE|rBeCpFZ&fkbpWw-zIVI@~wls=tZ(xoa_h zTrYf#emgeIVG1wP+KgAMfmF)A@)y(HhllgHwe^4WRu0FLdOLgV>Ml}*BHQQ7#(D~e zd4K`!4YEOz3FpUKT-$fj7C&FyvT+uB@A&v1w!*1_UgQ05&>3slm~yFdMgM`Tp-!U5 zsIDzebd7G%@p|#Q5EW~*isoK%OD2=^>$&G^uATcv2R);cNj+(YhdHeHnzx=|P)p0s z6B1N1n|N-Jw4iT%jq=`h-@e*xW^*KK%hhZkp^@9Sq$!?YgDpNg6zdD^8$bHdwJ5G6 zVbIaOYyIpo!zcPb=o@_RUJPH7;UVk{yi<;9WiW^bic3qPFA3Sx_mgx^rGO`ux}4{} zV~_4o53V3CA)DC>H}Np#P*xIM;S3k*MWL9pSf34T6mwp2NE9Pvv(By1{PkAjLOiAq zo)Zo>lhYzj5Glffpfx>+AfSug(hs5QY-rkSw{^yoyaQfNl4TvyElI+&ZW~Vm>NTm! zId?8;+mb}IrZc=Z6>E>vn!^RR1RgbDorKlW#Ri3KKdc(N1J7BhpdC#H5qXB9NVN;cK z@lk?}uf{BMKs(CD)3yR5UrY+t@Bj<@#wcexSG^5C2?!~^ExSR@-sJ2rV}?{X*4ZV~ zPBud!@S&PhkdEHYF1@QMek;7YcrOX%8j1-w;c9r9-(9*m)}PdmKr zBUut6kL%m&YWPQrf>y%96_N6TRnr7Jq+f}a^W@$hxgO^ACZC5R#gDgCeGApwAGY>( zF7>|`oEEBSBK^j4dd&$gv7B(5!%kIR8P`B?{KMypUHKu^t={Ae9`sj(r!oZdaxIrK zE~A>OzKH9trv5^{wh5nHWb5Bebg9wc1Nd=@YrD7r_9nY&vh#$3Q82fwhV-uf zX}z7M9d&2dH#$G4L5_B;>l@SH(vyvowBOM!PB#f^DR1v0mTXtw9)jAZ^j7ZiHe3B~ z0TbQH6!jGD5pa|ZcYSKE!Eb+FNO7N~>r?K!8otH+5nHcc&gN3-ICzi%p>#X=H4V1z zpaDvPras=)q+ib}miKZRq%SA?gV#!Sk>LEHR+-4)te&eJ3`~ZV?9$vc9KWeI*&<_A z_>kwS9}!SrH4NF2Qr!528$u8#a9w}?e}H*@-M~=2PFB+wAK(r)-)DSv4uDB9w@5+Q~yc9Q?Z3*Jj02mnFnp{lZdAj%k$w`3j{6zglZc zH2iAHRyUi)2Ak`JRg~|)0=`1R@eOQ!Xxky_4`-_czTYGb22Tz83C&SIp9?krGU&lT zy0M7*?cDD9#w7^k(%xok@Bf6BnXgKRAd}Kc)H6`E6AZ`>CC~}ZkwX<6K&7Jim5%;r zg^`{z%de0|XmXZ+sIC&T{G7V#Gcll+tBE;(nhUe(BsSxkcGyR3BjNr7tOQ}g*+1_7 zj@A7HpGW!p1s~~=#r7xvrrfgHk`$+I{NX$R_iVVtI@x?kzfhBEI>8o{YdKNpcPq3z zMRHVlUQ3JGw@b(>OCFl{Gg>VV`uyc35kl71LZ{e;-;g zIa7Q0&2RCt3ml!_EC+3ubiuE>a6np>cQ4KSUex}*;C!PGGnKsSl8(>||L~;(QQ*1l zv=DivuglEoUZ1O-4L3XiiiH~<7yR2H$#ZP6(h^~vA?Xc5U-kOVi%hnoxAK6*m=F)E zXWZ&A+Lpqag9>%TlfqXgR=~hk>|y*FZ5By`5ue-ZKQKx97NM&&On{?1F*azLg+tKJ#y%B+Fy~?TBeaKs ztlqL$i4~-ky(V5$E{l2A=JHdCb{Maa_{oJokG^JIWQXwj4}>me)m{;Ou?;VOcHPzVRCC+Pdvi;Y)h`uX6nExnUhtwrBbi z@;CVr{&xGvdZrig<75oNz5Z-6biWhv4>nMW@&>Pf_cnKi?>HXfwoqYcoV+14K4;Us zp3MI#Os=Bl-?mE$L814AiObo`XuPcjo0C}qaiT;bIwRHuYT=PB(@A$#_*S@P50~-j zpC>9!n>>FjT)2lrxGyHrE7EF{rMO!i@WW-N2sBlj21ap9M5$%$!O5E*z!0$GudMFS zeweW)5F=VWOAZx_!*D)9kMGRZWUwgc(N2bg@;E+U9&myVq*}F;m_7?G`I{)0q@6Wq z@XKTk>rE~IEWOFO{M9(*wudgvaBaT_jF7F(Rew9bm}M2DoKtsA4{9U&2!}Ubn~%Kj zLl?}uKn4aIetLG-zBid*OV91?>}sJt7iGKlaps)!hVcDx*S4Ot)kRnR0aga5%6L-O{;pW(DT4)oFKDL8l_ZLd{tT#f{<>>FpC-Ni56P^f z;S*09?HH{jf4il(v%i6cdOMYDs@HD3y|AzAQ`K;b{sI*h)ujt3Pj|I+#S;8ZlJ)PAdYYzR3^yuVAUHgr}Cx83u-mcygvT;3FcXxJR zG{1kk^q`x2UFx-awVIx;z3A+EiAlup#jH?|C&O6z59BnFyTJ9FQ8`cDKce$J7Ioi z0K27u!HX&R{f8LEIOdiq*>V-*3ZbolH>bO|D_qzEMpm8*SDs!3vU&d>Zc_>5%x5lC1MwNT7OF#9!6IWg*OSu2t*yD zlsHzAZz07)F|wOV>a^ud^hsY!Jv162Qnd)79j?U~bWcfz@iP6pMg^YuheI{)B&azH zz_JAog-%Q0hEUA`Nr3F{_^E6k!P!5R15aRv;{6%A6>bC10GbZWMH)%U(g(JU>5EQ{ zj~hEidpud08?i-4?K4At*96S1q@CPa!}eNS5bg0~5;iwws@8gFPj;;qK#cY ztwg^S(4j$6QXn?Uc~Q0H7sd%7a3A&}l3)DIiO2 zsoYZqCzsz1+>>s*>K|9XdITDUJ>~#FoWBx8q4!wDaVkHCfx)T3T}pZ(54b6Z>fPof7`=*@4k$XRa z9$aE*>AmvSGSKKfqbC>{UujIsR$N_>$tW%%F7dgUnO{&E%rV}^^%ta5g~t>*MHTB> zTH_8(8}uJwb&Hr)^S>vj`&NEl^6YhPa43G^$5N(jtnq5wkgrU4)$gRQ^mEFj7VFF8 z6=+4VS(6^SF@al<7%j72q0g5BV38a3n5a){PW4X*U{lbhNFVw@_(FkWiJ%I>?@s9- zb1(E>7PVv*o$iM}91mSs8hAobu{O!}fpjbWwA?DZ*rG1hijAQ=bSLW2rJLQgCm|3g z%=aMtIPx(aK{=o>nmAm~vYrS(pBmgX*kTR*i+0rZL1=Vo@Rin-rM4lVXqagWd2`i& z3>*}RI}1^x0i&z>;gf3U!cBGxq=>Fad<rZ}B($dXxNp`!)XRTLGi? zlGMYAEZp!OKekG;}M@!q{kCtz8)UpkjS?xxQ;ZZK&c949q%I z9Yx-K!&~M?X0oN{`C3ryx7Eo1Xgp<0vT_xS?vNpsM5FTB;-dBT&zNGF0{s;c*}NpBE}zyrr;g-% z-4ttms?CzkFr_EPm@k{&6|bAqJEe$4iwdr23emi=a8!U6E&cKvLK=PhRBkRco6%Co z8zhq8p0-1fL7qe(m}w11UzYcEZ%reM-G0KWVg6BI^>xCQ>_?CDH&|?J%~Yoh?@{0A z)|uK4DIaRK`9`S)?697a53l;9fBJZ=|yU@_wMh<3&z6r zb({oVI0r3G-0$ai=)&18X{btSKLU+T-rv?B-4pBxE5JQ7YpC1!^^oyMwjIory{hB3 z5m8}SA0n)wON%Pd?FC(|R^k&5*L>|yk>>fW#y!+$pN~^ON{7A_`5-xG|N8 z`Xw@>H+Fwm0sk4d7%plG^hUXmT7bm85+n*Zh&7NYi;izx=w!Ykq+!Rv$|#U8L%4y5 zpO*#Y+gCkL$t$>#5iyH2rsVOU>NNLQK~Ls~-c~?TO{?`By0CW33ZCAxM+G^KPX;-T z7G!^|=|`L|Y@NQ>r9ZCFUjjnI_K$K2+dnxNk;3u9v@LYS6_O;SQ_7H=?;6`abx<)o zErnKHTFA&f5@}D`%YU(?rkgt@7tq!D{nt`X0@eFET|a8J9rlhgH#NQs=xL{*vK9OD zKUw3OIwz(}p?HzG=DZgw)+hhDRkw)jABm|7D>7oN4_Sk1#=J2E)@iW%`TgG&Y%-@y z(d~d!JIEZQv|4Y8K8+Fh*6WGS0F1NqVcZU&UC_yhCn~S>4cER8F`Q1AhH8g2;k!@^!4u4wN*2C-oN<^0PKXl3P|>(M z|0O}=5_^+;D|)y15_%_^e6e>5y4T4jQjVraZ9uvk)&|A~JCxOd8@fI<&(lFqZ}Mi5 zH>lm@7)E)@ULbkH#Jwi-p7}oP?H$oZS*=(^pdT)gSV%=W(Z*y0xXgxu{u%v|C=@Ww zVkuD`4Z?Q?GT2w#dHApi(f9){he-JfVEayqJNYH{axxw&W>V29m2&f_F(^hk40VRo z>6bd<>m={!E@taaI!DR@WJ-mn!t)WQ4dy3O~%SABp0|eEpw2K5Sw#u!*U0hilw;0bwfO7RJ5M z4FiZ3ag2xv?#R{oxpy87-mZ-3Dl-ssT(1O()v3_8R@BduE!J&2MCobbVlzLNk_i_Z z*RBT%){u)YnDVT_;|42z%pwJ?@S_oDq$ugdDc6ZOZ?PW zQm8oz({03Vl`SfM>SXl;ipiUpJ#+oIef~-RE!QqNM1`%YMsUt?h;3f z>a@m0or&xoiS{8*#IC%k*N({n(CHhf_KJ&Cji*#2c+S<5a{C@zQoeZVbQzvHv2bcA z$>s|>t9prWGuaJ=Z%(bW#e_0mp%wP2RGlMC3k0j;iF@3(_Krk{Va?Q&8JdkER_=QVWDOGtTkh9Q2uvII| zlE%ogE&v`0Je0rEcdj~3-3y$i0*02rYHsq7w+vhn!88m?^SKBMM;fpQhgTyLed4BTlw zv?x)CC!$l{Xf(?-8~7>+l378X_9V=%D?Nsbk{n(A8O+TL!sh9vEo_7cdDzkXSrSB+ zkTfStZHJ1Zo;%1krmx6^g}BrgO;&{JkC@RxGGKhRi){|G&=tILX<-)L@&3$2ruxKO zFcU$z5AXiy7At1k9-%TrfhQt!@`;4}$7r4v!6~6x=7C06L^2|tR0s{t2aw0O6Syz(@33F(x z76#I=PbLuv${fzpm~46&v1`|r;u@Nn{?PLLC7BMgACCv)%45?_>1!wFi7@)m=@LG6 z95bEMOnSWr^X3T(BJ(fr7x}=3g|ch-M%e@o!6(I&$y`vzS=1@v@KHB0+0_}4CsM~p zGIR2Bmt-yqt{@sumAu_I8pu}3DyTgQLENvl5Qo}A=0?i166k|mxyg4fG_f<3+R zO_jZbfKAfRY}YQ&ZKgvignWr7o86^b1${ASp2Am>hq536gd^gdiG_Eg1FmV7qF=6E z1y*k6%B|p8uSCBQG@tmFsK4OA^YqIFko%kXu(z+zou9yhb5wm(8?- z@5JEwq55;VzcdJ_dTYi{$>8sbT@ZNXtUi?9+=NqUAwb=XNcUldqa}% zOf})q9r7%w!=B-iyE|LUwR>B|cTM(qahv{~+MrB;ss6z-PQ>fQ53&7Sbq-JvmFEJV zg)A8X4S1?&n7i`ur@gV|Q%@fO8bf#liMUsu9Y}j`wP|g-vHXL2ify=2q(ixu?MfbivPy%TkBeWiet0v0xdPy`p#~efeWYqJP{dxan3fNTnpr-kw-zrL&1OX6%JtO zAk-EnH`DOlg(*O&II$!0NZnzji-$Xoiqx-*rwxz59ZZ@YHgUH|x8@Q>6V;2+X-k*Se#<K87urD|ks8RmU6Y(A+G0?HVi1%n0bS(I968o{l4m!5e zB<2gg_L>#G6Pp%Q;Z&?|^71E`<~sp?7WQqrs&r=sYuVW+x>wPYosg8{CnW2|h+wT^ zB*`u_7JSYZ90+D9s)^Dsvn}abMr2^9;H>Gboxs{`18dzRDFJIYT~#%k?Qm|~MJ85p zp`Bip9FmAvV(CR*U|9l!yEJ#X>#+nJcvI@ADWX8?&j1;*5{(7*i%}m@Jl>8}ZY=4yX0* z>dc{TwWs2zO>h|7@KD$1Id0axiX9Y5)Mb>~(e%R%N_^9UPNLhi`iHXnMNiCrEJ=)k z_QoYLKG7GVo&62Ar-sN>8+|b9u}aEyB{s#>^H9V1rp|=RI*w?s5|d(4+>DPtMLIiG z@OoJrFvss>IO%O&3O8wk+htJ`F<-A0Jqa#a{?|A!10(}PeLtE6k%G^E@caw505|bs z6h)ZRch*&3%aG0U+Wj1XHI9T?fxG2gnCdIckmE0+k!OV9E?=m^Zd@JlBykL0;;{w4 z>XlVU{lzoK5w74#H=RgK#YH2NGC0%{Sgb*urNy0AHAL0B+(4 zNqCkW#>q9o%gY`+_56DlDmeNS|LI`1Oni>v-8+~fFrHu$@V{hf;`$6v~9rA!1l zL%XjqGnme>VPNgr8M)PAFjo=hwEV_GiAOjw^d-(DITT+I?sg>y=DUzFXl1a`S$Ujb zjWaQoi%W>cS)@NXc`m*VWC9A)@uc<@E(jz?3diF=z2r=sSnKjF>)rpqJRhe_Js%_g z09}|_<%Sz|;;Wv;c||K+Kn6mjxI@(sivw}Z0@*&{sTGW(5cwS@Da$<{|HPUy< zgd=qbW~*vaoH}rb&eo)?R%@1ccLmQ+6rF^iz^~A=ioSC$3f#CVcn{Fc*)GU{!+gj+Z91vbGA7GM&K zm(F%k+9JY;1vCGg2tzF=_^UJCf#}0?tRfd}l{~n(`4QWo&>i@;TJCpq*2NeY5r(XH z%mjiTBP`NJyu?2k=B!f%N#H!}N#!&whhgdmeoJHVw-QngIr(L-x?IM=i!yA7KSRnP z{hC;|2Fw+FocPC=IlM{4Hr~D5?%KU7#cM!Eji3BcxRXrP_p=brglZRC!mpne*O-O@ z{sw2hYF%yUOn&^=2K^+@3>Bl%F&<0a#3z2v{qn`oS(P{v7vbI)~gE1n8Jmu$f~ zLcWvuj^F0m{e9yO+N>n){injlW%T>%xodaQQ-lS>H|Qxcz^ZPn2w$VAUb%MPck$or z38`;_Uom?UGP+=<9+Lq1sVP zAn4kCw`+I)MJ+10t9`GIH7%AKk{!ez&L^l_;M)>ZLI3$r75!1Fdf`{J20?kPNTeC} zxptRde9G5cb;VP@lugs0qLRbgtd%Df(<+vPGn=(X?cv9p?YVf^`C7O1 zbCb|A_GTqeWhJh8Op+_C)@PbcOwdn6e|~4tQ*C( zu3Zm39*?nA+vm z#1kt|P{&aN6(!D9z@lom8TkPE1Fz)J>)FK@HC;x0>9L~k9`+7%Klu4CCs^@`@t{CJ zKLbzaG$`RjpgxHM(>MeBjzeIM z!-t)%p7$boUL;!9wCWiI7Rnc=UV9#C3UnI23$jflif%mA0J$*Yn2#1_4s}Yji0er0 zGUaf}nV+CwMy?AZG#C=|!T~vC4~Hwqa>gTbqW6(WV_Nk?+s1e9+&Qo6s!geP@ndI| zM|}kY#{J;e>qe3{RcOaKHB1KE8lJ`P3q-$<*~WJgZtH|Nrd-e}$Ktc7N$A%87x*Pdu|q^&Q|5B{@${S{b$8$Hda0RRk0}LS zdAPwK#rsxd4*6~CMyFM$=dbXcslrIx8UGT0cvdR;Umwip*DCnMiGs_xQ*|bOyLuT9 z7aH+RCZ1;+&vBc_#SbA_3c7KL#!$(54FgxsC7I$3sl-->u*wi#B|^@fseX*MMxP8M z*Nysv56S!Qi_FRCt{XLGdw|~ah*)tvA}ds-8_SM!7^_@5!MNLdiV0?@hiV2yCUe4y zrDvs+%Bz=Fjs<3E9oHY19Iy2)0jI@hz3CU_I> zg}8L`OHRJ4jic|>P-o9PHPjut;Mp>pA%?Rh+d<}5hs|9fmvVkIfoh0C0)>hhPaMg`4MI#5O?qNr;ih)ZcY>Xp-qj9CPH3$|Hu(E( z&9g>aFSl83cu#CS*8h>Pz~PY1y}xKTkDP;NRN2LuNz}0FA+gtl_6 zR#ZS@GPQxbZRe;j2O;K7juB)_@d$3d=cbXmz*RJdlF6u+TTrYW^}b(1Rs7c6+BvbL z^g9zeaJB!ikI4DYgN$_ZLPjH+^`pG|C@=6GhPod{2E^HBZglZ9-@$4xe0?O-*O-x) z>pNI=mgvyVB!J@){=u~?JCb>KV}?LT^|BCUDhFJr7g43V)J9xHpoz@soH~MD65CTi zB>lm}Wz!;-D<`nnrw+~eFROvk=Y(Ch3=#CQ4!Ug5MdSRyHhXv)v-7|ebPH9QK+*CWTc?WfDe?_O6R!Ff*`@#sXON7datXpx z(dtjPjF+7Uj!>uP*YX@bgi|@*C}5XuKm-EUQ5*w@`BWjMU3NUo&Dk#p1?!#8uC@Exu`+vb7Ss;#4*JvvmCP+dfo) zS?LtO1ky;Dtv%3aC$|u%*au!j3;BkCMOG%J>SZ_ik~Zbgf&}IDJc|^LP@-c7XpI&G zv}xMI&)V6;h@t=RvxIILQ)&@uhM=2-)?lmxpa!BCkF)Prx*rpXivpg=bVwzU0NCW{ z@1T+ate_n|%eB73tl-U{M2T&n6wwul>n7mp6j*lBQgGK}&_UI%0%|aA;+mJb>cy+s zoSDhmgaHC*nKo2Io~4{QuQlf5N(%bNroQIyDX2=(R^VZi-mrNVmr4W&>5t@5Azfi@ zEwUOhWYbnU$!0dJSHOw$o4Z_&=>>9GcoUZK$e4(e7MN)}Nt?l6J|?nSv-@seC&97G z7Ah+U7Q}*t)Q4{mLRi~LvamczOQH8}0=ZuJFD&TCR;$X78IgUBhRU2kyv#iT@Xw$x zelTVMM`&dqkxBa**hhF+caq57$#1gakzYwM#W!8HURZN1<=G&{OcWukVyq%9S-Cj& zR>hHLv}hgNLh7p?Iph&f2E5G)I>&k|mg9LDp;MGhX>f&8#XTd3%@`TE!eyD_7NYcd zL`+|?3}L14ni5Pwoj=Mvg6kn;5}7)Z#y~RivZ7_q5|u87rB}PwDqAqyj+1HeK`y=k zxzIY;S2XAMxOJ0((S(VpQ)|MLtKIuOSrc}CY-3IAQELLa-5%?Ai1Fr7wGEH7abzb$ zu4gv^`#7YP3j-8`{;XV#ISvtpiQt^$gxsjiVv&;c7t*^E;W~Iy-~Eh>@4;p~v0Q2#zuK`iThmWFIQ1#3${#|#AmGwF2g=WLw< z7`pB@y6#d#$7p}*FsgRZ68#)($qn4dgX`4(M$_}KFqM){^Bc3*to#BP^pAn345>(O z#SYaNQYHm6%z|$78)Mci8mQ7Q-AWWDs{KT^Ln4>sg#PryfP5;7@mzUcJG?RPsD`(l zLmTp4zz+Rf=;FU)-?-{h`^Gyiw{N`ha{I>FllF}WHe&{ zk;j3?i3sSDU8Sy?OtM%=G*1}9)r~e+tBB_1b)$XNxlvKiZ*=VWA-IC>J(6pUwAY=f zs~gF6g~WzR)jmQz;m}9d+zu96VjrcXcZ6pudmJj%jSf_QHM+s38rf+!QeZ8Obk&`! ztGkd}ogc|figcP6OY25ItWK^Q`Ek`3T)U2_lyiG&aN8ManU|y==6#c|d5rhic*1)g zQv%2dDI}Wpy6SXlgi{p%)v~O)S^{M*>#P2JWR*2?xbAFS-Dqz0Y3X_2H5}q%edcS@ zW7J`nva~E4s6G%WrmrW=?&U>RB~5gE@iOV^=#Q)56KDeiza!Gd+5elncY%+pJQM%t zMlz6rGr#~*QKC{sgBlI3zyuAKgg^~W5|Zd5#BC{?rQMb$GXz@$fk_~f(?Q&-wKuD~ z+HJe-ikBj~mL|j|fL1~MVMMEFZ9N#ajY1({O8(#HJ?G3NfZA^N|J%io=aHsn?L4hSY3CAL1)7fOW&SYp8IOI5@nBk3&63>pnaYi9@42%i>N8K$nqJHg zA?WHuJESC*)A-8M?0x1BhF5ep#5kEuNHw80K7DlRG@%(9{6#13*P zBxGEuC?jaM8j?=iyoF?HW9Schp_B?e`aandJX=yIrNpjqHcH8_<8J&_j(=d3nnCB& z9LQ%zGu%3Ei+s3VG9Aoc01&i?UzNyFUN@n?2J#QYCXtF}Z>V4rCOdD^w@TjvW-q9q zGrp$l9r~MC3}A>8UR)5^rSY6+QN)X~w z78_a;zVLFbA{}x>T!Y&RZL@No&NLQxPf{ULy#d_K2c>(s`S9w~9>Lgu8XZ19ThTT; zXCBP8Ddhg*dpx}X>ygR5cr%pFQ;O0Nc@);t4WD5N>8maNVcJFtgWt1O_whoTT#|*c zS&>WJY)rBcK8*7&r}u8W6-SPOW+1WY@&-9DD@K>$t8@GGQ+k&*_z&9lU?g!xWCFV(_-`#jL)`;b@wt)cf(lOR`S1tz(!snYO54^&u z9!CrXY9G;bT~6d0thlW+2Ljq5qk&K27x2vUCA^g-Mn$^PRVlc3|VB-a{wd(YK3`#w~74V z-{98LxkM1I;@}XU1aOgs;e0A-oy_lx-{5d<|M#E#6)nhc?yg(^j4wYTZ1UE{}K+=q<-!2?u%d@d*qvH62DeCBfYU5DzrDUyR?ie=oZ4^(R*nd!X@r z?TJ8T-YcX1D`0X#GX#HM0+DZ#%hCSA$oOC%Vc9BuOHBQP{i!;Bn1NH6n0GslNQ78Nr6t`|kT-wS`b&+MJZ|FWNBSQK@S~5+=9k^bk2hGDRg4cG$l(nh z8{0-smYbrZt)=~FUjinag*5rK@%9>1<3`oyuuf{)ZUTLLtH~2sYVIekgv3t>?pYl$ zL2j0DpZ%$|kY09W6s|vUJzZ$Pc-dFf>KVhazer8I)xjx$OwSPKSYKWqEnkY_t@ppd z^gS`je7$R*dVOWQHT0UizTSLY`>c9BA>Mk>VZKHo=_~S1@XM*f>rBfi;#y$ePc#E! z8HDnns?F#YuAPL*zI>M)WnJVl2heQ!I}K*<r@lq|t^V$|J_$4JQ!LgEsDzgV=1?c=l>J@@9eYlzjfwX zWo5zn2@7~;2Dm~v;IEh*aB@Kt`_5)0wdh-{iWDwbi>mzwPIx$+u(mG z7HuP6e3b{zn|iI4oKkQ7KegTDYF#?k;%#f%3f&1lyu9h;`@)BBsViwZd1N(rcQjz; z0!Pkyf+RQ3@+dhKi;Wef3M*LQb1u*P<|{>7d=ti>4C-+LM4* z{9#cj4#tFgw+m%3v-X%$hPN`Zc2R?3-pw}sseVmL4fS7CKpeDyxHJ`rp{5f8#FJa< zh|?%QJb6b$fvOE4CTq*uqiw4k{D{g0fNJ}+&Z$}JOZY!TcYDq*KbVpxKR}uwOnj!2 zA1G-;j-&n`c1Y=v`J;j-)1(RUs=??p_;D7!4yZ(_KNM&_IOA& zw1kHGUZtTaA_-brm*7bA7;i&m5_z@Cs61))MI07yewX1*k%b(Uj%A_NnVTx^ zmuxG9T|$yr(rE z=O^Z)ECxklnDg;8sYP-^=9m*Ybwb`hbV7b4d@5}C+ykIZ_*|cW&&Rbx0Q6;AkS0CZ zIut2*wC=uR!%MeM4$RYm`0Rh4fk(9_(FeN2seB31Co-Qeo07#X- zzm23sp$R1^8TjiZCBnF;N=mZ-B1s8SPLY)C#oJnvHyJ?GRv$7@2c>sUYOavxR^Jq&nknR_>m=cXy zdZ*y8V*p?^tC=rECnwD!oorup=5KrP~#pNlMb#<`yZ*bm43(k@dK>ZRVk9 zp#X*K>cl>VQ|yxn@j3=#aAuhZOWIwyq`-m8S%f8$P31b2lIu5T$n|mZ5|NeUpxsVr zym|KXjN~a~yrX>KoK;>j&jFd{g7Z$`_f+}FD+pWuX7UlFB|svTbbgePy&i#^q7Mf!dw zHob}!eWrK%KFnT!v3?tM|4=f8$>x`uNxWwnK0 z8R<8+eqTC`;h>!6JYM&EEDlI;9Ue2v%>;MY3B;(81em_y9`=09%Nnv*iu)Slk_R}x zSL$u`WC00WZPh)HknknfcUt|oChO0$i%+Zn`egl@ogl6L3#9&Y%=(wzuj*eB^W1DT z;QsUmsQIyVGF+j)S;i$k@0>#8-$cOF9+|;hXUIOWPcu@*JhO@-I4ec|U{aM@i){56l~fzga{d4h zT(~{6>{V6l_7k}$oT@oBL#lfYHciye6>aBKHGOzXWI=qkE4Z_MPSb79NCckHp?`YI z{~7H+HZRZF6rQRU4la21)Mwl0<1j@#HZPx#85pDOsq0(T&BPOhax=aLtq0Y+C7tDE z`pB0lOfSo+u3g2A7{mM3q3ruQmMuZ*$LiTJus~j%AaG*uBQw5-V|kZvQIxxv z!-w?%L`^o1hgbHykC0S<-nha_8Nr;=;!0D`psDg1B2$f|z>E&iZ+u1(p5Yvw@HoTQ zm4va9508!Zdn5B$XgBjwl?IMASIwPU=wd~6?G4QOl}^voLzwU(CW`dm(s6n9jWW%oeZ<%>zNtt*t+_>}o@3UX{8?8ME~33! z%U9(cQ5Lt$gmU0ptyLj~*26sNThu~tyNfRp$)t$13gv_b_nF)2(oUN7rB%`_dV5|< z!-joXfZRVq+oX{TAM6hroPKcBF18c@fxa>bsu#R3u;1@bf1hK&-;w^_W554l`g=pc zh{kMAfB&5Qes%i$t@it6>F>X1zb{UI{|)>7mFe%-*zd=uzpu65pHduv*+~Ps`haT$5W0wKCRe#Yi)#bdKlAM;Tr$3hjJ^7O;#DhvF>ngZwZ!*u>i3@0zb{Vx{&MPfX+0Zzd~MG5NwmV)t@tWWO>E;k$z+P# z$Lcd=rg_Lr6WerAy4D&lrPJZ*G~Dg zJe_2AsCNEHo|M8^+Ihb`y}`Hm`WF?pTBL1~M{V1dra#ECxO+K&WUd8T)=V1XFDo~_ zEKC3ImAwBVRpURD0*skJ<-cKPRm{u|CSV8ZmdFU!+LD}-nH^jN&wVSF}oO#r_*oadeAa-Vme zXIMt2cld~`^Zw@ld?QC`fAd0z7U&;6W^7>G_zBtPoU0Gcr*AiLt>Ai#>v66@F4bRV zha)5HpVKA((q5%JsL(^|6wKyS=A<=t&Th|4TwwsAUz60wMRh+WyXXyLFgR9!%nTOQ=CZbSYm1k zEqcv5JjY^+t_wMlu0$@u$f8Q*drjA^YtSr13zN=NUiZM54q2BsF2{7)G_W(zUm!iZ zAlm3#*KmMu*gXvWy6$b8UNqIfj29b&Ug+yEvX1Y?oF=-GKlq&XH&v?CTIhNhX zAUVINmrcex93TCC5Zn2<^dODGMubvCJUm#b%<$XS4s+r~AUl#VU%Uwr#Pqvsi|1d) zp~xby?A)d-gR?LdgG7Xs6B_F-)(0aOt8INokvB}dgn_cEP{n-696QO{-*6UTvnbDJ z%CQ(?HH#?2;>G2U>aQ4U`}NQIvPyhJo6mv7h33x@*){kXv?;dr*=0)qPsK9s%1SId z_y)(8j#TUWv9lVBT#g)VnFZ?_CgA66{=&!+3c-ZhI_4(Aw2zI? z{xSnM9?V#OjhoQiY0dlJ;Ga*49hIj;mhEU-xec5kv*z&pLX$^Y&0^uhw_9;b&qGSk(e884?~;-1#iR>NuQUB7V( z%bjy+toK`$JimImKF1>mu{cd7eY#e^v4Ax<74MfK$Pe6!RS9O(i=0x0vX>pLk49kE zI=;=>TI|FQsMik|(2Wzq%-7}_&9MXQwEhjecp5Wi&%Y%8(QllbXwT=)*q#|?d#+4v z&xL928H>f0*`V2;k7&_DXKWGUBl4FIszt_p0$XB{EQ^@OK0hPKWH~i(j6fJu0nrkL z!DJAwmtHdt3NF{+U(tz-GVWQ=r^(8xmq&MC-CKxjXxA(eY_8>8VaEw?t2VzibAIon zw(+eK+KQd6^PSOdmw*z#fi@U3#cQdy;p@VIr`sH-EawEi<9rNf9U%VIxfny@OVC)` zUo`#$<-0TSeOStO*)R3EQ$S}vbJh!g9A0k9JFvM8->g?G2DuTk+|9W=b!bJvFiys+ zZB*NYlKI6m_eAC*v3b9Dinc@3=W@EwTUH_P@?wJ3Fh#k_83ovsx18w#XJnkLmVar> z%-=mTqXs8(;^9^8G|;HB)7X7f_}2G{{n^SXL|=7qDW~0T6l#}qLycZ-huAJRzfQ?p z@aLm7zsQ~V^udLmGgyRM9|=&It+NysdO{V|!pMtFD}AiT5lGQd+`ed=Myc}piL!jc zQg5rO+lyB zH{_+&KIjTnXyg$kB#`=WBbVaDNWA#eQ)-*LwP+Acfeh{_8Qh)t+39J^!go>X0Hd28 z+-DBA(hp@@gPyH`G;}6djS%AvY%u3fZS%Rr7t({184Dr*MCB*si5zR(9j8KqZGZ7Uj_?in2}kdb8u33-;1DvXKukP&zH z9g-NPig@CF7Wc)R^*h~j6O3JjxZL`>65S||#>1EbGP&?q#XOm;wG8rNi039nUwo{; zrc2C1S>}R&)tW^tXNEaEUVoKfBpbfqk`hlfSVtBmD8}?xMwlYz?D4vTh`jNqog{Ju zUvQ*d?YR(PzVH+ZrZ@!WQv@%ap)7YF$K)b!N09w z6NwJ9*YsJ3{d`BmYP@PsoKb1{WA?{Fo>&ML)}+_fYUIrA{@lIsTShoz#40#aKTqaS zhUn{<-H5zztVKH@hwwb+&Mz|`BFEh5)S>L5us%}vOk(k&mtv-0&e8ka2okaZ69lnU z(hNa${@5xX2WGkp0|J`ZDkuSEyEVi%So)~ui@@!FZ+rj-_Wz#$e3@@D{*`#-ohq|h zP7L|t9wN)48<{A?6Egj!*>c!&m$pIn7Gfc48$=%q<@PxtXC{Fe36S$$84M5t?opcR z@r2vn=JWSnEwoH%In)hgprQf6P7RZgO&1PgWHky(rC6vGels9hip@82uJlOVV~3P6L?QA~%-o_t$59}pjA zEDKNyAot>1qs%z}mqJI$seV~jte?VD*tK!Z_a8khF9R=&U0)te2C9IrfC9&?5DdbK(N-M_g!JD$uf!@85>q5%m2)AoHGQ}BS779V-CGkY>7$``s`A@PEiY98! zN|36UhH|)uYLjV@E&ekyMR7N9iwrM5z7)CTdM0V3(ht`?}FFAL9N#v+< z$pZ}ru3Z(y>pz5lI1FvP$7cJWK5`(n1hTz^234>XcZSp3%5IW#+!24|PkQCi zGX2@ubYqQDiE(1;n)cIn1U25-YB~Dc>PV%GIy&IS$??E+TsJV%3^^mcdhtp}l|(os z6fsVeiY6QFY&XJjG*0I9nOEERuF#oE_@Y-W50&W)bHpsKN+=0%fwo!V5+i~U5{@BV zdiJF2yjt@)B<^*qPjgtqR3jT0R3g|e!Y0PUO2g;ylQaop@n>4 zY?yx-2o~R@FIuiIm<)PJwq&McU380CD4Fpe!QvZy@sQ{V)wKFoRXEO7v}RFNF@9sr z-ij@bAbX)U8L8ssWvsv)wa`?{m^IS4Z%dM*J~darVHXmVV7C=lR zAuIBW4@{vVfD1qYa{x0l$t7r_Eocc){1Bwp;H}Ma_Y4XNI*}96Eg z9?oSx_+z-=%#leV*j!(9Q<;810km?9jXSpdetp@Bjrw!nmIxLO7334TqUah z9Mt3ZWbjppC&Vh`r`%KGZivRsD3OtSf zooe#GGF{X7-*)vPh5vn+7dj^b-|U^B_`@IY+%~oibTf0W)^aCL!jLx76Z(_N^ zOrABe$l@1-Kl!rECzF|nx;6O*nBSoDG4a;&PM4BA;)a%>3QeH0dXEJ1biFofk@N~vv7GJD~AgOwJ`+o4!qVT55Y ztO_fth2`CAz9<)-ceV#P2g}T4c~cT98qnsud$s^I;cw*-a4LWMrPKM_o6o}E&Q9fT zr={|@ms%ZpCMmCq-PKJf@_*grOA|XEt*y%LqGp z%X>^AsK7R*!#Eq%Ty5N!WNt5JkS*pGF$$PP&btj^Zf$<$WBFUtK0>VXSifb$843kw z^0!Co>)Fi`{H=6AG%8}RYHcNJ7l3z6>+^=|kbT&0JRN%AR}WsnVYC;+<19n;9iGw9 zP3~Q@A%+N`jrpVJSND^|dgByP?Ds%{76EJWTVnDgQca}&m>q~p0=12ea|!w3U5~jr zB;d8U0664z@0S0n^hcV9aZv48c#LH*zs+|I8%}Hl7JQgYH1W}Xr1hrjHToZIe>2*j z-%zD(Yw~8fP#?FvJi_JQeoNg*f9Lka>h}iqJ5T)Ny?_~9xqkbo< zU!VN$qQtFS4|3hd^%Jh0T-&(r<=V*g&s^W&dYbEbE{8AmpELEr<@OBY8GS6j!xLXd zWC==U3RXPko#C%9BsPce(=bw3H{ZAS(^xBJ&W?E{-sLhrUuerON<7-OvcUz--%&|M z5ZOrg21`73zfbrF0kjiY$nii%@Xj1=8cLw4L~b-e${&yD=vr%OAzMrl9gUAnN-V7^ z38H6H;0$cX7E42Mm>$rT=%?8$-N%&1D|)>Y@3@-@q13q9NC+C7m4I%hcQyx>NG zleTG(G=DfhPli?V6Y^ix{vV&I{Zpj)iBt7+a_bdQb#b8`kB6I zJ4Te%eu<74H`LoUrsoYRkmzQv39$x`wnM~UN}i-X$vM7@!k=$CacOuN$J>srml@&= z7u$eUpsGKQa8SoK$R}4=OZ!y!J9Ad(-AoYUQE)HKT41-D`%3#hEgQbdH8H3jTH2xH zWK+#EhpIl)$~>5PEW;JaBougwSyP2wxkYd1y{d#j?Ix zytiQ-sw~QOh>=-gJdlYO!A4vYn|G|_dS&oD`91)qki*Wgm%AcC!Aw!qf{N+ zEX}CVchb5Vr6bW>)%#X?FE3cyu!=?zn-`UtN^;DjH_G>*Dr5EVF2_}a>V^5bx*}wJ zigq|vAI*2req9d7u?!~;HaAWoFNIC5u27V*kMcIFce|mGRhjxd3Ota{ia;lY7-b89 zf$wSd--}hB#r``TH(BYUhf9+32_=|Ny{;yV*geu&97!HnJ=($_O~I?eBR)%znPc2V zM&KR4vm{ouy;t1pL+WprXz`-XLt$=tVZ)&K$w|D4H@r*MtC&Uo)5M!gB^hw^YxD5g)esxQ4`E(mpFnQz}Zbr>^=dp5|H%Gu&p^1e67e- z@W@@pf5BR-6Hrh9v5nc}FcC^jVPSDR^gH2WhuhfK!Y@tJ3$&Im@C9#x`Ge(p7bh=b zczJ$a8Pc)q%+;E|!{=!KJi6ac+J9jrAZ`pPYX)Di zk>IVPcS5xP`p7t|-W>Ho)*fO28ckixq^_m?v9%t(aR3$%e|I1Q7FEkcscL~;+wZVI z6OP9d9bAB=@?eG5(kaC>dNC4xhdU+N>N0+jOgAgCI$qQ#{gnXTW`$v_ILP`OO`uk* z%I;#1G|BjTt7c^&SOk-O6u8m<+msP3Gm!q1uo^>&=Z&KzHi6Q7vm!*TiG&oACgSww zwBlXl=zWT&V;>{UZn=HfLjgh!!sE$IUPpMmmG-)1T9HXa2d@jS00AH8h)$?>O+D6uN)AEzzlcXMVBe#f+4VTC=P{@MA zN#TF2y_Krf5*}Etw!6f-1f0grs&$2Sl2=udea<*n zJ@m-KFB80)pfdGO;lGHtcA5fcmEl9U8Q&<_pXDA?SlC4E-D=Zl;zJrzwDRlr_7MmT zj=R^~Qo7YUGNk5^{Y-3yy)m_`B^Bw(_SoMAg@4p{!B8LdT}(~S{2gbBUh9i$k5}I! zs7P;81j!y^mid}yjELZecQ!tOkLK=779;7=b+oDU|19a@y!X^+NqT1?(G=^5gKhOi z{z&>VZ(AwRCp3q{>q`kOtWxn^g!#Jji|te0vpsC6$HEuEsNkz?SMFY%J53xBI|sGw zjmRfUy){dsr!wk?>-)PsawNggR_SjIO&N41^@|i!jqCMj6JHerP4ofTJZ9>;ysCBh zG3-a=z-u|UbFp6KAua*_j2D2ji@oamN)G0Dc!vANg?{H&r@58e?N!WK9ztKtvGMv{ z7`=U;k8?5md=2adX`Lqr(3$N!ul;zB1r!Bu8GoRnK%(vN_@f6C7H?# zxX2zz^Rt3Ch0pfr!@szDDn@X)PrXni8xf!4hi2qOvSx%lk&NgX@PaSeJ{5l9J#|VZ z+Mb;@wG;KJ`I_F|!qoPpf8XZue@Z}Yrhqeb@2=*&Iz}0aUqT)QE0k)BaTrj})3(h- zJaH71pB7UcZQIwJyvT{3aIHpMUhLw9ix*n-gnKpT4U3ViEOh5~;B2!7E5&7< za@Z$z?nwuLCxKEu)%x@=4o-ES5;ZekyTcjLg8Lki@l9)|I3n=HUj142iI7Q@adK@c zs_P!}>`+a-I2A0EC-LM;@#|S3@gj;2#){sNFnq`nMOuoNG9L$t59aQzmT(3R+=pTJ zF77|DHSG0lk&&N0QC}cRJ-s{!+a{%f5FIpPM`ndjUlAz38nhmkhvnGa1|Bux5U?5VA_zBlUO9yVGzJ`BIpif#F54niv zw)hvX!By{$~brZrP=8XY8LQ847;kf1sLOi;&5 z&W&j`>C;dz!DMJ32mhU+>>Uo~)rzcD+3}EnMeG{lar*U8cE^d#6|q%gVkLoP#KM?H zT#&%}lRq!zxR7ssM-x#7`J#7p@@bi%Yszn=%U&zqrx_1qgoEp!M#@iFe%@(j(f@zHwZ1QFd%mKzqVb zgVlzj_`hm>V@2-}I>U_Ivdp;IdY>Fz?$mmommDvaR8^U-=^yA-d7<)n(d(?weT=qaV_n)jRrTF z&3o|*v#UJi_UT>uzg}ki(t1UE^uj!BV6<(c^+K=i#iI{#aZ(+Wz&{2@HB6BSBVxyB z!3G3V8_;L07GP)C{x3T85d=a>vU!O~~$tAD*hMz<$@6OWf=M#m7yt-X(x%&zGrb^Q9tI1itp^uj=ADJXZ9wK06yD zOuh7Fy&{i8s=&8L&oXBTE>p~=svz`hLtB_?`v^zj9D5 zM-TyS8nN?1017^!ANl&lyY4PI}?@ zTF2u74NKb+(MRLcy$9k92jLHWPf(Q^Zi3AjTd^g1`e)T_m}}1l&xn*JWr~^e@wyKH zh_dPGQ6pM)xeXV4Oj!>VVqMCShdd6YoCr>5afQ6d)uRP%EUh-o_*YSnZ6zRt!0UPLq>XjTa9z(GGwF+)eF%P&Uej(GHaQk@VaNTDqe`abCdeVGthg* z%IoD!o8H5}dOE#_fAw^F5C7^my;lRlN1y{I{ZIsnhysHjnn5^Iw_-KK3g7wEE{w)M z5s#yvz7daOio({yADHntlz|`QOCZ>Y%}6IhB*+n)yim2oGa<;XvRbp}}(y z>6IR>GCrp^UD~F#TuB;fQA-_ArnAJNAmFHJR>^xa0EXGZWWl)VvRX#~jQ-RB82$J! zJ(}7=31NZ5k46cAA*7fRDHBzsp8y#4vjnYJX@MbG$z4)OCAN+Ok?~>zU<1ldv39lO zLlNZ{r>TibG5-LC5Vr$a0bOmd#S-B$ZcIT5W%2wHNO}kXn0wK1N+=6u1S_#ilRh_a z0uC8-jv`|w1E|dgx0KM^wxmX_D6;@7gau~oX4mYsFMpXI7~_a3K;=XgzapC&0-||C z9qAL+RBUm!sj(1!A?ME0B@Kb2X4*#=g$=*@F2i5P(>h~G-yQ@aNwyXQ?@{8Yr zEk7mSqg=0B*LMEy;M&ji4A&m6yMJY)=fQKTt$Xirt7S`U-c_b+xX$}c2D0tYRk{pZ&W+kQUdiZ3uI8{@rcic0L7 z7aZjDnIkf6#{iv?t)Qy5kGszjM`x8&ORT?m3!!(T?Z>12_1Y#89HOSoxlE}k z9sH4yWN+xzL`gs!607#bGSz?=D|;!jPtXT3rd$$n&*{x9SQ>FJ44*4mddadqgC3^k z;$`u6&Lf}Ut#ELsr=>aawtGGz1@02ntAe~|%pwY^w1SGsE#=VI^AvyHbX@_*>gMAa z*S&5ynw$C1N_Fo^gRSb4^)y{~OXPF$VhM$`q-ibwD=N|KROsiM4lK@fHZ2Ly)%&{V z2Pj`ExKQ+mz|*KzeBPST`i%kQ6t*~aV<6bzB_0RW3>(@+Ab8>JIR$IiP1JVmEsj+O z&X}rTowo5I%1!#bP~mSZzZXK@K}TqtaV;}Qe)s##mPA!I29lk+;!>A?3s)!CAGvDtI_-YgX>W$S$gtO<{3V>nZjL!Zc84=n zR4!P{)bCHqygIZWJZ-*>?#wn{b(*i3->Qf)7Qe1a8m>wb;gPIz-v%|-=2o_{M5Ixx zKO@p8ag`(Mz1SNp_SGrN;6;I`(G(A~yI*v_7=t}5_UkbbUb*TDDl&{E zW@^kN|6Fes#ui76tu#pybBF^#w$u(SRRNdot&aQJ;{|)3979n@9vYnMH5qie#Ma)B zhwp`+%UOkm1)YP1)4K}`Nao;nnxLoYu@ZTmC->8=M}6@BkiMJiKe@JY{gg`}lm7_<|aUWf65+-NZ`GYFe{(1dW|thZgndMd+Qzm*w+##0-l- z!5%mm4CwVm2m*Y16=was59s}%W21=l%Jm}k89nGZ*gd?pzHcJdZoj>@iRwefTQddR zoT5l2QbM)alBmN>RAxMFC$gYGuU}=&%-~c?WOfk!VUKZ~ z?G+K>b(xV-MlL;s9vjJ1_ydbB?i7B`Okag~f0s&Ldsylb+)H2oWU6?e0hEDn?LqlA z!u1QS0XLZkCMOyKX)XA-EIrJq84VC!*ubm)3XBEE2*$~KeSm`Gw)H(9C^ z9YU>m#YlIo^Ob(gt_WD;{?2 zBFa*dNJWKI)O%sAINPyHxiXPtnyO`9tSX=v!w=wR4=dnbnyX2c5uM7~?zn5Df9ato z!97b4y#($`VCm!pknv~9+|5^6VZ*|stJS`1RFI3AFIb-nC|!Tns4>geU!e36kiW~8 z9=e5OPYF$6)N#jqH#sOLa;e4NfdPreL*|Q;nZupNJSvpx-c*PUK;VVTJYCFJw$Au*Cdkj=m(O<&-T&&e|` zl{b41ZQ;+5@KDb^r4Z5cMwmu}mX|SZSU1RR1yKDDMxlx^7^Ozp7&IHvY55NjzWNGM zwVEh>qR^jJ#&=ZX<=be92co`33{`+|NXellM?J?Xb2O#&i8)2U8=R6u0gY}6N#0g;tU;GPQ6=iDk{sGEFxi631J=O|*FHYW5`zZ^Uk&CN9Wdj{oA zKq+s&`HmWup~{;%N^|oR_306OZw7SE)p&`?d7WM^N8OWRt}En}`4W*_W$9xQax2fm zUSx!U%T)HbPs)jv26{*3w?~Tto*QG0VxP?UF2AkepqXP3m#>J8i0*V1iH{0ah3;x=w2 zkaW5696Ff!WF{6glccVFXv}nlKO@l4;mQ?y_|d!-&hYG0(XSOcBc+Je>MnXpEOA#j z7taQ`zUbGoosmyJB|2t#b&6>5G}J|#LbcOayowMkGkWr54*7{mzqE6OzL-K|qg7IX zpzse9#Kc=K8Q;JyO}ru-BOYTtw>nWq`=X~@rpJ7JL|X+XM+5qNiNXZg7wz^PJ8QXPv4%Oae4^p*}&*23INr7-lXGQHQKK1_A8Xu&B;>jSJCYBIa zPmVv~Y6fAJPk)I65CYhk+h}6Ci})_g%#zi&sfqW>aLf~%3j)-d|4zQ#A+O!X+{ct{ z9-r9Nv}PfmlqDBu_dMl#1;NCPNunp>fG}K71!}5tW0bt0QqiJLWx>aXa{ zO4#%Ao^dboRg{OS^*4;?RDpi7jHVGJuKP{m@DF&xG>^5Bi@mjdncYSA z3Tw5pBwqCRN_f3rT|(Y2@Jg!xsJJhq0+XnCDy?M$AlHlTnk8HpTd8ne z-SZ3XB}z^2rL%-_i)3z>(tF2HryxM}6kS)q2NE_tfN6hZE1`iOFJFm`-(6^IE7j5q zXghj<%oKfd6IHA%F%4hLGVuAw7=Nz-0rM{(y za0{1UQzU)SL{8IyK4n#Ls26@*^m@5V8Rv!L^2?)L61ivPpXVRQW>*VH6Pdh z@4fZO+kdOo*H75~ud43;740`v#+3HMT0DB<&j3SsrOcI1MTPF4m{DP#kWt6fau8+uLe*9zKv_i4 z18ZlS=Sxa`(T?NM6O$XtwQYBnTraZey9@Y}zk3FUiSO!=Z9&eulo#U)zBv4Vs?hh$IbHKJ{u|ycR8uVqM zuQ`r{QJN)m*E@Q#mHB1#;W(Na0=>TVjehpabq&LLDF(7(mf%E@l`4le-L8vBxJ3LStrMzI z4%+SqkJKzX-eWRf3mh-ca`|_2jl2#yv2t2?+E2=Bhs$Zs@4w;rPoAyDKbt>)(fC)U zjz3m}|Id%V8eq=D$-HpN$TG!E#nhAMEwLnDxD7t{UHF{`^Jbo4UShVot=F`7f&OLKWw?hsswDuEcKyOI<7o z@s42peBm%htn{FWrel>@78-K8vJgase^$48XM+xIo+E+I$SZXv}#!qAf z@4y604>p`T7&-cFIb`lF8Rp2}?d}}APq|Eld9V?Kd1SXd#|*e>pO4h;kfYR*^Akuw z)_=D{&_hbkZR8P*^hcoem=@n9N13MPb9sE&QVkGoj>Fu(C%4Df5xD5lrcVcW$tNt`G^YIKHTEn*8%5pU_| zPtq+zKBmR$RkZ&L4VSQcUqp`9OkJhM9ywGIk4uiSs`=Jo=XJlc(^>op4x>*swTbf&Lc2}RBqu6U9&t{nN>C^jrw*W>wO3>{EISB~Ox5=_! z>#9cm`>NMP0Ss4EpF_DPs`Y*4*qeNn{8?s;a$nFMUKDc2%}!^>vH%ANhkp0E)B0^< z2Ma$eP8n=Oez;MApUiI;u^_>dwgQb<(c`T}C-4;2I5N1aad>yp<4h5d)b?LfkZ|+v zww|Jt5_%Ll$sRD;zkxY^$i zU1#s_P09W$-)xYaDkg<;+eMFm3i=lYyYcbvqOJ7Pfmm_(y1?yl$MvI*zXp2dy$JyQ zrL28)x(gvoWOzJsysPMVUT>JAoZj{5CkEH8p0?7lazt5$@zR`Oj<#K{-LBpG$wl#i z)Ai!z2bwzbS}(xb?}e7ZhMv02S!xG)L2rBb#W7t+x6|-o%-f0tqJv`i(mRKmOym^p&y7qpBK5slBETYgs-sWm5P@VUL>`sizx~eofo+X zuY*S-2cnIKg^GkTDeBjnv-w(cyl2_B6)KNcje&XU9X~a%U^tfaAOmb`Ro1tx*66$RliGtjUG8@N z>lphA_f9^wyNiydHMN#MZ$l5}%67sARd?Ija*M-YNqab&H2L@vYehrw}|Xrv`IRu}z~`I#vH3euR9eQYws*$UJ$Q*u_5e=RXgoDQQglHm!bh+x{TFIAgHQ z9g7@{RlXxmuO5&^+%Z5rI`%>!Uht{#xZnp`vq+e`G8{O4RfgWZUum1`oLi80yQg5r z+M@+mhWiSx2p?_=ehDrm;+e~Do4cSsg3llWK{yJE-2JdaApBuZ?z$C66(`~~&NeIP z(}#qzC~yQ{X&AQez4#naQfgAuXu@=3Qa-yR@;(%XR3}m8{!y@k>|ucVAj#CO{pkU- zzXi*~V+tbSQNa(wudV3LaC}OfKmSTEeJA(%c;n&diJUs5HwNbt>772jU^!B=WBM!Q zy%{S<#ETANF?tkTSDW&o$5#3cil2|`>$XgaK_J_AsDaAu`NQ;4LKvR}U(L4kqxP{r z*$+5ujfTP)ZQH!9l;HVYbTY zF>^{6mUdntk%=Ywn5I2Ft)t~`1d)$b0ZnhaoA$g;Xyn$}Zq-=Zdj#igT!d=u8KW3yYA-la_i#~Qabh^YA zhBZ+{BpV+T`dV3hcrNocTp-38`y|SwXA6sVf_aJwhxYmnt=`^YPYQ5Ej{1>7gYh8R zryo#pgHdit2;eU}t#u@zl#m>=bHo9LT2M)F3ZfKnicZ)Dr?yE-{lp;EHpy1{Nx-VLG)u}#FV39> zw1g-oLIEff>z-Vs3Y>upc7zMNiRK@2x875j+gj$);<=S3C? zbzS#*LOyK|&!NQ>V2{wDFb`)KB(WEI{&%T^gr=hY=LrG6tUhyp5dE&@|IC--|4cuD2K!~CS0cY!3P)LgY5vM5307XJ{NR_UxEyF-q@>ZN2l01v6K=`@cM)O`Y+!8V zrP;qTx+kBWAQ#~xB$|a3g(SQh+??DH^=5F%W1oOT{2Ba!=K@RNHN^dGC+bg74a z3d4jOOtu58Wb$y7&l}bo^Fi$q;w}cQ7Mo7=8626fy_U@)UzAjwGP=MCz9J~n8r)F7K*w=?D5skJ zctE0Rq@h{A`O$oDb!ZJ83R@kjkq%Y*-0yQ*@lkV`a$b^UIxB6Np0tA)xy z*NE6kH{^Uu{&XB1!9K-JRtF}mPDdRc^yB8Ovj zdA!w$Ohj}MP@sh$L4l6I10$8HCam8hMU9kJW979XHrtg#fT}Xm=wIyOEJD|6k%E6J z{Xc3&bj>LYDooP+b~puTHnVFjx-8|o_DjvOcFje z+G>^A|DpM1LPMTqxBBG|E?1n2T5AotT1#w#+G_9rQhHjM1s167UR?4+OcP(J+O!um z729P-hscH1+0MbWg{+8AKuY_>FTeO_{uOzJ&+d<8xR0|({)j<1BY`R7gQ%b{`upqM zS66Ef;~k>E<6W=&cz$2(nyAyAZjGYWpZHf-ETA;st1l}MR?rQ;L`(%kc#&cV&ru8^ z%;BFM!W<4L=1^i$yhTDB8OZ5X1FH^cB^0EPB!`4nFfC%l%=D=AHw#)E^UJ@07H5I? zX3Ii9Am`RQ^LMh)Ik!$&7e$Jz31fLAefR*}7}M@RzIAv#HIet@62W~6QIG7{E`%|1 z7xkB6_ykeR8u6CLCQ*0DIBpt+nf<&&{A?x00{9DkuWVL)@rn4Um(8*Q*@=&{`=tZM zf09?pOj?eutw#TA&x4O9>G4-&PK2k;s`3ATA2x4=`xM7=2?%d2PZqoG-%}hz2DvAO~c8QF2j=smD?gI<-#am#& zo>B~$a_mLZVc5jt7h_?I&X3aWv$Jj3mP9UfH)g|t0nFIa0Dag{z?i75`|8{t{fL+X zE3$p)W>tk>o>z=}Uz1MQdsS{mnmQzm#iBxqtuO3+CViJKe_W}yvK09v@kr$Sf?y}5q10c{n%6_$fC_X+Zuhql4!U=C^t_q?F7!gC*6FCJb?Q@Er&-;6Qbk72S7l95WsRn+ z+(?$HSZnSE^kg>`PDf3JQ=cfDXo=SHW4^Y{c8YxXK5nPac15lrdtT(y{MVGdpDHE1 zX8Ji1|MZ)kk*w)AyCNBqBK*5}+2r0!CFgQ8CvG!TX>L=~w1@|CYukF_WjVq9t6lnj zyd+*Ou)0jO_F~oA^MRF8F9lHckUn1U-T1^c;L!cS=OgEd3rNvyjLfG{_Iozc~s*w+(MGHa1(fLEX6=aX@}sTbaA*y}YKA!yCSg7&-3yWxQY^+f|HcfCJ&2 zrsuo992}Ay9+&! zgkQPx;vssTWItbXca41sk4^^?`YN84zUmnIs_wDqe;JC1KQ>BhmbRPfs=BKhSIyBv_d+~21!m^Tgs#%3b_vAQREMH7%tt@q7-?*O$B!tGr<&41Ceg`h14CJk)p^= z-w+i(_U9H6&sPaX)U)Uta!Y-pg@b@}$7g$NbZzo`aM)suoz3p)cjK5ecbEG`j_sMt zE4YWLwI_}fr>^?3kNRaW#^S`gVHCQVU{~EAv7_oh;E{_ArQ6NyNR1Y0(X;*(d=C)< z2`0|)CM)a`pQgo2PofFw6Tw57=^YQG!f^J{k{1?q9CFP*^6y|&WqjWJcDjN}`j^m` z5LR;aq>mwwhcHkIKA%8-;<;Hi!M|31(sh%aoBBi^S(<%z;!iAfPu1UO${T_`^&)1r ziN28>M@tfY0!n6e|NHXtOu`fQ3G88{xVk4kG4*xsi>cfFQ0hzDhK{Jr*>TY8M)6Yy zYq`L!9^?nt2nwF2eGkkM0!N6jQSphcSfiLB>ixa)+4zK==-BD3DV0lks%!V52-c56(+hX?NR(T;rt3z9pb3FvY%r- z6~Zp;Sv}#goar|Y!eig26tEqquq60{d_v{&uRxtSt`3=4`Y(`}H>8wb#%!|%rt<4H zX+4@8OYvoOK&iims!Y&(Y1>}acZy6cL#eigGTW%W;DnD_+Alht)P6De`6$3(i)r8n zF}X?a^hdz|^F!dj9_fJ%|G8&}zm|aiygNN915jrQ9}*%GE8g>vIZk_I*Wrj`-6XU$ zQq3Ox%G1|P4INgjO*DMf)d`t~W@ z%8Mj+;shpitTG$sGFB=fQ}82C*f!2&+^qaea=34^BaBQDL7;DQ9QD90cX}n#`b|!y za@DqJl0w^7?c|JynPM?1hRN4`ga%%~2ueilfAYHI)R{yIf)<{-L&ybqR>r*>{yh3T z-o$;uo%LfMQ8eFG5@O>5vJy}OGDG2FyrCV?ejo=X;`1DXSRg=-WRecV{Du-MBq$qm zjVsKU%8v@BOnGKLff#ZVENtSL(dG>8Flxtro@cmStbAAd3|UNzHxfnffZ0LNpnR%K3bK;75lL^%M=H%mZ*9`H% zpyr!@)_0%DajYE{-96ar{l|6#HXBR6B5!oNc(HUej*M=fCpTS1e&_8Te&JDl;6ZP! zRTA`O#9HOM-g*naY|)ya_$t#w6;4KQ`;b^B)W=6JUf?fp+97uF4uG{0Ue7u>t#6*r zeVOt2MZ7WogI|ttrs$In>8G3yd-Cv?@+xZgl7(QDcpAE*_s@gqk#`NYy%bvn?)Eml z?G#0p3at_w)ryCwjnd@UMrrV|PFHy1N`I}R5YG=gFI08KHcA690Cq=z`{(YBAsz}h zx%|;VSLEy68*kvz5y92f#>H}XkKEnF-L2|wId?g&^6pl7r|wp9w^iNAG4Z|XPP*&( zmZYoYuJBH|YvyiMSnh7;u3eSa#9b5bm}P|gv+Lh_m!y*VxBRo*N&Q>DDR+NK{dspw zj;HThc*6?m~oo%?Qm&^Vh6_qEn%1j;KX3bi_O9Ps-ga5W#;;>rFd!(pgC61~x9wI9(!Soh>1`+?}bNE)-N;o43jhThKjEa}(S6BY8x{v00jqOVRi} zsug$9it^aTpGum|?fiCce1>277u&dtJL4bTlD7F0K$M1A|7aimNj5p!zBSrEtlD^<29EU!(b{4;v}p{-O^SkXWut>$-}+3%K+qnM4(3p~sv?(>g3D z#g}*VP$-?{zuoGseZ9_N~`}_Ji(n-|LSDkkFS5}p>E1Zj@Qyd;&L$iyCl$jTTa!YVl%XH+3CR-=S7q<6qH z!095!iShU$ZtqMWjb_x!QgYq?yxzU6TVW!pv?gtFQ!V2IoAIiSi&IILdBU6dbMkbG#+Cz)m05UdKkwU8GKOpGuCFrX zzxb1|=4+_-8}|^?@G%E&iQ5Z9Mqtq6Fdnj>a>t3c;(+O}j7f`7@B(Jr%B<=#<9GMk zrDj!kyMFWIv^)7<%FE>^&#xkqgBiF5Tce_OmWL#sekx=dJQs?z{E%%?I&a3UG zCjwHZKT)S?`JP-Z&o#}OLg)!cWOUasxp5j_Y;-z|Dt?+~U8cVHx`xS(^U(XuT^WC| zE$Got7#%c#LGu``69AI6hkm*VL)&ns5&nwP(L3z_*dZ(ZHsV5(DDeKM00YSiv5YDt+pbzV(Y!qhZd}l zgxCap@@SD;t)-UsZcNmu6oMf0`+nCxGnquawfA>F|KI)p^h0KybM{{Qwf5R;uf6tK z>SFu^)WZ-0QGGKT4LGYaQykpQ1;vEaOUQ~Z_GTA7aw%ecG~*~o&&KhcnJ{c}DBR{q zSlIzJ>@M<^&5{X@L1c$aH3hDL2AN_YA`RW6^cUX<|#bVFDf8>OU=ZNY`Upzgy_ zqNrq1WHv5yWK2!eJ28{sh7gKT+}{RVpdMqh&~dpSKJ)>Wb_6m*5u;E9nnC30yp6*q z_+_Q8^@QMw2NfkM+=nSIRCQ_i=*Q3+7HL3ZgX(tlC~?muj9gClAgxtZ88%I_<#c$Y*s=-m$ zb_9RAEV%WRY}y>pginSY4mb^xPSXX^)90)0EE1;5nJfH%&0m%xyim(K%K&rE)0%17X(pjYg(#xm!m+L-W{*YApP%^R)!ZjS` zT)0q5sZT}(!-GRiN znQS}>8*NA5wI+-FMA8Po1FB&G-5pi4*by#@yqX5w=rch8>bLnD{FA1gpW-Y~Y@F1V zNmzyStc#WfP>myjdRK%m>*1}xI~c*TvO9!Jyu&B|em(A?PPzT-b34nAJ#gvYE{8qD zkKbtvC-QnxRlx%)U1`;-eyyv)+#L%4W?{@-t5=etfvEfle2}TSH@-Y^;*GP8cMaum zhZ1?m*mRQofToLayQXw;q4^A!8@3n7#-)%|;cWb@q%*er>=fyU?IYj~_9t1jg+AR@ zLoVhP95f=rU9Zc?S&}$!H>!f4)@fIzVk@N93&>-$WG?!f%kYaGacWrszNxJx*uB)c zz4x?MN7gNKHNfoIa$;al6J?rgZQ~2-!+*EEk2i)|8>@hIXO<7Dup!pQd3?ymL(~3s z4A5%%YU*7f7 z*jPDQef<|ujaXWFLBmy=i%lrH%I+9F9szY@>iabVzf<2Ao|;H*Z{}HubT+J}J|hr} z{1D~-TDrj{-Qd%$c_o36QdV|#VCR-p#l9>R!#)zNeI2lJ>yqU<`MqmU(< z^?}xJ=V)YfgyKKrZ}eQ<8O|Mt(Bnqdju-J`4GIS8$6qzs_jqv?xHw4WVG zI>58{SDCmD9ENQSiioK^*xCSCHe;?k{tb6@{IVH{DLxi|UC~kan;jjFzixPQ))GPG zU}Jn({q2OvRxs4chcbr2+Q!2No2Xkn<_s9DW4GN2ZzFWLWoBa&fN+-Z|1YN-%MfeL zx;kT-xgdki@k1^q*GS&@J&-^{Q9|iSs*L&uEkv3<9gM7B<|?zP8oijkU7~7|)?i-i zyR21@IMQ$E4nV+8V?py_xjGu7=))-bPpr9k$AS4@@|}36?Za&CeqtBAb6|9A#qq)7 zz5lYGDr9q0H{PN;PjW2Gr;lNX&8O+k!@Qs2bC&1yHB4injI7t&AE0!rj_C?rDa3D*yUutISr9^Z~+62z*&!X0_+VK%qL zH?ey0CKh+q_~&6ztbL%w1$T9PE+@N-0>h`WG6<;SBBX4siV?9FCxh?~{O4yiYg<1u z?d8k%OU?X!>&CNvpBVg;)V&dt~Im>8va)SYDyE%ZC;ccW$b8HlwRFzwyHb zIWLrYVy6<(XxJ(x<4n9-It`681_7XwDY}S!xIQDKx=~Rk?sY~hWm0mR8OZ@<3j3%I zkbV4H5<3^DsKaDk>8mt*hPJ1`C*Rl5N3h}9+-ig}ZDtmI)-o41K zKHS2+wg_xm%S6KX?LVYvwp5nkvU|JBH&q=kE3@kd2 zQvwNIPbP@|1*#BW7#EYXg3M^1buTyZ)oL_0?7*SzS3pv&sX#;GOVnnRef7hXDm@Kn zSjLdjll&1Q45|#tL{NVSP2OK+fSD?U1_S?y4vtq9;ub7tJ9aUU$i*`yekDech*fS- zXL3G+HJ}K1cfqp8wWcfEl4BUU@+y_)V7L;-AWcA3(3UeeNMHRRyn`&@gIMnK01No> zmFWF1BqeiBr1}*yv(U{+aTBrthz(+orv|sT`h?AKdN%6K0$N3B6F2x?35u&8H^!<9 zB;bMC8dp^9Y)BNHoX8{TX~N6VWH$zqqOIQ%C%H?p#kTDkQd+`vY7qWRaU-y z%X=8{jnBjGq`1xBxiW+DxhD+m^se901E$CMjHHABwxpfiu;G*g#-%G4>Vqda3WAR5 zcVI-ea?`jPwKd=o=a$iyoDIcu3o<*+dx^%B09~5G8VYHM%;!%YkHzWZ5orZ=$dF*|wq?)(BO?0`Zn6Hc5z*N`eMF$nyhG-fjL71@ zGa_iuhHhFffNj#Fn%^gVPzY<)Qpwe0XpS7-iTK zjn09N|;QAs)=?YedRhw{mbW8Nn`oSOp#O_-EzRO|7*-#XQyJaW_TA`OvZ*{d@R{!bp z;wR_}c%CEnpu9qhyWTjDC0l_9QH=Pl^aTj~2}_@b+{*(FT=RxptL>$qc@8fe(JR1` za5Tl$p*{l}S{&fOjz-^C)<+|96k;J>j?284dGy%`RabIcqUS1RSq7)mZX~d$)8PFd zB`%awbVBDd{ODYZlPOQ}AJH4bEjgdzMoLOGCeZ2fms#hO30P?c@D zZF^6YX`00P^_a|0*wcXJ>Ptnql$Lzi>fSeEZ8^2GW_h1doJ`mO7R&JXD)*bh0glstV^3n70 z?g6}`gb@GEoq(tz>BeVF=L;bv=(|kTT}*_AD}32-MhI+38iCO|dtq7r#Mx-?#2V|Spr*w>c+1#%YRo7}#rsKjQAt}DSlG7 z+K(0Mlkt;Y*mk{>=U?=~F^y3)le)T4ef54deXq@dvB%SrLtAJXr1z|EaSSxs2$2vw z=5sG_>Xe`%pxcv3ZTIeF5_1@KKm8X};FoKZ7{@FzHw&p>{QwgP!YI7WMJvPVp8N6G zt1^e6M8+Bezk}_K8-{Foo1wNd^X|dlZ3Tue6uA*oranvc;`#`gd7{o3jl+&R8b3-L zFZUD~R6y6LClRg+Uu|O`l)vQYv+L9SXJdYH?SqGDMR_K9QXu*-h#D8YB>C;bVb)(R zrMHJ+lZyC;?y|-`(UZlya88b@0>_v%97DKsJo=&_knKl*;X?O!s{Hu1&Y*gYl8t>> zfs5(xFoctbHHHq?m5zaD)Ve;hGN4QIkGGc@!?{`n)VD>Var0b{oEj~;ksaL&4I|DT z<~6={D-JT>mPD=-?G6l=kz?0U`R+r?9<_cD#sJ~hj<**;>x|Y#Cg>i&eVFvP$b9{q zWXnXsBE;>c<~U?l6eRL~3qsMJz%7QfH}aC|HgRg`>Z~XL7lsltuLsjeC$zZ)+GkZ3 zz=be!%}LXB^718)7vklIr5xBr^d#~okj9T2N!4bnq_I_-Q)L4U373iV1B=l08E)67 z`5|Nx4It`Ln5wj07yht5T0tJ1!X}y(len^m)W|Tqd53|2F=)li+_)xO%H`HB_7n3aW89ldR%zkuzRA7X>j>t?qfl$2cp0kSs_9eLc} zVdVB7O>;WGz;3RTqZv7j2P`akMY+&zisHShNM5o9V@Vkx6M3j`DR@4e^0k<+KP#pqBL1mBG1fdq7;)h5p z%X@W8BQe-Z;B{1#Km_F84zK=X0h)|pKHu5Ff$kq|u%9KD<@M*2%hls;& zU0J`@@vYrDf?Jsss7mnjiLK*@ND&jgx@CF4*72p?I)Yo7lKq54pEQzuA8ExOj^G;n zrbdu2?bZ?8%9KEKCmPvr?55dc#}APk@JC&E9k|NS`;b04d{g8D8W<8_E7Pi7HB7b` z^@E-Q3)n-`5iD!@G$>5`hAKZtc#^=+nb_)MPEQQqZDA2d_Nec@?SifQF+C*hMn!~v0wO)w z1l1&20GVIBwpF@6SB59a{oru1$nMo|wjveWgkkEqyN2jN6y`T$YzYn}yti-0_lsrx`X#4x-y%WbR}D#VoWK*@8&9NRaW7X@UQ*s}W53g(B%4fvBdDBIuWv9mya)EZR%1X4zS_1ueFIHZ(2 zIo<8PznlATt2eQq_Af61Zdf02AUr(ZR!#}U<;rJDq!_To{`y*`x+cDA8YN6l`TpaO zVjCiRgw@t4r8U=1O1w{U);&x1rVQ-*6GJ(1mPRK^w*c3OWCgiwY7U}V+`T9iNTiRE z?lVv^)GLX+2he83%uWAK`or!HexdvU`Cj-Uk7k*L+rcth3{x*q{486X#ImE{JBWPV zhf(yLY0m=#08`bYyt_V1M0y^~@YUz5ckg9?q)?#zpwy`*aM^2ml&D8|2OXeVQFQ$p zNZ_UKxzK#NrPC^3*=OlGhoFJewT4u9IffXA8*>(dD{m2=om_=u9n(H0Kr&H3mJBY= zL^fCc_IwO9?)jIWF3xHmI|{3Y&z!w_XQ=-;KKi#rVoMo@5e1i-a~3DEIHp_qMny^P6k#yb zwAau(G{4&P@9ZYec4s?i5=53hVDkp zjy@3qyD{zM{Aiz-^SG-av$c3Ph5qQ(HDQ%SEo-kjL&IlpPzS&iTd8VXqF6}AH*`Yb zD4aiJ20}qq2@PjtA^z0QiadHhNbGXJvF9e%EsEUm7PU&hodz1DigT$t@K8UdyWJgva9ERWASnr%wXIZXz!aHuXw|E^aA$OqEEM6;W0be>dExra#5+y#5%7$N3(oC8BshFQP^4$d;}B zI?`sbGT!~`10-?Yr2lv+ulyQ*=W!B<@Md-?SOlovkg5x* zeC}zW7Q1jzk2DO6NE+%w_56dMwf^(nye)0xkygN;67cH+f%gGS6}!s0YDGW1s4uKO zSg`IOJlDe@Mjd;b!vh62V(WoJ z*9R9V%LIY;st<7>4s~9EHOllE<*JM4YL3m?OT}gg95)(84t=Vub zgazs+zy|oY{35t9t^LzigCJK<+_DBgna5*mdOQwejOBu1_Um0pHuya%oa&?kDxv%(wB@Tk*! zC--2wiYvydQxI5SKpPBoItuV!Ez)YeqlfUYGmbw`as-}bXR61zI^%S@M%A(zd2ELD zawg1z+@|Y!R6V5M$rN#Bu|3YL2k;k6$ej1QN7{*GpCGU5!D0hNM<}yh-9d#SgrH=; z|Ga0^l)k30Th&HXmzz_cdH^Sw*iCwwfcnPrg3(5Wa^NvmQo!h*xuclj-DCLPqZEI7 zVFKK5?C0=S?5F??BdTlaCj``+cm@kaO?l*oql883RjQWzZ#X#r%BB0UqJ2F`5z-f| zAB#&O0Xv>;VLt^n(sQg@@~sZ*!|zC|ZFa+qBICoB z;f?#Fe<-GzwACg*GX4dw^!WvO9wZ>H(&Pv>u0X3cw+$h(PA?wK$9wQseGT>spzh2b z4APR8@`7#7XsEIE)*Ee3tR{b1_8nkf!AJsZHLlk#tN_Set$z6E8Cb;Y#5l^m;X)pw zoGRYU!k8eS)*=P=^Ib40OeLFvWGEDa;xjZa`gQA$su)SpKJ_J`%%c2!CMnQ1wH?I<~#)V9bDYe1(mcNpBz!@Wj#v| z;A-u)+7a*=C?`xdlO$}?#k>eqKx0qHABRLZ}s*7EoUW09|?oe&CPCdYs6>K)NdNL|r>hj~Q`MPLFRz^bJy% zC1Vv*I2S}(S1AW#1;Rt&ClJyoLMm5E@EmV1GA@Yy*_Zhm|F6Mqn6q?mr_Hi+Y`n+Y zea6|T2K?eDGL~`H9~#-liR^6{-k)Ypa-X-p_c3C}5-jw`{d6CH*XJ4b$}23=eB1y} zHJ|3qM&-hBN;rggH2hD^;&!?RbH1?`8_Ba)-z`k z22KyMjUj3(zIQqv0}jZ2S@;kgrXG=`E^p&qczwKmjB!1Uu^X9)nXXo6dN|Y73I2?N z=o@cJG5dfDl3NV1HgUlq%ZHc8aaXYyaFu96{UG%xeuuvqTv>lER}g>SM(n|P2dKo` zNF`JfrcW%VG`tvo7<|gZcEyQYH(lT2qyb6Eg1$bG&|6)L)pe|5kDAY){Av~jr}{D zlVeZ*_CM_WB-T9*4p@05O5}o2eMZn~rZO^K`=aczaz$}qTuI4nzPnm4x4iDqjYNsP zJX0%&O&Z#(Kf&nfo5%1;D|&fdgwv48#);{gebkZMk0k||2apA}Ryh0Xt#{!{gxFP+ zv_YVLED9()wbGM?(O?Q&v)@zl(0YoyU3iU4H*!g$kf&iR7-^nB8t@POZ0`(G@-HQR z(Lu2z>a)=<)+9dvMd+zzYN7lu(xRc(Eggj3=|zHS>Qs3P7KRRCua(d|f^+XS zW2yzQzRcI+n>jo>25a4jmVk3+&>4lNv5Z(-29~xXQ(L>T5IFY`W4-jVqf0hA^h^Lc z5eqk~jI*sW-k2x|2evGK*6XvX3L|d(QXHfqju+~j4QB#BnCn}ySMJc|TNz7vpDP%q z3jpb9_4X1@8@E|mrZr1-Y>NF^?X^E5o+hBw{Ee?Mk_!<(be>}t-PIu|Bs zs11A!&p*9=cyq-X_>(+;_~6b8fBqo1yQ5;}sE+Ir9Tkg5;{9Y_Zb!vMc^wsH1$e5< zIIFYb9QQdxG7$9US!l%@qrqD^_nm zySZXcbH$go7d2NbXs-C$(nXiGN~?7bA20LxI?HbK?eY2ycV}6or^D-O$M2Ed_(c*c z2OX7RW#g`RC56}gdS5`TvOB5*Z@i5p4y%$wBOkwz{D?Q+MsciK$unE|SepEZH{Qlw zl9NZX7hw#XXih+VMN)Xo3cU6T=N`X+vzQaso1pvIi}oW+pT6^-8O9V!yA2 z$*+7}YgO_zwJ+}(5kXKGUteKWPD_53P5$BJSH2!?RTko>K-w=KV6crh(QK^}wNIU6 ztD(%B9$P)lVO$bhUF0w>jIEyNFviDLPjDDxv91{fvDLW_V?=EABu6xx-EX90u!1+5 zaHbcGn1P5DtTF>tR&b#iSZDYvoF=X~-!so^C;`DDU7Pr#{wD0$)Ru?EF2Ty2G1nD!#jf@g!9-pFAMT&Zw7uFh zsLEqyTGQn5c!kFa7na0yN^OO71H?_=#3M-i8mAL~zvOgU-|+2hUIY96PO(4xjLai? z@y=F+n+K^tC6d^%z~eqwOzShvxh`|A+nk$g&Yfh=MK}k!Q@PVq!4jYk_>B0yjlV)i z?wp6MlGXW{^Te2E$Cv(rJkfJ@7Q8R*_<(z{yb?UHHRvQWRCM$rEYmw5sFb=tGyx;` zS?eDiyt5!@lhI&-&&}b3o$x*&C=U(> zeVVFN7%=54+`LAvYB8%wa}_b*d&X-b6AVybN`8tOKoL}vUuPOzQ`gKLe>MINn8h|U zV7R9|1)nJmjer6IH3M2M(4Bw|!)J{(v;j&cvzan}XTv@C+pr^}&nyM_V%>p0;UAU9 z{c=Rkcn%>?MU7~dKc$u}V!vTk_BK)(VKG+Xj#66Xdhd80FR+1{`g>`PS>DQ26p^j+ zr>voyp$e=CM;^hmIRwu$0&~M*-7s_?ffKCUVKx8Ao z?5?vvYlx{s>d@=NT`0I89i7CDOlWxjTp{(Sq|KGI4XLz!P0;jYC6@+!I`8|+!WV+u& zLx`CaP#57ZdOjZS0T6i+Eext3W9oC?rv3m1g^M=$Qf;T7x<^&v5u<|QsCOTMX-cV2 zw`mIn#;w<&X8Hx{AFfAChsa?d4y)Vt%Qhcfe-;TFHM%dHwVzs`E zrN=kcJ+H6v(8#zWj%PD`5tgAY{vw<0R_9^I2Ba5<>dWHMr>suZD4S3hPY?64W6i?% zI!#w!vUGi6b4D7hQ$hQMG$2hro?ul`%RRPfp~ol?5gE2$pDrZ7&?hAK4eyKAP?4>J zg%PfG0WD#}5YMA`2j_9&?Bgz4-Uz2P@R}Pik>MD2EiTWx#U2vPEI#U3x($ER3fKLYhV2Y=K;Ugag|IEqZLZ9>R|#U8;g06WmHY+Kl&3E zr6mx$yf_H5I#J=F@a%2cgNN|WWk~-dxy`dhRD-&qYJjXVs4hC&s(^Aj(!;KZYvqob ziB&{sZNtBl)@~%NsZH1@YJHRSJYpchqsRf-Iu=?Lo=3Rg88aVYiP7nZzeg!>D6W6% zyYaXqIL@kw=$A=@8@wEZb)Q+fGNv9EOD(iHWiap%>TMegu*%xl5cFYnki;*c=3A4v z^hayI{@Bt822UlVe~P#9ejrNg@c2@pPpwH_F&reKiSQ528Y9AOm!ZK>{%xhYl>S=c z_w$W{f&{7-GW|rf68d(ZF+0BL5WdJ1tyY<`NyHSLxfzNlMA`gHAk~s9B+U@y|q60PU^T3&a0E zbm-(=v`#yAP9}B=fJl=F8Xg;96hKjJ$szEn+QLQeMguTYilmsDjd=tn0nE3qpe%+MO&N5I|FQMzB(*1 zrqbJb@b>h!vYvXreOi0h$z2y558!yQIv!`6gX+S*Hs`8F8)LG~z=!#YZAg*!HwJY& zXv*Y?+92MY7~ap@hLkVq1L~V3K&EzodTL3p{T0(WI^+GKqcW?o)o=~U>>wpE8sOX; z>GA`tJJ-9Zd}QV;9pzr!iE;Z+dSg$ZsSpov1$pgAhq1`2!RFr;4A;<1j#`Mb;+`q~ z`32rPOeEy~VpMZ}MP*g=#I;>t^FHI1&|hu_6GJ_rELbd5dVCd7B(XZw_{ecQj*_0skgU3<1K?)uI43%c62 zpWF5H_Ahkpj4yDa@KwI%Z0o&B|5~^Ao7L#-F1U2`Ivdt{9d$U6Uu3wUCKP2&F^n0t zvTNg)fO#4I)#Kl7_;(Bbt-!yp;omy^bL1rdbLQk^Bw_w$+7B-NX4wz_&`)-9@HIWf zy3_p2Z@CWZGX_7pY)db=aLdpS@wYxFUi*IaGQ=&N+8S|T9}wZ<&^-Trv-|_RJ<(i# z61V$t*0i%5dnGtb`iwia9nBA&#rxwsI=#NFELbzY!_?&35(DV-)9FVdNzxg9>^7G} zhg2){l7vV={!=Tz2+Aqflyg3!J~K`rm4=#fay8|wDoG=m^IE(OCohPOh*!Y>cfqQ$ z2nBM%1*8rSJZ|yLkmg}lPo@9ViibbLAhVk}zvg=+xQ?s=v9=>}^&HNNIx9RyAX*cl z)GG=*D{`?5gL|q1{~=>yI1v8^q@ne=y2AN|yXsWP`OMXB9JhS|SNXMy8@G-=VLiv} z=fvXKRIK%giQ$A^q5;UQxsa&Hsj_CQc0+gXhl|e-j7#95sLJhs#kj@_xMI6}{(ZQr z>o$j30XLK%E_It!!qt@lrUq*fnBIidUtf}n0oB=H0jL%%0C*6{t$+4XN*PP*0LyRZ zT+oU{HK^+YQxu5)k)vuW8};?XI&)(G(Q8IIv**rO?OJ&ejOpV+rx_}UogBQPa0A!` z$ZzFH%Z?5QYq+`49c%AF5er=~CqM%O?&5vLuVn5;WBm&}kd54^ zvgw7Tfr&9n)&ol9U46rX_OW5O=A1C{VU{6%oD+MqH`WU8x)4I5Z-Yd!Y-oS$G_c5o za0`XI^(wNAMl>w*G7xTTmt<;lXTEO@$6e*OmTD0g!O^B3x1NUycfROvFqE;QG{}Hi zgu~wveHZK!VL)J=Z`(VhzVqvQ=86Jyg)7Y<3iU;N&}~q0Vi{d>^nS22iY_>~%;~I< zfec&gT&UT(Hoi{lbN&jq;YGm#6sxX&k)(kX@RW8X(m~ChOkoq$g=r}&+9SwZTj zcS>U^ZHpY5(*tVbP2fUnLwQe*LL&iPcNQ7`3WGzRKQvZ9aCJGh2ZDo*9 zb$!!657M83&YY9hkAhXhZ6sH6U*7-bi{<)>9h!>)rJp_1qqtLqy^KTOSMss?935@=JqKY8SSL z_hOAPA)((mD!ALqur4((6S3l5t1Z>`RD0l#1n>B3$}n5RC|3R>_R#B+`IMT%2tAoX1vJ$eRgr(R5b z&$ST_$dCF3TM#eFD6sM-;xq+OgpSYiIo*a^+ayHancCVgV+um5;ig|Td_pAF<7j`L zgRVc9|2Z2%C-&93$<@)82m9Nvh1550RM_pCUQ|5hu?!wa1Ps)NQ9GCOXXmr?CK5o4af7lUng`*c{ZM?q07!mVNXE_?$F50l~oCRMx zV-796XC53n^#r#T0Kgq}rkXC9ONqy-J%&&@As*3v{V5mqCJpy)hI0kD;l-tF<$uzJ zds+DS^Izj``25rMcial8?G0Ar2zd+=WcG~w@WScDKf08~rLF^OdA(q8`u-%|bPU>t zKx8!bsod=T1S%wOxrRhu1ZJO9Ve;-PSV-wjc#g-rk(w4C*dsy7qL|kb6jP7G0;0$M z3TYg%W;>hs47W&*&L-wd;D#%FkT9;0w~^bVic%gx7!@prD#+N=MCgo8=P09uV5A%M zL21MX2P^m(P&vwi`^*{@lfm=PwG%h7HuqoM*laBDvjiR^tP!>9iEbuwU=^1J}&l>ZLmJbDkxAO&%L z!C;j=$rI~jC$ITwvXc*j*C56u{P)=XKd;}XSqx^F{N(BP>70lI&+02bqKXRIL7W2G zw4FnxBq$M+6J*HR+kRK<`FRhzzi)r47Ghj5;bx$oxtJ31rkmJ7kqi()g4LU%r%F7| z_rcWrXii5K=aQ`)y{uLJYPIg0Cz*BGj7>~${NB#3v39lgXF%Gt&s{$vvKd;5$ab^{ zr_^%1jZ@Y7tjO|l$=7@B)99eDJ|psI>h*q&qAC~eRi8_}1@oN3xtnA&O>H2q$csR) zXS}mBYdNgjc%8j7%K+30NS*yqvM+~QyVYS3(a;9G-^!$sR`&KsThmiK!xYT9l_=Eb z5Nb(n`l-vYbs_qtGixz94gTxQx)BiHDbHT%>r5E!M|Ke5qZ;AXJMk5LJu(p4`~cGZ z_Ht3g$ZPc-P?Va3&K&67U(5653_j)I8jk~q=s8pO@8~@nI<605y}nghJ6MBQ!hOr2 zxaV_-r%-z|3p?gIx}Y+bA|x7Cuzc@he}o64J9o*tDXvX3A^6uV zh4YEz;-=|aTLNHBrHBy3MU;SA28zP6fOMdLuHRhqW>mPMUxIs$P%eTuTy8D!WbXDp zwgASwL3KV#N!~2Z)>hnB$me}bdkKSlf0qb2U1wP#Pv%~{_tgiggX(kbIvaFO#vW+e zoYMq>|fZ1 zFyzV=m8^H$)cq&kLj`F>=zZ*Dd_k`Fu~+?V(M#)n@Sh65u{1?>LrxhDGjvM(*02Ej zvPieW9EYB^`w`6usMTyea(Hlo{Y`Nv($~RueQSNFXuuzIt?)r?!-ae`zD2qOfwozF z_I|Z`_FMM5WbK7Mr2n%~Ntk4eL9@pO)a9rnct%pz6*=0bf_~DCrZ4nY(3iN%1w@bx zy^nREysKQrFMA(*KA5=|4Y=YlP6`nU?eV6Z2A|ZcvYy4{fIC)QI#2owXRLD@qhxen zMRx_%ZBiExyr8jH`dY5rVB^D}`WLvPBVg}`xghK=3RMu2r^cFI>=$75gmx{U9>>`W zY6Lzw0AZ?r21FuP9U6*973E#BvS3={`yf{TxUdK7v{ z2;#tv9Tv*QXau~E!RIfwOl7)IdW}kzYL{0(Go)_*4SPv;RPl;LOO?~wi@X0uZo|pN zxS(+I5|~-+#asFV=(cthca#we@&`Gy-R4@i`Xfx#IR=Q`;S={G#-QSkQUuI!tQ!=+ zoxcbc?T`A5g*7nZk@H=K~pi8u| zQqqViNrAfk9`x}0=waBkxvqc_BQIE-3mzf*I$v=I_Os$l$>wJ~&xCyGgGvt_CMUDW z4()fjAB6xvxDHl17i2NI#TxbqkB^Pyt$=}38tDi6FXH6pwE<`yagu?RD`Y;s8kI}r zxkbJ+&JmkXIDqT3)qLq@z0&>qe2{mSTRX&n6=Qrijm>>Nv%r|48t{PQj$8S2XrQV2 z0qi=hk+3s|u5*lvV*77Nsj=sZaQGHhA5`+_gO^26>`P*PywEN**|+>fdl281Er27z z@}3#l(Ja06FK}DKp?Ek3vC+R5^a6i|Kj&10?eRFPbP7cM<|PTl8f{pc;1P>a40Xz1 z-5asd^0bU@I5o)_({QTP$n!oi8X^5!3jH$+OQJcm=P*3zYb_&SyLB-vl%v_Uo$~0$ z*XTFGx6N%7Fmo0%GlFMt-J&vE6!9@D?3oQ0Ht&NFQ7eJU&F`@g?lkC1l=mV`AF9)| z-`n&rLK(-f2wjPG!P06nhPa_*JUq9k#jT_X7lJ~LkB-7ldrq~w>OonOtI+(_E`Rol zg1SXKw}AY^wi)lmPCBFB#H&(&R05Jw6=9hzR8}B85TiN&m>KPDo_>p!3&2s~CJWK& zC!p%_wG&hvq)B^)S5Xtu5BSSH(UA>n$Kx!K_lY~o$2*UH8$i5Do5f(@9(k4j}^scVLq_^YFHIqM3n!b>)-H0=CVHViS9;4p&@&9k|Q8ht&3**mvUK z__px}ZEs1uhL-zDUs6LQ0Q$d=-Ga@{l4C(yirn=8+TZXwhjoiPiUGpebJ3%ak1HO) z?B;FPhkqao6J)^ozHl(&;>QbMwm z1Al^4cGWk?vTPF?p+C1(28~K@HTQOy^7uz*!<3Y42b)$^1Rn2a!*2;jW1xNo1>r&u z1K8lVh#Iuo31-zqYyLztFbU45%qmJHK{GG~BLbnNvy8W8@Z)9)vDREn81>u-oG+Bn z`~Nq!bw=eR<1*`7%Dk%(O7La>2P?;-2d(fV9#wQ;-n#rBtay8AXQiNB)V|cL^ek$} zAL2s$xRl1$PjVO&t;!^p4qzE2`Y~X-7MS*>VFLGvhjXie!K|F5K94;&Wz$NLN(W5$ z|0;gLT>zZyf^?i5zu6N(7(=lL2H`=S=Xe;z*)B7~a%Of6>>#9%&rXcrx^Kaqfv`qN21_2&!co z4I0Asc{(hoe^>MbQkjTt(2qdaKFFgd*+v{^u$9B>opvkHCiET}%6%J@Rhigo8ZQ5b z(Q`o()`BV4%@J%5p}ay*NT~W^zslyE8Mq=cmZSL`R@?gZsy zqk&B?WDl&Iuz~M^k9v+E?iI5;deGT4#P30XB=pvL9)&N(Gi+q{s!O@Z&iBM!oyahC z|BNb^F><4|*TR+Mgi$i%nobu!Mc;{EjXdssUFHIh|Am#A=2CaO@D*!!@hfd7GtDKC zuTH~VA~e@t%VB18O<2V3gN%nec9M?MuX_b@O>f-u3M6(;)zss0%id#Vj7csBdg+wH5(zD1!P#=VCCJcJ2 z!*HOqfbxi3;lkCShbEwRu$C}?eAXdM97H*_4vKvR>(^?`img2jBX1671q2~yjq`#2Hm8g#uBr02V|q*m0E z=t?}H?m_iu5JbO~N7o4T(UTFuWFeOOtlgNxnbf&pt-(Oz8Wm?+o*0!-Fr&pV@L(?s zpww%s-+|@$zoyP(tb(XbEk3 z5b=cTiNDj^gkV09q2S`b8?na_UrI_uis+jhMch+anv1yy#SJ7psV&x^TAU-mA+5Se zEONlM$bHX-I*-8tDkkayCI6m(%&t+2tsTqk1V;hwYyN#vI)*=pRTRk=v&O&MSQU4@ z0M@eFl|1jm8SP$}l7KaM8-D|Onz7~uu$dR4|6!GwYmZo~UFKS4oy39aD7aGnFo99@ z9I!sb1iS7456ik-LZ+`o>S&^r(B&b&>F{@zHNo9P*eE1bhTGw zC#aUNezn~YM>&o6VhArar=7w?3|k+V*PL>x)6#%mAVeGLxJkk}UeJ}q<}_WWAaa`X zPPyR5K3Rx%fs4}geBuIA>!G?Eqe5*=Czt@njP(Ki#JVS7^F%4(c%{#Y-IBSa+g#FX zE>T9QRXGvfhlHDh7|_E>ZeEEFJ!a(yH$N7Hos;&@K#|T3jw@}ASLVX7xP@1jdpH8H zaQh%SXle4Z70$&z1TM0$-`IB)8kRP5KJ1bY1F?UXw{a$^2;8YBKCKPa07(P4tl&rb zR&ZA12&#b<{0IoWQ%%yQ$<|)z?g}99poD;W6=qMUfWQ2(w~3qXrrOt|i;uTEqmz%f zco zbOJcvkG+sdivuKtqu#kd3xWaVE^x*E?2NsDS}GvLrl#wHX*lKL7h*(R>@RZfCbrV`mI zkqxU17L1L|8SRO?I!Mf1DCOo^aR(MQtZ8Ou7gi{)4Zv0WvULE>F77Bk7}V>Y#;%6w zu>#serDsN^Cz_SnYF4_JVuk6)h;4=p`X$f$JN6UcpEW-v4YRlGZM5C7kBc2igI#Gj}L^tHW}6`$jQxxbmC-uVf70JP5Y@hhxR$OXk2A}Yg@ z60RCm;$F}cLwx#7FDe48{@p9`wca44E|nT4W0RRf%%uCyKhRIe8wOxECQFm6E-xRAy(_nM^b$O=Q9- zax^NhEgWUIUL6o;w`#4_-UH3iHMB1~9j^l=)TqC1OSRpYN%jKMj_P=YC!m9V zLNyNMx_SUtQG_?=a;9BcXO2lK%4{pZx<%qmUBp+r)d6+)&$#bS`tb~fBM)Kcjbm%* zMy=8pakgmnM4Is`lz26A6MwfFILg99KprCcp+p{*%R{V`DI9fMZ^F+~V{m8IY5?4s z$^HY%HK?m{u=a4l2+^&!eU|Pmh8iR6I;)na-o6WPr!}a)@jYztpNH$xJ?n?Aft0v1 z$5#GfMt|4;{F=e*T?o?cg6ie@rafzhujkeATL!}kvn`z2O-~MTI9}mGuc0nIi@v8hf>tPvJBo9u6@WI1`#H+D&MF1w&8AD_1aM_RZB@^oE9o#0Th)D2j zKeg6^B1=4J!zD7(7FeMP@Ttv32LebyAY6&9Yu}L%1WC7UppPI%zE>Rpfsr;UCe|m= zKTol$W9geVz7CFN4kWVSs#g0GLA*~5VJixOrlKh2rJTAgBpnvbwH8c3Dj&x*G!ZA> z^uC=$K=t~8sjVB|Fc~NDn_v zXsRw)sxUAo+8a6i4z5qE0g0%0E|F8wSSbkxejhP2kek#*JwF9g63OTN*auhe>>PdJ=IeJ^dknTKWsgz13N=i_&ot(dC>TyjpSI zEhs6Ay^7U>s}`I}qdC=T2ry#$C6S`OU=CnYIE)IaA82-v#-rrBTeGCjGOdqA*Q7=u zmEUyA(FmUzP6|@qY;K8ke#7&Y6JT_ zpk_akF0V5E|1keG;_4U?39zq%uveb+vGNoa#rnF)ix8CQ4-Zbjc{3ze{Dmgnb7Py= z-x;2ElMQEhH{>Us;X(YbRHh8$ zAoR08iw^KseCC2}W<3aCy7!PPz zU%@HpfDxG$aifj>M#O>ie}6=nO1met^4MEDc}K~=@0Jp{FU4d0c2@)Zr}Nt9;MuWO zpa1z4A9NLR@74wd#C07|QF=P(b$GZf!lTwP3B#w$;_$`brfi7XSn?6PP8A^Q zKzp3zFm*yrj?aA?6FuBX`Oh9qtxUH+$xkvrzDUq$O%E_Z38)PdDJD>BHbQx&0z*-N zy^Ry4#(}oCa^Tu<&x!r9H=VJ=nZ<1gXUpvokKSvK8O2q`*;U3!weJd6pW7bf7A*|U zMe)?y<z(4g<6rU8`$R)Yf;Z=yCg2A< zKDN250sxh7U8MlH)$5uDfYuaU#niiw1p$Iqu0747<*e3f zKhJ?or4Arnf3Yz^TM6YFSHsS3RH*|fqLXm+k8I_o4rGsx>zehfpqXBZ`1NoNQ|dtJ z##yCAW1OXae>o1A`?vd9si(H03q8{SWOk^PbAzfC#;Vq#8npwzCk~8Xf?d74@iol) z)jBc`{mTvA@G|naHq1w7IhhyMphh-cqIUEh=a%pA?@#A&d=DQD)Has)(g=xRhp{`5 z7|y@s2D67J3W&$TXhi|lIza|}NC#9c+zW2_v%8ET1JuJjXQPIh)R+U$w~QCXu+)-l z<9#1x_z6s$Z}V=HuYu7*2LDEqDfq?xeq9IrB%sItWy>NEa6CJ>Pg15tWQ5 zVLzKHmi@|O7W_yvINxejhbQ-Pxnz;T^LgUg z%{}puy=(za($m%$JwDuNPldbxD3n2UERJK2@uh!Pn5do0!PCR0hwwB4X=5u5oe&Ol zaE4mj4SPi#2B8oR7CRQF*4V9+jNWlS|qPvNs;Ipt}^8nuH8h(?qD;~ki(e3JWs@-c|i4|(U`1^Bg*p`_7xPR zFXm?9ThkUW8Ck!q-W^=QkkG&Pmh<&Gu%XR`={94AF}y6+=JW4baZG&$1#n*v?9nW> z#C}RmLEx9ezYyHI1T;lp?hRLOT>4Q0npCbBqIAUw(r9$Uk5Rm>;GS9k#jrZfwot8Z zWc`{3BH%XTrJ}FX;^#R`Jf6o%id&(19{wW{2JNFyae>&M;Mrc51>94Qs~_}Xe}>pW z>livE7;!_sOlI#9 z3X@4jMQOD%VIXN+*RTFF7h-rA{A*vw;3@E(tkz_W<`)t{U-f0)>J zc?85FwzDwh+W!-KXrhUq9@`ZCqjNB>CqT)wPcG$dG_Ki z_pHUSJu9B<9|Q1M2mL2j3%LF7MrP2|lfhUMQc&>poqEOM3XYZn363>%FeQxR>D6 z^>4|n5ZZ3KfCMHkWNiN>LG{}p8cy?hH<*~l1vC2MU5nI+IhyI{Nh|ZUzdsp>6j%}5 zn64~Zy>&Myo(q<#-bQvcOwVWHb~Nla4&#a52M&p!EG!E;x=>BaxX3ij%#gYkmEckz zR=?ZIMJcy=oY}wo{2 z$s4`TRNw;Sp??wv4M}9U_n8H_1$o7n?H9(71&Z7Ye81yIl zk0M0Uvspcg0P*1& z^dzTb@xJOz7y)>ZVF5D8sKetFR+OdcJiFQ{4*kjFEl1o~5X`69VVyEY7#WA+`A_}K zuzLJ%ETOQL_u*1k=3Mxm_#?#a^mrM^u+c+RHO-JOP@SVw^m!oHm9|b5?s z)M8^Au4PPtcXBk!+Jzhasd%G$oN>npVL7B3fpE17JWNq0*OE>Mht(8q#6`Ns^#q4C zmg7D3IHE6$!)?^|W@pRr*|-aG66O`}HLRHm?&@3+JL9}%g}-Oz*We`Sh1i)SKe_%> z_{r>4Bfw9lr5XW#a)Iuz%$GrM(J__~bBAMHuq>edHXj2m^#P6Q3df%2bai-}_Mg{{^m>E##xX=Cg{Ah{u9yYrcyoMaJy>q;tmAp!E0Uw zTBNG&FKPU%|9yLV{sruTRVMa_8b;;%h_QGV*48_-PN4-2CubVx^BUSYxQ1pFCOyvc zLRm*+mNXi_Y^x)#y`W+LtVJZCg9jX-b z3n&_%=9_iaUT@P~GH?EdM=k2W;OIa42zAE!3m# zhBa3T6=IZ#Gae_ZYJc0xAz*j`b>}TEM^{0-F^{j2H4wjNDAQG|5p}|BLbwi(XUN}Q zL{d!YrF9LZ9>l70;J&C-ye6nR^vuwlGrma>1e8NI#aweZPCxOduQN#o^ojh(mJe-7 zL{knUHxMu453}L%CJq$3j-wG|d<4eljptrzJx_OJhvK(7$zLNQZ?v<6( ztK*GhnJb{4{XAxk)YisRt*((>E-X-Nx;r-0=JQfGSZcBCTV){?@;hKBup1=pX~dZI z@pGI0Fp*Y{;9eD!3t9jBcD{yEN>FfsSB0C-fI>&_=6vnVSaBfMdIkogbKlBBx6^QH zv3OlddW0i#SE^1vD% zs19QUhO6)2!x1P-i#~97iSEg;dRE&uK+-9dt`F<3FR7B5Sh6r-OuCpf(!7~}>pB{~ zOVYun)F#$S8&gM?&=d%b#c=XnY=2>YY&S`ywRh_N@UQs_#pZ9sJqG{F zD_8iR$Hn-4xTQ%qOsi5~TqJty8&gb8~GTYy~G6SCa)#uT85DijYtS&kS4S~J> zx9k8za5!c7!pRud527=`K5W$kd{V_ad$e7K2`gXTy|7lhi?MQM_A=TyEG#78cvxL^ zjoe+P=6+FGv2ptBxrv+K z1x1uF&v^nHTlLd#vYUJ&9HNy-5B`CoEuS@r{_~tOGY~ugXA1B~Gjk4u2ef6xc4dIc z9pRjk-~nl^LjDci&t>+be>m%#;h^3l6gK0tuo{~8&%@u&rZW7MF_r~9I#e&@ljBWX z>LRDG{6QT5@J^vdiv3YnhhuV?FbqJ8%WCS&Ivp2HzR02eX+x^xyp65MC_Zl^JQ|?M zl^a*C;_Hgkf@j#+Tr_7`Nc{|XKuSmNmgEi0gFj(){m(hAu0!3D?B0BwHC1Dvi$iX+ zsR~cjuZ>g>!=qEIB;VU~4Uilij6W^+GbaK%c$;>kZShU3@d!zOJbD{!)oi*3&lKr$ zk@!RtJA$<|*4CGV)fp5mi2`;aZg}cTs3iQwRUq~z1MjUkzrw7?+C%_>Dk1lNzHo;= zQ*eT0_%eEQ!Te^!bzW+570#& z?vn0Eqjq~gbtjOg^Lw9W5x5h+2Oe+lX$$M9Xl9eazB&mZYIvZ&L~UP(Cn^UpJFkRI zYLUMA3*I&|ja|P0|(nKsM&stpLkZtW zWsEIZI;8zW)otjL0qwU-Wcx!AI%8B#KSCd>Q6qfn8-y75L)}cMpdHXZ6WX>5&=NwM zU!3VvHInWnKvxku?4JSuEQILeQB5b*OQ@L8w#oQ<4xtCm12l?|?|eW*1!|t+Q<;Ps z2%UTgP}wDuafuXA?76bYc0Rnx*L%M^$)^rUg6{!(S)jKs^r`0vZ71}kK)*Q8r?wL+ z`X!(r6Pkz!nd&}5Zp747-zKz~@0tlQyn$LrXzvj~2BC$o1F9#~L+A!VH~axmNYW9y z1`w-Q_68n4$Jg5lO(#_JM?l4dHWE6AkmE3*QG}Kg8cOIep-e)7KLI)kT}Gsp(7S}D zyb0(pgdQOD8X@1G0qrByK&X>Y522?CmHh?KF9_YhraS^jdXca1W9z>|kdO`t0H%L18_g5i83wr=vL&&WF%^|d!PzfPLXtJa`3TOhM z-Gs&vDtQZ#kI*(k*??HZf-XFqd=Rgj3B60Gm(X7bMY;jKM(7ZseT1ss2GmJt2cf44 zO?(H?F9>ZW^avsMyMTU3Xf>ftgm$w579quV8wi!W2WS+#a=%6_&VYSlr2dKorYR0QZ)$B`-FB7QiLWB2J{A@&4hkS$UOwm^MqCt zY9*uyJwa%p2hgL0b`yG#P{~k0n+a_r^esXK!vHlBY9@3WAgLlBzRK5CK0se46k%(w zC$xj_t|c^aIG}Pud-DMK36STu1aBIPgiTHEsD7r?Zpvxf6{0-OhDhyr8*_c-MmLYM;j1 z>ovslDPi2@Lx}DZrxRgbS#`@X-!7Mfi(1-$z+dlH$MUoU-HMo5ygZi+fdi)00TIvs!0(}17vwO=m6-@I=};Ho-UD|2SgfLT;Mb4H{LSKr5d@E3s25P_ytAK}2blql?n#&`3f9iev|f zydwDP5y(7=+!z~%JxL;O8VW-QT-)X%QD^A*xau&hF;a#v$*q92!tQ@78yW&*7L@5u zpeX^Gm~t57tD+ZL!|31TTzDsBkm`*n^;IiB_My|Tqd_NNMwJci5RR3J82Ho&OVD+# zMYzedLgYB88F2Mmy)LiI0`)m+xpb~OR=SZJPU9lgYJc0p7XkHobg6pOeitdjE?QlI zR}d)Gy?BeUqsxk*I+y7hlXwTK0^n7K{j5H`nOXY#U_kvgbobYf+xjyeA>tql@jM|< zMsbwoTx=lz1%!(s8l#oxq9H<1?>P;!Z#nuyv>j(zVLQvEa%vcwnowJ;WB`$K zq17ITTS;2;BxtJDmsT@>Zr2yZyb6aX0VnrkZu+M>S-i`7rFeI}V?!G?Z}bL!_O z-ezTRE*8y~4SnVbt$2hUY5e!0cpK5bia%U0Rjfl$OUlSZWn&7d7`{O!2DXmd86$<; ze(vmRCTelirtzt*e$Q{VVY_bRmc@E6Uh!Uh_S>=E;j4!2peaGS<@(K|_}KpBFg$H& zo0q^b%05K4+XZ*WG9^f_@UN?Wi7yaa!^1yHm{Zay5Z`0nY3JP-jQIRRn1GmX_ThHn z@$y4XBO9Cej40-TR(Cy$Gbxdpvw%_CHK^s3x96X>tsGv${82o*>UYA9i-NdEV-%;( zN3&BAIl^htI56BE`ZR$npg(}HZT*ivU*X}Ec$J^ia;*~zA3uG%HY`7Pl+S5g;(cPz z@ePDCjPc$l!WqZE$p@E_zatmFYAw0}xV6tM+(Z$-K56WP^9K`fmM{(PGcp zdKNU;aB}pj(Ox*MKRWwKj@W@Wn-1Ej(4(pOM;k|kG@qA|fWb(9i<^pPxH-RsE_-Gg zgR1xcVe>;jxN2b>#I^jhzP zeggwQ`oxBEuxQNc*!N)-#7XT;r>g^~g6xV4_`+JQ@ac8VI zd({`MJRkb1CIo9C0`UDX!x~GETAUw)trOlSvS)eprxDS;;yQ`7Sh}dMMz+xs`5iW!4IK=7x4t_>mX zjc~H>rctgQHOY@rddg1FZ}X_}FFBg?x^`Bop9lI!v-P>?Q|O7%jq2O*jYIELK2yS* z;-FumJk_)?*60{-Bkq4XP$j)dMnylT9K%57z3Svp{Cvz4+8zvRD(Lo27@>f=_W;Mq z%?)x!*BnNtELTr4bFX^*6LB=9A1a&z+F%eWq21#X+P@F=1J%erc!D zy>IW&H={%P_UFmXdALT$$?cTgFp@Y_J})z_MF6btVx2Ra*cs*~#%9%hj=g#=EE1w> zy9aElDZ&qDq3TwgAbo_uKGQhM`8YGiB%>XY*7-Px?prlje19xOe#io*VQ5Y z7z|q$3It<4GXy^$(+9i^eq)WogbP&HsGCuGKz$#Mn(d*_R7*v>&@ZrUja{PFA+Xys z^}-<9X;dZI^ZFO`zoEMX+^DbrMZcJfQ4qB!jbzSHCHNx$#(VG=499!>aoFTmc^_-f z?8G??UdA`M&@WcI`E)lEZaRd&YFs78;YWL!2Y2Fq$D4>Mv!m%0ew;>*24HhR1PI{W z3BREqV3`{1jU7Ng#$A51N#T>f&3gx(As4rsO}}N*$K~ybTcGyhv)ObQe?{L4(+Ys_ z+PFz-%5#!+^BSIJfwb+wk1^O9j?4V{rn74_R_$3+TSm9|nhVOBhtKwRu6W;)hQi(h zNiV#g8HzvfpIG&<^}mVx7Wk;DYwwxNBpH%{Gr#}=qeejq4r(N56DD8+NhUm!OcFAK z=|D)&O#o6L z{~Eskz)iD-9pk<;`d z;cFZX0F~>V6~0O2SM7<)mTzh+Wr4X9{yKIMg}VnpTNo*t_c!Q3?=!wN2|2=U5+!4P z|1ao`e7JY0=<= z-o9<=1L&e80{xAG#UMhOuApDB_>Qs~CGE<4OvMn_0lp$vQfvKcBu-2uzRysP1z0MnYcu8Fe7;n}J^ zJqHqgyXmasGRz$x{vMcbI4)qOXZTg#LhWievhFU?XO7l}mUY+bvrGYm3JiLi?;DOQMoSC)jyPto-!(i_d{raH7ym zKA%e)OJH-Y{(2|g^jSoFJXVM}ee#4{V>PTDx-L$xu^3jwe)J>64#A%ju}Qgu54$0Y zkiTgO6YtCV{ZLyt0g4RRxlP5rulX@34*sbSqB}XbY@N&39+36PRY{39FY;jxjDocp z`cqYcc0|GR9g(kp`5t9U(ghz;-?KIttnfE=(?^l*O2FYOK#f}WJiW5}P?`2c#!!)d83 zi!gUW3v=Azx{J35V*XUoWR2r@f2^2xIFOhPuU#Jt58=Yk_|@gXzhVd**jNti zH-PjCb#D$7Gm$LPA9zxbNf*1MJ&y!(#?65Vq#g9Nqt$RxGHUtwF*cwJ8pjX?+AyRa z5e^CSdYTMb|Lk9uHP`RU`jNjZtI%JZ^?-j`mdk$+*3>YnAfI<7Sk)l7?sG&g`{(H7 za@%aG)IPXK>A4Fx1V6ihy-WLrv<|=vd+;;+fENkD&l37yT-{#aS8n-Q?v7nG2?zC- z-)*urgHx&Pk>@6TK=c? zG#P}zChY_*dGfPKVW4mWLWSFSL2}0j7=!qwpF>i`y)Zxy+e7?`(29sAlm`D8!hwCf zu2j1du#&c*p`B+S>uUI%nYhl3z0#Xl{#52kg9D~9tKbr|dN_MO@fcztU1DvbGNUlZ zcDAh@yUya=MMYFgm{4(zNt}aaiC6nwIaX-g=)R;3+u}FO$Kxe-%vn2m-K}e5 zol8Hg~czS0D9(R@gZOlN&Rsz8yk(`(kU(Y zI}fZMsSN&Ex_%BGj=ukUSPgMxF@8pYR&mhwfpxkuE9K%Vy}=H+&+rG(CA7hUE`j_W zkOPhxlRB>+T4f@f0sWOcxW8zz#^&;njnq@}$#o?PH(uBKN!i7_h?}2lF91^A*l^ZV z>JD!4#>U46Jekl4m?-xa|EH+MsVv8#Z4LJ;Aet}K#7UXyLCox)~i|Gd-)6)u+nWhot^ za%-O~g?^d)tF4=62wW%EbQ=Prut|ZYqHK*8`d-B!Ncd;1&7eFlpo8kqE&%ZBCP|2}d~#t&NyRpmBlW6=+GjkIY8Pt)x$* z*kn8f`Q632MkJyGT%W^Q1lcG>Urv0|-Wu>8+eV^Llo`uSQAG|O+C+GC!FSNZlTq2k zN3ysQ+fLYXA{a18Nd%$uQ2_0OanQ(c(9k!x0TKd(^nu`O<7i(3R+!qyi=ardd9`ti zIsH-G#l?F!-#E==n}pw7{rUr3*WmXgekbtz0>7a^Ob)Kw@cXNNrEfa@VSZp7pLG`# z0;40JX4YC{DP(k{^H^jxsr?{{qm{S*=6dJ5d zBYpDvOJq$|a~GR+D%#q)+4AyG8w{+>xVTGwJ)lW#D5?Q1?&b&CNBHQIc5ySkGw((P zh_ZPq+UQ%VKZTdRZpwN@BvaR%(x!smS)%HXi)q*|)jz@W^~mk(AgxOEWKSx05Ahcv zqL6 z{rRS_t69UR@QG_>IzgGHQ6@4gBG_MOaE$^@C zpyN&XV&50P1XdDC?>FcpZwWyA%_+11=5|XIcEWa&oL1?b}Jn zxE;x;hkgof?t6*eT*u3|$$JarHKz=q9jW23_(&_3sF_sEs9gS`<7EBv59yzo$>V+f zW}rr_iP%}-a3Za*qe%s_YBknMw7kMZmR|tkhkEw|UmKCw*soIkn;=(tuU&b%8#U_V z>HI8noqrt0>DY%t}01J+a-%UTCct|Kq{XeAoooOq-_u6t4P4&G-qYb;O zdAxL=j{xs^1XT#I&=-lpW?DvpY0$dIt$j#exjIn*AiWea9@f}f=#%`!Hc?UF=ROL8 z(7}&Xj%Fr+f9ffKFA%d8B>?OWa!tbSAt-jwN4*~LwU&RibeslpVT3i$!zWapi;JFr zk)GlWq+r8JeAxuFOMkh8ULuMEe>p{e`73$}K{A6*exs{z*gr}3=!^I2Zu~Y>27_{}NE# zEbi9602M(e-Swt9Z@qLnWSsPXB`4+#MoZ_gdK~0WE@cjNPsjI z0E3F-9O$X>UbErnwUp;=0;m7a`;x`6l z^)I+Sg)4p2<=ge0xtK`$=Vubt^SB9jZz4&%a-10(QQ#)&`@OFlAQy`R`^q+2xGN1U z^p~$X8`UrQ{UQ8qZMcZPMy(SAjv*9;n)Nt@n3T;JkD3L>Fd!80i6O8dfe`aW7}@|z zk3}R`h>T@?8OdT)v#dPLwHXcj@ZEV#s$U0k4G-(HPvKwdc8T21K)P3Z0r=s?<4oan z`?JtK>cC+B62%V}2j`Tm4ILLDw$|NyyVUSgx>;v5sH=?FWu}K+)EZFOYCJT>r5|s6 z`Vw9=KHbLul0G5}`Jf9mp0-R?JI&2X>0 zzt?flD9*6^(~UR*jPRxPaxc$T7#?v4LWH@&7+#k~`DZFYlNxgrA!p;W|u4vfd4HbdZgHl3Jn z_^!T>r|Dw{|M&@;{s?^p?x|~xIyNAN;fYtV9#ycX345%JG-Y*{5~YUo0IzQPW5{Xn z-=*u9n(zNJmlm3TN>Yt5w)17+op$d%n7v6${>!O+8rL&O^)7U9UvK{j#89`3I>X>E zjubwTZ6WI5rKRuBP>FI%r7c0XFBv7n2d~ge+4Ld@!3)V7g{Fp4meD4O0qvD)~=vLv#UgS-lG?45ABY>*Q`g4RYc9 zMymgu$`#bTnp;)zdy)tKgT8FzTmZ~U+{tT_Y=IGaX8#aiDpouG8K_bv!WF0>=j|Jz z_Mpj;W0~*@2Rf*RV5GGsBImDELC8S^`tCDi$mD$|)vq8qgQF?Bo%IXx8Wcx()+z2kzwY6Ko&}PiD#&M~Mari2 zF?;J<%Fa^#7=o0Co>NdtOJE-pyA2U0oV8v@lHNSn1aj(i@~Kv8cm~pN$8Z!9#d&Bp zVuIE40a)sVrzqc(Gz9xLexL+x2BAcnan#zhI|$pbP{{TOeN`oBLY<5->d zfc2asDZH=Uln<;nlJB-b`EEjR6aj2Wsc#>0YT-NeIc44*G^bF0az?3>z!#SXw)+`c zH%68dTaiCpA3@1DY4;e>9d2LURWP9kBV|6Wm&vxAAk^{6m?bD#>zPX8RkpkrG(h7B zenG|wdoL%OqGUac582N50%!Dn4Uru^Kxso}($7K5*07D|UjeZ~D*MlEl zZ;^JFBzjUo^Oa*YA2fQr?!_HXr1hOsp!7T%yWE+EEwe5Jj;e}tvkhdfrWPKGiXIH1 zn--!x8hZ(Agu_WgsizFV+!2@tkwLJ(T?1Ji)`ZCzQ6n+a@%HuE=R){uiMYZghTtwE z_qTA`C$w5Do;1=hYp+8o7~c_H7vKYAuuH|UPVJ*SAQd&>K%-RuGZaFqV-CHitO{Yq zHjp8v4mI3U+A#s-2vkI%?j)$a0J=O0N^pde(hfczXF&Lwq2zIx{c9IN<|W8dz!h<_ z6fRp46SL3`^GQ8`w)S)6APxkc5g6{#4ge@jopk<##E8gFslLb+@bh>`P-OY5iO4v_ zRk7E^GT@EIGHHoR3CW}3zh&uT*ahn5O<|2@eDUIoN1M$7z)rnzzsZx{jieqN=DeMM zvsUOaWOaK58xE)QVhFWWap+Cn-Q|Z$Q4{cspeFtwfPe_S95JQrU#Y(lU#0hm*gQb+ zVXwPX`yDsJO<`?Hp^eOV6Lfpo5aLtk80ler(5*vEy$*lIpceEX#}#%oNReSF^I|df zeO?sYhyscI8iIlJNr4HE5s4zcLNtl$Fc%L*9w$*cm!fw!NTIA0$5XLHZ8!o-ilCAJ zLo)V0qE_ykOnMM@l&e|=l|*GLLN;2iN2pWavw!^b!8hcZ@Xh*0_{Nl#YIl7jOw_l1 zFj3Mt4hOu}U8NCz=1B-$O23H*;Sa%%W9c>Y^UL7 zJ-86ueY|vDa8KVpAdhQRRo!!pSIf8$$fctlcA*)*0^ffvG1^0Vmj2Gg9qJbwO?v!d zUll+W9)MFmNjfPIAWrVhym_^+Q$dF@pYAKPECpnYmhyZh{~BmL38O$+==+vi()%hX&;FPuxH z;uyuBE%l4082u8-ThjIdV|M+byNrRqvohm3C&LKJ%B}O0$W*|I{3&xG61Dj%3973&i|{ zVA2y2&ivxdfdnksD0v_9r97p4V@#l8Ss~Vh4mxQSLUmO zNaV}-%TRITkiDt>>N2O{#65wTZg-X0Cl5|=5{e36(*J^so!7`Chp!=$KwV?j>|Bd6 zfqaiWOx@Gah1%SD%BKF5=d>@lqQM5crKPZlKq}`hf^1zW1~jfE9^cO*$+g``i^E)a zT?G|6Rn6Wl#nKip5J)N&tBHLR;4FU3!d7#ffd@+E%wpYFf?0^Lp8VA^{Z)w%XAnfg z*%j@_v>(k6g<;XOrG>kAo2jQ1CEHVL)8nf{pZJ#J54nV=s8~1|Mq#nIcYky5 zp(T5JM`iQ#Qt}l`b+Vc}quX~slDoABBz?>V;^fnoM)QmTlp?C|x^YwwitBSI5pc#- z;IPkv9DXRt*;+kzb6v)wKpd9DiLAT8*}AHWjO|*pjeSOU93NJQp#8K4G44!NvPaoQ zEB_EbP6!GxXvTj5a^d3+zKY1j8!0FhlQIjz-z|v$uFSG2v+Q1FR#LM8#R-3E+Sgdr z5$9(iH@mFi$R6+s5w`z0pJ4jEqjG@(B)nc%W*{**f^G=(|rF_^xj?^A@1*EE;-ST1U7rgY{~`$_JhzMAA#Z0+qhBGWAt}RJN`Cq3FEKx zE1bKaL!NySIXeXBq^tX$L#92`zs7te$@y3vLIO-9L4Xz@knPLYP|wr(RN*5BIsudk zFF$3lZ<#^lY75(h8%~HP-?>5!vVdvP_N6F<-?~{_i8dCy0T`9qaMC!y{9wZ|tm{|#3v?LX-GCaIwsqe!kvPFVX-kX)7`9SCGqf^RJ{;i%(Xlo$$Sso}fS&D)C;*eGh{`P|_~xB$z;%YoJL zj4bp@BH+@}p{+o6=*&3crTS&4l;Sd0MH~+iMUbwm3?E3B6KXhxH@r~~zmJPp?{YdN zB?i!AW(`fAAf9)eG9Ls$~fgMn=Q*3zP*yX`+ z5teWM+0dg>EmBpkAU_;r?z#+JtG1;Guk(`G78)kma9Bfw23W?&5}uU*!MI%ug9BO= z@U{r5hgOGRcjlO-4Y!lRDF}a+n0Ekr$02ljRlLv7F}?=NY#rk?tkFa9&j^Ia5W=6s zJDj&bQ3MwHx0L4t!bzag_MGPkCyW|F#U!M1Xmb#kmV-M0&bI+MHfB4DQE1k}K((uC zvA4B>2EI{yK84_bR0=0yL7tF1lC^Q)@NUVVtzU(B-{BkQIvlf z+eTL^b5X#ndU~Dj``yxRG(Zl1C|@jHb&s@NDY)N+fG6q%Tv`nY;VG?#A>rFw4MW4J ztp-bYe5)ZPd_$|@=J427!$j=a7#4*k{An85=r&>%7B)3nvS%R%?^B{D!D9q^!D9yY zDCZO#L5Vm<0-!ke)OqI^u1+G}+3*I~DBH#pz=T|IX~0N9L6W3l;Pz=WtKLr2JnaZ=8b{=76Sa`$hXh(I^|)H0DmJ3M*s_&+A8UPR zeqb@+cMClW+dw)LUtt6|gdb;!`dp%okcsDz+6QZOu1)DZz5U1vDPOIk_|CU%EIq95NXg}*hpfknOoQi zXRJcNBd={F=n*sarEW);B>%G+Z6C6d_btg^S|jxhTy1!?F&@rjI&J{A%ZNYF{uX!n zfFlh*pd425PMZztVaG7IuhVAh*?nC!ue}XpcMwenS!r;Cu@>C0yR)!z@&Aizc1yk5c)eoZ=4N2& z_b^5WGHHIhgY{~0`bQYTJq3WY^GPUo81+esCR0*)k$_z*7O`*3(=%uh>Yocp*#&x8Ee(l$Kf@dhRTe~w-W)hXz;#*FHVM1fjl1Pw@zSR?# zr^&e78_MhEWS2+EHiLFIEC2+}sex`7nDMC}EgFxbhkPjyr|LKWQL6R8z5%lf7L^Iq z5>!e4F#2_bz?a-<)ul%m55dp?1jrEaHqMs~fPDlt8MZ(H!!2ostJV#Wy-Qev1G4DX zaA;zTLj^i+0wa93SJ0sI5^vD6l$&8DIuF;$)9=EAA&~TX8Wz6V-cBye z%NRtoQO$n?8-tBE!c=KfaNdr*G$0CmcfTaGp|gKAf4e+EO&18Asa@Sy<&-HdSr+lv|1HrPiNS(yD5#op|4&cM0T zp;W43ZDvCWEEqNXga!N2hbA6+hNmd+Q5T5uuKTyytIoF>fl{j70`2pu3Svssek_gu zh*3w5Jj%~0i<-^M+)NtIEvYxIHx7;@^U-R)T|$fc;b$AxR2-qVJwyc9Uw!ns&oDqBm2)c);SISv8Wo*V0(oebn?1aFJU$3 z!jqmM%H_aaU1wdS&ec=a*#(1BYR&~->NC0Gnccoq#FlF9#78h)e>8%Ij@9=vz4b-j z?&oizvDQ~>5hXt#zxSuNWW=TckSYVTK5F4sF>jFlANU5EXh4&+n-&x32-dV!i-UN9 z*jz*$w3gw=hencNhBBCqsn`BtU8ox$u3%1AYDr;>L{&1-jKN6JT|TPf5i=sRo+v^-DWOwH+<4LtJ31rw0p8#Io=GSFxE{cVu6owmWMq>zT+|PVKXxUF*gAgQ{g#O3PSINr60P^ zt-=Z&@eARr4G^$I6M=iq)D|*tBFI>Ii|LSYr1BkB(}_+K>g>Q)QfL1=`j>R)eEh}& z=ZYmV+A8W`G3vh$BnS0+jN6w+)5iBI%m!{84e#l9=_6C0YPhC@2M5V;DqF zrlj{EDN^M-)7Kss62WeMNEflgv`>H61A7IyitV50B2y}t5A^+AUoNmfN+yz0C>|Y1 zaqtEYrN^<);U4y&Sv7l1d!Qm>4_Pi;P=+5js<~5cg3FQ%@WW984)svniP+_k4Np9X zt#Z8}p%#l!k*)Bp|rm*$K9LJCN3mXbkdDLJz7i8A>%LcrC*_i z^3La3j8IXA;^+(cm@WDNp~4L9=;Es!=v^a!6_$dE)pq|g)Om23q)WPP*p}Y^Ebq`W zN}#JM8ym*IUx>g8Zz7%}y$;3gB*fnaZ*kBs7M{CbArmCSeNocH8*gy-)DP|(&Dls2 z{hTuBIb-1SHQ*)u8=2d>xc6f&4`F*ryvpXm=U`|Q)oHxHtVjhcpL_=K2@%oEO&=*J#X{ z!*@2m!~yTUP{>>lk`BdJ>vqgFe)7b6es6Fg}(Hq(2WjM$m8WKD)?6>=Q6k{N=Q`O1zco!0hTjgo~;^n^YRtykrIR8$!%G3PC=DzRqsFE`aC_mjQ zPxBYc`@Zj_?}zz!x>cU$FWmG!#`h$3PpBaS9~ws@!bihg+`)8B**2B3eR@6b7WCT? zwbrEUqL)zBm;f^P=iKSN`=4^wTsK{V&Z)*KilO`P2!$6(t3oQ8l2uKBj)_V)YPrKW zJ3|RWs8e{Uz8*m0)h_bkZIQd}D@W_EJ`=B$hHPY}66tCO`KVh6lcm~U!OhrWJKILc zFH;-nEtYh<>Gdv3%v6Hg3#E_9@EpFV4TNU5)`Mfcse$py%{_i>$q5zf_WZAQN@U#!Bbj`0vf`p zZta7|E?z57X93KHvScluOdw>~GHz_M`8**WMP9NBeq0B<+N_LP_&^TC4NS+7DE2&I<&u1(mtA zFSw!}?*6pRBy0oN$pq6|a&OAoET(c|I6N5SK7C>Y_H9&{4t`jGU!tn?5{sV)u>X^O z9@x&@Mx-rm8bzzHeI!Bl`E#Qm_U)W5(SP){Bz!|v3eHIlRm*n* zYy%u{1{lOQ;W;?a58m$1UhMO z*84?P{X4;~J&0F0%4cy3q&#v4^j^11FcZjQPZKAk77)rxIoI)Z66-0`GC4D_;}{k3 zBA+JSj^}T2Vy=Jul2f`b@}X4Xe4$Lc0adcSSm$63C-C3DTXoR1x8FQK+Ft&d<1NP- zkj{zVNlW4Zyy)ULJOVRh-%;$a{PDd_KYbqBD|q(`cElAGKlC}$BTxAUkvyM&yF425 zd=<&_;kV245%P3I@|^v4dCnnEdnC^n-!4xWdHx>BbMo8e`51XRBYDn$yF74vS^h~R z&wqZqJg1STJCf(rx6AV>@_ZS|bMf2dxr{uQL{d!Uw0U{$?x!8Kz3Q)1K(T#_>5mVC zt3;NgT;34z?XWYiu8l){KrH8J6kJ$22J4l&Th}$kak<5OrKK@0AF&@hTjOER-Y+kc zl@J0m^sY{(bt!3IS@7oc8LKdzF8XDs+;c^K&-|E4y5 z8~Ha@`@MUB{A+#{^6#ZfWIgadmVarMj+Tf2vHS~om-fRecl!HO(_Jub?w8tuC&cN^ zu8LI)>$G#&$ig)uy64q#*k=&zaGr#zmOgfFOlLU|M=jm|nbiHB6%c6I3oGWdzrd2!JVr0mKFaLAVrt9wHQo z{$1P=0;km_VE*jFjMD#n6}=4Os`qQJpOBvgvD|=rEY)pO3sMv}IRR1%uyU5t#xb`ATLdNSO<0rWn~ix^)3M^frwcKN1Q2>#>2j^|=0%qWds-CfA=@wQ&N84q z-vaeDJkyZ%^GplsI7Eiw9xRf}v|8Gu;WZ)!RoXJRQq(7RCihWj5A;DEsBzGyz9zww zP@V=scTJf(oA#kt`6(Pu=q=P}8JOwz_`Q=Fmdvy zyt7pM;xmHDTIR+pw|0V_P-eara#be^Jjg!-NvGs}X{K(uMl%GOXlF{qzx)p*|1=71 zgs-4lU{l?QI0XX{i{gf5oHdkfCL1xOAWd;k4H>e~lZ*FQ(BXso+rz$sqpM-iff31| z2v7&Ip5(JuVb9S&D z{z#(wn*)z()AyqV5vbjuSE5k?cyw}3A4cet7WcOEQh51f3kbxQYBwy2F^G0Xs)!&b zQZyRq26objm4vckXoCw!k6sIvckS|xD1Z0j!R1r>oT*q`#VziDL`>)70wd&y(hT(C zPRu)tJAO`oBZQ9hSBLqXuRc)ca+{d3sQJe7qKBP44 z#7+8n%uG;Ao(MJk1&^E_wzcC<*+z~tLk%zEFHwee0T4P{rG}S)jWSOa_D>+ns`OcV zV-d(V&q^JrCdULXo8emGcx`&WyuF3MV7x4Niz$UOY>^W-^&CQ5ksN|+gLUtyvy3h6 z=G*eDuMi~C?k>lhMDnoFJ@!dYuLnveM=O?m*L?&Ek17@x)N(6Mc#O?;MYTA-)4Ae1Z21`Dwn_e&TWVP0v* z>Jg~GrbXmzNcjn5aw{XXT9G2k?48CN_htkpSJfL)hUTLLp$z5UEv$_l3U&+&eqlAW zsksS>{Vb=yedJMr9LtOLQKRAk6>nq-Z{?u2X*bp|ronB%rl} z;3>3hyb|LdDPOq^Q_z|>41PNIQbBY6l4guG7+K?%6`tTol(Z*A9+4utHwNhcyzDaB zNPPqZvlURSN%e2jE!-u*!w>DDv-~8ouj9uNi4P^nwdUKT209-If*ZqTrj=pwI|mXR z%@a9k$cpyJmrc?JnP;CIOU@0P$ExFSn))NtLFUQuVw%lp2km$=SkVHdc9~MUNZKBw z;lsx5t{uSx(kXI)g{He^HOlg~T!fz1EPJWffbmh^HE9X>>FSk`Ho15`Ix)?z} zsAw`$<8ljfLxHO_K0+A6X@SjkbW@(BG;YR?;$9n_TkkKcI`iJ37N^dM4|bSLd(>QG zA}~%q`Pl4aTt;DnkOGqYkXs52Db)%<5+E7g2oLr~5o*THpyIGJA{^{$3*TTcF1Itc zz0o=iHx*G6FdgB_Pli`3%H+I=sFxHD?rxTC!)Z`2_IK8Nh|bGTHr85=t8KR&4Gb zq`P}@l0jaFvz!e(DN(+8`cqhx!50D2HoU;oVjF$rGr3)0AFiYdKHj+sn0Zpmlo?8S zk5ZJ%k50(lpw}0X`&C-jWqOjN20E)k{_L?$vUDL3?t;TQJ4B~ev*kwP65QDVe5={U z^oJt)V&M-!iyci=fO;@%h5F3{v+Uu~DGkwOL>_nB4Vzt~J0_E%Svh-yOl<_Gby;{a%oxhJrK$rs)qcckuP+lBYW?XNo- z6LXjqB#V0s-RtkoF^6fFx5$F6(wIXa?nb+49*(~xOWS9YtlMa{Q9C?%E{!FW6h%_; zByj>u(W&AGq>64?JU1)&7Hum^Mj1`BNf_5l#zE!PIP_3tM|_NI>K7f^MafOGM-6}h zUFb=ce=^yCLp=GG5>}iBQmwm>Ggx$SC_e?5G5}+Fgs~^_hZUvC9hTrpD?-#*KpT$} z=4(`6?P!GZs4=s>VF|3LJ5JiNN1Yi%hY*A9afxkm?J|SkB=1jYZm`X?TsY}C zaJWM?Cj>RKX)n#sv5q#@s&|k^5jJ^-5z>z0DcTS5h?_KRDQi}AT!eHWM{BC$q}SAfGH?izhOb7NT@z zoU^kaBe;<#na#1m>0Og`I9mdQV_!u?bus4Kjrl3m_d90VQ5kG9bkyERwO&=vxc+)j zW`2ruradsMAK|kBJlZrHk8T9LwvQrMDYwh(lhoS38F351$#>CLypJnT8R*dq4K@ra^1_*8n4 z1*Mw~FmD8)#4g4^3u{DtB z(e9o@C^d(_BgUgg^Mh{5?ybgZ3f#)cHfMl!88U5jW?EFbW81 zd&t0@a58FPg^m?{OHxclHC9YtZ$l1&SOvf2LvW;49CIU}8rBQiYNF{#XLGRkzKiH4 z!h7*NWzci_pl6(W9FTsSE{#)eU+5Qx{ccK3}x%UD8Xu0=6|M2y_)R+Dt6o5#1JcqKnqe`)3!i!u=G z5HP{&j0RT=0FrC7!Rfa1yIkDS5}rVQ4p&sfTp6Y#FUrFcu$EIEPolq@((sq;T`17dQ$qTM%#S&bWd45P zU#v&Lc%WB|$hP{qVMhpu_0dBt8*3h8K!QD?Cyf>>|LmZ>YmCvrNGKkQ(ntoLSVFi= zLpMQHmuG>O8}0t1J-x;mcw>aUWk^@fqdt~DS#S^7H0EdZvk;)j%h?D{@42<=H-Q$x z%gouP7HP*Lh9#uP{wH|D@?;wCn5lemcsB#E2&fJ8U)9?P_G0kiYX6@OxfsX+v? zy6Fm<>O53C4jrhtma{$1@^dwgrbP+8P4QP-Dt<$)!i^lXOdE?GDT9v_y7a&ZY;^Jz7B2o*u z=HqGoC8EZaIFc^^gjR@CXkLkG9_gF-a+A5NyXo+l+;Z93x^f8XitHb_{21S>Y}#2g znz{VM+kOFxGZHcuVO)ed@hS9^_$C|<_% zE5(bnHBXn?wg*^U<~U5nEgJ;UWDXH#>>L|R zSCJT#7|mYrB+j$qNzFCDUAL|#;`0}+*VB)S^L8%(bdK_ywICQPFa}V>8$O9Q>eAIl zTtR>aPs;NiY!D6KC^M=Rp{$s_t=t8co`3j>73Y!&Z`S{|gVu;P*S#v9q z7aE)#n5E4UBs5 z{VT4e^17waM`fV{c)Lt;t6UsoC* zd4kW{_58v%^5!3L1T!~^=I`k(_8UR|Uy;D{C{frT5(j)3ZJf3@XAc~0ZXCW4qfKg< zkF2`3Z*U1$4-(g)g5r5WVvtCYL9%e*C|M|m3C2fBnJ4`?4H0E=&t?pW$oP1M7FmPG z$G_;~<3^JhAK<9}2gV0#rXd2tVa3Z8*J`EsesmwIGGNqH3o=^ym+)=X_^XzTSHk*I;e`tueSd@EeRliVy|Npf^Ji~L1DG%NV;Ddk;?HtTg^*BaL6dx4`x2!SSZRT;R^ZLoI5sev&m1%4a)Z zx43E<7<^%h$0D%vxUOGr*PJwLv{zsU**dFdqUKp!NzLvc09UR{*BN zr&Ra`19mkJm^BDK6X0W#-~q!2@BofVnr1OqaxM@h?vcDfE~ja}bY8~kp;gJwv;LKt z6)FB7XI4z`|4^y0D-{-{A}Qn|w~tr^T9kYnR@2H7yK`@iSWx4UwOx4cy8ZIWSIXqz zgJnvQxr~+^@!HN@EI(qkPj3MGfPC*t^C(MFwb6IeqgnfRcr&`6J7nb%$XK{A_apTp%EvsWd4HE`@f^`tF+^BTF59*)ZwMHV-sJ- zJSvtkkMm{B6Ji;&iIy=+@mlPSELo^LLEHw5sD*r`kDJJq@QwHwDe3!l^Bs7^tWN}D z>oV?=8e(uG*G$Ne8oI%Is8^g9VaA?wh3}6+vy@q!5=aQSQrw^5G)-}8W--jiL#_$S z@d!;%FCOx&ai|)Yu{+&9=f}13B)Scb3Depsmzt)(O3Sk)$d#R z-;8wFXH`}uEAaG*U7{svT#1B9zc5J-is;ry+m#}>TGp$0un9J=K*7FQFh*F3o;>`8 zGDFu@C`%?Qo(0U6%8FB}V&F?PqA<|k2o(ZX7(~Cpu77^69E{@w^=NyQSrMysZ>k$W z6R?|TF2_n;uFasMcW5mZ#n|_;A`S?&2!2FD$u;FmK?V z-r(`c_apBW`-9jjw|d~@blpVe&Qhl)$X8NUC40GRer$UlP?l#EkFT*E{oJ@ZL#zxU zx-fp_=Mk{Y4%h$i+{D#td|;63U9PcXVEhtyq<}-f#d?wemD;4tS{fAjVo=nkX4cZ6 z$mfFsS~aNHeQCGoswuD__<4d)G*Yc#SW;i+Cp+K5NLg*U@E+XQ9}ZqJ&xDPbd@-%Y zB%hSjl5Y7@{Hl>2rC@@xda6c7@6>gaIa+O})Ful(4GnhcKwKmJ(eIwCN-EXH;Um^s zdas4D=UHCEarEr@G~7bjZp&VJT7XXAW`@KsJZxpMTssvEp*!F}t*k6Xr)oAgmE6RU zzeY5K%x)6(00u-qm<;k$q4p`7z@>nrA4BJ(nV&{|#Vdk#4&EbTNtZS`;!IM|Gr|dVrg>?l z8y*S1vYJl-?YNa!)28b?bcfS53EY8hLx>*lO$@|^;;n$6!P1XT9%BCxW>QceWS|{N z-XRuGY6JyG+9+yPdnoT1wN~y8f{pA}$qk&9)lk8;(J{bO0F)-h$R2FwK zZxV3#6p2ISbc7ZT_gqQo=R@(}o`#MWhU2{}CFFvT$_*CcVWtJz=|$OaA-@GLrR{rz z?G|=0*nwTM^dqm_h<6vx!2in{cC_wesqT^mF`gh9mUoA;lji>w5l@OxXfSMM2SM7< zz{sb~&f*ExBd~WyiE@&%DbXY((kVtju6XU*_%Q&7Ckuux7mmuG#K;GS0AAQlC^v#+ z(vPQ~@Ax}l#dIB-nUsoxP-Mpkctbdwr0XvP>L#LwZ$h0?VXM0gXC2d!G7!)wqQ7^ z3T_xX8w>xlvHW!7;xAco5(dJbomW+jB^4qK5Oy4W_G6;{_;0Eoq$TPf%?=_i9SR^T z$Kn*WH;8DmIo1eB*KB>-)gYKr0_DNge}5EhKMcn=*F=B-K&hau2;jheb%li--JG;v zpEjt?;b8)LkTO~beiu)CP6t5WRCQ&O1;KR0_|brfM`1b|N#_Yz*bMaR-%4 z3Y%khKx0^w-eC#rg?0Izp~&0WV2cRE`Dssc*uA=r9k`q_@Bx zR@!d0WdnM&6JP=Kn_$EsU>*Yk%Mv z#+1MY-O|28ATtV+NXmhvjDS*VIEI6qQK(Jkphh5u!3l?$NUEOXQUURE$dlfJ`0`i7 zJ0Tk0)c+B@f6e%>;7uI}?*tBSJW$F3rO}&5w0H~fDS*8IPX0~gh@HM9xRcfyL-#%~ z1|O2s=#C7PVq5Uy5aJF6lR-DS45TyV>j+ZL*90>I1%%=g0ZK^|z*O(>A2$!ucX zd;<~#H4iL8s$Yzu3C_{SHgL8;zZc*COt6SCmB&rhAA}T$p=8YK#%P+9*JaGp1kN$| zI~+(YUl|tZ{AxU26QRQhq4LQagW=fqK_e}D_A13o@r1YSH)58>Mu!C+w!d(-#6bdFEU!oDP#JPN@adA2BgBM`eJoSk#jLTBhHQ!wlG7wLx8mYRLM0isY>|eq0ZV)r~^VDA- zVo(d(sHexb@YvT>5Id&t`%SK;s85n_LHY0(P&_{(+ZN9&fQw4=de|d%SfuUyX-;^v z`d&fdpQ5CGWFQyGU#4UIdBGU3k$qK=N`va%w$d9Z*Lh}TS|2_{=^F=>-Y43DTBPl7 zIM1wj4;P@EMqni?gc@03yQ38S%U@8K2OkotIWbD%350CAW1##qpnMp<3g{_yc{if; zzNZ&Xbp1V)Deai&`Y{G=yGd@L%@<59z=nJ}rsKCjVx@c$YOgm?QJ_f8F`>4>?b(42 zi^5iaec0pV^&KUa&>oM{Q7$NkTjhplsX6f|ZK6QRif<|;en24zHkQZ>iEoAuw$~9f zbpdaoyiniQjdobAc=>(eAClmXKD5r78LBi%ddUVhu zXJ`B|nI65%XdK{dRAdw^;B|xy_((MDRxxl&t75bo)WeyGUX{+frc%L2Fq}g1oQ>A> z#I0DkzNU+|Jy(+9!}Fiax&mXd;K7uL1|p zcS#L@B3#f|h4?_jweXa*z0E|FdLl9`3r_)_qZ-&S1|d`_I%GJN3M=|^5MiOcH3i}L zcS*$GnSk#Cnscwthi>Hj!E19ii1h0$kf(fyjX@K8(rCAX17x@#S8ks z5W3t~b)?P^-{vqxR-?!_h#|Vai6K(gy+~F4x4VvDxw`8-qRVmxJpZFEL&5)zF6-|M z19iqf(>&4!LLxrjjvk>++fe+dzD}8mPPuI$eLaCZ{q*0*@2}N4xtwT4 z#kmNi&bdW9rqGVl7|@}3s01Onc0t~rLBJHYBQY!yAdY;;LDsD_0)y2O-A^?Yr{dAk z7EZ*2qs>$d?sN_#2#W=pIfB)rSP-&(q2@sE;}r(V31oh~zmArNgjLF3Cyh+eIk@a0ksXt!(>14iB z@S5PpV4@I%X`!BzMoCy6U}ug+48)b>w~AhAL$8Dv z3YmK>bqjC8zw$eB$F`B~Amk6!PteWQnu z@mg1$i(M1K&*m?mGufv8#$t z3Vt7;jMMnVA^#ZsbbYR{|MWY#?I$!5MH8TqxSeG~{pwGMGUUX008c8*p8zMpWYnu~ zXK-_vh|-(uH?dh`@sN?_E?^3(Ua};hxm2#T&>oP@FUd0!{8IlRAUp&A&*$Gat_L-22oBs4T@NlP5{eSoE+*d^fm7i8sngV-M! zb3PlI(cz?cFGvaHq@ir6A@mJ0=)}eT1xtYLI?r02Q|5&U_R71P>}fEWxvqEp=SEP8 zwEg|{A<}(0KMI&~!o%y2_|572!$V%h6bn-eb|BlCXI;^Xm2|~IB|%rT>zfuRpYZJ^ ze20*nt>N0_ow$|Pz*bw5*JW8&!bF$I5h#-Hz24(5X09x_n#7X(fec3wdw22IBfa9v z!WP^tEpMJCSaeBom$}ygCj> z&|+jbZspnuV4S3+W<{x9eRG8ZWpTY~4V`$OCa;+aPS!(>_GhxW6F^Y9Mj87t*lp!z z$yR9M!4bxaVd53TBx3hUqaGWe+{&2O0Yl?JHv-_j`aI}!);yKb@)T6)46e9G{n_MO zKn{CWO?K?Po!$q#W7PVG=?+_qY_8jJaD4}Ve=$aVY!iR)L1@NhtfnU_$0H&~(K0k4 ziJ+b}Rfn2P({fNTF*%$Stesk+JeVBuP^XhULCop5kozY?ETrIzRSkIT{t!@ zZMT^YtiN~$#JeJHeFt4l7uWad&tKA&g`h~PNDQJcg&j6Ffb)$ww+dRkY+E@E>@yLT zg{HQA)5W=gzYi>|`N6vQx(lBNzmV7-90+#oF@=`&6}SHK%MgO34IiOXn*Z>^kC$Bd zB=|)lNGqJ6FHWIvTOX!G%?r`Hd_5*@A8mqj8IRF)kzZrt@?GSejTP?CAyj}%B{8nsbLiciD=qMH0_5+Bm-Ib_BU34bE&rw``FWv zn5gzJnhA_a8)&D5G;(Irr~H3y5BA<8O+wrzKslf*zfB8SRDW`gM&rHE7M+!d$Z?>I zy$3>t4Id6fdZj;{g_XLr<6X7h)`iaq7bS9h#?eVRAS^o$fL={+IS#7z!#E`t;soSE zL15K(L9P0ePP@}T?ll^!V{fO+bjpHzURb*=F(~+M#4b~3zi+4;$O7jTQiOS^Kgb3z z4L&Mo2mY>ZAPd7+U@0>2E!oSzLTjo)>G{H_QrYkr*nWD8vf(A#iQn)%|NAokOSV{1 zXF~}F;H!jfc4KWt#~dtd>&uI8rHEz5^!I zzo>K`T`?AB%#Xz_Kz^j5kJH4{Dr1iR0#Y6_Jb?^5C_^?jV9R@&w#HDNXLYIwM{ zD1X^Z`2{fSjA=gtXgLqmZlK+dM>tgCm^X5xCzmLX@#?rsOhrH)bqqX$Uct{(SqXMJ z$FkYjk<<{bwEP5Kcoev1Ex}>hhivd}QGQC=ftfP+u`Tg{(wGH6l*VlSH<$k{;D6`w zzi$3_KL5J_e_7*vYQF#|eLu8cKZ_CX($D88i;rQ4SP68mzc`&%RqA)q)v8DfgZfwL zrQ{T z_LbuTt7&Ib(Jfg&GrcMO;Vok$bvzb9ki~4$j`{x@V|qvW!@I`KL>1ujm^2lWIV zCpS(s??nFb`5bY$c_cn*hYQBua3Kt4L;iws09#!FN2dYHD^1k?<)&ud7A5x=abTJJ z1h*WDn?+mvWFCs+4_$08tPQ0d*UfYut}#3J`YWC9)FhA@F%8S`osjHCJM+5z7P4WZ zSCzdRchZ+p@}-1zL-5o>Pw}|fhnw^xCw~&zYbeoi5GWr zUJp9!T%c}H19|(BF?PR_b}T{T&`$VT;O$#{4(&r@NeV<2R$c|3Ar>28GS+xE$qTJ< zE;vV3YBX+-1buT&7di_r&f)7ja0;D<)7NMzw=W^BlN0-E>aPi==O*AUc)`w>Ytaza zxD!3|Dyi!o?_$_4mUjH*W!`@F4$jlF0!!r(ewbd`{$w+eAP%c15AV&keHd8@=v~hTh3+%TYs62XFoj0wkQhAwY44ewvE7@bBaGcZ^#hTulw75s%Ku8S_ zp#5AgMU;IIG{sWejw;hQSiP96BqMdg)4 z_!m=Qw`hRlD3Fe0LA!z1YXJHH4ha!S{a=Q2TqSF!YATO@J{Uj&w!G(S`QotJMc)<< zfh=I{+W3<9d?j$DBdP&UCv(1SYT-nU~~-VNkR~fvTqC6 z|F9!dfFP;{2)cGZMxklw|3NgJ9f+n$Km&QKh@i>TL5mU~ruCYLA+GcP05Md}KSa#m z<%>gV@B6mU{6qS=FaR;(k$gFU;Y*oOQ_;6Kg${>=3aMob%7?sT}h7|K{#( z;G(RuKkyl5z)?qMG*l`pDk}>s3o8mUNC&YT98d;ZMcdsXqL;N~%t{OzF&-WtvrWym za&y~S_qSGVnNn*7=rW}Ie6P&hvU2MZY+=5H>B|3m?tR|oVGyg`-~a#l*GJ~L@8_O- z?z!ild+xcMe+6CD^Ho*?EmQSU&i&@nHDA1)KvQEWRfQzMM_9Q2u*9Q^BwmjCwXdir z{|?v1OT1<5r347s|JUt$MZ4kJp{-BC_0aeR+ zp0$rrD_Di*^Hyl~@xcw(IRtc$W9fiy!LP6oL0BCpuDaqXwjZJgbUldRw}%?oNJNNt znuWvydLj+yA`;b+F(|Jl%YJ^LY4tLS!>hG&4W+{Qi!kh8O(768q>wd>76TtGnfBEr z@O#$&R@`w~vxwhuTFie^wp1LY3r;h8;_)bcwW(KKGwwA>;$G95Dh^rIv#20tOX`km ztEhTh4zHgk*K>VJxYbK4qtqS2f@?XPo_W_orofRg_Ag{gR7D^|V#}2|$ey($uDlVJ z*Ol)Hh6t%wcw2#3`g)vL;C`SZ)2(xV=~*}8^&(ATpj$D5tI_()#RefW=nR`y;X*!X z_$mu=LX6!vQH!rCIW*6s1w}>!JHIUI$%P5Wrsg{l=~?>@AqoP(-x46>@aI!g_5dVo zOVT$JREytoA_2E%g=CHP|CBBR(S_+YA|t zB=hmTH8HEjZNk!K!8~t*Sb|GJD($?+X~&H{6H(7!iF!b=5a0ib%@0*A(Dfe|S<9LJrgtu>(nh|YX z%m|Y>3VPEeEFEzeUYCJ5bw+sB$<*@nX;Rf13>e=z70IH>onKDZCy79$3gt?kL4#dXq6!!}STBRWVIv7^#qyJ3w0 zY+4QB+W6=y)eU26EuNwXe`)XCaMSi?M{VVfN7}YClg>np(nPXnxkjUpfS`w1yBXl0 z6arPW>If`O*$_VJnuFT_E|9v$)Nsp^?<9~tTuyTbyM4Y}GGB$}5bEu_=3*J*gbwD& z>5sh9QgPkF2xfwU`tIwt#U!e?|l3u4U z0VCi66Bb%BrJ9x|=~3<@qMH0gz$@sZw8@M-nvBq{A%nb;M~C5y;Y)gyyhubIO~c>g z2p37!d=$8=x5%SfrwW@N#CBP4l<%j97|t|fWra;P}V+14Mv{1#%kvp^$k?Quw_NDD=oK|9E6R>FVge$b8Z|k@~1| z$K0!n!XqEEm7dqZljWt9Nt(Bm6lxGX8|DosZ5zx0T6%D+w#VXdB-B1}AFT-&c&nLB#THBo004FK*ZgR>T zoTpRj<=xxybb@_Ozljins&Ei{GtO-gQT8nikD&t5?af39dBJ=FU0zOF$83q$Qeu}I zq=gj0#-ajwK7EEab=a43?b|~7D?PiA%0)%Z<55L&;$uaQ#NT86ML!;|UVJuTql=(#5c;B4tzc#vS$ZSD9pQz%ZX?XoY5>0KPJHcAMAYnDUo{Pk`5N@4U zeJ*0uJgLa@SyZ0kVMdm)Wvi9(8U5^hu%~UvHV49ZeZZ6!W)5@EJfBFO^RO!NfK;ecWIKjo*OCmXM9mfLN z+6mX7cGh3ZsKZJhs{GAMa3_PP0@_2hBp0};8#h^bFWfGwaO*r$=!B3!K);K5iVg9% zI@p>zsHXuSZxr*>*vw;lJod-7l-j|Yv22FXjJgG8*V3a30wsJUPuU)>?;ZZ3r$_lG zVAMLT_W1v{v$y={&K`l}8m`wn%jGs!K%1&*V&NkFDV-JS-M_ALqKN`9{<+xQa5w$o zcjlq64R2NQgWI_9)L1I)evYq-Ym$lcGHem z_!S!rpG|xhkVQn=ShVx&aP2{fM-@qYN2_X4qIb9S$m2p{N%B%+RT+WI+#B=Qfp3T} z5swN8=9LQ!H1RhB6Pv4}fE^pB0xQ%b0G2nP0UHFcU2o6P3&94U8aa3pMS;2%FV2Ab zj^f$|2>cBUr*eE~K49VfIB@JJ#cNY+C0?c0bUHOFu`@8qAW*D-zYVThE>QK|N9r7< zr+e1+OWjuSIgUMSu+dC)E}=r1Iib&Co^c}C^C(?lmQU1u8!vaH+Tm&VYp=W;cdshG zpc0~-oVaA=PS}_yKDY8iK!)Tb`bIq$P^Q$x%9S4@IoFAUYbN4kXC)Pp_vlrnIz?&3 zB+)lYi`oe!DxT^3d9AyDUhD7yruUs=OYOBlM7MoU{5h?M>ylG5+r!@^%iDlmx`yV8 z!u%=1aQx@MFu}v$9)|aU`vdX^bzjF3Jz$F@x05BrS2WP_$hY~R8uXDm(nJ#Y)=6R{ zCkY&t-6Tgs>!wcuRi1(SO0h}ttM9k*T?_f|mqDv^^9Id+<#l|BpU+)&-?}Qk;@5FL ztYb-zZQt)eb?3+(MBhoCwbxR&lTstdNu>BGC+{yNDw)SQf%n%4d{Jt^Ng4n%Rf>Cf za4fL$E0Cc3xs_iCqD`Xj7M(=hOQ_mZ>`vgySfyK{4-}F=GDsYgg32R?PY^24@A;)B zD!e@86IQ8#<3#40z!;i2ng0YRe%>H}&JU3w z%ISPdU2`0$0yYft4^d=J18d@4AZ?v6@NE#rNKP1_^KtTrp*K(uQTjRBdE?qN!go^Y zR!*9n1~Ty2+V?x;dSMB>Nm#-nlM6Pm>J_xQy04R_JL;#w*b4{ZeUH@HOXE`G!u8JW z&7tp1>|X4DVP)u&+fq*aEtzJh`xXLP*rlc=3j6$*C8$7LAYFmZeq>>g@=LxrSdpV64<@S zsVMZBD^l20#K2Ai;)OUrSC&w1c*l$9ABAu1Nses5i*oW7WeJb$Nk!VWA1l37#=8=2R7G(~-ggvUoiy;6e4m@{%tA@MJtg0rXa0Q*&Ncl~2 zU?`cH!}kMr?RWym-zaB~z}b`>7%%yE%kcY}{10xo13Q9JkHZhQnf)BWe~l$}q|Qi!2m0pmEO>G1pi7vPL}|117V%-YYlV$eFFN# zCt{T>-Gl{!t}5V6pnWDx;{ivmJ(IKwTd|>wxEy;Bk4JB#;vIXC!W)nqcYdDIjuv*d zki8F_5a92Y6n_&{xZN|qjRvber4GjY=|0@L;(l2%#QCk`;49!vIe%};HpeE|%+8#B zvCmzNrn9oE28NEb+y;jXHz5rK;$tbkR*`8UGNo)$`BU|zE9@e-j2eBdvW!do=SCtiq9|!i3 z;ReS|RSBU_#SUT;+R3Vd&vW~%an+e|Et&K{peUbhugbAk2jZ%ND4Imu&u(f~!DUI$4L6;w~Qvo_OiqtX3#kgu- zOaG^|%Ue~VME%+4i)mwr;V16&gDz7*_`6aX!u9d-bjMW61RH9@o5j-Xkx*t&PBUzP zzz-@~7oiPp(p63LA5Nsbn7vKrn20`Wo{b zcTAwCf*S5Q;*(PTD(MYwv)EC3w%J}3$6lk0I>zAXj`9Az8Sv)$jhrC)R=-J?g9-md z;W_@jcu`?o_^5s6canc4-N}{wtK~ZI)GOp(5noLPO&4E{WDmzVR%)f;jHG?>1RgT==XHGh1nHc41R~vurm!fewzedVm*5Z)M zs;Yf!F4<7!d(*QHZ)2tVpDjyh@YUE_&YycG#rPLHYOCR^-M^?R9~7AADw6WwYUzb6 zw{m2@CHvQ;Y?dP$v)nA7AZwyON|mz#j-y?1zuBTapSDODN#?vZynR_Om(@tu;w@JH znsI7xDjP}@wzJBH&<{{5^i={K=YN1D1BXlVTH;fWNTGRh{xaFOCR`u)rm*3IDjR(W zou2MnDGY`QUj8a7Zc}GUzBTE-)t>oh01Vkzetv^gIRl^Rz6G9n3HX4}+M~&ou3q)dIx;NLq>geu?#Pbh0+CneO`OnL~Rf?MrlpVVL z)$$Tz9TIfySi|7&4FcBP(*+d?`Noikbiqg!kRtXf%mO18`Sth0D23kR>-(hx7Xf0w z|7R^9vH!8l`Y46!d*`2B*C*rphm<{|D!Wv`MDX+X1zqCpc7zL?|FqIZZwbW*x!~KOhJ;wX( zfY&9T%6pqmHYyt4{RHpVp8sQbbAu&rc0;p+Kd5B=Zc((5kK&|*x^elt+2TeiCR{t+ng0+dl46aY>Iekls{8o?oYve=I5;JOnhgirmN8pK$zsy1E4`*}TLwu+eDLov{4@nx6bJRPykh($LC2idzh)B>UNMUnFrk#o*Rh^iOa zsQvFn>Tuo~p|2z!v7b#K$3gh063GYP+7XYfvZEpx2hv06VO|ZMrvbmfXMp|UP7dZy z(A0E#{g5sX(qQ8Q@csRO!}~A%oBf@`n*>sskAG$Y2K^Q`KLE_Ak=FH9rEjzrht>@8 zo&?wt`7aD;xIH&CiUWRwh^+aIz#r0pm-A_EEWtN$x#c4KaQZ$ZBCZF5q_pYCF;hgW zq8z9)-t*X5g20=O5P{$3*HHVAsFv{fswmTrGJ?Qqz&B0fV~=F7NbU&&1EKz!60e8% z6+u}C3D!oo3dB=hGn5b}Xzv-^eHS_EM2-@0Rb<=%lG5n&qF?bmRN>y~TJ{~cA>S7E zF-oaKueN^+2TvvXIik`KngvYPp}<2T=E)0*s2pV%A?hhn2Po_FX7i>X2@7(6TExa(Cdj>)faf$& zKZ-2irD1ckg;n4)!0syIpzQ~Z32N7fUG8}4ki)wL|L!^L@b1RH(?B_M@y|l<{GQkr z_8n1LBioGO4X4?Iq26pbKlgNE;;?vbmq4H9!uVz_xAS7R-3!|=q8dvE%$IO0%wBO7 zZ43JpO<H zYGYiwiGqmQD}&d}CACmr<JOC z2F)~PDt>O~&4>E1)?X9Uh0T9_Kwwc#|2vS1&Md#&W{ZCnqpkx)>D(V|pxE^t9{j@l zz9gr(wG2ER_&(=>OzxIj8Id7xbsS-(qC!6^EMMRDDXO&Ly4Y4WRm5L~`02_#oui>(KJ5$(y_+9LFq|K{cvHF5R-cbCCRrpZ>TCoP-o%?@yi0HQe zJbkAlP%8Rg*gWZcks!kCMV_DK@MW{GISXu6uxfj?k$^50LGh z#Io=Z9zO5I=aCI@Jo_Af*{gF2;#Z>KO`4d?WM z@u&IDk*=oucoIiurEg z(I!-L`m~0v`Ab}E;~stIfmhavm?HE?a1mJBvXxJv)dpc?Yo#Y^{lGV9g&xnL^EJE& z{_x#=B|!PY4J&FeIiuH{73-tt>xx=Scw=;!#uN5xWXdk}14T%g+RWnJC3*?B4mT;2 z;V%UF^OHfvg8>;{^xOvpa+2cTjZ+}J6U|Eg0)}3On-O?z%qb6oPFWu7oK7B?;cK8( zp6HxL+GV&R%5R@idLi`M0mZ*R7v)_w2Sv0;&4sEz=5{j~%nq zR?-YVrZnb=9o!`kGX-v5jAVf?Hxlz{(T`4}I?!KBdwUwGz4cmqn_K_W?SbzcXG8cT zJs?p>#pU+_a2G|qupNBQ)hqLXbWy!>k?i1->ca@$i z()UEA%l2pt$eAjks`N9Oaru-a$a<#+r;2XDZx67mrehyHIa4shM58Wp9p`p9Qti?coP=E0$)Su&0E01J( z>12;p{K?$LL<)6o1NH0Yi_gC)&I4b0uWN+G%0Krc`K|P8rT6BC|EbG1gjUqg7aK2b zAqR?W8Dx9kC?qgQzHO}a@92il1&K7j4O9OgX|y|V8%q$%7dGGVtzIX>w4P$!ehnfd z)Nu)@p5%lrYy~2efW6w4G4H+5(TT91nGDCF-h#kH79_)$kel?J?UhGD11GZchJioe zp=X@0h2p1Ap&E|Cy!=dMLI>9>O+sbRTlJa3eWo>xxu(O0yFavPF%gUCU$`~f#Lq~w z$MaNfg0ckmIkmDsS;}ve^M@2yfmH=0Kg z2J{8P2(oHvuEJ-DyLbW5^F^gvHUuk+edItN-l-{b%WWmSi+Axn@9KH_@FP=EPs?az zXyh4|>lu2gm9(6L#PvL}LQC{ijskSZkStp9;%B1b-^Dgokkqh?9fzt8CUJO7ivZn* zXIzd*Fx(2k$LmIPZs9*spa7^nQrc&t;$s-|U2N#RR3=Hz$D=(IPNJ}{mc+>t7tfJ^ z7%lhL)Pip?Orz$Wqs-_)>kJakF7`R%m3+o)Xux+)Xg_vdsN}MV0d_ks3&Dm(F2kE% zDO=&_Hb~Xwvg>cbGtr^FDJLYP9D|BD<%H~OLZMU){^bxfhw*~z<((-X%4JQW@cy7O zW&oA$n9NJ_ViHnAd(%T3JqymloS#l0z54=P%59h?ws7Na5K9mbL6~J}YJ{1BV$F}0 z;ywzWA5HbtdniuD{CNAS&hrEBD}mJPimw9%yNU}(ZcK+A8S3e#xj8W(UE%hQSqTvS z>s=%q9tp5{O`JtgL{xTl{!$A#^9(Zo#N!>1|8XM;*IdAVt=XGodw>lE{I{s{M4hh~ ze~8y?VZe?`&i*@ZjL)U$^tq4j-Z7DS@fZwMgRCF##bCS^ zPF2Fe9MTHdT}UfnhhZR#9#R86)Cd01g~LHMMbxib6m$3wus5JI(AJw91o&O9PvHOm zF!(=q=fMJ%e*~Z`zYR2mAwK{wPs+mfo_vWRB@Ej!$c{^f+Yr+LvpePhs+5K6MKF$S z9Dvk!rXmQpTCn;=o_IXH#21e26{v74V(jcJd@6XP9C;q6^$U>+jODB8Huk?bH#OF= zS3yFiH3l0z^VWb93i@Bzd<_hBRI)*sGrt3{y#|rN>bRv3NUS};&Q+J;zSt^3%ENQ- z?A3hXk&;$&m6Bw85T?JJ);D`G19AeHfTu@Fdk5J6@B&V0 z6t;meJt(H{o;oq~jfMXiEfywFPnm zeertrd1}IM#RS4{Z(OE|!mp11U-I;p!%^vK|FFvrFfVwG+&7oG@Ec_Pc}27qn>h&K zvmHFOTYR1d(WwBIEx@KUiv1IA=E573I}ah%w+!TFt`E#f4| z(tq099IHV)$sUQWziiF73g64Y%BfX6)!h>>(UO3e8TEg|$3j&xV^F(EPOb@VmJ_ATZS z+yitJYl3xW8}z2Tt`Una4kUUjW1UjvAv6tR!3i!&p-;RO+AB1-B#v6T=2Zv&-uMds z%I$0cRwZb^8#Yh_Nm3=jtV-YjvKO9PVV7swZFJ*aUP4s`OYDeLWqv7+RUkFM2E0b) zy%1MCm3}@kE>%GWT5ZkQh*B!sQ9Lj)7^y5qkOr}#@^1QKXRX476@bD&wVhlR)9fmL zn^kL4|cP^jtKDi!79i zR;Wu{!p&vu5bCXLMFe%Nq=&MECz62sx+AZu@Hjj7VM0;{#P^{<`9#40rOGxG!HVRh zZhHY1P(JcWAdQTvOZo`3aTz$kK7EL4fc(sB=x3=Ro@&^F#!jDYc0XA4G?5QLVaH!xWO9foRT9S+k22cf{JRq9zN;r&z_#(eSA$g#j=hg-S zqa$zW*n!%PgAI~TYp3+K+Nqupa!;4}l#pHQucPSzek;2Lzj8Zj?+_UU{mDJ$voRLE zST!Ycf|q***aEfCV*WcC3RF1GevYPt+&h=`#TrmzlhRSp!R!bG@fF7gwb2^l78J*x z2H+|=-pl(yGcCvxp5PCrs)181M6^omyUWN6yb`P^ItO+7+_0MP|Kdjl5I z8&0*?qcrU!Au3|RH-kiIW`2g>V!G*|aAadca_C)j#i8)2UEInE@UJgYhm({gYcTgP zkqC)_jsbzIap9?wzm4185lWn{WF3W%RDLM*{gr~bh2N$k-qOj%kD?(_>BL;Nh}wu# z_*fN>e3=N;k2Z?SDO2S>6ao2AE4l0{#8VGM=DY%Sh(-ea7=eC-l4OP2S1PF)JkL5{ zJYd39drV$J@p&BsvJnB8rehT(Yv^ezV0^z%9wWR3SALudmLIyX#QD0N-S>`RjoBaj zfFhVC7O-txVsOeQj}s3uy-z$s%1(bqdgxP3!v4^43A?YYUL z5ZS)qHnL~!IC~LlDe4W^DcUShg(V$6QqlU|RmdE$12|kSGQ-F9e**A+RhjlE0Z{bs z6w4=x{(I4ciU+jgh^mk9Eo|R#HjN2J?}wW3UTnp$To)aqSoE_>rYkCglIgaD&DJAL zcn~eolAaV5mNTP4_`hyn;9JTpR`))g+qd>F8h#~nNK}7NA8r2OMWa2g6h^%;EbSPO zhS^Woea_DgT`c#-)uPaO1@iT~MvU_=f&%wLc_6OdlpYpFJRSM&NCNmNPM(NJW9XwR z>e{dqEo{yNwbFd3_PMGmj$$WET~I1$$7_HTKAPZptQ^&K3{l3P?esR-GFRI-ym{k| z@YW)^@L41es67PtIg+=!Pdz@Wvf~gNv5moh_>c$GAd(W-^F$|)Tg~H&Cr9>~!=??|@tv z0q{nTIDQXcDzZF0zGqJ`c9NoouJeE1MSi_0UV)N%cT|K@LC7|ulnT-rn8Tz}Fv4PR znlAv`=k-`^GH9 z;T>^r$#5wPuiXcerL3epJdn_ezeZaWqhuyWMJSnpsIZ)Qt0g?i z5}q6t2E0(;Ntu(?`AA{&c@TyBp)>TO zFs{TL(sphF{*DWFVKRzu`b)|MhDxLabQ6$>_S5a2#a!FR!#Jw0!z+()!{oa1y{hb9 z)qZ-^sVI^G%BafjU0Clnlz=E-1X0k3OK<#!kX7H`km-s#0c*(YX$kj=3RheBZFOeP zD123C_OgU~M}dMC|3j2alw&{3h{_rtK zlulSXbXJI=eaIU_m2UU*CQ)NH*;DCMe6XVqv2}ftE9fq}fgv@9hF{PvBna9BPy(wr&3hDf#)|!rlErJ z8)3O;v!wANLP>XFt$xB1)sHZ@OO<=b2jKSTc~s6^ba*$xnX4@PBL$C6vDsGJr&rBi ziI8=^p*-~M2bB4TkchqTadruG+L#~V_P(CSaAkq+sQmE)r?+E1A&xR!j0{95XuV_p zdNnf999gSIcHrWPo|Cq$RQzjmWTP7SzB#fe9Jx=8EHFpzS0isUM=~|?QgdXh8kuB{Y*Qm0M&zACoZh+k_ay$U#J{)kZwLN; zg@4EKuOHBRA^xS~UjYAZ$G?a0?{WOY_+b7`IhFYbxdd%%YP1Msef-B-?xJk~4%UwI zU=y4ToNNnVolo7(KkNKeGNa|o|8#vS{b#DzJBj}TKeBOPuTt zp@Ng(20-8h%(uTX!JVrFoq0j%CjNY%&`n5Vv8B)tz4>7QNM7Yz+7E7(XS!g$uW>9D zK0D10%A>*SR3kde7gl*a8h=XWNUdt#7sfKH7rDit^}j|$VVj*(tnQ63ex*4JHm^q0%yR10+(Vd9@| z`GA-`(p)E$N&^enjUW@qF?h&NN?;$Plf31{MqNFG02>uY5Z%&uodG)ypmo2cwXck| zgMp)U_8zpjYQtGBHWe*<*5-Sub!i|M^-R(X)(cDh*8PYzY~J4!WIsA=U4QBRwW=@R z^;y&H;aW3AKv;C)@gqs2a-u&nHcWjlJr2`Ir9w{X{EuuYPx zY1^b}gB!9E6N_LDknEMFx!5-^@mUll#`T2smIAo>$&$Pk@*O662P=k}9p^{u^%8j4 zgLd5fBubvH=Vud;QSQ?;bPzNyxfKoQHO^Ce{fQcaI^QG;l7A-%4;)hNSy=~HnhWT3 zP`w<~(MH2%)G&cLFIzN=_s(!;r9t`*j1vH9f4u+_9DssDO9yxVm}Wq`0-5CjYdExd z8njb+(XOH8fI4I1HOWPIT`IVlz$;}`0p2-QZ&`!ZE13D}?F{PeXdj4u(_Dh63nk6~T3-rb^NXXy z9AuOG3Qz_3&lf;1J?=P4pN-v|{9h3EXHArp{ zkUVKF(c37IVS;1`vaWE60d3xndB&#!#|9%TBQn_vEm3(V zXvJ)Jj$=fH_F;3wU|ZS<)r^S+?RRkX3}~~=g>Y*`E3^~PYQy>=vdRMj>ad1!z#D~4 zBGgXs2`>0=juI8Vk5`xw<29N}iV0tyfbS)9p?;!};EebE!hrB9WL~jX9q!L@M%+Ds zg7-}|19+Xet^Njp4~&k5z`x>@9SDp!7dlN88qBR2w!vhOCg?I`l>2O?0Vg-;V!*>j z4q5*FNr!zEm(d20t@aCHf4KNDbsHznR;xBRfFAvC2 z2mH4*9b8NMuJT`@)-?t@sf zqrfxKqXC|v$9p&qRP^0!ZYjxVsVXxT`s(4n9pDW#7s3q{nlFR-O8r@Z816qKquggV z4R=036T`jOh|D-8cm;XB&2gf_`^<6^I?piR-JKB&-u?pKB6FcLMWNoDJg)$}+6+HT zuV@UYRR??pOgcMBIh-gB(LvzTWv+d&0a8dEVGxtXydOmqz~2YYnL2Tn1`pZXtOy$L z+>XpE+AbzRC(zvg6u=SadeB@sJq)d`YG0}&Y69nAD+gz+xx_i5gqXek0S?WDJ;*5c zX$36J^C5qU;rzQ1c^@JTWYN^Xx@$p}m>VBIYwCKk0bO=y=<)@0e>Rsm*C=uIy#}hT zBTxn~j;p-pFCp`2tDFgzNY-E>rRwZNfK`9w3! z-z_u2Jj{T3`t`BUdM8@N;JL8{tdmx>21V63Gvrl<@A0Lj{>2={>X}Z(9VWiW08vcEGozt6*e{Z79udo@m*#`|86z9Mn`p+3ZD4~b=?EKd? zXi(2ygL)4d6uUw1cq5{SXwc{`3$-lYAE zGJ8a+LzzA4;EQC`uG{15`F~A_gkHUk+=pGg0ty7^RQlq!0h5Ms;{{QSs!l`JhH|)1 zf})?oyP?mk=3(w1MYMk!)faLpvz>3!MnN>I>hG!0K@{TYr5rq@HH z01uRtB4Q(DZ(|C6;5|y-8oHkS5;y(ecq7lHETpqC9G|YIAGxt(2Pj~)n(9^-(%CE2 z1ACE*^P-L&)2D4aSy@QQTHyqvFt56zfvVOD>%ied=u>(x>PDaP5svnurBi%u zfncs8lZ{KnM@Nq~_Rtr2o~NO-NW`*q1mTz|2R=CewnUvaZtz9Qi`9FG{DB;5OZmVN za^(f{ASK*299Fc7Ojb&4E5I(vZArxJRb$>3F>xjL$(xz?V<4>4q|ix0qaPZb{4Zhj zjmP|ys$?K9l%U`>AdrL|_ZvxxZyc+k|CIkQAp zLg%s?9(0rrDk%Z2Ubm;CgP)R@gb*60h15X;eDZd-7ClGxrz_RGzQ<63Y)kjK;3#Ml z`v^t!E{Xi}qx0VpmH*G?{HXjEk$I3>`57v7sJeMBvPNqZMg14@BA&`hAOiC&Tzewi z-v5D_(5tN(!}V&<;zzA-547Xxe9J^NyorbHEu+=&TRa@slB$OP2VwH)7gu@-USw+- ztVTb{vpGY3m7DHnS$NE-eD+YA=>JEe|BF)4XgdDgjDJ(`?|%GSgnzbp^*?)@!@)nD zDt5(r>2?-GKY`!7(U>a?9JvBIo1M z2jE32bbqh_Sa}B&ZeuBE2iC2ZNj`Ua!>r3-gTwu5r|+4S#3mrEdd%?4uXA2kdLg`E zr|-P~5G;q`h&sLPe!K^8yyRZc)1T1|+|MJGqszw5M45(+VYX`bb$DDvCi-$lf(dorVuP(m?;KH5}`6u&m3J;@y;np!`<8e4F zUr&?q%35K>ePuj1;>Khwc=I~+cw&QZGi(?AwW%FJxkJgX%?rr8S!p`)9v8nw8yX;Q zz{KbNs@2=gDfKOJDH{SM!`YNtKncTb7jC#V!O&dhX7ujxiQ%F6cP>Z$4P&lqvZa2k zzIPzIpKHH}>uZ(#raZQ695sV$6s3JfR~#!x_BsEMed}9J$Df8VX{Xz&rly?%*Yo9N zn{6Q{Ahn0fHrh&iclb8SzKzW}P#0JGH&2@~@4eDq4H-#xspeVKQH>J_I5C>evYc99 zH4K8;$O!?KPy8wew=kyS1Hfz7`+(1(&4f=af4FW$bG9iA{0jS>1C<@KOVj5%%YGwg z!8X0LC+?&1x}5xO+Ur0AIf)sVvNhN7VID4Xd0EC;fF{R_^pvgPjo$J(_6}R=nGG-_ zmV?fe4{^6@x)gLr&;ix%Cnb^%gF=coMNTI}R-FUGzAC_;!gN+Iq!E81b2(uxr`a=z z;#BJY?etFPU%0=VVPsl)yTGy^cY{sUSvXK#b!tt_{j@ z!tcXr@Yn_POWqE$-V`?}>5~jPO?cMmL9c(}f3bcq;6M182LG92^whIGbOy(JIP=2s zo23^yPRXVB!euk{!dT;V_@C74|L@=@^}kj>I4+)iC>_fA5nd~1%PxTBGA`BP4hI~t z@zl^)B)<9+F%95s2w?-o%1A$F?wH9xtK?t}MzjLTX4;1KDZ>Tmza_4mDq>VIW^ zbp3Rn!kgX*k97Ho*XFuPd&%RRct|C^F?2x8T$9LPf%i^2;hJWu&y#ry?o&sdx8pn>cXrI_k3l1^hRGH&X9No!!*!jUq3j<1o5+^@0RFmVTC;**HK+JN zd$@5vF(~=l`8H)eRyWVu*@<{Ci0#8S9(TqTM43+lBUY0>Nw^KKxX;N|mXgqz$BqvI z52eS2GTaVaC*mn2o9E$FiG9iwT>-p#;XDV{m3xtp%ig2dd=$B{kE6^LKt_>DB6TkN zizwr04>*n$R5Z|LAjpSFG{^xjHd`G#JnL!|F2GYCZl(%x zr^J7pdwFfDfP}c2RboW=WpUP0!xt>00Lj1Mvg7dc?3wo@>V^Rbn|~f%txG@yR6ke- znn-EPWv`$x(y`!SiO)nPc_7}Yk#D4I+wigtMr9&|=sVD;!`F_SSkHMbHA|6W19H(V zsSyMhp5d9`%40(*i`E!gBEj0-f!CKFae6B60p!v6d}tR?+W#2mpx3{iY;s{)KHu`v%zXiDHLfj*fqCDfZs=MY|HmZ zy-^>8t3x2n@^{F#-xli8FrPjwJ5QZnF((6J#Mvjb5olzCK;`NZOygVUE^MB6R1iAC zIA6~)?FWOMPuI!0f{aW2*w%sEYz))}!N8fz?p=qEfq85xL_fN)nB&EHzKwmK1m_Al zvJ6QXzz`e1L(`ikmH?+U-6xug#yse~Ok<%D2yg}w9s+&#r1V#JsxFa4FcftUP zd`0FAhB5bFeJjg!EaQFknF9r^hMpI&*&yS5F?$-qiEuq^ekuS%z%2QUf4FW zb3}oT9LT3KuK4v0h+B9p!IU8ARQqd&cONwI*8ZHdJ@vk0=k}J0ns|cNlK2I^qe@x= z?#B$IC7^`=CbHjRj$*3H_|ZH~y!Z%T2+fvSCxz!6Q?t?iHzh>FL-l1jQ+K4-m!_mA zgiZ@c1Ervoy$_&(3ehvTCvXPv`WJiG6suNqQg|LW<&T1AmMP(s;PIIX;7KVJp36-0ALzryn@x^J#o1HI@H~wN86EX??eGqeguHe8oVZ6#7w_4L)9tji zR&0odkraF7Y7B|uLWr~JtvG^`w{W&R0<|kMgf3PoTP9T|BGbVauYHbc^ek)!UTSdQ zjncV@z*~pUdGRapXF|>I)ybQc%4DRlgAE-Wc+X^BfAmg%gKCqP&|hWVSyV%~!KuvN zrmBA74e9<#s+w1so$TPXD))~FXmPws+-BJeKM@2y3*kJA+J0#$6_aE;cJjP#c74??f$)5c?+<&}i z@B~30%MAJmw@y9@ap0MQtB~S*aVlq*O+wl=z)chEZ$ev~YhEpcP)5-*7RAvZF zD`l$>zF3l>%oxFutdwm#_@d%pomOuq_ zNDHtD;N6JOI=z|@7NgVqz7w6MUZQ{DRy>Ia1H$9%2G9tJyk~(%=vKZJDydi+&y+#&7iw<9_ zyg9&r$7|39-iRvjjz*63G8Q@;fT_}8E#y>4gV(lqbWCI~;OL1K+ku8Oo$hy$-{M<2qQbH^JVLlR z%;mnhQa?`J!3F`aJao0kL~Kh~fL#RG__lc};ya$ct#l~K^%K$EqyGykV_(o;^tFYD-shoB>}60OIxJ=0Y9V}X zPb$U>`U33lut5nHL`F}h_b#N-QzN5q#hBRG@p@FS4OH?vwm8ZRao8mv!(5!+atbwD-C5@iM{z4TrvPqlxkH3R9K%3e7;6=jUwQ-b3(@$MrNB!2c z9(G1v$uKtkVPFJmTc=pr%Z{giUwWNsf#>VT^s7Ktd=C=GUcjhgg_o|%{_|)!V=xUP zZFN&?Ux*ps)O;6coe*{G9&82_1O_&do5D<+{3ofef0 zVNX3|^h4daK(}*VtgCg90C=xH0--{4c0_J?fhscfC2A)n;1%%v)dWv0YUJ+nKu>t| zF#(3Au*;RExw(O7qVmr3paZ;!TzI@{T9UM!^wUzAi+w>$^#rK2VXyrmzN@CYBJ)=v zaUU)ZOUqZ|t4LbD20zNeL->n~ankY{d`Zhm;UO)r<$pKwzm5EF6aTx7|J}v^?&g0< z4y6dhbLpd~|+R{b~Q@`iuW-^~;cFi(%rT?RSAM z&?$B~m_I$#>-^X!x!Ft@c&NAhX)EexuNe_C^i;E*(nC&cdStke?Z+x{x{m?Eermc} zNF9+b>wm$hPSpRo5z)E+3r#`wI$Vvz6f_zUXrySLoAfUzAgtla;r@=H=&#|781ug_huT@HuKy|ZS0pfjZ2TxA2#5>uZ zcd6JAOMC*mvX%r|Zxj#dyd?#^QRk%)Q$H$DZoZp2Ah;XC^LimmYohgDlBL(@%2h*% zVZyUpaTR3hT%3ne(IgZl>6+c}5tpg4UdMAeLVde;^;c2yGVnEYv59|mUaw&S!{udm zHsn3I4GY{Aq(Rg@EEe<;a@wo>cPqQ&A0U5C>?$FX8Sql;k}YP0pl8T0!2-TdfZWH0 zlVn3Ufm9M8sYDJ;k|s`)vnNY8PnK`KmCnv`+1<2>h6Te$rz-Ktfm`#?;Sta?r~-;% zEP^IEJxzroF&S5O*cNDS<>0M(>;hE7iAd1T6cau=vGWxHvHV|$)8}RPb2^6PsxAJt zc&f3Hg`kLkf%2KUt;#7eVm0P~UrrCiA3=$bX4~23zIYdI;NQ^8fq~Tb$YJ#HTy{0F z8FFkzC%-Nd8=X)IQ}*@9ZWbd%f2xfBbTuDDdu?nAwfE9nruJHMS;If6y;jlQRb-eW z&vwHlG8&el%!DIzZL~+CxEwL{nv*=%O;esEw> zEfHMSjIM^^8%__K_sH(1?LvmEZ>dh>D1VsCuCHj2m3SZd%fcv0c zvg2j>m~vc$?tTd$K^#gr=z}D_Hk?zm<)zjtbKNjzI}SZPIla?lDm&W)p{JtZd15(m zd7MKMnSC-AmCYPz!O6p47ntJLVJ#B4I9y$uVD)0f*>orQ>;`(*VIhyH8=aeq ziPc04=TJl|{xy6nu?ku*#lMQLnA>oDNnXmAs04cFYdt6Jw~#yaW9adGjqI}5g%Xd9 z{OD1xCN>PIqoEdRgpd0swvZY$)zHM|Af^OFID)6&D?-6ZV2e%cR)pxc5&lxqToXG7 z5zs*FMPL>CgBQ?n;8Aov+ki|ZiAw%n6kX3Q;6>LnMHF4n_TxkL?+%cCQ{&qp_c3YE zdivL}6kOyAN4(*V;hqK8kUj;^mOV!ELGAR%V+a8P8L#s1CUo1z!lDI@5tSk+hjtUD z??m-jGV-1#vN!QWRiziVPgUvpcSE1o^jD*)8K$Pjn3_t8ZmK7Ox(cxQOboKl?KuBX z<=J6!7UV2<9e^RCJ&4^gC)z#q0ivzPZ;<_}h=?yyzvRs6Q@3YW4pmf;jpBL4W2$1l z&0T8Zr-QrVPrKQuH_;XxEYl?w%wzZiquIS%D%{H^;tR{H;)4^`Ybdsn-MWVKk~iaV z@NKfMt$;3voF!6%NTJ2)6V&Hf*9-guc)pL8NN&){DfP8i5L`~)-{j3w;Xd}=IT*N` z$#Hxf8;EOy%FT56>0m$H586aQvYADMll5GOM6T>&_d)|zyceREa8-}w1a`)osF9T3 zJ&Gdp$&DYnkwE=wWH=h?b9UEwM^t0+GKE+MfBDwrB z6qGj?;T?>s{QV8SR`jLK#$Nsxs2L`+@m0RI7A!rjcrm0+_Tx@FJvFos+?U~1l0mQ& z1#)YFRrKOoK51dPxtV=}9Xt~LBoc-j3Ga)9g(Bf1BcVwo`1k;AX8%C2ysS4Y>SQS2 zF=A6|2q$a+6-?T4u7eT&eS;o`lKz7alVBlFjvd@)JlVXb7NBPQDv+b?4gYf#sJaE<>3pg z0u`S|$~)ErybvT+FXZPD?AMer$0cuLZ>~ndZdQ*U$L{n!Gq2CpE(QOdOQp^r5VEn2 z=DjXvz3=QKLaC;l&<2uB2upJI{2^343y&Y_#g?O3EEGlAXJJ3E+tlBf>$Q?4x_c>A zi0=07)ZH&Y!AiPc&YhZ3WvT1_}C#z2C2e=zI$MZ5(E>t6oM{yca3h+n=xq z;GOC5FeG-thIn+4ZYbQiPB4*6Fbx7M5jqzXxE|yTa!AJWD9Ca-vY;nvlmj%iP0Y^b zW_q1=y1=yH*Q2BG8>Y6z0zsuD3y>~}uI%+4BW65RL0n=};R6?2E;g$~=?LF^!SAWE*n ztN|+M@!ikR*KVjslb7d4_pj3yk3^$ZxvwF-b6J^7`YG6K=%z7tHW+XT#jcI*!+|v5 z823pO9J*-8IS%j(`H+RsKTV$VOh36ir>@UlR6PpCg6xxNyZ~Q=S?_b>YDlx+2{CFpsjG2UJIGr?<^ z@oZEORHp46BMD;w0Q-7!jOQbCgjwJBtI5Cw&{OdqaiS%h79Eb*>6J300QCn3!FZPi zmep8#7kqn+$W3R1lwPM~dzAp3Ym*CWnBl@07sk27T&3*cQeZ^Hb~gYQm#iZ8VQ*RM z@4oyTy}ZD$?3HryNFiPq#v?IN4B1-0)xVgze84H?CdvVql$#`HxTVa&mbz$yQ2udF za7ERCy!qi3ahmNGrFZf#Xz^#p@k3})JrKz~*)4tIY}p!y5gvzdxHbU_pr3roNg{HFh`rt*`zC=YlGn;#gc_W zwEmycd~veA6Qkw}J?hGSn1wj&!HM2S6MpRvP(}+*$ltS>BMEp;TV`JGW3ER~o|Mc` zSDHw|0-CwUz<0z05M^S;=_oyo*4u1nE>^!s0VnK91E(OUi9j*@sI(7gFs+T^>5Bf(=S7FCK zeLhf0EsXI=Gw^Xzes(Xp|26oT6Rd!}bquT2sf^Dr|4kV!<0Ho7VWa)7^<(DoSV`k? z7x=hkJgy|Ri^2Fi+`;lEy5b@|IFyNi5~C9ZsL-BwF4$AN$_o++6g`rz2sp> z`V;7-w?CR*tm^?uk|z53@6gL`10N@)mr-y+YoZsi{^g!*{UdnkU3;*r13Y6)dO7py zA31+F61}8P=`Z5jj3%HIurcbnhRKUxpW5~fw&__1N@dMM}@EVl$N5`Fq~9Bv64 zn_->*i}Z>*Y-jNPwmBD-ALlG=KJ+`WjEbMk`#w&tZ^Zb5QI?bg~9-b5$yXfxKJLWdriE3Nak-+ zOAyjEI0ASYXSkHD%H~Fgx@sTZef9RUt?fV1P5Y*Hag(QW|4F<*#r|W{`ScU$KKkyJ zf_x8<1?{gUuAD-DO^vDejfHQmzi98`Zrc<6gFc_Xe$%;sBaJOjSNPS&7wx_I3#=tt z|8UD8hwo8>e2M?bX=0Ely2iPvsAMg{oa7V@i9^r|j14j{<*)8YE zpoBc#So-Q)UTUX!WxO4aJ&ljkO3)Xj#fh{yN^|Jt96Zf&HA*>P z0TFJ`xc_n5br~$3Byj@vr}h8SQ|UjgfSvpQ@&6zC|H)79|DOME^dIzH*!REHbT@VtqC92bJCjV*iNQ zS(1_0u?>^nu21@(n+|;H`a%*MNnbbWrjzRH^!(H-%HQ`#=Kq8{ZHRpUZjuKMd~b(5 z0R7J~yDb~qo^X4wSh>c4NOdXnMVxGCLK)sNJd1^NRXb zn(K>UPX+aI8!3sHAGN3|BCX=HD`X@pB&hE}QUk8V@F;ZIXnTlLv=GyC28eJ(M#8 z-wxKwzsbVR$@cN@Tk##wcJc2S_)cJr_>O^UZM_0M7M=opkMbJz@tetm$!NgN?%>}7 zxG5)TOstP+Z%g;>$yvkYoDp1r1P=Jo!L5gZ>0mO>*^Qg5<0%e4If98W?aE04I5w6> zarD|YmDb1UJc{i!PJ-UryN8E83Q^uPJ~O#w->E`T-_U1F<9#yvRZxqCejl<{Y^7gK zUr=Yqu&VmiH}K8ZY0>^VAB=@J!F{#)^o-Hosp+%0e(@Rbtr+?& zC-hl|uScWwgg&xz?H!@to{}$Ky2b_GXfvttKi0tx} zMUD6?Evn_;-2Q&iMvAFo{4WmNZISXC{G~$zdLNYQH}AvK?j2DIFW;C%^*@PhM*e3W`hv^p zxD^XI(J2LKyQ2$`ru_DjB=nzZVzcxz3#oM~wzLT!=8~d6UG^Wj(SIteYI^nj;5Us1 zeRjovi?ONK4CEpS`$&IwY^=iu)i$>k>on2T@i4E26lK--Skt+@;KU2;yoFbe@T2@l z0;y=YOhNsl^CfJ7qexaG(uj@rM{kKelT%s*?{OHik@zbuqTRVvHI@Ip6@RhKQ5KEo zNon{S;r{ukxKu?N5`LJ1_JE)EQ^3zuGk%;$fS-@ExSWE6=sswQEGFoDmCm892LiMx zilZAVeym}@6O`|B%JT_A)JktKgQ)V+4I(POhmN(!)Ci352Ne)5b1AEb5e~RAs01%H zqZUv1wNUw3^NZ@LsN*%pps$o?yW#i6Zj@*Ll8<2!+Z0)6k!R-dkhpKZ_4Bkes zFq$Y&Fg%s=#E%%i$>#RE;`bSH&KOz|vo(3f`wfsvryCfx4)!ZL{qACYluxDpMA*Gi z{e!joSN{w3L+_IXrH?QLVKSNNMD=af?a zW}|=cV6<#oEA{VIIqNp+UoloICtFW(IRQC1iR6*Vh;y)&6qlu=pH?-tn3o+cXN{n; zX^6A4a$YuB4i2HRhvu()iv5my z{$R&5yC%)KnZBl&@e@m)Dew^|J!!xfZXFUELfl^2bAv(hDlNb;4i}S?-QrG?=VB1b#Lc_=#=Lc>vg1KdKAtcsX{i#FjD~Z#@@U=M zp2G{|sq_0(;Z-_NxfR~oh@{&L0^&0O7;LGrS3>{Zh8l#D_A>{E7gvHP@DifYhi{3r z2ja#HoJS17In(1Xl@)xM1|AiV8H9mTK1=KjHNKx~mgSy&77D|4YX zROXlC6Mlk}rDT6uf;}#7ng?A{Su5*#IiS`o_SNm~VLZ;R^EdVwInRzJR_&sY!1wDv z7arJ<=<-*2>KZ=~ zdCKRyZJ~H}IqsQYCBsTGP1_{`f8?UP{1Kj^FJ+kElE;eUmO_pj@zux{nm;ohQvdtW*A7S6DG6=#}{%h_g|@2|KTL%4A}R$PfL2>v?^B>Po*NXFtfuBibe?Ma=#=9Iw9m{ ziZzR)_p*G}+-H1|ryP)jT81RwL z=;q*lY>r{?jqm8q5Eoou_y@t+NsL%P(VdnyV*~}fM+|to!XNt_=P+nrL*h;z6z3BW z{w>ZY{>Hy)pdGYytU@rVeG39b`)Mn5`tA;2RKEpZE2+sSd^PMd@bwnHW8iD%-kA8B zO3~foYo;0ApDUl-c_Es#$S3(XAxV>y^J!;v|6rtZK34`+4}5&X2-3moKln&@5l}u=dl6?_Wp5(*=D;;rmv?_tTfF zo5oY4@8AE!GS5y0-#_5?l7{cTUE=!?a+&eH(Fgc<_U*S(OWr;n6iePN{wdbH_WLvdU; z07m%tYB?+kqRVDs=Di^U6gZ#Ba^q%-+v!fqr3`|oYcJqr^HS}(4t z^MTcA-t!p6Yh{>z!I~PU4GcguG9GbJqSlkb# z2bK{N9CskP8v3{WxV}vQl17(9^2shhlD|v#?Ivx4t_z7Ex_X023gfCIuc0riHQ?!L z{;>0K8e@`QPUAt633lz1hM0RU3h3}retp9v>VGMAe$)NZz9n&ZePFK0H;qt=`oKS#G5#?Y zW6fX6B)&(go{J%*JyHdI&>m?vK4R~Ya&~|oJ8cQl5F2Fe=LP9LP)T_0(P4UQ!EEZI{tyLfB{ce^To7ZN(1MPv0u7^AnJsi zGZCwC;`h%}Ik9Q#@+H-{WeEI`u&7%Tv5&VI<(FvXA;YLeD03E_?{f>6g~9=WxC+eA z_w{ssqw6H;{07fS(z$$ml5{T5og|&hA16uYE4ZGn+b8Jo^A+^}VeW0?S1P#DI6Fx!n53K^E~*Ft|i_GLGU2o}Q?9f^v$A zihv55Kum&(NkG(~D5#t^CZ|DAf*|>Qt9oX3b`xIix##|$e?Fhh%=Aljb#--jb#-<1 zQ%3zd#rSi*=D919y;)+U(%;}cgxi(Pcxb5r7H4x_xQ1)2gkGPUOtRt;pw^5 z&8!~BI*M0%f2pzJJEgPr^Px@=E;v{3lNN-(d#!VXwL8)*I9JlhqGc@;>Me1=nYMrI zq8;y+9Ug=^L~1>_-dnC_Rnp8wOQU#kE`{Z;R=sOz+FFmWIMJKJvX-g2%W3Lci7=aJ z|A4|Qgn8;EBFwNUv4mmym-jN|_z}M%YW(Op-_m|w(IRdTp6aW+RI>alN6N^0;}op# z4nas&Yal9aDVgU%U1*i(BmAH~&n_Mywve9!>uQsm$z>Ba9PKdq0? z?igypc?xg_zmG$}Vxw|3 zw~|2mV+ZL|BU(O!C+3*7|p<&!G9KXqkn$f{nzfSl6=V7DBiLK?a7!-72nJl1VRFrA& zgYI$)UP2WyRh0MRD_?v&C{`edEMv_sBJuG&XZGh#24h2B@ml8KH9JvYloFT4)|a*b zr4DoOUlE!o@;l%|{BK$N5mLZMm#_L;CgD5=&{qPOZq38#;U5G|V9fGA4Af`&$SiGE zSHj;XZnC6*_axHiTGBss66w8`^fOK({iT-l7oJ4=vn}b{2b{RReJtr4P9lARCH>PU zk^U#FLk#-geuDIv|3^jm?O5`joH1Co(pQTWhHydWHDe?=lZzxj7o7}z8ZfNz_ni!U z>hDuADj5A;mbWreUZ`bcXZ9oH|9${`35`LYy&T?*`^Gr*)K`#5@hNy+o7tt}`*ve}PhHY5 zWvUc+eT7e49tC{5`Lan(jginUih~d>a&bv#4UOw{JUZ@vkB0v3A`A$)TeXL{=WBKp zxLy3_L)x*&Ev$$gxDNnp#aUd82OiK`Gkv%!&LJMgo)Rsdv9OlQX8^Sc6+m%ZG+$f{ zMDxXl335WPuBLS-=9cxeIBEJWg*~xxcqIXcS4aZ|db{uc8gm8Eyc_WysCz0rjJ09o z>$DikzJl2*LfHk&V<^T z%!Y}a*A)Ol_>L3jG9VYM9&o7oebT%{v>%)y4;I&!25@<|0!66u3f_BT_dP56eel zaT=24iRZ83y3h@{rn*OG3K#LRx>@}=bl5SguiCB-OdSxc3<(Fw9@>NxjE>nYSS$=J z8!&oca1~tj&FNG2Tl?ti?7^pT#qu0yS$f>)tK))y;nH6DJC%01ENwOJtm{~sq}Nm{ zy^hugaD<-68vk*;f-HYB z$Obd&b_g+G5|xyQ4Imi~hEOKKAFIJu|F{##c33ay>vgmzmb4;xC^BEY+v~*h=L$>u zhfa_lJI-v94mBeV`{P$-m!4=)IsN6Ox*~VuSS>IzPJXfg7L2_-G5wL;r$5QLjhY zxEn&RQo0BK1HRxHnsxwhMb{sM+jQJ76K@Oniqi0*Eg6UHx$NfCwNg$n6k$H`8=1gH z2{cOJ7iS>|j-2zg+{FQ3c6F%$IQpwDrF)vF@T$`N^d@dQl~R&dM!Mgc!QwV+{XS~* zK5HPo*e9{_3wr~ORaLt3kq0hnsQ@v|fD+u$+(T$KZexXu%}3UN&nFU$B<_V*fhL8| z93hFIPaFL|^#K0E`)7;h-J{?VtE@eQ!!O9P2YuiUhd+enX6AlYqyFw@lAS&Cd~1@fm%@ulKI5nIT*LAs@w?V z%(MttVyc=AABMN5?w@_LTAN(&&%=Gv7#UZJxY2Q%Rp$m0oODnG^JO6I)L)8poY4E^8;u zam^Vurcv)#%oN#LDYZQa7eh74Gh)IAI^l}X$QR8=;~>B=Gw97z$bjhLn03uvC3D!- zC#k5=;r8MgB^#Mtz$9-F2PN;%)3ipsh{sovbL5~EfEM55lyOH7(r0|I+;;1TM zP|AgwqIfuR5KD?XUdC;)v6Bzq7q;Sl>0CHZJVG!uZ3C5f|3uW1U;Uc43L!jb_Jl7? zbt5HsJuU!e;YY;Zdh1+B2qR5AXzn6^j$IywaMB7{%8Qi76AIugjv z{1Qs9+Ur8qX?!91e5FM`7}m*>a38sgz5yFDa#w`9ui*U|GE1!kU~nqtxuL=Zu@Jh1 zog$v)Lo$Dcu;j$u^XOa9_3(yJi&%p7hP*tEhDx$zWh0X5Z_^A{2ZaNTh1(5;$8)eb zS+g32rNxsRAN!%E`uIrxU&3(1W-K818^f0IU2V6=&=U>peL=1 zFy~Zvp`2u3HHk-t&d+04+c*5L;Zsh6|2=n{&fxFW7vs z;q?OXWjK;pQI_QlJHtM^qYZTbaUManiM>p*Gw{3)0-wO4l$hu=98r@Am*H+R+NC->T}s;hM5 zMg2TD);HC@sLYeLa9i+D^5!(3KTizt@aP$;8X2mN!;Q{s z&CxMIEdFDjkEP#uhd393WuTvlN?!_y(65_Fpa5HVe0K|KjDQZ`sKdXI4)~V&u^w>- z;&BKO)M%qXov7>kqIpf~*3}`IU);euLhd+_94)cTf9O9-y5V7)>Wk}Jkjp1pu{^+C zZ024RAtd7w8ILh8HqshDw)6NCl^?2U?*zXH3bOpjx25YD^aB2o_!Fg%Jx`1D)E^9b z-?~CJCytHa6sVPPxiqGSHGS4|M*DyH`_)<365g8aBLs(h9 zGiqEgD2VbyEo02(%n@?&O$3FFKV45+DUzYX0a+a&LCM@*biO@! zD(OsIq%#>GSL_JPY`#>TR~fp;5f@AW20n?wHCe!|zM3$%i7_(Fi!d32;|+8admI++ zxQTVf^pp>#i5YjZ6iqxP=l>)yULkQ_2N}TN&q8KUGo8;_o#vtfO-9uj0_h7ez<{R< zF(N2_c!G{h{*vFK#-H{50K~%2g)sx54v&EGpcN~R5PncEuERxpJ{OHNuurWQU5M;F z8X?}FgKH*ndF!ru1M-r$st3y)-ACJl@nf6g+r3B9Ck20)p1NyVunQI(nDuc>)xoC7 z^k0{kpYk)bZCgsW&F`h%m}DE}7!ZulXts|}PksOHA5(t}Cd^1F$8Gb~GY&S<9uwhH zlkA0$|K#_RH|1;okKd&cCnS;=e03G>j!+NYnQ(3B{qn1WbR4l!21bH@#@*FdX@ldm z^pEKcw^$$UQqk5Wd2@1Ad^Mz$*y)$cGv?=G%)jRRo#yN~KeIe$mA|iX?wzXG$droz zn0`~o$E*8>#adtK&)RQ)?vdOP0yEW zz5NuZ-4;Llw7{)MkOlR2N428Uki^7GNNthJTE$WdaE7z?88e(p0UlEHTw>t9_kU1R1I#!Eqkn~STSW>57ArU$z5`BC>5!7PSKxv`TufVS zx`(*%evr5gEoUs;zE=@|`|c1hrC>%}L42%+*V?5ngV=Q{1VE$-43Dy&xA@*w5F zI(eW^*r4L>jF@6PQMpD8C4&x3i-`ceMh1NVT2JLD+6ZvZWDJaV_o^*Qusx{tj`2hdL3_mAa|ETHaQkDCQnw}U1kj4Xj zQPf4$>n+7=BB>Cy#KWNeB_n~ySf5$rokBC>$;S%=h?UP`zmcCtMN>Z5@I;J!5FYOo z&m*wRiEFCzyN4%tqXG0igcz)=P%LEc;h2Nn?zQDUZE`YADbHBa+(c<20{a9pg)$*)zHso+yFt(7jPP%^qJ z89kJYUP?w^C1aqHF+|BYQ^`0-$w*c*E>tp7l#I)ij8rA#DkWpAk})2Wkz|Wb_}jgf z1VEZ#F$F^kq+$+!I%wO1uRaGar)*JtJ|(lelG#Ja?4@M(RWb)EnM0J!GnLG9l+0u$ z^Fk#vMajHO$xKx;uTnC{Dw*RWldj|+^q2je_ZKsMHdIxe^(X4P7LL}^yIa0w|IBiU z_b^mp1o#5_#%h}bSquyUwrcxKqFc&-q^LN_{vGM>>v4UVwftEAj{T11r=4_58LgMC z{8Wv=vM6@?2q6YL{R8hvKRoO#xo5bo_*7;qVye{?v2OB2x+TdkUSn0Bxe}VqxpIwf zEIWV4Az9sM5uQIWTN@8ICH3NFgc>DX#VH=DM-?N?Ih+IkN|INNgVqt?(^a5x;ScdGzd_eWh z<@*e9_Ypbbg$P5}AoXOqo(TFWI;1fgV*I{_lKDLCUrB)ax6AmJ^t136NwF4=ruZyZ zp4bjf216G>vy2^pdT}5`_jwif!s|5)_?52kd8Z`SXV&^tqv26~)^mMEAcD^W`guSO z#1R_-#&u9G(jA>2$-^218(Z>2nx>%|iY6e2;wj@V$*|7=7YC9EZb7I$%dZFHk{v zX3GNCh#ep61yWZKFH*+bN}y8X4*d7#K%BfJfkGKX=QYm72?>?9OT?KVlMy!UY4 z?dGb<=ne2chC3sz{6p>C_;2y=Kp^-Z@UJJje4W2dj>b1F$3dUJqoaq~l7dNe-AllO zom~`=A>*4AYN&PqFVr&1L&8<|CrV&ZHd=mmm6Zy;XZm(gQQSh_}{R+P(_$B|Lj zNRU3_s2Z$M>q9>{M9ITaEm1F?!yBlh1xXC4|nMYBn*JyRgRF< zPY95DL|Na_{oJLmQXI&ATG=G~=*f0>=`s!=Jw;yJ==?Z$=|czrqOVGH-Wo$+)z|<= zZ#+$g{Y-1aQFgU9c(y2$LC+%DjOo=j^7N%Q(fXT>CK5NvL=O@ysl|{0r-?_*MBFI@ zN!50N{wFqnholvB{yMV%gOvsE|NO|?ro_{++ef(HD0)9dOYd*?B`QGnFUV7m96=Rg zl)u*aQ0MWn`bfQhrj%13!rpSXD#s{WT58p-gRn)&%NKP|(e#W6w`?CXntG{O9yA=7 z*p(s5Q-d++DNhaM|4H~?s5~_ck1_GH+-gn5UT4W6`>X_Qf}Lz`!w(LZQ9aehitn5y zzr@Yz%6<@~#R~`Jo^}`6I2V_P7Z8@=1sCG~=DMl{ zgq5v|Ke2X0&XRY|qoCkmmC=D}!a;YX|DD*f(p7aI#hMPJv0h&M6PEyv?yA=8y=yNK z*lgo~OB6@qA<96JYjwa3$7@p}@yP#nOH_Uu|80k?`rn*S=P`>JfhX}=}GL0v+xt6jeHO{@Hyu*M* z@yEkYwZldbe?}1<@QK;a`cLCK;FIo5wpX;#)pYy^{YlLCe>WcZkiHqKXIEirg?nB3 zta$f2TqB{QBlV-s(k;cbEbs*E?b{-8l!oe)b%J<4&j^>RF14rPfp-FMj|qhG|F`2A zzizawfy5hA0TX|Y?^r<@Pg_i0PwM-{1M@KKbyqJ^M&gu&GLv>m!Jnz`2Zg@p{Qt-F zUZ>`IkC@yq9@xiy-y}b`A63q&Vv%^UGE(WTS43w3**<3eiT`1K*MFG5uJwO`KlOjI zO>}|#dMZ9_ghaMfsDA%n#A8;&A9iW!fw9vVsLYOLq4x>eqhRj`!n^~5#(UcKG~l2< zro2X$KP0a`h2-oMx)XV=v-GzeKRwpJP1g1AICC`#$E#XvPSGo<;U4e@W6eyGVtIP?d^ps`-(19ayO`18_G68_Cns=@^DrKbV*@v zxp)=oa^(31nsxwsmH;(jMO!PHX3~nbR+P|F%UUh|_&%jHlmq8sD;1$-O=8z#T3OW| z+%afN=*zhH3R=*evJ+|Z9Vm9;XT@h}-t`RO4W#sm_6rd=*-@zGu7~Iv@@|MLRC)r1 zJ0uDoLLqB|k84ea?yJS;+qo=bX*{H!QgmDz_qahy>W2SQ=RZvQwa)!j9s)~?nX=d& zqXD!j2iq&+=u1>wt5MUZfi;ZSVZGHL9{G%Hv1`SPR1-@3dx{+I!cNmLR15WinOm!P zfI=s`a0(#KFWTpFn^E|U2rn50MN4b2`=mUPi_f&?E_q@irN_lt;;bn~(e3yX%ePQ? zR885XHu#O~^CI;NwPYTff9NnJ?Sp$ah~5_iV2#*tIUY@P@<8;wT{iQf+qjumiIN&Z zVU~l+0d_{A;;au52>T|PK^{sDw%rY7y^S7hr@FOu&zD#3JYsYPfs9a)*Ov%px}=>v!U zko(a$K!S_NXx0>v_vLwSN+Rm7q-w(sorFBru%mro&kX}H&>bvxs;thOQNW@=H`%I% z&F>rc(w>IY!`6nqZDKhEVE63^6ijW{eLE!X9YN$LT^mf`X#=z84hn!$Zal0e(SFY$ z3nftVUA*bM^07eU&H}y7RtR) z%ixZRbA4T~z$X(u@dXB!4)p~;J|5x61ueAHIu=V}o`f=JhpvED3LaCui^a+Xr0~Y3 z=Q2Yv{V0cE*K;ZOV;OcmpAoZ@fv9(>=Ia6>%f0S>kj1BHx1fgxiU!^rS@9f08)FceyX zHzQ3F-YIOAC2Y3^sT&aoH6!*|=MpfKNsiEvrQI zTS|Q>%vQ{)3Mvv@%$CS;;&b$Kj3SR-hDVz={*u5@G*AgOVknHhI5bo}1(q%)B8V@& zc7J2NsI2L9sUs~BxfJiMqs-!RysKG@14A(mgvHHe1P4IE;s(6xKyocW21p=bkw)>k zJE2ewizK|G2IousKE8y$VD+qjcLBbkxkK&2987Qx;^3c%m<{4-dTLpV#C@BIFkT{u z>L2XO4kStB7Lk#*7=~GAVR?`g#P#AXa0{{^B2GM14ef9ceQ>Fo`1>=Wo*F&QVtjO>Aw4d#&8J6-Z5}-?wcQP+FO@r*!1R|1@01DW%7k}>HgzF1 z^vGfvIa@}~;z&~SG7-cY%hF<{TGOtAQu!^ zx~03El{%9@zyJKW@+#;L{o&XqqP;0c8*`j&AF^*aW_$c+h_aQ`r&dbC6u`e=z zbb22Cm(q6k!he88#P#PfcV7y|RMSY!9y|jorSz}3#!=3=bI}RrF9cgpqP-zXHOQLE z4ro}5UWin~{)$w?V6+pw2C07f(gG_^rXn|lzrRm*5uM}J*3$O*o<%sM?sEpxa@6|v zZIgrh@`U;u=IN-uV6iX)eHXNlM%0`kk5GTW9*GAUSeN=~5&4VOme4VmfIV9tz*3&% z!&ur*1S%Vw@LwGG6Z%yt1!xny+A-9m*3Z959^Db)$&@$wV4z=K4wUxi%A-MAE=&f+ z-Q##qV8(P9^6F~tty8#*ZdRAj!5{1$__M{?qdO#f!AMr0Tti2CWZ~BjXctoXQTjV@}ry0w{l&x`fcj)JVG*hsSn+>@y&1ln(Q<$j%YZUfvXVxvuN_e*jyUVf2+^3){+17@7r>Jp-| zS|wJY8_0ARs5Cx(q&A9-UlCeL*n;{vZi`b z{MsLrruZASTbtK&a`7Nag0*hHd~ksWK=f?)r=#I<^Fb5ZdM6fa;|UCi(GY<$1yJ_h zj(It`pfS0iF1es4xnTR9Jz&IFFT!Ubj+~XIw5RTJFKh<72z@PsYUN&ty)y_+YecU( zgm;3u&M{uH=mqn25V`4SyPU6W;yqqHGT^t+7R}cJ>IgY3gF`T6(Le^vyzV*R zqPam}3bQ;~(WAyVzlPj7fch>)U~^n#pco36Pwg741m=~9!WN`jAMdS+htmM@HK&bQ zpz^{h6Fa-GvPDlu1r;)eMXQU$Z&6a9@kG_28{Z zA_=cF)Rf%Q%|5#y5KKYSg0^*1aC#4mk>=YF?2_sJKF*dm7NFV!wB2?|N|*g`yG%A#W0Em!dV) z`nQMTA-OalsAZZ zvy8Th1TO(oMk{L?#7bAqcB!T-z4T?Iz9{4BF%#DJlUqvc;%xvFuecCi1%1gZ;}% zq;9wxxuB^#Qka@Ian2wFRQwspuA78pxkr+>#&4o>KS!8cT{nm?@ea)HBXhFq`x^0| z{$8h6OEJ&K*`A_kBg@OkQZnlEV0W3G>vtj6_88{`e|FWC|tfV9m|@E1{N4BEl9)TGX4}>tGp`f(%7|%*C=2hxtzThSv|u&XWUKOSw}~7XeMHX6 zXHUb(2(dOH?A_k%KqBnnk(*K&s$hDpgX;hkwM=HYMwWl?kLYYPvfSzAo)q9A=|%+1 zmFarQbOV8BbC$@Fsm|mfX)q5-CEyyN94MHLHVu4Kj z!a%YirE$cp>nqn7b)`Y_afk_Q`IRO!bOZE29K>E5(4v8k?bGVTK9CD|yor$KsK3Gf z22z)Mt?ztloU?!j@@xPM%2RwLfD9$^1RsBt$!!pakn5UxJf1@ULR8%qjOz-nDtcB| zaUIn7@KKhM!x0-~)_)Gbmtm+vbN@7qyGA8UVH7VI&*XX#f-0uJ&enq+#>WTQ+rlx= z7wh(3ai7SVeFw>kEdimgg{IlG~Rsk4JSokJst(8rBBX zxPPNH0$E1Zi(NQH)+s$|PKF+2=4YkHtLV5U^8X1vx?nMPw)!C1)~Jt?5f9x>`{8Fm zyNgeJ25yJf1nGKZ`exXJRBq$x5}CT{dubMX3oNLYTjSX_#S#ztb>OcH*e^uq`~|so ziWMH)avTdDRMsR9eo=Sq`Me0b7&c+If@Jd#17s9_I=lyu0}u3Av(=;r>~miHHp)Jv zXande9*!ibwz0WLXXVkR0^vX8IOP%lbpwYQ_9K!X$)3{+Ho<{!8!!E)c)>Svb{-6j+B0xlrS$Va zuhecP)y?LT^N$NQNpxn@qc~L@G>KTZ==0NmH@;x+w_rdWCGZ1n-a%O30jcun?=a}$ zJZ*108nA8mMIwK%Bl)8TU3Hr_7)CAb%kAh>6=cTp6Mp0qEdh2&IH)@op2)Fp@JE0e z3m6b647&w`0@39?D9ih6FTK3mWO?KN7v+5j7@e1gDNkJbwZ;Ec^|6OXY9C7flO_E_ z$4jqn)vk3^$E~8>QQH<8`jo2&D_0Lyu1-=n!N`K=+a%1loZ`VVP|-<){h|~vm_1=S zDXxP-q4bqP6R^=gaSSR!yYCe+T~Rq;`;;rIGKSg32N)V*n1XeC$-G3HdvSUAJ`$Zv zF1Hn*U-mGK?cux&)CKh7OUHFZ7y3oux4bEj>38S`kch2tJvR8Vb3&YgGnD2y_?@Fm zI$?J~g4(qhz>QQAA6h}=!6YN?D6p>=-O#dV=Cg3pnMPO~5^-kd;gm?dxRCg!Mr3+O zqKpR)azSsjJv~uB$Uki-p6}5?49Q1|LZaeVK<`8)Vg+xs-;$pr?cMO+aoU^D_XiTH z)14#%E9vm7kR%RnY&X0<0AE;CSWonQ_XoL#EpXxZ=h!6pM;R8{>@@7Tjh)y^Jr#_I z;>TmhHucV=yT2w&0zNMHF=CDNsZdU3UMLLBJ7O!Ij%{-@*(;pP#miXndnChB)P>EF zY;mBL>g1wx_7jGQ@sMTNDo^Q(%{iAJ&Q_r95;Kwgt)uN&)+Xb4xIOnsa0NAN)B#}B z&vYRmajzBqZ^XiOQc_BL*m?Qls1xLDS6JRA{*JxZ1{b$-&_ zUus@>)r0&GPJ8oLE_qT?VLa>v8?(he7uS;x;viA2*h(Pg41=xpa5c|KOZ99q4{5M5 ziQ9_klyj&&4)zqa;(1Q#fWNSh;nH`r^;9ri{CJCZ-GI2;d} zq#mBr1G)y$m+R!h6we*XVj%b*mH+O^mS6P|HPQch`S+Y``E&j|M(d zG%*iyF!sNG!;Gv}Vc_^)*K_ObgT<+KD+R$a*$NK5AX&|dGQ%!l427r$b&bmfBEGBpSbQBtU|nt zo4Zzfi4yRcxfmKcOjZAb4OYsuNO2)*{;DTU1!OCD@DW`M#6L z7WVILh91cMZiUJXf*Qn^Qd`6`!L@NtT)3>=jR0lrP;noQn5i7CMClD(3m zm^Y|67%U|ir30FteHo?E*$ez(w`4Cxl&0N)(#5rR5EB;R71eNvn{VV6%Em{6*r-p9 z4a(TT!VZQ2)4oMa)T(8$Z4-k4Q;6k%ApKmZY6^TvGT28DpyLsSGS*>YyI>ki|l?mlCygWdILb5+%}ptDZ=mFqjPS z{G#y`cw@Y4@SCTAJK!-uRY+ zCJgn9E96uxgXtpoD-dI(dYDq>B*B;Kx-4l&8)?NAj}ftm%FLT{_>))?pEfZbG|MwK zH%B)RP7aGgjP<&$ID`2~id2bOW?_tXBm(5_kH`y8zjYjWL5`0#k@2x}d*dCj?Vt_S zmoNn5@i96B`HOvn`fh{xYYsQSB2W)2;WUSu<4CP3?oH=UFYhUrmNi&f)?jJ*b8}Z- zTB@zM5JK04VmNp48gmx_X zKRQ>L@?)rFqPg4CVRFLhN?$$#@n=E`1cuB)&ob$@1^hlPs3~Pk;xrR)OQ};v;lLNayZ#t?qSIK5-Mf)Wrb39>csl zM?6P;vj9f#4wQ%E<$f*iXtrpz@iJut^`M4AC0^Vvac}|$bk#KhI;M=NFpdqR&wSA+ z&L;Xk4U2^3gw;eHt4j>5rlBhYMgpn0YVZ1yl%(k$tdUz$Hl5D8u1o53vqga+e$&70cJ|w%PN=nRURfhuWki z9#&vfo##m2TJZ}DwZF{i0cbfG@vf{TM9CF(G)&~nJ%Yx;iPsE=U&(Wd?O5r6q%)@J zMH!25iJx3j0!L>u+-b$9Zk|0cKSwMBbhSiUqM!=(tAsY`lpcDa|0ndgm#{ejJ?2YXPKF*m{(XAbPKqAC)Et{0dZGU(^mq+*JrO;g zlDM1%fRE%LoYO%9#|iz;zpKpn&Y)=Y-%Q!LTOHnn>Nu%$4BJ&;H2Y^yxzvfbM+qB%aY+B;b7BCfVpg+y#bc)mu&c~(#^ z88MO~JXTQT_Xjyj2BGkS)~fW75kAWC1w|NGLGq&(OM~;3?2_6CRi(lwK<`itX+Jnm5 z*hzUKyCAoNB8-|F)0~t2~@!h9nJDKOM}B^ZVNSh|8`q1_j{LF@8^U zJe`+SoP~UtRY@aHrX!OQ;Bm_1!6QZhW_V5oe%R$ACHDNhac*7}3UNM8UW@@(T^N0d7}&D(*zNJHsr!RpAUp4i;R4CqL76L3&b8T! z`wA@1_4n@5IKE($%T`!D-eI@HX>B<+?&!Kdy?mlFWXwEgz7Ln+`LnAs95(XuOU1Rf z2hS1EJL%9|qMPv^95kb_Is*XVv6U7~*mLSO19tfQ!G4d7RaAsNYDF{wqlklK(XsfB z9L;7RvEQH{yG)`T9QV4rNJ)QZ+fiEi^umI%P+3nla4}*Ch95!lWhYuB-W#}q|0@z1@soj;#7mui|`=KXr!c1)}*`bQZWy)Dd>0X<5 z0mkk^W$Z+8uMD1s;86L6J}qZbws_}?!`S|qFwwns>~L+0^IXt38+T@f$VAXCZg^I1 zaaw6Y^`qGV{U|1nsF10zJIa}!&Bp*UHW>%vQ1o*$>M9u(mQjdcO8uihAte4H)7*^0$}&fU zCrm)td>NKcVZQSN8ES1fXQG(;Udg>KgSRX~%^ zmk^-|w-=x6X%IJ%ez!rJd04E)8((&dHk+GJJP6G)TNc!EnA(e)3&$;zTjSxLT(rrk z-~CEadEAuw+?2ay#MJ1Bs4AoU1ud;W%v6dTp^-}tp`Lt2OT_4;lwu*-E^HFqn*^?h zqJd3ed-*0w9NKJ$wsA1N_pjjZclJQM)izG#5S3wwY?=G*BV2)Afg3f30Nn*@0|M(WWyaq2-t zxCZbnF)04v1eEU-8zr}n~7g=|+7 zDtLCs6B8c2K15j!PlCN&$%Fnn|?1KD780FcdL7c^C zBue{+Hj#+WGWW$~;SzTB%}Cup>mwd#czkK;v_D~Rp>Cv<(hwt_1Li z;IA@Qp`>81zzrIAuK#3EVVvZk^0b8RG@$DP3kDTJS@u$+-@G^|s^3`Xx1eRuvFcAl zhEmGI!L#4X`cqk!`X74`p}gW(nQIQoEgrR3V5+83j{kfRVa((p^0+Lcmr=$oa(D$5 z2=QnttQZ(W1#bV{G4&_aMOp4()_N+8FoqbVl*$Y*8AkQ`>0eBZD``$|r3qV3;xrq} zxmzi{1nJaLN=?QwBA%>dzQoMn z6SWkzgwp5Z^dEMQ1GMS$Btg&7CSI|qh^8{Xm_nhs2g z?XghhP-Wa{iwhC_I8aP;Y7ngeg;!8m5e@S`u>%npb9Vc+#ivnlwy3708WW(4YSR+$3{@uKNGj1!hb9X) zc+))q>E$3H$IqG8@e^(wI~hJnLcS39(4kM(VCKe-#vbvm7euC0foM-S36!yK$=DXL zbv(uLcE>rrd5jwc7}I2`N}ByWbb=Y8i-W|IdXXlh-sGrIHIIN4)r`}du%Hv=Sd$|d zZ)-91(N%m|d+3aVQ|9|K<*N}d04XiEg-r2&Xy%Y28BqazAYAZP15J9!ZH%FX_>3Au z&1J2m&&v_|ENE%7a&IeLy{g}aiBXl$rWHZ0jMC3Q>9lDVhZPb&w|&RRs6fWOwuHP> z(h$5sh6X8gU6@2v?Dn#VlD&eqKj6<&v!NXgB0umKdf!8C9yQH@2&V7PV=yHwp={ZC z2#6D2s1L!~I7lObtyoUo0{V_qP;Zvrz-N$C==Ag+Y|QleDH0B%YD*{%_c2llK9PTb zP?6=~RP9tG#wEP+TfBv`0gO_L9#_a(Rk;8zg5I|Xwvcza^iQnxHM!6j9eI8^uKz%^ z$8qCh^&S8I_rK?#knoB4yGsmo;_nXfNwbxIqGrFCEk`Uw(Ix7YjUZsVczQG$pv2AY zsy6KXeVlO&u3kKk@Us4Ls)am2U$&C3?}2MA_u9t7iZ7jor@Zf8w*&26fVkAAJKn`> zFTAoOaUQ*BQ~*SsIyeqMr@55G5)TghV%bNXk)y>cGICgSq*3k}fFq|@k%tv$ig9-c zon=ly9IhqdLC#3O4=3i!Jg^60X&KxYUUD|ovKzxG!YlZWX4`DB_G^fhY&&A46<;H_ zWM(tfBBUx~W|nE?`T=lDGYIahlw(OE0o02W(~+Z4Ev2%M<;@cGWb(pzKP)Y6g`thq zfrh&8-$!5Sla#YCbQl4<5rE75HaaOJwQ=_JW~Z(`T4Yo%5rx5vQXsC_W7bbxh`{Dv z+5=Yvkda2JAqaqRsai^qLgn!azZ9l2Mzdbm3oT>(3zdz8Cq%;mXjN1J!x#7A9iyVq zn?2aQuJ)~=KrwkMQk{cTP&A78VJvdG*KO7`B1bZ=0tW_5ky%|rjh_ue5}L3@w`y$e zpDLRid;RajS%RgrLTZ!U_0wLRbXV4I{X6GH$UgBgtaJ?fMEYKj(cJny*AkEZ#VX2( zJ>+$WrTvv-+d~#v;K}j(%9kwr{m1d2N9%OhC2tAMOSCJ&HSQ%9aD}Fg(MZD$;_bSO z_~76{a;8u&w(4KFgf+Nc$z81kkEDKFj7yrzVZ62LN#2xTK;Ft=5)IQ>O{782P>_#A zlrQn*)AMa$54EZJ92&WiW-#Jz($j1-(u})>)5N8GhEz7l-n276nHFLDQ93PFvNotg z#QB_lgHJ4E0J!C7Cr|yMdF4=ySE3rWImKkE;lw>;bas%|zm)O7ZwMJ=oIntIV^XWp zV^`yl285jQ%*#;^w;#Ln?9x65>6%Z298yC;VjN}btM($WDzh4`;49PMe?Qe*KI;L~ zAmj-QCX8-JR{(Q89XU+4-{aQy8)2#@Vg^qbp=5a=N5YH+ZGszPvueV*J8_v7uFcF! z)Zm4{56cauV7F4RM?AX=wDa!7FhDaPDXqSQ<{R=K!V!q?GcZ_SX#9nwi!@KbNzP*~ z5M7pX!@@@fH7PElzgx}P8AzlP(2W29HxamB84U`xMDva0Bjy6};S-iDamaEFuC5Iv z(n;+njV#-5mRT+mE3B!p^$%^2cW1yu7~XEAuENMb1;i@_JHrJ>E-%#W z4yrLVasa}ulrGD>{ndsr6eAP9;#xUSt1^~1!dZI(`VtxZFUTxqi^z2~?BTbEbH9W3 zs#$_cAb%{9VW60+QM;hs>tOPQQN?2KvjIz_V>IvMJvuh8zJ3?h_8rsd`h8xac87iI z_gGc07yT}Qy_S|$X$VDK@vVB3)Plj9E1uRm)HGsW3PN)|dGPnOp1PJ;M%hB&!qZKB z5VB(>{DW}D{yC6lg;+ozSxIW{P`W%=h(FVJ(1n5;#R|ZBJsz(_Zt+)qL9_xr$eE#* z+ep@d4zURGDOu-Ja3z9w!XBSQ*2ry$Xf7okADgIaT3i5fQwA42pdf$0!jKeptF zr3mB5*{Cu?{Ng%HKHw0H9F1Z=V(C17t(YQ9{s1M1>f2MRf;|GW5PStn-unT`%9Q+c zf>E2&E*>2L1Ys}yIq<@r4J{%_y}3qA0*0553XVq3*)rsTog6ZZvM)kuWBL{kMuX*K zi{JlDcu=k9A}I;#H=!I#Hx=n{TE8?G-(l~57Q(UG>3Kh`85%`5M2FfE-1knQ^yps* z^s_%Z9!z#)$3Y~(7>d$<4vQ)rDfuR~#(n=Z9sEBK2LI3BxYupfri@NV-Q*5!rTL{+ zydvowhS)`SPX;e?uM2)dU>-%5)GGJA-ymI{hMOI7MEt)9eJHZTo5)4zZzCXC*5jMI z)%NJ)o6;UI)6u?_>;Z|V;!1yZFMI|$K@_mz#xMwrH8q_cfrryZ?%XIN7iAFF!5y=^ zbODl!cwPiJ2)m{WV=Hv;g%B*@*BY_f-CYwz7vN9+4_?Kp0?mt=lJcd2kT?d|L3f2S zRhPn%POZ2Kji>W>b3H5;YsL8pAv`eo-HSyD_{fNVxy-viWeW;fQlZ^Rv=4ooAf^Me zfCt+gV)COfYHOmQv4Fc0W>6TmHi{z`koQFD1s%21mR#|cSvIq)e; zf&Bq*Zi)LOlrFrPxEbL;A%I*K=7|YtIUq;74ZICepAVk_eX(qW&wzGpQ}R`)0Lho3 zmSaK?|3F?;U=vqBBumhyuD2g{CXl$X5H(6d>cf(Xmwd&dZLa`9$Om#W*0-T#R7N#& z%z%H9=*mfYVm(!49nY>mmOKEmLnN5zA#trzTrCTJktRA;Ezmuy@NK0OtQYrTFB3P# z$y>n%_Q`g~S}#%v-Tne;HA#zl?p1Ux3s# zj2|usaIdZLLs@ekDnlbxy*M2)#5#e#z<;Rh|rg@0SLug3w*5Zrwid{yUUh~9D&8USJ@^636072$$0$ML_+`+ZHf)&4NzeE2BCbyb@{zf^k_(`p!6DH zBhOE`q=?>=@J>{pC=>rv&a4+eUkb-J=pt*xHna;pha)BLmNn5(G#H`4Y7H2^68v@k z0-e3rkYGZb|F#SPXg^Q_H`fBKOVoI%q}#U&?T|-sYWfa}dwGRWEAkmG~R{pD#WTgm4|X7}R~ed`93K#KMo62nFCzgTEUi z{Ehu+B%4r{)fjdXf9JlBDlZqWz;s1zU=Ca^^Z!Z`RGWG_a3p%`IE*)OSPuUkM96p= z!olD5q8Z5u)`t{MFK=-I>@Oh`zL}>B`Q5ysEFYhpvH1OQ9atRmCG5dywU0_Z{SjA^ zj|*dl20ZhAnd#yRaTkyd*K`OYGxOvmx#@$qf$JW5}peB2jXmHsB~42R(g`Seil0 zJ{W_qpNQ|`g!R|sLC0yD`~f$d)`OVuZ08Ez0T|dw^ucjLgBboOEkpOP0(U5ftSq65 z==-rb_9&W3`F^)`Jn(cX-?I#In=G#@S{-e-M;tvLkWB@?PsSd?435|q;TcJ>%q}+_ z=I+y^3uUOVMk=BgS59l_aTWI5|Uflbk6q`TAj0^d1 zyLe_ZDKY7lG?@DrQRRhe2p68TSQIZA56E%NBH}e1@G~KoHksA%Xpm z29X1>G@!l6&wB9(Y=z}87JtI93yBSQ)5ZXN8yTYM?sXmL#xskj{TE~VN1sn-pA`ML zPfpk-$@Uep~caGxO`5iaE{$$!nz zITGL5#J(d;xMTH9ImXKVdDu#yh#GQRgohFR(;9HBa=%u>fSd=u8j}cm+=@B7BhH9+|<348ZJnzzl>% z_;?w%X%;0T6}wR^}6oAGY^=;wKbI=6zYX zX_9*5TihFK;ty7Pn`p?zaSriku8c57-yiX5sdvg7mUzf4FZ|m#-pQwFOZs%3miIu`wT%KU7LB=GAX zF!kGMM6{oI24sQbu0O{T?Z$gM5i7P20?`byg8Y0pGDW0V(GN6`0|8N`5$nF2Ku#7b zZb6iVD!N!v1*H8Q|S@ctcDzd-iSm|+0`IYHWTO;YQ zg&3|u+=5TARv#%u9PJ8pAaRN}mYY=CNpeE&%vdU2xu6p&y*$uDCAw{;$wH-l+lWdO zN>pkBm99bvQz?VF&y-ztDlKc%sq|NXB`W>E&j#`QcLtSSf*5uTDg_Qis1)hIl3veR z>2*B)M}v5xTGESD>~uqzT0xY-8;6Kov762X@86aJjBXCeu99V8I26mX9rSo#Z{6U_djg{V=HIAva8X^y)@*;%>7%UR z>(ulyG2i22zSE+<37*peFI3^$Tay@pPv64wE&E&YYnJ&9r_VYHKngbMf^>oQY%uX3 zNpvG4@7v@{lgPz9LYoY&VjA88P^3D=19$0u8L)Fl?u?G4w&FS(ocx8&IoYrFL}uDb zJp{RqWF+>s*&wms=`}C;5_+6fANV=uo}`0oT$F2Y-j$}xC6>~J`%eR!(y9Ag)%}OL=_l& z=~b-66Qx1AjXlj*SGV_xqvqLa!hqmi0|yX>!(aM7f&bq;C>{5S9B>Z?5^PfbU;A$* z^(q+O1IB;nWvGY5ZL^txyw?Z(LKTAx_9j9^(DBE9-?T`7>ZC8>>O_W^5^#z?&ei82 zr@ov(1-j!5l)$XmE^k1gJq1v(U983=8`Tv$p{Ug#8reVa*c$B*tzhrDfF_$;Utc^G z!5990I;?)weQ?GDyQCq?bcZq>UQAu&QO528XHFliOdqODPl68snnXnlPF*Y0M=8_C zDAO~Q=@XUdK4tn8W%|^prH)+UMd-CBQa^xYQn+D7KXieJAee~4F28_;+GMqvR&+W9;?eyw&0|q!I(P5k~y+P zODP@E1TA{DwqzN;VB;RfL(}ShCHqQkA%#IdFTQzE?%iV;WRT||-Ymf;?tcR^30D-h zpp=JBX19vQqrbH0iR)HSD|=!L6Qm8~$9q{+uyi}{EBJHj>*jdBpk-V)E4?xJTL`{G z9h;=yHB>2e90Zl&E9kyaq#>E?6i{4HD?C7tR%OX{vQFvP3S;h&V~s}f{yelo6JA6W zHd0c`(GE)KZhYewF#LC~&B+beF;8S`V>5l!1e6I);6&MCY$MVB2tq)63SmMOJO^w> z!X~8jeKe+QfW)BQ05+LQZ-5c_LIWh?p=ncbx*rYTp)hKIn_kcxK%0=sfR}^J>Q(I; z)!Qu3CVs`TPUXs~ToY;uy?dfTu7ajDPi%RX8zi)m^4i_^V=!%3nSd!j!zgB94Y-6T zn2u;r@LGC+j|Hkm6dVmc9&ahKjEZFX$s+yK_-*3DBfqrgivn4eTD2F0x-Io%gZnEb z4OE7jP2ArC@M01+V~FZ+9Ke_IMjQU)=rO1cFHD(!kX)uWxjr656fHeZ za)_^&1FccYMrx7Nqs3P&Xm_LE7GG+zZMNsyq_zj zm7H>eOlcO82GORp9D%snCYs9<(9O@)mQV(`|D$r?Olk>X1O1&ws1l$33)2LR>hBV< zh>ptGQI|6frJ0gCZ7M8%#;Z$+ca+lgRD9`b{P$f8*HRnlOIB^sx9OYqWwY~+wU2@F`6&3Wa&v+m?32I>7&kdoMq3l&PE;G z^m@iW%@v9qZ!EKk&Iy*8lrG9AikiX0JQ#Fd`k%*LtWWCFB28(hK7qS@|!zLCZ z3|;0W1VxFAQO>}1dE4u}VlPH|_d~;JbY1Ai|4`eV?)&cn;?N5Bzdgazo>aQ#wgeA0 zBm%yoiMAd%QF!rf!~ia6>T@Y@7y`4!X=b380-Xr-38xvz*+n}YNZfyGRQB}<%obmm zft-CQ0)1kO8OYi1=Im?CKrY?K*`GE8Ir}KiUS5xhIs1*AeX<$I*(Y-L(MI4E z_u0B7x*z&2GtHIwpU@ZUlu+Bb?)$wIwsw(iCUMS^#b6*oEGy^&NZUp+pax;*#*C8-lJY)uP_SKyIf6PG6zL>M$WClu_ zaCV;=$l0&w?5SoTXHVno=a_+r zI|6-TqZ!DhujlM5%|On+l(Ro#26Fa=oPE9-$k~fH`=87}&YsEHr6Jm&*bbCMqtld z5zp6Tb)XdltX-_zI=Iw792v+GF#05 zBek;wZaw3xl?)idNd>Nvp4{_ORMc5)hL`X2i+@6W zgUQi8-vR$YRv%SXdXYxasK08^OXL>ngmMs}Vy}SeP@i;XRE6~VnEuoZ_(b2sH$@qm z$#?APjBAbB`NX$xTJh|0Env3x)-OE96q)9 zf|A4W#pk)-sr8F{kJzb80q&bx?%~uQi(e~RNx8CQuJDkI=4Ajk4e-u1?yGUXGYP&L z)*s@Wp|+tx=Oit6RqBtyZ;F;9Z&eqY=z;9W7I6C7KYveSB zNPC~b2-b5jeVrv&-0+XFut@4vG!@VGKDOUnCw%Wez%~Ora0ph>=ol;(%?y;Zc{bO}{qVnPg zKUAmpKf3d+$Ewr+A9LRWA60ebpUE4-=nTxnj1q0cY?F4Wp>6cV8cncCWiTqi2_Xrn z0e9EkQfiey84In!;3PGV>sY&@wOiZTmhQ4GyJcIX*hbAHG>JeVKnej_h?d$HlS+gr z14Q!so^#)O^ClUP_Sav3Z9kHE@7;U;-*e9WpY`y(>wTT2*+XyaZHIf)N?ls1r+p`v z|NL8*EJ69E|Ni{1P=5Kz2a8eu=J)RU5z0S0cXCB%Y3>t?Cw#lJblOi2hOg-?oqpwS z`tGp-vFV@T^?r9k6!0eg_L0v3-jx4cnh$t4Etq@?@UC;Wjsjl#P3JjY0uyD*jxYASy~`j zD7@x@Tk<+fizO!|UwparY-j0A$xzvUv_?Kzw@gd;778Rb?qnq?H|F9 z?|#joX?!}DbMSjrDY@l z;J>=;#oH3)0DD4ra!Co+t{>E2twbj-;ObPIzr=dIGN*Q@6IYYn9nAVURxM0 zBTIdeulkJh10}?<;HdpddI_G_gg2s0K0iFH`2a=dfyZ0ZEbp*$=hhOGwm)OxO2Ttt z;`$F`=L$S#I0`%o4(zA6uec{3Rq5@}Yl;&59WtlZ%RhD`#XO&+ZQdea~+b1Xs zvX!%umWkh5_1N74PN#lE9ErA0ReN&|sYk@JtZ3&1WmSQ4G_rXr6^XV@)k3jo#{^|r zp@^wxa_UFam~sw~BQ3WujL=B517$`;?TC6JClu4_hZsh&dO|rBY0Q^}0Ai|oB&T)= zKx#+0KnVbg&=63qyNV0d8ijgQT&8`7%gn^gl?aVko_K9)u4Vyz0|n4okk zhj4&Yv~y}?vng#+@qiW@Cf%M;kLJ`4LUWM30$GeiT3GvH+5PIWAZSyU6-L{qs>||W zFMB$)&;}?@J(+{rofA~(uzrJ9JE)uz^)XR99BH|caSy4>=5eiJ+!vrOE6{3(sqRL| zn(DH}ga%2h^&6?~kXiABx@Y2=~P>*_2H3;0Q z!Ol<(lDYBw0|y^u$;@WS%;ajD49T>xWK@=nQr}{V#jHm1Mr#=sz!zk(l&PW3L}@E1 zWm&Wm)?rg1j0`QNeJW(Zf`|#oXC9=WE}N}}wo=_~2?CMy&1cQU)KE);K)P5U&>7?E zvPnZ$gB<6PG(xQj(%6|Gjh&`6AOMpyk|S#^v<;$ALz}=l(bkRRcYHI#?VyCN0zpBY}xXA;#1| zE6MJDwW?PQb*q655RO{4jTo)kNVFa{g#+th`5YcAAIOay*`8Kd2}-bg8dQ%cXvr`R zeB09&X}Lz?8^YnB@GJ+Y`Qb!AYVhzj)DF=+uc9wc5mi1mXo3Ka=#=rCkJO&NwB z+5lEidB1XoTvyA@REJo%ETLPi+DY73VY%D}Vxmo}+DMe60=EuEBS1%0H?@wct;{Tp zb&!f~f}2wJsa4V#ps|>808In>kF-o-CaQYWDmEZss2^7MQ?1QlfNDt*p+OD745%U+ zhf%9ntz!GB+Df)iwUKNAHMfEdbsy+c;g0LKQXSZWQD>O_+b|9*flkO4s-T=rRISAz z6NJeo(n2*&8pjHxiv`jOnNZ!<TD*YZdK9!> zkO{9>wHdIC+H9CwRSz^Lju5(GL~Mx>+s}p+M4!$H5-GUswQw@2TM z5Szlj*r4{9Q4NMd3r;{z0NkRTf!7waHiB`O1jZD^B>Kb5@CZOeHZ!B*5F!@k6L9du z#NQC{$0ppn2avWYy$}$jM-C?#Ctgi%_+g1XqmAszBevURm4S`W8NyM*Gh-A0u`GD`@J z(k>x5hC~SwGKjuZYIv_hZ5q!wz0b|F#wLPLd2yIDNjxP5Jc z4aFryEz*>Y)wsIH*bp~Si(HlxJU43vZIgTewJ%7Fd_fP(mW;@38fUOkkqkJdp2Dh3 z%2}zk0tY#jdL2?z&+Tbb&L{{)?bJFDi}pwVal1fHcT5c(CTAS#hBHRsGvXa5MwD=% zu__@n*;WPM0~?Uub4@EmQyzt4aKU?8l_4cG1c`w_tK2d5go;Fjk}tS>ip_gE;EA=+ zF$KIJ_EBIXQbU_i4Y3VkU<|O@_Nhb-l1jz_$b(8~192Z3h8N~=B&k8|os0rjg_sXS zLsSzfrOeeR5QVt$kpl+%1`}W&ssQuIW|9`_fuP7TLYp}l zoB5=agAyVS3rWN=SqWOGm6H;L7LbhGf@OfC(^w3Yt^u$i0FZ-VWXJ&NqfG|fx{}DC zi)BDL2q-gTfc()W18!d9WPltAGC*X53}oVH3V<2emQ)7atT##|4H=jzB-sMO-B=k2 zk{U8ZV1f`(ZzDuuNr2Kc)QBq|8Cj^C`9a$z-9Y1{9^{Qu5>UT_5^N1|NwXk(SON&R z5Hcsx$Qo@z;09zD0@w%d$QgkX?HRSUs{1%y6!3w=>LF4Scjy8=NFLD`&~K#B6WBmx z!bwu%Xz8s5PLoLGv=N<}s*NxVmN%zU1P$A8ra)<^D$hj(KmExvHK%7#)UOD9m>G05?nd)6Z7$%_t z{-oj$C1Cq=4!ZX#RY;wY%S-w;CZX>Ir5@Q-oW2(<`ZgH!x!48jNPLb!fGj=;j(9_} z7|Ovie2DrSXlo$Bp^I3*K|vfCR(6rZt`HF5B$URVRH}r59f~G>GEig40(q$2gy)i3m~wOg@;bPBh<%Tq~T=3 zWAa9j-9dUoB)uU?-sU9aZBWn$v`KG+McyWZJRdx|2=!52%lJ@%wX3TKNx2j;nZ9Nc zAhPuOO*w~%NKVbk{z#BE3#=a|6O57h4U+lMVRS0p83`m~%x|Mo-)kpuqeb9mgFwHG z9?1wis6tJU9_VEHo?!WHCekqC!GS)B@ySG9s#hoV>XN*cB;;*U>JQt=+hmcq)gUjE zqf;yyd9{5QR3ZWPfkYcZ`IluP9%v^)hBy@6k&ZU~_ND3><(#9w1=L}~#@sB*J&}E|cK1iO zyVWxSf70-Wl392a&Fa_ku+!JY^bv8} z4DIG(kZ6qWGny_^2oMYpGt9o_c_uHW)Ni{8*LP5C&)MbPp_Io+zlQT`B_)j^a3_ZN z>;&$#2<$QloTg#e1E&6=&hG^#5N#LR;T<6is|=GiuOL@M-Vc8)?}x~d^Dbr5cIVff zq;){Qn}|F!c0j*I?AU3LH(f)*5ISaW9fthil?QVuyM~8=>s!y{VMKJNa!T2MA?J6H zdeSVtA(Ecq{Cbj*2lb-w7bg$uHOT8W$jhgE3GL@fYFp%(@rNC=jvi6%F&^7DeBTBl zkbGY^+J@=-AT^_HAneNM3*U#fG0ykxOrqUhI0CzNkr<$Pkool(^c83!q-GpK$2sR? za;W~8>035}KzREPb$*cFuvD+z`LTM(SRYaY^bF(87pZ|kU#~%5ArBQMb$;+I@O{*O zI>f$Z6O-3V;6H*wcW7wFI`Q zH7;a*Uy=Mf=GVLr9`!!>JMV+{*vczrKVo|QR zYnrH;ZYz_GGPx)-4P~Z_+w#St0&!QNs42FU$w!$2lqp1+V(pRK5}aVV&?)BN!aa|8 z5JDHlxtIw%8QJ)0o(74jC#W~@4*D+2adA>SlIv98&B0966#o2y`YwImkHjCNnnU>P z!i2?pkte+T`HXs!KJSYDdaANpyob*o72^bv=da<n!6Q|RXaXOtj zEnv7F<}@3{iPP!HIL&8H3mLADNrIl3)BI$d7BHv94A;*jO+#^FwICU*gSjKbuTXw=Dk!fgSzBnSr2 zd^(fy>0&VnypSm(QzzTaOvY&za|*AB;>Ao7nL24SD;cNR z%;_{Y@yKq#?|rai58OH`WV3ssi6ZanPWLIf62S;LWcI|sbT`rEghK#5vMPCJPa0Cso1oGWPqbrBNWXY=Jf2MJqidW?o6h^25K9)v-n>6EL$Cxdvk}I0dF)Ut{ z+O!$Qpc8l*D41CaaQ|3N9$7LWrq2 zm!*`+QkpJnl9UQBD5YYSl9Q#B#ZtYNBFCe8XmQpTDshBMZS*Y3IpydllDVwD<4N`&<*+0peBqeD10#cH$bvjGQ z$x{Uk*(^1p=;1ednw z4#g}1Cxgvm|4UJf{BJRs=i>3)0ZorQlZ(M-v-_ne)0zjnAc|rX zP@T#6bTOYk20M)jq9{gux{~qf;l|--MyG=!j$*{8CmEkU<};JQ<}*bU#fVSecs?nL z5u;hmXaQ41QH%o$vDkoPG#~{%p_41BTloDahlDXP6J%% zm3$v1Y{Y4HGESv?oX%jKOc5n)#A$9aPNy-a`3zTPg%z8K=d} zsgpUCsW_QBaauf%Q}60sjx@(Yr(=%GQSEU&=<}}rtw6^$+*yM1CFR}+f9q|cD`C8= z-}A11$GiG>-qlC3h~E2U``#Doz@vYT^HhA%cW{wM-L=>;gTdLnJvG94DKd`Tt~Gz) zMe?n#kMSD$n08Y*>YNkmvu@$0^|Ls>nPAkQbN^Bamb2#V5}jh7{%*{XqE&e`+=+!p zY<_*}2HLXWb9Z3~z}>b;bT1Uqd&aEL2fW(;&to@$F$95UIwuF)7CO^tWquv5m{|Qu z-mQ`Jz?@{|=lqG~r(U=`##68hrdlj@Y0F$1MpP<1VyQ=4=FxDgD7Gstb!y9;S`bUa zeOiT21pQhCF9)y46u~U5f|i5VXcgI!Rry$RZ7tw7_E>;^oGOYVkjj8zxl03*J6vM9 zhgO&a$sHcC+(|3LmpW^-JDic_zQ{^zMfmdk$jY$*|15aMitxV}JfB=Hy*wCM8DGvV z8UC~AGnQYU2VOJj<0xU_=x`3pnn==??1u`SO`jY#ejcyFO9x!S0*zYG+BPRDEqg!F>?I zDMWX4yKBvx2@R!N!!&OAv~T%Qx(z3rZ+9p`6Nu#E5GlC>4`5P4e~*cV=<&3s(YmW) zRyA}Y-S)xJ(dZx2aDJw~2&I7&n85}H8?U7HS?vr~DEsE>mw_z2rfprboQv);IDvLNkic-pi$sXhma)r$bqKTwyd}N77ZWU^ywwm(mb%xC z(Zc9J7E5Mo^erb5={^|kn;JcyN~B;Nv8ZhUY3qBhR1ZY^C#s{--(5-cbkXM)l20oZ z5@G(VYMUFxVFtkss(0pKNqJWej#V3RpKH)+>C~6}S21+&=jAdZ$lvFq^ zH0_pJmpWHkZCc+bwZ18?^?d|xgkWuEtp^%2%9|ZcWNzsZGPQxrsd>W#mv}U`Lam97 zm@W_L!NDc53#+|{)ZQCcyHq;qdz9pPjFnBA)?olK74xk;ty|xdQ`>8)9rN6*_6Di; zMpipFsK8(bHwdZro!H!fb^htND5@M|@7P+vts#p=({1Q{AS?Q&)4fls9P$7aF*JoP zB6JvYtE6($K7D9m-4l7O>H%6_IpoHSL0~us^Ma?{^|73CsJ_chB6OdI{0=w*gXvX6 zQs>)P=d)Sov<=gs=7Kt>(uU5dicRO-3Z>d*J2YDQ#c7^vv+}v}1@!+!M)^|?w$yaC zR73whQR3I0Ap_Q$*nrDn?>j$S|2s*n-EsZfoBd&<+4mSNzLx}z#*WrR&A!{xzi#Ni zS?d1@);~9@g2b&Ax@2eQZp#Kb9WS?Ith}1d7Of`d2FLf+j$v4HKX$b`v0z zH%x%r;Gbp!R9l*W3?ZiDALjzrWt2bXXd+WgkB}7_CXgvgvb1ODwbsP;T#h7j+lQOL zHZp;(-nB@ZCp$r!=n1fAJG==WQvW4;?n((!|fWzsS z8DzIFUBJDd4yXLkzZao@DsAYWs@U~UCU5B97DO)6`E#3=`lrelZ~@O}l)vC;B0EfH zt2OkWElP5=XXv%o#Kw%c8ff`&{kM?*TjTm469S~_N$=e<1i+-!+AeBawXK%^dky_R zC-wh4>z^A|peN%+B1h^UO9Xq6Y^0aRG(;$vfRwlq`OohH`b-yac)SbH-EaYi$p!Qo zF5nosfFbDu2FV5JV_ZN_dZ^cK0{4SJ5m`+CUW5ryX~P7liroY_KydKqrfERCSQDVy z(gcj?V&s0~T)_H_@(qqAGR1T@T*CyWiIVBsGxS<(VtYo6MW*$kS->!vK zx}*ufs?^%nocdub^0!Q&+c1F_qzSBN6X3QL*qQMnkt0oDkgiM%^rV*$X$VrVf{QeP zUeg46#+yLzI1}h06VRmz93vCx9b*DJ(?i{M6L_aaPyN*g9XRqQ6f0YaL9 zEv8(o2~cfm0!DN>p9yTtDBt90B2!Ff!!=AGUz8N!08+fh>7w|T!Ys0`4>f^7GJ&D< znm~(X0?4!|5rkFI`n38%S_2C!aN7!O%XpE{j-{d*Dmr_zT0sfu0y93Z6rZ87Cy^-r~>{*CBz9{qF1n8Deiqlv6Boo&~! zfG3q!nyt zE8yl8Xvuhy$dOiX7#G7MLrE|1(-5j)2p8!Hx=cr~ZM-AscEc98Q7q^(V!=*w1ijJ` z^pGRy9^(jF(nGCwN5J-Kj|F3T>+!z2*<0sUD3xZ!gEaKi(N#$!LK@J8-HB6vHl+4tgAv@KY z*s2k1k$Zip30%}bfRPRwV*qdihm#KkY?UU^!Y05ij0OUT9BBd<9tb#pI?!sGK+AX& z_`ezm_!avCHdcEq_;3@TD$)ducwjeyX3|{aMe9G%Xeht=0?MBWgc;lgbg*@$mp2St z#$y3xqGY!A4B3ztXrKW5S04yym1^I{Y9~Svf-*u4Qs;kLqX2T`XPoo@aD`J9sc<6z z*cJX1Dg23xSNIc#!k@Z;!e_I>gZS4GXiP6}9`LfZgQ8@fR@JBlnkkU})%*NiQq?<2 z)m4oMG8v&}spr4B-ad|g#?dwFMEP?U5#a;t43#`*kL~Q3IkuNK7(gyIuv1mG7k26A zi|X@O$@5vsPo$SWWhi;RD7jawdO{04MRD@4-mCAHO73ALKY=ik5qe50`ERQ~zmYZk zyeNO+Vl@1`q2U*78iwnY>TUkh)Ej7&x+RZ&KHc8Sx}~(v5m=XA{+yxP`$Wn8TGcu& z@EnbV|J8f!y;8S_S-0yDD>6dQN!|X<_0^?%pAh9wnaW9=M;V`)1)h*Lnj-SMMfL@5YN)??ywtH1Tpi^-_YD!8xHLus*$f zgQ4D~qNGl%TCZU$m(5>$dD1*Al3V~)yL*ED`4}?AFkWyrEbYP+jPr4 zUZ_{jdQxxZqIy&X(4JJXIl*#1JyTwn!5t|_U}JjuCPU9FMad&LhDHl)qEYL=dS7}- z>Uo&;yb)0%1DpGKF7^LOFPb~hZthCUxzwHZ4m9_ujb5~<+Uho@Q@JDE=EDSg_)O6I z*pK=s@5f@A7Ke`_Th+GoGAHl8!66nq)5|>AdP6B|RZn`EA6svzr&4t|y)4Ut#RBN8 zRvk+(%XI{&k-V!0)61qig89_Os)p0c3LL>gxYI@HWySG(dyM5WxF0Sk_v@TIdY=6{ z#`k^6zhnJCc(5+yIuB z?^oxCr?0Uw7KcZ6fg?)CuNtX#cVqclYjl@u&0C3QlfJ9I=I(Is!3|e*7&6vOD6Pgu zEI$9(6V8n#Oy1aNf(=8e62OQ!?osjiNZJtVYItXYpL2-oZ&Xt zXt^)+685dhg=9(atF1Q$T;6r^tXSgr`48cD>corZm+ZaXp8|!}Wq?s|7{p!B|K3ZD zkM3d!xQe26lSP$pZbeXE^esMy*}mj0dHsJ$UjLelt^YMX_;(!sqt7M7PbO65Qo6MY zS5fy0Wwg{|JMWk1qZ>7_)eH-ne4~5ml5<)pwJ_h?_${=v8Vt+j&2sYcshF{RDkcia z$mcj~f(p(>UJSI>0xBFT2FcvC+8L^(iSSPP{3=jG(eriVd()o{I%};Bz zXpIJbXVGubOFxM|ydDv=oa8TXsof|J7stlHi09YfJ_RjL!+!ZW&WN)+vO2%R>1`OD z<;2B{s8H1Y@XgpBRLu`lf%S6LFmck2Eg`w zQTivPn+5<|koy1N!uk&;>OU~@FxC;q4dACwnFavm<6bFN{~_F)N6aKqfB8sU|EP*m zs08hgkoJE-_wAJr;pKpWy}U3icv*F?HoKz)kJ`MBnMH>}_%ehyWq6!zVSo_;&SZ>B zlKI_j;4i0zrxi7G^_SLxN>F2%!QKQjApKYQ&`PoNio#f4SG6c`T$dkib>0zrkFQ{i z8w-9>c@(Vjk>3|x?p^Nf??;u;m1yh$a;?~v*OgcwZ_yO-_5{a=wSHH=uVhZ>lmVLY zyS6&f^j^xuKh@&IzISk+Vp_Q0RTerijsTPgX51GUnxMAgtGahV6GZ ztEHqYkP>f~XB!KyL1nIQ9bX3cE8&7&-XCBksWwqt|4m^W#t=o_tw zprMz;x)ZCx!!hTIey;7%aiueCaxmKOYIws701c-n0E_qORvLjL%3~F|0q*364j>wP_Bc%NPUFsT@aQhlNpV?f7 zcI9slregi@mEJAs`97i}KS<=H<_A;P-mc`ADJH#g|9f~4o1;FU=~>*V_9dUFD2%Ks zzQgO^&sQ@zWj)?gz^^qovm_hO1T8$Iq@^9Q;nQ{DV>3fh2o z%LnuecT0JyKu^ef#b~h8Y5PXvsR7$IFJH0=7FkM zyv90%s#EH^7bR7vRlFmrR=@9E-Add3SMTsXh~InEG@CAGVN2*T~LsX*lg5G-BT?f@?X@ zKK^p{85i18OiK7E|_s3$J_5|tBh%E$?P{SIjguy<#_uY z-^!BNZ(OvNZ8FHD_8YUxnD+ZVTW&IWv;C6s!iKdEv0plFN?s|BO*u*Ks~LCSI4&1y zt(%PL<)*ICL=M8p8;q+)&>j#dWDY>_L&j+OCI5%-l<8nI)#Z^{qfH$lWZNzb7j=th zUK{ccJgIv{^wj$zx-hS;mhNZzOUJYL_7^d$AXJX)Xfmm1iXjK?oFZ5L#ScCh#WhGC zTr24CHhmG-TZUKXV|vhDD0HdbXBN|E{m`g@?OV})?48ldeSmvc(`(+wYw32(S~@F` z_LHDDPOw7h-*35TiZ6)VWS1U`VV$l|OzOs)3V!qNl)!iT!tK6rJ+6(}j_oTJIfvl+ z7{qddz7tOrnX6oR-Ql)aWFa;@xv;BD+!nh|5qFJTH^(ogTvwebZgXBYD@#lwte~Ny zlKKRX09 zh;t*i#exL%ZOXf6`L4SyCg%9ByK6*LXI?i&%*wj%Hm7iFb9}V#q}nfTb!oFQfhNwT zHYY2z$v|Y9iM7^gvSfl^48F^Z{FXE3t^eK7!u?gx%>b;)8 zFkfVrBD%tFDB*3Hc~RJHitNJtqLaHYls5xj1HJz!Q9%|30_yHfdfN_4AdmC?W%4*v z3| zX9FLF^oL2d8m?{{^?iBbtF#+#;EM2Q_VS6ZX4CIW!=ssX6JDk6j5rz{NR8Z@ z7FkxTm130FRZ~%|PkS7a_kRbUQ}PZ#9k-A={t1JXk=vv?@}j}&YW=zn(n2nfcpLu- z7d`^2xA9(T+@smvrb65(iYraUjGeG&%4;*dTb$qI1lPM|uke49QVygRZJhhcSKckJ z$wUNi{a?o*2k&x288z*f)B{r|bQMk}q0Y+2pokjP=H!Yw9(tap&6zIS&z$K7%4k$a zn%pX!kBfuRflQJ0J?zIDO~L3M{;2Kr(&gRqI^F}VcZ=uiJnomGwyRNfmJe-Bo0YG2 ziCLN2oB}lpRC41DQgCqZ`IERet;xG(kF+?* zpR5U+8l^44zpLGvLRj12KVeeU;56%1vNmZ?H?aM-V|W55OAfficGl&0(JO<^JZb29 z2YrLCVb{;f@6vJ&U1N8m`gPKEWWG;zg*%;^TLhiz9u1>_WiEBQR)(#*9(v9Sk4_5- z^kIJqU8mesk?(D~15HaTD^%WBN0s*@rNwoNBl9-7B4tLrSP;{&rSk5Wwg88?d9=G@ zVgZgQ#+wms0WL_vn-Os(y|_xdk{+)T3!MDjsV#8wcP9u6zk{x|FZ{Nz=-l#+CVGUu zn<@&uU+V%b=KXP^%qOHssH-YD76)-=+?qEji-K5$=Sh~uSyB9oxVo2GzyE1*bq{{f z&1Yx;vPbFvm6$=XPW7XCxKK;naM&uN`k$m-$H{ORh|o)BT;45z5+WQ+mjn~GRXc5L zVW%9catt35v&D_XKZYC0BJ)(1lqu}mb|pt8!6)U%Ky_a6;efsITp`B8Xsbp_wQ^6< z9fjVIsSq_kQUlM)8<=%w{K>f-D^Fc9xIHPa5+~~i5CC&UX(fr|x{=wxw3lNu@KiuYz zNm@Nne8povPxfw^>*WVAzZlzc%S!0sj0l!t3Gptmbfy|Y`sl(@v)o$fu+bZ~ahrHn z+|Y_2*qs;chwC`4`d<+GaRIV`H4^{lv@AgEct#DuRT0`XeS z`lgsc*x^0tH}LiY?`!J(dH5>M$V$S$R%XGf|0N6;${NhkYiPu{SAH0cfzAiNy-*F# z-_sv%z4vw;p^mef!MV3F7v7BnF?JQljP@qk@d(xP3QfH*N5qfkSCSVRd%|yA8bLnS zVBhpcECig9Zq;Hb0E#6?NSCV>yaEsgw~%+i_i(Pdm_dTjLN2E1q$v2sbs_K!co;>K zAPRsvrY@xte3ilwlR#2egH9TwD)_lnh)N{^k+iI$ENTF6942lwu)`cymwFf`@SQZ` ziK$uw$zTA|rr-inupNd=UFu^PBLoANW7q`5bR`18I05*!q*iK9Z0PKh1TDB=IsaC)Vl zM))%QC8Eu!dfK}MLEH1BdRi@{;!whCxI+LS#P%_jB}@VVJQT?Q(*S{CWeH=Yld4G| zE&>9O@uG1EAc9(pjW9F@$dm|S5;4r1$@v16QCHHD4!}hikv(q5JdSD7&l+hkxY+02 z;(r#|1ROqfQhgOP-xBD92gbF#5p3?LI;MYtI*eWVKgvJ%$UpP&XRPlhFAD!19yI-D zAN)KbK|&xfBLYUY7hK}qvTB0)!t|f3O#ivc^q;Ft|GB{Qp9@U?xd8rC2NctPE`a~M zJ?=jb!Ec6Ji^+waM*JrS>QZs2`2jJ5@(1#MViT>?RgewlZ&~9YFXBvx6w%Mpwqyh&fdH-RH2W=Qtn6A-@Y zL7%bjrBKYSi|I_jDFM^vK96~XW&Xr^&=w!ka*ADa94&W1Q0l~L?F_Ool)UR=_8S$ z;VKgV0S)n@aR@Lp*fjO73|9zB_DDjc^M#5HSG^Ji_ZcL^kD{T$mE)bw5u;}LFgxNs zDl%j^QXWg`tKlOPTyv3_U;)}OJsaaHaHLntKbOfrZv3%1Qu3dxF#ecU+rJE7YWsfz zM;lL1<$cTNy`MSp?q9r1n7#vLveIT4Mfi-$bQb{wDlZshr^-`VwYR2{+{q3=f9uMiLUl!FG2 zpL!p8&xXTq;kdTr@!M=TGA$e#v+_6CaAa9H9M8=9z=k8+!qNL&!xwBgaxEM`JvCF< za7?ps{LekuNfPH{x`pGF1NUk+9QhWGkETEMMH`L+3&+#Tp2JbymaGab98cc5>fdcR ziY**}=h}R`4TlprNV<&^{_$f|-v=nx#{}_ECUB5+*Ii!n^AeoeBXL|J9-0mubnf&M zlPd0B_;G+@eM}ag1`0Ys{Q0YY+_|M6)UrDK;vv6DY&Z%n9Q!BU*lxq&v2Z*->9@l+9N89*Kl<+u z*l-kBI41P}?mulfTo#Vl50B$|C`(jX7LJ3d4}IQ-Bj3WYcJ{Rq8xE2cDdTfDogoup zWjMt{*}y=mSo8j}#{q_$hFhwF6!Fo2_|BIAgcacv4;2DB>A_R@dom8z!xU;^686lu zUs-R1?K5Ei=k6nS+F%py1pB@_fAzi%_A~?b(w0|WvB4(U5ZxWCzl}o_O}amAfIjp0 z-+$W%nnX{aoAQtM*g$6*peJ12j3b{dvPrcB`ljw4n5qf-p?m}O@4xl>TQ=BaIs`lR z$Pccu!FCz2|G9eOwm9rhD+k4iQ~TW~0ej1cyY*lOcb08oTzBpA5Phz z^(b!R=~Wi^B8?dod_+YSA`divlyBQUsw}`^uNf455O);ZJ0-6lR~8iFrbCK7VwFGI zk*=ITGT19Y730=53O>=!D{%EQ?kdMk*N2n^C6PvjLJxuO1e~LEnYXfFCcsnRk>HOq zcwjAyY*s1kjDbBHuqo(_fgJ>F3OQq7&jV}Q00N3L7{kE*a!M!P6@Eae zD8tpm6lS7rmtYMqdQ;sw2h?t*;@-&S>nX}aJFxtCge6ZGqeH~`Vn2{jq=|O=m2>KW z9Q25AhuwaqVrHZf&z(H|lERF|&gTf3DV1%S&vT}mWfz-%2DAJ6> zF%LK>(u~2e7&s`-jKxt59291#Ge#KLRG5z-LJvlszls7)wB4=rihc4WX@bH` zw8N*I#7o@H5~IT4_-2YTxHCE0HbvQw3$j+x$^R5&2x^-+r=C$`%Bm8a7fT^VLOm;A zg6113z(}ZPRorHV^JtV+(A5-*Fm|Z-11g1=1gQCdO3@_&Dx`fQ#g?&9%dm0{fGNJ% z0nelGGLgaygNB=0c3))kR0=F)ryXu($YcVBeGNsHXeaKop@vQVq!2d%qJ>asArTx9 z=T04PpHXo)AWlbxBoJ6IqEG>YXP-aiYb5%K?Ayk4gOV%C}UvH2W*NbV_=ihqi8Y) zHu)KfC1YTtwWt(H#=tHEY>FdeV3X&dC^7~%`A3Q&V_?I^REi*DU>5>5#SaYplT=KZ zkjJCwAr|;xrlo$u(PmsenMr>$5dan`9SgMuK3qX*yL1xq0X?8U|NL8=e4Ca0b0HVN`r99_6lA9yt%F%i>339^7*Y- zT8qb*(7CMd?TT+9pmSM2Mr#h~Sk^3_gGY(V6SRqrk7eyb;k=_bl@+U~ucV<=oF*HY zl|Pr(us;G^z=E@xts_}&y-#C&FZuqeZ@g>mk9wAtnVb8ozF*D@XWMukzS`PZ>icv1 zucJGS{_C;7Wd9ZGcaaWMiUpa~qD^f_y5P_0RNEBXXu3}+&Bq}ntx8U}x=*Pn5T}$1 zTt*UYn~2l*Pbrv1JqaH{5zxp9l%vu13~@$1mjf>c$A@Vu1&7Tm*lE@dpdCu-Or;GW zmRzfZStfeXocBlD+z6Ct*Ruug_(lo|I5=z{{wPQ=PDMKufIXW7kE*Pir^G~iv=isk z0%p5XI$sj#3GyX>2$IAosT~e*)(1YWCBqg2d`Gl#QmX|J3&m8B_`ywxdX!pPD&|aw8YkW37snn=IL&` z6Yi7BviotVvWaZSeJbY!z#VX(fHDz1Bb%?_ZygG)bUxt5@xeLB<#UFpW|@I+^Hf=8 z0?4Ssm1{9dFd9?&CGd5-K_=s4D&f{|Ht}`459FW&mvhREFkg(-s*!cq^EXKGl=}n* zB6Dz2W6mMMHrv4VJY&O9BuVom&PatUm1Xxv8q-XKLvHlRfsv~rOfh@7HkPv3Xy+ud zDrhW&?j9m_!QLo~La0~yRr@74MIL6LEQ4=O<5#V2I^-AT2fKwG+z0*e%|(G>xM4Q@ z$hs>`ctf)C{K)fD$G}5&E8%sxX>`0@?Z5%_z_S?F5OdKEFEt6|3_b>r5x`b5^K+C3 z%tLrL;UPPqS2H6me4A6W(~WzUsSTkp-rtX)Z^9$NLK)-X-8&xMY}hduT;)?@+A1F! zp)j(O2O^CUWUE&x^)KRUjWg+*V~Q+FJFYU8QRS}?PtLcW^$m;+8v9uvrgi70zrC)^ z=5K@gb>DNOgts}3g?2%GpvnF$>BkZ=>7IE#A1yZROzD|E*{79Z!D+(&sZIyIkFVN8 zeQUoZeWCO1zq^YuBt73=MB{ZSute5ib7o5X9qn)I1DwthS~8Q;-szyH_;))#`a<|V zbs>DEo_J;8BYtnkUI`c0D-vf0|D`^IPdwN5Kdz;id&cmLg)FuGb;c`x&adsi-~L=u z+dt3#oL$>L)BYT+?JuxD-&fn8Ykyu^+wYG*FBx4tZL`PRic?W-Y<|HWoME{K2lp$5 zh88zKJWE{q*=uJl`*!+(t zIIvAvr_*rMy%0%btGPXy+Y_(U4i8R`1bS#W=}lk_DP?18T|PEwJ+KD1Y6P&;HWoxU z*Ps43@)a&##XKca-h*An-vU?$T6Z-zdC}Ics-C%6zsq2Nt1eBx#+EbFs^cU=UX{@2 znzPhAR!jn|&6&K$9s9|uEA{#=a+DrVLuIq$6H`}@gD^(@zq;AeX=&C%Ci-5AX((4vI6(9#@0B~2 zEhlPx$Er8W1;vRs{8Onjyb_DLGic)I+8-4!~W+!2f*n+o)bKp!M`68k#wO9}K=WA3aXNNA4AccQ!}ybZRb zbkyMWu-^53U@sr8q)mJTYpYSL3U@(tgJq?i%3>39Q2!1U4fHDQN{2#MMT{s|2&Vq# z9(|;QgC?#2hW->w%*meRW(=tZ;TYdO%3bKxxr9UbmN-WDnv#?P7mlDFJNv=t0;5`Z zl`o}E!LEPd$fHoj_^>yj&Z)KzOyHMA0oU@hO&p=ock&@FUf*gNfhhGTrDkw9><4@H z9V-+qVIRDq?wy1W8(DGXizBZM+b33BYQb!ee9nPN;&$g+=cOiBfDdBaW#$K0`}GMY zCV*brEj`OuW6_+JH_LzMr6sz5f$V3l_Q2EIcDX0v zAC0M~ZH3#QlQ?%&SX=y2p7v{pTxu((TSso1<%1KON*CYm;N7q*t}sZr9!t~O!mA;?^b~<(}Vg`*kwYE5-#9J^pjkG%WSxYLXG-0(1CGH%#X=~ zRry3)WTiulu630(z7{&2*C}GS4yM{QSO2#fYFPe`nKwcuF6~Lmzp#2(d)7lg%J9a= zZ#>$we)>`3Lle@zmWkiw1Z&%Tb`r>*r=5=5GdaH??XM=Au5Q{3Tf>xie%^`jIme32 zJ4@IcU}Dhb2%;?ZG`B~VJ8(4+B2PncYCg7gCw#<(M052i?3A1fA-X>S{s&3mUuVKM zKQjDZU=;)MgY&=#ZN9wyr2j(Kz0?6eby!7xwRlUS>ge3;TbKUyKj-i2SlOo?pt`mD1{O(z1fs-6_1^3RSZ8uTd4OF|E=o<{DKL zqsq6fD$rYsK3ihR*DGzb8CAL;l7ua`s!*Rszfx8y(X&y724>>4>EAJ`++bCiKvmv4 z#k?SYd}JJ9ON=V1Ru%Fj`p;#R`}AK@6+>tSVK;J>9(^|w3&}q6;1s<@W%0i7K`Zg|&Jbo=bO)KtsD0_P64%)W7RRtN(KG`cEgD-o-xiN#xa!ue(r1WgFN60ZYi=#z1K0_qI>EK*_lX-7)k4-FojQVc+gDNGqk10ltpISAzecWf%m_{`c+sE&hNNcaRYH<7b zlu;v%3G2ZrakQ$K9m>y9<3p`|=g$70K=3w^_J2F^S<15^L7vq$A0BQ(OG%Ww!lp=A zl#gPL#1LiUmqA|KYhoAxY1OFy!51W-&sY^%ma|ZiV@)|#i+fJ9S_i5nFpHk6>=JVO zogM5<(=nK(A36R3tjtxR{}w;cEA339F*PHUvlJx18|?i0l8q3NaZ*?f&DXyGd*w$Q ziiCj^eE~hs!AOc<-0o4L^wO{QK&&K@md6q#@+Hja*o?#E=l#VA{KSWsc1nLlDUY}5 z83?tq2Ez~fTeQ}34)*e(JCWC^^l~rc%gAyw=xv3k3<;z24y^)xjnE&dnNhFE)&pu^ z;a+qP8YH>6h3%D*IWB$0kmRb&qm(%-=*=2>^RSb1d|8)zB1N*U(GO{NEX2_=Hi6PM zquZntEYm+rWExL5Q{HO0L@V#R%XD^X>!NtV#h!4oNcf1IOLO%_)HLr(?h4KR3_+?? zUxzF{Sxxsb-sLPa`e@5MaGRWW-fXabJ@%FEQ4T>!y$exL|1{a_UC!bOp^xi1_zkc0 zQP6)65;SsaOeFF-boS7v^GqIy>Yt<f;!)PIKIqa*|sGeJ *aLGk_Vth5+cwEAtd>U*RzmZz9qG3riptEgbC|T|V?^ z4`P=$wq0jN3S1HYEk#wHhcm>j&P59i0BS2nar`ORui?XxS;y@V!>#Cfd6b~DW^T3q zB3@vVJN8;5Q=@{W3zhz(FYZ6q3^t;fS_3>oSS@TDX{N?ur04$*kB)W36C&je`onZ+ zWO)PJ0r}0&K!fv_lYDyX0!V*1Ub0U-0ZGkBLE!L)zlf^T4rHs2n}YYT&(at2LbMrQ zUUw4tazLjTcmph(_wy^pO$DdNT3)IjAaw+Wv~ z^**+op3vA9IR8cXulg5xfk(rcDsR&RU_*R|#<;N2py903+jKYH8138~i%DH^sT^Ne zfGbYJZK-It{0y$*T}Z_%RY9YZxBC5_v>}zN?unf%V(?Jm^zj>;E_WF+|gF=Gu_2;pGrG|5|9GeCZ&m zM}iJ}cxhX4iQ$uwoI)KWpCA750`ghDy5VJ#X9`>#lUliC^5P+cY9_hRAi3@z4Bgm$ zzz*P&qaeUF-vc8P&(N(SH&uANk6l9p5oie=X~^^%LZ1wG77pZ@F5@;Qeo;z3Fx7-` zIjWnKUlQ)jtX}O0Hn?Ln7VJP3$qE>v=q?E%#8Yo?e5mq4oUY?2EYHw>jj^=W` z;q;Wc?1s~sbsv$zz*gsrRvq}=N*wEV*&sMu$maT0A5QHu6sY9gN<)oTE$Y0L(*s;2 z%sBI``jOjChGUBN@$E1S9B884>8z=UR1MD6pPI>IkTqh5-ZTK<*u53dD`||NLNE9O z#W74;gg#pMwdG)??kbD~dO%C(=X@Sg0_Fc;coKf&OJNOFi#!{|<6QkSfV?)K13*XP zE%Yj7(LK#yyAMAM>LS;g5;5xi1J^rutF0cr;(E*C^{T9T#P39WfPNw3g?f?o#Z(>t zHT1=);1qo!WVi$)_4>y!UbDo3A6fWe3zrms!h>0HePeJ^?qaE#@dC8{=xsQXM4w-m z`^i~&s`HkGJ{MW(?hNSnQVaj*SKS~PPeFRBd6h;L@Ki=mx0~OJ>8a?GUxTx(n-RD2?S>@{n z{wZip3?Xmtn0do++N^3!C?HAL@HxN>9IJ%VF%5tzgJb$L{7OeJ21~4P#A_2hTee5`ZSY{xJYIE%-&)j-$;^whg?fimiR$vkA5?zMCc*hLC?pGn5pN` zlu5t?YdZ#O?3vqvu&F;u{FIa3Fc;KW8pMYby{Q(Z$M-89q}RzOdJU3ke0DLUV$L^6 z`h$t|UqpWiu@0h#!3c_sM(>^bP-`Df=rI)i9^Ykmw1m2NJ>#IAODf`Hl8l=EGr9O)3Os>y~6B_}lTL zAxfP`x>aKwgT_{Tm7G(I87G=R`GS*k14Ff?@l0v?4VKq#D8|UiCrtHUZ7Y+WP$qPE zKmkajAawylZk(iaKn`4|(b`mj@de;@;sIP}+DqaXa4G#9uL z%v4olsfGUeTd*VNb(#jL#px(G2FbD9({M|EXCOwFTX$6rCh(ueQtE2`>ORt;4|9rg z`krISd}%5SP2q4UgQVtZZ}PHSE6S?ftF!Gw6j>C`%9?!b#2nLbW6aDu5b zJnFBzEIfL7=z2iXKYMI+^i@Ala;O@@5-*$sn^sY&cg$e-#TaNf|5{8QUK<|Gz|>W( zt3ye-b)Xyd;dD!D9%p} zO_T8w=!vG7zwQ&^*yW+Ch^C+XZgg}j(KK*5O3z)A(mw>DhhrI`NQd*9lw0uAORO|mxND>=3LK4ebv~cS*0I{vqm2C{|i<-b>ZjYxQz7{66#8HEF z>a#Fe{n_7RxMVWSY7f?~E=j4NDS0=bhPP##&GmxyM~{uKzvIS)`bPVcGZg1Qp?($V zW|!U$KZtcFGnx>`yiI)dT#_-(KdXt3X`EKRn9x8y@x~X=C~rw|2%^Oj`q;D6)A zm+wg^g|<3zh5{~Nmb>&~bPLw3&gSRKXh`#`H1o&_orBU4Yv90efij+~w8xY9>fkY* zNqE7KABW<9K-C)c!=}T@#r7BeNII2Bk2A4#ABXZA6%!*TF#Tq7EzYZ0t*6MM! z%AY8T#%o;#x|LZTWC2ehSvdNW=$wT^agm;ab^2>8Pua=if5yb#F8zB(x`|a;-JPiXlkKGacdUV?apW>Yrbd6QhmWs5R1u= zYA-*6J{ILznAxAo+o5;so4yN@cH?Dg=S12Ot$(i#UF1}i%B7&37t58VA3>Q@1R>P; zss_0I7>G)lLD_34uw=S^>c=cJjjwS;J*B9l&W%mpbu=RsGl3PItD9V{_x-O87D4D= zL9hs_xA8LiIe~v-usmhP*ZJo!AzMzoy^TMmpHJa!sAO?Be(R5nK|K+uFQNVAz05@` zQp-l-M4v`22;^fSXi7iLvd#F04a^N=z_<)B%MXkJ^9GF@0}S;8`d=cY-{Y)CFXR&V z1%wvJ7vtlHIYeCF+D}6x(7=KJOq5TWA)k4rXr5!_(~8QLtiKB^1J!`=3?L-x&}ry! z0)__$pGO_i;e^hKJP1{E+c+IAw!@uIgv0q%IK{0#ai?Eiwu#*obDeoz3ahaKbE24- zh8epe<;@lJu;#1T($Swrb!7ASe}u1SrlSnEB7|x39@$om`pN6NHNM!_xWN|({re+) z*>?pF6a}@HjCMmk%@vV|i^R5t=HmFi*SQhC6IS00J&8MKi|^1PJMr@U__v}m(o}-K zH4(ny2v9DSx5Mf+k#nQXfv|XnE|?U2yF4at`F?qna_hHMfMD zA?)ijhLGFyPq3CHQF!zN!+482kw7yF8m}keJcN{#wY?&ePgFlsU=UU)YfaX_5d_Aj zZ2B0<#lOHCnI=Da^g4W$h`>aO{_DHX#i?F-h>dVzY} z8{0rAmN8>H7P@2jZfuI0__AU&OdK}ZY@0}>(b`{u@%G)$qGiV9#p za10hChV*%#rex^IZ#jKTpgi%8BuVd9JH466X2%hXqg>$U;n;5y`8Xm?jVvg{p@b87 zvCqDjC|}F#rt@yYr=82Ei{jLx!jLQ6@5JF~1A~}E!F>bWbi#f@@ZmWt@vsZ=N_>1^iQcA0&zwUa%~JmMXCUA_4T(F#n%o z{(HvrPhroTEi?OfL;(+JEY}700^^Z~K<~%7k~fuk>LwPsu>P&gLx=Z%ggOqiH1RST z8!!sqT(ml?4y)5je52I=pk-BbVrtz-8(yKYo)m10DtDpb>u6YX+rXuA+{cS=iObcD zcy%(a53EKTrwm@O4Rs)bs;^hl^c8v$#+2XLgQKH`wSHK!!@DkuwPo5uzqT+_TR6M< zNus5BVX*nTG#Fl3>#6hNTv#l>M833(*TW|E{aH@&p?;34(q2Bk)fP{m0@QdsO&IRS z$tyU$q!UM<5GIOvvdyQx+JLo_(#(IpP_HKg#~^OKlFm@So57t zb$CMEl+LD=#;+v~%Yotz9Ks{`V-AaKguvw+l8F9akdA!R=N_|u{ypimX&tH+1^f>i zom>~#8ki!O?x1MuBG?uBMmRP2Cj-WlZ=xboDws@kSIGDdV5|`Rk-cnu2}UmmPmjW0 zjf4yGq`sqz#t4w@p)UfrM17H;_fl;?`W?`6e53#~IqW)cmyjqFX$TuxpRMH1X@%Y( zs~{gnW;GQ-Z8qiWp1X|Ud<+iz^7hfXvr?m@fBoy;E$wvGLU;6S=h}Z=iiEk@ehQ29pPvydb3+_wrREgN2dc8dry0$(pdYjHcJ`!Wn#W1@4JC}%Vd_2>WQ;^ zjuT&)iKgW;y$A`~Enp69p6`A65~wtYj8Bp-Hi>MF%9h7XH#mYV$nrXy|EXBvEyU+t*E&-No{mp);OXD7 z@Do#HpYx@|7`QJ9LU_*1dST<8enOw&L3mK9rOe2ksat>Y;~)R{)EiP3;>@1o%BV6L zsrhX5ZD>z9W9S>3v*6YAm-j<7Xw@9|Z^16O@Yz)F*Zjy`jPtWVJWuoZ#44K3Jpgrj z!!`JB4BKD-8b}U*1&eZh;%<+)Iqxh+7rbE_T?HF7Ig0f==s`T{OTqqVEYx4*5ufEk z*v5=gn7m<1k2`PUq8$ND2jNnREjT?YRjV5GZu#bOY$KT%8EAMzX?+9VaMaOj(c@0{ zZtoV2zG5Y8^Xx(#a9NHK@Zp9xy{UUs_u}?;EO0%Z=El7Y|7w&9zwz1Td4*V8D63VY znomIkDGd)Obf_jIQa<#4)Qi*OU&r$2YG;SjcWDU;sztrt^t!ik3dkj);Ptw| zIb#ZJ0m+6<{b5Tk4fW@c7Kh#{K)2$k?%ta9$ zl?bfUk}OWCg#=h6&HhYNrhC?wk9xP@_%^3|AJc6JZK1PyUZ$h>bJU`dTVx?_XQX z!W&5apBWGk1eglD@0jUQWO2ot^DbaUQ!yQU>FjC{Hn2RAReG0;=cBA?}`KQ~^lNk=5RlHwYh@kQm zfN)Fp#m5!B;ZqR0oQlLOW8fTS-I5d6bCaZ}ufI=`oNMfr% zf4J7V76da2El_m8GCWS}epBG%)bEq=+!iH8D~mp<2U{K8G}d3;b_61{-=CRg3{ zX?4p>nqHRrXGPgulCAgu&KXA4>gec=!d*Y$#)TmxP<9kkY7Wz611`HcOp|x-(bFVs zc(HM{ztNnBQ%#$AX^Or3E1Be!=y{6ZDEoW(9^GE)Quf>%m9G8gWNLrp2SY5@>`9sh zB31VDq@+2*e$KuhEPm$u!Q_a<*;tH;`F9M)W|F)OOL}hfUAsN*n%L1qGJfpQHNNA& zUXqe#)qf4GfmAEBg);q}un^>@l73 z>mD04TFRpS6^HdfeM>`tkJH|lQfgf3od}b$M$w0U?E_*`F8;y`sSw^<8ud@y8K1_n zV7mn)e@vw}TBxs)QOB6v&}EM=2MYQ-T~&1Zcw@v8mlJcZ&dYbBkyT!nUe3;x+kJ+2 z!&Kq1*g)|1!mUB5%+C^jmgt`q{H&0lbF0l?--BLiSLyKgy@6lPXmLTK#S)Mm|Z5#+7xf zkn=C9w5;#8(L>%lqVH6cZC~-8CRAzw1;5eUtpni;%96KZNiNc#HS%2u{p`Opv1%w@ zekVbJv!sQ$RSW14!sT!Rz;f5z7IY)Hpheze^RoZW{ylO3ol^UFro-N7X^3@=e4JG; zcL=inH~In(AA6RWGq3ctXGh@SHm(X>@fLfzEfBZ*gl;K#vz~>iUFF~-9u#9yDBO%W zj3xMOIgeORg!EWxpHgVifdpCe`%C*0R}7WDw8yN#AVJH;BcUGeJ(9sFbV>NV@T;Dh z!STn#&7S(c@qO`g$zf!KMw5vxdSZMljNBRzjg2L_`+)m%Lv$3~ZcQR7P}Q$&c$TU2 zk-&2_1rOQ!EqGqE;Hk3V;Z*(fnMR~9yd(T->59RLyZSpzajgtY+{FYo9mU=Maw!Yu z6BAoYFPhjIpFS?u;)1Gq$>RP_cTh{gGu>H!4~iwnk{bYg#NNm`kGd$8bQ%I>5n9kV zUfNgMTDFHHT|@~dHurayeN{}jiF^BBA#X7WnkRP{v+h07fBn6^WLJRbm;!x9YrMXL zMGQxAaj^tt9+^aA@%pa*ud^XB9@!6Nqx;9Vdyey8v}=6(%*c`W?OqhUqCE+bXKm;$ zZJ#J3P}MQM&$E;Ns=DN#{o{{uM4KfcAY=6U(& zp!{=We9O%G{qeQXs3lA|*i+wWEWEe3bmzqPsSIgOdD{7JfscQxO!?=C(K3F3i|MfC zJukwwRpt`T2ST{O+-F~SX{8ZY;K zoY$(JIdrLZUH!qTY~81MK84(;Nh;ld?C$}jJ5I_^DhsE*U~WijT=UPOp-1Y39||l$e6@8fggziN{O;bYQ{}S0T^7}vSK}OC#WV~dQPlsp`dA^b zENFj0E$0hr*QBd#k~+#^?W~^kV#iE?PT;7>0C}Oc7S~bSlzL z$kmS1<}k#iofIF0GTO9hPMbxpHl6lq(`nxt7U4^2(;mFakgNTojP{FCd{*pg-|36? zoxW(_>5KLq`p~}9SCOl);*7qEZT^8TozCdM>5LAX&gj7Dj1HX6=)mc$IMtaRffBn1 zT6KD$Ri_79b$Xyxrw3YfdZ1Oi2i@uvyH#3pTBRkYRa$adr6s3TTC!W!Elsss;%lcR zzIIyTYo{f?wp-F)UtoXD7tYsw;e5>(_SgE0Y4#V?a=xIJ{e`YI-L6H6)Y5Nf#2d?P zxwa?xkL26Z^ER19#oB{Z3DeobT?wn$O!WT@`3^iho3Uj9%MXU%sPypzG&s}74{$(d z@B;+MNXZYNH!6$x!IVbQu)xEG{MDS{sC%)OZwM@tER6j54uo)UiW`K^EChgALk+^^}nw+wElsE4=j z@b_K)@i+QtU^oA%ntXE0@!wI(n-eecF!*EFCx4+G`nzd>>io`x;g;vAeBvkt`}n(` zo`LD64&B;K0K0z=4M=|nD66iZ`TgG%oK{eMC)FsH>VlWFFV(-la+=hwPWNHd83(zH zK6a!JL8`~tiQhcMYxLgHdp9;vY89ona^@07kU)BY_ zpA4+s8Mw8hg!lIameoHP*uRq^^}gMD{}Astr`}UJJ~ps+2VZQZq&dB$fxEwVQq)dS zhZHTXH?0Qc!`Z&TT8gSVov-2zLDkRN7EgUI>i}Ao999d8;tj=t+m-pe*o>Y!DUn9w z@U_;OdN}+$F#=uq&r`@Z8s7^xCN@k1eN+6ev7|gc$YVTlfxP^oIAztHuDlPj3@}&A z8&$}zwRPrqcnmB{$jxqnr5mQpALyrARHWLWNtKu@CCl!uRuvpVS_Dp9qf5VLl`g$k zx^fYxP-wF>A%I!rS(dm_e{+lTO(^{v!h;c&{ z64<}bp}jyIZ(v9mvBXB_yZfwP%Mx4FclXNn5=k$%AjrLq^7XQNn{|t_M`U)mtsa!n zND~W+wRAkBn?Q7ujH=)1D&A0{@+@(JltPM}vc#C{GM6CDo~GWd zZT3WZ8B*?3jW^7Q&n{O|v_uZWe4#%>jO^+6`Qha0#xuy1@TpnHmX$^X%90xn_&Ab; z+_14!QhcQ%sdoK(^epYw9buQ5t2PkQCJ=)V731i;@z#WeJOM z6uYPoMBUjR*b<|-LyfLt3pGeuxP-rzs4QCqCk-PmiY3%Xniqskrpg#EtA}@_2_+lL z7QsUM(uBtWC9pQ>Nhq;c+wM{O&^STsQJmtbddO3?Sy)L_pc=g8@45z0`5PpeSyAkL zU-V;g4Ki+ObrJZ`IkN0$bBFS;#oqU37K@i4Ra_7~&;7NzqE~z*`eS@RxCz#TjN^OW zPWU$`LXTA&+v8OOlA-RcZt-CZCcNy@=aT8|=F93uQDRm;htYiMXrvs>;Amv~HHEm) zan%ptaBHZpX&gF7JQw2AjD#B4l2`ti{i@0*9kS^+^a~D&On{y^_!|=5$7-sp^;fZN zv)Up>6byAss8v!racY2eYpb0`$Q=`EsNt(x>mzBQS|Uve|9zYbeg=l(OjGxRs(}H4 zExiFQl^#4(18Z~xJ>NJ(_q8sx2o*F+|GN9j_*z`Q3GeS4v(f4a;T;|Y5sY2o=HtHx zS)k05XYRMI4>qt%(fbP;BWn1b%u+(d0ihCv^!_5@zn|?r>i(%JUtLWjG!d^6tdSon zs!Mo(R&BrHnMTO0$lJ{d4qcHuFnFKvpU;7fgny%1Y1%h2wHibDjhuyKIKN{3mEZE; z&#dQ8{wjZp_`CfwmRz;#jeg0XGr*5>k#@t-Il_TPLy&H`dX_tn@q5d`aKGHyx}bQ8 zAKHL!-Dj;b$ySt+P+Uf@LkHTYk4h3$mR+nupTq&z6i!2!Z7N0(n;?v7zf1Aap zC+gtS3)Ks`l+Mkn6E&;;glPaitxNG~RZ#5%Ezb;cyV(ITFUF{aKYOI_2)+h7>k7e3G;u;xe+c-*ue%mcKR7f*l(Al zFWfl-=B>1P?GnW?zxhgGu=%T;_2obLYyWqd_4~X0wI@jJZ#}dBwVNFA_MUwr-{-2Z z_o~6X=vfl{W*ez5W)?*MOIfhACk!hY`Wg}Ui4q*xUb@@ZXYY?H&yUXl@T;QlN)FJb z8-}Wa?-k}pOA_C6?~FN>?3#3CeDx36{kE#7X9mn$$F1?2P>DKt zAQ&Fnuy-X_Xa3Z9c~8%nL&?`CT@}CXr(Waasvbb#^ZZ%=G^jF-{>*WV3VaGUQY|p{ z4ty`!@D=CBi(K$k17CP=cn5VquHt9jA0~?P#=H_Q{$bLbRB3dSy||)>w0CR=AYW^n z`5TT$v2@wcOm3q{i1TDXEOP?jDo8dvkmwN*`gi{d-m(USXI7lDx4^qmRz`pAb7p=@ z8goVKvg5PTzKRzc1br_e)H~Xa)Hv{cU^(Y}m;?P~<8Cj=h*s(S4mXE~N~;Fy;)FQZ56LW4AFQsOIAH#c8sQzp*JgW5TPN=9KSX@I zjbnuqca}y5C+^Ibz>iNFX3yk~=;*Sx+Y6(gpE+U0XN|GMA9flY6I=Q_4Yo?MDL6hh zyrZ;b;w~E4r#>Bu_7?NysTH^8{iM5p_R}>o^ zrF#;ge`rYjqf%TK#i##ZZtN{DCytIUrYb>V_ZAfBBX)>1xW?7s7wNj}#akyrv^|OG z8;$ehMVBK5vF%>6V;-e&@wKHV6Xjd88f3pmg;zFw#4CI58!^uW4uu4;e?%B%eoB_)1@s3kTp4e^>^BNq+9@+Wnu_ zg+xy6l89fze8IK|=NFjSB7~mfm{1&vCk;g*H@AG@lFujV=TLcgM@=;v z=l0K_TNF=nj$vp2rva#ayt>_D56u$6_8TvcC!R_m+9$DP|9Dsl^7(}Cr89S-45UTf(hb(6Qw}0JP*~Hd4>$nY2ua%RpO1L#M?h9YDVlmuvt} zAWxPsodG~C?l~$97=?ns*AZETK_-SS3`p-!*km{i21MKj9SlxbG7rs=)1V&WuWO~_ zAfbg0vY{XQv@{QcrBX>+W^*D@$tB-}id6eKDx9#e41F+`A;Gd}I4qS?NsZH>C(>sl zn#NA5qZF?S8z}YP3oR7Awfb==!MhYkNpvLlXbf1h$FReN@??b0K#4wL6;i?G*fWeb z8QF@5=UDJ&j0YHV+;h-?;dds&48bnSJ*YBHN0fMJYB`!dgBY4MdzJ9%I+|1vLkXWn zjO+>`8iMQB^|1G5O(r%mEWXLA!2H8Tt;%ADVD$&nw2#R5K})`ig<4-{K0N3M_(9iv z*el}E*A_V_D$_-}BNqD{RJzD#sX|RLr!$8jXd@;ijm|;3N{0q~HFIf-Wmgaxa}mT{ zK@hPxR5OS*K{{PHFfe3GmU z4&4*1u7!R$a)7NSM|K4V8Ag|guNJp#WCT^Am!){JE4Z+oqK1a*e+9NKR4oljz&S-x zF$UC_z^7WOvoXmSGyy2R1qW3 zUu1Xh;3vqTR~n}){EA)n=L}_kmb=Y9WqUg7Ar`i5$+Xr(Y6Z(~W3?V)<%Oai{}LP( zZkF_{=qWu1Snn{o#^d)wSO)|E_Nf+%X}tz#iv5JNwogKmL`XL{ZzgJR&}#XjRoBJ8B%c*S#QiD%bh zyrn6-)<#g-*IaU#!Y^aJCp$T{iE3I}SK%m2zCmSq%Y&wg~UUnPyO2(PlOl9%b z(~z`lX~B$U?MYfM@zM(QTp`12FYa>pX&Y0jJ26(JL(5pDTV*_?uyI(|wX(}mswNLT z?+*hMc}kUydIq3a(rKWW_XREul|>G%T+$i`Hf7s6HmXzVK&>Y=J)vZmV!GMZsZ*$x zUeKC(ZM!r#)V9TDLCrLi0$kgrgG}2>-(<9pt)^$o407w<1!@tZGYy7ghdC`fNcRa^ zTFFqTMF<(}#PC$=O`D-2eMcqGh?8XIMCfriMUY009rWjCZ`(UiedtL%)|YF(S7 zHbW$aLuPIEP-V%v?08Woq+QvTb5nE(&e^Z94+c5sUnS>kk6G-s1-^(Mi_rjF>dFsf z|0^LW;?3fq)&-%IF0d|jHD6t#PUuf)8zD~XyvqxNZ6n0NdJn&P!sh&aYGbrjJZ+9$ z{2;%2Li(e7X4`MO_+`d*i<8<$Ncd*m*Q8J)jhnwIzhrCiElUf7t!B+$)wETwy4tQ+ zU1QgKa3S?{Gr_rbamAH(aY+@$wj#6sy4UMEenbGswP2?lcNXCyZ}c1#uU^2 z!=n;TI*Yrv){td~Af>z~h|?f(BSJC7Te*Df_jp!mLAjPU6UGQVURTQ*4D)B|y$-Yb zB{)=T-Y&=H#XU+~u{af;fFB7`Hr7hSBV>o)_>ha=Nj$8gYIm4D&y!&CS~;qs{s;W{ zLW*LieUb&u9n2NNf-7FZ*lBO{Tz8@QWs*GDWd#{!n_jfbG&cAUC2_JJOzWx}Ys@EJ zm$*x?p)TS7y@X1u8_nAV#d^Z6#lLq#D$*adi2h34QEJstE4FQZgHr{JI3H(Y;^4EW z`&-00Q{z_yf&xKtS%35r1yfN&SrVwZpodYwp?R^2s*)v61^?>(B%F!(2(JYIl{32f zJ*R)db4r(d&#nT)z97%B~{D(o{^&tTwVk z{PHbn$``-x8Sk}m?=!l|#+n9mVw)nD6vT_4u_8o?s|(6@1@8J7u|fP&WQM~wzroW=Yjx(6hlF?O^vj-P!mpa8H4i60LZrRxkf<_w zU)xHzS4-Sef|{a77;5~{XC?eks*_^or*=BgG;5$!;~ndrN`GQq6%$4zVvz9P)|l{a z3J)cUkEtU%T-nNcM($T4QC3+$N6V^;67zpIn7A@;=BmNy*qJk5lkz9jn>*atomZ@|VkCIsRAlEkC zPMD!ng&8g`+kNX4V~^3Am~PnPcWHcv9T}(}TtQAkeD_BP@g)=GF*`ayER4ut)7Tg^ zh($siXCONP7xG}xl0qiGEh4+w^5{jnI;gt3fWxhOR!k(tp+qx&YFrX8zsn9|=sswC zBsmaNoYoVI^pyzwUqoiZ?>&?s9c#h2$DI98e$&bPXdfMsQ2BdYj{OGN>NRVzQEjuMt`7kYVrOvJTw9}z}l7g+@2 z{vU(izOJi`fmMgqC$-_dGs)LJk2Bh-&un^GUmw0)e?AbO|KmaVp6Qw-?<)?`BC*AQ zj8**r=l;06PubXmvDLAGJR@pGC&pIaHINq#@j}7A`Y!$y!as10T|;R2WS5Pr;52lm)j>BB&w|TsbEFd57gA!8z*Ah6iuI$%@@Kt zC)&J{@i|X})U8f%!2J9>MZR6f!PN{1JhGpaJKF04>;F}RQk1-}ZPrFF z4|#3gjq(SYPDiJdc97h-a)_hq6|BD)UdD{ut@qV(ljS2?kH=br#KWRuUeS`U?p-Ts zfR7W`6elYG!pR~1`JgB!=r0$O&g*TcG4E_w6M?9?p(_R@(Oe!U+w#Z9B7;HI>Ogd_ zZnfvL1iRpkML{MP?tg4GXQM9o75CoXHtT)`xRGSGF?NxArbcbNv0~i!@KLP%7vwXb zfkwXk`8MfQ;^ChZrKxyL>VDSy3v}*K;Z_YtbN*~^PiyvaPsX!GqJ-qXV21!J&JDXr zmO)q~Z*sC{ktB@&u53CY56g(#KgXHPy@>Dx%GssSbZMxr z!*JOn3D%e%Wyg~kE?6^W;#IDEYD|P4skQfjbuzihCX~k-ovCZS!sfjGS0}zY`L%@q z7sfaVHidpr5lebG6D(5a7%_^T%3u+bqMC@QF~NMPm7W13vBcL{7a!R+M-8!AvYk|~ zY7*OLuBXZq>i023sZc)P+6vMizyF*JaFH$9a5vGcH9oOBxg9rw%q133A=u^-RRW`r zPJBRo9?wNf2t@|B5uuoKF61tTXIQbBkQm5*ft6vv$fbiS0A{ zNZ*Jml?_ACkNob`ifya5K+yzO@+XruXLd)CFvcbPmyyYM8}n?FA6vKl$9l8SpwUpl+KM^05k+adgbtyY*iwFTwyGLOaqP8 zXjANV60!*=x_i&!S6BaZ-B8T}417iI=Ss#Zg3QcDA5%8o8qsIAy&+2HxKzta_$NY# zk%{s<6&wqM1a&o7Q-0!G%qHe}GJimpH3M}57);V=J}2~Sz|5Lv>`J~qqOP_qdE0rV zNmPfq6BZXE*rAv=ue6>0eP+$UtO9F)0fcKXro}I0lie+n*)dL<1KD3-?U#x8{sy+Y zOe`tU?MU!@u>tRj@t$T+^G)0uv-hi#)q#t3eFRgV5MJ+pBXCc$|K&05HRjH1RLTc? zQh;7ai^}AY@cu!RRkh?!y!X&afc7`CbXhUh{FZ7^4ms1LdF7u3ZtE#?e@f;v?V|yT zjFDsJc%$Qmo5u;2uavwEbG_F3Zk!mKeoig(nw;2DDM1Rc233y}#-xWimvqQ5faK%E zt%rWRj06jf4Q8*rsA5-P$>R7|SQ_LTNoQ?l@uSiP>|-fA9UaNMFGsc4ttrZD`Wv!D zlEHqw`H*N>j6-V37L*VRkf>jXi78>09cK5gbl|7A-BOe5hG;_R|1~7ld(o6l&@{y_ zm7hCpMMcV0xgRmSL)|Ie`aI90)N{E`LIQo1EkZQ=D2qZzk7?3{&SC=ERXLFZXMY+tt1K13n0rOIMB;wX*?2f&j?>Pzyt}#}4NpxDL zR#mS(t%j1jZdaB-CVX{db$6^4lHvM=BXsy(#)ut2-kP*>QF^-kwr?4idnFSo0$uB6 zp>3HxMjgq0d=xO>TP!A`-r;F4J>7J6=^mrsC>|X@4_z+CJvG@5Q|GPmbQt~Se`@$h zf^&s}L`|S_G0;`2%{njH%j|BznrtG{%|OEYw7_k5)8E8>x8%`P``gM#$7j-)miPLB zTKe*Im=RY;f|4^+Lti-|ebPKG5)O2`$glS%zst6>;D-3?to$8L<{pb4<=@99;`9D<_`@mK(dm59b=IYFXofE=uS%u>CQqdHO5!DCmg`K;QL+QqFFDA0JJ=muBnFhx++{tS!_;oqXf zY!(dzC>%CG|A3n>5(|I8o@3$=eJo)X-ZzNTL9!h*$7kI?NcNdX39HT)=7grH3IE>< zWK|_%yZblE%6v3Wm9Z9JEWtHU*1m#^T8oT@_YZO(%DkbW$4lT7R;H06Nt9YuV&*@F zoRbqE7@zf+H|~8bK9ZCt6If;>{67-ACv<0goNclgbX_&Tj(T&~78x{uZ1rQTR@R?lo-Mp@;PS_%F-g%`Z+hknX?hb2PUAJX^Y^@$?LEm- z*2Q-v=KsiBiuZ}MCRN4SdcbQ_MNg(N)2L)!f6!QU!nksP)qR{45mmj$sWB(nmRNPL zr`%Z6C28!}v>R(!ns*qHgIquRJTIOnV~077jLdA7B0nQQsmpAcpCJIo z>Qhs~pqw@sYbLN5V&5qibsT$}R4D_24Y$DE3^-~BGx1ei1`xdbM@B$0#;}RFtaZh4 z1~U8}rr8DaR(@8mU`b3;jEjZ&pAq-|%oy1B4mj=W8gqJHeD%+~Mjxr8T)U1^GOYB0 zZ2oO9D^JjIJ6@3``LunJ_SbL)Len&3dw6gE(ZIv4#xc);80>f*nkE}b-o8gl8e^xY zlO2@%)$&pV1>aPw(iKdNrFzL`8#eSOdRNlp?Ka(RVbrkSHI@vSdHq8} zS^8BHXBHa^N+ezA6!TA_X}NrTHaTh&3qs}#Jg4boF}!DAU@Vv>^{1P^RrT2-Q6lxH zm=DM^7ED7BFD&0DGQU^&AdxHvKm{TzG^!o^&56$=jhY04is*9_xJlwWMlHiiiQUZh zJ21(3)%#S*%t{jM&b-{;bbhU(zl^FpjwH8}{H`@8F)IC?uF=-{?zxX}LABts(EO7u zOd85D#RZ>P<_q$?-~yvzn&30td|IAe^!lx;pQ6_TJUg(-m;;}XY%Dt3#~lY>WGYJC ze{r=?OWl9r+3VTM#fU6(H)7|Rg?|)t)VT!1W*)0OX^S>h{@Oe%pQ-7gEtxek^4CiL z?)px?+kHSfOns|up@`4ih1AbrYgShdCudpxgtUS-h;IL}IE=zVX>yxsrO zC(22zTJQJF_JZl+Yc;0A{{lbyyzG6Q4YmQZ{inS!pBkR&?%{dHH9XIh$dKsad8R^s z%kcd6@}VK6zxD9^*%--UiM0s4D%mMdtJ_OelGKZ_RGEs-O82E-!6sYIY_%=1O;zBF z^#;Y{J!<9wm4SidW5v}GYnGIf$(|9hHzs+K*h=w5eeCZ@BmX5Xruu@5sXpmqstG{d zUrEFDzAV1FC`}tw0({O980cV0M%SVcWVNssjv1pZ2X%X@|{hZ*8n)Qu3`4p87!oJWO<7;PydG4ViB(_n00(zu`e$ z2Q4+0j80@1Gm9!{&ytY^k`;34evxsy?b}?&fWbYF`&}{D#-);p$hN}GdD+-N?vN%4T@ZV9|*K-}K zGjgj|(-{5=s~e7d_zaKT&FqYB#z;5nc!AZ89ca<*Pc*&^M9`v?V^}`(orlp_ua>no zMU0V&>3Nv9Ul4Z77k0avTb_T^$A|@t1EqVk{uVdoM$VSac~OhZ#S}Jr_jI>;H;GCi zaWR6C(tRS~&^Jv#JNi6K+X=r~*KymDb$QF%yH&$3i$%U?B8Ag+_HV7UClEGnFb`BH zb)B|8r@q({9FZVJXSp&b8o0{>W2<(Msmpeg*y>r?Lr(pdGYTy zV_IVo6qY9Osopxlx@$)+o>6y)X)mm3!ODCl_I2gs3Yr2LrCiUoe-)(eL?ufOYJFQ1 z5x&5N&B9FNJ|9~uw{hJ%+G>6tmjKb^+tScuww`X3DV*D$4k#PwSH%tewl^!Kq zx3aywd5J3|!K}F$c_WR4CE>%$neJ1qk{9INgcGsB5|md)BPEVCsDH0AYS zXT(eXCS&eMw>ymg%E70zpBIhAb&Mrf0pxV{w~;F{jyGWl<+bT!MticmKo%|S4tZXM zJclozJ8IHUl`r}VD6($Pk4B6MB3VXj*(G_hcyvZjJkwNr3mMs z1rMMs8ocJue`6mC))w_YuSOSDo6o_###t-*U1L7ZZ)}R(KV_2 z7xZjlrS3>wFe3ML;3z+yeA_cSc3R)dG>$~0XxbhU&(4~qM3I>aUw@>x+MKnLYg&u} ze2;RuM)Zg|ewPw$M!ccXzD!}#op{(eBpTQfh#ekD2>g{92HZ*qY6eoX*vz&5 zrt|6=8fJcvzvfHW)%LWZtZv?h8j#urGS6G{i4^dK`5(G+e65~l(yVc%56=i+@o@0% z+c6A$(P`$zIp8~2U^zb<76JPo=)?`SqYd@?P;VAsGUJl(d2Ffqo}cX@3ucavUWFqN zYF5|Qq~=8Q-#KbJ3|1S3=AwU>mL+@!ry6yfpzx-xR@KZaN|@Vb%xW}_T!B)AX~q{A zQM=89ymKn)`9tDxO68`(GZT2er*~~l-cGC1b(C(D1gcKk;4-J!MGUKd zb>I0>?wv?1MpN#J7n-v!Houie7`Ix_m&>-_J|Z6N?fE?CHb-!KP0tiD%0_W;t%50|N6fOX z%^(Jrvi0BK7y-Uy^Q7$JQO)_4af)c$&Gp&M^*PP;In9mBY7Ujq=ULQC<_lSfJRmua zY2dZny~gRDPn8|I?Gv&0oyEsFghOl07cS0Fw#=j{eJl6kO@y>rggmfaE06b41&J!t z@?+znGN0nA6<(5Vl-u{r*CeGtr{yjt@KMM474v>RMxTgR0*5xcneb^<^Rq@+xvs`m zMsET(awaSIz`ZS( z>?D80u4@-{Pr%sGhLJ?}G$-9uz@>tP5~4KwISZf8PUnD#MTPjKHQ}K~F_6p`p97Ny zFRCT}UE+e6mL>5Sig6u<4t%2fs(g*+I5sNj88PHuggoI|gs9Bx)*|EyHPEsMnfm2) zz9Fsx=hwpd6U_Nppxy+jvIgu^0x5*4HG8)AT8#6v*fvmJpib3a8GTp4 zcj-u>Z!Qw;mxz9v#2o{YY!dxuo9dW8%i30-YZ0q}p5rUMmWV66q`wm;QbucLVV3;o zglAS3Jl{yclQL*OF5s|OAcn%7MGY!dh8Kqc@GXhMjY=`txy+?!@TXXoYyYjG`_A;? zhwkqWXOi$?kjNUkn_3ly4#V{@eV&=01<#~h@VwHZWgqcXjuuW_r*0}!{wenb)7r~xdlwZwfzSN#@TH{k z>$0mfjpe=%#Mvx2(*_U`*)1ORwSM!D;%&$Qm+c02m`u}e;^WT!S2bBv!9R<|#x#4$ zO+j<&Ol4=WGu={K%Kh7J{wt9cW!brumc&-V&AHw>z1#GecgFt3Y^487>Hnb_+5Jzt zsel_Z+f8mv98xA`HVo22##;*pjep$Le=hbR0l*ybd9~@=>cq1u*TmbZyA2B$ucPyV zY9iX^v*PebABD_-6aB08oQtO(KgCzO&2O+n6(-l~H-}5&Jl{A+`H?j^l*V3>!BwsXV zF*`8^XPLX8vMkqPtV?7&n4|eMh?YF2=!rd_i=q}1(WMR&Z~Q`f2&}&!%H>969TQ4U zL@s{Tjfmq9ASCUcLWT4itzXfVvJ#uVn1~$Vyg4|*K({cFf|@<5|16_N1ghS|H_c7X zpEx8KXEo^zjrsxpD!>2B4LFd@u`lxpVJaURuDT{A-yR{yBCnA2^hi*g?CUs&s(fX~ z#KSbGJWLlimhHUtlg80R`9{4Sn-njaEtw^VJRKvm#3J^mvMaIAIF+7XqU^wJY;Nt9 z@Nww@JWZj0R7OGUd#v(`@CdH&w8n1?yowQNwqk=zm?(q4^!S~0kKfeX8kM9Ri+atc zV6Mm2JQnFK!;RNR+I89V&}+C>tuUmOg=_CcOZ?)d#|;C zI9>fgE=we=Ky>%;U(8$ogkQZizX`d2-l*@8A>3~+=HF~DK!sKV3`TVc1*l9-b-J1+FYpNU-dr;e>dTw;o*;# zm@^#s#|hM*O+nqm{!T)#)ezKor%}B<{lp=PNuK5Xx6>)x`(T?@mYPg+% zf>19@6SEG~^fFWZsryZHbh#*q&|LaQx?np|pE+HA5oQjFEPaL%lkLP8G32tt_*<+Y zEaS!eM@FB;R5hyu`H^v9Xe$YCcgNnym)$g{JYuD=l!^_>dS?35YCSVEHbh`Wd+t9U z8zQixJ@=o84Y9V-p8In|xXh#E=SuV4;}7zn5VZS#9{$wJUd4V0sa;uevtt}h6gE#i zE*5KFX&c*?v9YY3G1~h3D}y`BfFR8Yj}tGox{_NUpJp1#viMdC1Z?1l^wNJ~_2odE zvC3*dkLp!cm04rT{k4gZa@{I4B1gL0axItw--!af1slV~jd1qnjf zv%0xf0+hO-)m>mqv$Btr24YPmTOW!1R<}h4P=a}C8zSkA8jY3QEhroI&oNp`PZ`A{ zRYES7He#Nz?Qw+&NYcyYE$ zIF(r7kKg7q-zRhdAT|`5o_D3_nN#12Vj?JRwbZDaYAd`Ohl@E{f;5+jPhkPs0M#Z7 za|^^xEVw{jQM*ZXm?mB>nFI-zNJ|~*KA+-hLWAg0Uf-#zBJAl9K z(`tDFeQiK#4}S2A{}705`W$#$4T8`i9lYne{|?c!nVrAgucklKo~(CM;}z>I$!VV) zmGpUm^^)XFOs&8MNgrvRg&Bz3i1Tt~ZER2#OT};GNr2hYj2P}`zd!xqL$5#LXv@!H zq8X9>Mit?q5b>|7gXRPj20NgJxV(@_=B3gDS7BZqG2eM%XlSGN{MlvwE8aq|jVe>B z_nL3+qlI+KS!ouw0@D2eOP6BWQTA@4aY$^Yf0dRzS6m%^M`~oM=G5}qiXWJ6dPuZ6 z+hiaq8x3(nLr^m}C{xo9(Dsl#asU`XW}s1K@V^!R7fS_-2VC08|&0MJZ` z&vuCmEzPOe%wR?G1!>Y&ha-P+4 zVI&RzA1s9|tVCyMe5yYfAEMcujRdf-O22D3@VU!81h}!bNyhHvJ0r|vyNLY_1b(nh zI8<3Y#cqq|N#!Qg7SEF<)alU{&yyAMTU$IspBNf?QkDmLx58m)r|pe=`wHnQL9!ky zhVWm;&u5=6Ogn)+3pw&v;;b{qCW@6^;n`REVv}{U5TkIs*6#+5 z07uK%ytqITkM?yURdN6z82A3pz*`+Kyqk5o6xUE3A4v?j89k|^AM8*(&sxA_V;sN2 zAG?fGwkVE;oQDvrmQ=s`Wv21q3 z+guZXqhW3}JscL(m@^OTT#0fmHf=phH5DG3nqV9_C0lhwW(w0RdY-EF9Vi76!cNmC z&sEJv{WkHkw3`E{!8F9gl(#g5#;3=P&-C2#2pASJMq$|;e`1M0`URGAY%^xtQ;=Ky zI6C#rzbVzlCgZGOBpNO*TC-=HQi2n2%1=a^Wj?<|mT!8BRMOpZ5fQfz4t&b~YvxNT z)^|N4h}SAFlqe)w$rSJ%YnUe`cu<|V`T|i2YsH-Ed#}}t`zb}B*x~oh6C8-Q+)@sq z#4UADy;p9j6v5hfnMN=xih#FX)2rwihyRAzAA!xf#>Z|w0$uo^7W>OuqDy9ui(V_6 zWg5N4V)h?P#&Ihat%K$z9EYnmMw(MM3E~noKL-&b9sBI`E#90o%OmmMUzP4j%NX)B$+b1^FO64rM=}>OuwPCFBpZlTl$1rDa z8~7)qR}rC~EfIQlw1I!EoyHxp!{W?9)gsI=sMcy!uNM0Si;AU{Ali zvKC|#e`iY<5sbuwa_|sVZz~*>wcKe5iOu#Wyz8WJVKth;6Q4cbJkZAb#q$Yh8@p-< z1y?6#FGN3sWWxUdpXcd_V|9G-eDgnkCGk7ivdymVT8@0ed9>p-e}^Q>fyVUdTFs8B zu+-*!$CR-3uUe0#wvhi!U~z3B-z3kjE#%*oXV(_;n^gVO7V@w1Y_Hcs=jC3n;WXAu zHfQ}lMa*&Kx5*drDhf~kd zi|dHPTRhwJ{^hwj=xw*3#I2XL9(MI#iH(!1Ji{Mz36q*0DS^LxA}sA-XUo?>ik=dm z8g8}7;HcH&XIQRlxpzDq>t$h4WlGivH7oX-1%h`bb+ZIqkLT29-TFLd|75h6(9}V| zN8dR8`c=P+l)B&~N>J%f+y56! zlE})@_3UyH9m=+6V(^`O=_WoCVq+o=pesU*$Oq zG(A7z5A`Ko{GKBo!707Oa3(%Ovx+Z{i7&S=bquQVCI1b>ZRzLRrt4 z=G2^>>mk$hsBrgRlFQ$LOWB`TYxNIZ)?*P$@ac%XFf@OU#UO4Nd%QlWR#``6hOu0Y z4?ZKH?KkQ>1+IhUJ@V`(iq1*q(th@QcVh~^;q6IGzQsP*{;1cVrunq}M?-X&6}@}n zfivunTJ%;Hh?eKqf>P6OYYM(t@4~uV46oK|*09Z7>~9)nUdXTzTRX>`A8Lin_)=x3_aZi6bg-)JGJnq^Oiqa>A`doHnM(!2U2T8a_?;oDMG>AZS*-q6EiXEdQfZ{q zd_XmlT8by++06p>xgwhhG<{7seepl%FnE-;v6R_Yq5v+x_!#^G0?BbPZ|e7N070!T z0zYXr$4d99*}^>iveus2=RJ;41mQ9XCWSc}VJH)Pzv};ew^JX)ScUS@t+9KPO zMg5<=v79CmoF7K@^@*x&So7jDAQGi{vRf*pyaVRTPYC3WSHRpM&uL@m0$bDT)2%Lg zH4W!4;WieW60$oiu>KlWbq(gF*l6sjieJT(th--(L_{XAp#=bWdgZhHUgYg`Fe*0{m^jsZo;nPxmKjluJ=NggfftTIgk=ZvIp?+*E8nfCwTD z>&zpp_(hDKl+|YUI{v}Wg4EruXo>{Z-_!V{mM{CzZCv^D0cWA^DkSa zzE9e@$Eg2_2;k_$bKLKS zhRAxe;kP_ABoa5mg67khUa~0%I@IV_kVDnp*H1|w&LQ#TRO>Pbk#9aB1tj9A57x-d ziSUk7SYZBEOd`l`wa*70^V_e<2M(K>h)WqaewI_x&dn9k0 zOW)4Qo*yKgiApP1wOOK5ioq@twjf0-{H4^7dRe7-rAU-jhnNxh<}QfOLLn&l*C=Xz zmO9!7<Z>u$0VT-nfL**<8tU7`2%V6o2Fwd+YLjc1sOG<^Zu;c( zpCoWF3V(?+X$C71F>7s6oGU9ULg;;tWxJh;CktS4-&p>GTipH4%E|BSawXmj?d!rr z)g*^CzriA&xw%2pUILx#)Uuvbnhxa@{jZeH*AzE0pODMBRDBj|l+U_k$UNp;`#Wa} z;PnV|oP1YbLP9=lH5ourIt}I%q6F2(Cagt0esBI>Kv4=^jo(Z1>>eM@jpA7hH@8&= z^Cw5X)yB;gzkcDJw#uT@2cNjFt+M#Gg%<_eDogra-`g4|MSrC)Ug>WY`aq7GDsCEK zylf5)o`x@cZJt^0o+S~rIKeYAB-N3u&L5x05tv|{BXM&>@p&Pmx+q>(WXvs&&nq^n zOA@zCOMGuyysqT&AbflLU!4pxieD};YKpkgBivGFd}SKRL_^{27Xo zo>k@XNS9|#U$V{bS<^{728Y(o@b++51@e6dB>A2TuET8NeET z$x~D9sps7oE}WM>fGB**vudGdO>eU8EYF%Q8X^O4xJAIZMgVCZ>)EcbJ)&W`&T}By z67bX?bfNk+=@1~c7kZ*cG$<_^qwDEefM^~`9(vV`sfl)@0MIhZ^OAys3`iP~n+1e6 zpJ$CJ&4A=&Ma%he`%lD7tS`KO)tz5etsNA^jr~s={`kC)%qZ4WF8d{cwWO$aloGNEo z^#nr2Z}f)cPrY#8WcjK~$pT$C@Qr~LAdDVT)gVnBoE)Kh(^Gk)(;$sNlwt6wyv_me zED*`S0%&YRs-5;23UIcD0s{3P7~_maxgL#w?s<7BB_iX{L}(2qOpA4GIL zFs5&7Odyf22P|;FjlxTIMMj}eWJwx?mPd3&h|}v?UIDe7ids-Cc7jH_B6+{MJj)kq z0nlB8%UBB+kdH!78yygVxttckMQMR-Ts+IG^{9L>GSf4VfhBSyGa~~TV9bro{0wBc zHY3--2r|drxIl0BxG*F#hQh&G=p0<2ckY3?J_8p^+hz~Ubs4ybyr>a8k})u|GH`)u zhr>mLRD;O4&~pkGkrv^wu;gt9nd#I#_|(K7DY+7f(UK3BNloFgFsvjDgrr(EVKabC zjf^FMy{IH`@hnGVteWsy+R#W%;WCsH$S4-qox^7$|I)b#JdS|g$=mr097>hwS|CGn z!y|(XrjVh%VUZE~=qB~-H9RgAR?TEHtfR!AnMashMn|CoBA|ojcWR0vrZguqmF4J( z4~NZs2S^kEgE%ZUN(>!z(lkKk(4h5FLn9caas>#HGz2kr;QtzHuq>c=^Old7nE$BNe z0+o=%!c*oIh(%YBN>7ju25n9)nNl*Q#}ooGnK^jEnaCJIi?|?8E(8#EtJ)cdKv-0h zgBE};ZEBQG)euz5jHf#l86OSSYB1lRM^W3U#(_ps_n$z0 zt7^ScZ7|FonM%rZHOR=;%xokCgGwQzR1v3V~B@GLHJ5n zgJFh8L6})rgZ+j@K?KODrTHb-APCD#HOQ|fx7>puqOEH|cXYrBr9J6e%>>YJP|XB$ z8D%(R%ATt06o-I=o(xVfMApm(m7wo+G$_?nw+w%uYLGHGZL)B}Kv*3~#ZI>xq)1Jb zdNP2B15y)az&zp19P^M&8@f$+NXA382@HKw6MkTxh6IDk;L#Pq28ECl8&J_xo305P zFm?mdB+`(94O51~MvH~uBok#0Y{azH?SYJAbmT$?#rENlQFEY!4QLOKjaUf|HsGBb z8yRa|3yz?@;fF>{1P2?L85DImgX|#0xG*}}gwiz`8pL?mp;2a;uF2TI7{g;D)2gn? z*oeR=qQmKf+NeiCGJ>oE2dH@b(p$cxcnuRJg^;c&+$P#F(&toUbih2tYPzB@8-^oD zITbU}=|2FQiNy>{4r=DrU4soUrm)d1isXo5Rbrznx)@DX9cjq0=1D^an&(CaK}yd22GNtmzh2%%e03g6{}I#%Q3Ym zcLVbtldVh|x?YaC1${PIv>LOm%m-3Wsau&}oJrnNqRLc&8&ZhKfVihe7Zx{zpwlr- zI6NZaHqr3WT;>$%!b4Q8u19m3b7Tq+QDVB92t3H-M8je&UD2@&bB>1?1X9mdBEyZx zLZ@SySlp%xNC3xVu|pUSCMyd-#&>u;WER)pp*4ra1HF(M4*0+t4)|W%T#N|b)fNi4 z9YjrJgUVGnpk0MGlS?$&LPk+bJt^O3zlRBTyWbP#1ILY7A-Z4jkPE!ui z>EtN@J~f+)aN-NqepTiI<%g7XrOLIc{i-N)f6X)Y8 z1BYt(fTw1zxIf9vKcIZHq9($p!lyl}W+*o=UR9K)nwOPV^?LA>HndlDKtpn)cvTgO z;^0+K99J0PI#w8V;@oW&=WaPEC}c=p6BQzGfLpjT{EFu*3q6Mnx@sNcIizvy2=DXM zT<3WSzb9@=VDMCiJZS!d>SvLM1KOzc&CUcJa-UAXQ&8jwV}2Kn+hP-^>Zw&HJ3@i|pn{I?Wf` z&5v@LFR_~+<1|0TZvHH%`KfmEW1Z$Nu$vD!%}=wNFLaupZZ|*DX?}*?{3xgSa=Up8 z`lFK}PjUEY;x}JVUT0`zA#r>%R0@WV+iqe=Wp&m!8Di00;_k8P8z|<}eZD&xnt8+T zCVqatDuHkoX2&K&I&fnSP$jA1Q4l~u7X+g;o(h8UGzdm&bQJ^)mkWY`3xa8B5RBBw z3k0x+3jn{SumWI88UPld6#$_$0IcCu0Qk}Xutrsp0K&US-~wRI1!)kR<$|Cj4FbO+ z0dq2g;8zeR-yVaYgl!Kc48l|efhu7T&Qc_xgh9{{s1gRDP?3NV1|g^*P$dk)NCg2U zuz{w5Dse=G5+Klnph|#1(|{5nfT3M1OXQWzBC9lpKAy-Bia!7QZzU> zjRa>YF3>+RRRRIs3Segrqa)EzMx7N z1Y6?a-E%?oClgcw0E~43P?83KB?w0afN5y}*jhjU2&MsGt1kf#$>Tae-jQun>sErASa{DLsKe20KLq_aOMvAkYfjcvrCqO8h9K z2MpzFY8}P3ADxsOMI9W6{TT@DgbEl(JFE#@ z3Iu_*yW`#|@$05|LQdgRH~%e#Vih?RLOIJ6abhHZU9_EH9Z}I zktqn63f&NlObr!USTl=;z(oSIux1twforHxPHqUsIwWB3u!c%~lN_6Zz+o0u(&|q^ zfD&{=P$+|791r)2-Y8O1tK1SOF1Ox*KFEtIL33&49VT2Y7MG|_Rf6htYc7NdfmJ1_ zPHP4gy)ja$4OQUiZViA|T&e^B^yCp%iVJCW0bpBX0>IQX0NlEJ#&7`4DNlnyPoNqC zWC@jFL!hT{4Z*ZD2rMf`kpLEPkwDL1VWm3t>|v8YPoP?LV$PzpY#LZbgc<~x)CIv9 zr8<>%vZipg^v{^Wg*tbSRV569wjPztS(cnC$(X|ZP{(pOFbdkx<{QVBRNpwGpdpw6 zY(JS`L!fO*N-!ey9BxD4p2N|$uz?N1ST%$4jWdU<5{C~o1lo{NB|vbNiw1fQS0&CI zt|8E-oGO6^V_jB+a7YRQZOtkOv@K}^pa($%Aam<)zF-63HX?*YQzWqFFJ((=J4!9H zQ<9);Nt7)Bs1gRjT8AieT3d3e1QNIndSQ^?d_k2!0?h`nj$=!zl9UypAP_0Nd#oyP zNC5Lj}LI^eG0ERUi|c zUSy6yA?iiuRC}3b3%*)D=|!fkbHrNAT4aXo=2MHz61#cZG*pW;y@s_7A~A=oHLMRE zuh+0JvR=dL#Q-2JfqEKx+ximQD--%J+Q8cur&@Ms;BAfVxq6D-x~(ZaOD?cmAERg! zYpY;Z4=ioVUpU1_qT&42Ozca&#_B14&stypT5o)|kB6X#BuZ<2aWb%#Pw}h`=92?^ zL68Ek=hFPO^1~OuG_ZDWU~St~feoQPuFhWjZeZ=(fwhMNYY(xHH*mW;3yOb$9ePF$ zTc&UE#h3fy)jasQ>QLWvC@Jf1vF>M;?RqsqqdF9?QQP&{gI8S~uaWI~jq#e2#HwlR z+H>~nSqZ0Tzh>Rve4Tu^a%WR?f_1o-^s?r-1E>X-bTg9Z*aUt_grXN|dW>B%7okuO<0{H2GuoVy}=9+%KuUP6woj+-iI z``$mECMTZ0PmsI2qn|R~Prg$~t{pN5_!_16!|cpXzR8^kyUiP^TzZJ+xVz#!Nlgtj ziP_U?J?%ASF@4cX{$ek;IcZ?6ow@pt+K<>|onKKdJGpCtt)_RIM-Q;iI&!xhNBHS! z*>E|Co9KnbX5~T-q=U9P63h4D=9v*+;3xW$DG(_| z7(S5P8RE*^3&RKQc!TpCL*Zlb3k_B%$=zqixq?z^hvk0<;^TDfLc8`*-cX)Xd;2K6 z_J8od@PR!o@o^VUvohxz!96?UTx@-zw5H2xWSsOpie{%t-&SkWq!QZ-0Rlzp^sQ^R zoNd>(Ahl|T?Ai*i1GJqV-kxmt#`ARwIN=vz$RLbDqVZ{su_S3X)CmHee^x4oD+gjp zE`d$HQxtpiGNb(ITc(W-8lB0WVC?W^;qnJ=xkx|ytjFnNgRw3OHr(=M3MzYX>uq;` zePR07;q%XS^elB(mYKAJC3ZJyasUYr#$?EUtPJcgl_s471 zpFeyZ{k3QGN2%2xAEo-^HT}g}f?-}ue;;A!rYS@F(HMTjr60-RN1Dc^@ITU})@HYul<wvPK`+^=~@FF956QXvdU9nlz1+(Q1EaC2bG#!hmz zUTN$}p7I2?gf5D`K5~61QH{r&dtEoqAF-YqFQ2i-25P)=#u|4~wGH!ve` z_ss&;-uS&u{H;q|iC=hA5Lm*^oY-KWcC7nj;k~he;=ltfiRB-=D)z<^+}>r!R(lf5 zPj6cIrS%&Zj(AEcOjH#nrd1fSDm*O}KBg)Zrz#W}N3HT_r2ITp{_#{fJ>**zo)ut| z8s4P>?~b;}I(*7`Z5vkeC{d}l4J-L2d8HDo3e+W8iC<3P34HFF1U~Clk9ab#Y4X-z z<6Q55J^5BaV%9k)5}{h|Mt`~V6(jV33_$W!fiK7hC7B=mImZVJQy=^w>w`CQd~l8Z zfpM?E8rX7Y)@N_z`0TpWXM*g9{p@SbXZ7no>^Ilj-)xoCTKS5k`HIB(k#(LvPrGLz zcKX7=-498$Byu9KrRqK755m@gEd$9nePd1W86NE3jo-vdb4Q zKM>fmrhiNuzXMxd7?Y%l_xU|08J~5)d%5Pq*r{OP?kGuCxZo!L+UQg+l31|XST$&b z?uwV+6|Wg2lDF_KZ+tcXjgvdgx_z;M;OajS4_qSQz>7#nBjG?TmBeode@QTun>E)E z20U6vs+t+*!a;DT_l0+C7z+F(8Ef&?)YT?_xD~w3nG}(*MYq8B>SOL_Ft+!1Hco7A zh;;=$MAS~w(A6_cxo=T7^j@l=AE}1=-Y4}*BFxLF2S0n^{7j#M)@4A;dS)ovD`2mo4=Xs||_uewQ|+cc~uzKUPI2W(@sey?#Ip zY^lyCQXJUgOTO+Kvy;aH`-jK=F(O_?y}4fdxiGM$Vx)SIf7;_!g9OI8u+!X5l3}Br zq;jE@CTLsuHT=O|dy(3vi;Y;=C8r05eVjD9NL4};NvJg+eN7ydY- z;;@g(ucR(AVvZ)IeuHt++%Z>eQK)NZNL=kRzehk$$*PkNwnna+;DVGSnr~A?bd4)z ze8Y`MRo_kdB(gh7bJm`Tp>vw1U&*Zm#`fq)UUf53SvRy9)T|yl=X#1&E6|s5k(BtL zGXe1*I%lQ&RDJlCe3+Iao%-a*{l`@PtUG$AqGBBDqpyLKcPFm@7oY;7_#dFckl6_2 zH$_*q)!y<(`AUmg4g~uXtKNHAKIgiVG_@ zPTCkMEUWUbCk_$RYow<|H5DHj*y8mW=Wq0$ zXI!>1G}E|r&K++`AMr~)ElX4tserP#hY0Fv^F7|!U-Fu&;}=W`Y^ljl ze7hj9WmI6xth4ew9kPQ%ERE~V$=i6{S$P|0os(B77H3|bg%L#~D5vs8=g;0aY0i*; zO0<1r)#;7(Id&1btw~FN2vR{Ns`ZlU(3&o|;6kaP#_C#g4r8}O&v|TM2yCg$NALR@ z0$VOD+t(BmQ zJN_(4*yt@BT@Ys$jQXZcn0+;u8%H16R3BV??Z$B%EA#E%Nv3GG&sE0pdS9+Cd`=g> zVY={NL>C(ua@|$bAN`GVqk6g4>Lw+=BELqGJxKAn-nP7^8Dq)HZKuXpwc7IcUocjS zmaV^1_1Ar<`eHaR0!Phh-$n;r=>m9|Dx_jTD98a znXN>Nx@TFh)N{$A?&3y^ZRA*z4LkI%$YN1&YU@;)W0^a{yX@4Q3P$v@EHY1&(di1bRKYs-atR{ zH^YNMePwzniG79DwXe()!AlUI=cz#{KQj=;cVpkv=-yFXkRFy(*`BXZTtE9KJwohV zJS5w5>`h>m{9QjF*|xIHXd-<)8Udk&PyoYKpKlqc%SB#cwd{lYLB@1@r_JoEe3WS8 z5bv*?Ehw2MlsAD!!f=G^yA@AF>RH2bMA{yeOSce#J&CYH=BBX92V~nuD&%2=B{3%t zdk$WH$*(-!GxP9+RGrzk2KQx?6W?U^+dGg?@$F7|;JCDpO+h{^G5Ecxzx`>%;je+! z#?n$cUdQLv1Pd+{aFaXc7gz-d5YD7`0x)XH7&h$*0&J_Z!q-friY}W9wg#@YWa*E_2%04>+fm+{UB{GE>DH8Y9 z0x{%C{W%h$cC%lopR}Mx>@R6B^ILbt&;&u>k*t{ysh-x)SGFK#H>*VgilPNqhkiDX zM#t*{bi4c*N>jR1uIwbI$HX=i*n<@%dlEu<>~lKEH8% zPVI~vpIzr1pQ@{74AV>D^josEu`9{>cR#z1e|EC#>2t4S+ry5d2hm3oxV6pbyBDKk zA8W(i3}D{~*4c~-p|R%>M$WqrVGK66tzWKqwckcF-7sN{cLfBKa|rb=XWAl&;59J7DWiP)qf|%Qblw7H{kSnrNe& z)&Cb=w$-myJ0E`K;jk@oZ{sb9^wyg+o{2Nz0eNJ%88lE@&}c_?ANg%)H;Vs2g49&xe!|EX`^;h}aSdI#3~dQ?hevD%(0kI+#8vas z%!pYkWahVO~^n?BMgN@?AN~} z@`<0C^z5rjgbkaT64{3ohBb&PBXhbufH1nwfvu-e&To4`iJ6U*@=)ZGnF#ScMFot) zvQ+u6Egm1E-@nAhQ#Ek-0v<;fFCAj-Kn<6Ga4!MG2xs)d`xZ>NbPb~o*G=#{nmAVO z^AZTro7fPhVPkn)56Pw7G*pg8pdG}h9R@8e%%l22^j|0*-rP|yyI?<{VF;Q95MZ#S zJJ~qEi;hAi;KsP$16y5Kwi9e_Ob#C0WQ^PGgNYC!AX1_0$6?6~VGU9f-Sa^Aem~D; zN;C{OpfVMS@)>mZ@Rf>|&7u|zlbtkd8iiG2KI|l}z9-Vyl+hX+RUjziD{ohHem>Xy z&whZJG;Y1(;FGPoF3~FcF{3O5h~GfJL0UZ*DLIY~QDe;IBwY1;t>{|f1(IR>=>-hu z5gL>8>Vw+8ReJRAl307s%TK7lcz)9G<+tD`D8J=fmQM{FR{pVXQ9g8o7Vs>yYrvoE z(RUg)ho2RDeb}RL=q4~HA(Ed07tfUfhiJO??#x8T|!~8J66!- zF~o(96g*6TGO$n?WseF_$gW|>DJ-6UjkP}z={$9kJF@#A0$ilmx_4D^z@Sezy^Y>MFPh#D8rx{*V1W4yo34z|wv+^75 zy*lU2HkWuRrPkoPX8?NnU~<4l7j|hN`V8zy_9T06(3m#0&G+7rAQ+n31pUkd!Gu3+ zISX-E+t(OcGxLj@jtD20TH8G)Z(V*^<1wtrYlEWIsLAP+KeGzPwI`Ym2ti$dl(S|# zI{{_@B3r>VkqVdpPvFu1zk+AQsPLdaS*$&lkN&G*J;@e)U&ouxpWW3cX0x--V6}1lKcRKZSANX%ihKS$C&G84?=)K z&HO?zVJ&j*W(|B$m@5|85t_<2K(LfckILN;->AeMl&E37H$lz&zxW5MwXU1^iEe7g0$)d})@C;+srE1T}Hm$N@!jg#%mr)#VE$@bG?!*sK$c@{EJ zsuEnUPI?oKJWjHkaWY@BTX1k+vS-K(jnaZl*=cF5Fk#73W@ufeHP6h{nzkKiS*Fdx zZS&Z$)Dh@!o3=gFk4ZS!qHVQki^veAYvzHxBiQ;>_BW9kxlG%B9!X`?%1#9BGHsK1 z(9H>2r133I(6)|`OSxE^fONi_wF#}*W{gXXYALmDL~6Z~T0bInqLMmsMCv3Zb<&8` z$tXCvbv*1l#89s3fg-FR6As=eo*r*{AP4Wjc=OLd0!wLwOTFU_<5BFhM7NuVuoL@P zq(f*?xIw&rp^mf=>Q0^R^@ec(R0L9xt&Gt_py+YE&}KH-YNAdwM@}HE%BGoX+eDsU(Zt8qJv{HDt0` zJZCn9-d9KfK5L?h?m2CzGNWI6_Kf^itwTAcyZ z*2D_-d!7J}59B0w0&%2fiWpnYeu@ZrqFdvrfUbhwO`%X??99vwa3M5~=-gTcy-_%d z@d!MR{C5-{S&P{WgblKW=|}|&_y{a@VFGw5avWSUNY}u7!zw*7qI<~93WH3qQR1C7 z#@S=dwEkIzcH&-JDGAL^B{3W%@dyCmQzXXZdCChE^XZxw%Xz7aZ%kKMrypkR#NDBy z1~wq_UjeTjZmkP!zQh3f2UEvwl5geghZhf^5Js@<@k{w^Bwa7-gcTA8

    ~zezsP%0>L5$_l3< z_5Hnq@8B9Z{*?Gk5*{P*PsPU*{;B-JQOXB$fc5X(0EoKVKS=a8T&3~o^TJrYG-P27 z+EH?qhSgk8n*&0+@78RM_4JzC=F{TT!2>-f=sj>7vX^CP8?w1Hbg)O>YiBdTUcC)7 zG??H#v*nU3Fhn**$&`b{FO!&e@O@ti(6l$lIlZIgO0DFa&hEu54I=cf=qR~bt30pN z;HmbgT&&O0wukhQZ-pM(d-OUj$DK+aV|@A|pT6-dbgKP>*jSb*t-=4QiB5Mt4fq`<#Bfg{X+L`@PA`N}iG>5~1`0CylcMZka z8({nZUVc?7%N&`9r@wz#`s&`b!@h}rXnZ>5nNqHlLR!1np8}794?FuCY-omqMDgE{ zAM)Yjas2jd>knA3l=iFB5Zv{QflkQI9^&hks1iTIDN!HF>x`8L)#|A0KQswa-Enpk zFUKffFS}@xRja{V>%!I;-|549DI-nPagyms_lfZrX5;75C>+VIBlCU)Lkc{hZx7Ea zOHnLr+W=3|aCi#O56^wY-yWXnd!qCIx$r}{*@q1if(6E$Np!pOGOkH$fHpS{Pcbc7 z1T!IwTsbg0W)ytf9;<(J`9ocuLqLjdA+cf(jhVb=IIadqL6+;<#Cs84Iofs;?J82DqSIqf49{3_d{b1&t$w+}OGT;x6 zefd{-TDve?bc=0CsEaru0r5oXsNEgkR-!?5JjA#6>;cBjOCYk8SwP`s8IcC(5?7xn zm*}DIEF-T-obE5A(3p@yHluM2L8))dKce*Y?fdupr0^|`^zY*TxPPPeXkA~C4n*Jn zkzN;);JXay)34xyKukb^@WDEfj$|VhU(x(+2=^H`pa`F(MaeBW4Z{nd{M+O5kNgHc zhgS)rdA9wQX4@bN43aZjxNQI!E0_SHt~ zX@bFw(qj<7&JgmP6V(UIfFKHh*?L-da|8%Wi82B4Fc^5C($$gv20k8%!81C&57?}3u(`7l6OWi-c5)Gt87YsA{0990?sy0NEs3?4ti#XI<3xZ%UyjyXniIdL zm&em#t@hJ;wduH|d@L;FSqs{%Hh6gZIOQj@$=?l!<0fEb{9LdvI(|l{Cmcnp zk9J7dGYja5jpawaILBD#jh~HK38NTeIgW`$6_`r>3u5(;VxEb4$ z3XS#h2w?*>tJJqHR^RCDjk1fI(Kfl$r{6&9w|3$fV$ukJF4Bv7GruMmDj8Nyi?=KA5L>h`Sw`(I9)ed6F-A`2;>Ywrafl|^bOXK zsc93K&6;aS9Q~@?#onhKOw2)@Is3$8{m}8i0WCgb#IDRg2EOCK*uhggc}@~|J<6-d zfhk9y%N~J0o2J3BawU`N`=yDXF)9nn*iwdD(MkBV^;b}1X_3T!L{;0Eo$WnAt<4yv z(&6}QiEl4X6P<5{YDJllnj<1JB<8#$QuUBjeO#($xUY!Tkk~;dy{HLr08WPkUkPkK z2++Z-9Hh)qm?IYw6#Q?G!E>(tK|*a@|K3VXM*n8=;&BQysyP^~fBR$QpR<2U^wtuC zCmXz=+|It_!^t2*Ngh{pM1sgka>|_wN9m0zg?x)OTH11D`FtA+vT|RH9-l;k;1y6j zRQ6Gqjs-o5J8~r_ZDip3WvP_+YGr>pkpS=2M&J(wP{Hy)rCxhtw($N6~)oEA{Zzss^m!FIr zWKjT>{#_7*?_2io!vqWZcc_mS9T9l*?&AunVbnBC;8p6|7^_e0%O0J+-8!}bg~M2E zelE=$^*vHOuJC1fN3eAW0%wmegnq!SQ&j#Be~zSzaq3PZ?W}j>4qUR1JaRQ6pupmK z4ZdwqFb5%(2Hzrc1-p{olW=pHTx5VDZZ+64*J}i921*O{MrEI!L?(Hzo}5oZt$GVG zLfu;02X^C!OimM9pd1NqY5xFebnlVIcQ-p&NcF(2_8 z9uhjaY6`?)?XF!5LXt|m1GYo7N2bcdx^8QA-nqJY)AVS6f9vzM|^vbMBaTY3nd38-5C>c@PR zrBY?TwX$!`QhA5ZI2m-Av%md)y=1dUm4`suWDdDquHU2UQY!KplhL*=rET4@wsqUt zCk50u{n~nqw*C;x(!r^Fztz^a`emuQpITDyE1b-ii&ANq@5Thw^%4!NoZTQTpiwH_ zLt|-=Zl87En*5woi}s%Y81rEfrmNmg-HQG(miun(2Yrzt&2DxtY7}jjtVP1xay=ml z&D7btP`9?UUk8^DxZv;*3NI+_U;R6n7_%SS2*ggJMQT zxn&&aL_LpMYId>9E=87799(VlSr%YixG+@Ncmz^N<`Pg5^fl?2EPNJ?b^n@FpJlRj zzv;eqG>l*>J(P0_K1@ox;lBjqL=S%iNfrFBd85t|08b$`6J_c+|R+2&>#)!QT52rSCKjn+=qe3qQ+9G7shh z*t5ZUpG*M3MQr8@Z~(ikh5CDm6D6V|4F+Q;n|T_gD#mTY?fI~tHynu0ucL^cwEh_; zoV-PR29wtfDtxhtd$@3-z=!hlA+*6pk8s&J7+sb$Y&5|(T|w5iP{%)9aJ*0Ap51XE^Sn@S+j ztT5ssBOpC{6Vf!<&-{)?knJc45%-irv{oeZrYMm*%i8 z=s|FS=R$Rx6XHSz^P=;xQWO2T7&fqXGBv{v5eKU*a`jP~Nnm-134sWd3D^um1<4yb zY!fupk{{8`Q)H!|8y)2y!R$+gzGU^SRF&cb+Q#j>_0)PlYH;Ji5fWx#bJPZ z#?n0;wm#x^5ES;(?Hl0(zMHz47T|j8CuoEeDv2tB1*~m?7S2c%7@6=W#Y09`_yrY5 zvtel)vtczC#+1Ne?}a*9Z3cn*Do46SJ`G;9kwlh7`W%xyNyY|<87n0$(0%j9fdTK9 zs?BUD4_W8ybCR(pg7=r&HDcQtJXhz_b~H^PvWc@HpFY*M0OqL#0fHd4n&qM_-EQ(9 z6Ydg&=5VP+I7ln%WF>uk;&Kv^iZUF$Um$>CtDRnn{nw#>^hp@pxf#Z0IIeS4E^ULE zIQ4NS@8e!DYMK{>F1r%}2G)%=rjz}gdK0oytc;u3d>**Aea*P)e zFe=UH4I@|+SQKYXoFzSv65QdJltgfOuh6` zLngHlSv$Nu?57V4;Q0;Ka{U^Etkvxx&F=QbXKsv|nd9;iE$$wP=Hcxzz`RY>0go)N zHwa%#aSf11oFA8qjnT3w8I_0YVn8*pQ8Qy7Jx;ew@pAF>Vd=TOojs6rC+tUHGBU=_ z5?%?R10Xps@4Xt%A}LecUu*GNh zlutAj4xzADfGIPzc^5e_A=f1EKr|gp#~^HjmkF2xz|bX5L$1`v{du_L!+BxxUR3)d zEW8>|ok8B(WxmD)kRja+(|&0f#gV0nynnE}{o^sZ9crQ!`TE8HB@ZUPPMy;_siiVC z8aKAFU5R^%;^g|F7FcoV$pT%LsTocd0A2v}9LNF%jSYHU@fbNNQOU;pyR|g6#ek8- zbD-gkJ1Ei)7dm7>{Sy?RPQ1~b2L;J!AI+Z!qw}ZOmpQ7sL1B2^Y$@p0*N_|ACVm+K za!EFr0IWuL;Sv|@1~@Y+e9o*27wm3)PD6!TGr0|BpqK1`KPlK^xfNF~wD)N)52v?$ zcOgdCUiLT^IFJFbSG@>o^tw zZVg)}Ha3-(r^ey>TRJwt<2CHll=H%G&%&DXU|eA;a}q?)U`k= zn#)_rM?|2+P9GQ;m%-bZ#xu=_qa}9sBXl1bC6(Ag1!*;2D(4bV9Sxg!1aQI~z9WtP zCHu8zaH_}}{b#GGw9(xusO!Z$V(n?{TLnPX9TEul5Eft;5%J&yC2qB$<^k(iv()?m zwxrlylhCRVil>>S5aYqc3NAtr6mELh6$|bJSIyC9)68t^H5}Dwkj}R%0n7NB7L>v^ z4-eM6Y99R~Qr|Z`IMG!TT8Q8~;)C^Wr1nxFi=)AbuMwMdvM&kQK7vqw(1Qg?Ma>dQ zcm`T3&;yQoZY1DuAlhzj!mC{ksYi(Tw}%m;F*W}RMae#jZr-JCWo&A}m)zcbKuF}n z(6o&+fJ@pThVU}WeZEJKgdL)q{?-p~m5P@I257gB5t5G&LK zifOH`k%|q0Vlyq&=q|8QqhyGTE6@|D7bG=jf^LjvgVmEt_KtJL<0F{YQ8Ggd3p6c{ ztIL>Up(NvcCAnnJfHNBC1i(CYCfV5QH9Xq04-9#$Hhw>zPlnEpq703E4a~QE3yjh# zOO(rV@D>;XMGAQ1qB#|QCbx6FL(75;%tgqm$#Oq+H%98hGQx)b^32%fv zq>LZj0tcJm1$r{`vdcw!NW^@Dd;~Eo^_NrHB-u>P^k_A|6Wc}Wr+|zwD-sziB(7Gc z$}X7GNSY~zOG>$24dWdiu?JqaMzIIx-x*@Q#xp@C$8UAqxtk}6ca?vn^s3;4EKql% zi_m+QbBi&yP5GJ(VV;DL1i+c@$?#~LB;#P+mwmNlZ)n)nBj0a%Q5fv zehn2u`jFBxVrL!td<_g66C|z7ErR{9;cruXuAzRM55yJ|lr~9n9_r8F^=sAoTMb1` zyM$9YJ5cky>k^>d+Z(Gl2lYDR>s&XByzF@<~h21W7l%N}{iDdK+Ma=mVJb zaM*`A2U1dv25Lc^h76~7qZG11B0(7GN&ZC_;7CV<9_!&D=;mNri9u<{`2gs$Ll8#8 z=}o-3f=MY{4E@+d#^jJVhVpj3Oq_l2ebdHv$Kc^q+=3|FgBU2&LO5yxaRXnVGMAL( z!DMehl$c&wjFa0*&EAb1P+Nlljqkfq8l1L#=*uYK}*RT zOp&f*Z&6yH7qqY3Az#!2=($}M95Ql=(XEv~bE(3hsuVhixg<5(JfRS7Bg@&dtI(3t zA+SyYy4((vaVaT`q#V+*3Z&ym`BRIONT!rY(C}5?2=SQUY3$R-OF%<_>y&S;9u6CZ zFrC}o1`^LP43ntiST%+ukTDO9pkC!wPYEBK^D&YDMPqsQme=IY0mIoqx{IC0r636f z47e~Hf(yg&F=d|iZK4{S73@RE1|$F`qseJCM#}p`o);^cA><}Mvu~)GhM|+-N0&X3 zIcJiY)6e7d7Pq28{L8c9^MHLoJh#*!_G&y+7J5tzg^MB4*WK~*_MfMjgA>J2Lfwj& zpG3vM6fvZ$bG-a0-YaU>5h!`-z+_tyocc^40yATTp;b8UbYUc7;R;~JsD8Oq*p_m9 z4`C+e+3vfXdLjexg)gPWK={%J_-_;c_3~eu!oru*#1_8PgI_re$T4$;1taDpmil*K z%+@b%e-3EYHcX_$YBf4^MOZMf&RQzp7#uHqI}wdmnqJf|xRj#3^+Osu$sQ8Fzv}B< zex=ko%l;SFnF@SWoA&e*CNq*3^W@cjkUmOm0J5s8Fc}SpqIu^qC?*gTZY^c7sUVn# zfgtv1&j-MG^qd?80Er{jH;-O@(rDEuQ1u;xf`O6MC;U6rCwxQo_xnd_y`HL{e~#+) z|4#KVyjPHaj@BE7SFezO7yV5T{M|Du2~hO;U>HFHlsp2(IY{8X9it$@Kp@OjK^WNw zhVub1k^~Hq>d%1$-DuTgkQkUB6Fv=sk=2|3o$A3u&f5n~?ZaBH^g*&Z42x9=MhruW zJ|7Gt`hb#0pg2b#oM{^c3YQASD}42P7Bisee*+3i{tZy98x4v{DipIuhk~O24Jau2 zH$d_E>%(xNu%s*^h(Gfw`D}v7Iv)Td=`kx({V;lLtT)h5p$%V^M}m`yJsZ>y@N5F0 z0OEF46=pPpSZ`S20ei`7D~cw;F*k}oS2Qo70n_=pN-%As^8|mU6jTNd-HoV%Knh_S zPn?s5DG@Mn*RW5}N3|a->ImeO@YeocM(3cPyaanHOfOa#^4j1!cwlA0FScU(^Q4L$ zDaGQMb*n!O*4?oyWx%iV7#iER(_EP~;5X98Hu{(_;5XC93-poH*#2ic<@VqL(~ENr zlnG~`{(jZD#lW9MNx}!^S&Q)lxhld&u7J>`VMg00IG)8`Kx#jdFL{9$Zz!XS%BWJxxHeJ-H-CfG$H?5ZYVHdo zx%(;iWF_|i1rh`-0acEYJ73Lx^e<}N<{IRF=Z_rz?a|z)!2m|d{T0#?-_qX1&><4xFLkkaT^WfctW zuV4UR(0CG=u|=af@_vMe%<<+dG{gV=tabO?Xy*PPe(1t#cyhrkF9JLc_BE1Y7=KSm z0W0$&Ue0V{?F0LP6gd>U%+7-JY7NxFTF6_NKSvnnLIOK`3b`SUfLcvr2lOB)>0n+P z+2vv*bY8wTc#^X+Y`pa69kG5POkg*^hOV8sT*_sS?*=J%1aAb&r72dwC%K^7xHdWX z4jw``z7tw9Pzly0oH$#Y->>s{KgIXOkmcv=PM( zD&YO%xP$)Kt~WyAJscD)`$BmG)-wR5{cjOaCMuvTB2c~y4x1q576?YqI}VhoxNz>S zvcBuN$-zEA>AqE3I+XlrCiJSw*h|`Jv<8GpY&D7tkpDPXisDt)kV6B%!)a#=q9CQA z25W%I2W;=T=#O1Vu({1OcB0)6o(1i;72x8DO>ID?g=O+mIlZnzLS(z)wJUHAqf6Iw0^ zqaAk(6iMiDvuB#sX09C*upPe`m3ehl{LNER&p(L*{{+@qC3k}Y{^?-H_C_h7??O)fLL@vRPLEY`;Pklayxd1c#}L%zs%9SKbkKa$Z}N4>pw5JzqRa zV|ao z=R7(Q@S2(g@{-YKF#T~^wjOorFZF`=%<)cz7R&@-SIYYA2)yu9U@$e$Lx!N{3sVCU zuQW|hHy4jJ&#AsKVocskz zFUsQJz6mt&;(3p|QyXrhXgjzOGl(h3NjKm^Lviskcx{lsm;^Q`?Uv>5QAapxj{MONK2b^FVR-2H|p&%`c#& z7P+(iRHE23wY?`vr}rrdxATNW*JsvKoJ}zA1VqaqPLHDk*B^O&M zl0EKm7kqs@LU_oGu*QgxYC@y^G#KB+`c37C!@1E--{ZX>9gXpvYSD-|Gv`#CO@9Va=IxA`Wq^aacs6`@$v5{dosoqH=?Yk5hWdm>{-ii92Afvh{{Z=DLsFXO<&H z5mLC|DzfJt@>k+VQ`T3KM^))cGmketzC?b3zYOo0)NTnQ9{sqk>?Z~wsTOzoCa zUJsu+(}2Cg!MxqL%kGr;Sy=23=k3eupx!#B(P|ro%uDIvIY| z^4uXTH?G4b+=spfw*Ll(km+&A=TAJV`h{G12hIUlULOPNQW}Zr4xI{>5 zU7-~N31G!}2L`bf@h1Wd)?B7-yG<_|%jEsy`+AST^xAH*H#c}e<1yOI@Z|b#`i=M0 z1qfZM6MM77_FdLe%bE)Tx1qo`w0gRbXe|xZPQ$mJ%!`CeV0pdhX@ii(jg53LVkUG2 zmtyPK^n9g7w;xw9QS)10qq+WRy)YSO>~qAP1kad8Y@C1?*N#C;a|(o^+V8MuAu|uy zGPD5=3FIE2YY1j32!SiBC74EZ9b_eEBk3bS%BOITZm@c2SU+NWGKdXVB~9+ID69^b zE?1?=yf#QuW<{7Pz>*m&)a061u~QSoPVn%n2GFwEja z@eu;`wLhm4Aa5)CuP(p_19Cd+3ih;L*I_+VYruKR`?i3_8c%uAxvMiHvgZVsQ~byz zerU3Sy$FfDogat7g8|}!f~CDTL402)o-(Yy{EMf;*_*c9pA2UNFH@x0t6yytdvvA4eO{rI=w^PnbPljh7o_%=sn+hJ1AeLuwyn$NC`c>o1-fLcN6& zY6%Y>2B!O3lFcY9(kG{sKFOgzDMA!dP~wetynFEfDE?bZbeHUJnXeN+<=8vHvG?b= zq6s!NO{}jzO%PA;UTBF+xiKysT7VX+efFtNJV8Bcy3dJ05SIgG_XCCy{-HoIV?W$% z#(xI>v+$pXf8aV3xX#2g15Ye>ermzff~OfziZ|kE#M6MM0Z%=i`r&<};OJ(Qw;2Do z;(r~L9qF4A9WW5r7yKKojq3-r4QZ6HVSUG?oAPpQC6%5*mL0Yb^xkewqQ={PMUD3t zk7yeAPb>Su8Q6WUfHWw6m0UM=L-L&P-9-=8CW3v)UC`vgdLvMaJ&+~aaO z^1@heSyJ2TagJ(nwt297@NKbMBOwgS9Uve(GZL2(i_1h@Mk-zPdNW@mS}^T=29pr1 z7V$h7EXF&N19}eHAz|O0VKptAi!f$8E7pqBuqx7ygj_N^RkU-%Z|JQi`JI~ zA=2?k1!cw&4ekU~84q&Ml-8Qj+OcRY^olva^kiTYPa~e@Me8FCUojH;J$oDgo**H- zQ{Na>NDK-jK|8Rz3^BB71q$}}?mxouz6*H&LMAXQ1cq5fx={tgz`-!1dJDiX;AxB? ze>ei6VC5L%0K`y1_$Fjf#}fdwUg8t@yn^zoa{+IX{|AJ4k8F=c}MUTk2U!X`JjB5>RCPlFaP7tF8pGiX zSDHmK`D;j!-_;55YEc_RCh`&YnH;db=a~jxaFM(_XCKc$ynZEN&ME6xYkvnho9ri* zVH5r3@HF<(-X=}^jgh0_tO?+TKBI-s?khC2w(oI4Bp z%lckzrmi0Du-2r<0sM*ey|owc{-8K)+Zp2``*Prq=8IYwIYuhn@QL-z+L_eI4&KDQ z-_S(syTbkQdqGi$g5F(|sg#!AH|wPOtqw4RjUu^w}-^ny==M=>sAwtlsyKt|71^1q3svDIc4+w@|)9xK+0+2un*X{-Hfycf%+Qc6nGx6>-!MOO@3-g76engu}cG!@&}~p@$cq2^Qk*YrK6OoQXGF zJ-cuyI9;g?gEE-bO3UMMv&RW(MJ2`7FIJzR)humOW4$;gr#<*NhaVb7T1;VU+0@hl z*s)X~`?NcLjRkGBq4|h#855s~4y(7YWKzv7V0(#mESLIwe};^-_8ssXcpZ4!P*88K zoyKm8<@?RZd~zUIP4;i=Feo#zOmI5beJv!_8+q}?s)buHKj&RsFq}tb+;%n|3VazW zN}N2TPo1LAFA0~zOaa@>@S);40JkTB;48E<7t!Q{i7P9vTx>Rh1HACtXmbnzi?^WK z4@U1f-dJxY@Yf!x?mdaZV{0}>mpF^?Ijksq`0*;a#3*-qkLmJ`cuyq=Mb8*$MOy%$ zDlYl@oKSPPVTQ}@WF1Xd&Hz@R2#v$$i3$0d+SZ%Ip0A}6Tmm4L8MDOQxPG|AAgq1Jc+E|Dlae&w2Pjr3n~r5I6M0uK}O2BDPe2|T~JVO z5G(`j2_BQ3!KDR{5_Ds1(98}o(kGMzn;;4aJ-oQ^wOgHR8n473QHYL5B5cuWv2*W% z^m>$i`7_$i)3VJElim(W;t;j$R|w4;D1eVa;3&;eJVUJS~igFzs4Ap;5u&jj|PEg{@j&CFZ{1omaa_f@W|Kdf?WzWz{YLTU7;TV%R54;Ys*jFkqK!v)Kb^wjC!um%B{3{mVo>7x=JNoQHpQ70Bl&TML$3Hl!-Hm-dY|H-PA)-t;LU{q% z_=X;C-+T)T(JJ_3YknO#@!5{r*&UIPKkyK^cKAsogzo8PA?OI;B15{U5Xu%sO+SIph_9}mi4E!pjET}$YYJM9pZk^LzB-i(ZzsnlCR5CO* zzexPJL+8EzN;-hqNoAKfnf+hr(%ak2W2x_tw1}_jf^a)?j&X4anox!z;G03 zzJS9SUaMzE_C*FuO0OAU;`Dh-hAbRs@4&aalq(BIS%AN>OJ#5$br$BDOgeIfnHatU zAY=qg&8N|i;J$V9(!tY^9jE&4zY`m8n_t-o1W<@4P3~y# zNszbp6k|&zRsDDlA2=)TyFUlXHv3kDU+3w=?ps%sxRXhw@oyf42aFIZUYY|ZDsW*G z-V9Zk4m&Ohx(*#uymkA5yO7)0lyK>z;qd0c(-4L4XiLu7p|fRJD>F7E`8-Ldf(g2Q z$k~aeW-lNY>rZNifqT(e;DS_)viCRvOJ2KW?^e88`y1wopC$)yh;RC^f+n9;&=~pJ zzw$OGz0D8eLZ)(gZ9(zZu=0+$zv*Gse7c+^h( zvY9x70wy5Lg8+Ja8x^5Vq=@z78~IbS>^q6!G|CXXU^{&j@ZdV*<@^~=v5P-otN9rF}>1OgCooaX0%Xvlr0rX_g6@nJ{aksGb)#JRws(}|Cn znldQOEaC~KrtLts>BYqB=;Ot-Ts-XfyjFeAr02?lS<~?p(h)HOFHTBv6~$CIg2u?Z z(^=ufJ24Ok`yoVJe7ukGjZ%-|A@0nkY(03T=ezVo!H4h^CLy33FTCKlDJHzu;s}mK zF#mEo@lAyj({i#Tu8qzPE_UmdxD%?d=)#V*Myd{pU)@nZ!L46Hcl4a3orr)fl!%9u z{>m#H4y0zCFjIk?&ck4TNHsYrd@I6nKZu-0z56)fUUp?#JL@M}_t#x)?bT$iVbyI}Bo9;Bc@-*tgx|Hq_+P1ebU}7fNQ$Bcxy9P6F2>1}_X1C_$<0vk>Oj zM2e5Fy(Bb~jYCF1(*G5_0RoAkW(gIy?I)P7lh}h)2rW1m%<#zXIM_>=@qCAog7VcX z`HVH|C|`q;uPTxct7Uc{P!H}H$sO5tenc&xkQWeLZ)1r=YwWx>aaZ{BKZ@KQ#%g!_~ov=Ht(^i2j7RWbl!mo zx8u%ppb3-eaoqnTBtrOXoWL%`eyn1vbT{1FL4aB8aA)(Y?VQX*2L-B)G>^U~jAJJt zzbZ-RAt}~9H|ph7YhXiC-305t)h02NTsL`xq45;gLXKl?k>qj*R{XpC+?^w-G}$H1 z>Y#fM1B&cf+4Yf-fAEkYJFAL>kQXrdCcSKJ-c{#D@nP>tou>8{Fo(@KAu=JJ{Du>L zS8y8v+kvFq)ca=zHn?`Q;?v1)BjJ){iWyXp?d0amZGDvOg35j;Bc9A9j^--#YM>+w z->!dq)X%H@13dZ=TzV9>np|B0cB< zGdBeI4HN4?lSQs@c#Vr`L)?@Cbu~Q0TY&k%vf+Y43FqA`wBBN8cZ=UW1I^I|OGCHKf&Tasza{`kO zx;3wO%eA=uU_0|bLuc~T>HV#5|<2wdx|Ph2SO z8Y&jjAcRy6o`MiU=Uo!H81fPqD=4g3cbf7AXCf|=Ig&Hwyg6YD&b?J4BhJHyg_F1j z%EjJ<2PHQMFLQH^7i&4d?Ezj?pocUslh`06zD$Ayu3dt-TrgK(pC)$~7i6~9eZ?EQ zP%1O$=v&>mF9r%3qa(aZAI@drV&ZPT zJX-<+Osj$23Raai!!vYyoN)M3FUb1|3e9U%36w8XNp?Xh9&Kk&fJJS(1@WU z%Im7EiG?tqSyRppny{GtaH_A6+cU|g_79W9!m(jtmN>_b;K0_m&kcX zoG$C`x~0}M4ueA`I~RDyAs7^^o@Br) zlgMH?oxKU!UWMXayh<4yh=@W3Xn;AFkim8!4iK{6N0TF6wI3KVC>UCOA^WwOMvSIm zZ#{;|=AR1gj2vx>P1wLZ17QA#tf%vC#+pZH?oHSLMu08p05x(DjYQjY*YbA4FcqDs z$vaZ8+#t-t`O@u8Bpp-_(f0*4TTGaWaBEsP{kU={T8A}q7}xEo6Mio8!nzDvbSgVy z2*pc=DLUmNGb1|i;D=tpk7ZB20hRoKR0s-710G=a26MJkiL%lT7`B0-QeMT($emHM zomx?d63Cj3=IW#FRCY5J0&+C~U%lT)D-iY-g<%qh7eg^zWb)BuCtv$FveRlFFg3jd zaG_(O{LD&T*c-Z8#QIUw%YfuMda4~2) z$`I?1Lb2~#jv>w4O=Wr}-eREB!pIbSr~RWuXxl^kyCzuo(>=o!6f=hzmuFpbpe|cY6`&r5D(V7{cicgaqZ| zMCJH7?jwT&rV3ri9;d=_T3Sf-?k);qbm#Fv$F(d=(97fCcr@F4u~!}gBFpw>!6uuJ zfJjr*QE);1aZ}Un#J4(nZ=;7`ByH7kY131!)Llj5`@0hC0Vv-OE60Y|S( zGSS(ob~fQ|41krHup3=zgyjJ-G<1Q+jV{Q#R~;K|Tu%#=AT-Vi5aoUR(YY<@F%@3v|Vp%b`k4V^8sn)U&}iFz)iF3R&#I zXkqR%{A9WpCv(^}P=7+K2|hvUK_@2VMvk&k))F@ZNUm0$z+vZ7l)FgPqQX5d6{F5w zs#GL?QfV*LVRGszN^YHRe8K;@9@kf+LxI(u!1?h>XU&-pMA>5lk=< zkNt7tKH?dzFlNAu)@roaPC1hxWDXR~(|RVl6IdS{6T?YEh<$(;94!fWGM(%#dIt}D z8mt3-nWB2S2tk<+k9>KERcKMqiCp37RUMeH;6u@4bg>2iMZ=1e@0?Q1HAU2G1M2`R zE>S&wMwrY>kcj4<96Xh&ig+mj*?a`k&qXvjV)~h?Ceufgu_ag_1rNX^6XQ&sF~3lN zkdsZX0SuhE_jEYedTIs{mYl52rbY+*Coo797BsOxOVNts*o_eY(46rzz)F6i9c(`p z<9FB8+(K=*82V_CoK*iXjZB<6dlV;M6zUSDvU^Y_4xPDp(GyV=dlqXhYS^RraOYzU z^C#@Lq7TLJ84S;2CH-YlP=W`or|S&XIlT22Hcg37j>Z3x^G~0Gddc>PI`3)F@O<9z z-C^NjhUtpWq_XW1Y+oQo3ro!QmRN%yjq=w940+l1D0>jgJAe4ve1%J1N*1`>K$5od zz!^Okfhl?>K*#rDgnRxH)7n>&j+eQzKI2 zb8$76k6NSk+tip8sWA)pEfrKhNi{yL)|ks{yf#wflW{e^hpFW3Hts^`H@5L%FY!XO z5gz!j1>-Gk%M0slJ14+fScX)VNw`nbAPu2gnj$SX_tv6VBG^0ON|xp?C%XrH8_X+l z_3cnVFZ1XNuNGFo6iahaWaXYQNrXrk@Z4sb|=sg z%g^y9<@S;xr?){nTez?_36|xF`O6bLhjGBw&hDk-P9nP{n8IUodv{Lu8#-V>++z%zj99>Y59+ByJzy}kw-xYmJ;v}~dHK(*L~3^!pt*7EvgTE2 z_5&O`GP;%?+X)z?$4F9ad5paCu`HbXmO1w1H;DFdub69wG(SsPlr7EAl@?vwYX{Pm zN=!}r0DxLi5mnUt8UhA+`5HZB#ubf!+k;Xu3v3Y_qK_a7JKRMMrLAb+n2kgTWJBHz znGntac@k4oEk0ao$z7N+lvlZ*g+WO~IUT)u9FA8}q+l+1tc{)qX~lGD#f;!OU-P5* zw)>hXI&Lt2z;Vk^IO2Gc7~3J=f2J3kPa@JC2A666oRZ<$uRcFjpC7AFUJN_IEss@1 z6+HF=!7;JmF>(h5<^c4~&ywc@gV}LVQKC4gO3wq;HUAk@RS~GNBv*F9W6!BjZH+;d z%b~h94k}6%2i1!p5MT4?xT4+A{{UD1dliNTLULWJbZIz!0VvlQ{ws|b_I(_WTPe-Q z;ve(Qd~z`{pljyqm@3&vv^rt0G(Ur8sri`x=3|zcpCa=iw}7sIGfRu0)eB}urqp?~ zxQVX^q67BRmCMR$9I1*7zM3I8AfB0V#YluzAUXNth^p)|W6XvY{rMUkhghfAcy6NW zWumC_6q5Rt4p&5o%X0-Ltq|_A#$$kfPSVfk^z$kGz)i$dJ`gg4It4`Xe+5xTg|D%1 z9>`!=<&aH{&~-z_ozA`+1(?ZLN1=q^LWrIpIiQ>L`N#nU?!Hl25&T)74hxGE1cA$0 zJ4mmstGyPMXdXDb8t=IRelX(NPwptVXRTEn9JZTBl2{i!#$LmRlpE}po2e49%}aXD zOZW%erZu{mP_ zBSOSkDiK4#R_Qn!>vC8H#pw9fXcygu#mKjyV+UYWY4!KWF^rDC8;g?>_ixkjAT$R` z*>QC2#kd2)j_^2@j`!l@8|l~=AzVJ*31Rp05De`o!fxjw=SNuiPoo4~6UEjISR^Uf z!iC7(N($C9T}z40lSZlu!*&m}8eUpkzIl5n%CR(kHLW= z?1g-J2QouezKw-pPI282BIP@oumt(t_<JFzL0t8Z0 za5AiydaMoP?GLEruYcu#6R~VZ*MatbycKeaP$fn%UnhhGIS^3?r`*PDd`2@d^pF1%!*W?!=OZ>nET@`Y#Bv{cve=TyZ@35cd3`GlebNlBJwe#j6NuVRgKq zv9dA0i!{aJQ39K*j$CBL0n2;sr4fuxwCHi9^9TEnKEwH&!0oYBRLJIJ_;K6inrI zcgN<^u5X-zjRrUAXo8U7{SYglR3YHGQkdHQQ-qJ!_O`Xrwp*y;Xxqc^iV)xSN$8IM zKW(2A1K~j|*7yne^R@k7#ph`IiQ6J=|0@AM;(!2-T^L>D>561xoDnuSm6K%-&o~?o z4rp=2w?I8>c0x{mna(upy67PD4l$sAh96&s4ag9ZO~dKnjAx*M#0gzIu)y61%ZFeZ zPKH5GenJQN?}T3V;FwMaR-GCg1v-rLV8XKC#jO)@Ru9J4b6O_RalKxx=W;d8zmzoj z@^0}MbV@uSPlVLTNGN>LVp_T!$2|-y{4JB{*ayxY`6U#ir(!xeymFEXv5b0Dn2(}> zy*LMb#o!JO@pyUy2SeCfiuV6Y4b*Zm6p_K##@jfQb9Jv$L&OPm}qBXFSG;JE6rk z)KdL*hwTh(qSM(29wP25P)^>img=Dn+t-?6ln8#Z3Ql!HZe0cLa2|rs>;zzB6$hHV z&2`z?B!bZ$Ii&B~2BV5GrfsFCa56r%T?MnWUxcI8h$*>x{FaU=7<>zUBc8fo?KqAz z#XH6h$QDtTDzNRMDFc^V;_C4Xc0C7@D@K2gr4Eh2+kj6fUY>LU0sXKI3Kgu-*XyJe zdN+&!S#JxmKO+v>z;N_|(}X3Lac~D&nG1%T`TE{V@dp3Zi(mqfb6BcAIj}?LDU{zN zmlhW8%0kf$56qjX_rll3erwknm=$_wrwG~`=UU%f{U!2SfNc*Ugb)_bT(bkOB=c>AQzMveox z!#K?y;E;EIf7l$E&YoHwX)rs51qeDOIGtvzfBg-e$;A$A=?Zb3a6Be52!v z*N7D{0Fh_Y>#QJnWv~Vi-M`F(j9JbIxXDf_#vVTId(!x^YHnRo>#ZSPyAhK2A zYyezO4k1l&;?r8s_@@n?vCmL5>He1tPQ!dl#L4bhj6nf031cH~7Z62St4-~`;;^zs zl|{|1zhy*ow_z(&6UAiU-L`Xd9+nY^KZn+3F#!25_w*F_ zvH=u0%56<~tz)4&@%s$9DD)f6{Snq|_WY(U8{3L3mYBHpddpx$nPLZHsHYqhNxygt z5$$9eA!aVj?s4KptC&X2E+3Uvmy6|Y%rTBXDI#~@djyBg*j^5wDBun2o6+4@_ zueTwE=3N%Ye8G1x^oA#N8rj3BMzyG*E(@?q=72@x4t!U4nEDFWq84yM^llc1Sh1LG zd7!!U_`T73E5sW+pjQMevvrMEtNFmQ<*S(Q!?6{~B<+M-7DJT%fRSNN;?FkDD)8s{ zr=SiUH#`)rnW0s7$p^oZ9OV*DQ~6D5AL*HYIqJM;WSI)}7!05F3@eQi2Ta{G86_^m zbh4WznKf-cR@E!?oTsX~9YWG&_K7#e6UA!E7-RagxosY!mi1TacbvQu_D9P-7 z=#A?s#YE0~E2XY4c6niS@2bmDrUPX2vKMF^OR-Kny*O$NfeBzD_2wOs7>w;=*&1YmYIVJIjVP3 zeHIM2n4XZhfdLjHDt;LU&W)_-yNM81Q;ZpKwakFmI2MWxPnL0t%mE~W%mQKDTNzd7 z&B+0;>HmR5mn1>+~XOC@9Sv5#NOue?iUSR*(DeAvCJoIt;wW(W~ z)JPzv{QLcmhT4GXHX z2V?Ox;y1{@vl5@=IEQU0jYrm|1h+lLEJ#_O6Y=c?kjEDnM4p^6^uK>6%O^HM!pv{j zEbwd~dAP-OH)b^W;`3rV0(J2zsBPNFJn6jV`G+)S%p;5lwcaYK;GazJ03e<(d=iLf zQn)m`0C$8FB&L6K5`!nJah_oCkQEt1zaRXP*;`pFij#{w)wQ7FVPR$dK+{ns~LVxzHg*ayth@Qey zwQAuSSgiwvzWuAv7~NmsJK!HluzxtJ0luzP`D@Y&{d-rRodoc~ha@gtdx7woErJR! zE=zFM#9og3OkcOQBgOFbK-}NStlq{YcZaieBq9ERQsoZ%_}SP$2es)#VyF5T_Rn7ieN_3sW$_54 z7@X~n_aOlZ&k)toH|SJI^z2uUTJM$W`b&jcVJg@f9yk6nZ?Rn{;}{RsAc)bJ|q$p z4hlDiiB=T22EmRLCiAcoOgRut^}Ig*<%utaN6Bk-i@pd7wx{HG6r@=Zuow}e+$QPa zw-)K6HJ(g?aPBDFv-s`Q0LN3`qYvTW)(~C|L0P@|6)J72})~MvWa6(aQS{zF#%Ygfv;2Jd=Y64 zH34u>nH?S;%cxNQ{2CQOvPWI~Lpc(AB|&UhgHXS{Gl%*+S~J93oF4jwn9D`~Am*Z7 z&p$3Z6*IF-`t4`sl=q@Q^9mKQ5rpCYuS?eG9BRbHbDVBdbAMycPpp}X{G6E1pGQ?d z4E)cw=uN~N`}uNCe?YzQYdzzcqEU*pB%G?%3RM5}u}RQ&w+bT(<9hs8)`tp9?)g3K z9w!He=UnwIzQju=t@WxfPi9GOYs%1gQEqI6y3I<1Dr@UGw1JV{kG8cHSgB4cmava^ z;`T4miL6gG(@vbCw&?05I9@#6UGk`;6xUy7d|_M4!)QIIZ9gH*ii1Nh?QchW6k{y$ zPsNhPKtdrUf=Gw_|0mVVSNu)K`nJ3aa z4%2zvkM%FI2z)Bv+-F|f7w@{V19zWGs_t}6igykE(Y078&ibx1#n>N>=|1@BiLYcN z_$EDfzV$FAVklYU0zW|Z&e^W(%vrji>b`K{bzxn&oO>yVfdSE$EIHTpY3oCM2pzyG z2au8mcjm&slY|Xcuuwif)ffM#RN2Q#rCZJD##c^=uRH^?yfQ~^Vj6KSP7Z!WAA)WZ zO*;I#vvO(y1YeyUU#-uh-uPqK61);=WF6oNXgy+Jehhw)n&ugxcm0-RZ%M1+H*|%^XwpvEdn2L7;{wf!F`FTrI3JIQ;UNgOtDurBZkGcFA;1ui@_zU zwZb+m#MJ{*czCd0kEwx_$*3oh(Z3&!I%J%vE-6rMx&khdLL3VBb zl&I~o-=VNqpFYeRzEC2oaU9{R@C(ze42rJlgL4pNc9?p#2zp_exF+w5?qzT*%E8P^ z-slLztUr@G9Zx94$Jpr_J+?t^k>oNak2|7s2jA;l6MR>0bp&@-MwU740~xyR(?~MQeAQdb%;iILTg|_61CQYT^-FpxzB!_4Jp7j zx$GvGv2kA%4wXqJ@Fl{Kw)jV@%Mq*C%;AqYaj6+ZYSA3(W_BwRVP)v2!XhEWtx%gp zB7<$jpMv509v8ep$13S6L~gjxamUXAR;ZrJm3 z(cBdKDspKNNh1`@DLlL?Tb1`m`I!}^G9!#<$uCY#^x97Eqt$Cme@6P*6_=|lQFxzJw$UoF;lzImR z1G`SsD3DVbh-O`V@s)jQ8A>J)g(|2ohTCsDgGY2luE^Vw*Bn&Nff*tDu+p3|lR6AI zk?Pachnn~&PFFZxXU6z?VZQyC&e|M>deMs1A!tf|y{aFg&rwt8&@gO~%i^IIINqG! zU*9|A!n!FsmHmZY&MxdWCKUR!R*g}^`{R7sijzqkT1+J?&9z8Q6)0QMsqiiCytbY< zD=!QF&B>mvZbb2<3g9&^nIN?g+3v#T)nL+gM_j`rb8}o!UETA)L>cEd?B(f#nto5#G_`l0=>$2 zfJ1Yon)GwoY=MUEMXpe;A~U2-WY(6~8ZHqRnn`>snuuj(;R~zB+m4-ZGjgj({joC9 zI;yqM^P5ufPx=9Fg2dL@5|6vpW-VdJGCQpxeLfdWWpPX^6M7VIZ6oqNtcbWyMkP|c zf4gRzb?E~ph%K^Ef|w?po5dc>A$tI;S;kDz61ya{fPP5&U$~PM@L%{}@Lv)A#}f|6 zJ$8EI{Qo9xTJ(O>1`lNK>A*Y1K48!xby;edRAFa!O9b>uRnjcGZQ1w4_ru1SlQyJ+ z9q%FxsB=g62BKieEl-~Mhe)Qz^2t;G4Ia@IkkCUsHqM{q3A`QfgvwJKjy)f!-!vhu zI-KH9-P0AEC0>mjo=^b^Bg3ZSsO%Zx-J=HY$&V8Te^kTC;0mdqTW#X;(A-I*-G1ln zSpJ*MTPVU2dyb&tZhtrL8S=h4zaus}zhg4yACAnq>}OWu8%ZopHg`{!+RFamaX^^6 zH~7;t$=$yN3iNF7fMM=h$PO)?bk!vH_x63&t=^XU?etddyT-jQCAia&D|A2E>GGqO zFv^l~qCmGN)Wdw*Cjgk)1jaW=EsR-e1+{CuX_BYupBX)KCyfd4YK(qkO83U$-ugcp zbkG(gvW}=IcJFzw{*Rve5lwVqS9B;p`#SfiXo|cytCNVer>nlh6Z*a`XJ6+VQ_5~1 z?zO5TI{CO1>FeC{gyVZ}b^X9UX78}6;zO=kLf_n@!%~kmx9sb*cZB48yq%2yn>n>O zI+h*xf1QudNv|Hr+J|qC6S(@Zr(%|jmP>8j=5TyTCZaK4`qrL~>3gO$&`>?yjHUbl zwLg2$KlgO(`5-xe_L4F(tVkx;I3h&%@NUH%6MGd3<8k1U4J8?ee>4-p5ip(`)PJ#4 z)fuFSjklscXGPs6agH!hdWe!`tsGpIx^1h>)dUBOd>)73N}aF(;#uv+1@{CpFkP2(dq7+h^?%pmU4W3kHKCfiOt=meVL)gYr00rBflSgskddz zPWdY+s~>QwJf1p_L4Gu{vUVCW&<`IC=0Zk4Du$RZTWa+U*dFK6B_%6s9ajLC=eci3 zWL1ht6_3P?sUSM#2alu}CkKzD8l%+Vg~^S+beZ1h@pSz5M`$8-q=rjYBD5ZM0GT8Q zC0K+&{NMPwWD^J zIz=j~?FjYlHhzgl?X)b?M{prItDOQ2lsdcfiI(8CP@Z#$m%o{{S?X<{^+8Az7nS!( zcG~IoNKwciKCA0TRtDMg24rv=k+%%%{?#-11}}D1sCy-Mb(piXhytnAcagdvS>icM zeZzW|V^(!k{HGxzd_WpgPkc3YDlO_o;^S877m!H9z3u11)}wx1Heh?aQk_*o^p{~$ z&3QM4)6ID`6ixQI(g3XlGEL318v{v+t8=Xmatv(XYFB*J1K8~)dSVQ5fQr4?s`-Ah z>zZTLd_S#tRX<1PlUu?HOPpR|@?7m*e`-Yp0copC)CM&BmD40tS8yUS=M~m@*S)J1 zKP@P^*U$>JbyW2MyyhMX2Fs#SUHo+!(JT%mJHedP_@wXfZRR=t^CHD5({qfoYCUC< zI|GE(;-_pR za1MI(Z2uhVyEmu;W$K%H)k=LX;G3s@7JXI!hNu2Qe&gVotx2&erFy!>xT>nk6Vl3D z0y}|PtbHtwN-|A`#Oe(D6{|-d;+RgipwZjppB*VKqBlmD)m&FP%v*~KYTfDAGt}?{ zkvjr3YkJPoD<#J6`2%A3tQa`IlmDstaPQVguxScvSIuHn1JScmm$1fR6T4-%6n`6) zk+m)a-r=11>Lt%EBvQ?RUkvlcFU+EoYn=>T3|=Zyyu`W>kx;npnqBhx+pNISaL=%w)YFgN1tZQmQhG(N-Q1yh#1~TnR=*wqlVZxMDuSm$LBs66^!7 zuzO!k=X?XBt6m_cWIjtW&wm3nN684FK`Z~qbVkLC1_dut_gld=;iZehOXr7|mW7v= zEK3NSHXcAeG04^i{S4>=r?KpCNc8qah_v-89>Srk`K#P#9+@|QdKx4Y;$4T zt!HC)C3RAd6Q;S$m=%DoW5i|EX;r2fl5)1qm^;i{R0fQ-RhKu` z+f>985{8fa!_6`R#)#VV^0`U3fB0p_+}Z6xPDg!)>o_O{$4#$o#<7! z8IoGXvNZ5ow3S7O03u;5wu*(Fv}O@x$U(+fNO&s453M9gI8?)5_3txfkHE}PG!Dul zA<-TwtC8N1gw|3@8TF8Ojn;Buz5=`>h2{V8N|mn5YJS({d^3J)iJd^XXINw8Pc!F~ z#9m1Re^VOJlvWlF8Is|-Q&>8T@J+lhBYr_#GtytNh&X--E#V{H6+m(pFoxf$u{WV!yu1G}cyU>hC_TkWc{+c@XU~?Y`M<6Eu96HXDmzgjm6~dvv znh`042;n)ncGm3=r_^E-rh9@Z%~$p4ZMyS=Tcv^s#KzLErtY%fqf(qmUClcWO6c9) z{Ix51=WYq9a20xuX{I->8xELJMDh&ZU&LRtOeCddM4s_29Na3orG@X880(&kO)+&E z`e$OKzD?)m4Sz@flJGkMDw@o%#AT=Uui{Q^bvFG@Ch^s-vf2$bC9F_=^)n3h4}_lN zI5WTL={OmzIOQ&%#027f!>WJQ9AoI?7tV!ewlZM-uxyyFVlh_~zEjGC)B2hGI_UPW z)kqy4zGD-CRxbfPVf7FuNoGj;?ZzfF+3s;u6X9jgV-`#OHp>K{@72%GVi@&EV?iln z5>8=-$#1bM+Q9VNxK$FYd<3TH98-@(ur19Qk6&;s$(N1hMO{rs@mm+ECHw}jozHe) zO{~OhNON|y3G?j;yIzb|k#U0j+{JyXwSR3qAcD!kE2jb=p?_6|HAO(RW;~=it4w_h z(ohwZ&Tw{^b6+fpcJSC1T1m@v5f2$W3HGg^S_Y=i_k^a(!`hTN=o#v#)McK~1fI;^ znpf!?&~T!>bV9M%>qT6d*A~|KdFtgFJWyEZsaHI-Y5GP&$2Qp@W9jrVWtE|9sjmK! zY;M9Yd3hVd9#mDJzZxUrI$5waYvVE5_||vKvG!HOClu}y&<%h(N1Ub6YM-1xELBbkH4Of zfK_vWtv`6`ze!nE7EvkI0+l?%Twt0L=5unD>_{)I%D z4Hw!hz<4{vd_f%$sXTRcpQ5`VX4hvzkZ2^pJg@uNCUsI?f(5{b@NWs7L=LHN@AG+Z?SHF>$3uV;SHX zh{yQZ#ZQI0hD3~g?0lBKna*iYJ z5?OTISPDCdBun$*#@HGm4a8A*=Gzk>J**vuk!ywI%E^Sjc0|_S1Q+=a8eXm(1-i2i z6|YS7UwkM{j*lt+Y4I=A$mvnF5~r%_KRrq6ddjIMc8*!S7sz9pO40wAMkbusPTg!F2%05*_0sr$~0=Cp0SwFwA)1QWCu2^bAhJ-r&BRx-Y zimhKnr?FDCSLdl9c6_M_%kaL+GBx_|^sv^i7uji_s`)E1(a>h=C~OwEEJl18$R1x> ziYGppQ_i5hGD{pV{din|L)xqlk=#BAgUHAwRa1x1D6!}oOI&gAzV$-gS=NDe!!PXF z$8kc=EpnIj(oCA;$CF+<6+4}Z^)8QP;T3|{n{U&{ApZyY<6#ccX#TAAJ8>$8zM2^p zCeIwwnBF*~VR&)l@P_op=EmX0^&NOeoL<@|QjuO!h)gW={xYkjOq^J_O@BB3AxD|t z)>)mBx4I7Poc%Q!mI2qa3m3BXYzeb5Rztal85XqkOH7*txu!Uf+U6F3>PhHhA0t&)Sen^shnP#W1M6z&bCy-e%OTl$BZeS`KXLurOw>Mk>~0|it4o?2&Qwt$_Udyn+9 zixNC=_Q&`WE|5e!zjpMM<%04MDSdUJh4OisUclMd?t0Z` zQ4PpZ!{#%kTVgk(s==C%LO}ekOvHioWZkRMm3htR7#%{#D6d&EiY!Q3SjJH3kMy_x z+An=73lI!#X@9cheVPv<6H=>pCVe8Bfxa zG2sq8cwiDoE15@o(Gw8A(fNX4HI=ojo`Z%K)MaBdR+o*O2ZV`<>dz=ASkvn)x++iy zd?#JGUt8O?M#qz0yt8z2{S<7baEIp(kxzsdALLSy<26mlcC4R|A>Blv_A@-&OA$ODwC-M%LFp@k zP3${O!QO(^sm>O0fpc@8t05gLHqT@2uI8uqKvv<>z8#4kDui!I?G=7 zYJ`39Mt9)w3L`CWxEO-qm|7Bh4_{_RwgeaWGXgC$8ggPojx{&9W6onmab439jA-rD z3U!$KS5~D{kbYV14u2{zM~uc37bJRlC6&ye66)7otkgFq%cPn3N}lgPdDZxr)_OL$ z3R_kUfi&bXuCZa7Mzp5(bdG$b=XLWKqf_|Gx=Bwfx|*2HtHzcu1Zp>bzr*30+K^r> zs^87#D@^W8K%!6DkOqBOH3s#h>xebXjzy&60kKY&SVLFk5Njy0{{F6&mlX^3ZpoNx zV2nCoc>rT8*x;Jolj%HoRoqFM;sWUaxU z&ie$3j|v{DY+kGX5)d1rQCxV7OaW>wG`Mv|n=5CkV-W3THDX^gp(3(0MP0_G!M0Sc zQq6drzJv0ZA6kwVAn*%yoYNC=-fUzeK-DNo?HQ;IE+JO&+fM6wOUn1B71U*|gMws1 zI$=1Ez#LcBP9^{4de{eTPzk#xORZ@z{T10Ww0CL6VA}hqw}m=dw6_3m53sIpat{E& z<4v~PEU!An3ct+?FGqo)u+so?U#P9wqKPrIUC6Lm+Ub0803mj^m8py_DM84%roY@d zW1Rv0MVseOY5{u(ACurBi!L8Tc?VojpKeht`}X?dyNl?6RMw7OhCb22o#g{vw%CA; zR98Zc!_(F8IQ*Dx(fb)?v@S5h`Wx-+%x}8P-aC+&2M-J3?1-i2wF}-H(`aF(DtfEG z8R?E#B^rPwg`Kz<2=?}^n^Ne@t~&$%YAEI+i1H6N%tLcd?=$!U&Ng3^UFOSrm+=Cs zWPi~+kQhD5*A*P+I58%B;)~qGed=uQ)!mku?Xp?)E;p71dws?g!QK^|x%|0$fh6`U zYlFl5AlQ42aZ<2%wqZq~DlDT!EI|;8P1R`!e>efvqS%<@eXx$X15^sEgagLSveBC$ zQ7SkhI34^+apWHYIaLo1LI7i^b((%`PH)_1;)Fe;V}zsL0PD?2i4KK-d*O?#Pf#WOiM~&w*u~KDf+aY{ zk|Bqo7e=xI;=~M%O@mjFF#fq~QoNk@R%VI5n)>h^iI9t4m3LsNtRN2?uNA~z!Ooi0 z2@&#@=yZ`XkiXr9qGFtXsM4Gj5L|JbH)e1>x!gQa-2O->fv1M6j#s!TAbgSBo$kt! zBO1yt1kB-^zqbrK&M!zTdUhH|Bt*;=&|%rDZySN*5)kb;CA~g|bd-QvnH1uG#3mlB z?${xT@U%pTb!&RNP}Wz4dVtd-TP%uQRhw?dtkMldwINc)O&Ugc=W?*#1eC7&Cl`8_ z0cz}Bu39ja9vxol+JrllI~fS{g@awk0OADf9n@d!uG>?2CI%X3KNq`R|Jf>U13lA} zi_%e+I$1v#6?Ti>QkDw9F^X^jW%sk?5tB_Z#KcLS3^Tsk%!?u=DdFMff}FfwpygeK zN7tWNStdzyPLbgf1=v_=usxSjrNy_Qro*yFz3s55w>6{LX1;{2KDgUXmp0drrEPIP z?C~Bda^)J?pemFvRBdpU6L&Cpa9YM>N^LB4vqO&-5rUj-q`05wCWqG0)>L>BM zWPvAWC*+4_13#P|yv^+}CdDsxt<40NIW;a=BRGHA)`VzSQ2ew}CqCNJTjJ^V<+Rxy z8_C+>;Ea;4+ALG)A|N2;FtOq92|cuhhvsAzac&aMu@98vucJ=@&zY5y9=j-bI(r=( z(^NS>Iz-d=1@#rO!A75){KJD6pM^`j7aqS@Es@M6-#-U0W>?~(Dx8tnj!7JJxJ-vx z)Mw9Q9pVNXXm+o;a8Mx8R`kc)cpw0a*nd#a7UZ0t^PA+Bh*c8NP>12oyCI(8t^4I~ zNI?I>p>F+GAnn2-rT+>*VpszehC@4fR&%FtZG0`#ig4&Y-q{B7)FNJOLa~YST41Xn z`RRKTLG2ShT;}l?a7m$Ube2}AA7wMco{__Rifjw-v z|9$?}zl`P*xI1$&!q(`F1)C+J=dRlbj9k(+bIBTyXWI_Xwm=tW!C;q14yB%L_05hs z2-P#1dtF>(+(nAQX3w3uq`@C|{&2T97?Ey^hc}DxI=oo`9>be;e{R-2xmow{X6eaj z6hR1OU}b205yeFC|CQUATvB3;;>{C!p3lLE#CJ!xC7)f<-zJ|^qCe$%qmCnMH2R(7 zr_`v)bKuBXs1CQetU)$6^o6%dT`>8L+?0&!>d^?B(JKfsHJss%O+!7~o(FDxcUZqi zFXmHtv$QOFft~GXgR_-ylIykbZs5pGMo!?! z8c*mo!maiQynM7xq7~lcA3etoWQ(wdEBY9ptlsfPA5K2!M(^Xf(K&OP|D?dzQ!eo5 z1Y1(=wDgdjb|rCU=Ia>eTQMG$ZjNtYu=HezE|3`4dFqSh_Xbb>Me=)#r+%9J`aSjM z$nRQD{b};M#8ZC~zv0a?A5PI@8?n$*Jx}g{6ED(qwJ{{~t_u8t$Zj4YdcLU&WaW*{ zIaaatocqL|)1Vj^5F@&c(}hfx-s?Pf3xqL#vgCZ&Tkg6083x{%A5^n(Sy5t4;-9;u z#1r~u8eO*|w=L8_VWe$wEvT-XST3$X~+TN z8yJ5pqF36ksfeCnU6kjkKjsp^2!G}yWr!-nAqh0~-!@G}q(aeM#0e^T?^KRK)O$l- zv!`BKC-sl<(bTyfryzqf<2-CM1R5j~I!NZ+V#*RPQ_J5WQ_UHl+@{Hv6croUIIpik zM{LvaB+yD|)h)VCiW7Bkt0nC6g_t?O$zEVJQl_J4J9p*M5+nP;yd5v~4rt03NN$aM z^lTm6T z1qgtAUjYCdw{w~70^w~Gq=GKmY2BnHT zB;%ohYr+pnZ-qk-^S3~gkGiZjy|1K`X{^>iB5>q-|Fu?-9;eCz{ds-5wf$_|D%9e~ zL|#A3)ER7SttMiH`s?uCc%!D|&|x9#jF>&oEOceFGd9%9SdvrS3&kvs8izvxqJwu8 z4%PEuuhh6E*etQQF2JJlf1(s|_;b;fU?y4%|9P80V=3AwMV`Ht|4IX#u zrNt3OBo--A8K0vRa1Ag1yAmIDm@-~fKO#;WPn8+3Ed+v$k9~8UYof4 zrxoZ>Y}i*`2d zj;_KQ`Z$cUMW~Zf^ah019WD3#G0pAW<#c=d_;-cN?fohLp5R{(|4vVLd-VY`cpz6U zY%6OsFh_~h?f>wX%fv_p{IAoJ?D}E3)omA{h#1xw`#I}^83foU%6DCywIaAVz;TUi z_=tutVRESlq}e;Y@zVWf_gcVI`ZK!1=%D=O`n|X?7KK7 z$w3IZhg%V^oc{s&AB!V^QEEhgwB=T`*o|7q!o_J}?;3=aSl=N0q7Q(J&?6y3h)%zQ+x+o61LQth@ zz&Jf?$aR1y#pHW|+!MMv0v_F#ie*f9IS1xpTw9m)K-saGX}ZBXLC??+mF0+O*`PzaUHKfTdr@4s za4fwSM=sROoxY@*tp3JRI46d)rrXBb-QZO(5omYAWUC=sA=n;LZUyL#4j(lPt2TY|wcMcD5j5}bTBcb*QA1O)zAg9~iJR-i3Sf@P;l z490;ov~3--lmh+D7#_b+Vp?DZh>0rx0Z}W>e&(kns^DJk?jModQy=Cpkd&UgbM$LE ze``~($a~&Xe}IUWdg|FQ9KmRsr+%mP{{6&zP~MFN!RQcANSs-=`8u5hVIx0bF1Ej= zSo$EoLV0>$b=STW%;oo0tA+d0_f>btcgqD`=w-@MSDupS_2>@XNuBdqsVM|@-(h7E zy+@}=6e|g1canqG%0Y^jD(Lbld|!I>ViJIBWXFkCGNF0Sl7uf>;qn~4*b0|+MD?VP zjx#DRHA*fu3NAHrgV7U>6M|6$4C1++^e##N4IcUsZNx%6^!EUDI4SBMkl(fXxswia zM0YcZ_F(bn-Io*X(0!L^%RzFAZsGIB<{X(FhqF9){)`y$Ns;?UxuM}!rN766v^I7x z54!zf9u7z~0zB-OTnwx5z49y?u!O%V&sSN`yCt$@xJw>@W82%#gEf49@7~hheWM5U zq8`ZZg$x9Zcj%$?XR7;7mSH@FkbS=HeJ3G;yXS*!xAzGDoYXgqf1~(!3jcmW*c5(4 z{I26)0neB4?=t@B#l}Xe{S2f{LVWe(?=y+9oWZMBJt{i-HDUT7V(D$L3urW7xQO$j zdhtyuABM{FROxWDb4&LIM4{~q7h;OGqOje+SY5jhNdl8^J^BWz>&J=Xx``wx*HQ46 zSMa)Kk75Q_Wg8Qj#a2j^y=iBBd6<_Q05NJKfIMq6Yu&NgYAH(uG{Jqo-YO~l^{D8c z%>CMM@|dOb=&h=XoqDJx*Z)uAlz%)6uewc_Iw2{D7t&dwn(0hjt-fT{*jT5YVDz-) zS)2KK)Fb*`q|aBQ<`Pe9z&6lE3G^>0uf*Bcbr1>UqR+}n#GhNPK5&S1Jp&Q-$Vf5b zVw6n7=}W}hP_YXT-iyQQ-QkP0yC~Pj1zD*hM8|wt(Ul^(OKb^3mWh%#)*Xq7n=dy5JSK+K&CPUpKv*(d^CPcx6#|@w9dK_?`py4S~KST#a0~P_} zx<44tUU!C$Mhp#EHL9D73LO%^;BmD&-V?9VZ{Z1#&x<{hFug(8hJphAFEd4}Dn(uI zr3RKryPpxXxD!|tAAZr*LlcrM`ru66qS#s}j^2|yjInHeqttEvasFt>DNphx?eHaH z70l%$d`WhaFX5I&^Cjx0sBC8Vd0^X|AWX@}`NeL?FZeiO zvuqomi`abZYOKdpIl1@-!!>WW z;hO0(%@3;Z+IVW=sdYvdt_HzTcqzYQMwc-}B~7f!(ry z92;gCCffZs6I_^RMhpD*&;@@*RJ3)v{yUYwsO)I#tzRQ60?htol{!w}HLJ17Njp>$I&JQGi4(Q| zMD-K04dyyjxIg=SoI|1~lDGKO%QZZL?93Ir`r-4M%vPuc%o*VY>K|l3G@Xa$X1D^L z`nz~h_xAS0&7zyYKr2DylS`>w>6e+#_yrl}2k}u?O~W9lgS&(JtBDPoO4f8;HBY_x zuZoY_&1PXfq`O2XhC2$KEuG@?dhVErpe`IL;R$-^>o-IIa|jr1L&|!tz%x&5MzcJkuvjMaB?@? zR8-jRx#LkXCT?gh;n>d{`-8My<1W|V3okP#t}7B74}iB7sZ6~-AkJ5aBYJGLX_3-{ z3xO|*NJe-g+AMuyr;d-3t5|Uwjind~YLU%KYs)I6j%G+2CEooq;-hNl80qn$L^E~K zW<+L-vpF1+xl^!N5_;}B1Tz=hD(!c8>i@{9OXevMD*c(Z`VI~ol<^Zfp=6Tcx$ALW z;vvaDu5*tD$!TT^e(F;Fr+A*qM_uC>J>ASDLr_;HHN08c7mi5DcEm5mb(orrq`qoJ zq3%gf=u#<1XPRo)(>Sws*b>ZKBm>op#!P?Y3A!HAKC3qHTm`y3&+Uid)1z*_<4a+r zRm+A&o(s(3;klma(f656`5iJfr_0n7jkLg4nKMWer5sOalTbE2(+WzNTOSd_d?Ei( zo}LJiPzjL=OTD+A);My`?C?V}Xn`X`!A;V=(&`viRNUY#D&BZ3KGNY>^=Gr&N|M(t z{e1?p8AF}P@rQAa6sO~*=)Ndy_1q=;Z9qP2f&i|A=XNnPh>F16sye1PGN1!0UBb8E zqp~2VyH#_UvQqb@Qu#Tar$##8biQcSKS=$@rEHc_tsQpdig1ppo8@dx*(_ODe^-RP z&en}vCE%E}Y|DS!vKv@8A8*+;5;m}9^Q2{BgJ)Xg&syU$4t@!Y$TYMYHlSH3j_76; zn)mC;{?rL6de%E%l%@^08uh8o*#tpb1;k}VMa|-2+=4{9V=Tx>q=anqn)Ss-nb}-c zQ4{@0D3M;7orP~=3TUq)J*qULwJcJ9p(H-u&>C6J%@VQj%?&TavMet&*3>5CDpNPq z8e(hR>VeLrJ7O187fFduh&R4j$dPvIX)S#8u=Q*SJ}R>pDky7WaQ;EPhdBH<1-I%r z#+2H$r3HEkf-m}uk+ORTucUsZRRmw42PQCmoaS05 zsal}Us1NY=U}_doN8d&BL*Q({Gt@P})k{>#-@5K?Gmjs3AP>bw0p|A4!c*Qt-7WvPPe``XrX|v*6Z+dUQeB?RO4{Ktwq_p=b_v3Z{d zk7%5&+Tr^xMa&b)4vi_4@KCwjrGAtA;IcmCM=l6$q8to^qKKi2A6>K-M`8$AaB--1v1DfwJezL;kgXI4rp{r%`9)q^wE?E&ftXws|dK6y{k_(|c!W0zX5A8S&G74~(iTX==D5Bv5O zRKMz}-#}J~VsR?UgBT5dMF(_w>X++>PEY++Jea;$!?>120lUHFF490E>;47QM;Yc_ ztOlDcI-i)qI+f4T3sIA z-SY|$um6Bstm#YQ{?}Rb<>lm@CI>3++4&py?+yE!VDr>A?jW=|si$5>$1LqKeS0PL zc7z#@%8kt%`B_+5WT#u5U9Og5O=F*C?v^CJPMy0%tSlq7j!ca)VJ%nZ^+*1)j=bNj z>xwiSBXa)B!MfckE5akp#hu~Oosqi(1U4+&&~U|uhOyz&KSUZOAY9tsFn0Z7X>wVl z^nH$~jU#5fV$AhCc`2=aIb7OR`22=5B6pQYyJAkWbXVNh^^Ak0vm;iweJl?Vny1_dls^Wg`uud4LMpM!KSQL*!1~O_gdDue6Pj1_>d;)e>??6P2R* zST0@$>9X-eip)E95RYTU^La)WK)Gf$SraItLkLQ z9<#kM`nOAY^lzui;5YvbN}CgV!Dff0p_LS{#(lasLxV#l)0%#DYOkm=Tq zP_D!$^3cQ9{9xtwz{z)HQ2X6Cyd3Ddt=IW{Bs)}0zFPwMZ>JQtuRA%f{Stn1^4hmJ zLhq!=bJ`_&?IIKHlYTdw@df$d_Q1jvM^AkF;}Gm{@q`3^MceZP%NNL;m6KWPu z$bb2gf_bjGOtbTnf(hSQcZ5oRF$eYPXtBe#rWIP^v-}CU84$q=$(F2sa;Di`3EDCq$nWAd_%4es{WKDaC<{ za*ZSSDZj=zd*&+R)R}9*^}Aw-R9o`}zeWbdB~dYy;n@_nkyvK1v@az#GOXz0gwhhlXis1P0+@`NDwl!&mu5Zx?#s;(kE%Vr*$~3-IV$2Du9AipQ z4aL2ya$%1|4h6nmPe@3HjldUt!5eMiHS?_~M{viXmLgB+S3Cq?DAHQpZN6@q3DF1j zN6u0A@PN{3@ZDoT3$&&gjd52Ux_hQDutF}x+j!M zjy)}oCN_Fs=IgO}8d9ZjCnkT7K86>g`8}s8L{yNp}R597Lmq= zI`OZLjX-869VKFoF2*%j*X{7sOH~tNQ0uIyxoaL^t-qu3vYeXy>#(<*Xk-vnf^;Szi>h+VC>X034AoBl0iSo=9dw!7}AtcvPB@gUSyc%cZFJrU~Y z&~o93>+ymd<4RE-7MENlKbIaBz@Lx69a?ijq!`?zwMWQ71SxXnNXV8e)TA%L1zXbP zgnSwo2I}iNxH$GrvzL45*tNCO)pb$XJcfxlJM4ObJz+u3!yY@Uq~Ll#dqk*7XAfp3 z!t1A5@JZn{Zb%an?n&@3I+{82aY_g;lZ)}u8?Vojs^cCmyv!XwIlOE_@N#eRaFR*Q z%{KMQ!5&!nIuW`XhsoVq`oRN51K2(Yn9zd!rj4!lar(i%Njs3$Vc$+Tn&7cw&s|;? z?Z#vCUGPkIyLgVwLE@U}xwDr+1!A95l%+|?JgTQTRCBgQ-BFx!aYp;9o%ATXRmKzzl)aK=(+n=o)E~{`GiYf9h_BF&tbCiB(nOGesz<_dae^*Rn>@31c+Om zjoyX~d1wszi7yv5FAFemlGi$(z%g4@(71^*IsvJt)u2gG$0r zh`_$j5_tW=N@siR%#p8S0PqbvG-pugS_v(kfa#g9i4V@+k*~9@IC*_lBTBcA#2Lha zl3-nKibf$tiKRN=8oB2u%gjW!y z%P2{EFg?dp-zEiJ=;j10b~;x8lu;Ic4O^L!wzpsP4|o*Yk_SB#)JJg42GB6pIlthG z)h52*MLrx?RLYm>Tn#?rKT|d67_t$^_DbTTYfex`GL>rgK;7iXEO>+Em;+`}blWO= zSS%pQ)nE3r2Gty$;8e-vJ5bG01$e9oEoILxS5HcWCA)Qn2=MZ5(sNNq%0^eP1RN6z)92X0Go7{fMs{X@13g{XhopSHcI$l+-2 z%IMiWnGnY?VK!^esmSbAE1}o6XX1G^J2gT7KEzU^W8~NTK?j(LfKzLJuLHi82pCcG zm=3tX4gg!=Peo>1vvLNzrf2rb6L8EKnZVNu+fkt&g8p-xm$vE#c!jHw6`$*0)>xck zKQ3x4PUW$5=DEhDkz!|~`}~&y6u2mG_$mPqI9CG*%y{1a{&cT#W)XiTabSDi7dTDa;+ym3Kt?MN=c$GZ#-}l#Bl&&)l~Hjv4XU;Zo?0W#vlq!6I40b zVRT-=kIf)OhB32uK?|YnT{RDYYhIy#!=9lBVzlx|`Ke1Wmduf)Q=^Xv*<=Fv$2D@# zWTZ`BnriI9bf-)`0-sqGR9DB+)E&%J7J#KX7zu5n_%f~qM+I0NVXbQ$#7AU^duT_^ zeNtrjp`Q>(O-mGfr>&8zeW6D}Vg-nC*RS}&$CZgd4@ ztfLGL*Q>86(0O75wpr)>%G*rw+Bnq$ekrBq50LmaT{Bo5Y!F2IF|2Ja8Xgi2T-8i- zOs#5+N^y7||K)_|OIvtxFG+)2B^*Wl5HG-(>3T0^6zP ztU#+VVl2LFLd+iteV@Rxh}N^`NNCU>!c+W-r56b)a^{I;Y^F-`h12#Y&8(=1qvLO4dZa4XBClGiq|?|5lUi zL``xE9(v5K$>YbViF_MSlV=(FPwSH(^6h`@L=}uqpevsMsRZJ%qBt2|YfqqbNg6w|#pJSD-W=5HyA^ zXI_igmN#*?29)od661^#BUk+e{y3Zy93EHC@MImyJ)G2-%&4>=A37KtPu<+c5Xsg- zwg<$?sl!nOi234!up~I9%WF2iz`wKjcOw75=PdF+TCn(_+^qTz#7;un0RrWSjpBh7 z;)sbxDsu{n67KV;-7{p2E#uR&Kr6S)epJ}JA>TLyW_eO!n=xjV2$76Mv#{TF_@}h_ zl+I-l4q>OiWMMSlUe-*TzyhBZWij0eo%KL56ck zUH7J3V~F+L9n0JV3a<6yRXeeb1w1934hwRMx{~??a~5zTIWo4cJ!3Ms6$3(t}$_-!!ou1*klREXA0-aXEV0 z@eMjTsYv_DgkLbHSqcFztqowgg^u7VIgkr*oHdo7lhcBFkiVb}`g)_w>Ge3ew4;MCp+t8h>l=tyapp9+-M$G-D9GB9ZYzQQBJw0d~3*R?ti#?2GDk|57E-?98SCN?MWfQ3LpY@xwM+|7 z=rL-xv2OqS&u(}4mq@F%cXGRef8--kJ$Kx|yr+_$J4N%;F8zc=>7M#85^B}pqo3(2 zP253Ap>L2p>ZUk^`jfddb>K)8f-{c#{9aB$>f48G3W7iO=rS5MOB(4PUkNJkH1*a{ z$xGL{g>6B1|3$i;&skq$-=$lm=C#f%xfX6Ki_D#+Tbuf><{*Uss&62vv$ycA4f~)J zOzU>>b&{oxcP)n}k+~_by62Z33%tEys{7Moh+Vd~IqGeCJ0tMUhM~^~>OOKbxc#SS zVm@9{S*F2u)_S?5xB~9nL4;(zCM0W8q_!h@(IP2i`u?249;>b+m5(y4E$O>*LEIXvVk_=%jqzqd z-2|@Iec6*YFfBF4sPVjROuve{tQe5Qf5WW=Y%AMAqA{bSG3*X)01C6gzxoap$_E}U zr=V`XafUQX&jb41nppN!V1DV=OjGZn!)!~579y7A%p*tHLF)12Wub``1J2W|Ct>O9!^}G2*B@T7$pRc zIW?U!#wTMpg6J%GEM*;@%f_wGRbSJYuvIwJRr*nOI`ueesWK@S)n=%DeEi%)ANZwqqGu&T8`pFpUOL}KGnxo25@4T=TI?Rm1j0e@`10}a(59h#eRq6=eF%ijUW z-2f6Ge7KUq#BV}rZ*A&A^o2_M7FrjbfV=&y%Hxo4fF@6B|vg zhvw#-=rx9Vo~+L~k+A@Ppyjyk6oj<6)n$xO5;%`b2GLu^HAJ`Sky|VO!TdK0mb(2X zSnF8*fg6CTs%SlpL6IGVmR$8=cXIX5JJvG@i3O9`-B^|~V_}Ms9VtthKF%0s?ufZ&jKBzSdXE29{$=B_^w8X?#7>({ zCi2Js)G`9K9((hrqOEAT!kK!h^gBtK*+2LDaVL^u*WF=mBDLgIi(P+1*KIu3NJBk$ z|Cr~5*wsQE7qP3Ljsv`fur;WkMC>YiH#!Z7U2l;z|KG)~XON9qy<3Z21(lb~5xdG3 z|G$e}|5hPl*Y+~Cc^_jh=b0yIrWVBRPK5kKhaiZJB|-#kZlKuJEPa*33X;u);MJ_o zMr~n$_|<&vhL=S$ix}3~3^?EJ8(+?GzG9a0I}?YF*w-%3aUQubKGRvZ^HLFyM*ffy z*tmFIxD=~7ZuAqv(J{fgc87tn7{bvfs5@0QoK8Fww#CLJq@q`Im{K+GC8pxQqhGgc zz|qgE*JZaF%N>D8@-z}>yNvQvuGy0v0k|;n1DPRkZ`UNjH?5`e^uVQ3fVORy zC1KOpWay@Wp_($Q`#%|U`=B5k9k|skpA5QcP>@SvUHZwOX#<05Xo#*|?r}qh4h*e@ z>vyZ3Z3CKN1??XcM9;a^OP>tdHZZ6r!|s($#|^!AP-vwccinMAR}2c3ivg)X*>OW> z3=FN!m58T*GAMgs&=%>T6$pRFH{sgWqT9*Z|6{9k2GZdnK>E3t8L0wwTJ-T)J3lj4 zVt)MKKZ>>Xvto%T6G0!)T4IU;c-RJLv#mMCzE+B;+cITk_j-WcOH0(ntW8dnVq?OB z0UP`lQdP-DIR4WPpdZObtF1(~qI;xP_ng!&~&6W9l1#7q3_1;?dgd}k1!eZb-btkeI7Rzfp8(esYa#+8l?|C zcn(Lwz{0BnGW(J5GfQG=06~S*zG5qHaWCs3>w&EO-l9=9$LIwwq#`#l$aY#W2RxV*l{KI78Sj|R`Bk>Ju z7NG7>_==}KolL|rQ|KewC(Ol#jdQ0?N^RQR+u%Ef1iqx!Rb`AV@een5^z0TGvfw+% z!i&9Jvx=s~*Q`*a4wqXcU8qaa6Cup;DK^dA@sbLcdI7$~XCO4st46(`8D5g38*`F! z>J-Qr94b>kx@B}}p4bzJPXlYS7=on;z$jhi1D<%ogKojhABN zG1OS4)gNz)mwE$r#~clrrUyq{wi-g~+~H|MjWW~IIDhzLiR{ynF9zAvQ)H!(s0_J- zsFQ~Ra3E1nlc?Nyp;nolh)OgO))DQrp`0j6mFO>*nHlj5(&MALMKr`5T}u>9kkI&O zxS2-@yCt(>Wr3WK((_M!gh*IQ&nYyNW>a!^s44R>V!lC}OfZG*w26FM4*v%2u z*%Geyid)A{Pf#v5gM zv9!h%Dvu}uiDlg}?sE@%Lc3{&^q_DNmo;W0woIQBy@MCF3{hnCAQ#y%lEPOFzs&Pw zW>Mhj+)KtVKB^I<^XEO=!tx5}#xS*sBZOs$75mz3q>3(QXROHd9GZI`O`1HXVbOWu zsHUzP(m2xeG%OlEd5*~K5^tx?$?J@r(9`)~Z3==hTK27P>y~je&a3WzNhFb??$OP4 zxurh8mXW6cbY4<|P%38_M61YSx&@FH9QGfz5yYti>u^uVWccZys4%?znqx2&YVq9P z>_qH*Fg$Ve_pgMM_dv1Pg}xhlyttro_;w$c|(Z)k|uGi}ozRcBuDLXUUa%mMl)pk~7%d zdxsjo#kgff*eNrmS7wSl9OhxNnbA5o{j{O6vt;IgPA`!|dg>s0aI5q0*Bxwy53w?# z{)q`RPA1S;*0eF)btqjX(D2mQkQJtbvWUi@K+>Xe<7LfaYyrS!9HV}O=)$I)zu=ME z##-6_*pWk5$iwmjj;MIY!Gctj)x^Z}D{R)MygXpD{wfKDTvsIX5(*`3{vaj(FWS-0 zO-55ej&q;CS#J@Vj0(y4rXmDDPCFZgz{3&*#hxlxw~J=Zan=0qJv?AL|D8^x%^sfR zky;;bJ8yU?v7J9J*(SGh!Fm->uh-~H{*hp8CFDELpdE^Q2fYi{+GubRR*jIOtqj}> zE`!*eV{I%6i(3DL`(HwI+-He<>u@z*ayqpN@cIv8zeTB@9rE*W8@D}19$mK$VCO`sox@wdp3ZS=bY z0%Mq<0SJsw1|CQM!bV^;Ba+FDO+yg8xOWp?>uy%Ar4~RX#$)v&jRWsy04n3YWo|*5 zws9FXW}~GSkm%`MQZ2jhck&$lJL|;8Wh?<(%eWw~RpaVqYoM(Tz7Ekaj{lcY8AEYT z_`gPF3_dF2KSwm5S_#_YabOuG-)uhHT@ZEiY$0ZS0+#Wguvy20W&AVY{YZe(R*80e zSjJ!SDFMs)!{l>r^jkcC9xUT6#QAJk#!`s^ETd0;fn}T}zrZqHD8IlmPLbcWpzFym zz=9|88+=rz3_miqjz~xrUzrAGTp*A*p{C;lGy0L=4+3VqA0Uf?xLK;47}00B;!OZE zKE;k}1NVOhFykh8YYohJ7Zo8z0+_KJUeCfG`+o||coYWu^MDyUh@*iS#qKl#%qWT* z0?a6v(>CDWClMe&1DJ8wJ!F~yW)zAhZU1~=#@nR0LBNbNEnvnAZLl_m_%nbRFOa5K zLmxeZ=RwGfA3`mwq9Y`VATv&51OTJtuSRARzZ1b;kQsj`%n!(nYXzC{ERD?gF5hf) zJ*zJ)8uvMO%|?dgYQ6eu`^BB)^r$Xa-_l=Pp&GU|02*O;oGgk_YhBL5XEqt5QL zU>Oq;J|C7*2#JM;8;;1vZ9_5|5D0-}yi)i_0r1_%h*-13(*8P~d1OzTfsl+B62DPv za(y}^;|Ss;AQ{I<{{I^!<9Hw$$7o2#5j~y3mmQyt$!O1+#N^QH_47d)HQwNV1j@Mc zTpA*vjMl_Rf-*{Bju#clMN;!j38SnpC=AVViC!KjQrDtdfWM%}1SpfdhJ;0ZN` zqm5-5)R>eSl~E{?x^N2zphxA=@kvxhkF{X)4}r*d9o=c;a)R)#0Xj20p$PRk4vYOU zoW?E$xE(D3s4TB^d%wKc?Y)nG-Ta$b;r5pB?;HGU;+xj;E^o9D=NgOMGgl&hW2ps> z^6(K9W9F@fduBO0x-Z9^7-j{I-f9d99F>3$0y<`k-N4U@hfpch!S8Rco3R7+;v0-9 zGjB1@oar|v&s=MqGINP>;*8gEPIUAJB4-~_?QUYf(}VKcA_-LcAOcV z(W{R5S7+359OkNs%m8^HA@~A(5h0oYVznn=fS3e`^!M9U=X8?h zyK2|2$KJJTpQ^o4C<%oUP{@WtRuqy@NUsHD$5PoODm#M84yCf7g!0sa5^@*fz7d8| zWlvge0)&+bVS?>x*$Jt=67v#nCx+rhUIVRas(1D6g&LI(p)s?&dK~6ZdsD zcYihJK=)Vny>1?kE*@B8;ZKHQp(u3}-1^_|3}61!|DSh;IQix^h5PTXYCnih==*4j zQxW=iQ~KhN7WDh8aR$Qo&K>l(<_-G0J%|6@%>UlT|1Rc#3-On|^bzK_e-j>&a}}k$ z1FHp`FW?aog{H7S<5k-)p8JOOi%$T;z4bqa2YtVI6Hblj`^C<;|M&Kb_HX2BI@sjf z?m_Lo=O@$O?avV<{BMMNX!>?)ja>cbVX*hXk2q!V`tQMC{5&aswDzvU+eUaQUWQ-a zb_f4k#{a&GzZ;%^8Gk9BdNxqO6K?cIp0d!(D~B8N^?<|g3#|G6c@zX-n7pGw4sAif zgWyA)cJUMbe%sKrE^I>=_-ZH_+t8ZN!HAx@ljLfA1B4$MPMq6WI>cW?1L7!P%_*Rr zZmv~ajhMJd^UO+hUoKi~a*te+q5*E>JeCEI?2|wWM zyw^|prNGh9vnB8)o@3vk^uULBtm0Wb7>&U8P_D?pKk)21e}!BFab-$>is*MIdO4TD zoBA7gMOY1P{p{DpW5*BD#iDZ;Q|JB>${_FD$MN9ZPJNFKuENW}4mQ{NAA6sM*_)2N z$5)^t$cZk!B0rFZIbg0;s^8xVxPswENMPVWkTw5VI!MTygy;>F82An;2nLKtzN`NL zr?>Wr{DBVv@;ya89;m}3Z|`G$?HwN8UUhcA#!esi-!)dQmNAZDL{d#ei7SJwp^ib(tVG;UyHZERKS!P%GaNCPVX>IFU5&CPjMp7Q=EwNh!b%ZpN7Pc zl4JhB_jvWpMVyF*S|a#Y@N(c2fJ9H>Zfxi|CGakuH^%Td5tTenL?hxv&~!0*LS=uC zJlr5r89{L(kmD7~5v(NHLW~r+pK{#c`ZZS0wOBdVVCDQ3R?fSza^B^8ill+}6`IB1 zWs>5sJ2W`h7hlsV1@(0*qJksU|BeTLH#V>_=ncfW+E7DMO4LA?g>S@mapS?Tyg%TS zU*I%sdK%SOZ39Q~N~}UrBL2)bvk&tuo9wj0Ln|dB4VjFBho`w|PJH&`W zItKm&Wx8vqp=5|~Kh)RH>V5sv5gLMb7xE~ao%=#V2u6}?HUSnsxdNY1B1L^5O9btg z$p4X-#mLnp9>~Hl2}GrJEr8_NPBnBllA0h_laiyc=!gMW<##iBRRlL6CiC|EQhsza z`q5mgiKTfOgQX91k7|rS6{Zc(@mc8mmzaKAfSqo@pfnDGvAer~h3ph^^#SVRKLV`r zp!_cR!T<_LX5_}@mUZ;QPF11>+_Z7O?;R3mIFTh}i2>9jD0J>zNdK38}X>wUI>kY zt!*LO%M2uQ+sipVUBAX(ZhN_ur^EL0C2+&`B3BC|%zdsjZhwgcTzQ_V#6)SVxfKtf z(o}uuK`U-&1B*`p4+J7X9TrM49ATYO)~ojKl%p8=zE0_-59ylhoz5=Dp;4L=JCUc0 zGlwwU5a((EHR#cYfEqLbFr`g_V#sekkA?g}JPo1~)@@Uu1SIwcX$$$y+MK|7CkOU?q6{v_yA&Z zH~2pG!mVo1M zESVssmcU=pD%1{IUAKtZT@UxGsS|4i!hI;N<=?Z6b80*?u$h#C;M z4s>@lq{!8OMTxG43|Fq!%#j0^iJeg8XouXd`i|3j8(lYqCKiMpcV7k}Ob5X=WCo(V zvLB{DhcLNE^lOM~TVCMW{)7vOf|W$gBvR*U*ux|f7PL$IAzu_i?s_Rireb{RC3@UkcsAKh^i^<)a;a^51=F zNaDgN7^)f|W6goj2$Qavi!LH9MXbZF8TR^6=qp|)U!KLHsm3B;3~tZi1O`kU2pB^d z5HMzH`cw3Taw2Fy49 zsMnF3jFyyzhmzrqqEIp!NTC2c1AA>kSi`>Ya@z$rS~^gId0rx;B|XAu$@d-`W5hlI zJA|5R@Fg>?EB&2ifRm|`eqpK{&c|XCULxEDVn^;?n*Po`NLas6d5VO-zlLuvb9xuK z#w~J#Gu&0VB&} z(t?3()M{W;QuuI(?h5b34HvlCuf0REoyNaQ@04tn_;*K%WcwBVjlWB><*t%!qZ#9ME1qbkUi@PvVGz3?d*J(1aG4JWDL%WzKCJP znO^O7Q9feD;37(X0b>AQ+1IZYVKZmK)gtCWWetk^SUy`oNW?i9eyoMAJcvLi&<9D! z(vB3@wMnE1fL)@6?E7?C(O?+5E4aSk4qVGQaYSdsH=jS@C)VivW`fJ_a zaIj`5%X^E?n$c-rc*0sj@$9aAu?!Bbw%vn2IOz)sIV@drPldwoyTRgMqh3UQc=BU5 zdZ1&gdf$-R%I*Tz24$c6K2XQ}7jiV9zI9Ze1PHuQ;v&mRi(J!y%&Sb3@#ob`Otw`& zzs6+C^Dm5r&zJc}i0zV4H7?a5SWe(sWOcCrz{#T`1YV6_f|GQf+0#?p(uU;e_A_gL zSc0njR2AGE0k=E9RAOg$Q0{~9f|&zW&SwF7#|3t@W;(8`!P~)G`?$f3Tx{bO>p}|- zo*~388hN9|S7=d(Iq5(4#NKW=B$)&T!42MHESVnk_VT%~(865c61W$VKt|#}0H+o| zO)VY?01{78;{S^te)k2c4L$rOJzUhoQM@|Q!?dDmlMlz=5UvI{wG*gUF_Zf`HZ=7Z zYvp~*kCtyG64#J2eh5RKz5;{Nf}RG!8ALF3CZ*v(ycnoF@9tPceE5x+f85Oz?hTJF zy+GlZS9#!$k4^ei^l5DXXC6%j?43`52$%cat;+5t_>10;_gk*_%t~=b?S@Jaz(_5{ z_uwL>2M#}O@UMu>V{`dCJifWo(BpaeYzUCRP{{`h_o^gcnZ{o;prAKg-!nYg+BUVl^%Lu6Q(KB)obOe6q>cKD( z^b*KHy2z-U5^y#XNnP1egbNXt-z~72J0#C_bkG*gq7Y`e&XoWq z-FXvx>2+FF(9iV6CABLWZtS~aB-d#i^HfYYhK;uf*D;)@$3vv+5I)I-Jh#xTdy`_C zbJM(et*-HNa+l&KY0fX#+}rBDf_?i824NF4JnT$A#t5;`C}As4sEA{`@fO@bZ{+C~ z61j)Z|JR7)apqg9s?W zo7sJ}l9Nz%HYMu!qg=MCvlHl7sycfqe%+UN*Cm_a#PM|$#z#2c1}ijBrH(xDV8*5V z3N!A$cX;#h5~}3ID0-J#gRC5hqNmwvJZeL*az(k7 z{es@b6vVXER#i|9f-^&-axEW~s|FvHk$hBaEP*QGbB`knIcTQ7^HxaM9?+G)Jlq56 z1s_rb?xLO49si_05W8Kr?+<$G(r3BdtSWJBXw9=`j^c z^WYdHpJZo7GMp(YN&Yg6ngXT{-k+>QJ0PyjI0(<|z|KGBxrf~7-EFBkRC$;@R0Jb} zIKH3V>CNu3*<;pkU2)6CY`R%(*}D5vtBu{-hORDslK9|BbTguYvgc7bbuFu+2i#3( zbMe5kkqMR3try=0%F7xA$Aw+_eiv$qa^Das) z;QcX+`eOpzA;1KeO9ng#`z7jju(L1G{KdQ;x|BL3Sc%1m)*`T@+Fd$zRZb|uA1*S$si_FdhpQP01?*;^P8c zt^*4ey_&!66D`#SE{hM`?9jjsl;x@9{m?zmTYe1g)X~H2t305^5XC`*#7+k`jv}tz zX3*Hog8q*Wj?0gcZb4rI5Na4AbCwgIf0yDx$&p}LA$2Kg1-62Vb zV7C^6+y#g(wjV*g#H5_ChPOhB2TrcEldUAOsX;`?oX)igcoFne!tWzQ09PsHY8o%D zWXjdGc!Y6Lu6`E}Vv5Q&$M8(kRIZ_K-Jz*ET*#O>E@Nlp7KvpSeYUsyRYKzJ@c_^boZJ87qk^NyFI-K&Vm5Z_8lc8maBmd_qR8?fAHp7@h9 zkPg??H2ybQIhMchH3$bgu)A=un^9O(qOiJ=+EaBs%<|yWc63$&JL4x>J$N>`7P4>g z=I3whgwh>Mk8!1G1M0d$>=-5n-Kij|zvGj9R-H?PL!*^za7hbnn&hpt!b2PT0$WUl zg4H5;GhC~HzLWy?A*LCuKM=h3+|D(3!G${3);#~ZPW!d%7uw5uDzg-+uFYc3u_(^Y zngf_w4`F6a^A~k=nKRszKmf!GFwtG2&!tR0tZ`%6fu2$82nOt^UzXO z8j+8sij4(d(8`H!{0;W1>DS{*2dU^OqPBrkFOE;WD0U}uvhlnsu1X*(BkLF{`1#_SJi|I5Cm&!_Ib{(FV@3o-t)gLU1*1Q3%*e&jG3 zAUqloPr`VIDdfYfhA#;|g<56rdz59J4L-l%NW#Cc1BybrB#t`Rmv+j(sRCSyfhNbb z3Wl>*tA)gXj3Ae9?MRsjL7|Y_r%vb{Y&E_B2uU$InA;9nk)GXN*%Cz1!0b*iC^J>= zP=8J{vHTziQi|cL?l9N$^ce|Gv$9KY$_Gt{Pij6Taa*^;+n4|{?b)sJ!w#@DTInfI z#*sS=I{Oe#@{5W*GYVU7fC+SEs!A3&7Gr&f| z^-%akVt4bxLfguQqc<^Mc2N8O$@eNL$}szvuF+NJE`#)P4OeUvzp#J0dPrABN`zeV zdvpkHdzTeMa$y>WbX7~e>mZvrf~s@0JzHH!n?=`6>FMsP zy${;VtEZ+HB|LDs;!IX6)mNR$&jaJ8tbaYHV5c|F85+UsS zf-G|BW62{|&RpujE0MGu-H72)?(Jo<$6$FX2<}_CLTM>R+Hys$I!hmp-BItM<4jKW z9j<;B7v0iSZ-t4$dmC&_CI@(IcZOurw)b`DI+3Wv!jU{dit8nlk_khDJ8Ekoa*LdF zgR?+6>zYPG!%qJbg1-!N@GmgtkZ(}3OAG@9OGp`D;Dy}_rHBy#A1p=tX zlKt`|CKq#BQihekpgW#ZVzm3uTx*-h5=zcij2f^`1co%UTh%ifIUr;I)?RWy+<1cSL z^9mMWO!<}AeXYQ>vUZsy7cAJFri^EJ2u56!hb5Og;d>7LRGMhMa+l}ADO;(9bO)^V zGhbaj&lXJFBs_-2Ayi2uKOx=(MOE|4=HwTX6WE6^A!sL?Q=q&OoFuaMLhA!E`#T3( z!BMcDR$|v1u^bxn#WLi4~UnMRl5*>oV z0zKzI0-=|yBbgm_Djm9eGqQsq<_M-W-6;>nVCC|`<~`QY+=Q^q6rh=Vf^BLjk%!Mu zwcziZjH;nF{3bg1@3NlLvaPj5UbAm%iTr-8sYK2{hldZ%lY%4xYa=FIfcQ8q7tOBO zjKM&`srL3-A9a6j&Hm5s=@Y!XoyUUhQKzGalRXexi8stae-v=$OM`ID#} zr{%&9l>QL-J_Wu{#{*vj@C}a=wcOf<=Q*1=ZZwBC3py-E*>M}kolrv$g!z5bf6&9S z_fN~WGGIOjtOt>`4*x#Fzr#r1hu?3Jz8lZ`k*6E@7pGcLZJilOpW@$cB#ou&k@y8A zeun(Vk+%`gz_iCl1C+ifUWBJVnYifyV!bSI$HfUofHB!(~1 z_8@P1+i7`P1JX_*Ek1>(6}FeiWyeo~+LPNVaRhD3zNjD$1jOx|MnB$jCi(F;P>X0k zt(D$3PgR`A1LvS~ih&G7(^yx*6|Ma}qq zAKi=@Q`CW+t@y=r;RXC+;unD}%DSmKpeR%6;SByArBc|W;s@hfxC{S~aRm6M1^=J{ z5I6DB9zOOqPE5rfv>Q||`v9cuPbrZrnurrRDrLm4z}VG!d!SP!L%F4{^OL_u+?T5- z#zBEIGcb=oeeVorBsc}*xMqJsiF^*YemGPjpTzGO;C=uz`^$P|Z`F|?idCHxbLJ(W z;`}lvh>GNz$G~_Y>d-y!b%wAmix+GyCS&WgdK zPpFU?gAJ%ngcX|2Sh)| z!I?S!Hwhpd8gal%)$r90HJ$1hNa$&nn49`b=4`L+8MVW^8+!vMMGTR z+V^*vHIY6Sv2icq{WTzMjTvbzQJblo(On17UG==X)L#<8eYEn~sO*WuY#Sa$6K&gQ zt(xm7JQ>*KMtlU7GzzN{Swxpk4OilU{Pcc!;JnM2{>D%Gld=B=j($y=XU z$(NAI-gzh(grKuw$BFa*ri#g6Vr+@LvoW#r$%%MnRhdwU=uBTG(ZA~#_78V4z(z1djr3Xt2mXHMe{lC71Kuny)SaxGsy6*sHD7gO2UQh_7 zq7a(FvJdbJxmM5x*fGG4U~$B1WusQ?0sKSI*KJ;jR7$DGzXtCGQ{|EsL@fS&f)r9n zu&NQh_iM$p+eR+(g*s7f16OSug?G+lk$AgkZU7s{LKy!WOuMmh@54kN6feR=lk^~G zU%oKcqV2RK=hyLafoj4)v|*0A5-IZ@!Q)Z;@Ef=jU`nJHkWw31#uLig=3`bI@8c>% zh3MfQa4;Qn)Z|&6Qq?4;(tV_Bc1$xOlgp9!KQjwWtLl52yQrbgoK_!_XEoEX6a>iC zuK(p!U^Zz#PwcKKL*j)YC*1btrsh*>yq0kFiRSOWZem;vvajp9YHmk11b1&{U!Ur# z&u~XJ|1{);E3&yY@L%8}wg4;=hcC5PpCG^OkzJnyS)2Ozz^ecT$hz8cu;t*PX&vYe zQ9)!w^9kfR_H4^`+Ic-ID#&ynXg<}T<)8xRP@rK*lRK}Wxm}H+tQ+E36D z!i>bB%_k9V#vLuF(s76?32*{{Ky$15ZOg$xJj&vSh${>yA;K-|lny|JTXSe*cr(mI z+~4&A4b8R9ZHJ}}eR!zt>*ny5Jq2KIUu_$oq8@E-I@lIUYi?`qAQZg^o1gur^-u26 z&9#T)F9kA6145*^;r&+};RUg(&4S96ugnKwC=E{hzfetc zlc=ZpTdjH|{Eb)q?Y^J(y}$8&RJ^gZ9~JwK?K?JP*TuB3I&>PglLs3ZeSJ~8Xi#Yl zqaW6AX!|eFnnQo1zg;zS`!>TRiKfPA(<hx?Uel=CyoygfooZ|-xdj0`bNPqMTzm+C zqseyIi~PB*4jiHCtf%Q>ZOQH_sVbK$BVFd^d!>GRaoUvAR6N`U&TVb(fGntM{;8$8 zfr45?y9Aqi_g0o*<&AE>CpEg^H50B!xuUnyJ&uOMQ67*lX0MV3ode1zWL!t3%iI## zfD~{05L{>sR{G|rM&nf(BEP&RuC6T(?-rjm_0ZH*@2(+0iCL9pX+SJFsUMCAp3qQI zH9M8lJvH^_h7!e+S~3T(<4Uknx)Q-Z{K2{v`%M!IVEwqcp_y_L%Os=%CH6uBtD10e znrn`rcV!6i6@v<6`W#P z(|9=kUr11QwnV)gmhnkFBYN2Mz@HAszfBqcAS^=8mTbxC1mm;)I9XU!^y(At#i69< zose1CQqfm|qHkKecFlk2DgqsjPx(c>&55~yv8Iv_@N7MO*`i7#3(1C*(a(^ClKGH3 z9hJ>(xaZvkZMP$so}J*nE`7Pp{ZKPIxSy(sdI<6p|3___e~B${3@9cWPpbH>t;ep1>JEPM*5o867?#OB}W|dGpD`%Wa$tSy>B4(cTDyYXz~lT9}?|buXvPj+Shj=(x8qO3f#Wkhw_NBN{PG zZ-V!1gnI(kjj=i^w@gVx30?~-R12X6{+1-Na`s79VZ!3}D+%^1bn_Xx!jj5p^6BO) zY{6))p3KCSe$D6D$iDv(rNr;U5aW03bT9v%)GFR*cJSY2PzUm%=NGn;E@7I~0Ukr{ zO`ApTt+kI*sbw7{+-y_SiC-83xS0S-+GLa}+>NaoT{jOzRGs|^#pOro14~r^dacO} z&F+d*AgZgbZHT$$ZsX6(z7>{Moanl9V+Ay;P zm=lr6|fXV@C{adwBe9fNkRoFGdSnW?pkq-L?-g-vU=+DtYT7(6^`-+hF`% zgY8WL8nL2j8}YB$u`EU?%z--=dbiMaCHlU;?FuU3yG=oS-=1?nD!}M*JB(|SAoS1v zJ5pi3)xLVFZgG71aWZLhR+=h4caH%A^4gDPffmbHSWP^9FfKsIRAC8e?0}llLQ7~~T3TwvowAvDUT`I9}cp4Src1dldw3c*R|x#*NZep;8B;?H~` zYj)JyC{JCpI>ev36VGN(-C21@ow>{J>e#n&NOhC@E65KN+BhW3b2f85{4?KNA$bC~ zd(MtpYtB%a_Km82eV;!RI<_jYdG}&G5#xj(P*-|N*ZH%n9m*q{G1th^_M|9$!;Fdq z3p)yeiM_nx?$`;BybO4Tnn{_1TQ=c^U5@pie_kI~z{X)RQ&*1gK28}ez8$1;`gXjH zzX~r~$0ED2caET2D`_9CU_)TQZ^8#Du___5o#;wL<|Suwt1yHq@z{3bRIk?i{2<8B z?CxGf?Z%fk9R0;L1e~4uf^~z5@)3>mS9IyX-m}v61v<3vutL8cpx80!f{c5p3;dhu zgD~5G(#lwBXs9z5aF^o)dg4=j{DbdtiH^YqehKP$N8sDt1gMA93vvy8Q`UNOfsJ=% zBz0wk=t>}P?c{~7VJB*}p0mN$O2q3#3T*Qh5DA&Z#)k2-wYnD&nQ4cB&^;Z5(p_+z$4g&BxXStqlf3pVQG37DyTDtr~eZ>*NhdUtn`zMQs?5!y+d4bZ-St_+Fu9 zOVhKB%6Fg@bE&_n!-rsKETEoxd|(4;u8-5`r*FM`Ax?26x%*(znA*~aj8$=eksziL zf{Cv}USSGY+!mT<4d~JrkQGIE?lCy}w!p?7(@Vo6eo2xkgGj{3<^?#O4;4&}QAV-= zjJb%E8viQcMbs_A3z}Y|PIkq;Mx*Gmyc*M2&8$A089-(0L-i+t>g(6~q3TB>XI(ma z(6NdRJ@Mlebj;jsty;>3Z~)TVIFhsFm5yB1`a690{Q)NvF}LB8 z@-B%knBGF*e8r5%q4XH5bh95ZPtdGxh7*=SFAm6wBjp998LJLH0UUo7I8dZS8G~C) zLp^p;@LyE$CKMcvg6bj;oKC6+NV{s>Cw3J(G{h_icD7uZkGlepiaste@9T)1A4-`x$NHiJu6 zx%x$5!hxW*Nx{T7_-9$U`f);@O=4g~iLx}Q#3$h=xx@#T;<(eJ6x-0wRmw~fS??u4 zKPh$BA`HNv3ouB?GAg}AfI&u<8KvWN28543aFwCz>5Vq8sKOH@H7FA0XSw*LL^RzWY3axLL>uJM{u7DJp%vHS+NG- z1_4G~udE>k;FkqB4&b;!38_RTCDUrpvg(9b4Nw&cRPjI+ZvehdfQJG+)Brq2fMtN? zK?&)?$6h7VX3w(egxCyF?L#i~2&R43Faz*E1b8^W!wtZ{6JP{c$Qm&yA@_1Z66{$C zIw1)Ls1^!Ti9nTT0G=$sBLN<10FD*lO8~xPP(qFo%VZ|mvyyZ|k_=GoKrZyiD4-f; z0DeJ$!J%2B4Zt1&9s}^0K?%8q6OwGtO4bQUHb6B~pu$PCtg!~*kpesp;Bf}vZfHh$kWAB9oHI>{(3jIGkU+NbdDQ zUqDR(pb8j(TLk!ffWJ2Y?+{=WV0BPJ=!>zE*>2Bj*9mDiK($7oIssHC48Y3-_$0t5 z4Zt%6_y>T07?cnjC#1uk)u9v8VSws;XawkyQ$Tgf0Q`Xfp9c7}0r(98J_GQXK?!*j znUu^-KQgAn3=P|57IXZ5=NL#&Iv$S3P-C|{u61*qXC zp!n%x&}@J%0CeGE&;)r)CgSSd5QJ_Z_tQnhk_-ty#No! zz2gN~=UyW!XDd zbzmCamCPgdtRp(1M+_*55mg-pp~UXtlrXI8=n|df!@*4gth2o_p)Vtol6l;ob)54z z1i}4AD|xxVrLW}SRjm-`xBaP+DhyV?ut5OiJcSd)7&v5Tlj+Zh=Z)$-@a*EWm@Y|1<&C+25EDi4$_#o^@I$ z#Aqcyf(<{~q_5=Rgfs~7VC?_00PE~;Oh_f!6f(Q)SzS6IMk{%dK&7wb;e=c#z=N?r zc}hhyboMtUqzm^pmCSSYtaCacMl1O~ z)}HmP-b+Th`|ARgzVwIp5^fDrO@nbfOxFa~Io_C%g`ALpJu9FSVx+rI5UBLkKb(+A z0UnIw$;BrK(K+6jkR8aRWS+2RozMv}(%qjHsPxr8oR9|vcrcE?S%7tpHzs5zC*+hp z>y%E2k?u||KG7z9^$#bc19!Rr9*pDn3$V`d#)P~<_rfxNv}gUO6Jn&hKO#`+tA98l zcM0%d9G@e=I>#FmGLaK<)}D1%C&Wm1{|T36(PVw~4=3bn0UnIwcM7o1@y3Mwr^ux9 zeRvhW7GQ1Jm#fEtQTwU>foQxx;n*VCqP9u4Z?{Xft=M5Vza!b!zbDyl#ru_bUxxQH zTP544|B`Hfu9a*bybXV7c)tPfWAUDa_qKN>TirhqUSg+Yd+#mDR*3gmc#p&Tbi99z zI^IMb5AOhu|B`IW@jeOfJ*b15!s+9ZO=EmCGe>nJ#Y`HRTKxjzeU8Z1B6r1o2Qag_|eJV0H@>n#~0K8m)VR3895rfe2I~|&GqKJQ_k7X$===2+|7;VPq=;YW$*;-i;KU6G};|_ni$D( z%%JqR6i%3X2v*quFkck~z_qHpRzg#C!~tN}PhfT55$ z&l!Lt1Q-g5^ZX!Wd`%_*N3Y%4>$+FW(cbQ#U<(11mQn-p&eJd3NdisPu=c~tL-qy3R_?v!h0aw|j`_8gpf!?|dr0P74K z4({fAe?unLSN+du=kNH=?)*+CgLpa|(VGGhb{tM4X8w%;55~-80<1H0e+i_Mk;}^Gc7%q$7;V9Xrg`~EVWnT^QUO}l=_4|eAdIvK>*;bd$Qh_Ksm z8ZmQ~01w8@zYt)ZnT^Pp%gH!ncb?J7AifSKW0XJyUUnMs@;Sb#FB^=P4+*f&%SL2y zU&fAZyR(}M4e)gt@pU*IzZICU)o>ay^J)PejG6NUSZ8JVXDqLX>V zfR0~^ieO}L8ZrEQ0UnIuCke35@J3`tB8%cUZg(En$)E*1yuF9{&OWq=hl95Z@L&v& zgI*+{b%r-0UwV8K>>e(>fVO3;9BUNMFdq$(Ss_gE4%p z0P75IM8_$mPg!tdXra+;t$cKX|ED6B6G9M0pM}T!XU_{36kVSC>?9PBrhLJ*l zk3gjB0O9rI3h-cZ;O7FY%K;-Y;y4*6?9LN98Ab~Iw}{<==IS~?I2pSIcrZEe7XjAg zfDswh$f7t-*`25OC5E#+X3pZGb<43#mN4-0Y z6#6fa3l!=~KsXui3b3xwhl8IMV13~?BE!weIBR#F)yXhY=$8sax(*Ot&vXGEOb!ea zU|kLvk#Q2Iu+dIUo0qE-FJkR#+}jQ+^?_d!Q{}?1HS9gGwT;ACLL#T{?4n%L#%%+c zfacRF#BgxE0K*cOPbU$>!7m9gEOAS48d0t$uUq}K2ru5l2%r}A5#YW9A2;>N!$6Aw zLd8oW)tCQ!94P|Fl8C@MY^7m_gfUVaA9*}HFUr4sec>m%f z$@c1g$@bgNCEJTll5GjzQ}8|n@1yYE_@QKbW*=;%pGmgg?U8I5cpr}U^Pj?wiudzd)b)#gv8t)dopFkb)c>gQvC`X-l;+<|p;XYKpc8b;EaFEZ=MW&6BL+FHi zeT4j1OR9Q`j^PG3MkCGY-5nvnWJz6f%D(^yi&OL~08a0mW4L;C0e%3-KZ)3B^x}mZ zyqK-H8i~tQr6zxFuM@sLbKx&Fn7ElA!*v}gLC*kaUA2|mh_1d7XK2kV*NJ29hvRV2S#3#X*I*H4P=fP%-^qsZTY0#h6)pkKzY<&h7iv_tkYuEg|Wo69z99Q>$d zOYnUdmDYrVzG<~9ZfRBIUfp&P*jSZGrf{E++ zIb`JKPSotY$m*0?D5_^5-6A zQa;X{XCHQ?9m>Oh1^(=V!b9mtf5nmWY+O_AfaA4az)t7gHMuLh-Rz!1*Sd$zDrQ9O zG3vTfbD?@AE}*-U2c%k5h5`}p@9{U%{c(R7!qwvX3cr(Z!~GO=CDZ~=zKB&~{hyG( z2+D7W{L+X%`s4f4KmXSFQ-c0V{KUzzDbaF*9FtAeiK>}+AkrCDxCcTkObiJ<%%=8NonEv-%DAECKxF{c?M8U51M7x+>@(NLtLELN={cTR>8|bz zCeQU(xub%a7Ik<5``z8$!3+o7>@Pxd__=(3B%e<}CkgO(Go$mAgLSI#&yv55?h;P? zs=$X^QkfPdR(|(9xdQgUWr}R_xqESMZ-o6Ef-p3gX5-r8?1z2yfeGiHxEwkjzQogppMgq!6FxO8a)Fd4vvg@7gX!IA+>imP=`Ru-ow5rVV>-KUg;c(UQQLuu%~ z1L=E|-SlhTuPy|Pa>4-%<=L<9Z{Gj?{)2Ic+?|vkxss84KB_ow-oJXN9r@Nut7SX# zJ`lTP3IJo4r6#8x!jlb83Aqo(bbr--5DvGLy~?L>29K(iy7jc;6)Wp);<6pNAaab7V!i|Y!|NVBsZEq#O--?n1jV( zJo){=MpH`*`Uhb`;Xd8MJ?g`mya3u-nw1>%2F~1okSN{fmMU-%d(XAm` za1&`=#oT`Nr-kaT!tLMi`W37i1M5fN_{g*%M&v4TAPHyGt)WU?mr&AYoElnQgNbiL zGlOU10yeV}cqn$!R6Hoz2aEi7?twT2v!{K;HvdOxX~3;;6eL}$ITz+4cpF}x40p3g z=;!zv5mWHdz`F~6gnMzDTT^pFa>_=YdI`lJfC2&^+S{k@5d&4Q0C@(1%HzI_J$X}T zkzp&lgZ=YC-3(R+9^+Z9uFvy#(7XMM2ZpnieTCjaA@xSm-g0dA58R2Q+CgFC`i1XJ zF8bBE!7_5NPRXRp@q1nH^dAd+P)#?*WgT?Te9_y{MWFUyUyE{Ll6p_??GtluJ z5J*_Hy1=fn>~FI${z?occjA-_4t9A1$s(%IaxL9fJ>k6!7wCFdtkj#D*V` ziQY+MKR_whoSV*Dfwcl*?(WOSWn$C-)jY3sPCok_IF%k$THcP3TN(Cl*KC9mS{Wff z33b2cMQV^eYCZlU+R3%(_?LJ}gZnnzYj>rlkM#iLN@lx9^Vq#Nu=SxTQ5$#boL<}` z*ZdKZrqsf2PeX$=6t1s*Nus=a1V9Toh|pn|(BMlFz3+2(fYP_8lq0M~FS`Q+1Y+&y z*WZ*r&b7opV+XE}K7BQCq7pZ5O83H5Dk6d<1s=xiBags%?O5oau{C|1yQ2!^2j{LS zEw#V_D>Z}t19d_XM1{-MC7jMSl(U~NkL>=&zcA9?C_i=$%FLm1vX_EL!Go(-)~%P7 ztA_)iEbsMOxB7jQW*3(j|}6v{rIQytV?UD!q(t2RnX( zThEP%UiI7(sxg+jw^8|0y@3mjUMKvmV@vro7hFBaI6^qE#mtR$u*XrRNc}|FUDp$( zSZX|1EV6lzkFPr(?Vms0;z{`lTRe0h`Q0x~Sapzgq3yHL5g!D7#fN>xt!Rd~=Yl(# zr2&sO#l({OXlPJA<$-q*L`1Sj%?9SxqgkAmuLF`y(8}hc+3J2}PuL~VjUkxNSL7X62 z-zHnBpRx}t%UjtBGzmLiiM>cw2w4=2+VI*u+bRlm%oa>XC(+c&qzG2=-bGfaF*Auk zSnL;BE&j{|fRGePI5Q>h?G}?SLRUsa7s>3W#d?#;&qBXm8P1M`KV7qeFia_*{p zU<7*z^$2C^lOoK)-75SeC(*((2gfzRj@akRHBX=supcD62mRSpVy}DP8nC$x^l=3U zkMg&*oDbNQoMhU4vs64Pv`G)GTwJ)I)MfVbYg3qoYQ1!YZ#pYNt(31uvm68-1bc2E zxd5`G8|i+y*zC_HwiVWez;rzObFEGU_#y*If-}!oY~_Z7-WiKXA|-&A<(g6|k}KIi zaBcTRsPBnM?(KOi2v`C4|&Q z0+Fj|e{MUoE}+e4JC+|DqH7OsCNl*bc3=`Y*e#+QR)HvpJ=F|p>nZQy>v|`dFSI4W zDLi8X18cmVLe5gil3oW(9#9-kJofMngSIuymNw>+%9<8LI7M_ER+M!xa7gof>#SXM z@aTc(*g^~95kX(7?W&7((KgDPV~Io(jD5A$ALuER2_7Sm$)YDA0;3IgzmX*ANj4;5 zO4Pz?q!21Z_2}N8mLqxFY*!(M4JDZ&0eOm~r|b`CDHc7YE@0M@BKwk}c+$iVkiyC# zxT+gy3Q6E8+p8A+Q9{@uE!FZWLM@97k*}t>XyU{cT9xScOyK5;b*`CnNE>xsItOmN zOfD zwfx_Cn=ERa_M&>5Bp>xXHqdo#QE5_DTPxgMUPVcOfDFiD|c3tMZEu{?(mzp7P zq~<~!hu?<3-rH_#YlbZdi4Jv!l9S?XJ4yuRpkz`D~4*SH*4T#kEk)llkJgh-2?6vRiTcQw2!9x_|0JZ*&|UC~gCQTzUT+pCK^TrpL-?WoqX|K5%q1d~1Bl7JfY z7q{cLWxR@rZk|N0#^h zg;Z_YY|NG*Q7olV>Lr*iQLt$jH(-8PN+Z>13dRacZE*vfNqAc$%~-xUX2eP8hurGA zLr0D+Efj?nm@M#Eh@}+fNHDYyJF4ttp~4}OUuEWevO6o4pl%U9QGscMrkdU16S85c0jT4wp( z?4fncGKoyY(ml7b?MoEH_t1845gtvxV9!y8!1#E(~$*!Q@(32rO-`lq+y-PzV4 zsCGvI5yXQ<-NS_m>5tSWC!1?qz8CHz=RXvab|8Qc5GHC=TGMNU!+keLAQJ+}qvFHG z(l!#LLe7%hQAvAG*ng+BI~(gQ;v`wTxz>pNcS<*OWy59L@%$Y>Kw^|ka1UL9VA8Q$ zP3Yc|Sl(+0gHFyd$%iMVl6sjFJ|A^on}tY}-(5bm?t6GXrEVv&|2=g*i78zSI-hMR z?}afNPUgg}6R(Gk^iO!Uxf~XED|C#Bxz_G|B@?rIOXBMICrE4az1IF&og1Nu)J3?4 z9GbQVtKp$y=~+@N$%Uv$!H_+R7t&ji=QHG?6w*gy(UEZ6+;R|aQ1qHr0_Ze(=cHnn z2uH2*o5j6kJ47T-q8PzrduWTg=;stICOwLBx$?25WVmwDqgsL9l>r$mzllJ%KZn}R zV`yWEf%B|6W9i2X|34T3ikIk}MEMczTG~nZwOu2^A8QHhXQ)K$Bb^>A(F1B~Q3B+I zNRSx9?*zTAbNiCw*;OJb@E(2z9ys1hpUG*yl#M{D=wk9nh<=W*^aO(wcgNK^SR!CN zK#?fMzQf3pQrtF|pyY~9m(^f=VLD{&mUQ&VA$;*A!(k`1XoNB2-);jtq3@tG(hFfb z?E4P-@Uvv-vm&ot{XFGBq&E8W`dd5;Gvl=DY1JKxDaxwwrMW*=VP-5rR#HX~dj2vN zoD<5OXPEn1Knj_ydQfqRa)v8I z%5rJr@asNxM**TPt7}8q10`{L3CX)nCCVwFD6yB>R$K9R_}b_ab6J9V6`n_~o`Szg zYbPOPv^pN_(c`y9c4dyWxnkulzOf()n#x`(uDEf|1ove2FVu+YbuA>KP-U-LxbYIu zeBGz6)1kUi@j_p2Z;3sJSDVAD&EeJN@M?2-wK<|%7^sY@?S(u7ijUnBO0b)bMSXHj zGsd@nU*ogKO6W!K#?~5{Lx2u#sMPO5n_8P@z5R~Md!5%|L-3YEJ>ae)Vdv&1=Co@e z>B~FG$C&%_zUiCI zhp^%n*jqQ65ed zJ!IJe*gfE69PUV;r<^kPIj{a!bTY5>Nv#7^<)U7G0V4!UR7gp*_GvEInUGn4r2KUF zq$|A+t72y%{&bbzNR9;6H8F)h!bi1`UaoiN^P&$d-SwXxBb&c+r$bAcOP;kY%4eadrUy*9B!{E^l{ zb+U#Wm1xLgfukI8dPC0yt9gXNS^m)4Qznm zA_2b+Oq@z)s=S++{T{yL6JojgF3OP0txa<>w zV}C$aZ;DC%An1OYjeAA^j9|thVu=9n{W65goBLcSdUgglLdkxPrJ;2V!5OoHi46AU zNx@1;_nrv3`caUEx#k+{?J>JXV7K+IE``#8uSk81CSuHcrWCXw>OJ~eP~uw>t>ixK z8|qtPOKZx+t3*fcmRRu5iho0VOHzDGl6^~(sHo&yk`VY6Cad6=L`DtnS;GLkJd%F!%j{we}ih-3buOTg%}rtt}}^_AU$q^=UlciJ=>P z*ZV4LfwzH&CfwqeLLZe#ABXAwP-#RSIXFdtb9@eFV~+)cuoF$Jr_Wc*+i@<`Ra6?u zbFc`&lsyyo0z}^7Gsy~H4#$sU&dD2aXvy6Kw+ zlai>pRwC`1XQ=jd(UqStZMq?UL;4*J}k>UR72%KH(i`^=iV(JlI-8hsQS9_ka+Dp!-xp=!D|p*PXG zBs7w^p!;CXGcc@8toJSNL`En;PleF4=Si4$2)4xcW_QO#cPCEd$kkmKc~sRNbTdNw z?2-Oek%kRjiGE6LtJaZ(>mCn8#gg~@1?VaZffeGUG`2ACUuZCPaZ}8a> z^{`#|Ege~ePYtKk&3UYtdVueFweO&swLFhKEmjt&A!i zS}1nScOKNTzefaEbvf{h1XmV*p$xb#qu)rAYaIPrOzt_JMHY`T#J~8e)Q!VDN2)w~ zBQ^|EFCUotuxD?i$~?DQD%JQth-ZSQhR!kI6g5>*+`v7an`%v_C{qT#^JNPv_Z)h4 zO`O9CNuI;$NM!e38CsC)ux}|H3Hy&@RXa|g1QT<>J0Y`QP= z4(E&D(SGwRo^9>}y#H^e)u(yvia&)-8RD@#HUTr7*#4L*vy~x)-rQrMHBg^0v(Y3m zVJZlf=vgQ9Qmh;4*^<~oZsp1zUf0+l@FgX_^K}_dmP~al5B`fd!IA@-T;h84F2s^~ z0&Fqw(!X5m#5&EmQyka?M&~1^ZCaneJGVa9ff+($>We@Z`!cb2^<~=d_1%|gpz(kq9` zJLw2 zeUfB0X@C7>lhrLrgszQgYfq{VrU@2vV56`RyaNQO}xm0~?pFCk#n@k$i%ew?AAtfGqOb)w<S=OduLR z-kvMv$CpxFJu~IUaSR?QnR3k>q@W0{=Aa1pGw>jgZM}fWA2{_}S@| zV;}|n^CR}ZL{p4)LMUUci!l8^7kRMoZUKS?Gq$s|mW2&f=nK&?oN1}h;!gE$z_ zfgmOUX@uKKnwQ?TRObk`1d^UaGaN^~_t`#sYg>E8d-2I_xvi~OxtKpR|DX{5TnrLz zs;QoF(i&}=Opr3~yY@LV84!DI-~GMceZL&BdO&Y;K6$!*}_4SHLhcJ@pa|B3{=|sm=3<4EzN*cpiI! zU%F5Ao%;jbocqy!k?#9JG+IZ1dhRHSTE~Fl44wdyCoG{C=x&8*|3VN9Awj0(Oz6b> z$Ss{%b#5oF<@IPM`p)gdoA_wa%Dmi;Iyp8aSgVj(FSvL+%05z{`Fmi?=80XFJh6re z0UmN}iK9DILTbUe=aFlK+KmZh)+ zNP>qr87T4wKh}Hl3cZEi2d|_8TIz<x1wH?$C;WWj**P}@ES#zO;ag3?f0cePS{YPZ{?VP5^(KE;fU#BleTlzv{vPD-VgA0w-w$^|;p$A4 z7Ik~XOn7KA$Di|L9ll~EY~uI{If^CrE#%WoapR2--S@I>SY8 z8>_z)5nauA-#?}-I&n?-Ud&f1Rjzf$lB`wvZTEYXPL2=Md``pQfF8$7TSOv=#h>Y7 zsy-d-GE3W>dSEQiz}D!*aVdqtYWA-grvmq6h0_>+JhR(45krQuV{fEK)SGd!H_|ccjVJb|M&6|1wx1%0F(gH&TMyT(hxJZNr<3pE0i!UHR32T= z20RwvjHa5+&J;YCe6H;)oi(SEwx%=`D=gTbFMu6AFHMkQG~p=6waszIcR5MRPR{~X zn`1RE8e+&SY}qc@Q?=o?=FPZ&%=V>Dw$fPIhXR6H+8pv}QBSyzF|v&I=oQjj^)^$z9jo4c-YE@NwW)98)Hj!WE02C2OPYlOm7v6BG_l=f z+3;(Z9o64Fqc=O-92WmPk|X!% zGkbU!xOlI+HgbJtzuacFb8SmK$Q2@RJkzBfk8_1$9D$3wxqGHZz3k=6kjl$G?f?`s z$)@x#HD;tr(d8|BBw0*H7qZohGNNDy2bwm=76H4+Zky8RG91;Hagvacw5rV5>WpTw zb!Dt^IGUBxy)JN1##;CTrB0MTl_*;5WDy|ESnf2E?uo8<$~U?OcgGA#;bfKrX%(yf z!zc%D!8OIvJ6(#m@Uq4@Qn;uF2k;S9>Iaww-Lu=bXD2me6)x9??egRlC{0ZiwkP3m z?p}bwXVEQDpG#v4H+y8$XLB5w)q5owio$|L*Xn7}z{O9~_`tcyUjSUYWoyd!zGKB zX~S!S&Kr?!7QO_tbf$kUJ3d^N*!(7i3eQO2wVM*D*^-`tF&0IqG%Rt3$KrS2x&*0M zp`@bhtc#uSCA8sl*&p_OX9O}sLk!U~JwMp8NPv~?Rv!MkpX-dPIOwKC+6 z1q&&yV;t)!eQjKJsDZ_JpGA{T({Kx+W`H7Tc(yT4SR}?APW(Ov0i$rrz+j(dCMQ#uyj4^--swCEglldU_;n({;vp{dCkn zDZhE!1mlicr!gMLVnZEXie8sJ7Fr`?ayXOzX8-e9R6jnRgJi@&+n5Fs(d_$C$eGO^ zC#wrD0Xj^mfvw{0EnBs)?z>{l&v}D50aQ30azvH|rAvuc#4E{N47}TKO^UX}#CG&+ zuISg0H-c&NT?k872Ajzu@<_z7HmS;4`2>*IXI@yaVH&xKlbhTfi5aqrI5*i zP8nu98a?l4iJ)ySGdD4X2w#drHN+I(pE~NFFh}_ zFnR9LGGnBn%vt+IB~1d8I2@|k+KDzFugh8)-cS(7ZBJU@&G6q|u+CplFw;N3puj(` z;3ogvf;s+c3;t${v*7DnXENP)7Tmpc3UfqfLEhHvf|~oB1!o_~7)}*wu1r$-w=7}+ zmvhIoU9zdlIBeE~Dz-Ga*ta{rFfe9;FN0l8S2*{Y!; zt^S##%g(D5+R8@p7Oaj0#x!^%uq5P@u|mit1*3aS(_9+lZf0vogC|gup}m3NpWRkm z27*R}jo8ZyhGiq`LF~C5v2M}(6ZXkN1wtqt4a;L_FAG+W1jaS^MjCu?z;wa0T{E8C2{n~xJ}#G5F|Q6?VHKy7gfn?+(+1y zZCosLeB|BXqZ|b3zg_P=i+#pFmqSfH#orA6F5z#yZaV$!#nrB<8XLa2YD{=M4!4=? zaZSUKE_G!%8E@2Y5M2iM&*GpjV%{K3NuQvta5=+3fu3@{oaYsCKFOSj+~%XL$;4X$ z4=(ag_((1UWlWA%K85%-<;;XIhrCKr2bi&wBc{V#fQ0r7#ffaRN$5gk_SSi6Vq~^Z z{5CMe%oDo82!DkL#H&V!M+s~LHMni5xLS>lLXF*kwe~_(w?f6HP*FW@nSZ(ero3rF zd$Q6TzJ66zhBdA&qFBW6q}-z<8FS10Dc5VKA=h^EZ4eiuIE^aU#+pEK9RJI{MIq~k zdtwZYI~-F-N8s}L_c*N+vde6I7ZiHx_VWqTW}R54@>q~oK2m8_*V2MP5oil#@Y+6h z0Z#Uh#|Uh$jPNszOwnmsTHThEg&qB~KNsWsVMF$yP(^=0k3fViE~Ez=?>un9DiK zuQ)xT2%$TRAgJ5{%~;apKH4Puu~~I3gRs={&yZ48H8*m`sDh2LTN`8X5QW+#x{|$l zEd0yz8S=@LV-V<#fKW}GjL%DRJ;H%BDvs&LrEys0EIOy03(@Bn&!f)^rTNzRX)Kc( z3z@m6YVuZU8H--t1twxvZDw5;SuE5bk&L{qP#T27pmzs`(IRDG{>*n4-DxE+#-oVs zXAyH55YwLntBOt8$;sMNm@U($dA+nm^qs(Xk-doTrjQ)v#2V^QZZ!NUWa?4|*6Rz3 zw|yEh<=HXJQKgH%)AY78rFFvTMVy7Hpv6j9s2$s$WZS$73jMOL=R9Tc^A^}gki>ip zud2bYQ&#Gq42vk+|a zi;|=`utL-#aMejv2;9alJu-IF7A%(i)6=pl2S9OnLZvA`8I`^AlUmu$kHC|~5Ti=- z>~q*++Pu!6fyLIvpA)k6s9w;~R8bLq14hYUcno44IZNAB&4&)|BSV#6x2I@Ns?$$1| zt5A_iayx3@l+Lx0DxF~d7CKo2luXHIQh`v{9Tr~)A+0L5Q$YQZ@C@tDYgQ%25)PQ^ z%!GDR4$h$eX2{?jheYq6Y0F2LGq=M6!vZghbuE0__1`ZldqvEJo;o?YZMmoJ8tZ2= zKMRg|>XP_1lwD%`ok{XyfL_Wg&ra+Htd7Z{J4mQ2R3<~LYE>-gZ|>61COxo7fPjtp zk*Y*FKf`D~*y=rVtqAPhu-K~7ps&xolU9R=9{FnioiL0GK9{Kv)`|YAn{bk2ZQp!F zz9^t-7R(gCZQlTHlbA4&n+k;R6pR^F_o(Fk=HrswOHMY~kT9Y!Fn=?0f%n~6P|jBMS%UnZsm;XNUI|$ zv5_}PBM(q|_%hOK6R~I+Nj3AY+#_Z9Q}>upA}__VY{Ap8+BsvhGcb8nIw*kk{`vIc zd{*@26g~5H?EZLjxFI zUPY9DwUmFgRsOeQTlTH=xDTyU9ePjbI}kCwm^azK9+0~2%AZFcqH&j~%DYuHJ(bs53@PpV2lc_O zM`%W<&fHBbKZ~w#xfEO|Sf`K{h7KcX+f}D@1%2J{ZPi-yR;!w?%D~cYeih=B`HfjH zPXW7TO~RN!OR5~j=6@%R>V`;h)p}`hl3fJsQiW5tNC)#uMddMVmt}RzyxV#g->W4% zD%pH+!MC42@2ygA*wJQdl1j;0%yb7W?Xh_x71U6QNz@V&N{~Iyf)lt9&mlnuQB4#w zJiu9MQAWi4h4hxCGufrRO>Zc|Ln-pq?czx(Q{k_wmooA2Z$$Y2t(0I+%n_YRY7s)+XqW zjDF~N(s6%73+$F!WucsGIrqgn?Mv1tn;^cas7uktCT^|7SZ%Yg3rwsTp+Qyy#ZaKV zCkBLzkzqwuN!AkdpWc;j5>oL+tb{!M`3aSn5E5pB7;e? znfQcn>?_{Dr|mjQt2PAd*Gp|dZ$oh~SjesU$nRAD|DBL!yV(lAhC+a>8YsH|B&&&) zcUW{+PV!JY34-Ct7LN1JG>9$o3r*Q0lO>M!9n+XoeXM7Vsvh)<|5XF~y-N z6eansIvHwsPDqnep$x1L;i*mgwEg)mPu7p ziz_#R9pR`;s7Hlts+qq=yBHyY71(7l_8~^#HEY5eoP#@lgm9B@&p}czajsc&&vT;C z_(wJWdnr!Fk~c*dw3|61M@t6qTMtip&M}>&9+8|P&?@gDY4{jTLPZuyVQIhJa zLfuSrEQun5f-eH-&rn`@86SQ0{w<@djxKYq2_bNKYwFLSnxr;wF9hdMmV|%fj=cyd zF}93baKKad&%8v)dFsB;O;xLs*vK1l`Q zU2ML9BSpwZ{u$K_=z9a@eKn_7Ym=>n3iYCownA>96shj?Og@U0SRp#-tt&PcFii=) z2BbBoZ}!yx8#kiem`Bi=5}D+SF(zp@JAZ34$%88rm;@6{N^$sc$t%>87a!8pB)=yuzb*+9n&+}LT zPuOas(RuZAm5v)RlVBkNM)}o#A<_0KC#x@A<~tAp7`|FWkRTG`?qL*)&3b_y^PY@rMSU&;alC)3C!Y`A5*p(a8g3!5K*B;0_(`l{yZH~@#sud(=DDyoeGS_p8AV;gBwmIlP{Tn zm=&Vk`}#m91+f4V{#m>s`3Rlgsp95(>R*HKR#k2hk=Uk>+~s-f1EIu`Ri5B;Qb(xR zQ&+>ArJg!*_+RR&e~u>`;vF_geg!03Qp&NQMY~9eQ9=G%kxCL~NZHuudK$hgWk*sy zx|~x6P0v^6ngMf8tbkDyPXuL1v?)Duoqv+vc!5s(ztG{!X7@iz20n1hQ*{STN>C7L2r3TAyh!ai!QxfcW& zUT*1a=6rpM*rA1mlm$(MSDc6OGrV6F^gd3J+r-8tb_K;bGKYg68S{`F{)?d<+YTsJ z4WiTiiPUtBu!oFkXQYSTXZY7gO12!;M@l`9HY*A*+qz#LS>t(hhk9D4{X9-R`pBX! zJ7IyX>H5f$trzPf%eH;ThR*d5P;@qpO?VcZFK2SE=%Z%-&BDlNXS+F}QB)4oyqi(P z{#Y2ZErlnj0JUL+f|ck75>;-thxp%!Z1RCWB~kiASJI{g{Dl6V_9Q;bu|v5-OYB$K zHqpLBk!&uh+s#)Az*l#in_Ax)aSjW$b3ba`tL@(j8UUViBB8*V%}F>Z&k$Pn?T+fc zx~Lu$MT_VjQ)L`sv#6N)gTCDXW$zFA>dK6T;@)C8x!`CQR2h^=3aC+>!dtyjhc?NQ zH9@ba-~G(6%^!$4Hd72R>QM&fcz25(X;6H{EC-pSnre!ukdez=Bf686p#*x5`MuxJ zqIT0Q1f`VRL9>@`K|#XP<6MD0XwDH5MxYOxIoye<*q>$h5n%}|T_5z5IiT9LE}&X0 zbFRoe0fhvH8kzaW=}%Yge95-vi1jBycQU4`6%*DDn?#Aizc-+@%Yw;ehVlw3UR)Yb zNz8+90CH=nQEh3*9taerD8_4SJT6C3C!&hFIQddmb0 zw(#6NPm-%D!#H3JtW`PE$dS5e$?Y2FR9NXf_1_bQQii%rhCghB=2y`-6E&>sV;WYE zS=nRtmN{ujf`$dC%tXc&n?sCaA=#nMo60As-0b6xGB~m!4K4nldF-bjtpxJv%v1WS44%V0AgeL$#|K;w41|l zJU=cW`?$r%&W@2HWy9vG;ChfaZ?xyteruR!c(5%U|bsgl2K zRq)TEUc@U%TxAoHdl7;Hwurf{DC0bKDXg09O(9Qiboj_^IfwmYpOZn`Pvl+&AmgH} zq!3>mm6APcih$G(fqxNtGVk)=Q{=y^$iJ+}pIzinFVa3&c?J}OeU$_9vqbPBg{{rd64o45f93oET$d8`dwDpD>*YNyX7afvP-3pnBVMWu)uw8hBk=n z5VIt7HIzFHm>4RtmE;QdS;NGv;EKHH23-QKWgj)bDjk2+mP~E91mf zg*EeMYU8Xq*+%3sGr=oZL>8N$VqCKxYeYDMSIRiJgHZsgpsbjWT|beX&5UF6+FEXJ zg$QlJcKD0qBO2~;=3M`EEGA9X%@l3X1%#HM%v=rm$#4`g{|PY^zQmqaMa*Jh9A$Qo zm>VII%8c&`awF!Af>nFb$x^@L@^7^Ya%gvGU!c0z{GSIUafZ>Im*>y2y_DNHCV>vt zQV5b;O9Qz*e@verT9<)soC7)eIFUm(gvg$T1TAEQ7OAh|2d{ZXUCKgN@fu5eEo*Z% z4#z8w$+&L|$$9F0Y@5LEu6%Gt^E@S&qw? z!En;bM6&7ZzCSF?#qgIEV__Sd67c6v=N(SGJo6=y_H2GfFg5+gEtA zPcXNp{s!?8(Qb<8#B$Sa&a`0tdy*0OD&XD2j@ruX?+bwd`CfAZjZis&cR7VtW~pzz z=4o=s2lG}~MBxV^W+TW`_Zf-Shr}RSB?$A>P2;(~)l)x--#7pYScfm!i5MLYit^nfTfrm# zB|@vb6(f zOq&T!wks~!ZFas&!gh1MqC~7M5Ktz>hA*x)9815=&<;vBwdYm$`@PIs%E0;wY7%ZT zi!1m41beJ+r@6~2>URJk@1>|_$Y!)+wr2Z!L+{dsxzVzi%ktr0p+dL$J%RZNJ2ea@ z7dxpYM^$5kLXR9I#1K3<6eEySwV6fs{mFhXP7A0L<5RpVgigM0S#W^Up?G4a>9pYV z3U$hK3Bu_YdnSCf-`u3$a?8|Uzgkd(gPCDOy6x1^lS-{9f2m5HNcmzhXf!)X9sV?M zE|0?^G*)P-6OF}{gG#vXG&j>g(EpO~cZ{(G7FBwfw=u5XEG1@Enkrwfd4;ODTS
    mbQXe$CB@z5zyG_cb@4g1vZSbL)e>q=rUY7XX}lFe+(@-9 z)w_KlQW3h^ZnTrx#m-{?cr#r%NF`_?0fatcRS!LUozAv%u+?cNB4-yR*0a>UgI|_U z4PSeJO)GtY+nmCDme?@(rWS^J&96z{Nz9ZjWNY-)|5UpBSk95!(~e3IjO^}Jb5i!_ zLZ6{R<~@?^kHMZE<){||8VhHnQVJdI0@HV~NC+2m9kuQkwlOY01vE-XD zN~76F=P60cXerO9z$!VJ-gJPI-EM_@? zLH1=A);Fp_AaV3-CHYKRY#hT!f>Cnz3E8@GizKga_NQ0whaf^~f=!qwGN#1}hBZcu zJ-@_R2_@FPD}!ocsbec2r_}ID{VyqU&bdW0I42hAsa$VUZY3lYzF3Mz3u(z|S}6PN zJaun=h+yXBpTW)fjaV&)R7-crJ!)wNPc%!c`T9;-+dCsKMMsFe^j~F((mkUhYlN{K z_FEuS<)hE{hVF!7tYq*_6oktx(2}#EJiZ~pl%A7&RCx~=EqR238Iwb-`)~rr#RK|& zsMe-+w%-Y@i*5a5GY~3`k|x-kHb*Q7iYCJB@hZe48&fT#DnohYYK6(2W`PBKwTKV} zyu~i~D50cW9%or9_0)0R6Q>v9dJo|N>UmT*irGx%*NcK zxL`if%rW4w%j{O0_xu;dQCvv9AqnC3iOZ|8%F7V;V~mXz$%2vHYx5{FeA}NX4?RN2 zzsfPx(zqIBnUK(QR;-{=oLp_=WT6c4?dI0w=i=n!Yxrm{`{#`g&IwqbV%Ko$u&QCo zc{QMJv()Uz24^=d%TE8imA)Un23De@&>ffu$z;8cY1^pt4o+CBO7}UYvyf%!yZ*Vg zvqbNWDm}JGbm$i*IUH$985T_%lY=U!mvTZggtL^j$5k_eJxzGUGI`B+X*u(YlC;Z} zQRH!?txC5hYIXEWISO)Ry#vGpfam+<^BCzh(!E(U*}hBVrGEvs0gg{TDIq!g=*WGwG+a!n)TUR z(TF+{C^yaTiS{SWVG-QW`iydufG-liv93WCig`ylmmr4vjUr4W12IzxCL0tLLeh$< z!W^gY&Kr=0TGOi>fH{Lt5sKDhupl^3eH^eX5568T-gJu;ku@H=3T4lpXz>%C(*u6oVDjl^(Q^`9X!M_<)4aS<)a?v;nkKI zqF!y-^M6We(Q@iv)28VgdT`e26=Q{EjKUt7N|!OJ%WA)XDU;L>2IN(E98MkoXf4d( z)JKCb2E?3(YWw$qN84woOxc3431OTGT;5Rb4qZuOY%s;!dODBNp%U+B{#5BZf5v>Z zR>uMx@7UUdV;-b$A<^Bx0@db&cGtBEqz#)=9jzuSCN+Wu(iQpCDdrHwGwuy zgx!*`hlGF;a~*=5H$p+S8?JH&p6RJu$}8MKM`(v`UW%QDhko_oD#eS;TA~lu7f}U- zji#|CbQ4)5xE1bKtTSK=dwdboCCk~@+{=QbOzqpv6d@Y)e*hv@VNz(EWLtzQ-a75} zng^3_*1XD2f}23G6izuctQV}n{AJ~j3eU&wR53F4)OP`$)wQ8ta%+2on6h+WO&i2j zX{UMn0ag7=Qi>35M6~Oet5TS^ppR7)JU#D^n+vyN#;E&WEyZ84|HOVCQQ$|9h)y@{g<`?Uh4`_)E0&S(Ui3azoqV(_mR; z(j}j4MK~2`Pm5RbtfEh;ByhGPvQ zcv=`A>hB~YR6TXwLJG=-g87(23zGuBFl@W|Gv)?y8X&bzOAh~H)pysOu_%YdTV;_L zZ{^yia*5y)o|6+rBan+}770PbC@Oq~RVjH@7i9fX)wnp87pfz#Rim}3N4#v10`*Vw ztF#Q;S{k3xf6W|d0U%ov%Zk}h3k&?BXQ}iO=0Rpe?W}!1#7pyE!in&AnJb}1TK$R$ zgqYiC0{jh~3GmB+w94Ej71Fo?Gq(}qdhDZ}AkH7sQZ*mJXklKpUsZB9m4vqP(Y(rn z0<+?KuRzCYKazq}>uQ+o4(*WCUh7h87 z?qtI{3V=4~6XFD3x|9mL%w(&=jTfx&A9-t;BqHW#pc~b0UHa0W#hy$iv@&gvm=j{( z;uBhIH{;DbSk7wh2O>Z`=GE~R&+U^1Kn~%>gpDvigD^LI-SmtoJw0Npwl~;*uURmW zFe>T&ciCb@R3S#W8Cp9h*2n^$Y;yP`ig3Y9?bCAjkXpyfqOk-ByiwTScN{mg{KNh% zyP|HupIiQ@i|}-Z)rnJ}2XY?;yRw0&32A*a!7-yc;F`p$60zyTiId z;t7PKuz##Q((4~i^E|Aq85;h6zCrUISQiVrD0A%z%G^d+K(Fpc#q41!irB$t#Xpak zpM_m~pnfJe5{2%#;=FE4=R9tHR>mD!zQg6e8@wd+%JD$8>e{cXtlY{9UqYoHioBE} z$x5A(p_VL9-6?ujub%AiUtlzN0Y$0iCWn8s(k8w8W5Rxy`7lBZ!O$-3Dh4)6qdK76wei!oJRI$+N5IIFWo>d&U<6Ig+X(~Wv>MnQ(QOd9N|ck|t9nGI(b zi*_O|-xnO)aMG&qrB*hiCO|wdi4+j`1H@2Evdrmaj~?imu56RL%t{dx?dEf;uSU@` z)RL5tc(R>1-%7mYyu`UjUuJ&h$Sav_9aP3poHx7$<<iK zzOVXNRMTtiC75q9SOe(^8H!~Yb1SKx(K(HX$KP_R>?vCyG!6D!BUW1NrkV0fUyA*8yPy(c2@4OncH6*K)_S$6JFJ63FWm+Xu*@Dlief`dYb=q*{s~TI5TTBKcHLs?(gj4o&&uC3#D5*Qt6o*egii=qONu7+Q5~ll2eKW zTi{Kx$|L3jd%&;n5%WV-ZR`P-PIl;DIP}JQ21uf&1ldGYHdg{e3w8bR5Oqn2>u!N; zsNW( znX1_8Je$EhKabez7bli7zYzsYk1XGIr>AZXqZq*k^+>ws;SBCQKd+g7Pg)imPF?&| zcERAGw~$Yj?Q(Zg?)J;wVY%BYcfXXo7v%0`?j)>a-2rYqkBSqw9$Dpi^k38sTRy*~ zZtxfVM{aD*(1NnT8gKj>8}!JM>QXGS3kiDZpQtyAFS6(1 zc0O0`l6n_Yn&J%1C*rSJT)BhSA`5@Z=)o?O(3fdhY}Tb7 zZJLWdVpUgc-i+`wiitXk>_S^ZK6&cpz&2=J?F9`T7cdqX?cvI!y%Fo8C zvumx*SoGiwO?E!=0hsL0JkRd00z42@PtrVG1_JnSLC+r=@gXWL;TKPG02o80M zW1{S-FCz(LWiIv?5(R?j4%hH^C4Za<=f7Mxv$bNv@JDsi>(Am%27iQp@H+|raH9>m zwIMm28GpP>RnlV?phYmSRPWB%=5OgzQm!JUyc7tpmI2R4 z7S-U0%v7paE!O6XsxhPT4!a@UBoj?#MrEP>W+#P&FOqkemGkU(KOvP8d`i~@;X+@7 ztd@Z1u&}W;`mR83GWt@dEiZq0cUsnA{!a4u7Jt9vujO-T*v}nFNy*76DXCU0yVzE6 zJeU%E;%B#%Dl6&zbhmEK^jGQTRHV57(?$L)8T~C|)0)s1gd8kTBj#8@9Tn*T=``6- zWQ;K$a9ZrfT)!#Ot-mz>ejf)l@1f3!&rIePviP_ZpVTqPn`f> zaI^omg4KT1VryicL~+=4#S77bB9DE`641ul@Uu$b`IU6mz`NJYmcWLA zzb2RWuglf{G1Vb-?hO#^mzC9ZhyhN_Tgjfw0gmnYerPBfdR>ucKaY>eBi2#Nhnh7S z6-khwKei_IAHMmRa}q@p9rYVFL=z%p$^A@cipm0Nqn-cGBL8CHN&l=O{}NpKCcrn( zlC4c`0Tl5R&TAWk9B?Q;v|fvkZ})bpjrVxm80`HAE|kxUhDNb~#}1 z^VS)D-p532)HiOO!^V!Sa>j_3LxwW-wQ81m*&~9GtWX_TFD*0IQyxo24@i=p)T=+A znZ<@<>ljWKM*>#O!T|Jrm4)**;%CEteah?VfY~wTHg>iroff4{2q_42`U8vKgnCmv#n3+$qwk6W|m zB{>fFXR1Ioi4_LB?8zjG3W7Lg^Lx z-JUv8Y%B5yJaw(yNB{-tE%I@mBgZ->9O^i!^!08jzuOEyYIkDj$K*NLc<$kkGfsB4 zc?bF6=ssed?g-UK@QP{kMtE6sveSB>`t{8C?KdXIZWn1+j^1AK`fI^kqOW7N8st(P zq5UgX@EcwlOOsH@BEJ-J(yDUC8}`kV@R-+M<2cCf;4MiMBZV|->9>Vnl6Q!2aR5Et z^j=o|h*mm`_RIm55}|h4>z3QHMFbOrLIe@0I#v7>ZPGXkoAC;qdqoW}@PnxC~q z$>_9JF`v<1k>eYedlVM*d<*Xhn{6u$@3@!bKv3gI1&5I|bBQ-VqRSt`@ zP7IC<@Cu`3T)f;P75QD-9LUIOO%<&v60uc8kRJP};i_a}pE)^!VvnppbJ8$kq4qb9 z8fa38J$7a&t;=Pi_mB6v}dE~>+0MNy|-7FGfpMlEElpWRa8B@ zQAUY6%!3m<#srIAS)z|zvWei<9ITffeDIZ9^r$>GO{e#ru;y#)W;ey7vT|4zB2JcR z+--5@rWc`g3Z^NAMw9Bo4M{fyI@~L=g`Wy z>Lw?0eh&M9c9k4jG0ChNg=Sy(P*ed>E=Obe`^rN{peR%kwYB*y_oK4hii4T|2W_Hx zH6>I{lyhFHDS(|fkHOky9FVzbf|(Kj_BZNVSj+9;2*-xYWj{@x&$TJd3}I}jx*~iE zcWS-qtUhwZrVir}Y5eZoQ;S>2%n2?hGNLh(dvqA(`gGa@sWr`I;gp)@xeenNEvAD~ z^UGcL{YEaS#F7pF7}>RS`=sq0p|Z4L>}~lY_kEDt2%(oZ6Qo>4g{;m_XVBBq>l+^& zQp6a*f>DQF$QG4@vI)X zVp9`yVTqT=IBN`(+CO3V6fq--{h#1h?b}6!B~mGaG1Qx&VR|9lZ&D z*4e0&uY$OQ&n?zxVm}sFrSQKJJQY@&%wwV#Kf-}W&-8I7ZOaek%2`K#=38g|V=;2> zz_~yEMjFz=_ zWgH96Rt)N6Y+ez(E$KV@o6~|*lDTX8Fm;yIfFixU7dImpt((oU%!S8#nHkkPzi&Ax zS?y|(W<7)tRKY{om8~X)HyCZPW(Ypsv5H%vAMU+P-;m+RJv#faYQ#gqg7I*bGynAc z4C5IxxWgKOf``IWu|H2SG9HqaL`Pc^y)BUaka4)^K_HQh=6ZYdSZ`IzniZABzBpnX6;7HZurSUE=v-@215@{#pawM10dF*XCYdVelRLPlD!zXB> zG3BwGH`wyRFQ3D_aici%z$@RGV{b+A$FI&HG*x8F5sX2`k=b3r?EkSgzkSPCb-Mk=7r=U-@&S)#aFdSn)^jj`{&3FXc2#$VOgXbMMg;v@ezGp4;~5lx@5&RMq@16cNgY2Ri_7m zNEqS|&+FA)4u47%XY|D^=Jy(1vsbzDJF7F}=#g#ms6dZp^E=ynJgD`7H?Y($6!dIx zUb1nd>Fl`@L*LL1_4GQZ7ux7;^Y&Pz(=!Y-y~dW{fN?m7dD3ydzQ-Fkdtu~d^>N$? zj=cjg?BZYpSNE1K<3v?*)0-?eMb^x2&Ofw;gAM|B1qG+~1_v;NK|UdR>*KoWxZPF4 z|CZbd_zp^^SRDmzAA8&A8a`3)|CZ{fZ%IGhqMuIL@`vzO|6D(H$3%eDPr-SU0$NZ0 zu##o;L$?wHRyTBqr_pPdtAyi&_lY$4fDSt=atod|AceKG_hoo5Xf}_t26JVGxd;j83b~Fc1kW1F!X)Ek_WA~ z7V{8_XbeOEM8c?1k9{}iLwQ)p_?-kYeihFE<5oDZJ?F3mr72{L8^NuRv2@@hOssay7cWA|9<2ixOBho)&}$s3pG4! z+GYLtYtF#UxN_Bd?j6mi;{AWr3+H4&%1B2IfmoZ^Sl_2TCDPH0#`vlm8L36)k8OnlAgaA#x=(m-31rZkHnJx|?@4_+7G`u}WnS0(43S{+&im057* zPc?zg^T>1H@-G*r{|5jzrk(-76a}UTRPh*1QSZ(Zs0nm@J{kX?k@?KVgiKX;AA1WC znIKC~8krHA;VvH_G>}NtgCbGaSP~V-4u_8{ot`k^_qjYTu1uGiqtQG|DWY$R!MoJ>8qSyfMzgV!Ie%KyZ?eSX6VT8+aH4te z-eDZrqTBqR3oc8-QLK}>)rickhEOa_2O6w#i22uiV(3T!q~INB=rRq9O~RP1^@*An z#a$mQy6?fGq)v}|`*D5LX;+*wXItw~ z(u-Yq_h8i;F+MmFYI=K``w;#rhO6nFX-D2Y5^^72$MJoEcn-aPZUP25-=zbaNQURd zRq3TDW*AC90C-Y1ln-*>uYFrE8qj+8NrQ3@<(vr4OE2@hIP{7<2eht={KFpo_cX8~ z|5cCvTW((Y(<|~c?0NWAd3xn$^|UEnjxKbC-jH{K{|cPKr!Jv{`pqsa8^d;y6Z@x2 zb5{h4(nEVmmfKOm9`TD*ns2zYw7lD$nlsPs-$NXvVD(^|*TktG^I4x!Vy1KiO1kTh zZY|bZTpaQfoPyJAkLSgUJTJQS4gC(!k2{UBGk{JnI`iKb(}xQCBF^H0O=tWm716Q! zmwS@+mgK0fTd(eRYDc!7wXMmGssS{rerW6bVQVQ)yGFcM zF=dqu9@#KJ*QK0tpTt^c_gcy@mM^eMmTkY1%Zij?ejY3`4h^65yx4(K95uDgj+*E; zMQcKTvFYX8oZS^?-doQhy_Y5<1N6}P+@s8s@=7v1b?r=Ys>cnNXS}p|iTb2Ia4Z9r z1zal0vm_B4F6$lMp1Ybb8S1|oo8RfF+scP8b!4P4fiy^g?EMf78I@BU={%Z*U|aUrK|FB{d}dMhM-v@$G+>;+Na>ek4z8|BuO?jqH#+Hcy+}rwF;@K_u)^V$=L>A4BsaHqBReeT3#W zkJ5bCg=n6g#~hU<6k7i`xK4@h4lx#fWxn&v1oBUaW{j+7aVvKy;BCOTzJ$Gij|ZzU zLq8JGo(%Y22>5med^-ZZrvtvH0z^6VJ(1^|$$I1IAH`Qcm+u|Ii#t@nGP*3zl$ zBMzR!-=)NWwv$31c>et2oSe+{%*Waw5OIT(H>l=w~Rbz`Hg5+ItJ|$dyQs zoaGf*R^<-YS-^Q7S_ki(R~Cnk@KSsV@0X7yyFyvyq~h?UoDOuUUj4!)_cIi~a7_z~ zmm`>g;Y61&$9v-Ty{3#+hrqpEE~VR%}X1f)4?al*s!H+p=5A{ z{*G_D@5yM@*fpV*+*geaZRIDRJsnO_(ECTOy{^FZz@BB{E3G0Q_?HDO;aBwP=??#G zR;j^dQ7ZKBr|cJuBYO2tmJ}WjunnmjZ|7M$-5N~)NmFISZFQMhrztoZL**6fpYHJV_TYq(zn8? zFRkPm0u8$h2&7z2bF5GAhgNbQ^}RqnD{45nlOR$1S;l-p?b=A20U3Fp%00?*cwWhq z)yc!%Dc;?AB~NQ9Dc;F>B|Ef~+1@D&r7q87Gk|X4?l9PUWDd8n5hLK;B|$_?D7rdx zxLb`DykjiME6LWzebz*i?FJjG{D z6b?>F+GCBNDb|p&=hmwm-CAvBi_Y z>h3o0PBm(HpEOE#rZfgho?_IvGk9w%`=@pU7qf2lw88(99ctXb77J1QxLGMn$x2xo zka6P$%46JsMPka5cL(Ff(~KKW>D5m$ZYVwJ$*6CqP~W!4Yn!Y=qhsrhMs-hpbn|RO z!%OJ+kqyV2%xP-K7(h7+I&XG2%QhC_V!d zmJNQ@-JH{M{04{e*1knxs9R-!EN4l&l7C- z+P~B?z}Ge@b;x%xoR1683AgWYp-HuKuvP6A>_q!I6uz8pB{swpjrxXmN6{>>T$3sp z^$q(StSso&yVy~;5T+#H6D~db4XxEE>1;By!k=mL%Ce+`B8C(=vYE6dw}aOZk|XTl zMtUu`0}zkq4jI)AZto)%0T1&JTsWQxY)TH4jgetW%|mW_MFy(iQpXn_O3V5ge~yRK zvaaLr0sel(Uo(G~HKb*Y;ZNspGk;g{XSs#+XCQ5GnFignb3{A=?bw|=@09{liT#z;ZGe@yG* z%mzD;_m5pDYEia?5W6m_O^dc)%6inhfmNpo8l zr}DWlc?wC9uKE-%c5h!^#puHWaely^E(h|+}rylg|QBwhPy`*`v{*hnm!ce-&6NR-YC`oNj6;i_S)LN>%Q^C|E=Et?6V9%Nb^<#9Wn zp=-3>&{6u|7U;JPd(D_Y?-POEYYX&uL%T>N(}fAK=>ij7<_gZdx&9YsXuFi+-)JfR zyHM{%@!yRIFA3g}iH9CuO^BW+818Y@hO6;$FiOvOe%!8a*x?8|)AO5A{hLIDOK2V> z+zv-zNlf*B91;EmB7C=Ay<1d&&T0_Ve^=Dks8=^SwgoS!`s3=URDYu@rIDGg8==Qe z88y{yM3`IJzn8i0g;ak+HbDR>k(lbgM^t|%6U>{f$?orEDiNFP7D=ohrn*P+ynAfb z-yWyEGSl@9h!-_8-4Qj@y`bvP<{t2y=ToY`_w+|pe_N7AJm36h@MBB!oow(^(!BRP zX>KB9-tg2{i$M07O`)t`$jYZ(MyKbSKY$pF^4Cxlnbel-j!!g=lGk!i=CtQ@)xIS} zh9M$Q&WOus-|o@xP=wXx(HC>G$0Do&&%-y!lSNnqn{F31fo)8^p*%@#ct3Dg?_@;j z#q&1ceN!SVc;BuFRKFX#8#<#ILlt@2PR5mlF@=}`A(@;YYvA{(OJX!RA)iFY} zREmMPO7T|`;z#T*HW3ZPRf-)SON2_Lz%;YtqXeM6YpE2cluFTeu1e7hI?N&O4@ux z->7|?esXNRnk06vFPoy%g(X+77{E+-6Gi}`etNMRhrQt4PL7;m%pi`~qY|b=9pcd! z)OwY?f0lZU>RzcG6*I%(?!-c93^e_ITFO!P38N`EFAcDQ85bEP`)E+S@zG30Gr=-^^cG`)eeFr?v~Le)&kdC9%|AiHuKSY~rT7loHa<+X z2z^7_OZfJt(BzaM+(nHx>TwS=xEC`UvSyflt==#bo7-Wd?Te#&IJw}4YMe?N!?)Jt z=V@uJDRXbnFK@j61Exsw>s(wBb(~GPrW?z$XWxN_)hVsFCUH8q_mQ}Tt?p-{E7j!B z)lzSVvxX$+$@_oBa39E0LC9p-Z!Jto!Y%!5XU&_LHOEDGt-9}XkStkc>= zWy>9^t#(awq&c(&c!+8@eD`EEyDfZYYJanEn#1vng-TX+YE$!Wn0d|8EdLFjAK@nJ zi`4$6@hnbtBQ=+!@=RwR%&^*#S8Do-M8!ji;B0dFXI}ICP&5{b9H+zFMu|q9QkAuO zsLAP1(c4`^O)mL$4mD-TZ}L#nbooshYD)H}vNy`*Smk-~NU71e&ygV}X^wkhYWT7P z$ow-H3Q&rbk^?2;o_k2kbhm}m+$~}9BG__X^}F+rX&Drh#LSSNWXG`AC1D9s0aXP~ z+}r`HCCi^wIMiYT(_#b1)=q%Ql8vDJPM2_hIMfTA#(`f5AUT~xtPxmpfQ1VE6Y~#I zWv4bFQ{W9#GmuHm0BRhN*+L5OB19|0T06$w8cudM-y`KZvF>IHV3w_ayM?Tq{n!8H z7?TFo9eq&Hyh>rJw5lRdJ3I@Vsz;?2`ZC&k~g*E6L?-qjCOKRV3+$PC*)W(B$M^cMiQ`9wC5Z)|j zo0387y9&smW|u$N-7?hdl-fbpP_rTdpp-M>E#xTpfLFSKN(Dgag+u;n!~mL@e?S2S z?$=wmFSKw!U4pSw`?{PX_pFu8B#tEiMD?5OPiNJ^lj*1d@p^ku`vh=&JSD)GLB^oQMM=rDJdzceEAGl=1bR7V|k$|LVDf-?~pICg+eID)e7g z8&YJS!GlNu=u9L4l&55mBN+#Jq+pIOWB6!Jm-`6wySjhkv)Rw~)neHVTe*e~5F8P@ zHqOMKlzCOG8xq4Rg7Qwzy_gBVP6zx;<9vHq%d+@3EmH&+tlN@v0yYJBi)S;0XDrdB^Yc=5wUHIKGlq}lyyouCf6Y_(10mHeEIR1YsM;sL90B+U zTLE)ViG)Uwtrh7_XhfF?bAFv7=J`z?Y8L1ZB#BHPL)26SeW%fs)9RnOP{BQEp$&Gc zc9HwnfPKIQ`+x$Q2yZmZf}89LZhjfgt-(tbDymWzaq?t$LAJwF_jfcncgRv+STp2l zmdFai8bBLNV+ZRRMObk7B?SZDA=h)Zm^kEoPJzR_WD70aB|Rsg06&2_;IDQ|MjZBE z17yNvQ}`|9HzWVBHjzX!I*#L)!ojkNp762~7DSvY^iP?iWChDDg(|fO3rQ`~LREpd z;M|3-94}jIa!$CruoaJG%h+z|g_r3C1V)y5BS8#AV6Y)8EBB}+{O(5DSqb|4AMJe#HGuEY7RFcy$j@6TrZkMTh6(Z7ld4j4@q7qlwn@PZWe zQAS9ls6WpM>_hgsjJDyfoaUUvVgbpJ)r23>G^5*W{NwrjKosS_x*hPt)fPX1eCe$W ztp9>mii@)2I(Kt{Ve5_#e9 zU%(6Pxon3*t~P;xrSv--HKSvz#SyqL)xIP1Dp(_f;UVL%Cn=jX{2I7TejvL`$W2>0aFaPy#pV^6ofirle7x* zc2jAY#Q2%_!=Y$!V^XOZdX!mAWX&3a*AleZMR1FsJ{?=ha2jplj8*c_{NX={14ZJx zPke9sR~sk+HTG0~i#FY?55}D?PLgMS`58Z_utfCr8~;RzxsB+pgL(J3v;}O@B|P@Y zKgovkg`S@u8M^iQsZRf;h1;_h9P=+=QP}hIqfNhYxjXZG8JfE&dZN;HorijRj3QNc3E zo|u(j8J}J%AkGZANFEP0@Qpp>91Re1dYP{iF7`e>7ssP;zq;bP?jT*#mQe9lcX$1aBC8~YGOM@M^kKe`kjl&ZfN}~*_0dee~mTtjdROc zZKq1{4anwT>HG!oK5L+IU;_w)3fioV8-<{pgkeUQorZC|iw7vZBO=b8Nhg9oHpLag5C6H7I915CvX&|xPS|}(G%G0GB&%0+Z;8_i{{D_@?HWv>oMd;M`1zsLMrXe|IF{OGI~)cqcH z-K(yR>bhTD+tu};x^}7Sadquh*B*84Ro6asHPy9WT?f>4P+cSHdPZFxs$QqMy3{p8 zU9;3RTU{rs>lAgJuC6oH^=fs!R$b?+>pXRxudW5^TBxqKs%w$D-l49=>Ux*Du2$F2 zsq1=my;of;)%9=Hb)&la)pfJFZc*17b*)p^26cTzT_0E1C)D*xb$v=*pH|l$>bg^1 zcd6@cb$vly_o(Y$b!}AFjq{{;{#IQp)zz=A_p0k=b={(_HR@WYt_|usWxnK{tggK` z%6+4{?pN1#bv>xAUFv#VUAxt_M_s3@ynX84RM&oW9Z=Unb&aU&8Fh82^iFkkscVM1 zW~pm7SMf(6R29?T?XJ=~Ifhts%^ujEJ*IHM5pA4`PW!iOw=W{JFCVw^vF1KEAII)NZBSGTb&iS+Toy@*L}-}tqub|2D-g|QFvp8k^)b1ajq%0y zK&zMXUh$%ouZ#0>KhB-*QfKQoJIp5}quI#HnWo}KWXv$!WZx5R-!m!9zGrq=?MYj= zTKk?Jyy3o%?R6&M&HwTP;h5lV?2UbDu=z_><;h8X93w0Ezo*Z*i7D?D{R~9$!PJpg zIe2zr4OWBzXWw(VeUCud_dG;!-t72VT(a6PHqGE`1~5D=+Jy~yqoz$)d2>KpUsf86 zr>>N9E5g0!xNS)MIq#u7oonh~{xiiLfFoeg5j`!U)tAZjGTBiPcn-56+wY+TgDQw4 z+fm+G(HL!yOgZ6>-^bF!NjL8~@9TJ<61#T1S=_5E+3C4#Y|5KOXGQ;}IQ-lP-UA}2 zr7eFCH_B};m_+sYAwum?FFGOVuxF@Qz2Uhxz8od_kOS+8ah|%iHIytGg zC3a=ss_9{6=TnP1m7QO`;yWkub~&R<+t^i@Wzv$+2p;1a^bRZuU zrs<>ERIeOve<2c9Ux_WA-B_XT7>!6}^_#wF;VIeCE4m1~vFfLx_PniPEcU#yc(GZ! zM)`0plS6RZx2k8^`E4}##0uEDW`zxN*5$R>ekqlNe;lyW)L{5Gd>TcIk%Gf4F)aLX z7jid>9Piih%~MnAit|_-9~f8Gx)!=yw}6b+lSMa|wW8+X-T&2nt6Uut#l*h|Z;UEy z-Bxr(S?h_FJyNHv_j{g>Pm7v;H@El5jycR-YL(e{BMK4px3x_h5EESGgzaf6*SoxuDhp4n7bQ`eH??H zZ>q4B-zw@~&J7+Vx_MgtGo0|9`c5}cs7$yC@x(@GK=RUxO023l= z?YrKz@A5?b{2VrliFh9vu3+?B;q@^o19lBSxb_lrtCZfl_5!k3R?_9%hbgn7SKpvc zGt$1^n)U}j)6-6wBnqxbbrjMiYdEL zR#b$}QrU%VMOT+yxU{mj_qF(XyyLLCdsRjl9SV+kX$3Al;h%M{O&L8g&U+3{4R$+Euf180{aoPQ=yG+wfme zm(iF?R#tQw&D~Hvfm_R~qq#K}mgV>+bPWqOzxw?xhHo+9wGgoLy3QjiST5?U7rs$E z#lEL!IjSmFlA?|lXRJLhR7bHYZ!NkVRk^YM+VfQJu>ndjf_#g@)7y^$)b@y{b1K`6 ztIN)B^ym#p1U}RC_5*DjlVfa5M0q*9+0j_b6>P6B+X4;tv%=_0vsRAu zEyw!ZmE-JtTw!dHUt^107JCg8MO9vg7Wwun?9jW-{?HT9G79V1{tZXp*h@%pZL44> z!Iu%9@>;NU7xB81$}!5^Y&fqhQ~wq>zJ zyag+=%VMuHt#IpJ0n~ST4JLyEHv-``y8nKk_TT>S!>|lVrj^C^#YafoH=t$nx!N}V zZ?W-rRAhu1jlT}Rf$*CIJVL)$FgGY+!Vf7MD_mg}8R5dEXVv*CwkU9w*4tKIg<7ZI zT~=<1OvOFrP}n1(Hx=4HeUx{4B)TXOSlk-<5bjRctB)1H7j?T2A3Q@H2|76NShvU;)ur(K&VYB{Ym2N+H=3-?U?l~^-u66=ajB1GT#`QQ}4+~uXRP>0j zJ1VYEgExv#*m;j1O7ml5O8X;pXYARA1XBtYV^aOP@(W?^*eVi(U8jD0Hu5n-9PX|aohzn<-7&FoT>1Ve#B?o5_KnBa$)|*& zSzUMJTYuHh7#*I?(IVuu?C<<4Sj7yRe@dgBx-kk=^^oJq{*H>ycT7Z)Y%D~sU@W>~ zM(#!XUfwh(cr#{q5fJc@g(dp9HM8r%SAo)YpcmPUUSzXxLPEH2*3R-4*9vT_*Xz-q zcvVXCAhbo%r(+mr~9ZUte~c)6;t*c5ph&=ZQP;5rT>Sc#L*$Z|s8J_WO8y z3+u2@%&1kDAr}^s(zc@;a&%*WBjMk%Vuib-veW@9!@Y;mH9{?japkB>x196GA3`9S zIm(R2Q$R{9oS-%3F#bR6o#kANZOx9x*H80B--hMo(<1=RRXc~bAkVSuK78PM^fJ-E zyxP7ecaC@J{OR6z;u{F^j?KvRH(=vSZ=LaEpcfeJY1^x|?+If^+VNC>(Qb$1DWq!> z*56>~6zn%jmV&L7`Fw#!GjO*;&G6RZt6}0E?~PpV*wvpsa`b)ieleRpY|YtuHojz5 zQxT5u4%yK3;^Q41s1CIM|8G2>ZhoOZo=^GzfBbw(6rN9c_<#2KbZZkF^!q=b3i!YF z`Ltho%RT79=B>p<`H+xW)zP!+)~1`pv#Rl+8t6@Y-)B`;5K({s70;{7+dQw%slxY# z%ZmO_J+SVK|E>pC+_=kI)~x7+k3lp>U~0HlLm&Oswc^u8i(bVdASD7@K1OCH+<1$l zlLDp#BQt^ajNKaj4$a}ETyK(n-H#!s-|zgH4)?;UK4s_E7R_N7|L&@(c=+LvL@S3R zkjLBkC^Mnz+Oi94i$?Q7DG6N#%Lv%bc)aP4$sL%5PHwli?0a6f@2TO)Q$3avj?#NW zNQQR`^uPL4txf2Ei(_JX(nCCY@ZN$$v5Ut+t|~zPJUOp&h(7+La^qkxI`7Tu8H6x* zniqeFuPUJnhkH&s{(eLy`1%eHsQL%Y=MFvh!XHp@F?=|3c+aI=C<^y}jgOW^6Vlh>pkSbJte-)4FUedzLgl=^8OWfym0Rce1yZ0|CKI}g(pyvm~zfN zNZb&L@peYE_Z;pJm|OtW>`~$_@K*dg5$~qFR(u%;sJf5EI6syv!)n-E+?qH0+;Ieu z9{O__pu#;7w?SWQXVJYK0Q+Mi@RspKY}AXfevAngHg-n;4tnu&`#i4ianE8mI9Klm zvlEPAANw9V?tEbjP#g=oy{0~cyC_&hmsYb?^q2@O5fz;$qKF#%AW;Z(MFHV{6pr=Q zd%C3KnxEs6wv?}ne(KG1;xSD0Q7=x%(4$M;dj1^mvexdLQUApFX&fBmAB>w`3Ji>f z^!M@Ao2bY&&z6p?$0pr1;nutBkt4o}n$_40z15EH{mQPqF<1ZfW(OV_48QkP+=EW$ z+SaG>ysck%d=T%b^KEKA!9CQ&1JRp*&FG4)9}@iuZsR&y*(%UV1JO4JqANBC^vO4f zE=P&#=&e-(U37%c%ISLCkqQA-{y^w&rkT)HGX(TlXDa?Spkev8TWCf@7}3uM(ku8m zqM0f9gW-PBaK5g2V#pFtjA-e_Ltt^Yiq3h$=@2RE3p^3uQ^#}Jl<<_JDYe5jrsXpQZAQpLk{t-27^0FIdWHl95mJ5U+si+l_*=pGsU zA-E7Xp%#64yzC$4TkSuLg1|W7+wR*Zr5q}rAez(Sh}6col*7dd90rVMvvZ?(Ft)Zv z(TujkE$|-UD`+)E?KvC);;`SUer8sEan$bz@W*XM&Qa%9#=_0@as10Uj=v4#_igUV5eX-K#%_77mThrag^v;>1OmJK12t+ zmHw>zYf_--Gk$O@^(CXKu03BBh<@+^Mprb_ja$DBBy)DGKnpiMA9Uk6V+2~bacj_x z)p&uX8)H9o-O!6S>rmmZGcrv>U0;c8t(!;a0v&2=7J2#@`w*;cT^lg%$Da$RAHx7^ z&z}mo_PYbyX4Nc@|AarYef}pUw7!`0sk&58j4w`1i15nfRA5KA0Lak zs=HP=M$N_XLCjgLZRhhA^V#0~2kr0v)hzv&0v`QLsqm@1VJ!VwiKhQ7dl1nUM>i7j zRpg;%SlfCi;3fk-gF5o}q9R7O^Y<@C3bgL;+%!P<%$Z37t^50$Ky*@nfi4s6^DG^j zbb}5R{=R0SsiEbIfi$CXF|*MhD=c*MWuG&;LYYCMpNKc3GmZ(g(uL@5fts-CM*=Ns zLhGl2EdKUkMC)9D-~WXFzxfaRzhl?`KjH5xKf3kLw&z*&?U!$@Y908}0<* z-(%UkylVxVd@e%|vY)xO$m;Wd`c9u0`HTAh+4%kUz5o8>@olC5U~^>ozj1t9=|9Z0 z;{Vq8w$eZGKQq3q^dIrB9p6s-PtC`SZ95;6+|hi@(BOQ`mHK?l<$PG{&iNQ`EoWWu z=zw{2hW_RAG38B8zK@M}uFv41$=wYjb@=+o3cNO8`NpqC;<15F}zOoyW`LJW(gC~!uo_O6IlQZf5wAfT&NQOP$ z8k=&o^a}f)q_RF~P;@|ROSGw$$%1msI(bPD9Z7T=$Ja;C(?VI0E@kaf56SG(vn^=QU zL#Ov#yeBHf<0y4h^&J_Def?6?Czg$}DV|0w1_pTyySb@;T*<-4Uf$?GKT7E|jHO3~ zPvzbXrLka#?|#1%|85Gt^UhM^-{NL+c(?v1h)%?3V=ju4s=%4I9wl`K<=G85T<|>*)>sV3@Lb%_5^G$D zxV2oVpm=6_>#(QwA->VC^My}xd=0v;#f!`-rLei`TAi$k=|QqePg%%1+}P6_sgsp? z1|-;gv+>>L=ZywyWsl*rd{7xe`u)lmxjqv4#5J6k zH^CS1avOK)OpDoT#|7}TF4O6W=$h%ob5d+hZetg-zGPl}nQXoD*$o~iw${KTtMJs< z?Y9t9hX4gOd+@F4Xrz1Y5CL|wk)lpU1EfvxBbhJ(22xCv1(cL+l<}AYu zsmFnpN*D-=KO9jSg-_(>su#Ta`TM_(PNpuVtI=3nrvrqyry471KPkixL##i zKQUP2z6-Nv7`eeX5XNo}xbLvP+yhn2Jsz2J--b0j*k58+lsj@FJ_J+M!+VWhpl?-0 zU$yRSx1;QQ_=?_T=l$-U6?S|t%HhU-odZx}nf)qzG-5h# zfEWHU*qfOkj!3hom=8RpI8zUGNlwRi*v{n59)g7-`aGoRw8B55Io63C`tU^q?ueQM zBctG|DaTiI@*d|}Tz-~48q-K{0Z**=e0)pFg_2dM>G!SwaP>kFZ?wZ=vDw;+q-0EU zwTN+DPf4P`v8(Cbes9#b70JntD(uQ|(fif98h5Yw?5LD?N=J_D>=};7m$4JaVN&Fa z@=|* zR@m%}&yF^7geJK(1-6d}*#0@!-cdci!4@UVhas_zH(`&tTSffO#kgptF#cyS9(ebg z!hVdoHNjjMjcs6KvF`^)Cu==@vAE+(7C~mSRtDFlT4qtGZ3t=D*#7)8?b#W$k>Amt zVQqY3jnQnu!!gYm)3)&@8A-x8+c zS!8+#_NQvRQA9nCR|_yiESUX8*==YA&bZ$}y+1?e_&N59O~nAzZMdnUA7FntP2Awk z=61cuXntvEjh|8DFnL+tIwDm-pTHhKRJN>9{wj z`yUo<2Nyh=^-Q!E=xuO_w;y^-acC)=5eFh#4OTEZR*5Q%na~DzZWg{ai6#RlN8PvE zUmlrKiiRQu-`9&Am)dw4ZjA1Pl+svtJeu^XExTb+#g%0joNlZfzX+x=vB>C=h`x5* z*omnr*-@T9LfPF|_HcHbXfd(};LeCPDjY3F+5m61vl4YXK^&2!9plS~!kl9db@orh zE1YNW;&EWzg4tf|P2JA@a`5%$J3HeBF;<-6ZNNKlws)L9PG{Tg&>za{Fx3Wu*E8>oGJiE%;7Yd{iHxA*VjdM%I#b_9^W z#n+ej@m*ydNle||IltyPc(|hzuGZ=3q8@uE{9cd$VIt$7eO+il{6}vX@mDkcP9Y86 zy5MZ@2d###)<|4S6B-Uc=1=H*F^wO7%=o_u4U9jGiY4M-W&B;HuIF*K_eEW%sK>si zcN2BM{=;7~zFwC>{G?+>Jo_kn?>6&sKXu(>>YB%E&j<~*kU8*2L*@>~-yt+G-i4|z z(nw_dhh{$dQdgZW1A7nbAqx5E!uV;r4B~fxWW=Aw=lEApUFBh;Ix>{A@Fi@8N9U zduBdf<+b-syZ*#$PwM)RzPrXq{|UyQ(e)v|y2*(5GTtLl2T(7j91!vF_)Y0oKre@q zvC}v_o|w9QQ^j>6XV(Rg*l9!An;1Y6Q@1ZqTqknI!2q&X44GfkICLIYDgQ zzI^=}?7L6LQ`cjig|6X1@p|3fCHgh!@`%s@>Y6Q3t$l#v^}2m4^lQ+yN`!=669wvg zH=uaEZtny7HRvi4A)zZ-phRoS>vj7|^=t0?_a4zb3izYCr>&T^x54x>_Tl%fHn8LN zzNN>zllC3bvF?F;-!|gt+>1^y|>AnNKQumCi zE|}4oGOBguq>x$cZAD2OeoU-FH+oyqfU@(gD{r#SVs9&o!N7TRQXfys$a>ER_SnQC zzB@Q+jelLLs%DN!=A?B6kDj6HKlE(S?Wjz$ye01Ea7Idps ze}(P2IA7gR`*&Lf7Myi7U(GH)dP(i)tFxV$g;`0+sYA0G>9sTzYl8FwT9NhgLqbouu2a^s8^ejvcVhBOZ zt^d>|kYawj)tKLQ?-wvJzkR-ZqM4S{E1+Wjr1jRo{K?2lfj)x;t$1%yH!G4^HRrtF zK)bF-#~tZ_0r2||=Sxc4%zw9=FIo9!$I~Qc@#*6;82|fT>&w4u{`y(i_lkwKb`=8tAO>1;bFkOQqQU%>Inem9 z!Zm1)bFkcX1jf_!#V_eOY6bvY8{D#3DJY{gk&BhZT?!DY!P;HEhl<6um;CP70$VCx z>{hqb;MEZpi;HaOt zKjGPq3s`N_#245h5gZi6^|ej7qbb29!P{bb-;NPSwD{2MzC)*2XSm>VAPbJ~=z&jN zyL{_sqAAenE>Iw2q1@R4<&E70N@qM*oj#`3k5W>95!X5xU_Q_?kfXM}IZmkuq!*e4_ZKZ6!EFoGmYDj#AuL9-e>$$zZHEs(G6F(Jrps zjTD@Kjmy2g=u0l|L&7t#U^mjp6um>dIE0~NyfIWVPhV2Z4dAZuzE$>#?d%6vyezD;l}eP~qcpkfDZ3-_6^!HR{)p1^zUjk&=S$1Ms>=7taE1luldb~aa8g_4OH0VN z-@c9;0pzN+qxFJ_Ou2^K6kS_DqOe+;ITNFd;+?Hu->c_rFPd6B+`t$1v+&h>G2)D& z9q(gI5AR!k@(gJ2F6jM`sYSjI{_ghnVvJZI6xNCQ!#ZvK^?1kO2_Hm(3dQ8PeFe1A zyq<=03_}>y(=f=C{g28BCQ=?I`o$o*O?|Su1uxsNznV-53M&WuC#?4!y4olp)#1 z?8?3Bl1b3otbX_cEa+W{JuzeK>-wQ!)mItQtRCQ@#oHOxhR$GAn^+p$#|29l+tvL% zSx=3|GCINUvvYKFO@1F}(<0o2B!Vr%WXM`ugchR__J=gWxuOyF3g5IigHbE*(7lA! z@+(x!UN?8z3?FH{(b9f2zUmdkWVzG_cEW1mXu{3D)P)aE_$n&$dwZ1F>t4JsQI{7i z9KC#u^1z!86&JnDUd3n_M~d-3?@??h(i3lJyp1>eci}K^!$z5~Qi_h@pD!u^4t^7Zo zt>@0zV;0+7;2ZDB*ObNI?!cRG^?vOyYzokJq+IaCs8?nL_F7q*jiEza=QoRTFgM`I zUOYNi4f_-K9Z9KkU(LB0u{%3n{=6p@6Hs?-;npi$dy%W&bhp?HW-+#d=@Bo6ATdmv zf)B?mU}^wwy-UP4t`sWp!bEpS=U_(_RJ3pe(LG4HR2+#-fP1Oy(gQ{vpK;c&T@M{T zcTzaD@4n?M5;c@}#Rcv-<5I^$J;p?#?@(ly(dqH$B1&73$SsU6#Y;jl_(J*OFqi5) zdkNwwtDecTTI}oU;XZy(i^i=_z|sBVu_YiL5EbvRRcI2kvKISuv$8O&;(ynR2?zK6 zoHDtB(MTvO%l|HTndK|#61F%zSM6 zb3tX)I5Zil@h!Q&$gu)1bfQ#y6B>0ha;mPHNG@)HO^w%L?-o>sIz0D_EL@R{ZELy` z`f(hfC_BE=F*^@?U@VR*Xu03+P@9LI#RqQG*GKYt{zdekxO30Pa42$k3Zu4oJoBp< z!@%V7DYf&kv;I2ZpGpRvN@9wjJ;gq15i}fDdc#ch z&~)(~CT@g-{!V$zB~N!XOI&6y8c(6CWJ~EZ>XzFbMcvhr!3&4jE$>u*vTyCl&fe(9 z$^fvxJRv+1-*~^_u=~%AVPmmI+KuIpqvqm^PDSqCqgHwzUL1b6_gv!J*JVd_yJpo> zXrShKPc(jx4rjjj5RlWIRjzh(k1kh7xO--}Z{kUGx!T#jeiB<)?1)reDxu3o`>w&Y^l~fqq6R!+K;ytgf<^O(h?dxU8!`P}YOhEm@w1~6#*j*e8 zUDt6*x>_(AT~JIJMXf*Uai}vVkeL{WX`kUL3)$bP;&yZ>FblaGFy2Gz z^@-pd(+fGOx)JqP-xPLjQw;bFHb7+dH!Acg<-R7mNRID$CXV8Fsx9MDm7R+E7aWa3 z89YsIJ6=Z`S-E)sssh!A7IP(lZ#_j_1zbzv@3nz_jh40kNSv*X_vM8x4#OK`H^M`j z;nio|3l6Vm!nn$J_Gjjx*Q|@+Y*JO{QTO0mTZ25EMotWK4>`2H8Bpx~)Y+xJ&1zAx z{!2vU_(Z%5s=mVMNTkZGN8pQH%YVb{D2?6yy=t(Fh)cZyWL%K)JOXx z#=D+4(fgRK{`k(o*HNZT!(E$CF1FeWoytG!>x=y*J;@tU`&s9+6A~iBJXfo)j3Y1f zM)eH@K^2?TS8)!XPR_J>y5Q>_Y;kh&mR3tA%+w1Nt>u5~%egJ3_HR&?6dC63GEM!_ z3n-(-X!#Z^+_}l$Yd+}1x1DV2ub#z~KtwO6$e_oj=d0034{Apz_LI)uD4*UeOzs(3{&AT5RV>{o_h0gKRXYjs#-FbB)6A=1 zJ@>wQXG%-SmFhWo2{P(X8&YWkJD6hUtCVx4xHZoM`?-x>SNF(OXCj!RP8Y?28%l1D zx-+yD=CTa7!exV4L$RU>cQ$oDRj7M~99!g7B;iUR@IYJ29n&tLjl{NTSEy59f!-=V z!Dp*Ojj5G=0m9oFuM3F#FKEPPIrQ*bxZ;UU`C31#OS%$$f$ky;qLB?p(5t9t2f`98 z0m)Hs_ri~0_t}Y2<*X!^{=oCuw#sYK&Q#8h#GO3G?{U!O1`p>Gu%nW5f?&t5;t^I! z?(2REU8ScJKZMnO&nfjjDl~Sa7I&sph{MJ8yN65d2}y4nwkC@<_L&yFS$x(=PlYw$ z3uwo6EJNdV`Z?ZVyni?LL-T`+XVEj%$1NS+&Uk5KRe8&3_q7X9R#(Ekyq(dKy^BQ` zohVb=g1t8|G+{Y!Y}WfvxnAt^?LCUNXL52EG#|B*spZLv=Uh(e;z*Zz!}WTzg=u~? z#)pRsucY3=ZnO^aiT#qhJlT zPf^1HcEE(ZyRidvvZuwqz6Q-Q?7SM!+@Cm+(ni|QT5wOsWcgoHQP*MCZF zY0Y^1IWk>*cCsa+j^Ca?_tp*d(=b?cTyIG2<(X&v>iJPD4)(rs)sntQ2`sp=I_sjk z3}-01JCVDzGuj8t%aq9U${sL3xwGeMtTOZa$JooK;IKF{*DuIXeXvnCV;$D6>jk65 zBLuY`FoBQro$#fH;}dop)q#VM>8JTMKMPF2PWzWj{(VE*T z2aCei-p3XF^AGX2V_!Yk`|`D>y}0?rQ;?Mg`|N?mC|{1R4h_k#2oj7PaVRbHP#enwsISW+Q#0{dUit0HBLA7hFTYM4IHa6+*@a; z$MPy{#VBqvuu|`}@)I6+-f7f!?&OU*6HJQmX4)Cj<)v8Cc$J!w5~x6r(EC=_Pwcm0dEo( zHpIV+=4D8E%h;ms)f$LFkmy!m@6?AN1NP5F|W#Lli;klX_w zUabjVfNzYq7{}(U_(Q&#;TYH64t;2?F|5@cvMuO}WBl>%*>xC{R)4=x*xUG-@CP)q z3$R$a5j(cN=Dr+?>@VPxey@6(7bH8}c-{@%{yow7R28)0p(qpf-+3oSa?)b4u~s*9 z2x8P^5r+qitH4WdMiHv}AO_~q>Z471Grk_5;|-tL*bQCgSM5Kq@l8;|OwSb0z$-3C zGrItN0Bj?R9fhzos_4sE*mIK4DE|I@?D6;KEkq36%Ksz-k^#wpWI!??8ITM}1|$QL z0m*=5Kr$d1kPJu$BmjD9xXxG3+WGl});uG+8t)i3t5JG8>vFZkQoN_<8rehu*|@nb^qzRWT$ z26!H8nl0&+A)R!{c_&>&Xf!p%_J@XVGuos`x#OpF`7(=qRf*s4hhStt#rYbja8boR ziwfGi8S-uNca+c)LN{8VBA>(;gyOdlzk~R^j_}p%TH5L_B>oujgF3|b3&D>eUbzu? ze|nqTA2}lUK6uWLV|ncot+Yf0*Vf+CRZ+B~=Glup`*<_dW;U;_ZQxnNM?^MhcQ7#= zudo`~1m<4bk3{=1(JD)#OmA~JM6Ld?h0qE@^DI!Eul4FBzu%=Uo_N83XPwV?3}@O4 z>(2RozJm-N5y2(~=ThzZW&uwTaP#ceHfMd?ob_yT7SZPHn>0O9>eT*#luWCuY7m1w ze)h8IU>_k=y^~pZ94dk?&HTpP8d4s+2u`>O;KDXwh!L+22*Ix*{vP7Tbbv3fjnY27 zAw*;{MJgx~)kegdW^<|6hLKL`ew2ZQ;+;9bGs)L?KzFqj++4h#lw2nJ(< z!S2CecrbWwd>|K1!QjWi;Jd-#>%m}6F!+2h_*5{sB^Z1(7_1BiR|kW|!C*l!I4c;O z9t@@jgQJ4M!NK5-!Qj=wpd%RU6bzmp7s&kSVDMBh_rXDoif|dV`Kfx`c;@}oO z=nT5k5pPQAW+TV|dxZqrheniph+K7r&S)Xh# zu&ILoMr+^hF@ka00mhm(^R9Fy^~Br)JZ=%nvMLWHM!KMQ-w2`l&BEdNs;*!S<*(to7U`7OLaV`Dcv8SKD@6bRZ_o* zDz;L^%|R7Hesn;77Uj)!S;#{vbc{l0021vj+s|)aW;!-)=}kz4_qeSJ?s2;|TBPaL zXR9IY7y#)#LRxRH<0x!RQ;tPn+>jg~2*D?yDNt4n_}~=ki4e~_y$ARf;&+AO!3TNx&5ME>!;cpFjI%TJszp=>Gs5~? z)Y6R2R6A-Q)Xog375Z%2^SVAZVOLpsWL26=vE>xIE+7`j0~1>Hpd}&lIfxREQ{wdK zko>X|#4$0`pGl_wdEo1b-ye$iWxlAz{RJ&h<`K;)j?ZaHSMhwO=E5#b=8?d=a@nh}0=2zTS!_ zzekC$Cw^mle3Q1>if0LKA^r^UdF}Bwt$=v8qxmUq{G6uEr)XRPMEeO**2j!}%(w8v zHw0M-I<&7ovgC?Z-b~T?6xE>D@EH0_{d*zLGyXpUxrxaAL8LA~{N_;nQQ}`Deq%>? z8y_8*pT|bF%g>_}ZKmi9Axiy8q52C6iX9BlppNwK_|VWlOxF)ezIoS@CGs|aDrQne z6F`CLBKqG2ru;X_?d&`A2gq%p+;c)sH1wE;?Xe!D-;l`b_%<5j2CK)6fviykTFG6;Ds` zKMQ;r@%uZ%+pKtzf8uu%zp*3y@h@Nx^0Aw8?3X#USM;dz+5~N%8BEo7nL(Gf-3-pr ze$JpZJ9C&^1Cw)xCa0IDDeKT?nL(#EJygw=R5KwFYTi!@utGcIOd*GMjTv-mmxs!~ zznd&k{^xDw)z3o=-bLh2A`62^y%?BYwG~hP*ARc4_%R*fE3A0(Kb82%p}^Za#2<$} ztbe^m2>*uv{rR1c`VmDHsZ?OpNUk5+oinAQ@emD(mP zS}FRwnZw%gHJLIEl!$Hx+XQRE*NgZcMmp3>s12JwD%`e}gUhC}(THelL= z@@Vaf)}gwe zH*2#X>dS1VFmT67;2vkHl+-@* zB5^Z`D-k%8VPEDktzM&1r?eUz0|n3Ls)s4GfkNq~5b>K0{H9QRF7Y+Q_c8J8540Rb z<)%NP{hMM*g7){L$Qg>9g3&C$Cape*U;1H4yKv_Bz{e#4|6BmSIf#Gy&msB`5`Pcz z>kNFec3ic9Sl(L*svsy&fJ|}>f3k^+(WEI>Kcw*sDfc4fZZhTQ7c|*S89{tA z@#lsZPEI~EfaB3Acs-^iS>=u)9sg618;}gS{id90BGa)Euk-uu-;AJB`$`XLldLYo z+&)Wf%c<@DP;GqPIHpxYlE>x9FONH@$#f>#Q%DUvsbNs429}@g%|LlCooo>$WkV@? zlA>n@>*bB-gSP?V{h$+~Fm7ii#i|=geDp}*4;uI;ZO0n{{h9BF_{Xu|0iQwqqXGRf zrvBd~w9~(x_!8ph2lQtI_0M`I#C|vNyNMqf&|l4dKJ8CyuYUsZr-+XX;OmJuGszO^ zOOPW4pcBww7Ogn4d^d;UPwxPJ6!E({!nb@1{klJVZU&v&CwfpDDZHggdt01~uK8gz z=+s`+gIcuJ2U&1GV48E7W{NdUHfEdAkD{NHQP#{C+mCe09HUGZQ-=8C2Hs`G)35pv zZyN>tdxH!fnEyCoQ?sUgX3*h`UJK>472TP5eyY zeVJReO~@~gaY)CPxm$CJV~rMLO?NBwo%k*EIY&d^mQa1Pck@xCPyhVQt?m7vq9qia zFGQLC@%uvTT|>|gg2sgECw>R;zRVh}?BDg{OvBFQI1l(k4eT62Jx$be4weM`yX8&8 zzD~M*XhMcj6vyt?tg=YX{01r#Zi9;FgbI!q6d2_($LddPJPs2)pWugs>Djbuq1>i5 zd}6VWe)2QoHxr*5#G^f@eWqWtoC}FNNZcS3*T$bh@neW@Cf+}wU3s6UX-K`j;b zupT8@?ILgf?T{Zj2J-tmlCLk&`RG4qQhf`<^fde|95O9mnLBhbD*n-x00) zB=dDd)AM&k+ktaa+oT4lDx`u9R4^!1fj3QcsiTE00TEILjc8e8DAz$VWtpa%Z@oxA%j~Q0~-wydL zkk?avjGB&7Q?{;2%r_mfq{n_uAyLW%pa!*1@03;@!E~Cm1{?!+vJn_TxuKLhccYOf zpU*)qzQL3`AHil}ywvB5WYly^s#Tn6gMzcaG7W7)sCqe7?KD*hj(omY3jfA3gQQvXu>)T#mgI4dQ zzT+;nzo8GotAas$Th#}lx<^2DBvr?zLG?kvSf15d(kmqEpq3zxsum}X4VqINKhk1w z4DkLO)b0Nzl%!J08lglmrT@qN=*)xEr)qPAooK&H`&nD0Nk^cfgetaE#i%waD8Hi* z<#%Wfp=YyZ!!eNlqYxegVcoxmQO!xJ`37kSzMs>M*HM1E)_`L`j{UkW0l9Aj4;4^t|GQV*L_yk8e&6Msr;?hSm}Ntf!of%#=uF7sJS z(^3x=>!3n6{Xr_*KxHFMWy1brnr()zZ>yI14d}at`WmS3?|=vMqiPwk zv-H`nWSHEYjX^?U@on*I&aQR$H0X_2OX9Mm#KBhj=Pm-@URU!!@f@-ISuC*<{f z{hFFGsOes4q8}VT!2Hx}sp5ECJ3ii$3A$$?Mb=SdP@BXBAF8$)a)EN-c-0usl>oj~*`3!39|SkIK% z7SCJ$Wa953KB_Gq_b=NPWRlP4QQBYjgHi@#wXc3<$vQhLU8!O-RlExQMuyROx`6h5 z*lc!!OBlRsek(e`s}OXl2m3PPrr}kDLQ&0TGtYYvsy;@QD-4xb(RLO}KAlBUE&=kuw=)C!D9^RszOQ^&a4rnwVP=0A|;IHuXcG{cc5x&buW6V&w!q024o zk8G=Jf^NSO&~^4_&^3P&bnO9>l<*<3`j_NW%s2X8CZ7n#YQHvP%_ z_9t0!EF=G8z?BfU-^2y&E7Lab)z{Caz7iY^tXiUXdJaNVukZV)WH*&OXi>uZVLiqN zX=e?`2~FQ%Kks3RoT5m&DMH+4fj_2gvf`PaT;gLgf$wADylci(*V~-E)aGngo3m~DnXYg) z6dE1#f4`G0Kq@AQp7poo z4)DiD)u!E=6{LO=8`bA1x`d)`A&O@awnv}NWR67Jc3ECf#B4!VLrV7|H|4ideyA=F zf5^xU$IknMw&*8* zKk>_nU(*iHewOb_>uqBahr%{c^9xq(rG)OX)L|{wq{l3zYRO4)}n-@Me7aM}RwBfbR-{<9^&^Nw4V=$8l7Q z#|6`4FXb63m`nxBLKK9`)7~9zq$5BwhT1?}VqQhqz-zldp@U+`T-3+~X~0kI2){WLpGW)* z;`@f;MLmqt@+qSCbMCTAGc(swx`NUl_i0N89D(g~oK=GNz0(^Z@f;;~g-Xz0T8>!q z!}euA@h6F2(;koZA9&IJTjiL}!<37j4!Lwwj`nOe@S8&Mxx{A>-zSJasTB~{LAkmR zAM{Ha+M5bWsNmh+Iu#ws#ZZm~ZMPc0f`0cA$nU27PYrqIOMN|rk6na)L|CB#3skv| z(%S*~pq(tTWmI6xf`Wk|3OEn23V1#jFBiw>v>iu+E$XCdh!3Lp1d5;T6;k4%=~G{v zNS_3T2O5%ZnSecTAu@2`0*N{Mn$==H@4e_ll4VS?q)n3Kryl4)`@&+_NbpXAr?vqL zf5iPi6rV@@apL=i;!Xdjh~fWwQ2X)g#$9TZmTq31tPMC|v4w4S1a)7e?z(7j1tpq| zl4d`J{nHa0fN{+LW(zUaA5s3VT6D7c+C_W`@r51Xt3&Z?h~Gi{n2zujq4=r9A0yu0 z5q@SUzAy1gHt;8|=)nHCQ2c2x@B@h7)e*kMdcS0T_Y*&p_%$8j>qGGyiC;(j)Q<3* zL-BdU?YemgyIhq zKY{p%gLusES^EcU@3#=QoVYv_XZjHJ?*N|mY_-a7p`{-joK#f$qN@owU$w#OgTDh{`TkAvXDQV-d!$fev4%Jnhj*i02rf4kN?Pa`#OVLuzycX)xN zy6+{uwLX9dww?Pdy$ADYWpxEIc7z$b&C#|1jj{a41SJhxsZerC^N3@Ow$!SOqyAuN zFqO@j31wf}+bR?7>jwA(`Gx-*%Hk|aCRMQfABKjB2IjftAahcqHH%||ri$Z9 z?Iex?KY9*&o}`|W)U(jk!*r{yIJO^ah>N-txG^R!SU+mC3L~8|aonw~633I;a;u)5 z&@+O15~$}~4f2#TI<#M!L8ta(J*fSirOeb)wKbu7CsJ<<^>#M& z3MAHlh9YB$oP0NsA4LVoFzQ1bz>oD{W+{}fw0~th4kmf@B-Vp;Rvu<8z_z^wHtZVXO^!PQ4QJ^;#j726PLue zg(8A3X6u_83JhiNmIG`}c59!AsD6xkPXvX`5fKzJwFm|%;NS)*SYM88B{5rR-9)i) zo~3$DNPWvAs)?w+I?8Y!QGOWzVj3uvIpTOwn<rq3hCixW{RHN>UQ>YP{7(z+=m9B~_kKz}N~txvlv&z3zcJg5^^}oX zDO4FnU&Nr9Hrwk5sHK5gl7m{9&*M)77mr{ARbM zed6=TF5+Fp7j}d%0G>VJ-F%PDtN|PsSV_5clpA5n(VnDG{ZonCO`P4tnHdQ9`#aYd z{6ImvH6ilUg9=Vj!JCmG{$ld`lYqQU>k=yeW(nkD=0X0EcJfAj%L>RhYb%RG2zrF_ zlPNzXR384bneRuMys(<)5x$D>z9DeYeiwwQA4L3i;!j7k&7PP~1*g;0KgI_6d9NtM zzAt_N1;?r2nGgl^@0Mzdee{q0gh$Q?d`$>k*tg26p7@Q#ClEh16wmbmF(vd5RZF$X zv0t1^x%rgqW6IG#W&+1{=;ROdqs=Nuzv@f5t&}?v9?A#gTm zmg@F3-*2If_PszkB_DDtO*!K0Lvb648$jGt6BqDb*pp$!)1ExyXA<8x6wi7T13aID z=U~SJo*#dZ=O;DAs^{iDJPqyKIpENTH@d1ku7{9?Ysif4WciBBSaPBDGepCy2{76&Q>*p21Z#HSWm+^d)7K2SB*q?ETV}oWB@*io< zkPFny4Y2d@Dx_P%bRRa;W&a?q(tkP;F)|+w2t|+ietINI zG9Vd{3`hnf1CjyBfMh^2AQ_MhNCqSWl7au-3>@w>`Kj@a9bLtf>&Im{XFAlVr>&new|bhyF&*le3+p|wV!k0k46O~G&w#Z=6kFnQVf%b& z$hXAK0Lprv37eN$WU_F*?K#sn(^C{Z9YxV~^PKu$}1D|?^PzvU8ZEsb$g2D;{tvr7A+E?83mrAVk0g$fBv$3<79?M4`miE6IZi5 z#&NEjvI^uB;1%E$U=&&2uj*T$YeH9O;Zj+mnO;UCQn4+9G=6GL*_9 z)R27CF4mDE@Lz;5f4VD|iQ^2kT~kpaS#2=-)hy_pqNI0l1?2##Or%YORlBKox-wiz zfK`birCWg+3v2Xx$NJ~QUm^Zyt)qRJu;6ys;uNjmd=a-mSiDqV3y_mygy!OZO0eFu zjx+=)B3xgB{~t7P^vKJE$Ljitn~&@B#C6sJrs+g# zte;L?S%&`~fbC3`*Oud#8g+?9!p8^V%IzYTy4IzLW$WS;$opWwZo3;blDTKAUkr^) z1g20~fH>;oJ;2Nv>v@o;TLn)`kv7{S)?ud-rwl^u5@@>_XA7Wj8P1t!=8`e=0P@Kk zwP$LXu#ujlx0%eJ?lD2h3Bo3}O0;WWz>m!G9ug?;Wsqf#^MNB<3z1hRQdkaIw%oKw z*S-WtJ-3XdUj52aAWiE-{c3O^osRvATH2O3b(?&T$31)o&RH(xUcaIW_LS{t0b+O;U=PydR&SZfj9Esy zo&-zze1CtSR^12b7rbBd&TX|X!BRfoUoU&8?AMwaiv!l0{*-_x7&G4sP%edn8@3v} z*Dc36!)(QQcjBE+XNGqVy>-`R=v_fB=ns5e$w=<*;PXdwQg;WR4L0M*UWW@u4#2dG z=lC~$7>lsw7a7(jtY)W z95yF*cU*_#j=|j>t8lEqksY%oI6jD@3&)E%X5i?@F%HK}Q8##hWSv=38$S z#w+pob)u!DXVZiAp4DW8*!w5jqWcFw()(5eaSs^?{iDDeH7^t>-hbIr>(bP9pI~z( z{=W@qmQC9f(jcez-L4b5U2U|Dgg*J>1Pg6{<4}L~-?s4`pJgbHfB3sjY5&_+VyzPV z4O70~uT{fCO7J%v_2>Kin)1>El#tSADHr_T_JFXpf#0z2`D2zKj}L_ff5XGR->)g( zu~+|RV*jHWz?7o#Yo^GvVA!F2&kP%|Q2h7HZ4l*9f?+)r7RFYvZ0$qk>(>A9;fl&C z?*`u^8#n#v$3J=Wv7i2I^Ur_rZ;$`-S6jAj`}GssfAiZXcl_?D-|u|-nO(b|{lg!h zdw$Og)qD5->BW~`-d|IDpzh!+hYr`j`r7M9-gxt9!&`5^bL`LWzIXin4?g_pFMmC8 z^5akb{nV$Q{Y};W-q`f{7pI%Q{Oap7-<&#o0{U;i6#io5xi_yM;L9F#D4NaE08!$%|~k4zag z`nEBtbMKuuKYzi(f<=q(Te5Un;qv>7irtZ2yRZMCq;&O~|L*+% zcc=ef=6~$C@e|S}rf1wf>5j>nQ>MD6P0yNvnHY>X-AE?PFf zaAlTzE(eSh#Ag@h7fro)asE8_?aLMv7A+MQa#3e+ZSve=_gD-qt;ly3F83@Kar(Kg zM#;n|_1uN|;|f>hD%lvT;V5|M`1hs<$2(N~vG;Qe{%lU0lD5p9U*udqx43v^VbT1N zPDG2oVRsxmBk<`v9cwlF`>gPa32b;7vu#AauuP3N(=pm3#g{#^Imd*>GCV-!-H z<7^2|h7BA(aPVNF_|rUAKPt++!lg?WVN9{8aLJN;=gzy2@x}RW=Uk7wP`@@hp@TS~ zDf5A>X_Ij=^G+P|vZvfWGtV`3`V8ZI#(0;0s@o2nd){&-D`V`CVZ)uYa6S_$$Y17O zG*1+rbLAp;fwOq-5{|u2Et)%TNxt*;W%HPixsb^-Nms@%$)6i6gHSEYmv|N~;&3ed zps;A+BIg~2^A|0{(5J~ErGv_t){^;7k(XN&2M$RL(TD3g_k~O5Tcpxmla(njdquu3 z8Y(q?;G}_BiGre$$=iYe*i@>n1Z<&y`s8l@e1xd7f-tytY5)uaE zIBan8umt@$G+94lq*TNw4#6=oX;^ZSvILrzjiB6+q~QsP;y5gs_#xuDejK6WhUmCq z$&}~y!ODGwi;_Gd-64aMhExBLgk+H(j!4h5c##;>7V$`zc={jDP5j^?Bb4Hm`LIHd zADl3P`2m{egNHCbOXtp;#{p}cCnOA^eLN0U7T!A~aj>up=Lt!Ae8Lc=IFK&mg?v%r zQVdR;Iugju;330A-f_eraskG^^H=I=4jYDNByxkJzztQDg_zKylZ*32<2t-qoa;6W z8D^ft^O1+a9Gm8Kn4q68!AOG0lcLq!BQQN3brA!{`7wA4VLH)H!eY(8T|bwC@0j>UjP?RGJE6!3OH72(|+b z1rZAz90-bl0-}iG0SBBM9C9F{#)jS4VpL*`V%OLUii!<;Y*-NsmKdXn1`Ea#%l|X` z-n%=ne*eGIVYct=%GSul38kST( zmsJv{LW9zjshYHO327HedK!-H!B?nZ=!)2kG$ofM8JSg*VHwIa6{#N)=hs4GE0ZzI zSs&uUP$SY(0LFHjGES1LOxC1jNk}b7QWVK}rj@241w@ZE&~HXa6nM~>iG-=pQA#KX z)&^B-A}PW+m6kOck0KRni7F*t1p(n#*&nM(Nx^I(NmnH+H5u6Pk}g-USwa^k@Bo^Y zqLgV8XhW1$!LQ|b!e~%1mU8qcc)FBnl!b*QDYL*kJWEND#B1KdVYfkOa9k3TR0<@Ew4Ah$bkd){paYzv_D^?(oZ?wY{Wc=uaBeH+W+egvR$?g8Hb z5dD{cPxIkNfXDH93*g?thi?F0$-5T=3+4X^cM+fdd$=d^X+{GJ<)p){=ELKFg|H$6y)m@77!d1?GqRr>MKXQPhfy=P*`+dUzuERNAwL2<8+#D-+sP* zxi~Cq$gPW$%fe(-FJESKWxp&aG=f$_LVRD}KHBy-V~>6#Dd205Vz7+7sYv`duZA_r6;F4#i~`%UXJO|2hjV>1Kk@MD2~NASH;un zMJXPxP~#yf=|pk7LZyaI6p5UCg1Rw!H5pi8q-fH`30PTS?KT`sNQ2CjCP*Vh5aUP% zinST3si-ZE;;6b*4O3WRrbj4GaHv?PmXJGL?9fWiPA`%I$nLUKBS0EZN>Byn`@=fSO!2NCq~DrkeAkxREAcWf>l^T zrDc*B%34^t>(lUhns{jtPsDP(qw=`9iM5oQ@<2hTZL8YanT$zq9F<`O=QvOiD^njoG3a~#o7{`iq;TqlvbnFrDu~9!ua*LTBF62 zXk?_e#j8>jYB5i}A?Zjjq@lT0lcpNUbroSY1MSLmEL0MZD_xb2QViz>Aur7fDpo^s zaz=`Y;OJ#p9!P)%G(0+W?-j$&*B#R~L{R7}{Yo=;0WP%2^*g_Mvn>&u{7oRUE|F2tI6 zM5;5AQ?%d~8wcF7gbk%QReU_UhBzK8P8O%n6hrg4I3|N7a1;;FWQdb-FGGyn7=x_k z!jh*w0%?_1xpcnX_TU^?Is$7bP+lRYSdpNhT@3Aa+3HCgOMNX4q+&gWRg5|doQhRo zZ9olCKz-pR2?ieI-%8uMjTlQIT08|pn;7tb1`*rp@Iwi5V_KuVSj#SC=;TX-fbl+F zNjE_l^GQ0Yl@%R6vHg-$@^#4Dli z@Eih8ti`Z8g|-%FDzsoyN~}_inxG@O5v@&6PKRy)tMxo2ntZsmBk80JEioC}L2+v^ zUDDA}>L_hfS+NPT2`Y?-jF6U7IKqVEz%87kNgp_fc@@@1Bnn7jGl`+dxE>7}BGNQe zahf=TpS*-hh(1M%Lplmqrii&EHP}Q;>Mqnq5*#UD5QEh@I)rjKW;DG*6}N_}Z1aUO zz;#6m^m-aM9{4GY%SGJzV*PB!M1zD$FcuvJ-My8TC&RFqFVMjAPhSpAroxC(%LlQ!uG99P7BT7;i( z-$#7sEgr~P0-*N+)A2X z;xHEU{fc#7HT8ZP_-ukp(WFSU7|SUMPUM7^#f>*Enu^qZqBvn}>FVKU0;&gVEL|`< z<_Hx=2|p|7v=F6}Q~mq~7)vwC$4C+uYm7F7l28Ve1OmW#!-DjBod&0nL_hv{y`GYx zRtue+s^@evFA}V0(x}mx7uBj*WeTYoO$sIkn*68$HWeGHdzwmz)A(SMI-d#m{?xS6 zL2mt8f~{}$I!~DP1#yaeX&s=`QYaB4Fd2ug5Z))_v}(n0rH6=*gBA_-4~`Ir2Fm*R ziv5E_!$Ln37aArD3rD0crtCN@3ix?T)PuOj&H}_Zf=Pg8N{5~lJK(G!nm;SR(HiZ8 z9iZIPm9$>r`Z&KbQjJtrTJNH-c5q?+COuKtbNOz_=cNgrNs;DUTH1hR*!NK5Y1z(l zxpmZvSt#?d5}9@t>SsFo|1&SG@VMUpzt$b3%OH>idblG7t(ay@G)SA365B>@sowpxw2R|(%|Pn7D;;p5!bu<0g;9tz_*2){b()0og;~pRa!hDh)wJw zP{FVK#oeH6+GBM^n>?{$AYZnQXstLwlY+HoCf$k^_K$|6i}e8vEpCuFU=rfA7}xK3 zwaGP-ZxEM+kLN}k^`#1hVyt{zB;Ox}y$15IO?Snd*wSb+5^$u>&VxQKr2<_BY!(Qw z*hZ!TNc{g_KlYduIq7_a&Q~};T;$-xggAO;Lhfn+`q6;{xrJvVlwSCeyK;KEqa);} z_*wwN{D$RGJl(Z12MF&UhzL(yR5gpW4I{K5a|6*rn z5BA@Y!stM7?VIW%$5zg5v3(Y`(zbGT$?(wqK~~%ff_qy)ucpmCGliC#v(o2w+cD~ekxt3 zLnt2e4>n!8XH8C2M;zO7iF`bqpmq|lm=XJ7QL1%f;5 zRoZvc;tktKEjw6Jro^UYvF8VravXZ*Iaw=bcl8MNhr)zkh!^Mbo6GWgkfF(ewmVXxU7o_d(WLtimGZla@1gkMpeScWGS zFJxGG<_uwRbZ!-I;0MF~I}u*~W8-9CuRj($F9)8}^hm)K;Ewh$l4|;h#@+6eby(&j z%Dp-%qT_HM(TSjhhQ{FZmCbdz+klGEOe! zYmj1`mzO`!@5O7+hgE~!d%HAoe(j$*A+qZM?bw^a))@uUt}fhmVR6?EYt~+F`ENky zZfnZr?`o}b`DNkv=gjsDY~`cb5ZBryv9{|Un=$-vd(L;w{qe(xTRc7oz8nPqZ?X2_ zIqdg^wn7&6Ep#{#TwT z9KW)$41Fm3z^$9vb1nDen2ymTxbQGPiMu!)Crp|+8i4d%lHwASDR^Fjr_d^0n27dD zG**ZnO2-Ni+(HI6?C?mF?*c4cx`rO;q)`y_3u)nZ*4-iDE{9Or-C8-e)sCh=IK#%z zz)OBZFZ_D1fs-o(WeNGITs|+K2Y#*!_PhtHJ*^X+u)^Vl!v*TwniQwB3>wqyw8u$} zC2#a_MVg8pKv8*o@~`EKSK%6rVa9LF$LhmLv`UQ?*8g0eLB{{7K04>dMk894kfKTB zTKbLoxprb#P`Q9_%+K13qY`?vr~(1sn4hcvQw4mxtQZYAOiHdu!T%`hPZaQtZD93d zk88;O|E-@@%KVM>#wf8Xq)z?S^s3mery2GQY0|$TjrJSTq<%x15g5^U{yPFsNWa>? z5l+x}-;{3HH>P9HWxt_b+O~cReW~A=4(-J`;5U`|4fTR6Uu^?8^3^ooA_H*ZtL1|Ao3MrDu;2Fl(@5iEqj$%rRk_)Ne|U#}&AS z@x^hVqbX69hNlfFiar@QmrbK{v@h~UmoFB#HT-BI`*MG(kUkKHk@Q^RTX==ZCK-=a zxo2hHn1}ALWh&C*XyANH9k^{qO-}!cp3}7fM~=#r1nz#!H;W;Y;~(SCVe( ze6bAn;PneUWy{RZ$A6-1dPs^TEC%VP^&=nuNxInTl*{>1oh{|2f*}rk zR8}nQ?mm-;#eE_V28H5N`Y=wZjZIVOc5)xp6|YEE;nnev!ubXKN8#M&Kth7~D2`vx zP-Nw}3jF*mO`6oKG*v>Pq3)-PUIyh+IU|zQPS~H)6;8I^CBM*N;TDO_E!fLRVjQdp z=T(97-htwDMS=&8ys%+#pmGM%DhjtWxRv@K(LlB`Gzie85f;h1H2MI28h$}5)~$#K zQ5;vMu-dH<$x8Z8ix<+FU}% zc__=NmA1PB9xW?Jq~dx3o@}%>%#X+ASS@qs*`G=)gbB%7X)6?^ilftYJTU_8x}qwj zK~r^6ICuPXe(sUqSJDYYpj`&-F>Frd^1)7hnK+M-xN-hi` zS|JrXYe8Oi9ois`HdT>=t5^J48(*hdGhC5Gi8#L+x9l^JjH{3fi(}dJX<|nzW4Q}l zTs&@oBXIGQWW^ncqN=%x^GkIlo?j%x@?>fer)V zad|2XSB{^CLKE^C2#w{dBrp~x$bsgYSZ!ss%Y_MP1X)qK%Ce#`gTcwlGL#j?SC$op zRhAWn8Onotount}H7`V<;=~8_J6OhO%O1>IWRx9s^lXd}Uctn4zp# z8I@#3VU=Y?VTQ6Ie`Q%wm>>sURuHquUgj(A+EpBt;q2_|1{de_Atx8uQ6Jb*Y#%J7 zmb!KjBBZV^LTW#K5pp-NeYlX?+0REv?JV~ZQ@vuDn6E*qi}I0*?Zt9F+|OATE_X&a z%Hk;MtPA()jPL*<92o`j

    WJ%Fz+(EboBua3LH7@ag3;gbxtH_2tN^oJb*@8pDww z*U_SH@8?*?<*3LQz8F7ueTi^#t#j3d`M82n_F{03Vx-PGx}?rNpsNQTjyD*2x}+{H z2p9O#NzW%&;!`g^eTPoE^c^50ai9>c6OpuoH^PJX@Q%7RN;|qDye}W_sxJpGc-o6Y z`EWO#Jf&_VH=cjePCEWcJISdFu%E6UNS*cVa7KsXxIsOObNU#z%~w~JvyZR7ZWpPp z&3LmBSsCM8{e(iKu9P^8E8IouErh#BsV}g^Zh9)*T*P1E|NptYI!Q~NF|zb>*7Mbw z_{#S+slIJev`sI!jyfJmJJR^k$xSzMr5&l?e{MVo?QqqxL+VQ7K_@reIFVA9VqEH^ z?Duq?tEUDd@{`o?fx3a>Ov>O>B=Uh3#=SrX zKhB+fb@F%i=>)C_<6Nrm#!~9W{#+H(4*GU=q;am3vz{E%#X9*^9uDCGsg*caiLQ_I z57p0pv59OPF}Dxr_VG8+6S^@eb#>R1QK6gYxLr|8fCNrHpAaoSUOVYpFYV+^#qh29 zhRN(x!r+xnrJlu7N?r9;xngc&G*#*xUBwCo#|#;VW&Fhsw1e0y=6R)K6IXHt)y_sd z7hZA5^DUO@`iE5N#?*aRaTcp|0CATiO!^Uf`S1!_nDG}QlWU6M2s2PehSLt$>nbckazr_WlcTKo`%6kY_g|GN{p+~G#do+DA1l)brpv;^%=~8UByFL>YnKO{m7fa zyy0T1h0zlALR^S0V6I?iH(F(}Os+y&S6&CXQFmu;5URxN&!u${ zNY_>ABGgTb1(wT2D8@x7*ae-5%A@&&Ib=e?9Xs$b{36Azqfmj1ki>@{5NHCDH@m%8bL-RN>Be)gZ9 z7Wt|wsCN5suF^^?0zPSl2?yosnyGIOmgmGNEZM<3KAW?fJraoHC>T1p8;fUq1|g8k zFRz$@Yc>}s7jkvPRLfN$g-H1dq|Qu9s)q`xIwKL{)LhSQ*`k@ zT)q@8Uyv>wGDf~2T|Sw8C{IPjxkk5cM3+Pc)*d;hZ{?l&K`7^kzO&q&>w?bm&Rl1L zgX>Ii@NATKma}5yow-hq7+yWfWxU#v%ecPnESK@xOD^lcWs-q~M1-6lMsT?C0lXHI z%ebD67_Mi-ArK=Ih>`j5nMjJPIDr_MK#aFQjJK3a;w`kzn;Txva&LhcZ-Eis0ukQs zd?tYgZ-EAHfd+4(Zb941efXNNIpT8r2vrJ;WVx_rmHY6`_VMGAVpGjK1m4MoQm{|v zqXfd`d}dxXI?MfZX@%zV%EVdDYeYC?d?`W$u=VFdgq8?9X}J%-P!ijt*({N>F9_tF z)L;sf`pQ6>-~c)mM&J~0_LLf@Qgu+r*-g$BrdFxY>VEI6yBB`%8 zUprjfASLQZ&|FID6@rrn0wbP;EjZoy+MRu=Ac_a^x-w<@lrnS`zDz-rf_NIm97Did z!O61@(Rz}o+w&;koD7dbf&2!Y_0yN&}?5SPhES0C(^k&#M7^91npOMMy9d|iC=aZy>IA5J0A*;i;jT1rmVQXf5|&?uUIxC&_C@J+tDP*p8DJf|_rJhbw1$>~s?W7I3Kwlxd)R(CN#!^~X z37K^L#8+SlQsRgxoGSs_d4Vc0ikz$j-QF6B`9?WQ-E=9XZaSIL+J2-JyB#}kyGZ>c~PT1rkXyS}A<9clgK+qZ9UU*ORIZ(KCPi3RjQZ?GRdcUN#G2N0fxm-S?(b1Xt`Fk%Nap^_412ar_4-)D5_zni~!UZ38 z=ZBAXF^Jb)? z1YW(M+dO^@)61H;8Y+%umdL2T(tFp$=a4kjaI$g$`(NTXjgqs;!|qTqJ0Awtf5fR7 zF?bW3QVJEJNW448jIoh{8dZE2-epctgf$YrcDn0E?_p4d%-n#!Hq>1)&Y&v9;eB=8 zi=6b1H}lZ_Biy2^;OEQ)(0k(Se%&YDpBt{yL;<+BOX;OK{?%6Y+AIIgDRaX%APqK0 zvN`sNa?o3{w0Jd-Oz=e0+u2lBiYA)fO61;AHF#&6v(uyX;3&uCsp`6Svz1Bo%09E+ zz}|0fjTevU{wJCjCzha%ZG?BQ(f`rk#dt1=SNv$C;T`CBCYUehu9P1Jq;M%GQI+~_ zx#?vpUc8M1Mimjo%4BbU)7#(%%tTLvRi~s+5dzY1vkUJj z!>|XlFbLBr?5$;LDHo{|K!taXwOPry2$#f>&P;jWmZva3e3VtE_h4oQMp81A>`nLz zWmK+B_wFc^r6wZ{pC*H88kk6-ckrnyYB4X7&+%8ML}{EeHhqG}6%zfAeCFQ^=dAj` zV#}B5k&_HSK|C3T$V}r5K7FRQJBlL=EJA5DupWq8hY(~uj9`lK&^uP?=;#Q$GKw_f z1{)^C5i;l$M`MQWNh*@rOsrPMbLM5B4Rq;Qy@?P9(vl!CC-Cy+#uwT|)qRFMJ}v1l z*wRfRdFYw&ss7HD&DpqNlyN$W%n3k@pUP2*9xWMtz(8kW|5AFQiR?U3;?Gepl+DZ@ zQA@ueZAJ=S(@*+({a;QMuD_#+B^9qASidG(zTU45#w+Pq9pPUPmELYg9PA}wUi(sG zE4;5QZXE=(RM0f7GB`sVZS-X^jrYZ(__RVt|MGa{+VyQbFw{d%R6I8@zNioS!gds_ zt)#G_p^Vi!ig_#2P<&W^(fwG1SkB%QQfD%=O)voi%WiCi7lu_W-iXd3-Av}kNau#p zbk2sP&c+!tLPL=(js|^RRsPrY{ zVX3gl%^9uXc*a_#%^(|mek2)mP}sCiOYb7a5}CWC3>#OlUr0uIh$BdZv}79wMuHHi z=2r>4MG}4)F_Np4Iv%Gx@Lon3=;!Pt=?zT55*;jT(f2GNSeR4NkU=xbqQ=WCe94J0 zIEB}uu_ENkVCJ?-Rw!H{W-CT!;E+wr2F8!FILj)+s|*J9GrMG*JlaD7+<->QRcf^| z0WD&U<$3~jb<`WcY#xv}u#3<#YYIrO){s;{2{Twj26%}^UUmp3iIIsu{d*Iw*^*cl z+lFr&vwepyfV=9I*1Kd>4VKG56if$j=IPjsY`88a9STLonG*jfp{`F;8<^E1p`WL) zzJ{{F{)|*uQ6S~PZ*ySXAr>}XC>Fv%`Dx1axSHEC(k*Oq(8J|t7+wJI#Xz`?C?*Zd^DJs|n zVmd2^&2ls+gV)%XXkj6WEdm9zfS7-UJpgrXe&)eeSHbiLIw6~b*@8EYOb^1SDMZb} z{XW?nXkentVsvp96v21caH4(`Nb_*H8B*TR4Jt;1Yu-(Mj zLp+8Ivn|Rs1DjK(OKIaoV<#g+RdOhcDN0^twfV;y~+YVG3iJL~`ATH0T|i zbuoSv({_TJq!FCK!~n){_7M5?vtR%n;WX!9ZetxG+`!@-d-<7mdtCi&&~erbSpQ>F z2{%2$3K$b6dpQ$B4UFejjo3jNq}9MSD4NfP6hHZ(A9LmKaa?&^B(Hl(NWy^P`ZV6P zLAwl+3KD5UXgelEaCcSSoq)Np=a^X+1bGktFo^yf#xU#P>Cmnp|lZao2|9nXIz7W9Sk zyO>1k-yksol+sK_GZL-`$HGu4wiAeC>}stuM9M2NilMPZxya-*H|49+nOpaK;SQ*EZ9a3@X!Jz*N?|ZR1i1D7YNzKhk zt(ac=(s>E(gy^gU=UI5ap@XniX0}(MBf$>N&>w%i^ZBzE8k`uGR2J{ll0Bnuv}44b zmmz%sO)ZY}RwhSNbSteuTQq`1WQ2x~hRtZyl@c%1CkpfL8x6bNa^4OzTN86fLQtw? zD8<7=n*pJa@q&QhXdju6zi%`?Ceqi}!vkx|xM-#x(mokSb+fYZ>J=Gurn&<0?F87Y z!j}V*qhV;Dy&oLbI|OEzqLX2a7c-`0dMKOd~qhAbii<`co8IJTwir#C=E;WD-QQ&FN& zwNjb-QH)P0wIe`hm+hkjq9kZVrJ9*gXSSK}tP|gDap~yl3YVKp2V87%!RJ~&uV2t% zQ@JrjDn=BJ&rtaWu}=ZO-76?KBIt{GGr3QCu)K6$<%lysq8XnX5P68v7em52lONcf z$DMdgpH-o7*xF*4J-dRx1MdgbES!#}>HS((4lpYRn3V&}$^mBO0JCy{SvkP09AH*X zP~Yg2aI^eNTjY^H^tIPuNajT(J+i zIMQK%yh6?BgPB_*Q`bMWu-K~_fleEX_k?LrOe-kiU2p!Jj60a6e%*#WH_)#$s7_ki zvJ^ts7xA5&GI(GDPoR|tS4Jz0k+`8Stj^8Vm`f_8#T5PJ)EIlr5&|R4Lnsu-%Z7#G z$s+L+HLBvU{Evl^CpDjx8fQ=fT0=_)UPg}8x>jwqt>xTf8usW%lhTFwB9=%_+<+Gz8BUQ4jnr$wLqR=&ft9SkfvM?GhutJ)kuXcWFHuhr(N9;PZNfqX%KF5&-H9 z#@AMAJovzmo`3ks0z*j|h4l?5^ohJwH)=;*dq68~djQIkv_&Ys(SU9dqtRfCEPM|` zD;~uP9Zf}kM`iY72kx!81u?B;*dCWLSWCR9F6baEoh1^XQ?PIObYt=)`k~dhGdgZr zh9w8vQ*iR8EpJPPDmkZl8&&f$V{3C`}sjfP+h$hBWQf_ z^27?BRe=Kp+Pq;Wg!)-JVI3ya38|2}W^smLsExOp2u5L_1`|8%P=&RNo`-vggZVB; zMX)i_9v=!|$1{A#A*oS%UAYo!_NNQ}I2})WI+Q$ib(LvQBxBc;p~=85Odx}m-WmcI zv!`EN-GX#z{mTt#kbs3dv4z`#ap^u?9#^CU2>;o-S)14suX-{Dm{_Lj6}kZM6DzN7Ijl4oFb$s!#Hd&(-acg?jXh>4f-$jjFG9%61A>(HF>ihbLGzAA_PMlAN-*v3+RS`N^IG2U(j?6ZDs&y+1qqVZ8G zdX~yl>==s)H!75U+ks8s!dx!cD(#_6=VJK})=+M*nj zvlxeXhRQ-hm<0+heHcEugXJ|DG4%@wWC}^&RzLRb48D8Pkq6x^KsSu#zFfiB`-P^{ zBdY2Z`Nm~IbNK^_FXU#KK?}Bt>I_)6TQBg_3wY>D6GZ#Pt|Pa5583^3l9v7?$I|mJR~|a z+|LhR74;43hhYo-BPQSu^zk7BCAutq?}W}#G3FRYs6Bf6g^sB8pv6+<4oqbV4Inr7 zyG{0SWp-HUzkO+G<)x`=<$uBSBglDWX%#ZEs@0s+##Tel7+XEb03fX>wW`%^U@PD` zKN?$MzJRg&ZlK$afcLfl&HWuRzp7O=SPwS?9K@op2582`BNY6!Oy9sH@Pl^HOF z3+{2EalBg$m&l01$yLQjRE=}f7p_HCgn zL6MayBxp7SAcj8|n9`2}c)@=g@z^Dq^#?SBe~zfGX&1Oh0Nmj(64f@X4R<1-HT+wE zQGnT0fIs|CkiQz-VSsufYqQ^xza88-5jDeB&-?`PTfiL$u!DasFr}Xeki-7~`KdhY zNX=~F&k@x#4TT%GBh9+Pe--U%05?v?%&-DC&j+pv_#O}l|10FL3U^;X9n`-9^|yo@ z6O)+>;*X$ySf@9O0knXBC2$qM1b{dE_mCe9G>Zf@hW|&@?*sQp00yo3CDdOR?xBFT z@b3U7eohDUgufj5jp4>HGpjH1LR0FRqBofh2Vm=Kej4?|QoR|LPiFS;ZvZBKPX_qG z{|Ncb;T{ZV3jbo%kA;WX7(jRUZ=ik~xN#t2hQ+yg0k9=t79a@zw<1pdec-Q+_Gk0* z9|pe^@r5X#=v4rk!@nHZ2rwQXga0n#iJk#~M)3c@%RdW#Z0XG}@bbs9&#Vpn+klDQ zX@CIupCLcVGaO(ovNEe;#K}JcekcO--+1{e;co^1ufW8wNdRB?|3ZF}{~$mU_!sf= z{|E_r z&j$Fwe;@ft{@7)jfzRd(c=?ZlzYF|Vc=_Xm(G30Cd?zsVrx}1=@c)ba)Sd`H0|WVI z!jB^m^RvAC6X17%e-kj(HwDlG{(q34_=#g)GmKmFCA|E{!tV)xDKCEw{5V-Q-w#av zX*M7j{`b86KR5mlp?s465P%rzmjR0a;{aap-$6XdzdxWM{BwEvkAUAD{`0*26X9b90%YP{RZQ;)YCVox_^o0Kf@{{~OH~!D?@{dOvd!*k8O#Ge<@Pq#`@{{}r1DeAB z3orjM@OOv*CNKXK_#NTj2W$zL1qg!w-B-r{VU$nwDge!qeg&`*U_3wu|L=$=dIkU* z!9TCk_`k@@UxoNKh~ExO^iBf=!2cZiNuHk@|EGBQE0LxZ(ys?5eoX@S!v8n&ll%t( zn!x`vFaPi0?*{*MUjE7OOW@xNYyp@F=nemW$WQYB-1tAv%Rd%rS|a@#U`n42@PYqN z@#G!96=8JOyu z0_XvM8S)c9h4H_Xm%lLnZ}IZiAdM5!9{{cem<i{AET#O;b&M@2f6e;Fhq(Os3QHYQDL8R8$BAoo ztb1x;4OA0fRIw0M5m}0i*tc4+_v4?N@|UCdn^*d9c0Y#?rM)K3JLqf?`#Ad8AWpdO zJX4V-CR>%L@1uCSuxt)1 z?xXRUuzS*xi=*}!jasX#lgQ%>7pm38(UlOa!?}AMTv?dF^ljuKxUZSu@fKa0h)s*t ze^o^|>*3?brm#UB0^fz4+>JM(&Di(Gu}8qTO(K+oBX4F5nV$IT3@j5tU-&_*K+<}E zmWA~7fKi7Yp$BLU^b$RwE28J<0Zx%s?K&7P<|fsRYgDUQ)uM`}5&C|mpHQbzmr#dL zwooRtobGu5Mj$s00%P_;4eU9z#>2-;2OXKum>;_@CIN4m<;d(90S+`Rs-Sz_W|_* z3jqB9R{(B+oq*whe*u<&S%4tGS%4#86Ceff51=Vv31Bdw6wn>8A20^+9$*9*50C*4 z0h$Ar0Th5cfZBk$fIfip04ZQAU>M*jz#K3Q5CAv{XaiUWPyrqS8UcO+3;>h>Is^xT051R*fSG{afHMFIU?U(I@EFhp z@C#rN;3l9OU?1Q+z&k))w4kn$xo95nJm65^P~eNe7lAtfcL3fFyd78ztOb4!{2aIj za1G!Yz%zh*0rvtv1$+v)9dJ9~^}y?alYoK)1ilV@9k>f{ z7vR0Xdx1v*j{^P=_&;EpElh!@08at#0o()lIPh^`2Ve)_HNb0t6Mz$d{{;RMxB+kj z;Dx{ofg^w;fQx~Pfja?r0^S9@3pf)v6Zj?YOW<0-wSZ>>&jt<#4hB94d=A(N*a>(u z@Md5Qum-pcxD2=%a5Lbgz)OLnfun(M0p9}l1oi|z0DJ&=Ebv(155OOQNfUSh#DF0H zD?kXq8ITH~`P3iK8juKR2r_?huPbN*Mo(yvCPU(K)lEWYxy{6GF` z{U3j9R;yFDZaw_0@vG1O8t}h{m41yX{ThGeXY-Yx?f>!DzH?sPAaDd zM;X;CQJciZcB}Q~g@xJ76XKLEgRNA7_hdsZrwvP3e*Rw7u01YQ>-BDu#f7B2>Z{(Y z&2v@fZ1cWya_@Do*99AXS6_9>h`Qx_&f?0A0o~58-{4$g?;$GfGpYG3}6{3@-@rW!|YE2(+R?qQ9sQ$}l?Y6m9vU*AM=sGB5y=N#FM z;U>eb8x@Ez#dd8CU9R@`;odr7wHR?p)3 zSGy%2IluD1@+%jIK2-U&ej;H@glq4*6A9 z#lgy|#{S9TnkQT3B}QzYq!F86ir>BTp5m(aqFCrjG2$SK;gV{P&A2Hmy~7;mw| zu}jpxT^rSV4vMq$AMf0LU{TGxJF70bur(q$-ePWlm zAG;4twpbfEbYzWlmiK=7U_Q@blc@UEC8poW=hb?0VTIY0#AilL-xr#w21UfYJe8mv zH_T1l=ueZRl;Jj)-rsI_aa!caYgVUw-b~Qk+Wq=<+5TBR`*$_CJZsC42P>{z88h$v zvLl68-TOYf75p?|@A-s?g4H)o@;rOFZS%j@&f;pIP4&(H_N?kQWMnnj;j$sO!fz$4 zEt{1h&DyW>`+mFPRO9UUg4_$5R&I9_`_9j-dAe$k8hdxwuhPQF-Z*@Jmpw;Y*Vwo1 zhsYi6stw&9wdO-{q4Zo)-i#$B?VD^W9k``1?q~aFv5yw77@ARiUUHRnCgzJh-7No} zlwg`xH$r4Qq3415^+x93-PCUDD0iEkHGiIU;RlQTm;cyRc4Kt&Ti0t`x^Qu__uWfR zUv0mcn38?1*_rzL#}BmM{qLR3Et=Qrnh+e25c*kNbvRqiGaO@=chj?dvmI!{P&u{c^i6<*_1a+lD*`d zZO)WFbqaf0wJZs}m|8yK2U(dSym*siz598cit}=87fj4H9*|=*q2W5w&)45bwk$g3 zwr?>u<=XVLQqcG;zemD`S9 z`*qWxL4Pmxlea#dq*1*~9lj|^+PN&Ut+>6pX^mue+d9wP{BCVC9dvA1Sn6YEWm5Ue z=WX}yER-(Km}@(&W3K6({|c5i*fVpYWmf6BR_DE7b?X{el{h;OQ!V>ob7Dsd41Iza|pS@;5^|K3R)-@NM zj`Eil`_71Ylk!{Woy@R7d-qtYb5~sPoH=!&<$}2(ImM;5*PT9o;mw^#^G>~a5mg+M z*5rOxQfXe8*V2i>1Nzsotly}mb**cu?sh-RT6SBL^iFv{^8V;n=BFa!+>3%%Y?^Cy zcj@$o%}=lCFyvi9+s$|EHvUuOxkHj&XQ_Id`Q-I>{wM#k4lQcqH|D>@Oy%8wvpk)4 z`P;dr+n6_TZCj_>vx3lG+h_XsO)XVsNz29zdH?+Wp9c!xy&pZd=yJE*Q>Wj!P0!wI zI(N~Cu!60fm21}a%E|6heO*qAlW+3Or=Hqm6+Eac+EQKacgC|M`FqR4be}qr&#SbQ z{dFi+eQ2C4^JEm9O4N}}gC*9{W87*k zme@Glv=w!)Bg)wwD$UNF6SHZ?`Ov%{>K`q?8Btz#9 zbhgi3+9rR~(R;6#mcH{i{rt+PcMr}LB&{3wE^^yX-OZP78|yxC{T4gh_D`)%U0wXz zHq~ZIO?TW$^?y;6)W6p6HXG;3+UC?pvgT2j) zN^(seJs&f8W1+G~%3Oc7Q*LO+1GhRJ>rBmC4h^$2Yp3+AeJ^Xxf>r(nv*T^%u5@TS zec$bZQ_q&oEc!d9^xeUhW%o<=-6^Q|wrGuoXZCdaG27;P?zSsRePw-0)y40=_sGn5 z1M`xUH_9W&9PMNt`p&m2s7I(_RYsS59Y#uuAs#$f(lIq=hMAb-L zVQ<^x?MGnY4O(_~1~4KepWngTzG4J!xwp~Mkk)R*7=b0vhHg8t+nR{YOU%; zcC~Kl{J!4ES^4#Q-y7NBwqI}e;`XyU&-&qHm));wcy#?o?pb5v|{wwLC%T1%&9p?OzWVdGax>oO{Wez6?O6&uB z$Fv;fklUigPxr;v&+VGG-Z7$Wv(i27{D!=3Gpu^|*5$UtB!`D@Z@=io3&)MiJ2@Sk zxhVDJpZ$kz8qiexQ->RAwyox6whQhvJndEp8IHT53xTW+3?e9_ByWUSKdH<(NLjE7d79;Pu_p~po zCBFAhza`5bDawaDzIWH@!NsYMA5M2$|M$Kq)n8BY>%KdF;r!br>V@yunS_66Z`JsX z&B&|&c^_HwI%8AREB}Y0=VP-ApV@bt_@s7(-_svu=H)BeoqF+R{>+zWJ_P?eWYCC` zh)#R1T26a=t@H2QuNS-+Ryt$p_FEU4zqt8FuTD3v)=Vlci(hwT=hd=H2>KujR3+Q{xt8AF%8{;r^DUk8#go@ z|8$Mh-yK#D56@UB^W3%SNSjx=&wV_W{rYb7ieDD)UtXj=lJ{pLlO6llcD8eCgu3yLU`4Ey%uYyYJfI$ODf%=Ip2O zuS_V&pK-$c*MBzW)rnXbGu!1!X{Q^}Nv(Ff7ynY$ddJb}!DipZ-QK4$nqpL(f8a{b zn^%WE>yUZtc0a4w{!1eog}iZI^`PETt2e`5ZSX#EaMZ1hYmO)Qn5AXdYtEiY9lLS9 zb;H_{|8}kPTvF6Zp-c55(Uff}_YkL= z(T^71eKKIul!bqEJ3nQza>s3tn~}k@&v~})kU81v-e13G_U~(P^|4ptfj+O6y=wR5 z$kK)WZJk?021hKp`>!To>^!p<$G=#w%DdjO4xIF7;rZL+#;7cvPK`?P&pf=mmc^;#iCeX8j#pCz=H{2Y$kE8> ze7N{_Oi27cHl42Z>+P`HvbC4-&SPa;R<4*@)3))J>*L23Cv=`W;zGOfW-0TkzO?TW zKW9<)R8Ob5kNeMS6E;M?Lu&e}=B>58o9;RHboHZKKfIro@93A%;ODR^sSc0!`ac;~ z>J)g=dED#k;eYzZF3$B#dVM_mVuM7tLus|9XPxRa=-#G~J5gRA!h%}mJor~Tb;hRp zWu5B3@|sZX7l-ZzcFi6(myHPjX>;-I8NY3b>dCnvV&}56`)gE~(?|AJy zH2JNuQ=5PWJ%e6sem=Z~%YxcVrjOX)E~iP!LUFH?_tQQ5PHTlry?5f(6N9{_#bG+7>600B9 z?%&YXuXZE%mcv(#%ZNXA)yin(koTL;rTu(T`&)~rD-AmqKH<4?Q8?BA9?SW;)$pzky5-|2YP(QV!Ofa$e;?|W7ikI&vaeD3Yv z>;4-x@ZEri0Xf~npEfs{pq4ATU;6dzjAQ54bsFO?8+&Dt{o$1D(}s_}^3QKaG*_Ao zsJ?aCoqJ8|Js+R7wpX$0RNZZsnd_sL74GhQY>d&k_v;q_@sqV(4Yypkls>kz*3^%m zTsZ7n_x7cl_5r&u{`h2kHS_uR^S6!)uvpR5ajT7g_X|Ff0V)3-{`dOB)pHa2G+Vdj z^vJU{(|=U#j(=)mTuS3V)6{vvKbDI2KbiO0W14)VQR>k0*$-n^xh{#DzrynRRF|w9 z`73WAv*){q`A^(0D{Xx3(s#)> z@BLkVDX&lV#YxBOs&o3qC9OU9F6Q;f>B^Ij>KR1^$C{{1-qh+AJJZbKMVA!ui9nUr z;LAf~_m(FNOY2(o-uYhDF7>!zG3ni^>Ul}7d28QzZ_80%-+S`PhJx2#E?3pR``(Jm zxN*he-1_s~2H2N4Z|GAh@;Fi4XWW4zMg8VG%JcedKhpBtp7}rJ?^{)?b(Q=j(Z*BF zj@2yLwzbAXJ15QPDg6@zYadcHS-&%0(rx$+*&L(m!%T86hzrUtTeTgwb<#AGoq5fc z9=NyaLH?x&uO)ijae?{TS$8aR8jc-mvDYlQn`CaBdg1-pD5IpLKIOJ*Ma(3n z^VMfDq9C=&{Kqy%t42*QD?jw2){zM1mXc$2c0CN4u|NN>x4Wl~j=9<7mi0AB!1otN zKmYGiptAQiMSqXHK4(`Hh|(|Zb-vs4)+$-ItMiLjofBafKF#4v&=#wGF!@4?Dk^Q>AhLzRHn&Qb3iFvKWnn~Mt$6qo( ztGKsxaOrEyHYF!>wiM;GI9|MV$AInK8aVE-7{6;@RF{K$)Egh?XNwmd*uJys&bu|! zw_aHE*X16af4Pvkx!QGmYweBt*}uousPki7mx&h1L%WY08oBnK<+&R3%s>26U9`#J zJJTgwpVXQspJKM+LQ|t>i7J!A_b+2228~lDoNA|mz-hE3mYk%462D|qA4B0a4@|7zq9xOXQZ;boZ!Xv@Ap7lMyH{t2(f`|mqJd>OL z+uV9xwP<&3b9I|Qx2iq=l~o%#y6slZd1*j zHts)MnDz4?m-kzYzEQTT#`RmxCttj9>FK4r-ibH2ziM_ZJ7xU-`e**#Z9h=6CG$?h zT|F|Vnnm9KP-|#vyh)eeY8%yvnyp;&;Z@8gl_KfG#ro=VzPZ)!jwrUc@J~>+?6A&N zw->HSuvZifssA!S)uVU!l+5d!&JXQbawYPH*R2}yovwCyvaVoLXzAW1M|*5LH`Y1t z!^3_1p4tAkr*L}c_Ib{BJ65dTQ{-lRrr2a&SZPF?mL&-Xw#ScjJfi5CKPb_rN?T34 zA9vK)-|2A8S<51gZ`nwyl+C2^-^fNPb8(BA&`uJ1I8J@E+IN}dzWpNCOb(VUIDVpJ zy4m@{y#a5^9!&qeyyVn|yx^L7o5u8Bk}a7vCCBz$&%!!=LQ7g&jVMpOs3?>D;JB$c zyi?x&dbT-v#m3na7fi6p8St}cUBfMsH`mv>omzB1bI_m?_Zaxc>3VZ>Ict@{ybcoZFlYHHszo}n|_u1E&W@Qbh`EM)OV`R(xgq| zwvlBuOwHTZv2{&VN*dWtwk5j{jl_x2k_qKiha=El{=QP{78FNf?J2qHa@So+x znR{BTE6wV} zuhlIdWFK%ZDRj?0YSDaed0_PgYi8G-x!`QnX_2{avD821P0WnUJE6br**hp~MXuU< z>P*ioa~D`HEG^CnIevOw?MHXsTzK*3)V#Eq;;5vo`%S#U@=6B;Ph48xvQGb6)-4;^ zxu;(1)>8Jf@?FxJ(KrBzIAz`{sK`CeXzr#J4W}=?+hNV==4}h!4cTaSXY&rvqJNgw z$(Bqu-=;q4Z@0cE)cUXg#`v|ltISMv^33|z&CY*U6LXvNYIWMW_6jX{*4KaL_AF&- z>X0#I(m(G%fB*hn;epFVb4Q;(mD??Qy4#yYb4~YdEeIR2c8#)gm+YKgEppaXH_v-> z(rVMGsnKPFg8j`Xm*-80nYo7CUHL=BYQu zK^N}auk{)S0Jn=L&fL79PS2e)Th{m`H}%BCy|QUlTMqJT?5;MIT6-R8ZD~0vAvni- zOxU`r;aP7E2F9EkKi>Ulvz(UYgST6*`Q=yZ1vmZ(6WP2CmPQuG#QbzNEA(o>o!|x` zZ^rZ+RVMj=i0>Yo8H#p*{Oote;3VMcJN(kOv(MSmh}qm?6X)? z^wxfQwx{RZZDUf4>~^b8S-cmSaRs8l3Fg0I$!{k)q-_>T0-ew$Jf0AY0KKZrIcbRRm zaMG2U;bnKs8^4Y-yE-ql`VvX?8c{vE+1js2ZF28qk7mm=n>HQNq>YVJjSY<-D}HXc z{#IclRo;s_uFp=?eVOy2_E!7VR@%V1)?Fj()qC&UvVQ)okqt)P>+Rm#?{??e?Tfpd z{9%?yjaR!p<^Oc;K6zq|t_MB(cB>MibQ|2Df2YOYX+(YoE9(q?Vm z4)JT+lL=^d~srtQ>Wz{Qy0xVIIRDlFSShvY)ZS);it@bt!#()32v9( z;AvXM<)3}WOuI2+Y_FZc-&vT|AAK&SdDgh}l_R{H&Kp@R_qS2UetV}baNLqSMm|O3 z{-!kL&76lxd6U&cGan61l=iKndhYHTvw5spY_hedqTTxVArBArQLZk&6c@Me_xKi7 z_axj_&GaumB@VD&)}U8tv!I@HGqQq9W8VhZnx6`c-1S56oSUosjx5UQVe(|NuUF8& z@(Ff_WM^FDUbXsby#wyI^_lT~ldz|WA>kdWBt&Esbnmz8ih1a(gWvb{_;qi{=z4ei z>>qPE@^0q`1I3}Y2E;#}+<)bit%Lra+cB!me^m!-d?TYbIwjw6wHbH&<*;3Mx1Rgw zckQZ@`(3w3{rSEm>5u#bmwO{EjLYnMy8m-eTdwoOZR3k#R#rQod$z;H`>8`O*fkz>bntttBa44@ICib= znBxt-0tzGgojyFj>wkwXPFQ}h_JjGSdT(C)+w7xDPn;Ze>|~AFPfyED)6Yy!_BnU( zV&k(_c1z}NUf?w+`EBM8?Ytf5J#5`*!Rqdd7siFHpWouIXFuK^cXVmN@mEX6Y&^Hv zz3zfv-efJy$b(jBC2({`ec$&QF|oI&4y()v;3=H1wE!In83--J;yJ;ynw0 zjjwig{mPWroBkd#dUKmPy*6q#)Y-7{SgSQp$2VNv;cusv8R5fM?edi6zG`!1nTOBw z6{Fw%x_tk_U-FJu%kVX+pAFOhI^Tujv zJ?{;#hTR%<@I=D#H5)V1%zVzA)!5J9I5t&MyP>t`%3c3WX;`$R@wo>M%eAw;w;U61 zFdNz9WUkBR?hR+17+AeaLf*D1q9Ugdch$m2(KROxcyhPf9}A}_Cr>$l)8qDzbF+gZ zGdr~Q{Og|8;J*4ElT*7T0a;UNxPda5$Yq@r7;&DZ_<84ava|7j?oEL8|ewg!5e8`x7*E-o)?sn+C)7Y!^$}MHb zY->(kG5-3N#+?(2$F{pLV(z?@X60S%UslatG$($pljqbn^ZGyDAs-S}^OdQzY45eS zRzE$r=lu`29{D-uPYe6GLBb=4)GEWC`0qU#=u~?B^*HBP-#^2XJaZRc%s&45kXvGd ztm(DV?hWd6D(X(irl7D7UjIJGX|-v_RBioEW%a8~@Oo9y-Qkz!51ZNj6h1;W<96}p z4pIM)v-g0DqFLg#hYTvXC}z~f046d52ACxYD3TE|k`X0m47gxcRLog1U=ElwyI{_s zh*>db#hmr~*G!|n?>XnYzk6$KYO1@Zr>AG0-Pv-xy7=_VkUc$u9##x-_suE(^`hD3 z@E%5S*C+MTIcfFuang(#c^zIZuPpHUvOVuXzoFgxTXtVJQ)gUj>x4(T6HB#scV8QS zSax@f#`lS(O*}iAZ~Nfg+p0~*yQ$0d=P&GBk|kYmt!38xQ+<>kTg@*1P*DzdiZ1#z ze~6(^r*7GUSIl4Vu9)a5f5_R4zOOXT zznKzo)75x-BP82TkljBdQn@p^Ju zm&Ds$=3d(GsiU4-vwh#Ad8>6jN43~)*2?9gqsO;|5YwkOJ|6LlnYH$8@rKrE_0Ers z-XAviwua^#`h9Oag;1lY%fF-cahwa4EA8YXS2aYxk}#qZl_7bq>G66`~`Cp^c_Bx{`WG-CcDh9CXcVUC9HaawH|v zdL$~7B38!#Vg6TN1PGHlA!KTblzz)eznId*Z#L++gmv&cCtdueOdr2_YJ%T8wZ`wQ zI^#EZJ@EUh0Q}}E6~Dz=jo)G2r@35$&>XP8U;5aWUJcl*AzGrB0edyVudC=K!LJpM zq6HQFf<)|t7LKorJ)7Y-ke2v8WEg%IS<0!Xq@n3{^Zil&OTN<9^cN>df7iiL_y zC6qc)d+gd{)*g$F(HO5Lcs0eV8D7otYJry_UM=yG;YDNC^sC-~%limueCvQXx_~lKi8FBoOr^I_kgY2}*yW{wp;}5|S^H z9H|(2-k5vu8J`p<1XHAG>`%j978TF>W$U(?ndEe;kkN>)Mrbbqg?cb9J*e zb#Zo~vBg}=_EreoYGD=G{r4XVe(K&A6J_xK;izxb)mErZ+m_v|{%l*>3)|EtHJ1{O zdFY)GE78Tql#WFj5r~9iqWte|Yr#PKZmu4tt^on2tPLUiUwQwi>mT6sA9erA`&}1( z#cCbmQCvOTgk$_WkM{dJ58HIA)Mf6keg2Wh_W4KNU;Vc>_3!ISkzn$ z!u1ja*Mynj1f7v44MW_)C`>yNZcsDKE=D~%#(J>u-{#Uv)6^yJ z|3_JE{ZQ1AegEhFg~-cCp6_IJN$5vSVIHpZJaoFcBninENsd&EylT{wqaMwlO=_ei z8UI61QViR|Kuc2HSW8j~J1JXTQixQHKyPtZJ!cppya-?b`IZ^@HC`&_G z`53$p_VBvI_!&-L>l=?NTDcG=juxe*Q1|E)kp=% zFGMOpSs=>fGq8;$L6RYbZos}sawH|{tf4wHQ3pwaBtzoRX9QLS;p9RlR=M~CNpd9He z<)1|zq);Rok}p!>dv%GST75}5bPZDBIgA&Q1gYk!x+IRuE};Gg)c=g@i0v9A3G72C zv#3OUBneVY1@`}@E~%%E>vjeEAcZ1v-?85hb;%&)bLe9=@@kL@kt9FWC7Y3NaSi#Z z8WIV#@)!0|(~x{ee&}@^7fA_QQcpvoMDo(Wx!*(`bq$FW$@d2Cdp!-wQshf-p-rT6 z9wv5^Dh9Aqz_bq{j>Ka&WtGsfhMW^Mw$j zYXoM<|6Xw8EZVARq9NfL;`pyIR%oXN$=k=9-Uc~CZS)!A6xs~^KuV%*BMsr1%=Z6M z2K`@3?R^{(Cp>jhBovDKigOXp^S4d+eC^yQ@nIk#Jw7~2@NNBBGelf)MEcKVL%47R zxS)4xr>7@JKC$$f0l#h;)|5+7K7R`MWSWosRg|FvfJN`Y}GJRB=d|GmBqMN~ti_(;Usjf|o&keo|Oc2b&qRD!>l`}?~T zHI<1kYE&!@ix6UU(O6Rd5UZs2`4<<3{!7M64O`y8$d-BLv{bIsO?hB9Pbwiocg2LR2jDLuB{A)&=cY4+gPh=rftE z-4hp!akn`6$ z2xo-lbYu6^fo1)=z0rRYvL=t8eR+uC zriB}|uBr?i*PvvAWxFF|>IJ0@E=l>h{kD9a=FKH(ZujY>47r-Rk4+v({bqi#V+r)3Kw$ zggSdo?2THOeUIU)+gi zPi9xN(_B!$V*Vsejvc#v^HX56zx$)O&e4JI)R?`@jBqq7hIOXqn;7wz7Z`ZHO zG+v2QM!lsm+djW>4;r_w4NkPDv72igdx6I9V(&N0X$dN5t%Q1d1(be&T+)vTmz{i0iBbGqIgT1B*_ zYyR|b-fg<>jk1sLpljbbAmu(?|D|EZ&(1rVoYpE+8lBiTv7*)U>T^|Nu0PAFNO(2Y zXUvBKiA_>A#?)x8U3hZUu`v;Kw(5QMM;syZ@-W%Lqk%Sh3^eW1xQNTM*Jux>d8<~c^-bF3n)W9SvH<&kt~oxkz^-f z>);vquX>V9Jb$CC2B{p0Lky`<)LVmk^3#9XtNnicZ@rJ&JM*W1|1Ph!@iCl^aPjo% z9Y^mgMG+Cl$B!QUc~NHOtLdXgebjyX_SfMNBdWG1Cf*J_c5Iffg+-_uJbD{qV$P50 z+__(~pFbamU%9gT(WXs(7iVO=a65f^=CZuJ*0NKlrc4?$=4+#`Utd2D3E6SV-F-&g znwoDfwrp{VsjS@jW7Vpm&$exIQ!HKD_S2d*0rOK*9t>0}mz3?@n{N2y$K9=S<~R&D zHuf4|Ya8dp^Es-YK7H^xdv@F)8=GP8i;Dbg9zLwle)HzboaE$tsaLOV3b3@q?~faG zs;925?SJCLyr{Nqdkr~qWLbx%O)XZ=pKn)RL!-{_#f!~#w6yeW8#HLQL@saPaPQt7 z73m{A=&`a9mge=FJCe8__F;~Rbm2v|F;qGCUdWtW#POGBkn zS?llL@1NYhJ^SUrfqP1Z53lTX=FIeKB_%O^x^+vr-MjbPN-wX4Px|*S?tS6Hgc0TC z2RprfeeB`JjXsUPeS3M##bx4^zI_)5SzAZXoiIWFf}7h^$(JurHC0vBbH|STS>x+l z=-RVq)`1l(+TUNlzRx>9zg0h1uO6(F$>dXK&eYg4dp3V)*)k&!d;28y4yUI&})%R8Y`k+~mo1PxtDT-|^L}6PNbw8`0CiphNcU+w1NP z8FHv)-MWo4Zrs??qq_P=KP#(9@9y2xTGgpzaKqDcb{$R4dU5UAc{~_2Xy4$nvK0xJ zE**Zod9%yf@bIJSXU*!iX5KtYy9W<0P0P-HYxVf?WtWpDr-j(rCG3rlzn(vRx=Q5b z%iAw{doQitu)%XzT-?>K{rYXzdjI}i`)AKCY>SFIJ!9m^_fNNOb#JPzExCN)K;q=A zteV93?R%#nNM}1kL;E*t*9M>1wJX){`0)j8pFBA;`qr(LOViSx7`1Ngc&ksJ`8N+A z4x2b_n(ld?V=a6%z`qFij|cvXfd6#huM7MS1OM&7KM?r)0)I8&zXAA<0shT^e>m`e1pF5R ze>dR24EW1{|0Lkw2>3q-{-=O{UEu!$_{RYMAHe?^@K*r;Pr!dZ@E-{L%YeTj@ZSpj zhXelsz~2e@s{(%?;6DiXzX$#{z+W5q&jJ3az&`-^^T59z@b?G)QNVu)@b3WpR|5a~ zz<)RJ*8%>vz<&wwcL4szz&{rFp9B6YfPV|%za03d0e>^#ZwmZ(0RLCO{~PfC2>km3 z|1jWx9{7&}{*J&u8Tc;%{vUupeU0RRe;42%3jA9G|C7M~CGal+{=IQp zmB9ZA@b3-$M*#m$!2coeZw&m80skw&KM45G1^yR+zXbSe0{>j#UjzJIf&T&Ee;@e2 z1O7jOzY_RQ1^!!r{~_S-0sPg0{~h383jEW7e+2Ni0RGK^zaH?92mW5b-vIb`1^(v1 z|1R)f1^mwf|8BrP82G;h{&~RP9r$ks{*QrwXW%~%_!k2Ig}}c8_>Tqt(ZD|k_-_LK z0o{_=f=hy}&;o_(uZ&i@?7c`0oP#UxB|C@NWMFa{Eq|w zeZc<+@Gl1bnZREk_@4p(-+_M>@b3=%BY^*C;GYTnM*;u0z<&hrPXzwQfWHOsR{{Pp zz`rx_{|WrB0RK(EKLhxm2L5@#{}k{a1N^@N{}ABs4*Y9?{}$k13H(<9|82m3Dezwd z{8NCx68P^0{y%{K9N=#Z{B40h5Bxs?|Fgi~2KW~N|A)Z;4e(C}{#Sv&CGc+q{MCW~ z3E;I9Gv7XyDS;NJlF%Ypwr;Qt)>n*jeEz&{=M>j3|8z`qOdZw~xh z0RIHwKLGd_0)Kzt-vjtp0so!Aza#L!5B$x6|9IdZ0Q@Ti{!6>O1pZRs{~h>m2mS+r z|8U@c2Kbi%|8Bs)H}LlY{{4af1>j!}{9gnAjllmK@OJ_JeSyC<@Sgzu-GKiW;I9h& z#{z#};NKJYuK@n*fxjQ{Uk&_az<(z2pAGz%0e^eo{{i?P0{(k||8?MR1pFny{}=Fo z2mCJr|8u~<9`J7n{9ge7p}>Cu@OK6NO@RMV;J*&|=Ky~P;2#P62Lu0Rz&{oEhXVgx z;O_(cZvy`|z`qmlF980NfqyUH{|fl;1O5iU|2FU+0{rU&{~N%+8u(iQ|L(xQ4)FH` z{+htQ9q=Cn{L6s5B$x5zaj8n3;cHh|Kq^_3GlxK{L_Gc zYvA7p_#X!T(}4d};GYEi9fALD;2#bAJ%GO&@c#(>^?-jE@b3!zgMt5k;6D-gmjeHm zz<)XLzYF|71OJ7Bk`M`fA z@Sg(wb%DPE_&);v&cJ^d@OJ|Ki-5l=@UIX2i-CV2@IMIr8w39c;6EDpX9E9G!2d1q z9|8Omf&Ve!Zvp&OfPW0|?+pBZ0{<((e-rS}0RE?ee;)8Z1^mYV|F6J51o*oH{~F-G z1^8D2|5d<$8}MHW{MP{g6yUD}{(FJ{58yus_!|R%Tj0+F|4+dGEbzAh{zbt5A@F|# z{F8zIRp4(4{2Kv(b>M#j__qcAM}U7*;6ESuYXJYnz+VgaHvs-};C~PJKL`FMz<&qu zPY3=wz<(U@?*jar1OFDlKLPj;0RDx*-yit*0RC0Le<$$o2>kB@e{Z;J*U+uLu5qz<)LHmjVBoz<)OIUk3c`f&T~Ke+c;R z0shy4zY*}40RLaW{~hqZ2>j0h|9Zf`A@F|z{D%Vn1;F1G_%{LmM}hx3;GYBh9e{r% z@E;8Pn*sk+;2#S7bAi7P@V^QC+W`Mgz`p?aPX_+Ifd4DtzYq8u0RP*-e+cle3;b^Y z|7zfG1^l}M|2n|m6ZmTa|8~HC5b!Sp{+EFNX5b$V{AU6GdBFbx@XrSRkAeS5;BN=~ z`zYQX;^@Ye(WVZgsD z@DB$5`+@&N;9m;-TLS;(!2d4r{|x*W0)Ktr9}E1i0sk4me=hJ}0{mM6|4G3ADeylJ z{DXjh2jG7P_~!%vmB4=r@Ye;704z?2IMp(4{{1J2J#gW0->KD)IhdCDj};N z+aOCJYal5QC1fw;2V@S!7()N`#zQ_q&O&S;MUaP(H;`n=Rfr{o;_s+KPC(j1jzB1| z&U}alWHE$dJy9qhIpiMXIm86A1CkD*@K56)T_6-=hvMiYKn6exA^wmakSfSdNJq$h zh&g0DBmhzY5ug8GzLY|~L$*T(LWV=mKuRFpAiW`8kp7ShkaEat$VSLF2!5a8(idV4 znE;`mZG3^KLdHUTAw3~0AnPH1kkt?wWF}-bWEsRB@&R%PvIlYmB4G z0V4$%XhpZbI5XIzb8`lOgo; zlUI;^5Q?948!`k!;k7{jjcSM$q&tK{aCt%~Mprus{kLivs_onFg5(NrE^+c0-~e9uPIiM+n813WIco1Vi>iCPGRf6wYcnUK_)?-Le4{iARQogAo-A$kSP#~XQhBVf;dBlL7X6qAoLpxigQ&A z34|PkP^>E7b?^NGqMUM$tZCAG{t%PyXGU!5@y2ag>rMT?Zl0rdzfaIt3+c!Bj4iV= z&Q5OcrTXFfm4`l8&U88^ckOCCb9MM`&jH(S$Fw-mexcqox4X)pgH_Ybk2I+9%3Zyn z*Yx9GE{%9Rbf((8rL!A79~rN>9^yaGbX-a0C8MCKT~D5xF?>o{dRF#~_ahwp zFS?by-(-)^-I69-Cf7u7mY&+x{rs4wz3)WiSvG$dYVEhcZEQezb!O!D@moh6G%U>h zIk&U+m;S>%lA8T+n>i`&_T0UnNBOtCl)CAjYM+$gnXT`4+7uM6)#3RLwX)sji$@)s zSfp)t>FuNr7CVX*Z4V4rQyrAo_+@9q3sYzB%3fFVR_oo$d$Hc<-d~TpT%w`bI^3pW z>d&=Dmj#&F6b8LpWp#I|M*8!QYr4G{S9jy-kQHv!|_(H z!;kvzo*p<$WAM@B#7a%|UyXc2+kIQuY{ApRRcBpfQ!SVTiozW|w)^{F#s@3?<^d2&9qFl%0WoN6+N&2n4QwGF+`=$4zpZ$dTtuAkMFzIG$ z8+omM9iz2_hE)U(T+-dlw%zuaI`!&54xV#y{;OTfXDw;rb4)2;CA}FI+b45Cl~PK?_sAxrAu|2o-LcSXi!&IM{moCX>a$h-(z}mLo0&`&E5oiX{B#Fcx%zCAE9Gc zuP|2mbm&9MpkqCvU2Sr{4b151GVSK^wr7TYf4L{(&4YcXZ(3fFOo(unwOPR^u>I$6f&-Zu?NP}#fq#rnlQRg(Z4-M}>a2KHHM+RtwjFCME!*;)Eany+S4OYuYIn#xZSCp2@5SW>)s3#Mv+li6 z{mrNAFP!vz&y6+zIIGF@s=gV0i(k70_T6~)j{5uu_jgw;Z!vke*S^+WO2_&|^-_N} zWZB@Z`6{VtlfIfc)%`Z0Wz0_hi#KLl_mezr=w20{A9iK&$a*X7W*B@9nbNc2^H(Mt z&UZZ(w)f>j!@E7VyN?XHb8#u3`Zg&0yg^fi-HFs^=RO4t-1uXtL88N*i8oxca^Gz` z?Ra$9{w_A{?1#!$m!%wO*|=W&dxxfZ_Uzy^Pxn`{%KC1%ch=Kh`e4`9g3^AGVdKw? zO4?&}Ic-p!hjZ}0a^UX^`~!gh5#T=`_@4p(Z-DMFb{4W9jj=LF082A?f|F^(@2k<`t{09O5&cJ^*@UH>>D}nzx;C~tTw+8-G zf&Vh#UkLoIfPXsh?*{xg0)I{5?+5(90DlMIzZLlR0{**!zXtG61pdE(e>>p60QjE; z{+Yo4Fz}xY{Oy7N4dCw%{GS5?E65Bx6z|K-5n2l%f7{;|NnG4QVg{2u~;7vSF#_}2ygTY&$5;C~YMPXPX2 z!2cld{{j400RKb4{}}MM0sa}l|0eJs2K*y{|7qYa0sb=JF9rT%fd6OUuMhm=fqx(1 z{|@-S0RH8`KMwdW0{<e;eR`6!

    {#SwjYv6wZ_?H6zpTOT1_+J429e}?v@LvP` z9{_({;J*a;hXemI;Qs{p4*~ukfPV?_9}WCB0RNr9zX$MN2mBL&|0v+E1N`3ue-+@r z9{5KA{~X}24g4Pg|7*biJn*js{#C$#8}PRT{uaPr3Hh-wF691OMB=e<|=U0RCaXKMDA!0e@%UzYh3E0sl3?e+cj&0sP&7e}CYw2K>JQ z|9Ie^0sOsy{}teW2Kc)I|JA^M0Pt@C{Plpp68NVB{~F-G0Qi3a{zHNPQs6%l_=f=h zalqdQ_{ReOlfZup@XrSRj=(<|`1=6=Ex>;>@b3=%djo$<;BO85#{&OM;J+347Xtsz zz<(I<{{j5tfd6OUe+l@j0{@x7e-rTU0Q}2<|0v+E4g4no|03W&9Qfw}e?#EE3;4eU z{`Y|Yd*ELJ{KJ9&Pv9Q_{DXl1UEu#5_`d-DuYvyx;4cCGQ-QxG@YewThk^fe;6E7n zR|5Y=!2cWYe+v9vfd6vf?+pB>0sn!({}u4B1N`d)|5n65@Sh0$UjqLm;GY8ge*u4c z;C~tTcLV;{fd5+H9|-)-fPW0|e+>NR1OHjT{}}L>0{=e1zd7(f0Q@@v|7zgh1o-O$ z{}aG}J@DTE{F?!PE#Q9(_=f_2W8nV*`1b(*Il#Xo@IMaxzXSg_!2c%jp8)*Z0Dpbp z-wyaY0DmjszY_SH1AhzP?+^U%1OLXre?Rc|0REA{KOgw>z`qOdF981Dz`p|cuLAyc zfqyRWzYY9n0DlGWUk3bV1OKzYzZdXt2>kZ}|4+bw8}OF{|2e?l6!SB7~mfP z{0{>EEa2Z1`0oY&i-CUv@P7pSF9821z<&www*~$>z<)IG-vRvF0{^+dKN|QS0{*9g ze>w2K3j7xW|Left7x;e!{#C%g82I-E{_4PgH}Ibf{96NmKj8li_;&^VX~5qJ__qZ9 z7lD62;O`Fn!+?K1;BNr@djfwG;C~AEKLq~Uf&U%gp9=iX1AjZ1pbM@{|4}X z2mFr$e;eR01O7*Ve|zBX3H;{)e-+@r6Zk&>{-wZwJn-KG{09O5VBr5A_&WjrCcxhW z_-_LK%Ygq^;C~# z|0Uo*6Zp>t{_()yANZF5|1{uV4g605|1#h|1Nip`{`-OdUEn_%_)CHRdEkEs_%{ds ze!xEf_(uZ&5x_qe_-g}y58&?x{BHyQQNTYH`1b++t$}|K@P7{ccLV=pz~2t|cL4qh z;I9V!8w39fz&{)KYXSdQ;C~(XYXE;6;J+65n*slKz<(<6{|NlY0sm0opA7sHfWHy& zzXkjqf&U}m9|-)90)I{5?+g4F0{_FnUk3cI0skk!e+uw#2K-L~|IxtT8TgMU{(-+7 z_;&{W`oMnx@Ye(W6M+9#;BN~2>jVEmz<(g{w*~%nfPXOXe+B%P0DmR$zX|*^fqw(w zKL_|10{=F^zXkBu0shl~{~qAq3i!VP{^`Jf5%3=i{8fN|3h<8x{@;MV3-E6X{9gk9 z2f*JF_(uT$1;GC}@b3-$4+8%$z`rB#Uk?0t0RM@=zZCfI1pZ0De=hKU3jDtV{~F*w z2KZY8e^20V5ByI9|4QKh0r-yu{(FJ{7U16<_-6tCp};=}_)iD^SAl;w;C}@84+j3_ z!2dGvZx8%81OKMLe<|?q0sM`De+ckj4E%k7|2*LT3-})b{*8eDW8j|%{8s?~V&K09 z`18QO2>7o8{@%bp5BL`V|8>A$9r(Wh{^x-IEa2Z4_`e4J8-f3P;9mj!hXemEz&{H3 z4*~x9z<(0(uM7NRfd38PF9H5l!2b&HUkUs_1OJA=e*^Fj1OA4<-yQg01paSm)m3&s@mm*&-`TEFrMSfoLyOB?g zd{5*nB0nDaddZhZ{xb3fl24d?mE?OOpDy{v$j3zfKk~JbZ;t$9B`8>(LOFlvJ>5+ere5vGf zC7&hvjLD}=K4$XclK+_ersUTq-!b`;Ykj5U|0EwQ`DV%ANPbcB1(Kha{Bh)WBi}Xo z9LZ-(es1!WlOLS?zT{&jA1L{l$?r>kN%EhP@0NUZ>d zqb0vQ`N7FYOa4sq<&(df{OROhB%dPrUdh)_zC-c@lh2p@^=I+B!5TG9VV+;pI+#DC zt|^IxmbcTCgf`KXm`BB9(MhMsNKCt-1TE&YBVnODk)euC{je-JG0V zu-NGDIQ}~H4g;xEx1O$EeNHTO_wel9$E&ZmkFTG9Kwwb6;Qk>4!onl4#%9d0*x@7M z;u8{+l2cOC(lau%vPb@DyR*3?n&QO+b??#BL1_BduRli<4}+C~8X7ce+@xtUu|o3} zhAmsQmKwEb+pfK_Nr#S|I(IQOGpBY{alW-l9ov*B=KbGEy)NcEIciE|NWMs+NJ&Ui zqyi*4QuQoNiDWj)rXUZg5P4{Z%g4Tjn@|Tyx*6lN2iudyeJzk@u@~D5G$qAIoJ>o? z`D;le{V_-SEXq({s??Myv8{ymMg3|gl%GOhPUCpgCQ^+837U>)SPyf~_W!>fue=of z&Ou+s;@C*F#|xc`_HjPK@zAy}>RGH8kC%qyl^?)9NJ$5ATqN$0cz!j|Nk|gpSy298 z=U0gOp@&h96k3Azq06D^{0jSNNn`_Yo;as+oSQFFfiK1e(#^7?Q8RrVmF`xZ+B9!a zN41A_=e8{x)>Z3i)1@76XKvEENqsFxTQlQUjrBAgx|+6c*+^Hz(4d~W3VxWOU8kO2 zef@@wnlx)+sHb1Isfvc0VSUX;b(%D+r`}v!qSeAcSGA)}>-Oe$tvXnn8d-F)G_h~f z)vS|MyOvTpjx9ryB2h=^{-Aq>?j2#uzPgH_$Wq6)I%e2I3w4AV!p$Y*37P7sKy_$$ zp-A{cJ5xe7C83=55$aM~RG0Qc8LGgbV?gi+y9vh?Olm`DKq#YnVp~GH)CM(%f7FK9 zPqEKbogI(zsT|EvnNV5SPdK2^2l3cK+funi&WSG$+h~W@uH}#gVj0z?Euk%;ePIu(OUL7Wx5JJ{btxY&DYj6L`hvO;IsKu& z(O2P^psZz8I6Tj1V+Ruyg$xYJ*M$|AciSKIM{#`_e_rrESW=wopo1`ll_*qjtEz@=#qAXvv|Pxq-R2;)EyrIH{FUM_Y+*+g8hIoY&RG~Tp)pP-=4q3rdML-XSjQRL z90X;;6@sRE5EbO9U=dOw31!$8>QO1>t3p#^Z3t~s9TLiyiN_Vn>3Fqy;=W=ZSl!y= z2z{Y)XJKC|75gI|L+HEEu23SBQ9sZx&>_VkrBo`BL1SBZ7ZA5$a#G}Tv_W<8VIbro z1Nrz1p*(C$sfd$_+d>;cThu1%ARlGoK5QG=w2yc^)D`!o`a&DR@o1asP+dBn&g=D~ucwQUN=5cBBRbR0UiSXXFE*p|`_Kre_owlRkt!$o;A%A##LEQk5&RE7u& zNVE;jaWbkwWn|Gl!ro|y+CsiqSEwZXp}aqJg?8zfC==VQZByJA?Tg0|`XcNn>@W6_ z_QCOlo}-@7&)Pc370v+-i{~Wvo9c^6I47!w*Z{hir`}3S653ExG6>t%*!FF#DcOu| zNhjoEo7$o8F4op+GJ3ub3cAdT`XT=v^^(lBBsKb)5-&YGGh$oPNZ3aOyW!FZ2g7I! zeU(DXkmN{Mn3l8cXy$Ti)z#4|4z__SU({$e3}2`5PI)3Vf8m_Y*A98*oo=-xxaW_` z>c%n7iA|?AdDx)D`)Jkm<+nWxzkQf-(EQcN4a2f$HQJ%Y&n=TosoQ3#k$mjn#RCeg zOu9OreXRJjK7JllZ4+F8}Powe0rx^%jVlMTf;n0}|uo!nm-1)E|mz&{z&N zb*NY!1OEcY#UjfRZkof3IoQ&Kuy{Qd_Fpakna3+cA;_Yg z{xyDos!$}Pu!xAnbev-QR4y_+U6`Mn9hJ&C1qFJzc?VJq5l(RpFExiH82UnkCSaaayc7A1~-?YueCbZ-B%pXedGFc@5# z4IUSjMk5?ev3A-cYz59BG8J*0q(XQxOe_{=W21LM2~`Ql+}}SZ9-As$s@kS-Y;oNG zL~LL~PIKjJ+hBESuZ?(3{!sz#c>JDoUEBD7_ES6-&A6v){NLLFf^j%SoC>ZZrje%! zHzQ4%PK%~B;z*pucvguhPDxLYW&#r2xfqjp+&5?$w{Lf;5In7YIxf+$uoO(Fj!H>o zgMo?c$VWT>|D3f??JnggMphIZGdL`T+NMoJ4%5Y}4#%zSinTSdw*5SAjXPKEj#nnP zz=Nyyz-v0U(37k7#A_nAs1H}&2d`9aNnftIFJ5WfG9Rwm2d{K)g)dj_i`Oi!(2uM3 z!z+VZ>CaXB<29RG6~I*o;57$9*O*ZJGQ5$hSaL>`lVwG3uWDSjH6pb^M%r-gNRR4@ zm0TgCZL#t!{v&XV9agr5gR+=A;!cCIN_{%p;0c@Bv81{tS~4^6Dz zt*65w9Nb*4elk{|)yI2!CRQkI!g2ahoExq8*B}Ke^ETkPhG|?NXMiRCva!-ER$yy2tI~ zup%+m12pb|6@MWnCR`iR9YV44E<`wK+8BhDi0Pmsu|jZjOu5X$>ZDjd&@=)O@6jh@zN#cH_A5RLPY7a9kf71bmLhcjmJ?2g5u%*Br(N|3mkxpZa#?PapB~6-!7=olAMHI zcSTDwAYWTTaUX>E3oLkzm&;Hgq6@TlP+vEHPxKJmeGwKY(8)i9wp{{4eBG#zwC&^N zg$Px`wiCr~`Rt@8d=9*Spe4btz$8g4;TJ2?hJQ)i?%!TpQpvW@4&o#y+4j~DPNHPn z3;S~tT8@^MBh~r0WAT5x6#sZ99eLUR_IC~(Ga@12oE1?CEpML9^jPxedd6h2G^pJO-%UUNCsW6?J7M8$8B&Ko* ziNwVxMn=?44RFgHGoDnSs|$CAb(jCLSRJ3%N?9+FUoI$Tm>Cg&L!7ZrmW zIy@Vpgi2Yb`1-0pE zklJ-kgg>9f)*;(DC2<5kl9OT)-7OFmz$y&%pF#aZ$+_o3&8 zv=ju$`5m<9Palw#ARIUKzva;*&42U<=a?RkCp_wtCq@qOSlOTp&j$a+_AsV_o%Gc)U;F@$J#L#&MPQkL_%WbpFwn{_ZabiNH|}5vJpd)iSf#W z@%jCpCX5sH?{}LR(xk+cu#{K~GlKH`x!S_GMhuI^Qzd)k!y_+8$BL$Ljf_f-NQq6t zEycNIMMcyG`V!t2(Y{kk3{EALU48Lh`R6%POs(2p(=|mn9&<#WfvSxiL_s3()Rsig zvC*~nD!l~@=Om6F>zZ&6(0B@cL(6{+J?HZ~Bop2HFydp=&?XmT-suUw)8m}dV*;X*5Pi!#F~c<~0@b*Ph*T~p zu$L)4Ym&CK^};(fw!J-Fd|cf)Gd$jsZ5A#x3p<{$;LM`31QuvpQ!bw<&z0j1O>Lrl zVxHVULj&(<_>W5sJUm#)edUw!?$p7=-Zxl|_ba@q`T9=AZqg3&$#U$);cX57Tdb|E zm6eo~nUwU8_p=M5v?U&r`jTte+P^=GqOu~QYJ=C!McLZvV%hI|>aX(kD1Xb!-(%Z$ zhFDjoUtcnkZAYZWSL5yYx2R!Dsxb$TbbpKZc`aAI=7romQOjLLt<+-qXI}~BvROCLZ^gsJTT<$BSC&#B^g(%U?Vdk6$Kg`65ws$t>T4*+$I%@CEhQ ztXxCXvd0g_{ZEOS+bnA33}&Z@S{B6YZcMji`6|qQ^gw7wd79~MqL$4QH8+aohceq; z)N)g18!-LtzBnHDL@htX?CqlF7BM@QmB+Gtf2Mmd-HvHpQOiHx6Wgs8HFuinT`YgO zsFjnMoyznOrahUq5w)^4v+IhQ`*c@mpS#cW1yRcmidwmj*>hQWF4GB24`k&YqE>cg zc3W1i&-AZq@i;FwzC@>RB#R|d`Q3%hlp z%E7gK(v5CuLl&XFd{D$r@6EhU)#<~tb9=+KmQQ+UmemhxU{@tg8PP+zvqWMt~4D-tKp45PsC4+&nyJ#wlL%s$a!qLmLOKy0en zS^nj{Y1I1u~>Wk+&zN6{Nvf$lxex_4TjXTf3ZK_-Gp_422pMT%=&;{NuFuKE+ zo$j!0bt<#UdDV~x+8^K8J8%U%+MjPw!9P6UR^M;}#<%8vX5Gyd{OTO_x{?LF16O>p zpvdJSKe+Pglk_Dh_nosP_{K%vv21GF4>n)$<#ujK|Kv;j?f3VcD;i_Giif(6*R15@ zzklnqa!D@-uDt2W%Egs@(OK10yK^*N`u7H!UgrBaPOcw0t~<^@FXq1DGCyFOVMC3PGz zjl0TcTsxh=Popu$qo{xP8CUr`$!5X77D&Jz>HTp%LQl;wFS*W-JA1FdG85-p?r0L_af1(SydP*1_fWs4>z3!qxB0vCW>o7mR>AnVUfTKbHviS2vFX5_ zGMr!9a+{HNc+1h z&bYrM(v!n_-s49Wb)1z}5671eC~Q0O9>3OLhsCpE{7^^M)o|I&>xuwGZ26$x>o8Y* zf2(Gr>k4`PjvI4+xgO>$il#nTd`TvvR0-}Nw8_}y-;(-nno z)0!3U+sh7fvW<@yovBiE;?`Xr(AnuQS03=>LtvF+T}tnK?gWl6yEN<9oy&@O2MR8E z2l8ky(|XZ}%Zil82P~B>yB&tBYw(JXm5N?l`&^8FXOI0|j6%m%DlD_D7KbdYKE%m% zr|y?jDmtk@>Ckg{Bh=S?FmBl;ML+dYm&Rk!e$wvGjk;V?Sk7))eJ?@xFjq56+wj0e zh1FVzIIVehIKRRzZ#^$6eCF@|sguN$Tv6nr!)@N&8SP)s@1I|;Sa~PQr^^W!?EgN$P0MnH)5C@*Q(}8!Jd$%Z z4!xi#^*<)fXykzLbI{3|cwV9W{_EY^L|dGH{7>J_=M*~9nFq&5qCL6elZBVgD!QGV z{ovdOjl*2^z?Or)pHbNFcGfExZvuPVw?gAHioLO|pDr5M8|@z)@Y3hBqV@4`dBOtp zSMGYxZOkc!>banp2t$m2p@+_v4JQ>r1Kvg+nBs;0wOJ#*d_v(+q+RYh9OuK0E#0Gj zLh-7qU%b3Ku8(Zb(opN;3d7QzChgx?qkP1#EfL2QN7~HQveWL1`{P$$(Y&LI_sf$S z&N+5l|1J*C`ckH7YwJ-q*{{c8E;LQ`ooSiEVX$x6eGd!t zuSZ~hSgE3-#h%qw`_Ny`c-6!CO2x-8)3yt5_d$8TL3NiZ6#c@B_Kvf`@no-N&($tb zOdNap`;;J@57+lkXg-&xS_bP%memeB|l{faEv$3c59>rkbY>pV!^oO4R?$|f8;kmukT*0=n%N=sgWh_H~Gx>GZMBa z4h_!m>)8?aCpYX$N#aIDyDOt_L>toe%q#towN7Cbue^zNqVL;Xrh8k{{WQ{YjPnA;oMN>;Z#Z|@65Wdt zvlRS&pXmKhz%y6l)w;_hMe4)){cSvPJT5E#s7|J$-P3dZ4n)xS{x=c7AIN@*-veYn znf}4_cc#BF{gvr2On+wj6Vo4={=oEmrr$CBmgzT4*D(E>=~qm@Wcmfu&zXM4^i!sv zF#VY6M@&Ct`T^7TnZC#LU8bvt>#}mDWlVESE9BpAKH(9 zVGzDxN)4WmxTGNmoJ07jm0y3pcQb~q)a_X@$(NUmNaV{L z8zkS_f#*H0^5N%%Bz~k+C4Q7H9Y1Yr#}3K7B)-dsfS-7t!+-mR-c9Cb%^effT8+l5 za>LTaDSY9WobJ)r++h3Ox*d?pD=O|>Hl9K4m)|kgP2;;5*DTJvNyjhVo_0Kqe;8G< z%X@~5>Ia-0o6ZkO;uCFV)AtkYV@BOF_*eSDPd4z>pPpK|Z!`GL6WbQJhfse!nm^i< z$tP>M$fq2k{U`tQjmqLJ8q_tuF&odPT>o9At+M%Js=JTcbB$md^t8X4%^Nl8Fv{Va z2W;v5U9(2=2O_(;w=Q*tU0G(^XB2;N+1qDsI@JE$vJ&;td}fLDWXs2Nf9;y*RWh1a zd>$g{Z%WsHVbAK!F}#Ig!E&PsH2#Hqvn_M@(QS@y`{qaYQ#-9@&vW<#??z0C)}sE6 z?_9EGEWfnLf$(ubbpI91wlSCUou>C0dZuS5*gY3lrOElrnrEkH)usDG&uV*xoX-ro z(67ByXV~9&%`nU5vs!mfwJW3XwQrj^EtfBiy7YQkG}$$y{h#FWeKb;U@0&t)V41yJ z9>3H7NmlvGZcz`LBjIpRebm4`cDeKdhT`gXHh z%On~P=Uum5Ch{hmtg@PoaDc69@T71ee|BGR(30kK|NQ%T;q0u_TQU@V_8uQtxDwA# z+*0=oei@3WcGKd%wxH*Y>KPY5q$|p^msigG?g2YLrgBlbVs!q#6RX>M!k$}E>5#4{ z*}=Io`rX^H@oqGTN-Db)|Dn2@Gu;AKC$umSBKnR%{bnquRUj3sTg==n6x z_~f}%MXt&Q$EoA!d1zsDX>zI}Cg;-$y9pg&U$xucC{>X^-&SLFx*O~ci?{7hQKV;> zwYuxp1$Nr>HG@+WkG!TG$;kDAeMo^;nzgSE>zvnWRu(U;o?sw`7k#6FdvH?#Zo6b9#QPZtL1LN#XNr(*<*D zvTK?R*^#I)$gAgBvegK7sIKML1ckc6D%b5U)IaSuD$WUtDe2AX3^5|RJox;Sc*U(R zC8nn5@I1^d+`6zLPBGql*YmeY^t|V59NH{SG5eETzibhn@3|)>?ZQSVHtd|d&bwG2 zwtd#4^}`iA2I{Hnn&Nqvv-dBmiB)7z-I}wp6^-vnuQ2;q#lV@@HjnH@&x7SL&E>-s zBYM8OB(taM;jy~>M2upL-MH(sU$lp<{cBwP7)5+crKRyEdfw+o*#t)`elFYN+3*KF z&o*sxZ+Vnr%zmFW2i7~mw!f7BC{pom=%7RSd3|B~ZnH6qR5U)abA5ALx?ZwP*U}>t zi*BobvU*DQ!{_6p4u>o5?+z>rK8)vSE>yLJM!2HdY;MZCF^ysOAH3HqOmWV(@3G}6 z_`bxIU-6wAs(5p*$MGUP+CS<3`MpCG8#45s^^}`ZdC;axA~!g>?#6WLPvNR?g}>r) zgW27gZ7*uMov1lmX4^2^n%S13mRX2eX~t|*W_K2~ypyQ8j?6Y;wy~&{?L;kW z%WNZNOPSqD)bf_1<_wwLoY~Dpt!yG{Sz~4!FuNhM^+he0h?=X@T9`K8srZk=Y-Z{Z7=fx1v_oF#9#L zUo!iJsJZ8&mOo|o6J|dWwd|p&mG_x_kJ;6tmfsOIcbnNanSF!V*F-J5Dr#jFvoACI zlBneuMa@+(`vSAii&}YB)Uq?oKE>>l%swt^`7u#*N10v5>{3xH6{41vF#8a*4~m*Q zAZqzOX76S8Zf5TiwQ`53W!ss(joDj8&2162d=s-bGJCzKW$Q$(EMoQ=X0KxQN>Ou# zqLwdb_A+KK5w&cwsFe$uy@1*CL@l2yYHkj*XED2g*{j9-PPt0dGN$D#S$-kAFPWCD z5N-K#R=$jtGc8-n@|UoDre%voTfT^uFJ$FR%NDTm`7EDl**um%SJcWmtek1tY*s#t zl^3vbrsXqP{tTASv~0R)%kx?JG*-^EY$_|C!t$AxO=kI%M6I02%9)l;VCCak`8Zb2 zv^+<>@Rx zP1GFIvQ*KQr?B#5R?f66iRCAY1s%?KAhzd0$q}w9Jc@_hIF|MJ;1m z?#c2!M9nc>`}zOx=Ym>WiIvtJL!K_-YTD5E9j;J?JLzb4b5C6GBl>$3-WGczQLb?<-ufI(N%)^|=nQwFre%CSP(XAD}Cc^$n>f8gz z>yJ8htoY_iU-R?)P85@B96~-^|bB zx#yhceb0H{bMBobbI(-L-tRqp{*_O+^`Q4ZEUR+PgWgqMRI8ai<83OlD%wZA7plJf z7O1OKf+qt}p6+_SLoSC-a9=`-vLt zdGEdk^^mMj#P+2s{fpj*CT?gk@0Hk}t4q$y-X;hAFYUL|`&YbU*1qmtF*tfv{5~;% zj&*X1dpBABPeF77eLlflozj``#R>0&r|MRs@@pq;IbpF%eZFRl7=Mczth}%+Yh>cw zVeZsE+Bv7Z5D?XO`Hc6){?k!(lfR0-_9UhamCM&MIS5U* zwV&ASSqCbIyQWtZepUIae^@TspRFBSNw^#H^7@4;qCTLmy0Xw^Xyfqa>#C4`jayD- z;YLb);iDIS(U)sI>ggSY*2n!Ogrtl8{ZwCDMfk$!`wfjwi1QoYkX03adz>)m%-ud@ z-@0*5Rbgel0fAAw#qt()a5Z7k;LYcjWQg+}-&E}+-1gP&4W3+!^#1N9C*kFTS;wAS z5cB2LBE7n>xB7u=6>p3Bvn{nXgj09V)}6mmTt8=9F=wG=Kw-t%#~;)9es7cGER28b zb!bnR7~hLJT9q*QoeHO8XVCj=cC~J*O894jk9t!SeSX58xD~1dx0<6b4qhqxcdZvt zQy9>5PmO}%O{u+G{mhy|y7$RBoi+Yc4r!or5vqR{-~ZQ?!Bl>~VX%wvPf*2?#~O?C zwYpJ`i*Tst?7+p>x>9>yV|6Xz>?bvUdhqid8eh#O`dY%|w0BxJt01n&rcLu|37<~f zUv=&{v8-$6=_yuZKx?GWEViQ17-aJBs7&+frLw zs5Pg@rZz*w^}Ev}y|$q5IQ`4-wW7XOYgR{ClD6mm>%HRnvY<_H9bxLKkzT^0a^&x2 z+nhRr?>}EVySx_HchB~!x`O-Z(1UZC3$_MjbozQv;bObvt!kK?jmEfwdpc%WxPVPK$p zw{M4umGB1~d|?UT^usv4-OKlZK@? z61KheYOeiCynam?!5Ry#zpOfW>T_{@&l(xrSQxCXb#31d;`z7aot(x(|6c=QdoH8( zpg+}Cr55hI_i3eDzbFHxb@j@+j$6Kl;2>-?VE+6CrwWvqo3SbwQa8>XzR`X#4q&MurvQ z`JLqL*i^`T+GJX@W;ZbZ`YqkPn+lH_C+AJuL!bBQ&-93FDunvfy0kGzT)$^~rZyFl z%H3*sV>q?5YQ1ur3Y`|8zdZE=x*q6#dlxnp%Ae4Byyz<0&k@wkghwqmti6^Y_P12) z*G%y0TxY(qI+gV&`y@9Ll7@WH+F?1Zf4!?uMl+#Rc6_%n@$`9_{ypFPWC27B#%`BG7sTwR&+FLRXN+}>S;md)uhsIPa+R|m)iM6qpxlUU2kGyf?C{y3Iz~g4 z-bd=47xgpGb*N+fKEvtR$?&(S-RpdDZR3PPdum;Hi#~5+f1W>8+t?|y;p9sjI#PMl zh0V2%lSAqq4fxE5%ER*GY8&^nlYckw*`3PeFABAdE`J0q@%%z;pL(%zZKKzO=3W2T zF80^r(m$@ow^wfHIVnX;`ZJe|u14?AI~Mnu-fBVSQ*zrhGoxVT2Qn}UTAXnqE z17B4e^PeiI4Av|Z_m-^ zO>FV+yK5OM8y}qAm_Lxp?XJ$QW!zV*$9uho(&sVkDoUo#&&;>nl&@1A(b<(8C{Hb8}|L+NKhXt`&|FX#W=Xh<&33c#PM9d z9^_(lb?$QEzRLh=PrT8=#dyu<`SK6l#s2bdIJ+1NpEdit_)9-(kGXlZrg2TXdsR(y z#Qc}LwY#Qq&!~0(T!|I)v+CCDn#N%xRqsDZ6#H|(J+!8=X07ik9iIPy^0D`JDYgGBG;?6vvK+F{?&gODCRfyd~pq9uOGDYJnM+-FZIIa8pa*&3){aM@Pfv>G+(G;{9s|{ z;m1D`Ki^z<@oIHrlbP+kTu(it_STm+S2vozo&Uky6tTV0Z$fqBfTj~;s`keB7w`vz z%U7L@3tL}3_|?jxRDOARwv*Abx_V2G&0@LWN@FME=Jj888oWr<=lyQ1W*ioLb!>%~=!tKM$E=lm9NKEJ)Oxr$L+^j*-apTzlEd{d}mtkZGBxX6*WD4x-` zt~weI|FPw&?xi?CEpE?tG%h$gC~Ix97+>M-#*W4dzwY{}-p&T(|Jy&XT9Zu;J48Bl zqq29w?8-*}jYp@v=hlzP&kGt?Hm)0$cE#y{XrFb*SjkxK>%Nt8rikN@|1+qP@sF@i zAK%_1_SgDuaYf^rE-f704vOcW>F(x=#>uB$o?SgH+ONDPR5bRew_^BL+r{gN_OGiB z#>>k$yE|8;&$HRRzh*laYYbj_rTpoZR37)Y;9#7S_xX&EL&f;Z|8up1G5vvizb~hV z>u1M5vnv<}-kIvXVz{X9U)Z>UaX?Yf#Jz1>k^S{TV|n9OZ6@C9dr8dy%=6nuo>A zc%xaPDC3D{MczqjpPk5a@EMu=@4Wl@|z`tg{%D$o*nEeo_&j^$x-CEDritgr{QJev zvJ+E3te`Cr`yc&4c$V$<@Z*ouPl@Z({o(AQ?1iR9Vdo!;{_j0BKFywX^>p{P}O8qew{#gDRQEPQsl z-G}t|7N&VBJjx!hu$f+{DUQ$dboRsS9Urutdhw1}))yHcWIIjWGymX3asJ$%72nSu zb~CxSa-+IrpYcq%pZ(;Al-8=M^!EYg^?Y_=cE-&+OR79?OJ&{T+5cqEx|}_7j9Sc> z+mqRUXP-TM`sYu+6XQ`mExwmMcftB%U6#1Mi=NKDn;mn-TlJ!&=r6CR_)d1qEsJ;D zjuFS7{%m$Z_PX0QHxynH*F)s<;@jDS@BBT+Ge^9>sb9>#m3?CK8KcA?#tQNvQKWW+wY4_;`3Yj^Wv-77lOC0cIK%uc#m>+KYMWh!f56`#*u_f_|sTXV$qU-+u{Om?}G z&kuAz*j3cOF3!uIR992=cCt7h>~-D7wDjTDX`Gb#d{* z>>Vxx@^dDL@o9^TzsnAa%&XqKqBtIXaq-SpzF-KYk2+ajhn}5%MELui z@HIOQPwO4FZ%OL`0pA!a_lfH7)Be2CtStS0L6^=iKI>JdQ26ebtWR2ntk;C-r}qA& zZK3d<;_w~NrRX?sVoah*x^F!49+Zj&@z= zALeT2ggw4?b^Zh4gX+T49{!&guKnKl@y1OLgpn>^?k;dys7bweDQ|Yx17T}D@AH?d z_AykeaC66kUmpl(`}`I-pnR%kREOT5?Y{RwnAhxFKj-6yJaucfFZp zxRp`saJ$0~g~P4h>sC~}$$_(+(tIQ;bB)f+T%j!g?`JL)w%+{qb0M@3!9a*^c#^Dwn71zgF>yFyQx5>-%0`Wmqt>N9HW| zC&HS2+F1wZtum+&pYZDM^F-*hXk(JbZ>gbq(~)z%L!Stp8;tp|%D3wc37`3&9XaQT z5Z?Q#?^oWznqPX(9=|F5i7+yJZ!NDY>kYJ#Df z6isB0>`8|CtA{=Np!kXKq{hPL&YBcMV$i*Uq{dH$b<%+!?6i{N8Js2Dx8Vw_|6#jRDXPldL1%2&-ApQib++}#A%PoD}M);at= zwf1~Nr-Pp5{yO+nIQMp))BY}-HK9XJb$xjKsW59=?eELKr_;F9>mAtAp-5=GHYRl8 z%$0^(C;itgYF;E<=v4b`L;sD2FY!x*^#&9PFMjE>b^iGj!(S_X*I$e*625NncccQ6AsM!X3o)| z2?pokxj*~9`%LIwxP8sc!BY$YLw?^p_k(9b-h%ow$GUAad|&yv?tc0+q0{Y?D}G2= zVMs09I(Nm9XF~Ra$(j9U&eY&N_gi5F&xBb2+sVh>lQj{y-kUMr>ABG9T!Z%pj#*{s z+-}gl^`6g#FS=g-V4UX~&3nyz@2xQMxp1w|vfpAmY&4Ai+OyWCY0rh9!hSqH>*w_v zSCvAEzbbR%+#dQaB8CyFNFD<+D*^vHCI!m z(S@;x9=s6ZH%t4ituJ{EkicrnbOwTJ0`a=6ssQS}9Q@4KWHG@yT z)BgU1mpH%tir-FIsrlxQN~>c&dMWsh)B72AP1Z!E{&QLV-Am!Ofp<>-Tp>yG{P24t zt6hC5#BZECqG$dx!^}mM2eqsGO4u-cnol>w2E&6z)g3?V@Jg7vVB_Q&ej5!RZhLXr zZS*UlQn#@e%KtT2lQ!>Iog4aBLc!I)mKHg-1)t^BjoUw6rP20_9XlnU zSXlgNe!FXD)@fX`H+=bOMzK((OQk%Q0~-zbshj2;+*vF%TDAS*2)_?C_a}eU=Fpj9 z;g4w%YZC85|@2_fah`ZW46Q{CoKMBcK%4gnC@`Fandef>X7f_ zzSuiYSpMVO;vbABg@3v~f8Dpect2bb*}xzDy~1d|C$i)|^4Yu}YNv-@{GXY4*~Ht$ zTl>*eB}mN5z+SUf922{Nz2=wdUsH;_BL`U45L=-g@%!E5Tf65-QLw;nJg= zE1SIDQ5CbvQ56q-y1`NE%B;vKF79$DvdNZ9&c}+mheE;80r$rP%)!YF$s4|}IlH{1 z@2xfG>K{Hr$#GhaS)Qgfvy=;SN&4SxTRI=kVirH@D>2?~k_5gvdz#4GuIuM+e7U#0tC8NdIvg!`KB zdu0;y`(K&F@%+A5iTVAn(tWUOm-#-|66W{6GKu;9uT0`Je*de)eR)}8e*Y_z_!EA= ztHjd%uM$7!_p?eY-Tx|aC~ucoy8l&Te*Y_znBV`(B<|1eca^w2FH3yLd_OCbcrGtX z+?SUnuEfg{ALsY6N-W*~D)A`ZE^#AXmiRWme^ug4UY2+|FH78?mnD|&f0dZu|H>rZ z%kO8EcrGtXEZzSqv2_2d#QgqOCb4w?tHk{NS0-^Hzpqu|LA)%nnwKRm;`gUYoX5)& zXYjJbb9q_fal9;XS6-Ibg_kA1$L~j#_&6_1%H;ox(x)1|2dA z@qK4)&4Zp!yRiYw_ahc&;Pd~Su=REOeJ$h#gq*3+w-@eLthjXeKi>+z6FrCZcn9`5 z$9ue&m!(SN|OI#n`{3R^4P zKBEtH#rPc3KM6P{{M6;`u@f2zy;z=V)S@THh43~G-`^eE4gNm)`RTF~LTsbukNf_O z@u(+F|FHE*p`PQ1Q|C0p-y1ytj?Mq>l;HEC*;}U?L!UY0j>Gmm!PhD3_G(XzH|^)I z8jL?JESXr@i8W}~izWZ_Vos}Hg>^4K`(XO+y6|_+aQ)7&!l8!CT%0@+zu$pT4R)Rp z=FQLeChmx1FE)C^p=u##g`>apocDGR`qQdbHEeiJSiYh9mgkxHdr{%$BMtM;31{Aa zzVr)i%U-O=-`#D&dEu@5ONu(gd-P(CmzkU9f{-&M@ab>r&KS=&A?3~mVbKGh3D0&T zUe85~&!puG?{sKXdHfUPC%A%#!=Q_T@1&)fhCeX=NaLes_b&>KwGV2m7vb+Q`rp2I zx$crsbL)pe8wzUnVkxdO*ZTe@{MO#3}U;g|}7`Fby-(u`fK9vg5jOj zfA!n{TEjBiM?0&o3i%JaZJc_tEyn-JhTL6Oh3KO-RMmUe!F(OtKDW6^Sb8mONOaq# zz1U^#xEk>$AzR2~iPw=Y?=F7~H=2YqAD-E;=$p5CG5_|P?pC=b{86oUs|lCz_t>*p zXF89(CLBJKnb9B(>t$QD@4wx6O^DpGZNcaXZII8n30H4l6F#4}f6n)#>-A!W?EFm6 z>%xrjezBEa!d`Q%(Wa@_g=y=%By=8*{4|*Vtd;S)aPPY_A>NB$X_(VcuXifn5WEUQ zM-{B6_4{##-v5U1gV(N?tF#S!u@kqhXjk44z8?MEpTDg_J_74a`Xv8`(7RDa&@V?Z ze{A8SQ}u5O^IO;0e?G1<#dm6K*iGTf<1S~<6(Aq(UZDkBZwle_pRW3BDB_P!uANtK zQ)t%JGcRWk;y<#!MNI2k!m&k9BAb>+|3wWhtcboPY>MrAdiM;IhBOB#$%tl zaBRBL0z27+hD?Zq2D`x|Mz)(2hh6sgame_1?EFYq6|qe%Yr_tJC2i4hQ$n!0j4la# z<;Ar_bsW>hx5FMUv7)Um9vk5CV}yOG2{dhLo*EGzl@Qr( zd{nIH0~^)EQh7>5Y{c}W2<%u14X;azPnwVvGOeU+jz=xVBWbY93++Nys=>y7qEVST z>}CYVl1(gfD5_){?9CJrkKiOt4EE#+!?sLP8F>j$iVvX>O6t%LcGW@NWOWhI>V%N! zDIww5FGsSC2umtc#)MXJgEqryskV(X{G5DHN?-mtjnxR}r=>ZZgeZP-Jb z%Se5M#zjX>fkZ0PK1|qyQk-z?`;&-FMkGYE3ylw%9u*yB-ft>4E`Dmn1QgoEPQ!5m z?MGBv7aBJSu~DBTbvVWvAoF(8d`897?9qw;j3h?r39ZbZ8!at-2+ zOpJ)dW=~jEye=dOTdvXY#>ZnrtH_dWq;eTQT!TbnhtN`+(1>UX%Dhf#bhHln2yrZ{ zN5E2j$@z!fO{McsYBQf}rRSGaPv?$wJP|95CQT}1N2roxthT7o&OZ!V}Lsn$*O2#82D7V!G&Dq&jRJHX#HPPiv&K4tsw^ zjgO*DyQDfg-}&h-)e!@ctR%7UA@wRwkhlc!1tdN;+4Psxsd+FkEvcj96R9;+{f3NW zS)8|VP7}EgO9c<{@5|r$aK9k~vF$<))xf|JY$S z0W3viKb9==+bmM#AuLejfy`IrKFm|(cbQt`;mk?ok?iGC>Sqiq5P1~K2ag!lpXGsl z`;A~HsD8)*mMii|c7(G5JaR-Iwhug@?{KyY?Bh3tZ6kgc$0jv)&q~v-!q~v-U6dRG8C~nTn zSP(Yx3&lAQG%XhB2zb(UP5k&~Jf48){0NGiri;VoWI)XM8hbKR2Vjl#!HBB zHJ&iz!&|aoJVS&`z-Dh##H|XkJD+YEZPABIe-iCosK!gBcs|%*u9*@_W3r{hAGrS` z{=%o>#Q=tcr|#q^+TFUOSK0wuj6Yb?@GYltagL{b@2E96B2pT*;*exon*+DZhipu; zOrY<7<;`l!Sg*j_!TtPLa8h_8Uiu^^iQ6Zm?O)j` zabqptR)d!1+G_e_?Qd9V7~7O6yimfXns|j;nxdEx9nHJNI>Y6nn|~v}TR= z1Z;X3f;|ms(rDA_Qb*=_!|zLl&`z832uf2`;{T2I<`rAUYcRduilv!G|7FfuezH=1 zBF^u)@n+2w+L#e@HU(RrWA>>sn6Csnejo3=W&(Hz%QSdf+Z%8Fo8!0xfOqi>?+jUM zVCGC{u=Vu&u%VroJ+SjKq~dPLFcWuAZjSnPy<3Yr4!3DT$BdbG@D#UCWwaKXK#O;u z;;z>ke5%qL8op-5^$Hg(CCB&@9G7P2nmg%a#(ac zg(upK4@pQg7lMKkBco_;&=3@qkQiQ~(S=a8xexsMS6DRK=|UCmL3Q-VT317r32Xz> zfTe&QNCcvRa9}jx5A^V@uJQ$IfP0@;S9ycI08gMT-~qS;Zh$M0;Zt3u0vGMBuG;s0 zb(Is2O)IOb9KpFk)m08)2E0sz26zPMGOMc!!S{dyAP4%JU=wf|$Op~>dB6!E7dQgs z00tln*af5mX~0q-5r_l=0WZK6c(fLNfPFwJkO%|-o`558Zw>kb@_{@c2iO3t22y}z zU>u+UT!DKZV;n#lFcTOJcma;U<<+nUD1W=aslZHN9H0f{d+T3uuA;gI+yYvts;D%; z2w)ws5olSbiYlOH71a!2E%2yz6;%;X74>RCP}7{w7#|&y_yIAV6HpG0jf-tZ=SVx+ zWEE`@?R0p98AIF8lRhaH*HSD#($aRi#8J37gIGc&-kjlxcBH2Y+Ef?BG!fC!QM!bv z1gvNL>?CU|t7uCVc+k^o+*A+NYApPyv6(sQf~^c;$Vl2z8hu1Z_2{9Cilt?bQ^i~k zp(l8a*smIO*btoshajM7F>y)wWdKBhx0GS#^BnDXS4nT~&EcZ_@9x^M9zEbAN!q+N zEFlR#@p%U}?8P|6=Xf>xrk5b}o+l{=#KPjF8ahUHBI3u>R=;Rst?)lyMdAIqx~Wf7 zH5;243GeTr7y68Y-aq$%0>|;#m>;|Phogwif2Uv~#0KfIP8$A#q%=?1<6C@*BK}%J z4Bn*TxxVxYsg8_@#zzde-bsO4)61{Nk^%ZEdp)({lr$mX6Y-umE`A1%r-13XM}8cU zj1o5LM-D$|iM-LvBsHxQtTWo57i~VH#4t35hiPtU- zpDQdiHkB4)P$I5ln1ha;#Gh9Xk2`lV*NN9lYU_&f)UIA?x+bCwCHA!zH^AjvM$;NM zuWuRy+=7t*@E$#4lkj7viPR0>4}2pX~1n<~v*pNO$u#tYVfkXO3PwXr52(dm) z*S&M6piuF1DD&~e5PV#Ox4c1NxP-GXUB^ycJcD%jI3yv`T*W9;u^of>MhPsj1n zam2LHumrOxQfD4dWJG9u#8k5YpS?v);#xexm<3{--d(%$dNF2R;PHzC!;@1~6xW2r z$52UTk34k?l80>09glcT&mX6!XZZO!>4=zBv2&XB*N#-IMw8-qjlyFIl>5`A4ZM zR<2sNX4~%VYcm{F#r4#!+qUoAy=Sj}1KxgT%-E2qxM?#!m_2XH=B>NFTwlSt_^rmR z+O+HB-9zKs-+$ETcY`Bilc&#|HFw9RonM_R>Hi1o{hzOK!6&~@#KlWt6DEE5+k!=x zt6%B+yEgo)DRlhAh1ce%yjRK5sj5)N&9$jV3->PF8Z;U_aOkj+Bfbtwm^?LUPR#Xj z6)RV((z|xuT1{FuZ|2pte#1e2LjnSa-*`V>HziRYeUtJ~=25;(sia_8U780jd2=4j zf;Z=(qz{`1HSm<$2T$&l2ex1levGCs(}v^89(iDI%md?jU~k9+<9YaZ*Omcug=KaB zlAlcGf(liNYt+Oq9M{7S8#i-riQhBs*u}e7Z$aDFEKq9y_ejdtIO2zWnsml5@pyIh zWj_6|yMCE+2ZxH4E2*8TS97WDTC08oXH|>l9m}B+ol`Ge(ML*%q+~^0?)%BTDJN3(-mrAZNp-~g~Dp?JzbG-bFcXS|Ji@~&+%J^b)8i`%Ke|&+pX*w zuxG%Y0ec4Q8L(%-o&kFX>>03Uz@7nn2J9KIXTY8Tdj{+ouxG%Y0ec4Q8L(%-o&kFX z>>03Uz@7nn2J9KIXTY8Tdj{+ouxG%Y0ec4Q8L(%-o&kFX>>03Uz@7nn2J9KIXTY8T zdj{+ouxG%Y0ec4Q8L(%-o&kFX>>03Uz@7nn2J9KIXTY8Tdj{+ouxG%Y0ec4Q8L(%- zo&kFX>>03Uz@CBsiVO@N;X5L1=})>}W;Y$O)%mZ*>pzx#AxD9hWzxh<+0*(Rbqlh;W$)b=JDYHJJMyJUW-4+F}1z&$`dM$*zcqKDik+54kS z+9y%o=J>2MZnAYl+0w7*kpGtqKN^`0jmID5U@LZ{wKg8PJ-J;CU8>9{jWZqPU4WE- z(pbyC+>U%@L3df^i~7HZ@=FVUq_dU}xh?rqHLt5``@A$qWTQdZ4+!V=q_H+OvTKVr z@?*I+^xT%($Tk(_G+rm!P@8;cHHTcoq*Pi{;89zi$S!e1oH z$rk=dXU(77mi(#O*HvX$_{%~$7bx@m3!OE8a$EA}1znNMAK5sTcUHLq()FG+)~?ZV zJMuLeI$zk)F+IpvAj-iqU!<|-OKwNDGoeeB`J(GeI?98$ya+mgSt(0Rg^j_E<;)1n+;;g59I{K;*}z7RUSg}+pk(=Ggw&YC~DE%|fm zP*;^_;m?F}A#hXnT8MPk{K;*}Ut8$Z4rSx@L|F^S{gKX^Ke;XW^M_7n;ZKips)aw& zS@S2iC4V~TaxDDip=`47M>=c%_5|edlFphxxh?t2f-cy?pAKcc zg+J0+^C!0@e2Xp}z z{(@1~S@=u7XDaeXO#+&`y-t-e{x&$ zmkOO0wscGnx*i0e98ADMG(ui}Gl| z+L%aBW2T4voR#~awqWQ|Ec~XSoC%x&EMq0THNWIzDcY>%p#W{v7d^v|b8f8yF9xv&v`IFm{eK>T%7XEZ7>n;3|&YC~DE%{4@F4Mwa z4$65zTU>*r^OAJd{K;*}-%aS4b7{O}qe8g>;LYnvW9>X9JBME8&w$96e0>LUTdF79 zaG5`Nss>aE*)PFR}$ud1@to1LqBY!j&dB0Srg{{HD{|S`y zW&TNH&A;4^e2s(7v1ZwI;D)k0AjL)+Yrf=mSrsny5}mhqD4;3W3dwp2GW6iz;THa^9CY; zD}dutCzcHq0Upbo*jQi*a2UAskrS(y>csj3%Ynl{0pPa6iG=`ZKrZkC=)Ka3B>;PX ze86!P`T-(=G~hMhu^Rq?6d((5{usW2WFP}51ZJ(lF^~^9u61HQz$_pG$Omer!7nfa z_!`Iu>aTNRBYdmMF<^f*=Hvq?UC#D5-KnCyx@ctAz0k#5XfC^ijSZ5#%NCC2d zwp%ejKq`;}6aeaN@DC&dxj=>O7(bu`z6MNyY6tuSkw6Ae0J!Z$JU|NYC*Ymo#3ljh zKtAC38R~#_zybijHbA8`B;xdmncIY5=4a6SS{fCfhqE6_L> zeFCXK4p0PiK8CRYDZpW%`*A0>7bpPQ|LnwKflq;4pa}3d;l!eVtw1hNyAXHy5Vz18mt}ti@E4 zZGr6AvY+*}OLaSC$2qe8sGsf)%JwH854C^R%67X;JKODlb|~9^!lM5fb#2QZ}K z!bMK1rWIGcMW4&?iK^p)q*^K5k^f{eVbKCw?xlNkPMyp_x z$F(%ZL>QYK&6bwNr?=4ujiVau@@#0xpC|kg_Xg8A27+lEfnXX(5a$@qGdM5eyq5EJ z&R>JcUm;jpa~>^i=R&=#R2!(E4UlQ|3R=BPyGucv2`$29S=@$lkZ;3=VrvhLr-$v> zOz=nfxXbwo=W@%<^-i4Ya&F4G9cOROKAeYe9>Y1Da~$XCoELCT<-DHr4$gZx8#o{3 z{43`xoC`QV%7|8jwuKJ=o**{rX1vP&IeQeZg9Q}rteBT0h3<6 zx-=Kj3Vj5~v}p=jy-a&VL7OSl7Aj~>&|>)PE|}tc#>!1sfgLQlG08wCOT!j)FFik0l>WzD-~nPXYHq@!YfFgVu*5G=Uv#$E;psj@biD z{d5LXKRtQ5KbZOo1k?Hm1y=&=z_d=%!PJ)+zd~PmGHs56maQ%Idrv{@2`z@iytxgH z-_6r@+`-%?5=>)H;I?#5_$%0^b1m7WC}{I!+InIf7}dDlo-YA56Jv0jBe_ zBbd%l4Vdh7U=&$0nC#|(rF^#SY&$oZ+%}8b9+ufgD%cisE%mugL91S0dXDEQXai*0 zmkL@vv=n;^n8vY++fu$fyllswBkLzzL0c%(rYdOF8%kp}C};y@+5!cwUZ!>JVmr1> znO18)6(>_wr zX3DhcuC{Ywl4<=Fw5m;|<4so3YN4fA{kRR~W1E7F4qC|A3~oCQOm<7S4?4f|6?|lJ z9|yP%**EB>kOQuzc?eL@sy3U)=mw^_BmXlMY_#0QPiB*;V55VU@|P^Lxv5~2&TTSe zHg4XwW6$F@`7#@S1sj%Lx(@UTT2E+^BIeC)C||o2Y=WVs*dt{&CIy>RZj;72o!ip+ z=h|H%2hh?O^TD(RZg3xeb04(s0u+3xKQ)ipgR>`CdY)OWU>nSBBDoFaC{MvA6)=?XRh&{903Wj1FOZ1h}9bLQC7 zcI=rlt(OfgjhEgp%(tPTwQ1txEdW#h55ZLb8cb`h(pK{|zXq6IkGut@dUr7C0yvKb zOF25C(5If;q{wWF6l^lNO_t22t;Tk)Ok7KG1uJM(+syOk2A1Mjs$ipqmU7@Hv&m7g z(Qz$}>5&aBje+v*+sk&0>F@^`%ix>|rt>ZfO!JfjrZE=s_9CzpU$la6^>(wC#+t66 z4S*IhHkxxV=Sc2@u01Cde57)lG%)qKmGc)eAFAHA^O47W0_sHZ+v00Qf`bY&7RE&aq%x z@6*6^J(&+CeGZs>Q@&Cad>29s87tz^i&`{vwG z9I4!9J(&8~%5BB#pMtH4Ysqf4f>!;dd2Ak>JAEsw>dsXFpW{gxjvZUq&XZ(5G8KHJ za~~O;zvi|y7mpNdP28q{^Fx`f=K$L|Q)lwE3a0oy!P5LjE7%5eE!kx#Xj6GVX}q6w z&R=jJl;^VwK1|$40ky#Im#wUPjEe+!JdN7J?37GV2z|#8m zx6v1A=sLQ~hKBmh> zoAX66(0hHI4Gs0Jhd<&J&Z(T!IHz;Y;GD@h3oPa7tU@0qZd1UykaH1d_KkTAo}9fo zYdQOYsW0*QPoXazw@KzUG#^_3H|L3BPv=@%BZ&&yJnlbV=09D*M)j?E9lC)j4jR*0 z1sg52G&Vn(jpN(4X5C$0 ze~R-J&Ms)L0$oGS9$eoUOm|oG<~$Hg{p!IqM(WdHi0zm&Wm-=KtqEGVWCh%Y=3$(I zjVjA*Qy(nNU8)Tm@_!RC5Fg=8=l`yLl~j8H92N&~UliX>8@@=ZmuXc)6>=fdY8134 znKn{EtJ+^WAFCC#TA4OSL93H#?lY+j%g_w7v>j_4lQ*=@hiVGVKNhZMsaG ztDrT>w2u_Dssp9tRS&bB2dzx&ub|aIOKWxpx1n<*S-~b#=6{!h)+E#BD`-^)^H|)# z(mCfaTp?C&<0rH6Rc)WNmsBb-W2xVC1#N&#dqP30muZU>w3#xk`$*gQ zGRd?73R+c8X>2nUv|5=qLqV&PY0oNX)1jsL&EPgPw``Q{81uMIzRbo;!G;|yjWtq1 z>j^Cl!<*aCm{u#;1k3#A+R#et-t8URF{bh{q=D&tO$XCBGQhO%Gr@E|n84Dy*DCl_ z9V*pED`>UQl7ByLLw%>&u%R(jg{IJkhWs@;%&%SWOK~NG$@dB{#q~LuUSl2x)Aivp znBFTs29tda*wg!*I^0hH7)3S~O#Z^ah%6e&%IjY9}T=F|ErpoPXnd15ED&?}F*t z@DNP*P`w7z`@kwkomADqu3++`1xs-_jeB!$NK+Gb<7{ZipAP;YV~e=|Ente{b1>y^ zFPP^0dtN4{IDQ1v_c(vyWnzls9GK!T@$y|T#qk78alGdBRdStFlxJ747#CQI>!yt{ zlScgh|9fwaU5pF<;GQLeX@2xziYtYeiK(5K^b%+C`YbT@MK;k2eVMpT0hszKI{ymikh?Z#xfqnO38q&E#Xrl8q(Oh7FCE<~qlQhH|SqZjR9e zzcl6o-cKQza#RGSHO8Q$GBK?&M{d^&OwVT>!PF-mdj;8!I~ZE(I}%KN>cBLYGr-ix zLNN8Y0!-JL4PfeXCz$N=z$mhOFxi`UnV7~vwuuU3Q2lK7;RYrj&A?wRfoM#U( zty#)*qJm8T*V3G4C}{QE{|cG^vkEpj+~%~*#xcxx%; z({51Ert`7vk&Pu+!Nw%>|58D#$}3%u9^tn0qJ@^ObAH^0<~LBmMh7iqESa;O+tT$c zMZq?cYbmEk6tpI2X>0}DhQ?N?V52%+8oPUhLhLeafPz*BExKpPU>cL2a|-uC&jT|R zd}MMT$G{Z-XpTl_D@p;O$o(kGvnRc8FEuA~`T))ePhVqjNe~_^> z&go#P&j8ap$pll*vN-2(&gJcSVAAJvW@k#{eyPy6r%dZH!FG;o%?Hfs;5sRf`z+!ZmsYkP`Tiv2yW&RM?Y%78E&HFP&^}^&S^v2f z<8#|t8fV~+H^*5uqpTmxaa8}zcKdOQcFXzx@C&oO^mn;ri*f$-)tmi0e_vVE44`~U z^I++}gQ0A@<@nqDV7t9#w(a(>57}B{$kKQjmzQ~WIb!F-Q6}OYxE!W)$H%FW6 za%WT9?fPc6+kHBeZMTdoNo%{kZy$5JblzEBA3S`@+FQnF9AvwF`Ec9qYm&;gTjuxc zDYo0UOtsx!JGpGT<@kq9E8A|l&URX~TiQ>YVYY7z?g~ijK<>w7rlYDpKx@ldyK$DI z>KO2*_6KNtX`|hJwxg;&@Fx2pv_%1067Yvw@2HZ_MTw<0dQC_$#5tF|M>&CGs=Ex9 zbi`Cg4=JvF{M?tys=4NI>E}4A76DSde4d}qb5y+m{0& z8d$pi&~F5gMv8~7Kj#*g^(*g}v>)Y{ZMQssvi>OBE+1FMV@K5yfc#jCi|UWPu-#tk zrMX=?|7Kd`uX_0^W&B&Nzt0?Ow_mDMw%yYIk7I1N2fbUi-7FaCqR$yYGvW? zpGo*W{~UZD?FH^J{|=wwPeU8(GH0QTdNp+HF&Idli|_9mBB-FwKTfAP>;ONBVi#!w)0PIrIaKC;HOE-ZTT+nWzVAF}7f!@Db_( zE&9^IM=JU+M16omIhF%I`ae+q0KnL_@DU7u>YHe%`u!LKkdA(XVW$Rk=qnTYJm^xN zp$MwEpEp+=}3%@yl-iW&0I0jOo(W0Fh(2nCmw6Sl{j${3YD5E|Y zHfkK}$p?_R8lW-mg%6BPorJL=2P$w5IFDk*nC@V%o&!}<^l8s{DE;mET9Kg0QAXsc~*fru!^h_tIQl(6;_p1 zV@|9(tHEH;YBCpAi@CDetPZQo+}K;J9;?q9u!gJ=Ys}QF32VxlF?ZISwO}oo2W!Pz zvo@?PYscEN4$PBvWSv-N=Eb_OuB;pLX5Cp2){|*iFV>q0Ow0PPzRZXDvVN>T8^HY7 zKsJaCX8!DLHiQjj0c;o>&PK36Hj<5E@37Hq411T2W#ia;?0pu*f>{U)WnnCwMX>Q~ z0*ho(EIKMQW?I7J==R~!(QHCQVo+k@v>;tbLc-Lz_;3~(5*r>J5frCOjEaj*V9{|Q z;X&aM<3p076BAg>w7Brlu%P&en7AnsK@kaIA-V`u!dXzr`0){8i4oyJ@o`g0DApw< z#)ZYj#6%_XiZFPe5D}D=5D{Nm6CVP*_@D%gBsL^Q@)s5zmr!yn_YoeF7y@y`B<>+R zA~b1&bet5U!)Q?v6csx@j@L%SghqsiM}$koxbf0ae0*$)C_X+;8k9M(poGNus94GM z_=v=?$e{625z&$#Sxr!QR6L^RZen!uk!h|Mr`Z5FHd2rHd?C36T-WWiu`tcT{Z25{imNM;MVb3zMQsk`Wz+ zp@xS_`4x*|#>&)S@L>@N2@z!~6GK9yOKM``;-#cYqS%P!#IkvgO^PwEh7t)XWNTkK zfz0!yD_uyFlOp1$@dU?5;Or7B$~s3qvJ)2@Te_&kDr*ZLM;WIy59q zI$JP8`N=~?*>XyVh(?ddAvaElh~?uesSqa_)nzMW6GZ26nFUy)Obm*S3qxR-*eG0L z_*Lai&E^Y{bV`y}zQW9zmCPcO62s%BN-GvuorJ`YMCrmoiwu^M7A+-*F^N_$SqX{M zkeC=BgtMZInCj%dpbL#cdUQC~V&qD(w#+#xI+b70#0tv`nOGSiyCg{!CD$c$MRG(~ zQfY)VUh(Q8AAau5pSSMzx z%mq$X9TSllLK%^Vp^J}zP03`^7?9Vp^I}ePkTr9QwJvsMx!}pCxXPpBVkZPy4#09b ziR#4C%R|Xv|5cS3DW2PL&H#P4MtbP0{=e~Fj=Paq@+y@jepCIM+W&X;k{^jB`~Ob%;hjJ7tgb2q z?g0hBO~3?P2F?N}fFnQ_unX7*qyejeRA4EP0?Y^Wz)T<+NCb31G!O{{1A%});0usm z19$;#0X5(TH~|cJ*`d0s2)GC211Eqg4yE4^}UlPOr=SzXE7v;)3IBvtA z4T9%Ro7;p3w&SY#;M479p0nBXl8&^q^de_Ke{k!it!i`gzy6=|+FYgO{LOz*--qkJ zh5po*n_KF!TU!dYjD55JS)Ym)!*EmeuuJb>QnR2-6EAi8xBg82?|ZlFWC~)C&|e^xfco1BkPd(H5XW~COzjr{YIj3Y zC_e3OtX zZ}V#d>X2fPKh(GnQC=34%b3>v3ul!Yptf?r5g)u}0WJdtfFEz0KM1c^fJ7h~a5Orr zdidgX3yyvL@R|tl0@?!PYaHqW0Y5+kcmZtzcfb{J0vJ#<5U*{3%RnBG3mAY5AQhMi zL<8dhU%(r12iyP#6r#V&zzHA+$N)9~OM#g{A`l4#05q=VfFF+aJT3>6DUN7f&U7iK z($+4g(!a)1zr$48(@dqpab`Kx&0wmW`AlV6f^sHPsdt-y)7vx)W!O=jn0F-dQ9r!? z0}KG=ReEHhEFDW+_@vZs8@HSPAF+E{Y8MUtIKT@azfzyp`eV;Z?cAYv0}7D?$%gDD zyS!oLR3?Dx3c<;{oK=}X0igcUS+z@JzL#OcIOH4f1{{E!WAS3rTEG)<1sHJi zUA)!?W&)9b2XJo;<`0Mlya5lu2`C)>zt}qq04b~fZO;NKCM$x3#1aAu2#A4*(h@2l zDW$Z8w1i45N(+KC3eqJFD%g~ibeDwEjWh^+*Kg+By9eA=4Bq$e`!Ra$eVzNsd8VG3 z4S4SWvtbZ)gzOLw=PGc!!!+m$uR{dvFVFo6xMSagU7|)&QuLbhu#Qm|H=#rD z5*EWHG{ZZ=OPm-kHa#Mo`+R7|Fz)DF%g}(Yad^NSP{iM4Ghe(m$9XW2T25&YUId zBiW*laqg${Jo9Yce6b(u%2leq@@lo%r@!5%ZM*i79o~7jW2eqtx_0aSUXPyd_v-yY zpT7P24;VOT@P{7_8T#?C;Uh+l8a-z0xbYJ{nK)_kl&RCE&zSk?XP?iS{l%AGef`b1 z-|gJ>)6cv2?A^Ejz`;X@j~qRA{KPLOPn|xq@!a_f7k|BUc}FbM&${VX{`U2c{Vo~0 z{;^O0N3Q>zx%1{P_j{ux$B?m8({-S-WoihK-vxZ`r!-$L%{}nSRzy|JPst z{9LM3u2QftmttHBl_<%j>c&mw%9X2Far=%~rk{1wuh=u?|Ju(Z)DJY^ytQGVQJ^vB zt4%pqZ60_d&?3+>&??Y6@MhpGeo&}QplzUCpnYIcM2A4~h(2MdA~NvQmnLFioa_QzP9|8I% z@NM9`z?{I`z`Vfxz=FW{frWuZfyIF(fu(^T0?Pu+11kb61FHh718V|n1MB#qrwyD1 zZwhP0lFRy>_{1?mPJL>WOhtGfh_Wlv?F~na@_YeJ1v=1|V-`)jXd$D&+H!pwg2y~5c zc6TNh+|?+ zz+-e2kLzjn`7rP}-pOOPH+}dwVd1HxmI!^%#2%m9s>S^HY|UrxHhWBuPy4L!+Q;Wv zIz2wy%nL1>%~s$p_Wc;!-l|x(r}<*qUhsK$ zhIZ$_|Mf<{?%d$dRsZhi3Z;f{4ltD0eV8*M`Z=dJ-kWoch>?6>YZ&JQJm=Vr)r9#Y zerMj^-n=&dwdX70EE_yuVcp@(Yv(IX?3@N79e?F~C9iGFU2>3-HTYf~==&`Dc^n-7 zK7{5|UeEaokJzxHixPyV09#IvB+-a@<+9Q3#$9A9aH)7-HOK?im(0ykyY8)P(@IQRv>45KO+E?4NpfC=8dy?n4 z(CupnH2Om6F8;NQ#aSyeGcmSX)s_p3!;fIuTw3v;$XK=;YaQEuMEAj)AdoO&bn~a| z>X~D`-dE4|`y;z`V!d47vEt?p-N}MCbF4=vdV;%C@WWbv<`SNeqZj;N`y0K+-&k-K z+UO4pXzYpBEgRh7(AI+ucTQ&g&1#~G;7n1YIB{ay@j%qeMLb(QGkFW#C=J)HM7UkD zeGA^r;>3yGAGU5kQ*hNK5b;Ml7}J$>UFaeGyI0B`X)k_v_Na;8R`o{3t>dce6E%N~ z615V$;#~F68fasj`wvU*syfb5YvvG`v*zfUBjC1i?e!1+ zB%HScfByKpn<9GqtnHfXZ$lk_v*_!uyPdV6OG4eW4Ba8HD-k?;b}fU4IyfabJ?fCy z(mMWjCCsf0UZO2HjNZQ8@b2Q~WGSsPxD^eq$*7rh3)$kq?T5DQZ{ORI;5G2Be`urr z*T25~sK=jeJI3wXp|zcfzW!0KGf}(n=eHj^GPZ)y^$(`MeG0AqPhJ1$L(bySgR3oi zf<3(4G}{;3q}w6)$h5=qn+wlJes}fC|9Hf+WoRS8RqM>tRSErPx~n$hZ}KX)yHoH! zZeK%FY$u|wSkN^KgQ)FuU3LAL?5?lK~2;x5I<_-ZzOmZ_)YuQ3+_|5 z-DaQ7`j@L#7rZxK{nUNc^x%N$x^uFkn5O^k`gs+Jb^32t7Go0;VXrT-U$~;7cX(_m z`t>Qa)vF$SRMC$&dxISRH}l811OGs&V{qN8y6UKD>A#=9yM0#gA7$IxwPSml^55V7 z?)vmRZ7Xt@-=E)~8d|{Le$d@4x>sF&dwaKTEb4E;^Wdt3pZ{n_|3}k5hTir6U5)KF zutOAm{C{)6qZW&r@_*o%{_Yk17fg&gsCoeWsbl(=KXv{`K4*^oQ|Qovx2qESJb!me z=s@Wv*82=?IW&%beY<*k^tTRTo8P{fVgJRvezn)T{%f|!t8Q?Af-iHdD{34%!LYU3 zbbE2rmO}H}3-?v?2jWLx?4$6AsK@3t5Ca<>OCG})N7Zq?>%whiygAyapSu~*4QPqb-p`g|JgWr zT!Z__8HUXroFjNEn+4xgMPDV1Gp;|ngawy!I~LmF&9DFT5xDkt*Z4ZOquGCx-txbh zX2n%^hCey**8LA?%p3m)R(#`D!SRb>jX-A?aI^lZ@X13Rw zJ(iWmd|Rjuol{@C_Rv@>c2oUf@DRkjlv~#J?D{WJ>kG>N=ofod{}Y$c>r?0j;CjB= z`AhI+C3wQ*jv*fPl>c|nVyo`+x-42k2-q`olb@xFu_mKfQ?#zy8YoCAM|NwEi32 z{Tm+tH}V)n+{l&RT=yRxfv6uo{-YUh(DeWI_HWSDy1ClxJOVdw|LZ(Z|D&@-{JGcf zzxDPncnJU8_WsV?*LRD#E%-ABtI*DRr$kr35f8l$4*v8RdV}8-Uwst(U5(MNp8bZ7 zetS;%cQ^M}?_n_>i5NLQruQ(x&ejv${(toT;$P!y zoL_&sc4vR{^Bb{$I379x*R0YTdI!jB|5?=$<;_}X#aAU$-A+WM-3Gx+@O#}Mja(A? z9qY)YE`M;j%;j>ID_pL0xhj-yje*G3u3zJFt;=;T*Sp-{a-+*lE;qZ};&Q9YZK1!% z5Vf5jUBBJs4wpM!?sEB4D5IA9+4Z|!?s2)-<-P=0J)k4^yZ*p6{Xy3sithF1P-)wY zJnZH>;_|4=V=j-oJmK;emnWmw>;DZ3Tj#%Z96TPULVrW!-+Vp(t;hf7e%rk+`tf5P zc{=)eMIR##^+JDV#nP4& zcH0U2dq=|l29mIU??c$n*bDo+cEbJ~zOcXJBJAJq5c)SfEbZU>5clB1{vyAyDXF4Ev4 zEiTgGB0Vl1#6<>NWW+@#Ts(w}hjEb^7g=zT6&LzqmYTs(#g z`!{IB6S#O17rAip6fW%FM-cW8F^D|4cm@~G;==wWkI09M{CIV+EC4lJ79?weWg)2L zvM^a2ET4lqE{l+_fn`ys>#`VG4=jsAeV5OZ4Z!jRc-`fTWJ9nl0gYUiBpZWeDQM#I zC9)}4mWF07%aF~%vMjvevK-k0EXzYnmleoXVEHn%c3F{p6D%viTP`b;q6#jm;^Gxt zyo!rzxVU-zL$|}9J^pWV3`HAUw8cd`T(rkUBrZDO;vHPPi;Ir9=!A>Txafk5uDIxi zi|)914;MXf(GwT%+uEH1|3VmvM;;NlZpOvJ?`TujEr6kJTj#WY+@$Hfd> z%*4f~xEK;Qz`uUL@ghGb#VlOR#>E%7_!1Xi;o@ste1nT`aq%54=HNRaKg@+)&gYRo zIiFAd?0f;a+xhq89_I_myU<@6%=s$vI9RTR6Ho%yz%Nh|*1}2W>&R2i*ORB6Zy?V&-$;s0_&KoL4CkR7Y=H~T zw~`l~ZzF$o{v&w_D#CWS>~aTblz}<06K)>==;MEu;~{Ru@oya#`kT;pyZ9ICsP_PI zaKXd#=J`LzAel#UTs(k_6u3xJ|PcnB8{ z<03OIvfv^sE*`-}He6)KMGjm%ii@1McnlYhBculY@1hrh+b9!yCd=BckEJD5pmPMhiOM6bQ2bRU5zRTyy z24MLDyzcTvvLRU7b9y6}_MF}rEK5NXmoJe`!Ll?ob7{}B&B3xPyy4QG$6J79d1&df z0@(^IUxwB$?fK$Ou&e}cxvWfzD!8bMi&t>*DlV$w;^y(cdH&yn`-A9-i}!KS3m3g{ z@c}OS;G!=s`r)EKE(YLYAT9>sVlXZ~#KlLr7=nwTxcC?s!*DSi7b9>n5*MRzF&Ywhp`FirS^9|$~=Nn0} z2|ovxo8df^gDr5u`Bw6x^KIm>&VM8?K}FaOmtF24#T9* zkh`6KPwsKPklYKFi(nrVgvGEQ?AR`W1J0L{2c7?4JN{?cFL5J||5fMzVVv_rezs{R z|MEC^T-W;^kM0)N`+h6kZD7aT_xN`2D?t3D-#IE=Ke!3*C_dO|HGS~Z_obCbF`<{X>B@|)1zNex~?RwuM=+d~} z_q25BT-RkaT}H6wV8cm(}&YKSGzy^}c7PtH-)+ z{YDNq&!c2cu=M>g#*agB=Jovvx+mdzdf#)=iKp<~@B;Jr{xn@4*Zckq-LtOuJuh88 zu=G7Y;{ssmdqKv9z@ArqFHHBG>wPamSJd^s7o!u!@#n$P_ZJwy2==_~dkO1Y?|Uh_ zmt60AX}U6C>3dnm<*Z}2CUi|* z?|U=4=C1es4Z0Rkf&KBlC0#49*Gk`8(_Nvn{!RQXsC13LP1nZtzPF`o=X&4mHfv9B z8A*x`*2Up_0OULHu1nuL(scq$-#at5e+b>O3v_kqdpElB%wyRdD&UsyK@XR{_oNf= z<3ECBFQ~|}mc5}eZutT9ap`+sx@yd0*$-;smS>>9%K>CPddq=O7q=V)gI)UmA)WXL z7enyIENeLwn&Flodkn){&|40N5nzvZ-$&Ao0!!aVGadt$zK>-b$vQ2^!FZRxPoR63 zc`QHin23u>_++s3eG22QENeLxrhz@Ce4kF&op~&0zYw;0wVrHebZFC14qwFJ;;? zu#C-@Gi?Rr=Q#Vml5Q2)>#gsr>DGYVmwaDKw+`&~@_jws2G{$(k#3Xgecw#C#r3{# zrQ7Cu-|exg$Lw}|2jpiTeBVj8%k{qhME5h;`MmGD>Gpu7?|T{V13T~XeLvj+u=5Jv z57He1J666QraR(#-;dHAgIVDFamI$7&zvAnf@N%eifO08_S5$>bp657?w7iMp2N?B zoqPCxf$k#Yr}zC=x=Uck&G*Z6SHRv+_#WVKW`BSwKfUkv>z{F5?|VGDTfok%eZQ6N zHrM+epYC?B`>OAE(B0{J-|wQk8|-%R{T{jmuJ_%3!0BGF^BLdmZ)4x*df)BuZzTqM zEckvuT@u&(o|G;b*lVTl$>|;dJ3sL~1zk$8^gR{h)L`fBzDLlBG`L8Mrvpph(=&b$ zEPcU4 z8W(Tk;w@afjf*z8Xp4(>xM+`yv1~h%v1JERyn~AgOnaBHWk*tU!bN9Xbiu_Gmg&ma zvKuL;Gw#mV(mz-1K_`0R;(c87!bNXfe1Hpko$kZfvM(w6;i5k-2H;{KE(YOZFfKmC z#Yeapf{UTJ_!t+%a4{SgBXBVi7o%`78W&@5F%}o&a4{Yi6L9efE+*n)5-uj=VhS#% z;$j*ursHA;E@tB5Q(Sz8i_dW}3m3C-@dYlv#Kl**_!<}A;Nn|ce20rUxR{HJdAOL5 ziv_s&9v2I7u?QE7aj^s!OL6f7E|%e9IWAVj#SUET#KkUL{Dh02aj_d0dvLKA7yEFr9~TF3aS#`WaB&zH zM{scz7sqgM92X~W@e3|a;^Gu8PUGSXF3#fO94^k|;sP!%;^J3aT*Ad=TwK9L)DQN= z<$Zuh99+c3MLb;Gf{R;maT_k;R3F4Ey5JuV)^MFw1C#6>1t zJcNsfagiApS#XgR7mwf~8!ocrA_p!W#YIkBJcf(Maq$E$p2S5iTs(z~+_-oe7kO~; z3@)CU?5@jNbGz{QKWD1nQTxG05- zmvB)U7iDl!78m7kQ63i+aPcxOD&nFNE-K@q3NEVR;uT!Hii>KvsE&&oxTuMXTDYi< zi#oV?4HtEBQ4bgOanS%5uj8U2E*jyYF)o_mqA4z#;i5S%-oQl*T(rbRD_pe3#hbWz z3m0$Wq75$E;-Vcc+T$V;7aef%4ldrsMMqq8!bN9%5_|w%Ji6kOp+9tkDKHSa!&Ddy z?|JmVm%%XT>G3|k9L7K|SOF8Dx5L(B+n(;S4>C1CjpECstx6X6H2_4v2r_1t#TSjMnr|Cf%x z9qV~uY0P(dqmRFT|BvlHzmk0scK==lmUbUq?Q#t%*5ZbZ*E!7XKC|AX-TyX#l-;Xjr2A00tS;2XH z@@25R9V)uKgRBIWcS2>Ccac>fIou6ZAt~GguRuCT0I#|XC$od)y^zCYLh@0twEfQM zG7&WKpnu2#UFUm@E#Knc;bt zS;!Z_GAq33@)5EGSZ0HgE^i+H(CzTwI{yFm$8R<6N7cd7_Zp0Af~D`Z7}o|%-|H}b z4J>`H%eWp``d*)L1F-b{b;b?B()UJ;8-u0qO&B)?OW&I@ZVr~dzrnZ#So+?QaVyvP z-kR=Bu=M>c#&3hA?`;^j1xw%CF>Vi*zDF|d0G7VL!&tnFi;lSHgp1C&=z@!`xafw9 z?zngl7d`NvVCnn&jC+Bl@4Xp+0G7V@VcZuieecJ(KUn%cfbl@E^nDQH!C>k8hm1c0 zOW%hu9txJef6RCoSo%Jk@d&W=eI(;iVCnm4#$&+J_pyw}fu--`8BYL9-#=kI5iEV5 z#CS4T`aXs6RIv1Y8sq6;>H7@EGr`jLPZ@s(mcD<^cotatKAZ6uVCnmpjK2a)-@j)3 z4OsgAE#vRN()T%x#aw(I+z-CbXS@I`egB@ZScr>7_+qg1eF%fQn2<&4D& zT&%>!DttABgYRn?uXVle>*&^lrSBUUZ-m6)`zFSl!P56FjJLYZ_ic1Pf*nKOw=>=W zNx=7=jCX;h?>{mA8IpqUyBY6sz3+SJ_CYG}eLv#^VCnlo#)n+z`(e5xkQIDC%J>+h z1K*D`KH+-bf1x|+df!jcora9y`x(aKEH2LB=OHureu1&Lh>KrwaS0cfad8C~fm`?i z4M!L$;@~1KF5=hwm6)sZaA_5m_aFG@l>2Q%A7Z2hh11>V+A`>ni z!o|b5$c&3DxX6l&M{to17uj)<1FsI2k3tQXImwz}`54r4`8Zh{ET4cnE}tY{1It`c z*X2`WJ+RCT^<6$qHUP^!@Vd)q$cA9~EHrYNmuw7{`Jjo*{A5$GEC9`179^X4Wg&RO zWnr=fSUv|WT^1o*fn`x>?Xnp8CRi4Sw_H9?z73WyKpU4Yl5N4V1hjKml57u_r6AJf zOJoPIEDi6tEJMBvmSv%%%W`BVuq+RqT~;8)%ebhBi%PhtjEgF`sEUhMaPcZGs^P-! zH#hG;q5I9h`To;|+fj7IMK@e@$HjZN=z)u#xOg8Iy>QVR7a!oF4=(!Rq8~2$<6;0V z2I686E(YV`LtK1>iy^oeii?kNF$@>OaWMiHBXKbb7o%}81{Y&-F%B2waWMfGpWtF5 zE+*k(GA^dzVk$1C;bJ;2X5eBbET;!9k7g^RCo@eMA%#l?5H zn1hSCxR{5F`M6ksi|=u<5EqMZu^1OiaIq8@Kj2~+E|%kB1uj11>h=ViPVl<6;Xgw&G$NE`G$tc3kYh#ZFx8!o^Rx_!$?waj^#%dvUQ37yEH> z02c>waR?WOad89}M{#is7sqjN0vEsF;v_Ck;o>wd&fwxKF3#cNJT5Na;vz17#lw4c4(%t8J-xJX#hQe&u_xtIRxZd}qbje)rdvdx5T zJr!MQc#iG*9zmA|?Ds)@PfM2$?DqwHPfz!t>wV8am(lgUXQF$^^}at$m)Z5cXQ9gq z_WK&XKSGzy^}c7P%K`TLHNHPem(%sWKSuYs>wSNM?n$uU?e#qu-BYgjJvZIcuJ=6; z-7~KD{aL!a@FMH?Js(|u*ZW?8uAu9EFGN=u?Duhee~zw*>wPauSIqUk7pHsP^}fGA z_afNuW%ypgI@kMNitZ)X`(B!^jO%?bOIHp`v3}pn(^YW2?=RC;1pECO-z(8ocD?Ua z=&HKj_gCm%b-nM^=&FPLevR)n=xVy&_gZwdUGIAxy4PIqdtJJEuJ^q@T?4S+-|_u* zx`wXzy%Akw*ZbatuBq#NZ${S~?Dvd(e}k@t>wRxY*UI(2x2Aj3^}fGF_qOYOZ$sA> z%ClYH+tIamz3-899l(Ac+4pzo-gUk29qBr`-uKRQU0m;bSGsPl_q{vad+;*b^}PpO zPuKhYK3y-c-zW0DH{AzdzZ30yAG*G-_q`uof7kmyfX=Yz+kvEE_l-fMVaI1MY1lq| zND8~(egt+O9zu$txcC?s!*DSi9|89I>-$K$QLguWG~F22`#zR#oa=obPdCB!zJEeD z(e=JhqMPh`->1+`b-nM?=%%~g_Zf6EUGMv+bf3B2_s{8Ox!(8LbYFnIUiw4eU(XDsA?;Gegy59Fqbemo8`xd&buJ?T#-H)#K zeLLL_*ZaPcZkOwQ|B3Er*ZaPkZV%Y=s_%R0_PO5o{d5Oh@B2ZzL$3GzFx?T?`+k(} znCpE%PItofzW+jZ670Fc_fvGIUGMuDy0fnL{T$tS*ZY2fPF%#ruei8`i_5sUf{Xa6 z*+Hb^w$lILX==CRMJK@lu{ZB^T>pVG` z(D?)8ea=&miJYe-6Fa}36iIOLPpv-{>lCSRkqM80f$aZ7WE$rWlWCo2Ceu03LZ)|~ zm3+|oBV-2W*~pB}Z}j?ev7PKNnEii>%;7vY`Ka@!$(+vfkdHZkhJ4)lv*Z)b^O8?G zztQV2%69U>F!sL~ncsPFvVimF$%4*bAPYHvku2=|IZ_nC#Xq(F60B2{#6>CGu%ExJ zNWSE}G-+60nJnYHENNI@l`QAHJZV_}Dp|q#%cStvUx{A$>#ssD{Pn*=FZ}h_rLP9| zGs*SH>dxzvHJmpfYdU|OtmV8RS=)IdvX1k{%0fq(Roj@lk@k<&dz(0U7X+O^$%h@y}^#> zVB4ni56M2xKO*}&A42wXK9ub5{9|%}^I?|GZ}j@dvz_5!x7P%6g!50xkfHbWCo)iml!}>*}VZNCB zGtUG5wwAN3Sb>X`xY&fR0=r(D$<@x+kcRbZNwE$W>v6FGH*Efmq+uT0`nNE>rJH9f z*$OPTdHjfr?YP*1i~aacu;;M@|(hV`dOaRxW6KT8_s=g8J@?el+Z&-4Da zF0q`rjEgI{2&Ccs-w{TNIJk(5i+H%W1sAvC;x=5w$HncqxC0k=;^Hn`+>MKSaFGBP z;kdXL7YT83A1)H%A~7!R$3+rcB*jHCTqMWE1Gq?mib}T%^TCI$Wg3 z#e=xWfG^?xVwuq+6D}UY#lv_5ZhFhium`uy;*k{>kKiI3F0$hy2QD7PMNV8ihKt8> z@dPfO#6>P#JcWzgxOf^Dd2sOzE}q3jUR>nEMSffqz(qk^6v9PeTs()1BDg4ui(qB}0$ z!$l8V^u)#cxaft8-njSx7kzNi7Z?3-(H|EBa4`@UgK#kz7a!u{BU}u@#ZX*)jEiBo z7>|nyxcCGY6LB#K7n5-@1s79sF%1{faWMlIGjZ`L zE=eU@Ki`lsN0vBK6;wxNyjf-z^@hvXC!^Iq2%*Dk#T+GMC0$hBLi-ovYgp0+v zSb~eCxcC7V%W$zA7b|eF5*Mp*u^Ja^aIqE_>u|9i7aMS~5f__qu^AUzaIqB^+i>wC zF1F)h2QGHvVizuc!o|JtGIE;%UxHyW7W4Jhuixar` z1s5lAaS9iwad8G0XK`^37w2(t0T&l>@hdJa;o>qbuHYh&o)bSu7%Af5A}%iC;o=rt z+=`3aa1kFDx8vdtT-=F^yKr$gF7Cla0$haS;$B=N#KnEMNQ8^TxVRq|NpO)A7s+su z92XDZA_Xo|;vy9;QsW{57in;j78mJokscQh;vxesGU6f=E*`?g!??(di!8Xvii<~Z zkqsBwaghTTkK!UHE*`_hJ?NsEms$xTuPYS8(wvE~??8IxcG9q9!hC;i5J!>fquvT-3!yJzUhsMFU*C zj*Eu4XoQQ#xM+flrnqQ^i{`j^0~alD(GnM}aM2nUZ{p%DT)d5oHn?bui*~qZkBdlL zbil&cDP(f?gYz4 zu*>CQ@+Yud0zbQ4O6~^BA7GEmW#nG4Tn_tOt|0e=Ej_9L2>kTpY*630(Ywi<7uGg^Sa;ID?C`xHyN4^SHQxi;KAU6&IIqaTym^a1pq| z`M)EK6mf777Z>qxaSJYP#l>y7h>wfgad8JO?!?7ixVRe^_uwJ{F2ZqfFD??|;yzp? z!bM_S+>eVSxJZhNWVlFhdA77+5|G#a(75p9jk<@Pf;%!14(w@A65q0$ApPmt8(ZRs_r3P|4-fq{xGdXK?W>F7o0c zA1-bl|IqF5XODknj-jZ6i>kPI1sAX4q8cu$8rXpW0FaM1!6EpgEb7p-yeCNAE>#oM@OgNwGfXorjT zxQN6>2VA^^i+6F+5f`0s(HR$AaM2YP-Eh$z7w_Ss2QGTz;(c87!bNXfe1MBSxafJnE7>tV#aq$r@hTvi-EAskoSii|M$Sfs2{A_!JkP;o@^#%)-TNTzr9xFLCh|F22Ua zH@Nr~7vJGx4ld^6VjeE$<6;3WzQ@HvTr9%HVq7f2#Zp}SfQx0gSdNPoxLApcRk&D< zi#51di;H!*SdWVhxY&q`O}N;Mi!Hd=ii>Ty_z@S|aj^pzJ8`iK7eC?RXI$*Y#U5Pj z#l=2c?8n6cTpYy3AzU2B#SvT_#lOHIlejpAi_^F`gNw7cIERb#xVV6e zi@5j|7ng8x85dV@5qWobc;thS1D=6mP%b>2cLg`}5?P(?Gz@Jg^3CY&MRtnz8@dBG zSAO#d{C|D~B0mTZx95S#p*$9o!OiD^|IhCcH?RKxk4Hey16@a4{n{JG!4UkDK>@*! znFd5WT<~W_0^(L&+=h$zxVRk`ci`eqT-=3=yK!+3E)w9vPF%#jxJZbL`*4v67m0Ck zKQ5BsA}KDC;UYOM9>7HkT%^PWzk(SMsc{j33w}%^AkyL@9WMANHy|FwMFw1C#6>1t zJcNsfagiApS#XgR7mwf~8!ocrA_p!W#YIkBJcf(Maq$E$p2S5iTs(z~+_-oe7kO~; z3@)CPx$k)KKDAaXXjLZ?nXR>5* zsPFQ5vH@7W0I$1zk!%Q-C7_YZl4N7BECo$mzC<9PXZ3M^lS)-EfOZ-Qkdc*|vFQdGf3Rb0G+i&t?`4Hq~1_-E(X*JFGCYsdd>u9Ikk zi?+CEhl}>Oh{Qz)T)cyecX81X7oBj?85dn}(G?fnaM2wX@8O~cE_&kPeO&ayMQ>bu zfQvr3=!=Vfxag0I0k{~5i$S;;jEfI(@ewYD;9@8)KE}l`Tnxv>2waTB#VA~i#>E(1 zjK#$`T#U!X1YCTAi;1|Hgp0|zn1YL`xR{2E>A0AIiUqJ45{yn+J`9g9pST2Hn zP!JZwekcq};DGa`)Ur8QwzKT2!maE|elz=tx3zUSl zaMJlY@|5%S~`rr=H}-B9EO{hn@8aPyCa~_0eVlr`8hyhZhhl^ zk0iKAii>2pNREpKaFGHRDRGes7pZX(fr~V_NQ;YfxJZwS2XT=B7a4Jp2^SCH;$d85 z#zhufWW~iJxX6Z!?6}B*i$`&h6Bm!+;&EI&fr}?`kqZ}3;UYIKp2kHUTs(t|XK|4i z7x{3JAFmFU1)zpYJD;lwmW7~}OFOr#4VKSA9hXJO*TAwU)OBg+cJ;urIMjFfJlOy& zUx3$LzDPC%OFOq~KvxOx0Tx5J-3{%>;(MH^hS#YH<@w8upx zE;``i9bCMNi;lSHgp1C&=z@!`xafw9?zngl7d>#%6BqB}q8BcD#b?49nm!6och(#Q6&H zsPmQNG3Tqu<6yZOPCyA*1HV8?SPLheuOm-6Ur(NPzJWaBd?P6~;pf0|Gn|KVumvtS z-%4I|zK#6V`H$o!s0iEPvdbN$Q3mF~PRI{)VVCoH9={{H6x_Km+V$j>%?k3$#N^}ffWyT$ds z-%58I*fIA#KHcrE_x%pKJ6-SlU37Q5-uHXx5wQm4m(KORr>A=min4y+ zGtgxOOW!jwehBRO#P^5kGP~aQEOc32@B1Tk*7o?|Ut}+OGG# z4&7^D>3dzq^}t@Ue6LT}!1cbrPS?=&zBi(4?0Vmu&^2|v@6G6%yWaOV=vqJp_Q&^@ zbgjT%D}8TGcZJUSH}SWi(l!1zT^rZ?-j=SN>wUM|tUbMD!8n}%!V<2Y*%QMj5Se(Vb1Isxcb8#^b7xQtk0AC1}vH2pVEdk5ed@0kGfn{vIoM|f{KgZek zm2|7XUT=M0O}7T@zU2E_x^-Z;m+$N8Hn`sRjdYt_@B3!DEw1-{E8RBN`)-d_J!ZG# zJ0L&%;QLOxU9R{2C%T`(&gXsKO}7Uuec#J?AJ}=9@B8TvfSp(Pevs}E*s=2cFx?T? z`+k(}7|a6Sk25yxeC7ms5-el$Q%pMzwx7PAq3aKpcE8m9^BjI2?A*im3v?GDKfUk2 z(p>^OZoXfpy8`xp!uJ4=voOd{?|U4&xUTm-9^EZq=heR7N_U&{tp9!_^J*!hg_3F+>0z3++W5`#S!e7~PAiR*n&N|y}mwbJ+G zbPs@?pZK1FE+ts{o{Dj5u=94`{nv#wOc!bKbYSUwdd3ferSBOSX9T+)ea}Sq5ZLYI z`@?jZ!H%=~uL?@B5>4Il<0be1D8iJdTSeaPcHAa^d1BT;#@U zXW{(-`84El>AU|L{Vdb#Fu!FxGOx?_cs|@RKUn}QeJ{v3lE;o^A?W6^FxdkvpMxSU zeJ@H^3@m*w&iHw-^!)|KFM_4-B^Z|kOWzx_-=*j*8^TMtWfLfkTXtaIbzavQFH3J( zjuhP(muGC*9dvH?GM(r}uk)@-^rAPt&cUkCi>kPI1sAX4qA$xqTMmGFxMh7()ZiF3U~Kt1DH`IU5iW+Xyw35O(2GVa-_+(K z?VPU}ZrPj^Z{T7W(^@dLY)Oh%xM+=wH*xV6F5bpP8(h?4KiV?3Y)6XrxERZ}BNj4cb1dhASR+?}zdf38@Vt_N85^mrc^y>QVR z7a!olUZ?vow(Luaez@q5ivhS8h>JnE7>tV#aq$r@hTvi-EAskoSii|M$Sfs2{A_!JkP;o@^#%)-TN zTzr9xFLCh|F22UaH@Nr~7vJGx4ld^6VjeE$<6;3WzQ@HvTr9%HVq7f2#Zp}SfQx0g zSdNPoxLApcRk&DTy_z@S|aj^pzJ8`iK z7eC?RXI$*Y#U5Pj#l=2c?8n6cTpYy3AzU2B#SvT_#lOHIlejpAi_^F` zgNw7cIERb#xVV6ei@5j|7ng8x85dV@5s1qPnj?%9ac~hA7x8d$3odTO#cjBVkBi%J zaR)B$#Km2>xEmMu;35Go!f|mgE)wG6K3pWiMPgjskBcO@NQ#SOxJZtR2XK)B7b$U( z3Kywy5rK;|xJZkObht>5iwAL$0T&r@kqH+M;o@OjWX44nTx7+?Be=+hi|n|_fs03R zkrNk>;o@;zJb{ZRaghrbPvIgrE}q6k9$Y+wi)V3>7Z>?(kslWYa8VE!g>W$#FASE? zc@)7#QCt+mMR8m_kBb*@@ggot;G!fhO5x%qT$IK|8C;abMLArQ$3+EPyo`&AxTu7S z%DAY4i>kPI1sAX4q8cu$6v=at2uTg_$nPJbOnwWNcHO>nIh>pWmLp)U%aNoQh0h1e(Xha!9gFY5ax5%#IgVTe zmg8ZuOFLdm!15DV3U-|)!Vh5U@o&fLx$UO0jA6_EFCBk7*7Ly9nD6pNAAkS;AKQI? zCHt}wEbaci%B9^$SA*pmkF~gA-8zT4-DlRjwEN!%u=LlziScHz^nDBCtzhZ+%73Ui(u*duZ+bd z{B5wj3~gLqA=`pwARgxrkQl%Pd)*bN#IGBw(VSCnGBwCY5SKOEFXZUU8W%OfMrT}#$_tH8auTY#nSEg83Ro$syb z-ULhE-(vhWSo+?Eaa*wTy&dEBVCj1#;|^fy`#X%qySV6xi%z)cjEgR~=!%POxaf|* z2bR9~VB8ZdeSe>EFR=8zH{%b$()T`$`+}wK{TTNLOWy}D9tf7c4`MtREPelw@ke0k z`w+%M!P57S84m+X--k0E0hYdxWIPHieILzu3|RU;mhm{S^nEHGJL7lNhlix@8kOW&6;UJ91J|G;<|So*%4 z@d~i?-QVX`Oj`}%;QJcJYhCaAI=b~>>H7x88zC|HzKQW>u=ITkfPq^OqU+7M{-uF{LB3SzVE8|OG>HB5I zSHRNuz%88jgQf3r7{>)m-{Ud91uT8PmGNz0>3e*}w}YkccQC#aEPcO=@!eqQ`#p>k zfTi!@jPC_Y-xD&v4=jC8#5gfn`hGv-Bw*=#QpU-^()Z+y9{@|=Q!q{mmcFNAoEj{B zk6@ezEPYSQI2~B}o}Tf8VCj1X#v&swGU4JOTs(}6%(%#ci>$bK1Q*$GksTK~@akas zDAaJ7ldK7rk3lV$kCU~*@(HNp@=5YFu*?N@T|PzD1Iye{-{sR}1F*~kue*GPYzUUm zLL--X$;M!r51P2lPc{Y10?^E5L9#hm7J@fi7A9MO<#W)|Wf8I!SQdrWE{l}9*9Yt4Mbi+k=T)c;i z9=PasVlXZ~#KlLr7=nwTxcC?s!*DSi z7b9>n5*MRzF&YxcC|u-{9g~TzrR%Ik=dMi+Q-1kBbGk_#PJvaj^&&i*d08 z7fW&R11^@~VmU5W;9?~%R^ehbF4o{;EiTsKVm&T4;9?^#HsNA3F1FxeD=xO-;zwL; z$Hfj@?8L<`T>ONKpK-Ap7khBA7Z>|*u^$%)aB&b9hj4Ki7e{b$6c@*EaU2&XaPbQ+ zPU7MeE>7d(3@*;%;v6o{TwKD%Wn5grMbsbB4d;D;M;u(l#YH?^+=7c+ zad8_i;^X3WT-J?NsEms$xTuPYS8(wvE~??8IxcG9q9!hC;i5J!>fquvT-3!yJzUhsMFU*C zj*Eu4XoQQ#xM+flrnqQ^i{`j^0~alD(GnM}aM2nUZ{p%DT)d5oHn?bui*~qZkBi9v z$KH9sIZ>`}JCjV2prC+A zQBay7R*+&>?AX8Soy>il)q_Xn_&=WWZGOM&y|4G#Wb))qGMin7SzL_3#YkL?!o_G@ zT#1V@xEPCzak#h&7gyur8eELW#kIJ&4j0$sVgfEE;^GEe+=z=wxVQ-yH{;?KTujEr zt+==i7q{c$4qV)ci@R`fH!h~&<|Jz-?*R)pc`pQUllMUgH#rq-+~oZb#!Wr|rErte zAPYA+9U{2N84$%yJ_s?~S0wkIkBbes*ocdlaj^*(n{n|9F1Fy}Rb0G=i`Q}S1}@&j#ap=8 zii>TycpDenaj^pzJ8`iK7w_QWU0l3}i}!Ky0WLno#YedK7#E-5VmB^6#l>g1_#78s z;NnYMe1(gzaj^#%-{9g~TzrR%?{V=1E`G$tPq_HU2`Ud{-{T{Qix4hsT!e8^3Kv

    T2jJpB zTsXK$qBbt-;G!-r>fxe3 zE*jvXAubx>qA@O-;G!um4#veHxHuFShvDLITpWRmBXQ9T7tL|e0v9cD(Fzx>anS}B zN8#dVTpWXoV{vgDE{?~=3Ai{B7boGOEiT&OqCGA;;G!cgPR2ziTy(}o7hIfzi&JrN z8ZJ)9#TmFb6BlRUqAM=C;i5Y(df=ibE_&hOY+UrlMIT)B#l<kQ>xVQ=zSL5OuT#U!XwYazr7uVxr0xl-v;s#vYh>JEm`EXBpMxLAgZ=Wwwc7b|eF5*Mp*@jNbGz{P4@tii>LxOfQ{YjLp- z7wd7c0T&x_@iHzp;bJo`Ucto{T)c{l*KqMVF5bY!o49xj7h7?$4Hs|YVmmH&;9@5( zcH!b3T)c~m_i*t(ETl{;tO1SiHomr@ii{?;Nlxx ze2a_kaPd7Ze!#_#xcCVd=A`r%H0Re4eh*R$K?}-$x2c7_y5CDt%kt`ek5G$3OSa?p z7`1F@McwZ?)N;ML-%C>~YcZXUU+Hk*qFHfxknDasTy&|VB_DtrnQ`HTk_ZwK`ti?{%rw19N^Yzt^YMz^nVcA+<(c-S3U5HG$*V zj^CS7JJ_rH{Sa!0dUd}aM(uE~?)M|89SJ9}eZM!O*4(T6y#=+FUfu7lsI>-jJ}$qv zp>~v4_xsV*j`8Y#KbG2YUfu7+IG2-i6vJV9u}Q_fx5z=GFavI<+&ry5G;Fc9vK7dsk}Rz?@&p@7<~O@alf= zNv)Sx_xst@dV6)h_o3F;tNZ;NYUhGEf0y6Sqt?%>`~7@s7kG8Q_op_%tNVQ*wLxIc zGv@cf)GqYuejh?@s8{#;nYTeIUlLt zM^YQ*)%`x2+Ld12?_;Qq_3C~fN9`)F?)R&yT?1XXU%!v1cCAk=A7py@AfeTzX#0sU%%f=?LM#W_o>wG_v(IsfZ8;#?)T}`W_WeKKS=E%ukQDU zsXgM={r)Jm$Gp1VXHuKx)%`x3+8nR$_qo&__v(Isg4#T}K? zzL44?ukQD!sXgP>{l1vm60h#}rPQAF>V98F?K!XR_vO@9cy+(8q_zso_fx+=PwfS- z?)TNy)_~dn`~5{~FL`ypucfxmtNVRDwGCd~?;EMT?A86giP~nb?)O)yZSm@Uf0f#6 zUfu7nQ+va!`~6L7Z+UgUZ>6@)tNZkOzjh|?)Tl)KK1H;|BTw_VD=S$|AN|=Ufu6sQTy7f`+X0!Z@jwS zzoqsanDqPi%zprre*cmAPhisTku(j0`M>*7((*h;20hOvL!Rf5w&%HI*z?k4DbLH0 zS)TvZ+b_%g{U1!$^ZXF9zUPOM z4Lm=LZ0PynWFyayARBvrB-zCCzk2&eb3e`CZu&olZ0`B7WDC!aBU^fYJlV?g6Uf${ zw;{z*xcGN%|3tPaPQpc7++fbXaSGYa^Y)~{)K4Qjc;1mTnEDyy$)0y24W@n;+1c|h zr0{S5RO-UN{nM!n|Mt(MF8tf?L%l1QbCUEWyLo;N+1>MV$sV4cNA~o*AKAgVD_I+kpsZwN*D-czqE=R1SX${!C>C+FOV04$yZ?rnD^0Z5rB)BjcE2+yx3M|yq@ zIm+|#>YC9m}SI&zHX*OOyC|Esq@nfsXlW<75;_v!g<?<8;X z{4Vll&+j(r`Cq;L>DWkU&GWx{ z`}4VfX&n69~zL1>b`CQUq>W`D+3EW`n^GJj7y>0(VzW&eNcmDfY#C3#u zUp>v-Sc)6W>uNRmtmn%}gQ-79isiV$)K`!O<15KOvLEo@*BWjsUc|*q zxOfF$3+8ccA=i1no-~;H22yOq#ml(Zgd5EDH?1zi}ad7}H4#b6ni!?6ExM+`y4!G!ui<5ED2^XDl(FGT$;NnzVoQ8|jad8GN&cwx8xaf-a z2b0}kfS29Lfnc%+4DzxkIT%d#f(yMon;Zfrd&5vK`;f!HWM8<*%X7$!!Q{DciI?Y* zmx9TDaG96qlb3_Z3*ZVb|J(Z?&f^dxa4`}WqwvvS((hL?7h`ZS78m1iaTPAE#>F+b z7>|oFGJcoY|p;bJB(X5nHsF6Q83 zE-oI&#S^%ghl}~RSb&Qsaq$!`7UE(NE}q84Gq_lcizT>Nii>A)u?!c_;bJ*1R^VbK zE>_{);(J{DfQuh-@e?irMSSq{ut*WaMFaZw%@6>w1z7nN{Pgp10!sDg{CxTuDU>bR(Zi-T}c6Bo5`Q5zR^ za8VZ*^>9%i7Y%UH5EqSb(HIv^aM2VO2jk)pTpWsv!*FppE{?#(k+^7vi{`j!fs2;7 zXoZW`xM+imqi}IFE{?&)vA8%67sun`1YDemi<5BC78mVs(H<8aaM2MLC*z_ME;{3) z3ocH<#i_VB4Hu{5;tX7ziHoyv(G?fnaM2wXJ#f(z7rk(CHZFSOq7N?m;^G`!oQsR| zaM2GJ=i}l6T=d7q09*{j#UNY^#>IuW7=nwTxEO|ui*Ruq*PH7>5f#dus?i;L@UaXl_3;9?>!ZotKjxR`{C zn{aV6E^fiaWL(^ei`#HB#kTujHs z3|u^ji-&OWFfJa!#iO`*3>PzTF$)*7aWMxMb8+!FE}p>Gg2{QX&dd4adN8>FHhB3Y zxe-i01uuKKklX|&7r|yPpC(@clh42wFBg-qg2^TDnwLw-*TLko@P?Pm$Tz{{bMTgz z%gL=^as_PjawYjTm|O+hy?mbB0VZF7onEdccY(<@@Q#-+lJA1am*71wOV%y3et&e` z-e>*92e|kU7a!r`V_bZKi`}^R6c?Z2;&WVlfr~G3@f9w<#>F07e1nT`aq%54zQ@H6 zxcCtlKj9+4{|&1PCM~GvWss~7CPUD`OPg#6Cd1Ik%Ti=xFqs8Syo``d!DJK;_A*8u z0w%NJP%m@H!@y)N9PVXl@(3_l29ESHk8B1e^P#zyak2%NOh8L7lVmF}nS$0{7LaYg zWFZ{oWm)oQFj)?c@p3=%STMOi9OvZ$nHQNDS7>5UN`^h^>Y@l8POH*1}6R9o%y-^okEj6;4=QtlF6RNshd26 z)Zc(U0zaFY$=+liFzNTc%+CRnem|G_d0^7-=IG!*s-wh`HK85)`VAAjR zGQST@`h6<%`@y8&A7DNWO!|E~^BG{$?+-G62u%9@Vdjs3NxwhJ{4p@;_nFLRfl0s5 zWxd=;4V`}53S0F!=S&3p}*^!tmGzMAe+(x5{t0uj8yBDA;xk-)j*BmF z@g*+4!o}CP*n^92aPciJzQe`$xcC7VKjPviTuk9yKqcoNW1qu1{5PL}tP1BL6IF3h z4HwmMQ3DqT;i4ukYT=?bF6!W-E-vcfqCPGf;G!We8sVZbE}Gz?DJ~Ai#UZ#j6c>l# z;&5CXfr}$?(F_;OanS-7EpgEb7p-y81{X)+;%HnPgNtKvaU3p=$HfV_I1v{o;i4@r z+To%-E;`_%BQ8$HMJHTz#zhxgoPvu}ad8?hPRGR=xHuCRXW^nNF1q2OJ1%J8OxEPF!3vn?77ejF|3>O#S z;$mD}f{ROWaTzWy$Hf)67>_}V6)v8~ z#S6Gtjf*w7co7#b;bJW=*5P73E;is|BQ9RX#U@;A#>FeR*n*2!aq${1UdP27xOfv6 zZ{cDqF1F#~ZCq@}#SUET#KkULyn~B(aq%85-p9oUxcCqkAK~I-TzrCy-MIJ^7oXwc zb6k9Zi!X8U6)wKU#U5OIgNtu*@f|L{$Hfo0_z@RB;Ue$=pZ`59QUq}k!i9~CFfK~r zA`2H0TtsmZ!$meOa&VE0i_*9#gNr;|TN<;tE_0$47ukzmH@-3QYQaH1jLLq~FId9}6b^K92cS zVAAhbGrtB*`h7g}Yr&-7uVa2anDqMu<`coB-)~@kBbfC2B<44PNx$FB{1!0j_sPt~ zt+==i7q{c$4qV)ci@R`fH!h~&;vQVwi;MekF%=j0i|25$92YBau@V=naPd4YUckj_T&%&xi@10R7i)2`4j1cju>ltwaq%)P zHsNA3E?&XK7F@iFi`Q`RIxgP8#hbWz3m02)u?-h*<6=85cHm+sE_UJK9bCMNi}!Hx zJ}y4M#fP}~2p1pY;uBo##>J<&_zV}HGx{PtAk0u*I<4SnDl#1=C#12-)l3k119}mm$|5ii~4v2FzNS(%o~A8zc*&y1Wfw9 zDf5HDq~8x=ekhpq`(exv2a|q3g87kP((lcfHwTk`Z^67JnDl!q=B>e`-`g-h3QYR_ zXy(U&NxvV<{5UY__v4wL04Du@BJ-2Lq~F^zZwDs*-kx~}FzNS>%ufcBe(%J*Gnn*y z7v`scNxz@U{4_A>_tTl50Ve%^CiAnvq~E(T?*=CQ-ko_5FzNT6%zJ@Jzn{&#H<G%H3#QY4 zxVRb@*Wly9q~EV)F0RAH^|+XTi;1|n0T(yoViGQH!o|(FxCIxJad9gyZo|dxxVQrs zcjDqMT-=R|DY&=?7x&`gK3q)2#r?Q=02kA6F&!5(aPc569>T@LxOfB?kK*DnT+GD9 zEL_aS#T;DB#l_>ecmfyma4{bj3vlryE}p{0LR>7u#nZTW1{aHQu>==Oaq%oJmf_+# zTr9`M3S6wj#VTApkBb*@u^Ja^aPcB8Uc$v%T&%;zdR%P4#YSAbjEhaU*o=!;aIpm! zuj1l0T)d8pH*oPLF5be$R$Oeu#oM^pj*A_**oli>xOfK_@8aS;T)dBq4{-4zE;^I48e2T;$*)7Z;^*Q3e-zxX8yv92W^(Byo|#MFB1f zaZwf*<#4edF80U80k}927Y;7cxG0Z{3V3rcSrJ-zS&3{3CX1kzmzBxZV6qCd@vgA!NI1Cqu7U2rh==Vi+zi!o|h7xC9rM;^Hz~T#kz?a4{Sg zBXBVi7o%`78W&gMVhk?E;$j>wuENFDxVQ!v<8g5T@LxOfB?kK*DnT+GD9EL_aS#T;DB#l_>ecmfyma4{bj3vlryE}p{0LR>7u z#nZTW1{aHQu>==Oaq%oJmf_+#Tr9`M3S6wj#VTApkBb*@u^Ja^aPcB8Uc$v%T&%;z zdR%P4#YSAbjEhaU*o=!;aIpm!uj1l0T)d8pH*oPLF5be$R$Oeu#oM^pj*A_**oli> zxOfK_@8aS;T)dBq4{-4zE z;^I48e26ljWefm-~?|z~uhW(#r$LR$%f#Xzis#wgHoAILgcNyysSc=2qvq-NnV=gwdDEDJg@(Y&u?3vU(pU1?Qzio z7aehNGA=sdqBAbK;NlcqoQjLnaB(^=&cMZ)xHtK6;xD6M#L!^M1D zEWpK+xOfT|3vsas7f<8j8C)#J#S&aB#l^F@ScZ$|aIqX0D{!$A7prjbJT6|q#cEuv z!NrTXcnKG4aj^~;>v6FG7aMW$GA=gZVlysY!NnF_yo!t0aPc}W-oVA1xOfW}TXC@s z7jNTYJ1%zMVka(k;o==!yo-zXaPdAaKETC?xcCScALHT^T9>Bc7utVlGLos(O-OkRFGkxUg7mMyp6{s$cBq{~;RSgELT zm8#XM*EpzVt=e_!)~ny3VWY-PnjU<}p@$uQ#F5Rqb??!$*V(=M^gZX?^ZK2CLH_{* z2MxY($k1UIU3}WutB+apzrkPr2vb`=;Liz_jTz9(?HGM;?7_=B(Ls=05(!y!i{Bd}`sMr=MB8 zWa+cZo?E_R<*MgjSiR=Om)5RZzhUFcn>N3)<<-|-f8))!wr+cS`;MKv-g)=E_dodX zqmMt?{pn|)fAQs4U+?+m+wZ>rfipzz>*H_UqGhYrZH_wnm}8GS{)7`xYTK@ThmI$A z>fGg&Q!|f$?_d9WJbv>+SZ(F?Ut{Ifeb&mGxfTM!y#8e1CHy-puk9);FK-cZE)%}z zdOsAuZnG%Y|0#3bnPkAb?pm{rbynVlZ>_wo(2Q*@p3Qx6{jJZDEBJaWnAi4s`~_~m z(#l(l&wRkj+x;-teT4hw`gNySdBdkupTYbg>U^#1d=hrg%JfyiH0NEmkvVO0^XwkE zd55&hb^D&OEue-mu{P%~O$M?$byuWex|MADy!FPWXz583em;3Ww)zv?;o*TXO{N*-& zd%ifZB(OB_Y+za7xxn(kionXis=)Js7XqsTYXUC@UJ9%YtP89UY}o5Ywg$EZ-VSUJ z>cje zXJv(~u$5&+t!yjTDzn$y|I>~7ig{MtN?HY0S!+M*0L!t;TNSM$tBO_4s$tc%YO~$? zRzs`tUbmlbC9ITHXqB_}w+^(@Rt2k)RoSX)RkseZYFTxxdR7Cgk=10cxA(QO7d_ZI z)H>Wc(rRwCv|3w7v87|Jlo`e>jdj0tDV)sI@#)MonoD4ow3*3@1?1Kl+U!fTHURl*4b7c>m2Jm>wK%fHP9Mt z4Y4k=F0n5Am)kwd>Spz@dRe`#zSg-`KkEW(fHlav&>Cu8Y+Y(yZe8&&xA2cv{$nyh zyT?D#!I;pv&{d(U*}_D)F*GT3Q|RWc)d+3hPouRuzcjN#3@^I*p(4(QpLNi0N zLbF42LUTiphn@({3(XHL2t65kDzq@PDD-sbnZ4frpKsM)y~n-n)X@Fpw9xd>jL?Ij zheG2+*M+X9azkixXnknIUbnwEv?R1N^lWHZ=(*7H(2CH?(5le$p%+4{Lu*1Wa;vqW zb$h+7zl**3#?S+yDee|t4ZR+EGqg4Ic4$XvSLof)`=Jj*ABS$^Yxn);J-i&+6xtlx z5_&E4M(C~3w$S#_&d@ud_d*|pJ_>yj+8z2d^jYZh-`v99dMjT4A@pPDr$4-%FGF92 z_JqC*egB7VS^VBhW+f|7vI74bEAT|oyrTI<3yPjBda7t)(W0WKi=HW3T(qQUY0WT3)oGXl2o=qUVcVC|X^#rs&0@mx}z4d~MOXqV+`^iZ&L#T(qfZbI~hBTZ&#S zdada7qBn}(EPAVGYtgo%w~MwH?I_w=w5#ZyqIZklD|UPTwtnpu_!;5ve5v?1@2}F| zyuYHqd4HAv=KWRq5BHa67ugYenEimg#{RH*QBrW(^sb-{Y8HXyQ-aSUv58a zuV)1tgpUgMDAr$%eeQ28E4ITb?e=#g{wVM_Af4JyC+BkdTjqS|WT$J<*9GZo(vSV3 zzocEw&asEvkJ=mQuVMJ;aL;1>9kkE=T|+;+tUPc1?smK5Z&5mRoi5IB`djXN?Bt|t z)7O*f@#*=$=r3hgw{z)iroD-=QTUi}uVVex+~@wxYQ1a4|MC3JayyJU^_^3lkB(%Jg-==#IaiX&+>lvB%qU?N{lqN%**M?_&Mc-sk?l zvKCq&S_S`DKeL|vjZmktbEY%Knd+=|zHrK<4^E$!UX{K+y)gZ5u^!9XHSMnUb@mhX z>-5(&e0;c1vHt4pbAOYqW!A@5+2A3?I?QvL^4c5cJm9?OeC3o)A3|5przbPMSFFEs zb}hTRJ;9!Dzi9`;2Zv7x_bt|6-F@!w@Zbub(fxvl7V9tW9O86$u6Cw7Yn?q#x%8pw zGtw`lZ!OlFS?B%i+ICO-2Kz~Ss~rp<5K=6p*2>R3|=^We%0Upb{+d{dy>7-e%rRghlWoIpIfZI`up5p`QRGs3(H~k z##p!fqr(E{2Pdci5%EhlSgQ&nwnn zgMIFAiuIDU$Ls6nfAm+@Y3B5CCOD5eo9OR=^bvHnCw&LwhsAn4(6;Pz?8)|GdzT#v zA0BQO?pLh8hWp&#z1BMGTdN}Lca_`SJa^pBY2lpX+~CY~UZKAO(?`u!I?(Afv+yMNVRdE2)8+jrT|*&o`a!_C4S!+M8hvdKR8$KMd-&&O4x ztLuI1SKc|==}%`*IB&ZB9h>fx{x&`RSN&D6!}dUXioL@AnEi9}@X5dEuj#*`zb#fU zSi@UCJ^G5yvCcp`o9}FO`#Ub(H~n3D#;^LTXqU1F+xOb5?A`R&;uqaz{zkF3`RDRs z>ov;`)}+4))*aRiZw-r_2v7q!SpNX?~CVqapZs7~V1B&%`^gj1D%6h`uW@QH((BGNCfx$7s+k%ez#yDV|ubF6eFy6Tl4k-k5@A^nxF!}@j``&9cf`yqRs{f*r{JS04@SbxXtbAMM_ z3#{!{Zm=Q!ofRAu92>kn_*gLP)N)RCE~CF^oe!K~x`>|6PLE7KKz{A(uYrA(eVToR z{fNDR73>in8Xi=vzhn2gzj4-+)=sNTuo3-r4Gs>D3*LccIdz=Q&K30cob!`?* z-sw^4X};dfv)s@=+CJSLVLxWSOn*JY!@`4$^>^Gp_cz{p%6iAj4>qQ&ZovzKR|W6H zqE0>M6lVnet#CeZ>~xiM+jRf*==99=reYm7vX8OPptD)_X2xFOi^3Nc>+kq|?(bTQ z-}zXHU=zCP9vl+9I(QeB?KE&sb4EG$IIEmbop8Epx?Ore`pWdI^yXsyHMWnn&$O?x z=h$25@9gl!;UUHPJ7J&uyTN+e`oKyBo6=Q};LzYT!Mm|sr;&4pbER{i^MdoaQz~6I z-99}qJtjT7Sa zd~gc=m2sLlXE|e?`<*q;mrho?Ub;hiPSMZ|XwZVJveCJ@Nn{$;j&3VcB+R01TPj^fYPESbBO>ZmK z-@*0?b`N`^y};%#Wrq8PFAHB(tiQJV+}~~1Dr>j3fAFwi`{3EZi-Xq%@1?(lbEwn9 zxyG5{taH9`^3x5{C#Nq=Po%fEi}iPieWKmVzR`Zl-e!lw=Y%g0UtFxecKh7l?bd4R zbL&8Q>JaQ5yd-#i@IE}{9PadTu5})AHt-&fryHg_rH7<%NI#L@UaY@E?UU@@_D%L8 zd%GPDpBug+d`YqX+V69JcUdo5Us~zlk-?6^KEX?a6M|Feuh2Qt>Fr$aJmS1ee~EOX zbm#QY^o{9x=^e%TJIrot_qA`apRsq^S>f}-!^4*r>#xH;_jixA*80Y(5NsAaIoLON zS#VRs$=}GCO>7B*;JKSz(pKIUBd*dBD8txY!5x%Tg ze;xO^zx%B9)^}DVdg>HBCwO`AhTsGE{!UBhT<1n-ma~QaQt2jiby51J^t0(*#riwK zZg2OqZ?~7)@7dYm^TQ*0 zuhCy1(2TAwPT!nfmVT#Le@EIK>V_Sc-QE=k`~tT(rByQ4k8zS~}Ie`J>l_YaT$#czl@ z?{j|-TCZ4EusS`R66_Zo5xgll1Fzs5;|y?aappO1x&5_BcTHcKo|0bviw>LHC)tx)hFdO9_DesE;)=HP>PCFeM2kTcm?;B0gIYnkqr zzASxDdPVxPVjZ@yJJ}c7_u0?epVHsJUv$S~-sk=vv0k^r!CLfmTJVD4sNgNZhw#eI z3C@MitLiD-%RTbD=S!s zo=y+;4~`B_4nB-mbxv}II=4BCoSpPnA>BINgTC%duSjpFR*{$fWUzP zegFuR4^#+L3{(mf1u6%s1gZwA1*!*X1bPSh1p4wqJvVS(pkLtpzy*Q+fdPT515E-= z0|y5V2^<h`^D7W`X8`7J-(5R)N-mHi4r8M+c4x92*!L7#FyT+g}qHAGkJf zUEun_guukW4S_*{8v}y_lL8k8ZVC(u+#DDhxFs+wFgb8h;MTy!f!hM}0-FQ#IZk~6 zN2))`vFcB8wE9AhS6{>t>rZpc`ZIyI0*eD%Io^I7N8G>7G56a!>V5~u-S6bc`(51Q zI~;xgF2~=$#}WAN2dZ*xdo_+vug($fH8@KBAdYjd$+7CSINH57N37Se>RS06A0D^r za}0R{jzVw9k>rgy7QHdYlQ-dr^rjqDelW+SAHq@ELpjcS7{_W~#L?atbHw%~9P@oC z$8BF`oyk$sXK|c#S8F)OQjg$h?2#N%J<1vw=xL4S$m%OOHhYXU*6Nl&CN?W;Rz7XT zH^tgT+Qo-tFVDUwa!>rzc>CP5b4uklk2X(qjvN)8n5de)IkqjjEm1EyBAJytKWklb zdZa>jh17uT0jVX~OHyTX@^kVFBDviPW)<*|3Qvi&%WYTqP2`wp_pI*uv$JOBUz8n; z1mo@F_r`CC-jHaKFngZNe=jPQ;D2JL64js1#@!d6ohg^g&lG`6!y&OnLjsc zZaxRY#5+bh#-~Q6#=nSs5pNZ3m6#Nrl-Lp7k!Toem>eA&o!l7Pm|PbtN)65)oLZK> zES1bj7WB;NSui(eZUF}i6n4z*SlBD8SN`K!kLQQuC&%xPe;IF`xGAwS+9=j2Sub{F z?B!(TRJk0j!9UBJ6T1?PlVg&bl2uYeQp-~X1!osLQBbO|Q(^D?dHGrK&hcsSuj5B0 zZb`h8Xp$V8+?=eM8k$;>DlF(-Fs~r1uybLb{Q3Ejc$fI}_@4ODiOGp~6HSxjlCLDI zrG}+erpgxdDVSdnDeO|%H-ABXG=55aM*N%jF^O9f?h3DiynIDUv8h)zAjm(D1-k@oKKGOgX7NUj z24nmV=V-1uBY0f!c=q@w22bLruXg+dQ5Y;6EEn7_xPS0~;DJGYF%c{utPrditQ0H? zRt{DPR^?v;tMjjIz4@%%m(P3W@|mR{-*+$Id&dBN`!a~%AY90nhw_u$DZx|uZZ|); zAowJer}(|XLVk8%#Lvf1^E2o({G7L#pQV=Y^U6|wCU}<5-OKoF`y8Jimj_n_R|Z!F zpXYX2>?Zhm-#ubJ7RnBN82l*saq!bv^X!S)+p_EBjL2D+Qz5s0*2t{&SrsD#BTFOk zX!mH{tl?Q}v&u*MM;1r&qFtjiqk&l4*p%4rShMU2*;}*g<_ynSn^QiwUe<`Lby*c6 z10qWz`O$9CSy3z2E_P3>ZvOE6wfW`a{o{+{d5NxxnTbHMZE{L-cd}V(LTYQOZo%+^ zwFTu1>*bHgUzcAYJ|Mm%o}cKJn3b@S?UMH-KTS4IO-yY|)hifLu&$s&Vg3A(`Rns5 z#s|ii#^Z_ZiP_QF37&>z`&j$ry|H_fpT#~)w#aUgx*_|9)Z5u_r|Rd_FBq9KvS5AA z`htqN6$=|=HOL>8H7b8Y)`t8_kxKDFkwL$Dk0zptM2~2X#GL4yL?{+Yc8GOI-WR(s z`FZT~WXtT9sT;FzOl{BJo@$WOpkP$asDceS8wx7rRw`_m)i8f_*693=SsU|`{vZH>|LqGIgJZ`zUH5`v00n5sz!!JRzwP; zy`%G@S+UNsX|b;VVu*k|t*=V2W{AeWBB{n^_ zCw6r96Y~;T$RlWI~hwqSEX z)xxIv8`0>%>6L&=KNPHOmFmYJyu;lpI_~h%c*ON7~Yo;#E zzBIKudv)r7oC6BZ%Q>%LVa~#WoZOtk({oQR?3dLqe^J(={M<-x{EWyM@kb($#D9qV z5I-S$LgLQoor#a4A0-Zt9iF^4c5U*F*c-`O*|k!aWnY$Blf5Q&V9tRB{c`#hEXr9_ zkei!Zct-9Sh399TpZ|2$)A^+%rQ>Ht&Wt}Ac{KiGx)Qj0Krkot7;QXBP3!ct-x}bD!>B2K}&n&zk>w^4evYyE= z6DbovD{@x+vB+cbpCUiSPl}$DxI21x;*;nni6di2Ca;fOpL{F!Rt@$Y4bL8) zTARH#RX(SDLI0fo1&ebQ7v$yU6?V<-S~wtUK>m`fCHeW0{CKxWxA?5ctT-Qy6YZky z68A*!NqidpG|@cPJUKBoF}W?aEm<$SUTQ@4h}635b*TzD6$%FA3@BKVv!ozDH@~o3 zZnwgLSp)NzW-ZN+N8<7Bk?!%?k=b#+NhR7x+b8ag-kbO=`dOkytVPoQY8#aQYq2HB&s z%{)=qqi}HkvixMcXMAqlP9$=B*P(zo%9~&Q|$fFn$NX6V;2@YSCA^~RX8Mnd456s?D&N_&*h|Yd*u$vTAo!9IXm)1 zyi}r7;(^3hi8jfble?0QQ)5z_QdJ6u6fAe&??dxfk}UjrWNU%~_FCnAE17wsEe5RJx8iOq<86FVmR*6jDP56-_Te@lM#_(kzm@p6g2i3N#h z@|5I^jlzw?O~Oq%vhxs*PCSew?2h0lv1Z}s;TGYR z;Z_{)-iBkgkLI}JV}J3pd)M&HaG+G%Qd3IpF4Zh+Le|!-x{=|LwUP4C{?WzJyja)R z%vd11ZT6Jx-Pz4@Cgg0*sp~!VOy6D0%q$bgYnwMEZ+Bj^{0aG6^XtZk$JfToC;BHA zC-RbAlQWZnRNK^))b3QXf(Zp%3+lS7nYr~(fAI42`6Pbl4Kt_R_?=h&zrPwEmH*J? z=hLBhH~YEpOEXvCf5h+g^7H-vf%8+C8~xUKC)#p8is9@3ednF{`5Q$bu&o;Zi-L0` z820776M6&ayc5F;<9SDeyCCItDLHRV$$4w`;k-S2KQGOH&3S9;zG3A}c{6h^nyuS7 zKMd!snL^gxiktJ+y!z|&*0f#5*E#o$x$fT1TeF*Olz)3K=dIbuZ8vB39}`}~UuS=^ zh5I4@%z0~S{k)e8Y-`4Gxa>Qo!Xz+X>QXkI*R>q%m%(@Sy`Hy*Ux2$8?LX%4&uzgJ zYiGXXjuV?@?r+>)?$38sSNC>amw$GD%Bro zxc)M-3^|=N&5tL|uQvvh<~~m+&Hnc=(rmxT%N(*a^WFWt?UCjK)k1GRl{6op#(DGe zNb|$z@ud0KR+lVA=8^njC9wOv+&r^SUQK3^50mC2#(1(P*@x^!HYCp`^T^)h)^l_7 zR+0A^ zgRamKT0ujo27CR~vCm&{+n}rek&E@e^HBay&&b|iX20=?c<(p*U0?A({@-T%QPpX}$i9^!9) z?N8p{|LM(s^8xJR7ysA;`P0vjxto3L^zU)=-?3i*>^1Dx{vMCu7hU|02eeOD`NiG+ zUr)W>|F7@%Pu<^?wD-H+KXvPW|Ba@U|7-f0QX#Y5DHXk}(`@uy=s~FrSU&{`Tuo&|LVtWo`)&b|NQgvN1w;P z`}MWn?z+_Y?RA-QkoSG2W+wHw3}*8842IwL+y5r?k9COfFB9e;Cw_4$Rtg3KKmXYe z_kc|Pn802yW=o-e@%7*M@?N+5Z~ekA9`2u>TIL^AxUfX-CFG^;W!THiSN}d^)a$FA zmzR6F-pexPADp;s@a8XhIn>K>US8$pN-wK<|9EEpUx~R?@bU~VS9@9A+kOo%5Aw36 zm$kgC?PVP=>v~zw%lcj(?rm3RYpdz?QOnEPUY_h-Z-ti)yxSk^Wmhlzd3lYO3%p$7 z?d54+_VjY3m(#qQ>*ZoE-}f@;{R>dq%T`{t^YTT7~#A5x?AnxNTsP14Led0*G-{`Y26@9}B> zp#8$$Qu~Fypq@nQF?nQ@uxQ`{|N#?Rc!SMHeY z*30=--d|-{byO@;|OW>X~QOe=mN|d~e8%Yux7|Gy3;h{(XhN zE5Bz}GBalOF#dJOxPP0OS5am?{rio*^_KabnHl}p@%!HP(*9NTyT5rXjXZB!IU7z8O zkGNww_pxX8X3x2mRqmMCvpw$mLGH?Y>iU=NnAyu^_IM$83rD(ROLsiV9Zzz{%pUJB zcMDzJN@njj#PvSznAr!8bNw!Noav6M-0?+sT-39Xq?%@9z3gcf7!kM+rte*dSaiRNDO*wb` z#_fETyR~m!pX2&`cbw^t3*2!(_xk(0;{on?pgTJ5n0CkV?pVPcE4pJPcPw(p%I;Xj z9jm&d<=$h^9jm#O>h4&>9S?HHn(kQ39W#H&AQ3b_Q>EN-KX=UheTd9i`fIz_tmBTE zS9@*uS;*|WGQW|?JhOG(>-<0V?gGq>tHJWVW5zZ!Gc(2*$IQ&k%*@Qp%uF#e$;`~m z%nW7tj{bFZ-J4AEW!}s?yZh|4pYyv%SEW*^Bv-4YhV+;LGh!ypj9D-%X2Uh?ji2e+ zF$dT}oNK2Ol^r{e#pzL($3w=ko7ywvYH;)vk;I?H?9buXXa zjK0?`Jx*)0oo&q6){N~K-6N@fo-~kqd5_c4oZrceoz2*V@r_x($LVI~yPL5GqwjTL zkJHP{_cmi6GxjxOKStl{Prlc=9-8%gziRb*$SnWZj8DzzyPlWkwD0;}o72AayfdfYoAIL=Kbi5f8GYOJ#heacw#VblWjh0! z(}B$B+wLIdbWk$}Gvn`Xk8gS3?ecBEZ@Ycl`}^hoci(>B?e>lT^!EGqv+s8L_LIjs z!G8ALUf=!bTkiLxZ@>HQuXo`dhV$;j@8wclk zdVHhra-q$i)JM*Eh_&USd04gVSBZ(_K^3UDMND^U_^w&|RC*UE9!I z3(#FV&|SOGU9ZvIBY^8{w$^n$-Ssrx^)TJF1#58K&Ah8V!|48s#*Aw$x@$7JYel+i zJi2Qlx@%p!Yb&~IC%S8Qx@&*B>m9o51G?)Iy6XaNg==iOYht=Zf<)5 zvj>K;--hD|9EqcFwEXY+(AO^iZP)d8Zjr~?@*i%`3-+74hL`@jJM+rljK|r>KHQH7 z@E{(-!*~Rb;xRmqCuDja$r&&sX2Q&v1+!u{{28-j4u9sP=kjN6x@#W#FPPV#?r(~9 z&BuIxEPw^k<2+$&pW-uojxS`Eh~90?64ARw+3_{|=M8$CcP#lHKj26FgrCvDFZdO| z;dl9GA08+0K<}eH2nNMq7#u@jNDL*te;q}?0~d?)JgUTBKAZA?y&fkD>xqidFgnJN z?%(zFII)I0y6YFCGY=TX(19rqd*cS)k zU>uGka1@S4kCTIK&WX7&H|CL{xE14AavILWWw;7g<3`+$2k;>J-;@7UU9YCGg;lW{ zR>vAx6Kl!jyzb?G_fbl^`<;ua>F#$ey5Fr6Cl75EW_=Z z>hIPSqZh@J=-%U{BYMZ}xC;+r6_%-r<**u7$MRSMD_}!xgtf3XHpV(w51V0gY=JGY z6}HAU*cRJiC+v(}uq$@M?$`r+VlV86{c!*e!J#-3$KZIJfD>^NPR1!X9cSPyoQ-pE zF3!XGxBwU8B3z71a49aw75E#j#0|Ix*Wx-{kDIVAZo|#~+(O@hJJEx?a5wJ3y|@n# z;bA<2`|&6q!{h#}#UstV_W!E?HGf=>`@i*%|4&Q)<*~6n+u0lc$^Wl`tYH|AMfZP= zdsNxVOl$T-ZLEWN@E6RBzhXYjk5%w3zQgzU0X>f2Am6??!L_)jPSQ{L^9=nop2c%` z9xvi0e_p0v!K-*3Z{SV5<!zmE^_AwEL?Tj*|^?{@s3J9?aA+S?wdxV*rv zxQ2Jpx}57!Ezo8ur^-T!-s%18&4kxEZ(LR@{c$aR=^15AMRr^cn9y| zJ-m+(@F70J$M^)F;xl}XFYqP4!q@l)-{L!bk00azu?E(}T38$FU_ESr4Y3h6#wOSln_+Wofi1BWw#GKt7TaNa?0_Ay6L!Wf*cH2B zckF>Zu^0CCXCHcBbbm{7fBFC%h=cq&m_EdxL+QhCIF7)P=y9?S@gA9TNcUqD^P_PL zj>T~}9w*>LoPtwv8cxR*XUcm2N~Jx&FdtcaE5AMX)&&->rQ z?jCdZPhHkuA1C2tY|Jx(CiqXD!+bW+DB7@vZLuA;#}3#LJ7H(+f?cs2cE=vr6MJEA z`Nt2B(_iy*xh?Z>J}$t8xCj^HQuH_@Sm#K&oS7A7ehl+t_uiS;bpuc{qJFqb6qp;{#nAMmf>Z*V%Bq)%ifd! zeTYx- z8Q#Tv=y5`6Nskj6!=Rsh&bg1wGU1tzAn!Bt7+>LQe1mWC9lpm8_z^$hXLRrje#LM2 z9Ru)N;(x+`7zhKS$4Ra&2|^Ew!7w<6z>pXUJx+QpADZs!aWZKp3^OmdMt%RUX59YL z%w*S$j{nmA@Hn~D!*Z!`7#<^FMD#fMxKw^DfCXhF&W&uADZ+eF8HJgs7|onpl5B9@rCmVQ=}z4;}wCAA?&R6Jud)jDvA89wxv<=y68s zvWe+QFsYdztNCQ~xfjKc3dYmQN zlHBw>_zUL6Uoju%M;-sQd;xkvGqX-Jh3JK`2o}X+sMr6sjK|q3-H+nTm%x%(3QJ=d zEQ{r^B6^(tT0tO@*IAL@tz5XK`G1J)0hi5*5Y{E=aY=+IT z1-8Ui*c#hlTWp8zu>*F*PS_c{U{~yh-LVJuL>>RNCB5jqu@Cmee%K!eqK^Mseh_`I znaQM?A@pH597o_t)boEWqvOAHKSnV>8pq&R9Eam^0(zYMT($rfloOesWX>(Zd{H@> znJGBcoLiD}OJQj&gJrQCmd6Tm8kd@GE?b%TDp(b(N&kD;B9@rCmVQ=gs-H#dOy83HoCVdvp z#yL0_=b^_L#u|p>2sxkm1-Q^GGe+}^=!T;GH{%xEirdiRY|y2) z(|6!bGrxs(ZpCf5-7Dwr!rizB_oBzy!;*V(pWMg%emr27Ii&f6^h0Mf2JcDQP9G;g)&1Ek#a|tiw6}*bq@H*bWn|KRv;~l(<_wc^_vuEso{T|sNp2;7^ zBUqOI5#_KvR=|o_2_y6QqB332|JjBw_!Yn5chn!2{SQ8L{MY<3_Q`QPfhX}4p2jnH z4t4z3r7qAf;w3X5Uh|jfSMVxc!|QkhZ{jVyjd$=a-oyL&03YHbe2h=U`?zg&vHx8o7-EL`FdC%8_0ih{MXM%u%0&97TaNa?0_Ay6L!Wf*cH2BckF>Z zu^0BnKGOe(p8snm9D61_M!<*|2_s_^jD~vsN0*90kBPC&{20y0rpLj!7!TuP0!)aB zFfk^xfjKcBdYpAy zLoRx5%!9vRUexn{Eu-iE(*5|A`2tuF3t?d_f<>_y>h&L8wm7{6mNfH+G+&Bd8p~i= zbMA4j_XM8AQ+OKB;8|IY<;$B(U10tqUc$?G1+U^YncpmbLp>_p{X1~}I zU{$P!HBiTYU8)wnHr6ro;Wb~EUJu>B8rXo|5F24*Y=TX(88*ij*b-Y|YixsUu^qO@ z4%iVpVQ19wU)$4#-W9uHckF>Zu^0BnKG+xgVSgNe191=zMjiikEjs>VPRu2TaPCkX zhQo0Ljzm5GXZZqh6f>i7j5)W6=Eu^<;dq>2&MnDhOJQj&gJrQCmd6TMQBE|Mt;|dn ztcumJI@Z9NSW8ytQZ>zG>oQ*t>th4?&mRA+Sx*~mi|w#IcEFC<2|HsK?26s6JNCey z*b94OAL;)?$A8UC;&x5ODL56U;dGpVGjSHq#yL0_=iz)@fD3UEE=G?ti7lCoQ{)on zm*O&9jw|ptT#2i2HLk(6xDMCj2Hc37a5HYft+)-h;||=39#?0LwrUrBH}1i`xDWT^ z0X&F@(Bo{=We=OagWI@M9%1Gv9>e2!0#D*8^f(8&)ImHXPn-E8%pApIcwC<0+_QKN z&*KHWh#u#hmb_&81@+7HD|i*J;dQ)$H}MvFoSR(ZExe6)@GjoN`}hDK;v;;FPw*)| z!{_({U*aoyn`^v-j(K0~9p}Es5BL#3;b(O43x36K_#Fd`)L)Wfo%irQKEQ|g2p{7U ze2UNTIljP`_zGX+8+?oJ@I8LOkN62cqk~`YD}KZ87$BPW|L9K`5CdUg41z&17zW1> z7!pHaXbgjX7#71}c#MD%F%m|`C>Rx^VRVdvF)stRBI{gY$w7an6@%_FTkTV4ZeVlAwVb+9hh!}{0& z8)74Dj7_j9HpAxF0$XA$Y>jQOEw;n<*a16YC+v(}uq$@M?$`r+VlV71ld#{CqQ~i{ znPjF9V3~n3IWsBD{1DBgG<_J$440|Q%t&TN;byo2E;%Z7=!pTC_NYk#}F72Ltzap&YEk|Yhi8lfB)8DrY_dQ`q%&);y<~6Q}}tW zEhYZ_?^<`Su@TqqZf#?Ky@|g*lG`{6N8=c5$}-KcB9GY0Sk*X1V|98>tc`Wi;}jkh zAV7V3Lu`yqjo!;Pr?`ZhcE+wo@44OSJ+U|THG0qOPalYbaVQSQkvJO1 z;&_~hlW{6mj5a}|yS&F~rd#B3nqv!WiLJ1;oXw@?;9OjQ3vm%H#wEBEm*H~sKFamV zIlA|n>pW)W|6zV5Gok(6o9^<{nV*3(aTa=<;cV>)9Eqc3CC;sa)r{UfS%Y2+>tH?f zIFog$2J}YQ1e+PXmu*3Bg>A5%(R*$OdME6H-HhIId(eAfAM9uJo;!d(2#4S>9D$>7 z430zZy{NB}(0h5kmxPtLf8DkDzpTgEu3Oifz1jj>Vk>NoZLlr2L+^U^xTSZ%j@Su1 zV;A%|$Jq17WmhwEl9^L@8qeTaJcsA;0$#*Rcp0zYRlJ7R@dn<+TX-Aq;9b0j_wfNf z#7FoTpWst`hR^W@zQkAf8sFese24Gx1AfF$_!%Aif?x3)ewXQ^d-rVy%!rvVGiJf8 zm<@l%?3e>{VlK>$dGHs^i@#z%%#Q`IAQr;HSOkk=F)WTHuq2kjvRDqwqsK`wTCZ8r zD`F+Aj8)L%B-1ig>DA0kO3l=!*TK4259?zCY=|Bwqn6j}|Cm|+@uQhpGCRxUz?_&1 zb7LO)Pd^NPR1!X6{q2JoPjfO zmb}H!+TX@Ico*;CeSCnA@G(BYr}zw?<4b&nukj7O#dr7tKjJ6+j1GRmulNnWV*q{< z;U^4;fiVa^WSJNE9s@BS6!rZ-+};Ct5D(#DJc38@7#_zH^8bbX6O6qS97C8r_K7|4 zS+?Q$px*<;%vbz|-(}s{-lJDNtd9+_AvVIs*aVwmGi;76uqC#_*4PHyVmoY)9k3&I z!p_(QyJ9!&jyEUJ7^NZrp==aUbr-19%V*;bA<2NAVaQ z#}jxGPvL1igJJ5<_8V41<0c7QVSG%02{92S z#w3^&lVNg9fhjQ+rp7dw7SmyR%zzm&6K2LNm=&|(&zK!^U{1`1xiJs^f_d>*%!m20 z02ahTSQv|7Q7neVu>_XHQdkx4=M$CknF$-qJZ1^)~#~hdwb75}GgTG*2 z{1x+Iek_0mu@Dxmq=6{}%&tbsML7S_f( z=y3{Z|JOCWhi7R>`Ie?vQEx?Wjcu^4 zIkzV3uO-_t(;houN9=^1u?u#^ZrB}rU{CCYy|EAW#eUcy2jD;)goAMi4#ic zz=gO77vmCKipy|0uE5`LC9cBNxCYnaI$Vz%a3gNQ&A0`(;x^ol9%rSte~0O-)pxob zcj0bx?gnn}M!AQXy|@qe;{iN~htT8f)bfW--=%(peiV=4adYl|)_*{rVCE#A!qa#L z&*C}M>;GE*JpBS*#7lS?ui#bmI2W}1HTreDfj99M-o`t47w_SHe1H$}5kAHz_!OVv zb9{j>@fE(tH|TL*XnWq$-{E`wfFJP_entns;8*;H-!T9`FY^-y#6TDrgJ4h$hQToe zhQv@98pEI;hQ)9g9wVT~2|F%8fQY7tSC2%Gj8QPEIX8;V^*B*wG-je>42+4fFgC_P zj}uqR$EC-^_?Q3_Vj@h8NiZoU!{nF(Q(`JijcG6~ro;4@0W)GI%#2wuD`vx=F+1kK zoR|x9V;=kk^Wv|V5A$OI)bU@pw;;U`7RDl26pLYTEP*Al6qd#^SQg7+d8~jHu@Y8B zJ^$z4sg5%Uwh}}Xc+Hv*JF4bPvA*Bg{Schp2c%` z9z9MEwz((v!rs^i`(i)rj{|TZ4#L4W1c%}<9F8M!B#y$-I0nb!I2?}?a3W5^$v6e4 z;xwF&GjJx(l2u}O_eoW(hSjkK*2G#^8|z?QtcUfn0XD=&*ch8&Q*4IKu?4ooR@fTb zU|Vd5?Xd%P#7@{5yI@!BhTX9T_QYP;8~b2i?1%kv01m`KI2ecEP#lKCaRiRUQ8*gM z;8+}o<8cB`#7Q_Ar{GkahSPBd&csLv1FYd$rcmNOLAv}yn@F*U`<9Gs3;we0hXYeeZ!}E9nFXAP8n18?Fj zyp4D8F5biY_y8Z`BYccc@F_mS=lB9&;wyZOZ}2U?!}s_BKjJ6+j1GRmulNnWV}O|6 zBgRh{5CdUg41z&17zW1>7!pHaXbgjX7#71}c#MD%F%m|`C>Rx^VRVdvF)Ib6`%)g}E^g z{(^b&SImd`u>cmtLRc7!U{NfF#jymI#8Oxq%V1e7hvl&XR>VqJ8LMDbtcKOG2G+z{ zSR3nLU95-ou>m&3M%WmeU{h>{&9Mcx#8%iE+hAL4hwZTgcEnED8M|Ot?1tU32lm8X z*cY>oQBhJ2F}D; zI2-5ST%3pVaRDyGMYtH3;8I+M%W;MLO{kvb2bDw0VdaQ&R5_*`S57D=l~c-T<&1JxIj5XgE+`k3 zOUh;CigH!Crd(HUC^wZ`%5CM2a#y*h+*ckb50yvCW95nRRC%U6S6(PDl~>AZ<&E-I zd8fQrJ}4iRPs(S-QNAc&m2b*-B|t14SCxQDASJL8L6G+J1|_4CNy)5aQL-x8l%JLCN=_x0l3U56{G#MlepT`*MU`SoaixS( zQYodBR>~-4m2ygXrGipXsiag^swh>J>Piizrcz6(t<+KKD)p55N&}^#(nx8nG*Ox= z&6MU!3#FyfN@=aMQQ9i)l=eyorK8eG>8x~7x+>k2?n)1(r_xL5t@Kg)D*crH$^d1c zGDsP$3{i$E!<6C52xX))N*S$;QN}9cl<~?0Wuh`knXF7vrYh5v>BivEY=M%;v(aSLw6ZMYqG z;7;`5F5HcKa4+t|{dfQm;vqbYNAM^f!{c}YPvR*&jc4#Ip2PEa0Wabuyo^`yDqh3u zcmr?ZExe6)@GjoN`}hDK;v;;FPw*)|!{_({U*ao#jc@QRzQgzU0YBm={EQBM!LRrY zzheO2_4x?{Vjv8RK`{^!=oPUl*2G#^4QpU+tn&xm z{bRPDd$tso#-g#k*I72UcZ+hz_TFzE=N0$(YkY%m@g2U$5BL#3;b(O43x36K_+8%S z`!(<2UA%|)@c}->NB9_@;8T2t&rv`BIZ4l@=r8dVzQ#BB7T=-AiLB-I``8!>||*!(#-Dh>mq=6{}%&tbsMLmW<2V;$eJDfC(`XCdMR~6q8|cOo1uUp#yfrPS_c{U{~yh-LVJu#9p#FYiofmu@$z)HrN*1VSDU=9kCO3 z#xB?uyJ2_ifjzMo_QpQg7yDs<9DoCH5Dvy6I24EBa2$anaWsy>u{aLL;{=?DlW;Ol z!KpY6r{fHqiL-Dv&cV4j59i|oT!@QsF)qQSxD1!$3j7UM;woH?YtZ`)^}pg{t-rOk zFD7@i7r5!DN^c(_wndgxN3$ z=EPi>8?UoHxA7i6!DsjqU*iY-g5U5b)({ATV?>OC(J>BYzyz2SQ(+eT8S~&T7!&Vs zE%)&ezQO1C5mU2dU<``inGa2mgi$drCdaIp9W!EP%!@HFAwFPjY3PYD1;#@?|L3vM z1-oK5?2bLKC-#yTSldOsgqQIOUd3y86CYw)uIm#$7X4Suj|H$07RF*&0!w0PEQ{r_ z0v5q?7z96aEn(<>m=6nLQ7n#Ou?&X8@K_NmVJWPP9%m{0VHqyR75E#j#8tQ&*Wg-Q zhwE_zZp2Nv8Mok8+=kn62kt}ZzFARfZQcm$8)F+7eZ@FbqX(|88Y z;yFBz7w{rp!pnFCui`bljyLco-oo2>2k+uNypIp?AwI&#_ynKgGklIO@Fl*&*Z2nC z;yZkgAMhi7!q4d77yOFf@Vi_U&3pV_jcaf%uEX`X0XO0%+>BdrD{jN>xC3{h2Y2Ca z+=F{@AMVEkcn}ZaVLXCI(c{FP;{A*e2jj|P%pb=ScoI+HX*`2x@f@DV3wRMdPAb-! z8q;7}d5Lo`;}yJ$*YG;tz?*mr|M)pkKmX4T;GH{%xEira8I?!cYs!Ckl;_uyXKhx_pW9>ha<7?0plJch^d1fIlG z=y6(3@!rd={H|&l*uqXDy-q;8GVn6JU18^V? z!ofHMhvG0Cjw5g+j>6G62FKz!9FG%lB2L1|I0dKTG@Onza3;>e**FL1;yj#>3veMW z!o|1*m*O&9jw|ptT#2i2HLk(6xDMCj2Hc37a5HYft+)-h;||=39^8eyaS!greYhVF z;6Xfuhw%s=#bbCJPvA*Bg{Schp2c%`9xvcUyo8tW3SPx)cpY!ZZv6l4jy*!+EbOAtu7am;(!A z1#F4^a0_n5C-?!s;!okd>kN+TFeA4s8y3Zi*aC;)XxxRj(7|wQXM9Y9)v!J`$G$ih z7oi6OvORIJ47yvAn>GK6?)~C!mH+J-&$*M)y*F*PS_c{U{~yh-LVJu#9r7N`(R(}hy8H?4#Yt?7>D3c9EQVj1dhZ} zI2y;`SR9AraRN@nNjMp&;8dK3({TpQ#925S=ipqNhx2g(F2qH+7?Jq{43Fap zJc+09G@ik;cn;6w1-yut@G@S(t9T8s;|;utx9~RJ!Mk`5@8bh}h>!3wKEbE>44>l* ze2K5{HNL^O_zvIW2mFYi@H0C21;64q{4U+2jjMZv`D<%)v~hKhMgN-a5%phlGuz3$~qqweS2f6c0V=K7;GnRjhU|7+dHs=FWDEpacK(QmCT?e1HTGkvD_ z^WzMhiL<2t|G)ny^ndMt^xwbKAODBCGk^b)|KGFZzxF8o>r456T>YJ{pa0}>;@}tj zir?_Ne8}VT-{&*d|JUmM-&x+rc2xP-OSU3q4LxZNJCqCH=4e-_iYV=f9)-zg_>1{@44&dz4 z(eHoM`u)%U*<-+6#$Bg>{XaJRcaL+H$G|!1fBw(9$GNH{qp_X-_5T?2-`%fsUH#Ah zS$CIo_k(L_u1kOaiS2wP|9JoE=YM`T@Bi`t$K}KL+kTJp-FsPPysw7*cQo(y{O3Lr z+wJ?&lGwSVwk5MIxosH&`>rdNo$G8{7u&kp*3GtFw)M8Hk8OQzyJ*`b+b-L7#kLy$ zny=Rg=>6y2#;f++V73Lf&37yQ{`lj*JZIN(-nN_mny=ps=>6xthFkXBTLFFD+uVNF zp4;8F9=6@HtxUi&rdPLf;cdHb+e6zP+xEn^YqlM;ZM_6IcmL}1t39`hZB=cnW?OaJ zYS>oOv>*3xfWP_Saq8Q(G_b9qZH;VeY+Do40{nvy_q{9c3f$-E-sW-I&i0Pp)^0W> zKx)3G_jdR1A$eO`JD1M3^tNTNEt&m)p2@b6KtDk+a%j2+cw3vskTkCZMtnUY@2D@ zEYqUdeHGod7`DZ) z*mlsiL$)2Z?TBqhZ98V$aobLq7T|wA{Otjc^Tg~IkMq>FXSO}J?S*YGZF^qp-ockJzx4ExaxbH!E&z)|UD;mgsg;qZkX#eNAuXK5r`|I}1UgF1RjUUZ@rPaHZ zA8m%c-XF*I`eEJe)K5^^b7$G+zV_)|%Q{}e^fvc(&p&Ik?PcfK_BgEjI<9`|FWfLc zcY6Yc^Uck*%ek)rde=P9&biOey>s*J+>hlJ*g5yrXYX?ED}3JOzDnyPR)3^)qdDAEb}yy`1+WgKZgY%VgV+`(v?P-)6sze(vj``hJ%= z?phv)b4R_BY1pU_c=mJ+q`q`a~SWO+fvz<+BWZU?lYV;wt45= zXE@$VxXpX6+tS(QUCw*U{6DL5{Cv0F+uZK>xvzNZZ9m-z8N@5kfs$Nll+apk)%@0xwx+uZ)YfBP=*dttnL@}g~*Y`bim{vMRQ z_XpTE(6&Lg4YqBFZ9{DvX4`PvM%Xsewo#`2W*@t&Y+GsDYTMS>w$`?Fwyn2qgKZma z`*9sV*1XA{yVpd$!%T?SX9%ZF_3lW7~e5`^e7yIQNO2`*GQ4cJ8@tFKl~h+bi2% z+xEt`x3;~r?Y(UuZ2M^2C)+;T=GgYdwy(B*v+cWW0qp0apKJ?gTOiv4+ZM#Optc3G zEx2tVYzt{yDBD8Y7REL|+rruw&bIKjMX)WRZINt?Y+DrDqS_YCwCHAxVaAxh8)KQ% zvCSCAjB$TA#xtkm|87iRPA4*BVlyT&qwjfu?<@GsW!@_V#cawtY*gQX7nvr!g-&oh2u4l&jW^7=_hGuMJ#>QrBV#cOsY-YyhW^7@`mS${a#@1$R zW5%{-Y-h&yX6#_bj%Ms+M&Ij1oy}?A*u|XgYQ}D6>~6*$X6$LkUS{lV#y)23YewJg zPi0Q0G2`#o-_OkVH{0os&G`G%gUtM3GY&E1P%{oQ<8U+ne)$n*exw)!hjeE17i>j zioq~AhQN>*3PWQU^uw?i4#Q&vjEIpiGDg9u7!9Li42+4fFgC`)xEK%PV**Twzwy=c zN=(E|VoZWbF&QSu6qpiIVQNf+X)zt9#|)SeGht@Tf>|*e{*2i%2j;|Fm>cupFPIm9 z#eA3_3t&MkgoUvP7R6#%97|wHEQO`943@=mSRN~2MXZFCu?kkjYFHg>U`?!rwXqJ? z#d=sD8(>3hgpIKYHpOPx99v*ZY=y0{4YtL0*d9AzN9=^1u?u#^ZrB}rU{CCYy|EAW z#eUcy2jD;)goAMi4#iZ z;~w0L`*1%Vz=L=QJ9bZFXI)wir4Tu-oTr93vc5cyo>kn zK0d&Q_y`~46MTx#@HxJ~m-q@_;~RX7@9;f-z>oL|Kcj>Kx3$|Uf?UHSmZM$OIRokxFcHOocw%xStmTk9f zyJOp3+wR$R-?j&~J+$qSZI5kxV%t;Op4s-?wimX&w9Vrro$tL5ypPdjwk5YMg>5Nq zOJ!SX+tS#U*0yxErME4EZ5eILWLsw2ve=f@wrsZjY+H8Qa@dyBwp_O5wk?louk1c} zZQC2$-rDxgw)eJuu* zXj>)QD%)1YwyL&Ov#q*qHEgSCTP@T6ec!c^!q4};5h?t9*OJ1|_q`D*{CwXVk-{&v zdABZwpYMA&Quz74cO!+L?|U~=`1!tfBZZ&udpAlhp`g`fNV8G{3+v@MlwsclPR zTUy)F*_Ph647Pc1iFe)J>u|5d+uUpMHuqY*&Ak?HbFanQ+-vbR_gcKoy%ukCuf@B~ z?scT_^L=+$O50M|mfE&7wxzW#oo(rD%V3-LR=Df--U?syU5oct_~v}q;=L8VIp4K- zZ-sBpcP-vq;m&z)h1&)P%xL#tQQKMF zrV>kut;A8{D)E%~N&+RJl1NFcBvFzo$&}pOx%N4kf3OOUbR|QGQYKD!(fEl>ABorJzzsDXbJxiYmpF;z|jnq*6*Lt&~y9 zD&>^&N(H5&Qc0<-R8gub)s*T=4W*`1OR25YQR*u7l=?~orJ>SDX{W*9WuP)h8LSLZ zhAP99;mQbQq%ukwt&CB|D&v&#$^>PiGD(@NOi`vP)0FAT3}vP=OPQ_AQRXW1l=;d6 zWudZ2S*$EkmMY7X<;n`>H)W-=N?EO}QPwK!l=aF6Wuvl5*{p0)wkq3{?aB^ir{Ym| zDZ7n znsQyaq1;q%DYun7%3bB2a$k9%JX9VjkCi9NQ{|cRTzR3qR9-2sl{d;;<(=|g`JjAM zJ}I9SNBN@Y2!P-4I|c~mo&N~~Vjv8RK`O}p7vo`kOn?b75hlhYm=u#?a!i3~LwWZ@I!uolFe7Hd z%$NmJa;a398q>Jz!E~4&Ghjx{gqbl5X2opyGiJvem=kkhZp?$fU|#$c^I?80fCaG- z7RDl26pLYTEP*Al6qd#^SQg7+d8~jHu@Y9sDp(b(VRfv5HL(_^VQQ?+d>yQd z^{_rRz=qfe8)Fk}ip{V&wm|*-mmXvK`7ha$nO4{u+hAL4hwZTgcEnDYoO?Wld%t5U zOpR%aR$!BS+Xl@?q=3Mm-%@(9~a<4T!f2p z2`9Zm z2oK{CJc`HgIG(_hcnVMB89a;U@H}3?i+Bky;}yJ$*YG;tz?*mrZ{r=ji}&z8KEQ|g z2p{7Ue2UNTIljP`_zGX+8+?oJ@I8LOkN62cqk~`YD}KZ87+{f}1@QRkfjzModYmAO z^qhd+2m4|_^f)0GdH)}UlKq((fCF(54#puk6o=t(9DyTo6pqF*I2Om@c$|O}aS~3( zDL56U;dGpVGjSH`^|eKMmO-C`b8#Nd#|5|$7vW-Df=h83F2@!48?MAvxEj~sT3mr^ zcn9yI`xTS>%9Q(+lKYC3`)ZW?N|gI5l=}*l`|6YX)zJU0%id$lJ>6HvhMkSMy zS;?YgRkA5RE7_GCN=_x0l3U56{G#MlepT`*`IQ1nL8XvVSSg|uRf;Lal@dxxrIb=y zDWjBC$|>cQ3Q9$#l2TczqEuC?DbDfQd_B`)K%&!^_2!nL#2_@SZSg(RhlWy zl@>}%rIpfJX`{4N+9~ao4oXL*lhRq~qI6ZdDczMGN>8Pi(p%}H^i}#P{gnaAKxL3J zSQ(-WRfZ|Ul@ZEFWt1{n8KaC<#wp{K3Ccuek}_GDqD)n$Dbtl1%1mXJGFzFW%vI(o z^OXh4LS>P%SXrVhRhB8sl@-cw%1ULGvRYZAtX0-2>y-`4MrD(-S=pj&RkkVHl^x1X z#iQ&}b}M_7y~;jizj8o1s2oxbD@T;0$}#1*azZ(&oKj9JXOy$bIpw@^LAj`0QZ6f3 zl&i`$<+^f1xvAVzZYy_`yUIP~zVbkMs60|0D^HZC$}{D;@tt##8%=cag}&Vd?kUBP)VdDR+1=5m1Igb9R+??vMV{1 zoJuYww~|NsMairDs^nAhD+QE-N+G4NQbZ}L6jO>TC6tm%DW$YhMk%Y5Q_3q9l!{6v zrLs~*sj5^{sw*{=no2FDwo*r_tJG8KD-D!}N+YGQ(nM*hG*g-@EtHl@E2Xv4Mro_G zQ`##Xl#WU#rL)pS>8f;7x+^`Do=PvJx6()HtMpU)D+82)${=O1GDI1w3{!?HBb1TK zC}p%VMj5M&Q^qS3l!?kDWwJ6wnW{`vrYkd)naV6>wlYVVtISj8D+`o`$|7a4vP4;` zEK`;%E0o`qmC7n*wX#N8tE^MjD;t!J$|hyAvPGG!_sFIwQ z98r!c$CTsB3FV}6N;$2ZQO+vol=I33<)U&)xvX4Kt}54*>&gw~rgBTUt=v)WD)*H8 z$^+%0@<@5CJW-x1&y?rN3+1KqN_nlkQQj)=l=sR9<)iXR`K&m~7v-z+P5G__2%%$x z5>N@G1XhA5L6u-ia3zEiQVFGmR>CNLN?0YF5?+a*L{uUvk(DS)R3(}cU5TN@RAMQy zm5I7PCMj{$<0|nK_anY-32aMfTO!*M+m^((q_!oqExBzeY)fgI`~C360_$0aol9+7 z8r#y^md>{Hwq>v_qivaN%WPW~+p^l0&9t8Mvg z%Wqo&+X~uN$hN|^6|t?TZN+RWZd(c4O4?S+w$iqhv8}9a2t*dR_Z0l}Y58Ha$*2}iuw)L^CuWkKo>u=it z+XmV;$hN_@4Y6&gZNqFEZrcdkM%p&Yw$ZkYv2CnvxdP@2m|8~zeFYNO9o^>kadx@! zwoR~YqHU9Gn{3+@+osw!&9>>b&9H5zZL@5fZQC5%=Gr#Tw)wU#ux+7ji)>qL+Y;NB z+P2KL<+iP`?Kj(2+P2EJ)wZp%ZLMwVY+G;J2HQ5;w#l~5wr#O(t8LqC+iu$q+jiRK zv2B-ayKUQJ+g{uD*|y)d1GXKs?T~GUZ98JyQQMB$cHFiTww<)?lx?SNJ7e2f+s@f` z-nI+2U9|0zZI^AkV%t^QuGx0owi~wHwC$E{w{5#)+g;o4*>>Ny2ev)5?U8MdZF^$d zQ`?@|_T07?w!O6Nm2IzWdt=*M+uqss-nI|6ef$sFCp-7qHpjLvwtcnjn{D513*i0A zjE|pe3us#)+XCAb#I~Te1+y)RAL z6x*WO7R|Qkw#Be5CM_1m#yA)k<6(SEfC(`XCdMR~6qBLH@muD7WiKp-!|)gZBVr_s zj8QNuM#JbB17l(=jE!+HF2=+7m;e)EB20`)FexU(th3Kh>fr@Ho>OY44Y#MY>BO~HMYUF*bduc z2keNQurqeSuGkH`V-M_!y|6d-!M@lJ`{Mu{h=cI|WA8rTq$n15?;}A$GK-3VWR_^g z%&sC)a?VK3ISNWp1SE?Jh=LgvMUvz&tK^&{=PWs=CcIDms>Ye_Ie5IsbMF1W_n!Ii ze5<;;x~IFRyLM)FndyuD@ICC018^V?!ofHMhvNG<3_rjR@gp3LBXA^sjHB=q{1ivy z82k)B$1m_p{0hIuZ*VMri{Ih*I1YcnA8|bXgcEQgPQuAJ1%Jk=I1Q)c44jFxa5m1t zxi}9)n25jNuQ(qU;6hx4i*X4q#bvl0SKvxqg{yH5uElk@9)H6PxDhwuX54~XaT{*O z9k>&B;cnc6dvPD`#{+l}58>~47?0plJch^d1fIlGcpA^(Sv-g5@d94NOL!SQyng5!&3Fr@#8h}Irp7dw7SmyR%zzm&6UJd?ybW*1J1`60iFe`M zcn@a9Y?vK$U{1Uj@5B4?0elc2!iVt@%!Roz59Y;um>(a-$MA7{0-waE@M$c7&)~E8 z96paP;EVVY7Q}c=z?bnAEQGIOVJw10u^1M|68IXH#8Oxq%V1e7hvl&XR>VqJ8LMDb ztcKOG2G+z{SR3o$>sS}-VSQ|X4Y3h6#wOSln_+Wofi1BWw#GKt7TaNa?0_Ay6L!Wf z_y%^xZulm?h23Sm_w-6*uXV=i)T?1toMq>(F>`lelC`Fu6l<_l6HLF&v|I0`eji5H z&ulk+-q>L*yVF<(=i{&V+8)#X)P8eSemvgPcdH)n?Z=b{P5q4{#ub?HsHs1N&tdbE zrX~03ha_fb{m+=W{m&VXpEo{t$@n~8x@_u~t{BH$GnT(@EQhup-SQso*8Zv| zcu9L0dti6$hgW)<_Ab4GmU`E;)bGnu{fyl)5!XifS${J(>pgF5V z61?Ug8^6UDMwxoIPmI||8#~}PI3LaD@F#f1#~90CIW(V4p5P6{k@!iJV^j}$XLWAq z^PllNUcifZ2`{6ESMVxc!|O8MOS01VFxqROAa#5G_ohAoKf{?=kZYj8BDr0bupO4! zYUcL9ix_>KKErj|65HTbuG9T^3a???9cDfD`t45rEMCLZJI&lUwAc0P)Z3xGx>Is> zKaC|YKiBn>*awH>S2zQAp&bEE9x(S}$AO0#4RSCJ)Wt418JFRD+=}TLA2Q>U*o?6v z7h}TH_&gTGB3Kpe=K)IUK5yc9oQHN4 z+Iqpv-GgV)j!V-onfAHZp3!NU9iy<_6*KoDK7HNP%c32T=BS?F-Ng8l1~cIk*d6WY zw3pH8FrGm>UZr8Yx*M}&OKgjsu_Pl{GmK{(dkyR1TZ~}+qsFl$@0qPCFu+(CD`IE- z6zem-wT>F=#tkz4m*OZ!yb1U-+7YkVP}652F2^-!$G{#Rnz?q|oBENd&p z@MoNg({MV@z?m5GuCUIlcnz=1R7uVH3t&UsgR@vFMLILLX0~?O!v6~@`jayz&~qYA z!pS%Vf5xde4QI2?IXIn`88{PX**-W2=i)rfjc?;S*c<1vwp?syZp?#uF(2l~NAWRy z9L>J{vs)hSjpl+>5-CBvdwK8>U73$Wi)UDO1TdPyI)}U^!N!@C;ReytlwWF*<-TFFp zYhCKrdeoz>Pm8qyb!$WFjj%B`!KTL~~6n=uEaSVQrU*MPc6@HE1;8^?)f5h?l z6HdT)aUc%Eke8Qz&xiT(QCT;uxgzUfQhL^>o}79LY(PsxY=kLkPldN)YD|M^u^T-b zV-swOZ(}oTjxDeyw!+rf2HRpgY>#ts9)>Uxf5BgIJ}$t8xCj^H5?qSQa5=8PmADF5 z;~HFxbFd?J!tCsC4!j>9#N3!4AH%ok{}lBC*qxRh*bCpm-q;WO<3JpQgK-EB#Sd@< zeu3ZNcr3Lv1FYd$r zcmNOLA^aT=;}JZH$M86wkoNgo^s_QEYU}S_u^Bc;`z)v>^;T$~+x+=+mA*;qX0@M` zVb*FsBc*;|x;blMPRyRV7hf+x=moZZD6`xV7h%^x^C9FQ7vZw z{L>8r(~SbtjRVt70@KX`(=7tiEd$f71Ji8+(`^IO?E=&71JfwQ?w#@oV z)@0et1HSW(@!4tPe1JIpWv+->Ytx|qknq;e*bi?L;mShzV*d>FJ1D_ zPrKef-Q{%jbb|N!favLXuV!HRDQo@n_Xdtfx4`-vZVmK5@1MSDRrGX%wknem7d9Mbh3kRl)1g47yri%rpiwCAl1g2jLOqUEymkLam z_DzpXZ@#xBaz*6+R)~+jk!#)AoIc$h3V& zA~NlNZz7LAktsaSlz9T%mnX1&dH(oZF1u8o!1m?&<1@PK{JK0>v2~+;UK^RV&uSyn z_L*&D+CFoQOxx$Mk!kx3HZpCW#YU!E2JYWJr;W_F&u$~r_IYk(+CJlrOxx$ak!dsf zN2bjF`KQh3A3fiU{?XHB^pBo4qkr_Y8U3TD&FCLJZASm-X*2pqPn*#{dfJTs(bH!1 zkDfN8fAq9jU;1oyHOAXk^V6V3R{KocwCJ-AeCPXoo{2`*V1NG;J#C(yMsD$*d7c`% z*=MZL(;o!pw^$RIYo4Vh3(Rj8m|rt6-@Jbox!K=zL{FRFSVV5|p7}jS31TL zn;HEhQ)YY2Z;c{1`+L>s>0E~*Q>K3^-}=nx@1LJ`y??sP>F8-Q`bSTj(ceEkWvzdD zQ(%4py%LDr;yv>lt;j9GYceD6^uoi{LE=M9Y4 zc?08h-oWF0fms3q(>r;#<-_uV7Y z_I=vOw0-|QGHu_3k4)QlbR*MMcqJB@wy)NB#ueGfk}ZQsq0OxxF~k?AIZ^)(Gl+gG%a z{`R$PWZJ&UjZE9uyOC-8$~Q7?-|LS|+jspV)AluTWZM2FATn)#ClHyoza5B7+usdD zraJ}p$G-ZG%s21ZN8jvk4lKdR-jy^*a&w&?4kT(+LsS_WEDr}J-l`D~5UEBfp%uWA3;RUum?wMDi~ z{}Wih6|3sI|B9NbtVDX5W&CG(CC&FgZk4l@=eHq|r7CCZoUI=3aEIF}XX~au(Q7s> z{_Ux(Y5y5hC0pzj-N?5V{U7FR9Rq8wqGR~FuU8e_S9nWoXch3u0H_um9X=skQna z4b$TPS<*_|?0-~SY0dsexm93`^?&pO%b1qvJupYewD|Y5HRoaE{MP^TS4(8S%~Gwk zSN@+St<@uXgfe}4>Zo>#+Nw6{;U5*+1V$Epo`71+`4PFlb}^%bY4M+({wv#TVF#_l ze|$_!WUZ!8N1Yq~eea_EiazJ``2{)?Oy9`u4L!!uBb`3qK+`RJN2qJm$YYo6%41P2 z>i+y=kLhKeoz3vQ0yEFg{4M4gn!m+7i}bgc=aK#v^Gwp;VxCL-Tg2ER5 zDE%$=Ic0R4eOBpj32wFdL~Z|?%;$p#TY_6{J}*4jC%Dz-^TUIEf?I7qiQC`DyfYPS z32t-lm~A%iPx+VHoo;OUC!&77n(xu|Pn+)r z*piqo*iuKwIr2&1W>0hKsQTN+rk@d5bN!f>iy4A_E@p^+U1Zi=-Se?HN=*rsZOWV4vzSeR9UM{G&dRZ-KY>RXu&M;^`vaS#MkFqzjI{bpnr;K1(TGf#f-T*L&X{$4dzQ9Tywtky?lG6Pq0fL z`BnqFzSyz1dU`J~x>VPC{G-cQjcoqy8LQ*ve=Nr8+VStl*!2FNl=>8*f6x*zWjZHTW!AI#D8!0*_VH*;8xpbU;aM9t+vm;qWjoq zU;dWhHrr=k{yxEN&KTIeU`kAdw_<8cgK04xrpFAJ5i?;NX2#p_cDw_#;GK9E-i`NQR?LRk zF$df*q9cy4s ztcA6)4!(|cu^!gP2G|fAVPkB9O|cm^#}?QUTVZQ#gKe=Lw#N?G5j$aL?1FD#SL}vw z;#=4qdtguOg>U0K*c;!)KG+xg;d|I02jD;)goAMi4#oFz7=C~s;zu|fN8m{O7)RkJ z_$iLYG58sNj$h!H_!WMQ-{4sM7Qe&qaUA}DKjL`&2`Au0oP?8c3jQoZ-c+21({TpQ z#925S=ipqNCkv!DKe?X4XYo0F9$&y0@g*#X@tA-w<11JQU&X>$1dC!ZERH4cH7tpx zur!vzvRDqwV+E{;m9R2a!Kzpdt78qUiM6mc*1^}YF4n{P*Z>=1BW#RKuqigf=GX#T zVk>NoZLlr2!}iz#JK{vWb2LertV~gUR;DV`ll|{;8Wr?y>S*9#kRwyf#Rmy5*jj~o*r>s|gQ#L3Yl}*ZKWs9;^*`{n)b|^cQ zUCM4{kFrNDW%4Nk< zt|(WPYsz&cNp^jPqmoofrX*KVC^sv&C@Gax%B@OjC5@6+NvEV&GAJ39OiG-RS-DNQ zUAaTaqTH$6rQEIDqhwXGDcO}AN>1foQblQd}va zyrz^?N-3q4GD=ycoKjw?pj1>UDV3EfN>!zrQeCN`)KqFIwUs)`>q=dvo>E_Fpfpq( zDUFpTN>ino(p+hwv{YItt(7)PTcw@SUg@B8R5~f0l`hH~N>` z%5TaBWuvl5*{p0)wkq3{?aB^ir?N}gt?W_uD*KfE$^qq|a!C1IIjkH}jw;8Lt9K#BvXEx0%45po$`i_y%2UeIN&)2=8}h>1}cM;!O9S2sPeut zO!+|hQ29t1u8dGdDjzGOluwjTmC?!={j+DdzF34 ze&v92P&uUht{hg5C`XlJ%5mj{a#A^^oL0^#XO(lxdF6s~QMsgCRy^g3a#gveTvw9h z)b+0>6HvhMkSLHr({-cQ*Kx8P_ig@Dt9S& zEB7c_m266OC5MtzxmUSQxnFrec~E&sd02Tw$))60@+f(gd`f=hQROk^apei+N#!Z! zX{CVjjPk7VobtT#g7TvBl2TBKR}z$$l~C6!W2X{C%( zRw<{HS1KqKl}buwrHWEjsiss{YA7|8T1suDj`F%vSE;AeR~je{l}1WqrHRs1X{Iz+ zS|}}*R!VE7jnY@nfN@t~u@`ln?>88A?yrpzkdMG`WUdr3bJ4$cmU8RrG zSLvs`r}S3_CPWfILr~IJ&sEk*BQYI)9l}XBEWs35%GF6$TOjl+oGnHA&Y-Nrz zSDB}TltkqhNJHf6iAL)oe9Qg$nQl)cJ6WxsMjIj9^`epe1FN0g(=G3B^&LOH3NQcf#pl(Wh? z<-Br1xu{%HE-RjLMY*b6Q?4sX?$!0LBvq0r$(0n!&B`rGN+p$YtCCttqoh^RDe09A zN=7A<5~pNVZc}bo?ohHQcPe)&cPsZOS(R)`b|r_BQ@K~UPq|-tKzUGkNO@R!M9HP( zR`Mu$m3&Hm8K?Z9{HTmqeo`hV6BUgB zI2otl&o~vQ;dGpVGjSHq#yL0_=V1sF@fZ9R=i>rgh>LJBF2SX^442~yT#2i2HLk(6 zxDMCjZ@2+B;wIdTTW~9G!|k{Ocj7MGjeBq}?!*0f01x6J{2dSD5j={=@Hn2plXwbG z;~6}Q=kPpUz>9bZFQbQ7@G4%z>zL#|^T^(WNii8F#}s%o-hwGH72b-eF%720beJA9 zU`EV@ahMrz!`tx=%z}5~U3fR%gIO^fX2%?u6Ys_Q@P2#%AH;|7VSEI0VQ$QWc`+a6 z$4Buod>o&^C-EtK8Vlev_$)q$&*KaDA|`qRztYb{Zx9ZaD^r_CVHK{%HMkbn;d=ZH zH{eFxgqv{-ZpCf59e3bP+=aVw5AMZ%xE~MTK|F-N<6%64NAVaQ#}jxGPvL1igJ88#yz+f_u+m#fCupq{*H(72p+{_cpOjQNj!z8@eH2D zb9f#v;6=QIm(jy3conbVb(!E59gswSO?CEwsoT#LTTIKE5C-9yZ>6%((faamN`m z_r-UNO||9(@6<@s-ivMMplyivk{>bNecH^;%bK%|HZ5hg8;_hY?JIg4>-RG)OZJ%h zrk=*^y7zc*=?>!^2aQd+_baSzC0q6GD$_n~jd90X@4`o>eG~obZZq{bwr~r3lwQXn-pjB1iuVd!GyO}@f35Z? z!RyaDx3Q1Q@t(`3=Ule!6x-H8{S&+%{Z0ES9YgzgC3x4^&S!Z<4sJE=nRXfb=&_6U zTAwrC!6W6d*Yh}@-?2w&S!(%F)2H)((%Jo71c z@m^n+y~bm@zn5wGY_G8a_dA~TH(`&KV9!Hl?m>=ZEA>zC)@q#z-m0Od{Tz?)%j-?u zeoo|3&W#0Z+cl11Elwnekc6Z6 zF=x?zth3lD)4$(Y<5JFwvmE(WdR!8`2epO-?>I-`KAn^Ld{CVyI;w2#mpF%QUaYG| z=L6T2JyuJ&-*X)8t{j>1+A95}qmDql*MY}qA4loZ2Ge36!I|4l{T?32N%U#LwtdX? zTbQkS@_ln(=Q%5ia)xIaVV3H@$~c~*G(|@>!JE#RwwtxR%YAKGZTcMNY$?dOI+7!7 zM}U0fI0Pt_oYXIJb>Bz*etd>==_)=%%ft8xp3u3Wvko5~Y3frxHP7GjX>OvIU+UcE3_r-( zQj)D*!V%ktWqIsA)n3PYH*YXD#(3>@y!S2E-)8Fe^JPEax@gSVH3&~*X|9MpIFvK0 zH|J6(j{jYp8~2`IJ)9f%)1B|-92-exUY9e{+5aanBb}EniEcC8-^7}B(_%{VH?fD^U*?9(MDG@j8RodBbY&`6-s;NK zu1w>~w609&%Ji!u$Go9#Vam(H5%DY^7w=3_V ze2(5`jrR6joNbAB?T@+gNmoAQ%I97Af-7HiWg%AE%^_Y*81cgsED z%HVc9?b-{tGSMr*em(1^!}}NB-f$V-?nLiJ`iHka(JRRGOK$msf2{Xq+7sM#xJ>k3 zrTt~s{)#KZ$05AG;o}kB-tc~fw97K=HIAX>E_?4O!Ovkoh5pcT{*>-Kf7|OE2p_~x+`b6a;7V1xpKBE=eTmNE9be= zJohu>c%mzRapkYBobSp7u3YHKMXp@z$|bH`>dIxVT<*#hu3YKLRjyp^$~CTBOZlZc zPB$tO-TZJFa?|1R7dIU)6XS}|vcR>6%Xw}(T>k2&!{u@}eWP-bn;$Mq$CaTaQra`8 zbX>Tc5sTTnXAUoLUmccZd&TrKq|9akSG zpjkudxW%#@bIUQeTwDWMOrLVxOF8;frcY)1RHjemIMb&xeJazZ3Vo{3UWN85wAW&( zTC~@vPkrhQs5fA416q30(vy~++)Gc|`_R&ddSCkVrKK-@`qDmtmI2fU;$V6XrapxF z5bDFI52ODu)-cSqn`1JJZ63wkQM8X@Uq;bBnn!*#YZy(R(X3%~+&C>en*O8dKbrpI z=sAv_$i&=It%PywRV)`tmPg8BZelLpl+%nVu>{dF?EKxeHBii$7BK7FA%k(KnpK{!9 zxj56ue|DKZmFW{cuS_5R*=71vp-=c6Gkv1Zv|;Q~_z^VQ;D6L^^ca~f_dgZtr=l6}T4%6TN=$NBd zjz=ndmPU?z72551)suQJ9@$>hqsIq30))>cvxe~5V(PKC0i4wXxK0OCA547+^&!-I zQtwH<7xiA$>r<~!J$lWxX!p06p9Rsk1~Gja#H^=JOrJh6efq}q=^N9h7h_cPz4v0g z^0$~XGx}zau)oEO64AGwT$Rx+ePUYr#EYh zoGtohpBF{9{A=4Bd+&BUk6yF+Dd@k&wuF-Ud+1e0Y>OAKcb%heiC#iX%gZq>uf((z zifMT@rloL9OOcqCqA@MSVp@vFw3LWxc`c@;WK2t`n3mFk7W0$c{A`Kb%rkBCoIP^; z$1TC7?DOd8rR>j1e~bT|GC#}wBdggOf1lVE^Bmqkx|kOKb4T+`-oF>-4EL|uv;>z5 z-n;p!8eA&4U#5@$XvDUd*Af2p{G*oOddyEE|C;|%ORd1sFh868NA(}K1lJtAck@ce ze|*fV9{*CYE#~U>@995o@vq19@js^KHI%=Pc|G-4TKwBQBxWho;=eD`;$ORn~=HFA(;$O>j^$y<}3`ZIk<(1-jKkEl;{l&w3z$ypU>u%rhm<*#lOww6>DsZ|2Ugx z?7=?&s3o{m@cc2)sQrD+^J@Q^V_VES9{!_hM&sBP|C-HvAi+Mt=gVeX4fY9cq1jJ= zAM=R%@5}V@x0vSu{uVQi`PXB{D1VC?N&GG5d=9n*x5vD1;O}EzF$G(KOPS;D?_;h? ze~URc{4KH9a_p6B#vlJuqhngk85O)Pf{&NkcmKIz*5hx9?Gw9Zb5H*5F;}v`#at); z7W0mizs0=&=5LAJp4idYyiWITvpF~X>oMExZ%OnP2d?f!Z%Itc(wLTIF)hnuT2{og ztc+<{71Odhre#e`%i5ThbulgLV_JTTY1t6dvN5J*Q%uX|n3gRuEn8z+w#Bq;k7?Nv z)3P(BWming?wFQ6F)e#zTK2`X?2lbW8N^ zr_TfUAU=c-<0F_0b7LONi}^6WteLc?Ub8;T>(+uc1lC8=9WQ;)R_?bdSCW35EHwKDZstI!^+eNG*#eMS?jeO4Z;9mlNp_dC(Q z<+1ORn%~^$|H5I<8`z8XSo_c(YhT)99YA}m18I+SFzvApp*_}Nw8uJ%_E<;L9;+SO zW1T?%Sk3Qv%asMDHqB z*0n#b8vWK^f8%Md{kOOh?R6b}h1zSp6LX_ShUlx_jx+Y!x8uxm`rB`>%Ek3=KNHk` zf@xk_{`~v7?dW7jDm$Lok;INFcGR+?hy8qB`}v9Xb1>~EmG|eOqPqww6=V<@d zErb59P5*-y?dLw)&sMabFWHNB`?;3((>Cp=I@-@FwVzXIKPB0IlB4}RQ2V))_7gMh zC+FI6+m3s7^iI!_vEy?Z>i?6s-~L>Clrh}?bbEsO$87m1Y>3f6#q3X*1(!r3u|K?d>!key+`|vC-!sM8`9ng z8)Fk}ip{V&+V3T4Nxc=e#x~d%+hKc*-QUi%cfmKXD|W*-u_v0N8NS)?*|p!|Rxhh* zO-dj8J#NXVr@#ixZHSF9CGDy3R!og)FfIP)zMnYJo2QW{(F{0S%GCpVk9i!jkU!M&WsQ+OKB;8{F}=kWqw zl(Dx3X-(Tg)awFzUYE&}CDXrT$&whtx1^?`?lMJ+;P(D^ZQ@&AtP$nr;mD$QK%3`~o@Bvu8Qf=#%Ey^zCuyR(pq9l#WusV&BLAhPI zN6DewujGqkd)THYHT|3tuM}2ZQ))!!hwtk@QLb+Jzvq7b6WjhjZJn#z>v8L>bW?gL z@BH`J{<|Ne)$i%J4pBZ*Mk`+_Q$<^vcSd_p_1tuA zR~~o$uDddcYp?9eGW@*MXVbX(_qnpCD{HuZDKgph-|Wg;T$$39sa$!hD^t5NjVsf- zGMy`nxb?r{$_j2fD!Q_gE8DsGmE3lucgsKE$`@T(#+A)n`K2o-xN@BlPh9zwS+ace-+~E33NWSJoZR+ud>%+;sRny6W01yZ-mO>58sjCO2Jz=i7QG!}SYZ|MoSv z{!;cfdpygz<==P9+gIcI>@Iixg|Dk?ZvM@gY9=#dh$+pwQ@ZK)Zhm-s|9WXhg7Ekd z9v5y@hR1~)mErLvI4*?u<3?q9cf7*$!{ft^%JBHG%Z(2iGTRa6ukW`#{^>LQ&)ENR ze{=uC$HVUbpKBlMzpi~uW3@-_U(^1)`Rm%BOR?JX@PE~lTwO;qnV)V}`*Y4}f7V$y z@bk<5)ce=8SHhp0`ZhEFJ?_hDuY-T4Irf9%jP~d3pKGs>zpnjR9qTh(b5?tO{(bGw zf2%#0qqWZh{#?6Xd3j`?!e^tr#x`3&<~SE-uC)sF8u)sYKV~*t)RC61*aN?hGLiZ+ z+=@w9|7Gg;Q@7gxduo5n(t_>j%-nAHHAX+;t5}28{;w_T4tkzv4c460AHr(2gqFhh?1YVBv3C7+7v^>DiwMQ^7K89s7nMW_$N8`-Z7Htb! zqKzKMqObAjqZGYg|EqQT$VT6bzizH9`x9a|kA*dQ)QTP}qPHr#ZjYqZjxf>3!PcYK zY0n~iJfp{r8*N+kefj&CpK-Ru?1?^oGREkm8eNZGXLS8WBg~DCeDrAZ1ZRkK8+B{+ zsBW)?25jee&U5Q@`dg#-F?x@pxAU*mqeqyM+*fZb&V8k#UM7=y1+_TNyspX2R=pgh zy`o>B|J^trZ|0o0+7YQb?ftMdYp|x{UaV`WTlZ17e#KJOEG&Bu=0rQz*)gp+EzzS{ zZ(8iw)sMOz!3I(vhQsl8?$M5BQ)rov%Ww?7&X(+?egN$#XGgrVw8XQXLTG*mX#PKv z(T;d+Swllw?C58Ir`v`WI|hD0{d=_k|Mhq3$MFna#4DJDb*4Z&Hm0I}2imdmE_&vo zB|nx%J2u)!Cwf%0kJK>wRA<@GsL#d_+ADe!^`t!F889b4h0o$69GOwL05@VL)|?0J zCoQa`ekV(1#qd~R?yXEFbI*3XeIm2jvnMi}$NbAo=2^p|apr1!G|r5)FS2X`Hqf*_ zffntxW*9$|ZI?a>=Ze|wGm&a0Foc$h6u&6ka5_>X{n&JcbEWM1{z`GXj5^|{}Sb{aQbdl^^S@lv1j?fTi# z(u})y`SAOf;rA^|bFJ&IAYIuiP}-4H@8vT(>9fe){BRlaI=^k!9P+wQmUHtPxU#-0 z8@e+5zHrFv?w0G}%AT(5<;u4y!|y+Z-{*SOttb5cQ*k$4#+Bjw4|)ArzM5+fc>|el z?50};`iIwB*tHjNWieNV@28ZT4&Pr{Hyys;3U0chE5q-bRdLf*T^Zi4>TbG@D?{EF zY-c?;-N=>U?QY_xo4T@@D}(zJ?jPQ-@b-tdJG{NYe*dTUKfK@J^6&0{_&A66GklyX z-1h2i<`{(cH~ct;`vuGJaSuOU;pJ<);~y?dy6JFP!cB+ET5dXAwsh0sa)_G_mmj+6 zaQTs&4woa`bhsSjro-hIZaQ3kDe(ph<5`9k?gnXV)%>y=Z=IpuDx`vIkh@|sd!siM?TS|}Zq zuFC)E`~SPg?^f-DNn@8DDox&T8KlyrpUW_nCc|AusWchm@})|Xu`b7SEKTmvv`L&x zI+Z5lG|%LtuVqXot8QW*bCcCBn^c;l(ma!ss+$y1-K4Thd6g!mU3#cA z>E|+5rO6MvFOxZ{n=EqKr_$t*%T4NIlEWpRN|UEuQm8a}-ld>QlZlEsS4~Q|)KzKn zj!QR{CXcJHNjud|%(4@-ZzfY*LMlxzx=dGTGTSAEmNUuhl1-(Fxu#6=x_qS4Rz55zK|TF%Ra&e3%~}l}}|cTlF*+z-RDTd=8(-7w|=V2@7I8Cg98X3Kqgw zu`m|FqF4-zV+ni>OJXT3jb*Sbmc#N`0V`r9tc+E#DptdgSDF2-BCFH#eloMy!>~24 ze4oOO_yO%9ug?D?$LHBB=J>S!FR=?EmWsc{$Yb1xNBr{0hh7xA+78h(F;>oQ1RT z7hH@>a2>A4-M9x&;c2{rSMjDh&6X#{mr5{~twa>bGGQ%#DxXllUUO zf~Bzx29MYOuIZ3hTSqA`M4Yx;3E7D zH{eFxgqv{-ZpCf59rxls+>ZzFARfZs@h~32Gk6xy;WfOD$v8eYV;W40=`cNJz>Ii1 z-hp@GU3fR%gV``U=D?hIFW!gu;{%un^I|^CkI&*$_%s&4XYeIl$X>imy_XIa#$W;!wOgtYee}rV~D-?|J)U0uh!V>w9KE5jXO9pNAd4mU*}mv z688EQv{#KCRT60Vnd9&d_Qv8^0$;k~evwYe8NRfqcP zQP!hg7wcmKY>17qNt8{gH^b)G5?f(wY!hW$>g}TJK)oY&!p_(Qqqop*TX;YI=awO_ zgpPK|drdZCFIr%GoXj~e1%Jk=I1Q)c3|xULaVPG=V|W2mvd&a^D`vttd=MYOT=+OX zfzRRd_yRtP$650UyojrCHD1D#xCXz+aaff#RKx065=&uCtc7doe;GZ@O8Xo5CccH; zu_yMzxA7h9gZ=P5?2iL+D87#$;DaR!=iPYZdoa5m1t zxj0Y$o$>7lw(3V5k3ZoAoQRWfGETvtaVk#3={N&t;w+qvb8s%s!w@FoFZe6Y#|5|$ z7vW-Dg3LD+mzUwOF z<(FkRhW7uNm!n=DD_}*egq5)hR>f*q9c!TdALg~FhrEK^OFSmv%lHZw!dI~{7Qv!e z42xq4G~aI(@=9VUERAKbESAIaSOF_yC9I59uqsx=>R1D7VlAwVb?|kpi}kQRHo%71 z2peM)Y>LgWIkv!-*a}-?8*Gd1uswFbj@Su1V;6h_yJ9zd6W_w_*aLfFFMJ!{!QS{T z_QAf`58uQ7H~+v_-fE#fWZpJOR6}RDb+<`lB7w*PAxEJ@~ zemsB&@euxwhw%s=#bbCJPvA*Bg{Schp2c%`9xvcUyo8t0!z*|duipj~ z2mBGo<4-sNC*oxM8K>ejoQ^YaCeFgyI0xtAJWRx2@K>CV3ve+m!KJtim*YxYg{yH5 zuElk@KFZ&yZ@`VX8Mok8+#cl)>N}&{MSVB!!M(T-L*8@xcDa!EytKFdv>(PJcodJ} zaXf)1@eH2B^LP<2;brvj3SPwv81hQ%e!rz2zW-o(*7dB!c2<_LkBB|;(MQ-GF?)O_ zv4$yl3Qyw){EYh$|ITx~&v`|;n)evLW6dG29b4NTJ77obgq`I=dTz7*aS0cE9{P=l?^j?GfD1Rd4_^U`EV@ahMrz!}7c?I7mI@U158!;x)XEN%Y;Lu{XW{ zul85C-$Gavi(xz_V8~0Yo*^#{rp0u!ICEcheKOLXNxn==VJwBEu?&{Qa#$WKU`4Ei zm9Yv|#cEg`YhX>Rg|)E`zK$X95p79b>h-WbHo%712%BKYdrbYCQn!Y@C)Lu7mJ+T{ z0kzorYp&%vwP^g8_7?J9RJ{drTVgA0jcqXGy~0w3@Kr1<+cLME>r;&O;<7z09k8RD zTZ*}*v5YL~+RM>W9xGr)tb~=Zij00l?2&Ko)=*P(?Ge-a|MaheuVY=ThxM_6{5$7= z_{f>E2-{E)U&6+2t2%ODov<@@!8fogcEdOEE$ogxWb94jzuG&q{x0|icExV^CccF| zuos5BL0a~0>hEB0*Z#iR-=*FM`(i(Q5BuW)9EgK(Fb=_?_&yH95AZ|$2#4bc9El&} zDAf3`E%}7{r#KqN;Ai+bet{bQ)&EQC-?^44YWa%#*Z2*N#cwg>%~T(~|0C_~d)mk0 z5BMXF$DeQlPQ)n~@)l|hKU1HI(_H&fwNIx$182JSmE7+t*~zu6p~ZZQddORc>+v@< z-(nx~HsU7Sj9YLkZo}=k19!^qtiLBt!pZI)_t9s+{5$7=$UCVu9CBm$Y1VlL&*C{e zj~CE z3tf9g+B3;Tv@FIYxD=P+a$JEcaTTt{HMkbn;d=ZHH{eFxgqv{-ZpCe=@n2iAo%#;k ziMwz&?!kSi@n8M-Q$OHZo>a?0>c8V*Jc372&;Qj&E-%q?89g_*6mv^s87zzCusl}4it-9eU3JTv?^_RfRk0dYm(fQ! z$BY0d^fZnCYPrt+CZXjfOp3`cIqL6!Si=AuC{xgWGv4C*3{`td>Z$NnOylN$ z#InP2giK3&IyZNeTC%zNr>du?p24+z#!{c-7cwL5ncUp3Y5xYt$~fAyVrJK89CLrb zALVVd-;Q_SJ(vaW#=G!N%#JxQC*F%1|5@iOoQ-qjeayWdAHWCkAq;uH(sMp8kPp-T z2eTiAKU;DIui`blj!E==_pvvP z|7w4fWAYe2j!)o|_!K^k&tS+)tEHZ!{ye_m+B2&CMd~kML5#-)d>LQCLij2c!J=3k zOWL`u@N@LCfF34VRO{@ucca1Z;fqSdoi`QrQQzPV+S|46!%yf%V1e7 zhvl(?>`4DkZmG&@DNMbXYpF)h>R1D7VlAvK3()gfY=JG^vi0fHK>nTcKjgJ$J#DZp zw!`+=0Xt$Rd87M^{R^cU?&|2lvOQ(=P2;~>ISj>T{BJJk5EK6?Hy?d=EJf5h?l6HdU1I0+}C z-v80EQ>jnG>8^c=+GkLoiL-FFo4bmRcn;6w1-yut@G^RM z1+U^YypBl*n#brS8GY0Eua88#yzOU9>$Y+ z3XkAXJch^d1Zw=(y=eT$7vve*&*C{ej~DPFUc$?$@n1__rG5>syY^ygPr@f1--Pzl zlkMjwhrCkUV`(gdWw9KV#|l^xE6HT6C*)P3r7Bj#>R1D7VlAvK7qUHz(Zefl3+vOT zf&4q?e|Db9=JeU{rq5l3yTa};sqxcv;j!)o|_!K^k1u*2z*0RsI z`ds#Lo_v;;=kR%a0bj(IupoxKMJ%-#m&mfNeHkswaRsiF@yt!Ym+=)Wgs)=A`%OIy zyZT1ei%>6tuVG0ng{83!7Q>LYi+kLSdvGuA!~J*w58@&G9S`FXJc`HgIG(_hcuE%K z9*g7G?k_FRG50)Pz>9bZFQbQ7@G4%z>zHIvk|eFOn{&Snw#9bX9y?%1?1Y`M3%-F} zu^YaLZ((=rfjzMozK!o-Z+sW~U|;Nq?_qx&fCF(54#puk6yL{T_yK;1AK`Eufg|x_ z9EG3Yr#KqN;Ai+bet}=&SNJu4gJbbq{0=q#>j;Ou7w|=VNgiSB`kp@H@CW=6$Ky}9 zhMv)*(_g7KVhsE{WA8VdV`JT!R)a0CiM6mc*1^}YF4n{P*Z>=1BW#RKuqigf=GX#T zVk>NoZLlr2!}iz#J7Op7j9u^z?26s+O?(TxV-Goj<2(^V-rH)KWf_cTh;aaU;a$XV%PqIT9!~>ipy|0uE3SJ3RmMAT#M^)J^qGz z{;zdzpuQ0|x%Rni!#uf}mMypyx8Ziw^MCbO>wd~FQvI*o^#8wD|0?-+&i|0Nh4pNe zi?f@@WN~(Lr1kq>+IQkE+>Lv1FYd$rcmNOLA^aT=;}JZH$M86wz>|0iPvaRpi|6n> zUcifZ2`{6ESMVxc!|Sq9jyH1X|8_A}G{L6$Jy-oW`~iPN`wbBGyC3ZLKuo6HI)(br zI2EVibew_BSik+|mFCo2U`vdCd|T1d8rxu7Y=`af?>xS-?+N^sS7i1cJ8Di(2?xy7iLz>-+nY37!tULGrAWvq(Tu@=@r^C;@` zAkZwYkp%DM@wID-(cZnmnco$=;hXptcE=vr6U}<{t{U~X@g3}q?_wVec`LO3 zzSR4n{eGHBJdU$)HqOEK=+hqu;6My{o3$Q||F{*m;db1CJ8>88#yz+f_u+m#fCupq z{*H(72p+{_cpOjQNj!z8@eH2Db9f#v;6=QIm(jy3conbVb-BWQ2gE8@u5smhSN`V8 zjjr6{%5ARP<;s1oJm|_pt~~6@qpm#e%9E}v8Uyx)}%xbi_)KIF=WUHOPB^SLs=D<5^`W3GHWP(JOZ zpK;}Lu6)szFT1j^E1z{`QCAjnrQPdTOS<``T$$39X4%7U(pcV&VrH@Nd-w=1)|_8hMCT>H&#I-M)`y7ppjy0k0H zxbikv-sj5Vt}NlovaT%W%JQzP;L3`wJmu~ur<=all?7b+q${6y6TmV%C)ZC zJ(C30J=2%2!?anky^0vWhFKxw5(|E4#9) zE5m2(O>Vk|EC1c+Uro2(TCRM`l|@{+(4BXST)CgmmC;{vxbk;b9&zO{SDtX?DOa9z za;4|WtFFB6%4AvW5lG?6TU?pSm8o5s)|DAu8RyE|TzQ8p?{wweuFUGn?5@n| z%KKdTfGZzzWo}pIb>$hioj1AZ^saowmHAz%zr`A2_C)W-xpJ{9m$-7NE0?))xhq$= za-}O*QSNj1d%%^4TzS})M_qZ`l_yet7NolyHWmgV2BNerA}An8mx+|1(xup> zbTfp4q;$7*cMS0!>$lFiCb%D8pF5uS{o@%v$M-z1Yu2opSu?Zt#AfzARajPY*maWu7u$S)eRb7AXN`v9d&2sw`7}Ren>JD=U=W zm6ggWWwo+KS*xs5{!rE{8>DyfvzN*X1tQdB9X6jw?pC6!W2X{C%(Rw<{HS1KqK zl}buwrHWEjsiss{YAEqaP2~aQLFFOkVdW8}mh!0bnDV&tgi>31QmLanrPNiPR-RFw zRi0DoDbFkQl^2v3l?F;frIFHDX`(b$nkg?SFDtJouPU!8uPbjTZz|1|x0Dvj+e%BN zmGX}AuF_g*qqJ4tQ~E0Xl>W*9<#T1AGD!JC`BM2x8LSLZhAP99;mQbQq%ukwt&CB| zDqkz#DC3lGmG6}Al^>KJm7kQKmGR01Wuh`knXF7veo>|>)0FAT3}vP=OPQ_AQRXW1 zl=;d6WudZ22`Gz|CCXA|newaho3dP4q5Q6_R8}dgl{Ly*Wu5YevR>JsY*aQWo0Tof zR%M&AUD=`RRCX!5l|9N{WuLNNIiMU=5@q%F6NxbiCdFi!98+LQOogd24W`9(csHiU zILwHdFf(Ssd+=V&irFwb=D?ho3v=UrmAuNnVuqYP8;#dMpVks<*Ww0!k!}3@G zD`I7=f>p5^R>v9`k2Uc@dK=83adTi1^ zkM;2dJGPFF)&KvJFEY~r8)74Dj7_j9>igfcd^77`RR0qF%lHbuYR5KV{Y~X-%)E|o z;G5VS-@+F7HnzdG_#U>y_V_+_z>e4nJ7X8@irug~_Q0O_0e*-d;m7z1eu};EGwh4~ zus;sK&v76Q!Y}Yk3D3cJGMXDXMh~W%y1lmBXJat#xXb+zrk_%Eq;gJ z;}7^F{)FRk0#3w9I2otlFE|ya;|!dMvv4-f!MQjO=c9iAU)N;;{X$&i&)cyxxxTaH zVrG`$Qe1|=;%~Sd1HnaFeuedm)&EYv5?A4BJN7r$zg(_iW-YG6KX5&6z>T;GH{%xE zira8I?!cX>-~ZS8chT?0J-8S5;eI@T2k{Ud#v^zXkKu7VfhX}4p2jnH7SG{%ynq++ z5?;nDconbVbqwMSyoI+hK{jvSeitUhM3@+pU@}aODKI6b!qk`s)8gHj9^)_rX2eXG z8MEL$7zn2SLf_XH z(pUz|VmU026|f>!!YWu5t6_Dlf$>-qAHaw3VSEH@;iLE%K8{adZF~~z;8R!^pT=kK zS$q!b;qzD@U%(f!0XD>D_!7R1ui&fr8orKiU`uR;?_eOKg{NJMpOJ4 zo8e3NGQNTxaSr}-pSF!wSONpVu3Y|Z*j?VwBPAc^#{$?A zTj4wSE;eGDG{z>_6yL<=_!hRn?7W|x19M_7%#HV99?XmPV?GQ7hjTec;7A;Wqj8K} zd%t%(tiwNWJ#N5_xCuAo7Tk*4a69h6owy5k;~w0L`*1%Vz=QZF9>T+T1drk|JdP*u zB%Z?4cm~hnIXsUS@FHHq%XkH^;x)XELA-%C@fP031o^yebr&YYM3@+pU{Xwm$uR|{ z#8j9X(_mUmhj(LojKd6=5i?ia*rCtSuWconbV zbqwMSyotB)w#=u;V_vQ6ekGrhUn!szR0=7Dl_E+}rI=D&DWQ~9N-3q4GD=ycoKjw? zpj1>UDV3EfN>!zrQeCN`#49zG2b2euhm?nvN0eI1qsn8-85m7dMG`WUdji`hssCF$I2(l zr%G?-Go_ExSLvtpR|Y7bD+855$`{I)%2&!@Wr#9V8Kw+ZMkphdQOam#j51dFTKPs9 zr+lk?r+lycp!}%(r2MRmS0*SEl}XBEWs35PGF6$TOjl+oGnHA&Y-NrzSDB~GR~9G> zl|@QGS*$EkmMY7XUzOjK<;n`>cV(rrN?EO}QPwK!ls}a9$_8blvPs#jY*Dr<+m!9f z4rQmZOWCdLQT8hPl>N#9<)HGXa!5I>98r!c$CTsB3FV}6N;$2ZQO+vol=I33<)U&) zxvX4Kt}54*>q=0$q1;q%DYumb`L!of5-N$5#7Yt+sgg`duB1>>DyfvzN*X1tl1{l> zNw35y8I+7lCMC0yMY%`0SIMemQ?e^Ll$=T~CAV^)l1Is_+^^(Q@+$?Df=VH!uu?=R zsuWX-DrU9lz644@__Q7@{sbd@`zGP zc~p5!d0crysjWPz)KQ*N>MBnw&nV9-&nfkk=au@(3(AX11ErzTNNKDzQJN~vl$Vs3 zl~t`B?cx`Bdqxe5UkK`YQdD{>lL5b7i11NclqfQu#_5tPD|x zD#MiF$_QnoGD`VF8Lf;}zE-|b#wp(_-zncKKPW#cKPf*eIat$}DBJvPcOiiJsY*aQWo0Tof zR%M&AUD=`RRCX!5l|9N{WuLNNIiMU={!|Vrhm|ABQRSF&TsfhfR8A?Ul{3m&<(zU} zxu9HBE-9ClE6P>nnsQwUDmRpy$}Q!#lAwSd-%3Iyk&;+Rq9j$4Dan--N=hY_l3Gcl zq*c->cPr_YI3d`f<$fKpH? zq!d>42*;vW42xq4EQzJCG?u}#SPsi$1+0jburgM`s#p!HV-1YQn)m=dh!5ez_z2d* zNAWRy9G}42_$1cBr?4(QjnCk-_#D>5=dnJ%fG=VLY>17pF*d=b*bHC7m+=*R6<@>G z@eO5t%1Jch^d1fIlGcpA^(Sv-g5@d94NOL!Tt;8nba z*D;7U@Fw2E+n9oFk`e>KQG>N#4UWb!GC@IaEBW3St9zcd5KMw*eCNd2{CVm|X7 zW?{zn9(vz<>9b-^%!7F`7v6{WW4=4|w~v4M-{KzF0^i281-9bZFXI)wir4Tu2Jr^o#9Me<=HT~1a$+vbjrU<5%!~J9KFp5=upkyf{r|rq z`k6vs1dC!ZERH3xBnE<+w0t0#8MEL$crRwfY?vK$U{1^>OR=8PSO&{tIV_JAup(B% z%2)-fVl}LeH837);sf{~K7B`u$(6^9}kpvANB+*8E%aEo|mJE<-zPkMCm#?1-JP zGj_qQ*bTeOSMqqf&sXs^d>!AwH?cXsg)Q)HY>BP#9efvCV;gLX?_oP^kMCm#?1-JP zGj_qQ*bTd55A2D(@B{o1Kf+J&Q|yhOVIS;^{jfg{z|V0Y4#F?+OZ*B4;}9H*!*Do` zz>zo#N8=bAi(lh6I1azX@9=y40e{4w(7T2H9fzMI*7kKmZ;jvJ8@P&}u&Z$muEmqQ zbNq3{Y4!g*6z5A78`<*braF*YHhjiLLQHY>yqVGxorq_#u9ZeXuX~!~R$t zOJg~#g4Hk{AHZ5z8|&cH_$vf~ddBKmtLLnqw|c?qMXQ&rUbcG0>Q$@PtX{Vo zw0gtpO{=%8-nN>6XMK84&1yociL55Jn#5{StI4bef;omI&>;}aX*Cx$xgDRvYD%lA ztfsb_#%fxt>8##uHNDk1s~M~Yf<^88idijgwFEVzEtkn^W~*7O-edJ%t68mPvzpzi zcisNcLI3aE&aax)>Q-w|7u(P9K(MCGKVbDiY8!r*>wRpi|L)({AD_p+)Dx^uv^vS^ zWUEuG{$h2i)oE6zTb*Hbrqx+iXIq_Pb*|NUR_9w?V0EF@MOFh=7h7Fob*a^5R)4kn zo7Lr3S6J2mZM5zC4)u4NUukuf)zwzlSPcZ*+3DI_ecx&as~xR&vf9~d7pq;ZcC*@@ zy4Kde&gvgl*IV6Sb)(fyRySMSVs)$4ZC1Bi-C=d7)m>J1Tis)Iuho54_gg(+^`O;1 ztsb&^*y<6hN39;Sdfe&>t0%3VvU=L;8LMZlp0j%1>IJJ8tzNQv+3FRmSFK*N8VF9X z?LE=zB&(CD*KN6=)f-lCTD@iUw$%iA{m08)RufuHWHqtXBvzAJO=dN@)f84!T1{m& zwbe9M(^^eu^=_-_t;Si+U^Nh2Y1eO+)zwzlSY2y%9rfS6Kk;9QdHeaDc0RkT?xyPg zz$Nvncb)qu_P6H%tDjpPXmya)FRXrP^((7`tq!p|)ao#+!>x|6I@0PWtD~)su{zf3 z*H*u=I?n31R=>0Qz11JA{%G|lt3O*EZ*_vziB{jXpC2u)wzB$;)pxD7w%W#OTdVI` zZD+N;)%UG-u-egTC##*UcCp&kYB#Iht@g0m(`qlPA6Wg+>PJ>Tw)%uY8I>aSiRS3R;$^pX1AKdYEG-Utmd|QpVd58^IE;%YCfy^ ztroCa&}t#8g{>B`TGVPWtHrIBuv*e;DXXQema$saYB{UrtyZvF(P|~Dm918>TGeVb ztJSU6uo`dmF{_VTeZp#Ot4~_3W3{H$%6XgR_0ID_8$V?AVXL9rBUD4zJ5(RBH8$V_BX{*n~)Mst{oYi_(pSN1y>I+sMuv*t@Q>!mpZD6&L)y7tvSZ!$4 zzqkJD>Ng+lanOw#TI0Wa{~HK?X6Mt#YG14UtoFA$fco!0|KYa&5mrZ99c6X2)iKm{ ziTo|`ht>5~H(1?hb(7W2p?TgF)bCE%e&k!L-&y_M>K0pmtJOeolAZr#t5dB0Vs)z3 zX;!COondvR)mc_&Tb*NduGM)~=UZK1b)nToRs&WSTU}yxsnumxf3^CX)#X-KSpD7V zN~^1^uC}_y>RPMoto~tjz10m?H(K3fb+gqiR<~N+W_7#O9aeW*-DP#R)jd}CTHR-L zztsa)4_f`x>LII#tsb#@)ao&-$E}{QdeZ7CtEa7=v3l0(IjiTbUa)%6>Lsg}tzNNu z)#^2?*R2Mv-mrSp>Mg6csiFPAKd#^B_2z%TAMq#r8OP%UoQRWfGETu?a4Js2={O5# z;~boe^Kd>cz=gO71GpHM;8I+Mzv6OSfxqKQT!m|JEv~~qa6N9ojkpQ7;8xs*JJH`> zyXbf09^8xja6cZvgZL*N!ozq3kK!>rjwf&i&cxqvHEzc3xC8a~e@A%Fa-PIG_>}bb zum9ZV6Z6@JEx^z4P5c3W#QZpkXOok03jTsqaT-p?88{PX;Xl6T{oiJ1bJ{sL7w6%8 zT!0I45e9sNUD-0-u)DnTkX_&KrO(ANC+5Q3cpv7$yqFL3V*xCPg|ILd!J=3Ui(?5a ziKVbKmcg=E9xGyHtb$dsI+nuybh6p!I?Jb@?i6rRR2coxs$dAxuZ@e*FfD|i*J z;dKn+4ZMlB@V4xl+q<`S!|vDvdtxvA06)Z!@MHW8`(R(}hy8H?evSii5PpGQ;#W8r zhu}~ghQo0rj>a)K7Qe=Ca2$S%-{A-xg+Jhr_!It&<8cB`#7Q_Ar{FI*6{q2JoPjg( zdrZI~5Ue=T+YeX5%5o@s$YHoJk+;wy4B)@^_-(8PjY+f{}g-Uzq0LoP?8c3jTsqaT-p?88{PX;cT3Pb8#Nd#|5|$7hwPw;}Tqo%kWqH4VU8z{2f=~ zDqM|ga2mJ8bPNQ0u=RRkFZ=*M#E1-C*mZWj8pIzoQl(MI?lkE zI16Xv9Gr{ua6T@;g}4XD`XJr2Ur_yvx^VK@}0;x{-BXW>CSg%vV-=TaYEz!$MKcE^GE3$De?nY>eF z!AJ3VY=kf2+kT(fTQVKjC|z6&)qM1&ur7MD`oA4G1vlbJOv2^Ni6yZqw#0VWA1C27 z+<|4dw9jHc^e@RhtT{XSw~K$NA}>#8j_rZ|?c!fH|JLy@kAF!%i8Tj*~M|9bh?$Ukl5shZt!ss^mT4AGUsg zs_XO#n7>HxZ-+Q$9z*}ym8E|j+i=7c z#2fO?LuhMw$M}1gzt-PMcW$f5?e=$ie~

    EGvl{oBab-zWcb`unas^>6pT*5Bv< zef@p%U+dqm{=WXc{qO7V^Y7GuO#OW$@8|#B-v5d7_w|3K{B!z0iSD!?kJEq7A)NM3 zM>6m0KlcAx{~qga2mg}zr%e}^M*B#AtL9>zx$*9d-rQU4mm(j(|3CEq+#~w$pX$y> zs6X@f@A>~TOa62B(!V}c4pThq+A7}l4t^WVd81E-z z7Wvmp`p4>b_*g!dtjqfUb3ViR|8q8A{eNvB_z2gxmi+4_{bTj_zgYe$`6lcC`|9t1 zxl;v#4I=tR5q%Taw`BeQIV10xk@t-C@L|9YDQf;0YG+wflAbMtR6zt6=?X1sIz=_{TfXMj{D$k=M0BCPO(O4~cMfgcv27EDdav{Q_U_mV?($u9-TUs?Rta0# zYp>;%RTG5cU{-obuYN? zVRuP8ySW!#*W7i*-Etk>+(tLI$#wqL^|tS3H@C%gTV1!!b=zIH!*x4d7ruS}{(T~} zWFYw3SicVhUw7Rbu6xsU&8IlYb9LS0ypikbx$d^xY6;wJ-<+RUgy8|^g92&V6O`VzZ&aRuNyp;M%@tC4Rzfx*9~{w2-l5t z-6+?McHJ23PTB9l>6>$0cgA&RU3boP=UsQfbr)T?BZL2XSKqed<}SPLitDbr?waea zyDsRu8?L+Qx?8Th?Yac+mcGk%30;@Sb%|XU2+na^W3KDwxo*De7PxMq>lV2#;JU@G zTjIKz2E2h3kHI-AdQ3a@}gzt#RF2*R8WI!T%oqOEaqC^54z%I{&ffj}z$dMB-4t zII%bO4%M`rzX$R<{~e1S?oqMCE%!uX{~cO=hmSkff2Ye^%irz7KQ+Rg|4ys7mTX5x_ddO~tl%AcuM2;o@Q=FR+-aA)?(U5K`?&fRyo@d4{L53u z&8=|D`R@RF=kdFn^Piu4b1U6kc)3+>&VTpWTQ2<5z<-zCn+x4S`u;R`itx{ha2L8Y z^c`;Q_6S|}$e$^V?(x>@9U8l?iR+rWE_{2eapyZOE?u1eGpQl(3wUesKP`;z!pA=4 zeon7-Ygy;IKU}xob^fOd-ul8__*#Z9mH(-%x7-GI+Gg$$#voFvf0fgjcc4J zHQUSU{O25Amo(0M=HPYybA)8B^XB~LFy5TsC3js4*LlmOV$CUC=gs-gaJ*Caop-F? zrFNaSoOjJ4pH-cV3te}w^ZS#0vgIx3zw_;Pq2)Sf3@zt(-uy|s{UV=R4AFAlDZRPS zDZ-Du@cl1*dxY;-p;LNm4)tE=_y6xNUrF~2pp@%MyRMF1Z+-8i>-^iy+Ztuua%Ejt z&UHz28~OJQ{hx@hz2&`UWPM!M*LD3|*WYymtlQ%5iQ8Pa)pgrlx5ITiUAN11yIr@( zb$eYGK9BI4_qk*DyY7JN4!Z77*Bx@*Vb>jT-BH&abKP;*op9Yr*PU|RY1f@`-C5V2 zb6t2FoOg2)pgfgcinYC*WGa4P1oIW-EG$;aC^ABT$j*wiCmZ1 zbxB;8)OE>Rm)vzJT$j>ysa%)Zb!l9e)^+Jzcem>@x-QOj;bYUgx$v+-lRuj}r2T|U?4cU=M36?9!8*A;eM z5!V%UT`|`ccU=kBm2_Px*Ohi%8P}C{T{+j4cU=Y7Rdih?*Hw0171vdDT{YKLcU=wF z#ama?>H}6EjHwUV_+hJ$SgjRPAGPsgG4*j9*S7kk)jC!~&jUiQ;2Ya~6RSOiZ5tbSqjORHa59c*=o)uC30SsiY5gw>H& zM_C7d&+0cZb)1dAwfdda@2&n|^+&5eS^e4Sc&ihvhOYl}Hh$h}?D{9# z{3NTf;)ao*;zgqpx>T;_qtp09wrPWndS6f|Ub*^{mx%R?l0#VD+NaOI9yiy<+vM)oWI-TMb&hVfCifTUKvdO_0=o z#(bC6gjN$-O>8xZ)udLFSxs&=h1HZ+Q&~-IHTLlldp}HL%cZrN&g$J((_4+R`hUCq zGT3q%t!A>C*=iQ6_gKBxYF4Y+tY){G!)i{exvb{4dY{!iR`XiD-)cT;ek_0mu@Dx< zB3KlQVR0;hC9xEi#xhtI%VBw}fEBS4R>mq=6{}%&tby@Z6Cc0_@gaN|AHiDqC_aXd z;}cjLpTugsdajOjn0X58;?wvHK8w#`J$xSP;|usAHo%712peM)Y>LhBC43oQ!B_D$ zd>!AwH?cXsg)Q)HY>BP#9efvCV;gLX?_oP^kMCm#?1-JPGj_qQ*bTd55A2D(@B{o1 zKf;gk6Z{l=<7e0h`(i)rj|1>?9EgMP3;Ytl!ofHMhvG0Cjw5g+j>6G62FK#p_zjN3 zZ}B_)9)G|e@hAKl$KwQ?h?8(KPQhPrDo(@cI0I+mES!yVa4ycn`M3ZV;vx*-VqAht zaT)%Kzu|IRfxqKQT!pJ~4X(v?_y?}X4Y(0E;bz=|TX7q1#~rv6cj0c_gL`ow?#Bao z5dXwOco>i1Q9Opn@dTd4Q+OKB;8{F}=kWqw#7lS?ui#a@hSxEOH}EFj!rPc2nfFua zE=-7tFfk^cHZFnQ4LXh|@FbqX(|88Y;yFBz7w{rp!pnFCui`bl zjzPSEH}MwUmZ`Zs-nB~Oy0orK=eoOHm)>=8uFK%MjIPV%y3DT2;<|fWcdzTRx-OgR zvb!#a>vFm-m+Nx7?mpM$aa~^5-S4`5uFLPb0Bwy?Ycm)z_;FQ;N2$*x~`Dx3cId|>x#OrnCptWu7vAKx~`P#O1rL%>&m*Woa@TF zu7c|-x~`JzD!Z#Dl0n(L~&u7-7G+%_odx^k{7@45=EtLVB)uB+_2Dz2;Qx@xWq z1fOu1ptkFtbX^_SJ>|N(u6x>b&$#Yc*FERDdairkb@g5Mg6m#%T?5xObX_CYHFjMS z*EO~7KlfV;{wEyXeKKiW=vvSI0={&6C znyB~A!#@|V^UuZW{B!X-|6IJzKNqj_&&BKfbMZR=T)fUd7w=|#)W>nE1Bz( zyDo+6Qo1gc>r%TejqB37&bt==y1i==>O$w@U5n6M=v=&O5t<8~i+3$TbD?wbu7yA6 zT?@avolu`O+V<0Djn?V29_#d3k993vr%xDdZi?&lIj_y>6CdmJ$((iiWYao*o@AXq zJF-rnAm{^FMVwE7=? zZXNpV8SnGx(7E}a(R-utr-0f5b_o9tk^g@Ryy@_#(Eqy)qJKy1Kffmoox{KX_t3(7 z^w3^ce~(W;8*3=>N=@Yfrbwo>iVx z>M74F^_3Tt7nKG|L#2_@SZSg(RhlU;DK9IpD6cB7DX%MUC~qpwmA8}@%G*jyrIqrI z@~+ZaX`{4N-c#Bs?UnbH4oXL*lhRq~qI6ZdDczMGN>8Pi@`3W9@{#hf@`>`P(p&jV z>7(>j`YHXD0m|pfKxL5fh4Q8Hl`>cvq6}4rDZ`Z!%1C9DGFlmdKP%&v3Ccuek}_GDqWq#vRi-J^l^M!RWtK8qnWM~A<|*@)14`scwLD{HmQZ_4Fl(WxKLN*{SSO zb}M_7y~;jizj8o1sQjrMQVuIel%vWq<+yS}IjNjdPAg}Wv&uQ;ymCRgs9aJmD_4}O z$~EP>5>##|HivMV{1oJuYww{oA7N6D+)ujEtmD+QE-N+G4NQbZ}L6jO>T zC6tm%DW$YhMk%Y5Q_3q9l!{6vrLs~*sj5^{sw>(9U_92u2k=3B2p`5ruogawkKyC^ z1lGnUu?{|kb@6F@2A{>}upT~-_3;IK5gTAbY=n)m2{y%M_!7R1ui&fr8orKi;G5VS z-@+F7Hnzm_X}s-I0V`r9tc+E#Dptc*oa!BX7h7W+Y>V$Cb`_zO*7b5e9HEF2SX^41dMna5=8P-*F|b!qvD2*Wx<- z1J~mQ+=!cSGj74HxDB`C4%~^ma5wJ3y|@qe;{iN~f8rrLj7RV&9>e2!0#D*8JdJ1Y zES|&jcmXfsCA^GR@G4%z>lg^G{a$-{`Wtu?Z{clBkixsC-GvD;5hlhYm=u#?a!i3K zF%_o9G#CgT(D|md{!jIR;2}JWM`SvVy&KbG9A?0bmNpx%x`l472i`%g&b!;G*N|s=zB$mR`SO&{t zIV_JAup(B%%2)-fVl}LeH837);sf{~K7tzKpNntN0qej&I!^Q+{#|U1ZS2@*b*%pWAJ&s?nST%4VS9WZJ77obgq=~}pv@YZ$u7)v#ctRg zdtguOg&*LD_z`}LpWvt18$ZK7*cba@e;k0HqyLJ@e`m^nSIU1!%6~UX@89U4_il9X z-+}Vqeez!o{l7ZxKrXrduMXRAFb*Zs_%(im zrgh>I|Qi*boR6I`YBEVX`(dhP#lom|HJulO4-#})WHuEbTi8rR@jT!(+)dfb2; zaT9LFEw~l8;db1CJ8>88#yz+f^YM;Tek_0maUaL-#{+l}|HMOh7?0plJch^d1fIlG zcpA@O$<%t?pp;h1C}ov$N_nM%QcB^mlJc_h zit?)Rn)15xhVrJ;TzN}rp}eiMR9Y$TDDNt*l{QLS7aB}Iw_r%E=pIW zo6=qBq4ZRGDIX{wDjz8yE1xKzD!rA@ls-ydrJvGY8K8Ww3{(awUnpNHUnzr?A<9r? zm@-@$p^Q{UDWjD!%2?%VWu7u$S)eRb7AXN`v9d&2sw`7}Ren>JD=U=Wm6ggWWwo+KS*xs5 z{!rE{86JJogOX9nq-0jIDEBD$Dp{3mN_HiOl2gg0|R_;^sD0!9pm3&HmrGQdUDWnuuiYP^uVoGtPgi=x|rIc36 zC}ov$N_nM%QcB^mlJc_hit?)Rn)15xhVrJ; zTzN}rp}eiMR9Y$TDDNt*l{QK`{lvLjNw35y8I+7lCMC0yMY%`0SIMemQ?e^Ll$=T~ zCAV^)l1Is_+^@9Ny5CdUDeaZ_l@3ZrrIXTG>7sO1x+&e29!gK8m-2z~q4JUPvGR%X zsnT2dOzETaRr)FYl>y4<%0Ok1@`duH@|7}J8KMkThAG395z0tqlrmZwql{I)R=!cj zDc>sJDc>tUC_gGcDL*UYl?lp3Ws)*knWFrnOjV{S)0G*@Ol6ibTbZNGRpu%4l?BQ| zWswq47As4XrOGnpSLHWlxw1m}U0JEDQdTQ#l(ot_@y}Rko<# zs%%sI!*r<*(Ey2GwJ;<}@* zJLbCMt~=qnlde1Ey3?*Z|xmy34M+;<~G@yXLyk_&yk?Rt>E{W@sx-OaPlDjU2>r%QdmFrTwE{*Hbx-OmT?si>z z*TuOmgX=Q7E|cpryDp3C?s46{uFLAWY_7}hx*V>{>AGC5%k8@RT$jgnd0ltE>+-oS zzv~LPuAu7*xvsG5iny+*>x#Lqxa&%|uB7WqxvsS9%DAqq>&m&Vyz45suA=KIxvsM7 zs<^JI>#Dh~y6bAVF5Y!DUH5?N9(3J9u6x*ZkGQUu>mGI8W3GGLb+r=KN?4EI{jQh9 zulhNt&hMXa%hh(>ldh}dx~E)M*L6?3?itrT>$>M$SI>3NyRN?LUU1!uu4~}BhOTSm zy2h?+;<~1;Yv#I_T=%l;UUA*4u6xaOue$H^6nDyKbQC2D$DF*L~@_uUt3SbwgY?)OEvL zH{5k2TsP8nqg*%Ibz@vN)^%UI?i<&QbKSSD`_6UWyY2_q{ph-%T=%o<#=CBU>n6Hx zlIteBZpuICesObCT{q2j(_J^ibu(Qz%XPC|H^+5zT{q8l^If;Vbqig$$aMkNEq2`! z*DZD3GS~g;y5C&4+;uBl_q*#>x^9*0R=aME>(;t%o$LN^-Fnw;aNS0_O}H7i;8xs* z+i?f(#9g=>_uyXKhx_pW9>hQK5FW-OcodJ}aXf)1@f4oMGk6xy;d#7(7x5Ba#w&Og zuim>&yZK`ey&{!iA^5F24* zY=TX(8NP%s<16?ozJ{;k8~7$R$G5NrzKt!h6~2S-Vry)JZSg&9hwbrw?0_Ay6L!Wf z*cH2BckF>Zu@`=TAL2*&F@A!dVsHEm`(R(}hy8H?evSii5PpGQ;#W8rhu}~ghQo0L zj>J(o8pq&R{2IT(ariBMhu`B5_#^&=KjV0ufD>^NPR1$t3r@vpI2~u;Oq_+YaSqPK zc{m>z;6hx40bGnra49asU-36wjw|qYT#2i2HLk(6xDNlo^|%2y;wIdTTW~9G!|k{O zcj7MGjeBq}?!*0f01x7ycnA;U5j={=@Hn2plXwbG;~6}Q=kPpUz>9bZFXI)wir4Tu z2Jr^o#9Me<&dB1w%A9F+metu-=UAOME0`Q zkJY_a_gUR<^?=obRs+Ej6TB_|zuVux|NYNz|E6sJ8ur`Np&D;v|1ZVp{cam)wBOyX zYQHyq(8i%!(3T6;6}DWc`hUqs@5|b9{@?e}d!@FXk8SxuG4&H0|7pvG>T+98JDa~^ z<8Q5YviZ<&numV#JDaUS!A*2bawo{dAbosC2FeH({r2OEcKM;nJ~ zCmV-qXB&rV7aNCaHyej)cN>Rl4;zQ-2R07X4{aQ(AK5rmKelnGeq!TL{nW;x+S|sV z`k9SGwU3QMwXcmswV#bc^#>b=>W?-K)%G?H)vh)U)t)vE)m}CZ)&4dP)d4mR)j>86 z)h}!us$beTREO9&REOC(REOI*R7co2R7cu4R7cr3R7cx5RKK=ysE)I7sD5kXQ2oxv zq56}JL-l7Hhw6A6hiaEhtuuM|#lk!z)@Mjoi&`yawYb$1RtMU8LUpW-L)G8w-KoAC za)LLjzw&1FqSZ@QFI&B0^(wWGeamfUt6i+_vs&A}m#nXi_gg*0&!Z!F3{T)GJcH-( z5?;Y;7{r@+8x!&EfJrbJrodE~2Ge5(%!FC+Ud)C$Fc;p3c`+Xrz#>=-FL3U6(Wk>g zSONpV{1d$`S^x`TAuNnVuqYP8;#fkizSp}o*5F!PhkxLD+<+T#6K=*WxD~hIcHDtG zaTo5!J-8S5;eI@T2k}olgop769>rsL98cg$JcXz644%bvcpfj{MZAQU@d{qWYj_=l zcmr?ZExe5hvU=O?F4Xr-vpwIyH?cXsg)Q)HY>BO8LObod%(TWf*cRWzcGw=@#}3#L zJ7H(+f?cs2c9$7Amx5RcAHXNDHTJ?j_%;5F)9_EciAnjpj_I%zR>lYMZOn)Hu>clC zeMd8wYA6oF;Wz?E;wT)AW8}M>wl%iFw)h^l!}b^m{-EXGr|*Cru@iR2F4z?V!O2>_ z8+~`|fjzMoet;ihAUI3Qe?D3c9EO45IxRn(eguxhQ8*gM;8+XA%5o_$_{i-{TJ$2=3ML%h<24 zz}2|V-_!GV2-Dz$*cjhI|F0AdqW=ab<7`}vmoOW9wtQF%YvUN4o!MJ<0_S)UAK+9c z@lBR#hn?_4?2TVwH+>eYPu#H(7RDl26nkJ#?1ja!IDUX7uq1wnAK}ON34V&b@iXj$ zeX#^Tk$T_(?8kh6EQO_U0Dg{Tuq>9t@>l^YVkNAMRj?}V;#_v)9^8xV@M*SBKl(2) z5G*~(`#Dku%VIe!j}@>YR>I0y1*>8;td2D>9&6$Q_#i%n591?P3m?VD@Ns+sYvYqx z2cN>a_%uF)&*F1f51+^S_yWF&4X`0L!p7JHn_@G3317xn@Kt;bU&lA_O>B;DVGDd4 zTVgAG2j9ik*aq9;d)N-!l0!lPSqUK?egY*^U&wT$o|4^Z<$VeJfGg{Sr?b#QmlQzn=g0ByQdAG4fJROn=hr%VA}F`>Z!p z*I&EgIp&t&JKG>Vjfk*L~A>PbkY&X>F z*Ww=Bi~AycWtg|jV0?SH*Z+uBM|gdgk)F9md$z;xaT(si$H#cb*2QPg`^!`{gZ*$A zj*M`O`atla?i>31KX@6h;8nba*D;7U@Fw2E+cG|wV6A6C^dAc~=>7ZuNA!JhEY8Im zJQiNV7T601<9PHRFPG^3N6iQxHPlF%yvv+xtLKSr zo(Z>mro>EG0NY~So!+rM@G3?=PAl>_ZH29ICy&!Zcn)u2n?2ro{Ks#1`ipoAQ|=Vjk6|8}^@J00Z9uw$4yx?7i-ni?c*B{1vE_r<}EQlr1 z-_H!9Z-3carYlauMdz_t{ zkF-F2&EQ?^e^O%{mcj1m@0|{^cRG$2(BEIBW`C6pb73oNgB|cm_FyeAp8Z&Dd9;pjLmTsp27=w7yG~;aVPrwMt@H@Y?!y63Fz++ zx3O2;i}^-)$Ckj-=tG<9qaEcyN~wz?{FS2#WlDdui;HhJH}f+1Lnpm=xC3|MF5HcKa4!aeH(2LQyoI-=|CglV6ME+|5Z~juapLse z%vQ`@qgZ@vkhxj@5u-wLcnIiXT z8@A)Rq+li`27>WZ^c&VXE@WnU-z@aL_t5*^OYfVN-ZvY)Z+3d$9Q3|9>3wt2`{t(i z^)9PEo4`B~=B4+&pWZhgy>EW{NDDCITaezj5Pe}Rf<>_y7Qe$1%tTs}nMg}96KQE? zA}zyASuBU%{ae59j};@VMDJUf-nR;URjh{9u?EIRSd;z%d=MW(|2Fk+HUDHMZEJb&HVq~fByIWo&^+V~{a!Kd&Ud={U>dRQM{z!$LrHpE8Q7@J^IdYR>I0y1*>8;td2D>9&6$QSPYkONq)ug%uK*3I2C8%JY0aQS$;A7Qe4B# zTKoey;6~hv+i@rE!rizB_u>IOjA!u*-oVYA*A`5M$uR{!jn7~`Y=TYkC2WDMP=CWw z_ncJey?aY)487L&Urndrb@a|H-m8_A^u;h0mgcpR{~G*WX70j5+r8yWZuRtDiTjUG z|FPpg*8IodE8LTPo6-A^Bme&IKe8ep5$~|f)963G{6|(xW?Eqj^iSpQ6Z}0ukM!LxV{&*KHW zh?np(Ucsw)4Xt@Gu_1qj(ID;|V;8r|>kM zk^b|y$Y*8#b0`1jo&Oxke@5g#3o6BJXN2 zS{FI48C)K7{?Em1-;$Nn?h@YsLm8Xo)4VZ&qp8Ekm$KZ^~I z{pYjcvHzSlJoca6hR6Q%-0;|c#v2~{&wayV_Wt3ap11qYi$hcF{X+v)_WluL_WluL z_WluL_WluL_WluL_WluL_WluL_WluL_WluL_WluL_WluLZ+#;QPtiW!pY(nj%q!$S z6ZdB3d3*kdLz?sv1GWBh((oL6|L{=9d(TtDhxnlPj5U0S4{GlpaflCU?;ml94{Glp zaflCU?;ml{-Z18P@BOpz!T+Qqa_oI#5kABRz0Wbi2Yr&I&qTrp_Wt3au8;SrQTX70 zt{OSk-aq1?>mRi~_Wn`FYwsU%h!1M-A8}ysA2DX{A2rtAKjIJ{+@{Ys!iV@^1-%jo zAL4`FCtBe{&0zD-!z1tZEF80a-;LRxN29h^&0rF}cOO2)2etQ)IK&5s#jNj@6A?%2 z{JhsL;X{1TdleHt#0S0CGvR|iDbgot;X{1zLd@l>5;GpMGrY8S{q)l>;t(G^csXLI z)4-9DPs25DPs25 zDPs25DcrZ|`|D2;v%gLebNN!lT)q@BmoJ6;u6=)fu|F@9#zjBElEsXZ$Ba|Nj8n#p zQ^kx^$Bfg&jMK)9z2^gwm&p6B`{;4>CEOSn{-$;B8n%erks)%Xh5HN0{`D`ZpZDQ! zZV$cZ?jPsrzwaI%`|s0+$Nu~8;j#Z7e0c1?qZ=M)kp6pSG3`=|JpV@_Fv_O$NuZx@YsLl8y@@b^@qp)yZ+&^{~9?w z_CE;-kNwXC!ejr_f$-S>Y#==LKOYE>{a4@NvG<;R+AUxxLju0OEpD2XK{=2;4 zvHyN=cFDFS0v*iNVOs*i};@?_@~QMQnaDMJGtrkPtH#mym~2Jvd_)(HT8q91sYs=mHD}W{ zygzEGY&x&dCGo2_lU=D3XvHQC4b_uO3@>=_Ig^I-PQHn)vKX0kxx)#y#-+M|gXbh}k;&jJsNJa!3naFG4Z6R+a`kI#Eei*)g>$}@D6TaTwsmkbDMgKG@qa`BuP~I{R zX{%O_xKw4eMD$*vY|NfT-w~%7@BR@!zw&qNCA^vFduQ|`+q;AnwT|fRMc`A z_l@Z5URBpC@;;~Uq|-gYTQ+>CrrS7jPp9v!)3{{R7OEbx=kZTjo!cTu#Q8<PwUV(Yf&!T6%XK2wg-m}Q)8Si;y^o;jRGJ3{)E*U-JJ)4Z4@t#jc&v?%$qi6i* zl#z4(v&!h1*h}sGMWyI9d4C@&b|&^xdw(w~cA3~q?fw0z*kxibwf8<)^fKN%Q?WC# zmpR!TmpR!Tmw98{v<%_D>f_y}b>pJP`X<1*@ZSvbme6-9Y8m~VrHDiS459y18k(Gz zL4O}A>QFZ>{I`t!BfJ^^_jE!FdNa}Qshzyz-1Vw=Pkiw11EJG+uLFx6)_WBZuf5}g z1-8crBVV1y2h(bqc@eL7_3HSjen)$6d@$=e|CM~~w$QgD#%vvZ4`Zg-?bGy*Wpe(5 z8U5X?s6*2Xu`^TRVrQypJBR-gly~x++N#lW(=vqr&Xj)!E$^7ApDA{k`k5kM7kQ^^ z8ud!ao5^{{%*MFz--7a%@Ro_4YkJ4bl(^Vs%4Ce4sh=tOdU}^9cBXF3w(!n9cA0T^ z+{apGh`ra<&lEe;^p0h!-LamWcP!(bD*E2*oho`JcI;i|*g0=K(f4_8J<&6GU`Gnr86-E}y?tzPRw;e)DIl#l`M>tHs<}dJYiR zG)eR$yjooUsP%e33u5<3Q{uvZ7tTMgjd%RK@P4L*F27$-#)bcOoj;Q^L-=pqg=S(O zl{xk9O!Rt6-tibLnIU$~B{TGo*>9E15dN!n{`&6R_cqONGG_16RO^Y}yG+xbE&B3I z)AowKEvD(Q6Ma3VWr+Sssc+TIc;`&)eYIc4*e%~VW78zjYfh(IFnY#&PT>8&fbhY4 zP7pogJ&%c=@t(&-&v?&cqG!D4G0`*L^O)!v?|Dr0jQ2bydd7eD^?%sA3#d4@MqSrg zad(0EO0wea9*DcUySux)ySoc<_dwj;-Cc+$6_WGKw~GDK^v~Ys?0v@>cZ_>k<9%i^ zySk~at}YrDfuM8tt1s7?;9G6K`f^<-_*R?0Z|VBc>{nl|*9yMX_Ny<~b%JlT{pu^| zI`*qC*O}nkY`^+)T_^Z9M{wI_^M43k??nWCPdE5X@I4(t-`90r$NZnd;4{IGj!nT{ zN$B@1TcOSx|H9B12E$@F437~oB1Xc<7zLwZG>nchFeb*r*cb=nVmyqG3D5&QF(D?x z#Fzw=VlqsQe`5+viK*}(OpR$UEvCctm;p0lCd`akFe_%m?3e>{VlK>$c`z^L!~9qP z3t}NGj7883y|E}3!{S&1OJXT3jb*Sbmc#N`0V`r9tc+E#DptelSOaTfEv$`ourAia z`q%&)Vk2yfO|U68!{*omTVgA0jcu?kw!`+=0Xt$R?2KKoD|W-~*aLfFFYJwdurKz* z{x|>!;vgK1LvSb#!{ImrN8%_Pjbm^uj>GXd0Vm=loQzX&Do(@cI0I+mES!yVa4ycn z`M3ZV;v!s(OK>SJ!{xXFSK=yMjcaf%u0tPOj~j3!Zoj|cFeEF8^z<|=|-=#53O7#7D8SQ1NNX)J?fu^g7i3Rn>Rk0dY#~N4@ zYhi7ygLSbU*2f0e5F24*Y=TX(88*ij*b-Y|YixsUu^qO@4%iVpVQ1`uU9lT>#~#=d zdtqMYK$7I z#;NgYf|{r%smW@JnyRL$>1u|Wsb;C!YL1$#=BfE=fm)~*sl{rETB??*Z})+rygUTsht)h4xBZBbj*Hnm;tP&?HwwOj2`d(}R*UmZ{f)gk4p{M2D}L>*Pf z)Nyq}om8jPX>~@ORp-=sbwOQJm(*o-MO{_b)OB@3-Bh>KZFNW8Rrl0=^*}vTkJMxJ zL_Jl{)N}Phy;QH%YxPFGRqxb$^+EZokLr{9tiGtP>YMtmeyE?yQomGy`mO$`P#N`; zt}3(&qr$3iD!huIBC1F#vWlXjs%R>@ilJhvSSq%PqvEP~D!xjfJd~$Os1m8fDv3&} zlBwkCZnERD!0m`@~V6)zbc># zszR!;Dx$oUw<@ZNsp6`HDyd4T(yELqtIDbJs)DMhDyhnZy9E z-l~u4tNN+_YJeK32C2bnh#IPfso`pb8mUI9(Q1qutH!DEYJ!@mCaKA4ikhmXsp)El znyF@~*=mlOtLCZsYJpm)7OBN*iCU_bspV>gTB%m4)oP7etJWzWwO(yd8`UPYS#42U z)i$+V?NB?_F11_jQG3-swO<`j2h}0vtNheqbwnLi$JB9kLY-8n)M<4_omJ=5d38Zu zRF~9cbwyoO*VJ`&L)}!j)NOS~-BtJ0ef2;+RFBkS^+Y{Y&(w4ELcLV4)NA!dy;bkj zd-Xy2tB>lF`mDaFuj-rnu70SW%2K~nfcmZes8E^o{8yn>7!_89Q{hzv6;VY}kyR8G zRYgzRT1T-yj4+EOchrpR7q7z zl~!d`SyfJzR~1x6RY_G=Ra8|~O;uMlR83V&)mC*>T~$xjR}EA{)krl~O;l6WOf^?6 zR7=%LwN`CZTh&grR~=MG)k$?$T~t@qO?6j2R8Q4Q^;UgUU)4|bR|C{QHAoFsL)1_; zObu5f)JQc-jaFmSST#i zOVm=eOf6R{)JnBVtyXK)TD4C3sP$@t+Nd_E&1#Fa04a&Z`URqPnCmt1IfNx~8tH8|tRIrEaS` z>aMz{?yCptp?ahqt0(HIdZwPM7wV;YrCzHy>aBXG-m4GFUwu@c)MxcYeO2GoclAU4 zRF?Xs0@QEyM}^9)=f4WA!lcZsE8_(imaljs4AL@u41T|Dwc|^;;6VPo{Fy$ zC=cbS5~@Thu}Y$ns$?p;`dg(?DOD=Z*FGzG|Qvsz$1@YNDE|W~#Yrp<1d|sY}=;ZmPTLp?a!bs<-N+`l^1azZ#$hszGY78lr}(VQRP4vYP1@o#;S2@yqcgU zs!3|HnxdwvX==Kfp=PRCYPOoA=BjyWzFMFbszqwCTB4S!Woo%vp;oF@YPDLU)~a>N zN3B;I)JC;QZB|>XFPwiI+)IoJf`6@qkSRGME)iHHkolqy$ zDRo+%QD@aTbzWUi7u6+oSzS?A)irfp-B35xEp=PnQFqlnbzePD57i^}SUpis)id>6 zy-+XJEA?8vQE$~d^Z|&uzN;VVr?S*96`+2rKPprfJ^xi`6-I?s z;Z%4PK}A%NRAd!JMOD#MbQMFzRIyZS6-UKY@l<@3KzS%nl~5&8iB%GnR3%f%)!!(VO2zV zDQ{I&6;s7k2~|>+Ql(WHRaTW#e2!0#D*8JdJ1YES|&jcmXfsCA^GR@G4%z>v#ii;w`+5 zcknLW!~6IEAL1i?j8E_>KEvnu0$<`Qe2s7LExyC|_yPU#BYwiq_yxb>H~fx2@F!aM z3j^>s{=raL%`@mP^tC1|*XIapB2JQ5qnS&u;dQ)$H}MwU#yfZy@8NxXfDiEzKE@~b z6rbU9e1R|V6~4wd_!i&cd;Ea@_z^$hXZ(U+@f&`}ANUh3{DlGd8~ zhQsg}0V850jEqsx*E-H#J%K0j6rRR2coxs$c^Q$%B@#x)C>Rx^VRVdvF))e5yz ztx~Ji8nsrfQ$A|F+MqV7O=`2+qPD7SYP;H@cB)-!x7wrjs(os|I-m}!L&{hAsl)1s zI;xJTWn(8&Z+b2g1V?Ksmtn$x~i_J>*|KOscxy;>W;dr?y39gfqJMO zsmJPxda9nO=jw%esa~nq>WzA<-l_NMgYs7&)hG2?eNkW4H}zfpP(PKWeyITUTm4a? z^xHcAQlV8C6;_2);Z+0`QAJXbRTLFfMN`pL3>8zwQn6JW6<5Vm@l^ulp*&SWl}II4 zNmNpmOeI%;s}w4wN~Qi$sZ|=4R;5$vRR)z&Wm1_{7L`?HQ`uDxl~d(Xxm6yOSLIXr zRRL8{6;g#&5#^=4RZ&$;6;~xxNmWXfR%KLKRZf*x6;wr4NmW)=R8>_?=?s81uqM{R z+E@qcVm+*n4X`0L!or+WCgA`y$G?5_+Q*YQgP3b{!I3yOt-1CJme;Yo9=9@c10$p} z*Gh`1(wX%$upEw`&k2wA&z#vmA!q-qXZzn`+y4UF{&&~*zqYpj&7OG<*^i$2oSku{ zF%xFSESMFuVRp=cIWZUJ#ypr8^I?80fCaG-7RDknR(!J!wjM0oCSln&7t6Nx@AnS+ zTfXhz^&LBbxgFceEZf#%*|t5)wtZN(wSPCb?P%t0C$MZgm1Wy?EZh28jkzyPyqVBX+{h*af>{H|&l*uqXDy-q;8GVn6JU z18^V?!ofHMhvG0Cjw5g+j>6G62FKz!9FG%lB2L1|I0dKTG@Onza3;>e**FL1;yj#> z3veMW!o|1*m*O&9jw^5_uEN#02G`;`^uhJG0XJgMjYY8-7RM4;5=&upp3Al#EZaWivx9ApxaQ~CZ9nm|rnaA1w*A7g?JBMtmzfy+ z+-sl{^%Ji#^z*Yu^RFcWH9yxGgMW4Gs`)9}82l`4C2rF;9X~f~o1SIc3@qE`W!W}A z%eHG+wq3%q?L_??t6uHmZsu(dv25$hvh5j`ZO^i7`-o-R81^2(!9na7*W54Llg!&b zVcGU6%eJprwtdaA?HiVD-?ALkHtew~Y)^X}gV9(sI#$GYX#bqLt^MyHZ0&ymaf(~B zt;y}!+CQUfYyU*Lt^G5}wxv1J%3xWH!I5UGum4x^^*>I+$v6e4;xwF&GjJx(!r3?n z=i)q^j|*@iF2cpQ1efA6T#hSnC9cBNxCYnaI`qNyxB)lfCftl$a4T-Z?YIMX;x62c zdvGuA!~J*w58@&8ML#@@NAM^f!{c}YPvR*&jc4#Ip2PEa0Wabuyo^`yDqh3ucmr?Z zExe6)@GjoN`}hDK;v;;FPw*)|!{_({U*ao#jc@QRzQgzU0sZkKe!|cA1;64q{Ek2H zr`*WV?#FT77QZvoj^%+#&G~;24#puk6o=t(9DyTo6plu7eSN*n@>m>)<8cB`#7Q_A zr{GkahSPBd&csE z{}Md`dY~sJ#6;MfTWEnVnQ7_VW`1s`02ahTSQv}QoypDK+l9Mv5AMZ%xE~MTVLXCI z@faS*6L=C&;b}aBXYm|fz>9bZFXJ`5iMQ|$-o*#_6rbU9e1Q+~5kAHz_!7_K6+DQC z&=>viD&EF>cptCh4Sa<+c@2LH=km&Dp7Z*rH~Y2^_QihK9|zz-93+Fjt>6e*iK}om zuEDjq4t;PvZorMW2{+>w+=|<9JMO@pxC?jV9^8xja6cZvgLnvi(GL&f5j={=@Hn2p zlXwbG;~6}Q=kPpUz>9bZFXI)wir4Tu-oTr93vc5cyo>knK0d&Q_y`~46MTx#@HzTg zM|q@<;c+~HC-D@X#xrj|cD| z9ztLA!^3z4kK!>rjwkRWp2E|32F-nNeY?$mTgdS4s#6dEow>K%xC2#Q^zQ+&fk00?9e#S5O6~Ezk{DD8w!e1DG zzwr--N@YIoUl7|{8{-BsKFbNv13fVzCPK4) z`kpw;|6o!~hRN}7Oo^#538ug}7!On90gk1EcnE#b4-aD-T!U-z2p+{_cpQDLb9!X2 z@Jj4|a(?r*ZgHI4#yfad-U(y&z+LpU9x(q9AK_zsf=}@oKF1gM5?|qKe1mWC9lpm8 z=#L-q6Mn`o_!Yn5cl?1r(ZXLCfWPsNY{F636q{jlY=JGY6}HAU*cRJid+dN6u@iR2 zF4z^jVR!6-J+T+|#y;2=`(b|^fCF(54#puk6o=t(9DyTo6pqF*I1wk|WSoLiaT-p? z88{PX;aD7p<8cD|T6xx*GiqMUhxxGp7Q{kW7>mf+Tz3x6#d$a%7vMr%go|+rF2!ZI z99N*PRgv4TB>!i}e_d{;p1c>v9P{gVe)-^fyv%2ln|K@V;v;;9FYz_L#rOCPe`10Z z=Jpd}Vhn{>c@`&O`95n#X1Oso!KQeEEj)>*FbeaH(AVnEeHnlQage!R$;`*Sj}P!6 zKElWN1fSwFe2y>hCBDMf_y*tNJA98H&>uhIC;W_G@GE}9@Aw0MqJ_UO0Dt2j43*s6 zm%oyWP@yplhQ)9g9wT5xjD(Rf3P#0f7#(9^OpJxGF%HJXco-iOpa*(lLQI5-F%)k} zf-EOx+1EPEqjLn0;xRmqC-5Ym!qa#L&*C{ej~DPFUc$?G1+U^YypA{UCf<_C*sA1s zjiYfjN7owE`JXi(;v;;FPw*)|!{_({U*ao#jc@QRzQgzU0sZkKe!|cA1;64q{Ek2H zCtCOm1MoNg!BFeWS?ezhjbSh>hQsg}0V850jEqq*Dn`TT7z1NsER2nDFfPW!_?Q4a z&=V74B20`)FexU(YR>I0y1*>8;td2FXCf35* zSO@E3J*Lv1FYd$rcmNN|s(hcP8dk>|SQBeuZLEWJu^!gP2G|fAVPkB9O|cm^#}?QUTVZQ# zgKe=Lw#OdW6MJEA?1O!=ANI!%*bzHnXY7Jqu^V>B4WUE*$J+pAe64rvpZD^A_WJE7 zw`1Wi48Y&`2SZ)w{EwkA42H#U7#<^FM2v)yF$zY-Xc!%1U`&jKu`v$D#dsJW6QBor zVnR%Wi7^Q#Mf027eXZp9H>SXpmF)R+d-VmeHZ889Pe!pxWj&EHz`wX$J$%z-&E z7v{!1m>2V5ek_0mu@DxY6LAtw#wj=zr{Q#* zfirOy&c-=77w6%8T!0I45iZ6hxD=P+a$JEcaTTt{HMkbnp%1Rd4Y*PENN-L~J+T+| z#y;2=`(b|^fCF(54#puk6o=t(9DyTo6pqF*I2OmDe*T-SI)NwglzhUaPw^Q(#~1h# zU*T(fgKzO2zQ+&fk00?9e#S5O6~Ezk{DD8w!e1DGzwr--%3${7Ul7|{8{=SH)c60{V*&UZ|447E^mzS5srC7pSw1zQ{x?RZ;$dUU+4Z!^vO4c2dZ$1KNT3vY3cVrV~j zSy}Y4^!G!4oAt}F{w3WbZ)+U4d58OW1{41@YaU?R{Moi1THo6mJkHFY*FLl#FK_EN z+nJ9?m-U8Y<0#wqn|+vGkD9l&eulB_T;nVruXEDNn#V01 zduEnLefc3wygCduD^SO26q=d%(-;!DIA3QtN zvGeKgB+I8JJhGs}nchb6qd?@S&^b_M>Y{ysa2)PhCB)y{%X*cjtEEaA{m-?s2X39CfF@nyo6x zGom53z)2ikHFebK9L2WnXMZN+c4Ch-xBoZG-+6YYWH}Y)9G|Z&MbogG!hK@Q+ zJI5?i&J*Iw3YY<|Zq zcf{#DFIsbSO~j8_iD$$koWc<`f@7%{`#&MaMvC{`4#!4@k7hXq$JmtbW?6rgP?o{c zSQ%@g{^p=Aoq!Y3i+eW&4{^+xvyh%gzm4A5mHix!Gr-#+X8-gV`Y&toY>la7z}uRM zf9V+Ww({X+K2JrQU}ko3G?D^I>m`9|9y^P0sEPkT>Sk&MVnS+ne9EV=kRxEzgELX>-x6E={j`m9&$v3eL z=YjQjgGa3*k5ojoAG^jZ+mB#Qo;yc4>n7%GUlnWen3^L$@Kr}5p1C!#59ep|IZpeW z$NDyp_30pwcv3xob^Q1nSLySkmz9f0e+;lG$m3t$qSP07%HX5D7_GnVCc zyzmUi&*mUULO-5m^*A2vqoQz-BcZC!9^Te_>@&(Nd-Ldz400qKz2JvWD2mvzj0Tg7x;Z)>8||NrGU zyp4D8F5biY_y8Z`BYccc@F_mS=lB9&;wyZOZ}2U?!}s_B{qZAy!q4~xzv4Iijz91x zTKEeC@HhUE%VG!qeDs^xq4c)Q`JKpS-prWM{7&SUaqXXf4jg^02s&q&eH76dBROMa zXN=;EQJpcGGe&pD7|s~e8DlwPY{t?aWj)O8m-fi#j71o${>QvG@B3OF&hm4rEm+&!qW#vw!=7p9%-46uM$XvS8CyDID`#x&jGdgZi!)XZXWwpT=lzhzz>gDr zY~Wn4p)&^Gjwa50Q)krwZRUP8cis=Ye}UT@I0kOFuT_Zk1GnGTD#H71oa?v!kJ~NE ze0%5pz|q$#!F&g2zN0e+?uWqp8@N9Lw>R*91#VB^?bY|Nw``?zJAvaS=lzhz&CcZ^ zja!_{LmCe_mxna^I+uqu?sqN^X^i1%9|s|gv7F088jm=)AJTZ(xjdxtsB?Kp<8kNm zkj7ii<$>c(=l#I(w)1}Ac-477aD3&wA2>dC-VYo-o%5RLa zakn$>amKyQxX&5)JL3UoJm`$(wVyeU`#PhaGah!vBhGl#8IL*Rac4Z?j3=G(lrx@o z#xu@%))~(^<9TPi;EWfY@e*TFXFr8BmU1o+97{Uy2actk_XEdLo=Z6r%Q^FbV+rT| zz_E<;e&AT$c|WAFl5=_B=;N`TkMpn5K5~3K0>@%p!^b0VwC{nBN8o7R10RpT(Y^;h z9)Y91B|aX(M|%r=JY2^r&h~{g`gr(hdwo1kU^cC7--3@vWi4;y(v4iY(c`4n+2~=` z*~mxP$U1vjXD{pQWu3jOvzK-Dvd%u%*~k1o=Jzr0$F=;JKfyXDSU$<}NiIFfj6XB} z%=q(B{F(p7%rBM$SSNs)0M-d${x45+@BZ>MTOJz2dYXG3mgR6PhhsSk%TZWAil@21 zQJi_RSEG2EeG-pL<1ru4)6Bsw2lYsd$7I->t|ri46K=f zH8ZeI2G+^II@wq!8|!3aoouX=jdilIPEOXz$vQb%CnxLVWSyL>Q;2m6F<*%JLd;iY zzB1RX%ylcXPG#1q%sN+euk`yK&_1@z`~TIOkB7O2kH;glkE@*853Wa-S!W~b1Rhsr z9oM7Fth1MO0*@=Rj_c87*4f88fybCxC+LwDg?kkE2)aLNAw5QB%UvH4^C;VoLQYTf zadUc_`xv;-&BqP8=j}aKnKdi3X5bz+YX;rVuFphs?D=r?`gj!5njt?E&7~nf2hFAS zKKZYnYv!K*SI;uDRsTEBwg29;Ea)@qKYt#D{M<43H>77x$j=mWE!XFWIle=Fc9`{D zA04yzHu6XX9;N@<^ZS^$&sY8|f8>$<$a2v0gM9`FJeJHY1RgDB+5PP=j_SX7o`z*P zEX(0o4#%=T=Uacyw;x&l$npu6Pp};Hmi?G_oiXRVpl>JL);Z~RJHOo4`Q^4wfZI9& zZtHyHtRD2Ef8-4BI%AH^pl|jbcAYWL)}U|xoTGxy{BoNKaGSCBdC+Uw`#cx&Qf0r7%G+H_MgX zW_&F#eU~xlo3G{VHdEAXrkLAIakrTgZZjp_W=gruly;jb<2F;)ZKj;tOnJAN3T`tM z-DWBUn=zl+&F7YXy_r|q=GE)J-v0ee@N3zxqk~?{{ygbA<9bY)&tR$uOD z*YK`q7c=Af+R?m{cfA+p2zR|@GZXw;!9TkBtQ!1U!S9z@$8~SG&zP?xTyN*!%>=(4 z^O?x?mjB(1U$D<<=5v$lUj6qo!EZVEM>k*Txb7eGRgden+-Jx_8~;5uW@W3IPj&QY#2=1k%`V~*$GGr_mVeBZ!z9rG1a@R{J(GW*?i9rLVooiWFT z>x}zz+5MSo&Off#GT(J^oiRsL@aIMF$IIMz*JH!nj_Zv3I_|e@K9cK4H_v3(8S^}G zoiX2Wa-A{Xe{-F2-yZj~vH3dPb(_tx;d(n}n_XvotrNkX-M-dIx0zFJGpF5V&bZB- zb(=ZoHgn!>=7QVIMYowtZZntNX0Eu+Ty>kd<~DQPZRUpC%uTnMTW&MA-Dd8%&D?dH zx#u=>-)-iB+ss3^nMZCjkKJaTxXnCun|bCo^W1Ibh1<+ax0zRNGq2rd-nh-Yb(?wT zHuFB%%rB1&o`L_b#eSt5Pv4JqomsDSGI$344$FEE56_J*U-{dgb1w&Z-ED5!{tj~B zE9i$F{`#)I`A%ZsTUfnU+2xjtdHU;mL7$iAc7mU=%}nIb|Jp7;trI!4(af1Gedtkz z<-m`m-{>0lU(5EAuk}0F-te{l1e*zdo&VeK=L~IsL@#HY;*6!7v6wS32pu}{~s%%eu~44iJUR9GbVAyq|TU(@xOb2eXTUk?WA?abk3OG88bLzMrX|A zjG3J=3!}Z&`W?Z}{mS5sNrH`kocBXH_cx3)hIPhL&RE(ROE_amXDs53Ud~v^84Ek3 zw=))X#$wJ`Jn(Yo;}>phHRZ+A22{hH3` zYqfVC?+(t`(HT22+SmH8^|gBZAFcmi-=e+7Qs<1eEZDf*c|UMm<-8v_u6EuJ9DSVk zLmC6WUlcfQbk+}P+~ZsxIPP`c4{6-zTprRG_|-^AW8iDTkjB8TZ$cUaKQjc5_Ww20 zZy|BEEAV^If#V}*K5+DR-VbT~=v*Gs_{+IGq%pv`Jf!h2=d)HwV`%5{kjAjiX7IG1q5m4~XJ!V^z|nptX7CIg?Pr>K#bCDE*9vgXJHK7dBl@Y2Kl;lI zobA4#eP*=JXZG35K1aUeQs+F&+08y@+GkArJZ_&g?Q?j2*7=|O`&Ij#Z+`}`&%^fF z*FG29p9$>qYz%&O@gFRN_V0de$8yl${d$=h`)3`)>E{~tJLS>*jH7-xJ$69zJ9PE; z8Sn%K{mroUPeWFVYt}Kp@mGIg0%xK5ow)kVXlQ??v_DVSpDkmtC+*LG(OLd~^Y+`X zG-~sC&wjO0kL6fAzOm7M)n>njD9ViedM*d|u2=@MH{xM@On@HfiFq*}=Eu}{m(Q#J z`E?VrW+F_CNiZoU!{qokrofb#3jaa-Bc@?FEvCctm;p0lCd`akFe_%m?3e>{VlK>$ zdC>j-7GS<07Q(_<1ii30n!OqLmMOhi+LX??87DHch2_aCPr4sB`IEF1?5Mw}aN>~}IU{$P!epb}M`a2>l*T9-s z3u|K?tc&%qJ~qIH*a#bA6KsmjusOECme>kgV;gLX?XW#|KtC&uwy-11ov<@@!LHa1 zyJHXRiM_Bl_QAf`5BuW)9EgK(F#1^q^l^uyqVBX+{h*af?ypOuq6lM8dpsL{t%&aTpHA5jYa< zZ3wFUSGC-=Z+{fm8jWLcERMtRH~}Z(B%F*>a4Js2={N&t;w+qvbI|^{b6K8;^Kk(# z#CYu6_?Q4a&=V74B20`)FexU(_&OwF2UFfFFT z^q2uNVkXRtSuiVRLqBUix48fp;v!s(OK>SJ!{xXFSK=yMjcaf%u0tPOj~j3!ZoSeNC+@=CxCi&*KHQH7@E{&SU-ZMncm$8)F+7eZ@FbqX(|88Y;yFAov$OYd zU{1`1xiJss#eA3_3t&MkgoUvPdZ9NK#bQ_-OJGSXg{83!mc?>d9xGr)tb~=Z3RcBx zSRHF%O{|5ru@2V7dRQMDU_&%#D06QA&Hnj=p@y27zc4h0!LS$(!(#-Dh>yhEV*_l6jj%B`!KT;@n_~-XiLJ0Tw!ya8 z4%=e~?1-JPGj_qQ*bTd55A2D(us8O>zSs}@;{Y6pgJjmM<{6OR1D7VlAwVb+9hh!}{0&8)74Dj7_j9HpAxF0$XA$Y>jQOEw;n<*a16YC+v(}uq$@M z?$`r+VlV8CeXuX~!~Qq`{j7`Zu}gRvui#a@hS%{1-o#sY8}Hy_!yty zQ+$Tc@ddubSNIy=;9Go$@9_ir<4632pYaQR#c%i>f8bBF@D~Q)Z~P-?vw!B`T%3pV zaRDyGMYtH3;8I+M%W(y+#8tQ&*Wg-Qhd#I-H{eFxgqv{-ZpCf59e1Fgm26nxS0(mq z!V0|htB8J9DqZSl{e!774W`9(m>x4=M$CknF^k;E$J&LvaS!greYhVF;6XfuzUYUC z@dzHpV|W}-;7L4%r|}G)#dCNbFW^P=v&ylx<*@=*#7bBhtDyPK9e!3dtd2FXCf35* zSO@E3J*IFT9}}PldSXIMgo!Z;CdFi!9RJ1?m=aT=pLK-C=qMh;>|z&i*bo_uDHhSU&kAG6K~;dyn}b~9^S_X_z)lAV|;>7@fkkH7x)ri z;cI+@Z}A~CneL(YD`TVSp_z6Gb7yOFf@H_s%pJ?GP48Y&`M_%F_av87SRlJ7R z@dn<+TX-Aq;9b0j_wfNf#7FoTpWst`hR^W@zQkAf8sFese24Gx1N!4f{DhzJ3x36K z_#J=X|JUyCPwusazc2uQ;~xx_jWY>`#xNKb!(n)gfDthgM#d-@6{BHvjDayR7RJUn z7#HJVd`y5I=!pq25hlhYm=u#?a{L=pU`kAd|6poNgK04xrpFAJ5i?; zB(z@_MPWHAM#JdnXRYOS)}ars#|^j{ z5Fg=Ve1cE$89v7s_!3{?YkY%m(fsyLKkGeyK!5y*pYSt&!LRrYzvB=5i58mQt?6e4 z;BWjR6J<9?OJYodNii8FM?Wk4Xmi~N@^5D*l9p4joDx&vKbRWRU|LLv=`jOl#7vkO zvtU-thS@O(=EPi>8}ndZ%!m2002ahTSQv|-7kXn+EQZCg1eU~7SQ^V$&8 zxwSVcvRuiT$;Tecj|F69XTA_Kg=H0Hmg5RsiK}omuEDjq4t;PvZorMW2{+>w+=|<9 zJMO@pxC?iqpH)-abIR?Nl6@vw70VnUt8opk#dYXsHRc{Q!KT;@n_~-XiLJ0Tw!ya8 z4%=e~?1-JPGj_qQ*bTd55A2D(us8O>zSs}@;{Y6pgK#ho!J#+|hvNtwiKB2dj=`}w z4#(pJoQRWfGETv%I1Q)c44jFxa5m1txi}B!;{sfWi*PY6!KJtim*WatiK}omuEDjq z4t;PvZorMW2{+>w+=|<9JMO@pxC?jV9^8xja6cZvgLnvi(flrBKkG0a!J~K#kK+kE ziKp;1p24$t4$be|^|LPEMZAQU@d{qWYj_=R;7z=RxA6|%#d~-kAK*iLgpctFKE-GF z9ADr|e1)&^4Zg*9_#Qu?KYqkd_!+<8SNw+G@dy4y3x8n%{>DGDD?c6D4ZC9x?1{aw zH}=84*bn>T033*ea4-(Rp*ReO;|LsyqtJf!vYhkD3S5cy*K77!$j?eV#ypxyFsXdW z`D!4qUyhEV*_l6jj%B`!KT;@&F|m$vsz$FY=y0{4YtL0*d9AzN9=^1u?u#^ZrB}r zU{CCYy|EAW#eUcy2jD;)goAMi4#i)Jra4eeNg6(IG#|bzQC*fqA zf>UuCPRAK&etWW?H4A6s9Gr{ua6T@;g}4Y8;}Tqo%Wyfaz?HZPSK}I7i|fz_*W(7< zh?{UTZo#d%4Y%VC+=;tzH}1i`xDWT^0X&F@&=>viFdo69cnpu@2|S6X@HC#mvv>~A z;|08km+&%P!K-);uj388iMQ}J-od+g5AWjxe29%wW_dO6{|V(9-6Ptat*ABwXinU!Ma!v>th3K zh>fr@Ho>OY44Y#MY>BO~HMYUF*bduc2keOY`5*3kF3gR2r2CtG{*y}!Vj(OnJ8?Vq z=f0qy>FlrExA7TpJMO@p=x3GY7Rq2*EQjT>0#?LI@_rbzMGx>HKElWN1UK@TXcInV z#?Pw5E!4$&SRWf;Lu@3g@)blitd2FXCf35*SO@E3J*MPhTu>fhQo0Lj>J(o8pq&R9Eam^0#3w9I2otl zRGfy>aR$!BSvVW#;9Q)C^Kk(##6`Fmm*7%dhRbmUuEbTi8rR@jT!%im9yj1d+=QEP z3vR`2xE*)kPTYmNaS!greYhVFp!pr~e%2xMML#@@NAM^f!{c}YPvR*&jc4#Ip2PEa z0Wabuyo^`yDqh3ucmr?ZExe6)@GjoN`}hDK;v;;FPw*)|!{_({U*ao#jc@QRzQgzU z0sZkKe!|cA1;64q{Ek2HCtCOm1MoNgk)3#6bjB{&6}w?~?14S87xu^NPR1!X6{q2JoPjfO7S6^wI2Y&P{2(r1 zc_A*s#kd5Q;xg3tf5w?3IS=NQ&p3;o=8Sp<&*C{ej~8$cx3CxY;eI@T2k{X4q8}c{ zBX|^#;c+~HC-D@X#xr;p&*6F0_kY;(TGBp4x_>_M;cU4cH{eFRz%5+FOL!Tt;8nba z*YO74#9Me9@8Dg$hxhRTKEy}pXLVz1yJHXRiM?b<+rNpe-HcmsEBf&_uKcVa+`>>C zhQo0Lj>J(o8pojd+ZFoxPaKaEa3W5^$v6e4;xwF&GjJx(!r3?n=i)q^j|*@iF2cpQ z1efA6T#hSnC9cBNxCYnaI`qNyxB)lfCftl$a4T-Z?YIMX;x62cdvGuA!~J*w58@&8 zML#@@NAM^f!{c}YPvR*&jc4#Ip2PEa0Wabuyo^`yDqh3ucmr?ZExe6)@GjoN`}hDK z;v;;FPw*)|!{_({U*ao#jc@QRzQgzU0sZkKe!|cA1;64q{Ek2HCtCOm1MoNgk*U*~ z*N3)gSWX+nbS$UG3_;AuvTY`oZ8NiMn}ucDtSsARV>x>ebFgfilV#gnEZgR0*)|W$ zws~33hxxGp77St`mTe2OY+Hn7TQ8QqgIJX1VptqY1hFK`wxw9MEzPoR8J2C!vRn?! zV}&4AWZAY7%eIwSwynZ)RrIsU=&vjJS!J;tmd6TM5i4P3tb$ds8dk>|SQBeuZLEWJ zu^!gP2G|fAVPkB9O|cm^#}?QUTVZQ#gKe=Lw#N?G5j$aL?1Ejf8+OMY*b{qUZ|sA8 zu^;xw0XPr`;b0tsLva`m#}POZN8xB3gJW?Vj>ic&5hvkfoPtwv8cxRE+VQ374VKE$r#|Rh^BVlBWf>ALVM#mT!6Jud)jDvA8 z9>&K6=z*S?5EEfyOoB-<879ZSF$Jc?RQM03#x$4~(_wndfEh6pX2vX-6|-S>%z-&E z7v{!1m>2V5ek_0mu@Dx;O(V-fU1 zZ!C(%usD{$l2{5$V;L-q<#Jw1dLvSb#!{ImrN8%_Pjbm^uj>GXd0Vm=loQzX& zDo(@cI0I+mES!yVa4ycn`M3ZV;v!s(OK>SJ!{xXFSK=yMjcaf%u0tPOj~j3!ZoSeNC+@=CxCi&*KHQH7@E{&SU-ZMncm$8)F+7eZ@FbqX(|88Y;yFBz7w{rp z!pnFCui`bljyLco-oo2>2k+uNypIp?AwI&#_ynKgGklIO@Fl*&*Z2nC;yZkgAJ88^ z;wSu!U+^n_!|(V5f1-uIFaUq!9}G2_^FM~hFc=oYVR(#y5it@*#wZvSqhWN6fiW=_ z#>O}p7vo`kOn@Hfi3u?gCdMR~6q8|c{2NnXN=$|SU}{W*X)zt9#|)SeGht@Tf>|*e zX2%?u6LVp1%!7F`ALhpbSP% zRk0dY#~N4@Yhi7ygLSbU*2f0e5F24*Y=TX(88*ij*b-Y|YixsUu^qO@4%iVpVQ1`u zU9lT>#~#=ddtq6hNx6k=r=nJ%GF7lnvOSl=gpr7T(<8l~};88q=$MFQ7#8Y@0 z&)``+hv)GEUc^gy8L!|~yoT5D2HwP5cpLBFUA%|)@c}->NB9_@;8T2t&+!Gm#8>zl z-{4z(hwt$N`r}9ZgrD&Xe#LM29e?0YwD1=O;BWjRGwEMRWL8;JR+UX4RbMqw4OJu6ST#{iRWsFGwNNcpE7e-HQEgQ_)n0W_9aSgQS#?oe zRX5dL^-w)kFV$Q1QGHcE)n5%z1JxikSPfA_)i5<&jZh=iC^cG*QDfCOHC|0n6V)U& zSxr$>)igC-%}_JdEHzurQFGNiHD4`I3)LdESS?XY)iSkQtxzk~Dz#dzQESyY<)haB zFZRv@KBnyb|D#7AR*7Y8ESBB1%WA8e5JV5rYeIq$5fQx&CVB~?x9HKM2SFlA5JYc5 z5WPhY7x{nAXHKq(9kM$=`z`Z zunlYnAz%mC33h={up8_FdqEfo2m3$-*bfeXgWwQ|1c$*9a1=y=W8gSA0iwZ4a0;9T zXTVu-4x9%Uz(sHgTm~`V3b+ccfmm=I+yFO09JmE;gFE0ZxCico2jC%i1RjGYARasg z&%kr=0=xvTz-y2I-hj720q?+j@Bt(OO_7{0fSlkPkPGAnc|cxZ3-W<)L4HsGdTkGN3Fd2g-v#Km||{R05Sj6;Ksa1J!{J*n=9NCU5|?Ky6S5ID)#M z9;go*fQFzEXbhY{6VMbmgJz&PXaQP+R-iR#1KNUipgrgSIszBq3Oa$#zzw*AE}$#u z20TD_&;xjap1=$A0^XoE@Bw{*FX#*Uf&O3s7zhS|!C(j&3WkB%j&P3^syIU^CbPwt{V7I|ue3LFE+!3huzPJ&b5G&lp!f^*)-~s3F5#la2wnKcfmbyA3OjL!6Wb(JOT0GDR>5+gBRc>cm-aA1n>sD1qyfv-h&Sy z5opTc`43WqG$1WV2hxKKzy@RlnLuWc1!M);Kz5)7IY3VE4afy@gFGNFum$!bORosJLmyCK~LZXdI4|H8~A`ez!&rd z{Xl;(01O0!z+f;03q0^CV+`x5||980DmwQOas$F z0GI)0f>~fTm;>g5d0;+R02YEpAP_7DOTbdF3@isLz)BDVR)N)E4Ok1-f%RYm2nHL$ zCa@W70b9W~upNYe9bhNe1wz4Yum|h~VIUmr0})_9H~!F6y0+yrsp7Pt-WfV~OK-U0=@1Mk5HkO(y8@%#s=K^l-2qyy6a_zmVxTzq z3H%Ix0VTk%0A~QYBwdRBoi0t6q07?c=<@U*bOpL1U5TztSD~xY)#&Q9j<%<3&^2iX zx)xoVu0uQ0b?JI^eYyeNkZwdbrk&^}bW_@yZbmn!ThJ}(R&;B+4c(S*N4KXt&>d+P z+Li7^cc$HFce)GRmF`A+(B0`Cv?tw@_M&^y-gIx;hwelB(tYWEbbopPJ&+zm52lCE zL+N4ka9U4~phwc9Xans>kEX}aW9f19czOaok)A|Prl-*U^i+BpJ)I7qXV5e0S@djr z4n3EiNAFX5Eft?kdegmWAG#0iOZTPw(f#QG^gwzLJ(wOs52c6E!)ZM|f*whaq7Aek zJ(?avkEO@aG12vgX`T~8C zzC>T9W9TdNRr(qoOJApN&^PHg`WAhgzC+)o@6q?^2lPYw5&f8cLdVlj>1XtF`UU-x zenr2g6X-YeTUw#t(eLRGbRzucEB(Iw&7JYrSjKDAzufb$^s(&Q!!NI8dPIMHZ~wf_ zKd&3Y?+tg*f2F^13#QvKj)|#Fzace#Pip#Ysp)s7#&1qdZF;1C9_>Hxp=n*?eP{Ge z9?`Bp9akt@cGG+4y>u8IPVb{5=>5hPrSmdz-gcwi=`M6vx*P35cc**Mo^(&zi|z&A z=O@Dt=!f(p`B||q`}C#z(f#QG^gwzLJ(wOs52c6E!)ZM|f*whaq7AekJ(?avkEO@a zG12vgX`T~8CzC>T9W9TdN zRr(qoOJApN&^PHg`WAhgzC+)o@6q?^2lPYw5&f8cLdVlj>1XtF`UU-xenr2g6X-Ye zTUw#t(eLRGbRs+-bFBqnAy@o{cun}wm zo52>a6>J0BK?v9Zc7k0X6zm3jz+Mmr!ofZe0rrCf;2<~zBEeyB1RMoX;21a#PJn1| z5}X33!5MHCoCD{<1#l5u0+&GyxB{+%YakX}2RFb?5C?97+u#nk3+{pY-~o6D9)ZW; z35W+z!87n2yZ|r3EASd5fH&YRP{2Fz9((|aK=TLQLxR*G4M+>pf%G5)umKrCCXg9q z0a-ydkR5124v-Uk19E}fAP>k3Y(YNoEyxcFfbT#-@I9~tg}@J>Fen0wf*(OKP#pXO zeg?mQ65v1%f!{%CPzIC*2AS=770i9+(dnfQ4WY2n374 z60j631Ixh*uo47;RbVw(1J;6dU_ICXg26_x32X*iz*evgYzHA=2iOUAfl#m;>;Zd0 z7zhXZKm^zi4uFH;5Qqea!4Ys2M1f=AI5+{K!AWomoCasWS#S=V2N%Faa0y%nG2jZg z3a)`za2?zLH$fb@1#W{o;4Zia?t=&5A$SBHgC`&!JO$6dbMOMZ1h2qrkO1C*w?F~! zzeMZu4t7$^>Y0zZRaKnd_G_zjchKri48dIKNO z2l#@%pdaWD27rNJ5Eu-GfT3U*7!LGc1Q-cM0R!*@qrn(37K{Vq!2~c7Oaha^6yOi0 zf@xqn2mmv{OfU<~26Mn%Fb~WJ3&29K2n2$~UNPHAnz&z+0ezci=tv01|kPT!9T95_8##11JoNfTG|> zPz)3YKY^dYFQ5eY75oNDf>PjjP#Tm0WkESm9{d3+fQq0Js0^xrs-POE4s^gC)BrVs z1E>XRgF3(w)CKiGeb4|j1dTvr-~^h0rob6A1Iil3=9W) zFanGOqksYUfze@Ag9TtA zSOfyWVz2}(1#TwHh^HT5o`jR!4|L;Yy;at2-pF3f?XgK z>;`+lUJwSt!9EZH_Jaf9AUFgf!C`O&90gI}7&s12fM{?MoC2r88E_Vy1Lwg7a1mSr zmq84;0Gw>X|058ES@ERn5 zH{dN$z&r3Bd;p0+Qwh(1kQ$@`X+b)W9%KMEAS1{GGJ`B2E64`211-n_a)NI_E|44K z0eOKf$Opa!`9T5j9ViIC2X>$k_yH6KML<#TBPa%ngP*|9;1^H={0e>pB|$0hJ17mx zfU=+*C=dPs6+lH$2~-AEKvhr;R0ld>4{Cs#zyZ_(wLu-=2nG+ zKvUohnt|q^1!xIcf!3f6Xbakb_Mijk2wZ?G=ma_gH{cGsfUck$@BrOG58w%U0x!@D zc!S=+2lN5HpfBhL`hx*rAQ%J&gCSrj7zTy|Js1H-f>FQ#{J>~128;#cz<4kLOazm_ zWH1HzgQ;K|m<|HK3@{VS0<*y!Fc-`N^8wBP^g?zmZ z`VbvSAEuAc`;?Mfa2`@h(ZAEB=`wU#x*T1e{)4VSSEMV^mFX&URk|8ooz~IzbPc*D z?LgO}YtwaTN4hRukFHNQpc~SS=*F}Y-GpvRJJZeR=5!0XCEbc{O}C-j((UN>bO*X4 z?LxcKo#@WA8|_Yap}W%EXb-wO-Gla|d(vKXFWQ^#P5aP&XkWT7-H+~151peNFk=*jdH+Mk|EPot;P0rU)dCOwOu zP0yj{((~x~^a6Szy@(E^7t>4VrSvj-IlY2jNe9uZ=+*QZdM&+u8IPVb{5=>7Bo`XGIXj-(IMN9dz;6n%_7PM@Ho z>67#+`ZRrpK1-jY&(jy^i}WS>G95!-p|8@{=vewXeS^M9$I-Xw+w>j!E`5)_Pd}g^ z(vRrJ^bT5f1nfLj@iWd--ULiJJFqKH`<-< zLU*OR(H?Ynx(Dq^_oTh(UbHveoA#mm(7tqEx*y%29zYMI2hoG+A@ops7(JZU(in1Lzs_OnMeQo1R0@rRUM}=>_yc zdJ!E+FQ%8!OX+3wa(V^5k`AI*(W~h-^jdlyy`J7c2h$trP4s4Z3%!-zMsKG>=pFP< zdKVo^@22NFX>nGYdV2`L%*dJ`W^kA{y-FD%y2HJ+sNN1um(^=@O zbT&FWt)+9&Iq7fcTy$()sBE^mlYY`g_`rE=2!87p9BQMd=^uVsvr( zC;Dgl7rF%fEBzZ?k}gI6PM4<3&}Hdzbb0y@x&mF1u0&U+tI$>HYIJp4N88gi=$fUT(X;6}^jvx#J)d4cFQgaIf%IZ}3B8nFMlYvV z&@1U6dKJBzUPG^?*U{_g4RkQQk={garcHYq`+v^jtn~TY7H(}Ty^a2S-+$eb?c9@) zzqvm<*ncOziw>oC(|hQ>bQm2@@1rB={qzC)Abp6Aqz}_a=%aKLeT+U%pP-}Zlk_S2 zG<}9XOP`}ZH>T&g^a6d6zC>T9W9TdNRr(qoOJApN&^P~b9><=y=-c!i`YwHszE3}( zAJUKL$Mh3Ao_`Zb+EzoFmK3jL0LPk*2jX^mElS%h+xXOe4lEPb86 zLEog~=v(w{`VM`UzDM7uAJ7l!NAzR*2^~*ArJvEy=@;}%`W5|}PN3h=Z)t^oN57{( z(1~y=9#LvK4V{)wN2jMV&^B~NIuo6l&O&FUv(edUEuDkTNqEGy*bSe6Gx-?yeE=!lA z%hP|*73hj|CAu(ce;`g8-jA>D{> zOgqs{=%%zY-HdKdx1d|nt?1Tt8@essj&4tPpgYnov@6|-?o7MU?sONrE8UIupu5vO zXivH)?M3&Zz3JYx58a3MrTfzT=>GHodLTWB9!w9Rhtk97;k2F}L64+I(FWR&9!-y- z$I|2I@$>|GB0Y(oOi!Wx>8bQIdO96I&!A`0v*_9M9C|K2kDgC2pcm4M=s!E9jMU5WR|CO|PNX((CB;^aeVZ-binvH`80_t@Ji}I~_vrpm)-{=umn$y@%dQ zhtc8mK01QlPamKU(ue3s`Y?TjK1xT?$LQnq2|AiSNuQ!m(`V?j^f~%GeSyA6U!pJ5 zG4vJsDt(QPrLWUB=$mvLeT%+L-=Xi)_vrid1NtHTh<;2zq2uYN^fUT7{epf;zoK8$ z3G^HKEv?Y+==byoIuRb0Rh)H((|URYJ(32v@+gPuvxqG!`{=(+SfdOp2?UPv#Z1L?)|5_&1Uj9yN!pjXmC^eTEay@p;( zucOz~8|YwqBfW{g0eSkhlAEG1a z!}JmQC>=!~qmR=k=xF*ReTqI!pP?g^mZ!z5Oe?xI-G**Ux1-zB9q5j5`kdnNlYzFO zGt!yp%ybqyE1iu#kVC9>kUm65(ue6I^ievBK1N33!SreR zA)W3U@%$}Fm!dn;J?Y8xW;&9-L~C=2waU=W^k8}tJ&T@CucNK|_D_#ngwp&Bu3@p> zr~f67w+Lkc?l05lX@nApmI!4rYj_gpvg|eZ2jZ*M$fr zH`e;=kGBZrJM{l=d0mK5Dq(LwKW^2r^vmbo2&ESK7++8E`4934#gVQH|DADbg8u(4 z<7Rq(`}5=Nub)G#^APL&%{sUI+-vv$c$T-yNZ>I45Qf zE8$wX#e4ADbRD`bU5{==JJC((rgSs9Io*AG}1x<1{2Zb&zx z8`Dm76S^twOgE#O(=F(hbSt_w-G**Ux1-zB9q5j<3++mGqC3-Wv^(8}?n-x~J?QRq z589LNNqf<~=m@1mym z(evpA^g?zmZ`VbvSAEuAcN9ic~7=4^RK}XXk=~MJ+ zIzkDE7mv3Y^h|mdJ)53G&!y+l^XUcjLV6J$NH31SJA8KHS}6~ z9lf63KnK$s=}q)zdJDak-bQbyL+Bm!PI?y|O7Euk(0l1HI-K4|N6`D}1N1@q5FJS$ zrjO7^=_vXbeVjf)N7Ed75W|hp8h~5!s&93 z;J>GrE@xTB^fG3U(MHCMGG>x7vy53}%qn9x8S$;aQ*q|R_X09zl`)%)*%=d4<5TH0 z`0aaYd~0cON!nT&Wx&4|yJ81V@@BffiJ#JAFn`2K+r-%K;&b1+7H zD$Iz_CK&MzH6uO`W5lQGjQF0K5#LlZ;=5`_e4@yR@2eT{DGwvQvqr3wMw`nK(fAoQ z{+$870~deecyODP%lO@Pgi=zDLn%h%H}GP-O3U^#GM1IGoQ&mV{6oeHGFFtal8lvQ ztiovg6i{rx8vizaG9&)h$$s{X#_tqFdrjHyAY&~?aab>396b z7`3^K$F0p}-16pcHgidY(pawNBx4g9o66`cV>20>%h-a^`01k9k5;n1wTx|KY%60s z8QaU)f$`(t*{08Ar-EN=Ac> zelm`hag2;(WgN$twB3g}jn9D1-x$Aj(&jS82xYSDKZVgLr*Y3Cl&P|PnvBzB43Ke# zj5B4NCF5)v=g2r$#(6T%mvMoN3uRm+W1x(SWn3cTQW=-YxLn2+GOm;{NXAt%u9k6) zjB90FC*yh+)<3<@b$+%g@Ei!JEahr_WWekyVhm1RA+$CeEjJsvrBja8f!((QFz%I3X?Zy* z6W98QTQRxs_;+KJiN}p?I_2Ao#>`&1IWDQ~C)uV`mZ|;hZYM1*#IX=tnR6?tt>LMp zY~r<~Y|_c3ELiRLo!YO)`=q7czcgm{iuixF#Z9NgUr5R-;>s3r(Msq6ITX`n@-6fw_>k&PfBWYmNhWPP{+B9nw3?va%xsy z&Hhlc3TjqS%_^x`Wi_j!W>wX!nwnLYS)DXGrNkXEV-i`$<4M^>H5+$1sclDmQs#Uk zDLelvDZ6qiDQkI4WD!bTwOTzjtFLAa)U2VJHBz(2YUZS7P1LNZnmMakGc{|jW-ZjL zrJA);v({?XM$Ou)SvxgruVx+8tfQK_sF|ypbyBm=YUZY9?rPRW&AO^tH#PH6v+iov zL(M$Ztf!iJsaY>I^H#IoYUZP6ebmfX&HAcYKQ-&GW&_l0pqdR*v%zXMM9qe(*)TO5 zu4Z~Q8=+<+)ohfS8Pv>A%|@%)7&RNKX5-XsyqZl=vx#apNzEp!*%USNSF@>VHcic@ zt66}W%}}$MYBo#FW~vbIv6?MWv!!abOwE?7*$OpV zsb)cHwo1)btJxYgTdQX4)NH+)ZBVmdHQT6Wo78Nxnr%_Dt!lPS&92QM0{j7N%z5YPL_!BGhcZnjKKHgKBn2%_7z8u$moFv!iMjrDn&}?6{hp zP_t+?JE>-;)aDyEraMe z^d0&HtXCFp5$jeOE&4RXJV~cCquuEKut6!{CwlHcPo2`0?P5k{Q0k*yr_5r@GCGR; zvzGZ**iX4ORrGnumNcitcErEGq*E#**YOv!I>m>1e=g0vRkR;s%V9c>eoLprOi8Dt zr}NTv=_Yg=x(_{+9{!09N{R7e3*Pa<;&&)IMf@Jbpmd%f@&{}YzuG_*%=dK4Lb?L& zNOz-`(64CiS<%y(?o1m@Jcs!jdNaL`&V%o~Pz6_a=#;$7n=tpKGw^sHWA>?2j&tb? zIxAZyFfYygcX};sP~rka|7UD@O{d2U+Bmn?DL*jp1RIo~nZoyBo$`?F`7epQ7=3{) zU6AXQ$J>P8!8*kTb9tRoO|?OZoFiHy*m8lsLqCTNitRkHG?6XoV?^$ZSvmFuxn3Eu zUF0ro_oBn-NidFOfmm7-^JRl_o4FTqozjCILSLtEzy=Je zYEVWk7WokN4Cd0!^fvkk?5BwT?MbJ6KyFZeM{cyel4_)A|G~KbuwFUDZH8d|0_-rX9M4ts?s!#d?4T69W9o-+&5 z`C+{>liR7smZ5ZiT6`O2P>S-^q1IE8citzqFrWDv-e1L^iI#G-6Kqgy4vH2ruh%Iv z*i!D0XwMZXS_VBAo&f8WwcOj~%tMgt6=9uH=Y?=Px;O1dAA$8sejZDamtvdcqeRbY z%%?H;X1-{#XxF|HeHP=@)1ZXV4$)!@@2~~EViP9%ykMW_Xg4U&=zMs58k918loos~ z`h0KVddT%kMXo!6d2QqvL)f5vcSiJYlps8jj)L{dO)mZMoM>MOQfnEUXl+C9^pN(wU zOz);|!v>`ap2hacEw+3&O5~mCb#!5RH+_e0X%Ky!nIEB}=olD}OgzU8N)JAN&mz|= zzZ?+#pK`6|bRIv^=LjDE_KF+x5_pE{mG1<;}pBu~vGA~Z|phv@cCS0X>pmaFVj?x1LS z%Ut}+q6TF>9^nS13m?6EuoSPS>=TF?n!WOf9zQ|k%_oYUL3r%hD{GM3E8%qhNh0qL z87Ak=rYoXMglh$o-UUsDk?oInL@wMgG$q;g$40`U9+2 zj$^5xQfi)9nv>Se7x{Y_OQVDz!S+hm1)}HiK+%$b7C$B3V&QB{gr~yx%C3cCX^Ueb ze}mj!`9P;zB=YQZ9=bB@r)XB---A~Q;>>eE`JT3;3(-H&h3O)6QTj)^7+swHiT;`X zg)Tw=O8-Weq)XAi)1~P$bXmF_U7r4fu0U6$E76tdDs)x48eN?hXW9dbJzayYNjuQB z=-PB0+L5kHi}UjVr9Rz&Zb&zx#d-UH;zT#0o6^p7GrBq5f^JE-qFd8#=(cn_x;@>2 z?nt}Pu5>55GwnvZ(_QGUbT`_A?oRihJ?Wmb7u}2YrhC&qbRXK6?o0Qh`_lvHf%G7H zFg=7GN)Mxl(|URYJ(32v@+gPuvx zqG!`{=(+SfdOp2?UPv#Z1L?)|5_&1Uj9yN!pjXmC^eTEay@p;(ucOz~8|YwqBfW{< zOmCsL(%b0mbO^nJ-bwGGL+Rc09(pewMu*e;=m>g0eSkhlAEG1a!}JmQC>=!~qmR=k z=xF*ReTqI!pP|pv=jikF1^Oa=iM~w7&{ycI^ffw`zE0nuZ_;t}E&4WnhrUbSqwmuX z=!f(p`Z4{4j;Ei}&*1=d%T1)4kbJE|?x#-+<9y%{=OXs7%rSsDT=4ZLLa50 z=wtM8`UD+KpQKOGr|C2FS^6A(p1wd|q%YBz=@|M7eU-jO$I{p78}v;&j=n|Trti>q z>3j5j`T_lrendZ}pV0C2Q~DYGoPI&Sq+ijm=>+->{gzhfcl3Mu1Dyz4Z`Kj8j(~Lp ztRwKBIRY_C{0K2Wc}hQ{pVKesm-H+8HJw1eq2JO9{f>T5f1nfLv}w|$O`YcBP5hBs zG^E86+|s2>nx zbVX6#w2k}oH+JM--Id1yeXeerwCO&#=zpsH3q$ljH4fH;wT^&w1gs-q9RceISVzD* z0@e}u-x`6>zf<_%8VBpq{%?+eZR9sv+h}kO#DF+(7d!?pz+0fnrL|2DvVfex78C?U zz|WvGC=aTD8lX0)2b@3)&1n>c*&8@Y~m0SC-xEF1I{MXd^R~*Z~yM}EE+;62oSx^B~0X0Bv&;T?Atw0Ch z_TM$`w%z|T_Se=MW7iK10R}K0%m>TBY7h*zfl#n7`99l5qU|_111^DB<8s^EGCoY+ z|L;DYY@cD9ufSXI0i@0Ix1Phcneu3Dvw@r-5BT=)t^arT*V^AY0;VHiYlp|&kDvr7 z4Jv?YzyZ|z=O2rrYbol3MnI5+_w~l~y1pf0!z_uIC4PL<4dVcuNzqhP=XdMCT z2v|qJIs(=au#SLr1gs-q9RceISVzD*0@e|*j(~LptRr9@0qY1@N5DD)))BCdfOQ0{ zBVZi?>j+p!z&Zlf5wMPcbp)&yPL2v|qJIs(=au#SLr1gs-q9f2>8fbAe$8?3ip z8~o)_vM#lbfOQ0{BVZi?>j+p!z&Zlf5wMQH{~sfen5fm*j>da{S;^l6h=1htx3Z}; zrSUImzE72k2ixa^@tBjM_)GNbeL9uKKYwa%n4nT>ZE)q(+E}2gl3J@@no1j(IILW0~|a@ULIbkW>sYsa^?L zFZds*=Vzu~0a;I@|N6%_+DttgS?|I>P;ZQxdI^J#&n@o}U;p^VnyGhJ*7N@d>Wwo~ z?}DsnJMzoN=fCyuoj*jW@n-5p%6h>m)cd>V1yO2(nR+3zUa3)EK0Zt8O*B()nXH%a z57e7vre1)o=WqD>$2ZwbJ-w`F=lAvNO)*o?Th?>?2kQBosplf=jr|AeO*K=mfvk6T z?AIU9X=dtGko5+P`}*~!o2ge^)=L=w_3H(gsb?$e`QvrMWbvO;OP=pDOtunkXMBB6 zE9*u71LHZFbZD#xChr zT-=iW_!u3v$!`YrCfi5wmdeb{*kL4Z;@|84^d^?uCH+JB|E8EU++j%xVmQcBJF7Oz zB`jH;}}zW{vQUW^LowM9NGZ+_z*RcZb|h*WIZuo|JwETB^5(Vsuv{d ziFx?ft`}jZUVyA8=HXwv-hMOn^s=6qhkxyQ2h7y-mi5Fu{A<@cXr^8}Sx?NvzjnPt zX6iMN^~5~@v=HXwvUX+=72?LFzy!DZ@;W3=HXwvUbLBdTVy>k5C7WrPMWC~DC>!N_}8v?%1ph9vYwcSf9-mw&D0wr z>xp^z*RFTQOg&FoPt3!=cD=J^>a~*f#60|K*E?sXo`b9>K70MNUP|-ul+M@Z&D1L; z>xp^zr}a#_Yb-fm$C_*)T&K~5bjqqN@P*?u>HgVzY0=4a{uz=#r#k<S=a=fmnW?v5*0Xf}`K5Zd%+w2(^(>u# zeyQGVGxg@ndX~;Vzf|vznR;VoJxk}GU#fT4Og&#&&(itlm+IX!Q_n@#vvmIXrF!?x z)N3H?SvvpxQoRRe>Q#{SES-OTsop~~_3UIlOXr_os`toDy)3exrSs1()q8BFUP6E4 z`P0(*=a=d|F;g#2*0Xf}`K5aCX6i-DdX~;Vzf|w3nR+3zo~858FV%Zyre2_|XX*U& zOZA?csW(y9vvmIXrFt*S)Egk{SvvpxQoWaE>bc2!md-!FRPU9UdJSYfOXr^->uELo z-rr#pj=PCdX&8_)n?1`L3w-X+4+Rd9@{C zjGv<}HrYzJov~gPSzST9W0v*bF2lxucN$>KkymW*eR$yUPcjN@sL^~C(sWbvO;OX{sM*-E&bv7WcAXUTO4DUIi9 zGxc0#Jxi`bNU7c$GxeNgJxi`bNU7dhGxe&-dX`*=kW#&MX6luY^(?s#A*Fij&D67% z^(?s#A*Fg7%+#}y^(?s#A*Fi3X6n7@YkYmMTNVrFILvGTNPpFG|+4 zTNbtFGSX}TNMoZ<(xT$#n=R)!S;Op1-VT$#n=R)!SyK-T+xo%s)*Q zGfnC9&*b%t^H0;l+hT9omAKYX;ORhsmUe7rHG_B|UNxi^2S+!1II6jl^W&#OtKBc>pIuBg5N@Z@h zCF@x-|4ixlJj~S7$aznK{4=F`US{eA%6gW}KU1pL%S=6gSxe7 zOg&Fo&yx9PO7(oq)N3c}Su+1jsa_v5^&DkAOXi;`)$=t|uY#;+$^0{=dVS5*DYb7GESZ0% zRBw=(dSSAjCG*de>J2tiFG$w2Wd51Fo^k$ZI)52rre1)oXUY6Cc|GI&GpU|8eh20c z0)eeS^`iZ=G7}v_F-$)B)q~(DbH#+kEUB-x0r8b2V-^tP}PL zzgg3JeB7?K!^ggwmgonZfIo-@S||K&EhDyp7z;F+kYk^mfCl@dcTTO1Z;WMiQfs3f z@te0s=#ORI)zQZuzniOx<+aea9{SYAu{1=ETRiq5x_k0t2s5)^0zT*9(&4u?_avZ9e)Dz<_>J|A1>Lrc4C>M?AO)PNghVce^pu=O8&kKyJ z9+-}&Q+xEo^UmAtzI39_=`SxUWE0)HgKyVByZR4jPH}RVk|8j&!qRsVv|Xa zckw>C0N%TbwyHw%CdN~IPxB(+>(|>N>uJKXuylfEhDq&7L!N-uwj%7X>a}vUJ(<6)S^QtzNTs-TDo| z8#isq z8a8U|)TF6%v*s;YwrbtRc>JGV{-1IDc>KBGb;TR#fjy^3l%BQclg-sss*X@kI1B%1*JZY{KoF}g~BrmWvV$wa?=V7L=qvEwx+>*{K z;&Ix78w`O;daJ)k$lT-w)%W<2Ll|)!@jqEA?-c2}!GDe-!X!{&Xfb$suZ5!?2jnN`~iQ~(!neh&f5fS3>tSddlQ0N8;S^RsHtfFR%t z#C=v4Q48+Q%c>0p{=fy40Pp5z)kcFYzyL~tcXP681Az-D0pe$8)$RuYKnHSxhqJP3 zgMl}20Ag@1ti*P}L{JK7!HX4HwJ~5nSPT5Y0MHERfE{?a9Qy#afU&>@Gy_#YS`fPo z#|nl3M^FUBEyec0SkMaSKoMXA9xlnMjRf<-0H6Z}z}>~z4=^9-Ko*b?h}s|s7=Sl$ z05%|M5w;JSf#Sdh#4f~s0)LU0#v^`(aZXS{xfi&iJ(L9*Q+`B<5^L+&l}2FP2<+!1*L_7V!sT?qhjOJb3h00nZ~@*x5BxzO2nJyw8pMKl zpm~k;fgLCXbifI?0B@iN{vZ$pgD?;cVnIC6Bw&4D2TB1QZ~`vC8|Z;Q2n4|(3`B!i z5DzqOus*N@rGO4NfxEabw%|dTC?1%Oc+VT)piRKPF%rBM&$;Zz_g0MxXtaTa(hzT~ zBf5&4hv1@f}14kOkPtDB8r|;=Qz3mKh{16WbN*iiq9d zcEy88wB-OfC8Ay-All0T@wdJFyQcix2~n)mRJJ?I*b-4}zcmoG#pmf_J3WCN@CF5d z_#9L0t2hqPUk}7SkC1UBqF6o(h~)+#wlfv{1QyD_7bA-OTLwhEl|b|p-=m5B6Tcr4 z%g+O`+!li(#wTta&SxM7TmWZ46xa_!!4|LkYf{%_z_t#?5gPu{_|7Q&rjOLHUjOl8Mjs4zJ;T`v!kw=YpGFHWGkoo8~zlr@*xB z)4Twdmy&%P5!x zEO0bwYlqwu3@~XkJwEIG|8e|G$K^jgxAguuInOkA@aXE^$M@q2%5+(czMeg5dU$tp z_c6cRzOQe$k4uuBKU*}d>r|_u!|&xwB|D2H@8j^vx!a_63!FARI)Bo^bU7AvtmROv zjOpf0my2_2(plHEUA*3yYj4`n!F+qovS!=G>yEkYI5xDe+0nH4r>_fQx#>=rTW-2D zpDY(cVy>gCk8lcz^#_0gRCyiv28t5;Wd zbG6z>*SCh>rTUHaRd35pU?CSe*=4Nu-EsV!4_OzF} z36te=T)5n!mzVi%7?1h0+mJOdZnXw<=HaT9J{gOT``W_Y$D_-@&-F{XA5z#}Z4WPx zKHc2iOg3XWHqE)mP2GF1)G( zb8hLp>CXU58O#3*X59U>0>?7t$DQ(BuqpO#zZ(kpGvs+6(77hD)`g(X{zVGVm(a*h( zPp`h-&2;vi`uO^|cJ^)R;ng+S-X>o+sTp>{ zYTU)N<7euBvQDx!OtgfM?bt@N%{U2efj3fY}aIuVa%7!-Ob7Rh`Ih>te;KSra-l<+7H;7NEcieR3n?Vdftz{#WhN0>txf~0vqtK zb~fz=5DJ1o0MG+Z;0VeBJCF;c1utr4)7}LaKqLqOK_CG50vAvPh`uEdbBP>0bjYTS z0ns1~tOcTvzl=i=yMYFv0{Ak>>TnM&!%zlSh0P$PIEQt9LwTRy% zRtU(Z72j)@LM((>95EYWS}YgWg1oDlO)I_!e~2hvzU;7ETt{PzC>~?cXcyPML?PNC zMk4YW5kzs#NDN|bL@oM@Ygf+T?|g_h_*-0KqCphb<|ItdrWMz!#3PDR!(GH55aSRF zBgP^YL5x8xig*T59A+dU0&elcjWIzylyQ3^|5jgg^~U8Iz1Be;E4`+aF&F11ae+vx zcZrGOx*f6aI*^Eqi1dgfuw3k?xE{z3$c-bzEiknXHr^uB>5@{-TN^dk&CjCdP7&GF zyvU23NjZM1lP4*+3*D2?n5W%)IRE6bB47L{>UjQrryJ?);umi9{v~MLfdwAA==Sf6 z-uiZO<9rWB>3YvRKci;0bd!GCuu%7AZ=l1Y5jm%f7`a2Yx60WSClc#^KVaW!U9W?& z6YuE{*V#Ee$UfAsNAqG8C%yKL9a1Co%H*qW7AOyft!`3t@$G&$qn`a#(lFvjhvM42 zs}t61ZywnH*5Sd*GtXZ?xL$A2j1#roqYh2Ywqku?zMD&HXLZk;rSFK*efu4DY}l$s z+{)<{E0+t|^vJp4%3JNL9%x^A!oq2765FqcUu+lFZ281LTu;8A&}PDgqV31Mx!iU2 zjD+XK_AZHRe?{-r;PAXzcH>hwb=hCEFOFApEb3U4@upPoT{YABrQSjb)!VpLzSels zEU%oSURIAdUBW%zjaGTLpL|+9I(uP5o&0sy9$5Lfx^Bfb56=zhcK7xoa>pv=S>WHdS-&V>usB_VpzlW`@QfS=C z3M1>x3emhjUpZ!ME1Ucq@7^4}B2DFkhFUc;Y@5^fkITPRD$~2bslt~_Kd$TFrs9e* zd%{{?T0U~clC7n0bla3;Wz$6tt+#7S6s;TgYx%N`o!1}lQFKb{w9CS-?i;GyJy>wz zow03NPOY-nY19w4jq1C1dOvE)hV%hLbHx36bI1au=X z*8^fMXpbmJg}er$AT{!uh=MGM8jS;@ARF>phyn$DY9k81MSC4YL4M?ph=T8t*F_Yh zLS7G1Fc0h2M-U0 zqTmDaW{83X$eSYy79wwfC`g06C8FR;BaNmNq97gGTO$h6BX5H!$bh^pqTn6!c8G%9 zb{b84M1gp7&;e199sAZ1QBVcB3!ak0{87d;p>#JMw{u z0xj}Eh=Lr*2O|nHV;_bf3bG;}iYVBq(P)Mt3eF=pz3I^=c#Xyph=T91?np#|3+{_i zh=KxWHy{cMBKJcSbU~ldh=Q)j#~=y{A|Hz=D2sd?qM!!eyp2Z`=#Wo96a-@*CL#(- z+i5hD5CvtBPev4^DWuU%K@{A=KKLUF(juRVD2PQq4N-6%`E*1_^AH6`@#b_sqTmGb1&D&<$QL3C4k2HJ zC^(2b5K*ug`C<{p-^iCB3N|BOiYVBGd>Nu(H}d6(f>7iu5CxSnRx1$&Iq}#CLKI9Z zq|vNG6!;@wjVPFkd<~*tI`XxMf^V?yIz&Nr9Q%4ifyg%?3Pc`^C=mHZM1jaRAqqsk z8BrkeErf)bQA15uC}`A$T^Xym&P1!IthA__(#-;F33 zg?ta9;AiA}5d}XX4?`4)TH%NSk?%tkh&%#OAoBf)0+Am;6o~vFqCn(_5CtNSL=^l# z_TBg^Xb!&3E$_LY2z?nQ8sUYTD=<+T$FIV~C+KT1VL)Gp2{HNxOgPauVZw#J z1rx2%w_)O2^c|RJi@pmJ-=OcoL^t$(nCOmv02B22{{|D(4`G7(cbK4l1QXPcVS@Sz zOi(|CiEaTr-ZPls+T%4-m0_eFj3?`_D!vysRn4lgB6V#(%f_gMe zP>+EL>aj3EJq{+Q$HN5m1elu8u@D5+piY4a>d7!c z{U6`baEzeiHRocO@88eGaCFVN7=f-i7bDR%=VBDP=3I7n8?0GOT96cW< zMxYnK#7Ok_Ffj_f5GF>W7s13B^kSG8i(Uc~k6)-Ugy%Hw) z=v6QwK(B@gA$ko=h|oX4gc!XRCM4)}Fd;>+hY1;a15AXVe}oA+dLv9I(3@aFiQWto zq3A6zp+awk2{n2fOlZ*CVM2@E0TVj(PMFZ6cfo`Ky&EQs=shrDLhpqMGkPCPSkU`n z!iqis6E^fgn6RS{!Gr_-6HGYKhhf5nJ^~YN^ii1bppU_X7kwNi!q6vRA{_lQOhll6 zfr&`;NtlR2pMr^K^l6xgL7#z%SoE(j5r;kt6Y=PCFp+>h4-<*#3owy{z6cY^=u0q> zg1!tBlhIdTA{BiVCeqN?U?LrT9VRl+H((+YeG?|K(6?YB8+{ulrl9Y@#8mWMn8-oj zgNa=9eVE8YKY)pR^lvaxfPM%Qh3Ma5VjB7pOcbFX!$dLq2~3orpTa~b`WZ}=p`XJ9 zJ>Orz1ocappne4t)URQJ`VCA_zl90vcQ8Ty9ww+izy$RlFhR}U*aB>Uf4#s^{6Q(i zd^~4|!NdaeaG3ZWJpv{cqDR8SBJ?PjSd1PG6HCxzU}7nHEKDpzkAsQj=_Y2cVmDe36MN7GnAnRp!o)tb z2`2WV%`kBQZGnk{Xe&${Lfc^CC$t?V4x=3~aRlvziKA#2OdLbIVd6O20~06EUYPh9 z9R?G>pu=I}Bsu~nPN5@V;xsx6CeEOvVd7VG3{0Fw$HK%pbR0~aN5{j&1#|*TTtp|r z#3ghROk74M!^9PI3QSx@PlkzW=>PbZhGWEa98ZUd8|Vy}xQWh$iCgF_n7ED3hKW1q zDKK#tJrySIp>ts3J~|gB9-#AJ;x}|YOguyvz{Kz9LYR1jo(2<-(M2%v1YHahPtheX z@eExG6VK6QF!2IC9VT9)XTZcObU94CMpwWDeGjdK3F<1Cpq>d6)YULSJqsqNXTt>b z9GIY<3-1CYez^CCcY{*&e0UEiLoa~$g6Zh*;eB8RdLg_Yl%p5H2S5dSF?|Fdbo#OeGew6@52Q31DK%x4JN1`!UXm2FhTtYCa5371oab` zpneJy)X!jo`Z-KczkmtqmoP#73MQyu!vyskn4o?O6V&fug8Ds7P=A04>OWwDn!T|F z*aH80fuUKre*+EtaqkaDfQIPda3pAi9sx&z#^{l7G-!ez1;>D<=+SU2sD&N_$AM<( zv2Z+SjvfamfEMWSa3W}lo&YC-R_KXvGH8vS1gC&DXg)j{v_%WxH1H)_2&aQ~Xc3$N z+M~sACI~=F;4Bb`mcrTKE3^!r0y>~W;He-8Er)YJN3;UY1)b1JI1hA2hr;=w3t9yi zfUamYTnM_MHSjdh9j%3nKo7JIE(Twt^>7L3i8jCleeE*B1hok!sLe1zZGj1DD@;(^ zV1n8X6VwivpmxFpwF@Sw-7rDzfeC6aOi+ix1a-~z!}t_IZ0!-J>K=eeI zuAf2ZNibbMgVB7LuAgtw|M5+LV{|QjhvPNZ(hzjbwKNo6b1e-+*IY}((KXl72z1T0 zG!k8NEsa9gTuYLf2eNe00sV zBtVyD{Tp)0~6G9 zVSqn`&8wbAoo;&b!@n4tgR<$IWDfnEp`P0)*Aq78a6Oms&tfr$?2r7+P4y$mJ> zqnE=(E%XYQ@I$YJiTdbOFwq#j8YWt!*T6(`^bauc6?!d91f$o%L^t$$m>7iK0233? zKf*+N^hTKIiQWVgoza_Nq91w-ObkJ9g^9N4Z7{*1x5GpbdIwDONAHA*;pkm3LFZyO zOi=HE3F^HtLA?(qsQ1GJ^#PcmJ_r-khhT#GCzzl<3=`BxV1oK6Oi&+#3F_l8L45)y zsDFkD>R(`j`Xo$HpMnYM(=b7O1}3O~g$e4jFhP9|CaBND1oZ`&puPwb)R$m_`Z7#V zUx5kgt1v-*4JN3s!vysWn4rE16V$h0g8DX0P~U+G>bo#OeGew6@52Q31DK%x4JN1` z!UXm2FhTtYCa5371oab`pneJy)X!jo`Z-KczkmtqmoP#73MQyu!vyskn4o?O6V&fu zg8Ds7P=A04>OWwDn!T|F*aH80fuU1z4+r!eaQ_Aq2J~>4Frr7mgb6(oCd}wjFkwND zh6yWr3{2S2V`0LM9tRT+^mv$Xq9?$F3q27g+~`R#;X(6Z!iyHbL>O8K6X9qPOhll? zFcFECz(f>U3O{*_pKk!}+0akX26PDe8QO@Jqo1QqXa)KO+Kg7BU!pDOQ1mOb6|F+Q zM%&P8^c%DttwFyego)$$u~{$?faBRPv9LFO9SIYQ&{JVzF**k(mY{QC zVktTgCVHXsVWKy>044^Y3t?h19(Ni{q@asnA{ku_6XHPpIua%f=u(*IhAxANuhG+C zq9J+)OteCm!^9$V1x&O@SHeU)bQMhOLeGSWZ_w2+(FHvVCc2_$!vyU=2PUZJ!UXj^ zn4q2y6VwY}g8F-ypk4?Q)Qez(dNE8;FM$c_r7%Ig3?`_T!vysTn4n$>6V$6K|Z&dM!*)uY(Ed^)Nxb0Vb$_gbC`6FhRWuCa5>V1oak}pxz1-)Z1W!dOJ)| z?|=#FoiIVY3nr*{!vysnn4sPZ6V&@)f_gtpP#=H^>Vq&reF!F~e}W0>!!SX81SY7D z!UXj(n4mrm6VxYQg8FBep#B9Ws87NK^(mO3J`EGpXJCT*SD2ta3lr4mV1oKQOi*8d z3F?b5L464(s4v3=^%a<)z6ul6*IVd87F049uRAxz|=MKCcJEry9&_|sG*FcF59!bC z#A37(CYocDP?#8vKP*ZG6Klk{|A&bH{J~QinBb$eFkwdPU?LH%hY2nL_iZrI0w3Q9 z6D`pum}rGI!^CvFzXc{*brgZFpB#6ui+!NeT=rQvRv zXp8spz{J=T-220X8XX1`o6zAfF%KO96KBzpFd?3ddw-Z1lZtzPn3#`_fr&5iSg|ls z3%_?82NOngJWQmb6JX*HIuR!5_l%QZVr&-fiD6;`It3;q*|>*>2_^bJzNO(9;llBB zm}rO3Cj%ya!0}9&XpiGrFcE;xhKWydaPJQjuh3IrqER01{b8aXIu|Aq(0MSi1f35P zJJAI&aSdGv6M=ZFX)y5`$BSS>l8<|Tm`FvJz{Jx6-221COLQ4bG%Cb(0~4|688Gn` zK7Kh&{5%c!{xH!2$17nX2wepewqo4-!^Bl|HB5BG`^7Uw0~0p%4=}L=y%r|o(Cc6#9=#qW?x8op#5wejFmVCB z5hi{?Z-R+V*mE;X{EFjSVB$J@D@>e5Z-a@TGTi&aL;!jROnkum?1YKV_?Ww3;!C{G zZkX`lefGdaTO8jD6SdL%U?Lp7A137J12C}=eGvX{pbw#mtLUF#;w}0xO!S(Ldw-Z1 zg+2-sW6{T8qI3rC{bAzrJKX!jL>GMiKf}cAa_j{YYtScQBDezIZ(w2&`ZP?8K%aq$ zN$6i;LV!LC6C(6En2@2*!-TvN=Lse(=!-DX6`#)~nCOPS3=^HIaPJQj-EsUXOw7UY zYcRo|iE{}P7tl9g;xhUsOvF^<`zTCAqHn`Q5&8~HtVQ32iEZe6Fu|LJdw-Y+pN)He zm~f$ggNYva+#bS2%Q?9Bhl!KuM=(M6(vM++T8jJYC+Kjz1|ma0MMt1R(9h73XgT^h zItr~ozd%Q$mFSn~7<4H56*?BJLcd1Gq1EU&=y2>&;$EDZd zYaW+ghp%~DdL6#zap`sVn#ZNr;iY(dg1&~xV1mAegun!S4UxkHeGO5-1ienHgb8|0 zI20!6bzl`t&}*S;n8?F@uLdUCxKzG zv>(E6oaRprr6ZJsJKbR%Oxj_?o=rWjChMo=+KJ*Njm?FcyKTIg)xc7$%3%U{}R-&t5 z;wSV>m^g&4hKc%M)<0+f&;AE<6u9p}6YJ1(VWO@Q_x>;;LC=Q?6?y?oXwcuogbBS6 z{%@ccp^2^N#V`?~!o4_5>_#tziJthi|1y|3kK@Z>;&=24nD|nSpC?Q#Mz4a2t?1P- z@d~{LCOT#1!-U?9dw-aijXnkwC(y@Xf^We; z(=ouq*SDWxqAB_pm>7gU2@`eDr(oi1^l6y*5`6|HI-q}r2@CoxOvusaU_yyL4-<{R zg?|u*;}_9H75WlP%tK#>iFN2JFtH1L6(-)Jufaq;8}9vKqRTscU4{uOj^BidZRlGt zA;#DI+c2>k$M3*IEgZiK6C=_0VB%-=eVAwr9{dBl1NZ)Dq6v8T4=Qo|cQkPm{Rk#F zC+_`W!i9bU6Qj{jVIm6s3??R{pTopW^b44{jeZFeXI;1sU}A_H_x>=UM!$iHDd@K_ z5r%#T6Bp6%VZz^o?|CpW75xWHgrV6RTj2l70@o)rZ*W}>OdzazgPI?6J?Y=O<{0ze zV}a{a|8wW&dfET_{poM7xIUNuT`gX1o*&Pj_Yv=7-Y598?(6VA!=HrzIqwTzJzjlY z171U3BVJ=(6JAqZGhTCE3tmfJD_(0}8(v%9m%Mho_PhXIAnz+)2mD9P9eJI2oq1h& zU3uMj-FZEDU-NqMI9@MaZ(cC35APdZUtT|6f8GGzK>RHggL&WbzQe03LwUn^!+9fk zBYC5Eqj_U^V|n9v<9QQ!6M2((eB77|c_N+|H;z)Cj2FU_^Az}-DnfZGo|>oOX?Z%H zo@d}0c_yBjXTi5f8_&*j@SHps&&~7jyu2`8I4^=1$&2De^I~|h_!}eL22dU!E`DSKur3P4gA`ihU)%QeT;G zx^ITB+*jeN^i}z0`l@}ie6xLXd~?0k!~JfGxllU<?0k!~JfGxllU<?0k!~JfGxllU<?0k!~JfGxllU<?0k!~J zfGxllU<?0k!~JfGxllU<?0k!~JfGxllU<?0k!~JfGxllU<LwtX7e{r+9Ih-GNh5r%vG4~1g2iLTHgZA}(gMC_GP@vk^ zv3*ec7VTTMZ_~bQ`%is!d`-Bf+#uh#K9x`7`;@E0ea6-0KIgvR>T&hC23$j~uI~%q zTHZO#_lDQjrw?r7`_g9(v;}?{*e7s!$Dtj6;Pq}F+}=xMlpMFfY0Iq#lM^8s*$M)?5+Hd^t5BDVKzvtgL8u%Le8u=Rg zn)sUfn)#ahTKHP}{>M)@z!&KI%Gbdc3`F~;_K?`=Iie3;rrUx)5rOG`Fi_; zeSLi2`1<<#`TF|?_y+#dM;PWC?i=A7=^N!6?Hl78>l^1A@0;M8=$qu@`vg9rPvjH( zBtEH6<_q!3eF~q_7y92n!Z2<)H-a0%t<&Y zC*wjmIj7*1TqvjF)SQOXaym}W88{?_dvvM}h&N(E=CLT(Yam|Mav<(6^FxfR?>ZWXthTf_apt>xBn>$wfw zkK9IX6StY$!foZYaof2a+)i#6x0~C;?dA4y`?&+$LGBRu6L*+9!X4#~amTq6+|S%E z+)3^fcbYrH{mPx?&T;3t3*1HS5_g%q!d>OAVVUdP4elm)i@VL;;qG$xxcl4#?lP;#cZd<~QAMhF`f~gu{(OJIzj}nA+CiTOH3(`E)Fh})Q1_q?L4AS-2h|Gl3#uQ~ zIH+|{^PsPSf`hsR4GNkN)IO+ZQ0Jh2K|_Mt25~__LH&b<|8Ma%_+RhK{w-U8Ex;D| zzq^3GgQ0`5gQ=)cOSQIP|{vxqOxc+-t-6_UZt|v_T-A@-I;INi zSo@(Fg%twZ9sXy6NrKseal+>z{gh{vwL!VQhBA33&$3u3}!J|wJ4d6Kd( ztzMQXr*^)qII(z;WCni`|C)SV+MePk@jU)B#R6~t;@*j!GxIZg&Kf2Pm+X+Hxg1?IV|Yjje?sx|azWTmDwe&O!o zQAPh0%TMf6`7A?~qtAO(y3VsA=1A;;s>S+R`FVxwW=wOel1vftc#ZJOvXROSs@K}i z`emkwbG(Y(_PkW zH@>%Yb#`)XaOFqsNe#*EKD*xR{d4B4%iT$t{-O!$jfR8iv35hGlK);1FLCI*cm;_s zD(do^NgnfMf^P-$1yO=|;wO?iA@!AeLN}^zs7@wKwa&6_v`uuace}#(MinOPOc#CowH|l>eG&25T9ANo-od03fR_hP;I8S)^#F#0u zF)52O-c8M)F|B-d!86fLaWmz5>3UT&jopypSmP;-Kbj@aiOgG4wt1%8v$X2Ml<(y} z+leXlN;Ab%L;R(uwKt4g%!BRy?b}?DQCs6(i8FIg<+=*}iXIhzKVy2iqr$3MuTF7H zs^nKzo7yLJbMknp&W-jjX6eEr#j|DM`rnO%JfC<^dR18saz_?5;SZSgTGHAv(3w@y z)vgxpOI(+Axul>>71DWjp#7uFURjHhKNTOyxEnp0pDCCpI;Hwdd|rG{{6RBQ)>Uy= zS)i&^T~bAA6SUv!+ZjxTorX&$qvfPcVt;S{&Lwfl-GN~hVWYx+h;9*cE4FT&FL8cS zK=Ol>zNt@BXQx>+=47tVJ(jnkAgfq3y=+Fwj0H0+<)Nkd&(PDN-GtWwFLD95t3Old&qnF7-b#RepRvhbM0j9G2LU+K-)RXO-no5 zQTu-T-{bu2oSgQ6tGPSHof#HK&Z%jV(>>y`fWj7Qosv)>fo zDDzdwt3qc!o?1lO^q>X{8!|U*R3eK*_nt@~8)KcQe*ZJ6QEo)v2mDQ7eOX z$`7){#8}~J)tKV!^4|PQ&U+;y^;q@a_H3o7=-Di|p>Q z=i=7V71@pR7N#DbcB9xT$;la(uvf5C*ibxDB$4%y{}z=m7b%UpEM1H~)?hO%GpsWW zG`lTf*5UTXUTe&^i64?Drc|X&nCwcempMN3_>@ocOnKJ)Kc}^-ESkMJ<3!Q@Y94Q@ zxScW6ygRI_e3-pN*xr@smWD+q3R8yWe3kd9?KOXZu%pl-OcwPQo5V|G_2nNaj)lJ0 zt~0zfyf8+Z+Bmv6QzBPJejU{$J}f>V<@2~@Y5BP=^KTZMnr14FDN8JuRW_a(KWBt! zu4KI=N!wA^+FR_+iux;RL`uI5Uf%mWVL6{)S9n{JEPWqxQfXE_)2z^T()BlvvaGR% zcz=mJlDr{je%g(+rV^Dd!5TqJVKd=4ky|uZyhZ$%c&6l*q@nchasKN`b7dm= z0(pv3s?w;6RChG@G;cKV+Dz?C?IvvtomfB7P+>f8Tw)qzscju$eQEXJ=D)&z)S+{C z^*B5|y|cZ&W2VNsh4_u+Q|YgYH^%E>&H`Q0mN^qdgT&*d ze})WAj4RR4J~d}#RGqN!^oB8i>MrV+&b$=4y~ynHt31Mg8e*33RD_1w^@j}gtsh$l z*w@3Jyy?mW65VY94Do*s9!7(Y51+dF5r-a)k0F+HR&Fg)4=H zBA)HARIAvg{6Xc=O)}0meQS?$edm`0sK4uc=-eSDWved)T(RmSvwTFOEH1J<;@W*^262v0`f-ex-1)Btm^( zUurg4boRRuN20!psYuG0JYz~`{-uKWk_J^PEi0_~u3!0Q_yYy0f`Q_73h`^2}?3RMrw{rJx$Z{)p{heH>t za`ma+kHdDx{8lhm#ml~)6e)aZo1I}$kI(*ca=nZyk-yq*vKG#=cq6_`XrA&>THE|1 z)4auprYtXRCwwm+sFv80!@r4~Sn^Xzx3W7KdsV%3@AQNCXC*nR3C3QwJlBKB>rr1P zy3)Td6ql4&H4>eZIHf*Sj%k2%WeTt2V#o=J{crO8uTb?;U)4Mo)H3{K?Csf|a5Q0f z%B0DsQXi$a%P7toofDZ`oF6@{XHl=>2_^25gXObIcJl>-QKDgzGF?668QU;-U9Tys zSJ|%bwhP4b(S_y zdtDo@OEqR0FBuC=ai&kKpIduaW37{I3+(Nj-#E9sb3Apt3h&^EjuEzqLov1DH1V|( zED28&t|vB3T9P~_EYRR%6n)p>suKYS(;djZ7<>`B~D8IZt`d8D+|npHwtyrmc@S} zzF=Hr8g8yM_Q=aj+?}*C@3->Ul+QE9O})S$D;gouOTJgtRyR{$(R4J9G5%m~Z0}<~ z>>lN5?m2B!hw&p)B92;86IUf&O!<49|M8QTrY5Gh&KQufINO_dqTp3wd~says?w6u z%o(ezCyBFU{lof3*hSe1vdQbyPb5za5h<3cmz(~w{%H+$T!<`+nHEzoAtd2iLPc`> zl#wZ)OzD$1sN7K8e(pVCjQE)3rlgK=x$Ln*p!_y6Uu9OemE6+5(5p?ErXi+nCLZs& z`GMt~^_J$i<4;G^usadC{0A{_W6Kh+CI6fn@|&f1>!A^Sj1cwwz#PjN+A zn~Fv=e->6KrWsDBf0O-bVf*5Jf@t9wag-!XKhC7Hb@#3>Y*&0vpj96!_}MZuMJy@L zi52ZLDO4`iXX-D^BAdGSEWeeqt1{dz%KJ{%RIy0;R4*~^v3-*va||utU)6c0MDVfV zFI9uIwd$VQY+ZZ(1VbOA&-&DT)q5f>HT_IRe$M6sUh&t(dF7YpT&ljse=6yu?4DFu zGO_aA>?__*;S$M6S++`HoMSv~T;P5Z78u<;umAKtVSnZI&%7skJLNZA`+tYG2a?eO zY4Hz5PGzt3KQ+H4?kc)y?^p0Pa_p>coj>Fa;435_hgjtYj0;S6EqvDvkIK6${88MK z#N|obQoc<)n09o^j67A@t&l^C?{upy<<>OkkFI_(hw?fI@5FKP);fzm(W-a9jA@nV zO?{c(V(O^k^;L(H)G_Z8MM?X!c)SM14-!ff`)1w<9U9kz|D0c?x~|({9Bh4NyYF^Q ziOU(3$1izN`b$-FXUFiaQTH{|;xL$J<=oqvU)^y}5VX3iC~KjOC>_E4Fo#qxfXmR_}A^ z7TrSgq@qVfBTM5&E=3omOkJV<;3=BiCT4rV&1tI2E#YkiyIgC-xR?*+d{G1GHww98 zrBbgh)m2#3wg7KZ?6u5C1xCP4^%BDz^Ov6Wad#45 zruk<{inB|vt3R1NL{gz!ubbsbh&09RO1PbL%JqBXsysit!TDB{p4i7RUj2I3!;Ax} zaV1}d$A^Te59zIz0(WS^u;Rv*BIzOP7o}sB-xp_#Ch>n0Rb|)9SA-{;CX^W+8>;*# z$HmAcddIbCx5_o{0{&oi1Koq7+LDt6xoHVgeA+vfD82^j+!e ziYjs!PI;-xGp;gE&5g6&bbsgR8{H>alr(18{vX>R?PdZrQh-g&#r1D#n!|IH=9s5iCi0sExx=KWnj?zr!RBf_u zoqn7#!MMU$Y&qjP=3W=GI<_SKe8SO`xYR{yD{{8vZ7WDE)>d4qtn2KbTqXQYy;?Wb zcHZ?(be*_qsegyF&*Kf6d&Jf}aeyR0=Fv>6Zg%>!%*wREx&u06%;==S#WwpxWp!wS zl*zH(^Ir*^x|Ys*z1|=lR#=-IUhQW2&VhKkY1SGkvTv4zK?$v3RXsPv6LnNy7A* z=~Wr({CNd;3!)3#l#eW5Em&c;y1vXXWI40%7uQlt97`(`v*g+J1!3Z4x>4FMY()tP z>7V3I$$jN%u9F!zyMN|8B1@}377vm}hpbmDG@}IpR%>+ zOwpv`U4p~nE}^w_t17MtnrHl0>L-_(9IkFLmvfpF4Ntr#E>p-1pBYzMkGqB>Kgj$_ z_*U?dWVE!GVw`fe>Z$sPx{IbSd&hU1d^K%-#^+hT zXVsqCD7S0g_=3g7r%L~sFBFD|$IF*0ztx`CUC<3NZm_nt{bD<1FBJu( zEKggS&dV<-*i|sOw0C)XY4^(Jl3#^t$x_8Sb03>6(i%59@ovdJ@qG6b7f&%!D^+&4 zeMr3_IvC62In4LtrPbT!9F=cTcCya2op3#nUU0~?v(jhSmRJ8}6VKAv7xQlkev)i4 zcC)Q;?=M;<@J!w{=M%oskml_c);K##+SN9y`h{jj(n|F~Rhy~fy(hDpBz~G+r7TQt z=x!f(K^&+Ih|on|itAKim|fSA8rmXsk?0F)inN~fTI`#m%F>s!ep6~94|peu4%>I7 z{GMX0*g7ZP9N?^-nWfaJF1cDqJ&#?N(mbsytGB4L?ZTYLu|}Iu{83d0^&sQqDQPomNp$>;qBhEbk$uxnlmzK+#$`zobp9zTvxD;rr-f9!u&vIPOs`}3)Ud@Z z#?Srl@b)OLe^F1FNYGVuNPJYXNR^_Ur_V5)HPx|QwAs7`p1x82qI$&$6MoI9%o|@^ zQv9lTe|h25rpg??Qc#bdn>VDWLFFo8vbJ2gMYUOb(e&Q5$9&m3-f`Yh*R#<(J)(2W zn#8Y!)bQ9} zi@z8D6uvj-uy%sJ*zi@@xQOqPU#5hm{Tr=7XsdkdZv_O93< z9u|5;>#u8WOLk0mI6d{ljFEptK1!;UHaTs=)Dy)XQQ4fVg#5JLWoPqx2n(bUc}~~< zf=|*<>gU;>&#vNE@*B)@*u-#{HFY_{9gRN{K5Po{Nel% zzJib6+~K$#c6^01i!gyht zaHcR@xIws1_+Pcjf9~TR79J6<7w#3F6d5J!nq#VO)K@pN&iI6|BwE)g#+T2i!9yh^-X z{G)h}__+9-_^SAp_^$X-<#q83@hkCL@jLOyl20Z5W4@3ykkpqnm9&z4CFv>YB?*@F zmkgDRmW+}#=Z}+2lAIN3B@T&K(n6UeiIZeV$|dt8izSOBt0g~3HcIwM8YN#Szb5&^ z`Z(gT0s&C(!SCz(za5*v`88wohH>uBcyTCaA~PDNtz~& zm1aqcrBkGbqzj~*rHiC1rTe8nNtgajUjJPo-78%v-6`EJJukf@y(_IRYa;to`dIo% z`d)fdIzTp5W|B>meJxYVM#(~Ca#?>_uuLPXlBLPkNb_WKWl6HhvP#(;*=kv`EK{~j zHdVG%c2Rafc1^Zlc3E~?c1G4D<;-UfQs>3`LG$1Bs7vz1!G z3c>fvb;^Uv`^wA8LgfYJMP*QEyU>QAZ9?mWwhA2@>JA+j`b}uxP-CbwbY|%G(4C=2 zL-&U62)z}0F7$5bgV3{~??XR?ey*ykYNqO~`X#ihs;}xh)dL#o57Gpb)yw^eslH&s_vcT^8mPgSo~FH|3? zKT&_G{zBbY-CiB2?yT;w9-tnk9?Jk&hbywcRw zw$irLHq^G(_SN>&j?|9O4%PCta;;RW(;BsA?HFy0HddRc-99H(J5ReuyI1>@_80BX z+W)FE`k(u_XSKJqSF{(km$WyvH?;S)5468)pJ`ucpK4!f-)jHT`swQEG&;4euCAf3 znXZejhpxYFm~O0YqK>cYsT1i!baI_iXV7_cQMx!?rY=L5s>{>m=t^``b#ru8x=P(V z-BR5u-FDqs-3Q%m-CNx=-4)$>y-ydQ*Xwz_mih+zF8X@qqM+=}r0(db57G-mQ1(mHKpjq5hseQ$JImrLWeP>nrr7`X%~&{SEzQ z{dN6m{RaJ0-46Xh{g3+d`bmbShK~)e^>6e%!%O{py`Le#@In7f|AnERVT|EBLuW%L z!+66G!(c;qLm$JphT(>hhA2avL19oDLJclMv?0u3F+>=228}^th%r}0BK z`qcEq_{u0WeP^0vYGW3d%1zTuX{HsXSkw2WB2%tuuIWdU3$On>Ogl{vOuJ17O}9*k zO{Yw+P5VvF%zexsn;V-un){mDTAG_Xo9mlx<}h=tnQ!iDnPDz7FETGP=a{#fFPcx8 z510>`x0%nI_nM!X@0sglTsPk^uQc8=KQVW=yf(iyKR3TO^DKXwTUgpyKC*PMd}Zlo z>0|lE($CV@(%&*6W~?Q|BC|}gC@flw#u90Xw`5o{EqVFXmbsP{mQ9v@mV=f(mi?Av zmQ$8XmJ62amK&C5mPp%YRzK?pOO)*!YhP<0Ya6S;I?_7MI?SrIYOH3f&1$#mtVV0N zHPf17oo3CoW?3t(v#ooqTdaqzJFVNT|5a!7KlgF>TQ^(JS}$6!TZdThS|3=ST3=b; zSnJ!G*}k-Ov~{xeulU+F#Maj~%+}x5&o(%2Z(D4e zZ_BoAvOTeFu>EdZZ+l?dY`bl{YP({)X?twjVmo0wXuD^d<^0k<+TO)J)IQ1H+s?=9 zf9vht>`J@I9%oOr&$SoXSJ|iAXWOgonfBGrE%s>pkM?=?t@iqkLi=QUEyof2r;hXX zKu1T%4g2Sg2@Z1i|_Pp~n^?v64(R0Xi*R$Dk(DS?Js^^&JzUP$ZfaksE zg6Fp9SI=*r$DSTuulH+jOK+rC=l#z6mDlC%=8g4E^0xDi@DB9yy&b$hZ(DDFZ-#fd zx6*sqyUBanyTiN2yWhLcd&c{#x5&H5yVpC{J1neW*u?PPy>GpLd545O_x1{V;_V%F z%=>pZ`|NXI3$O*)0&D@c09$}9z!qQ&um#uxYyq|aTYxRV7GMjo1=s>?0k!~JfGxll zU<? z0k!~JfGxllU<?0k!~JfGxllU<?0k!~JfGxllU<FNFV8!_ zcV>1iQ{Je&v3ZN~*5^&mJCV09?`+=Qyn}g{^3LZS$$K>a@%%{Mv-$VsKb`-3-mCeq z3=FiEWo_`{LOa8HZ3_FznXu)s! zuNAypFr=VIfw~~GU{FD)f}RD#3QPsH1!bOF%3)d8`FI-!=y>NFSRY(+)g|`ZC6h58uV$owouNA#e^m@_PMV}RY zSoChuFGWd3$wdc)sYSU(*+uz9rA4BmilXwOjYV^cwihiaT3N&voi93GG`jeq;=JN_ zi*FWvS^Q&hkK$j82Nb6jwHIQmx^x|-za`i^nmCg(G#MlL{EyEh@KNQ6}>0=MD&xWwWzbGgQ&fz zk0?>pTa+Wp5M_ymiiV3yMPgBnNF^#3X+;{5LSz?tMFG(`(HPNc(Hzkl(LB*I(L&Ku z(Jaw6(Pq(Z(IF8nIxhNM^oIz;Zj0^{Ulc_|mqk}ZkBgraKPrAh{J!{2af0|Q@jK$L z#h;6tiQ9_Xi4(#e>Az;v8|VI7>WCoG&gEi^Zj4iMUFPi`8PC*eG_2ZDNnu zE1oHyD_$sGBwj3DE#4;HFWxOaE+)mSI4nLb{;Z}}{76Zok_SsZEP1D-Y00}K@0Gk- z@?FXIC2dQ3mt>a=E$LG-q@=P$R#H}CEYX!%N=B9hOJr1zk z?k!zjy0mm%>E_b2rT0et!=;Bye=EIO+Ni8?*|TLYmwi(9aar@S^s>~l0cE|*dX#l5 zD=(`o)0PFwCX{syO)i^WHm7V++2XQIW&6s)WoOICGP>-M@+Zq*Du22B&GI+OtIED8 zZ&v@*U;-%J-BXDnDF)teh;T%DM7$-Wu2kHpcv14CC!RMdD0EiWzs3q zpJk(^Go(kQ3#B3HOzA=CF)1TGF0GYbmXgwkWRJ<-mL6P;<7giSN)>W>r++4Y@a&IMB$yJ`Jyi$3q z@~Hdast2ncuWDTNQq{{>feNgplRkNxNRf$zyt9n)St{PA^s4BavsH(6kze-$H zRwb>ftWs9hR8?2`s%%x>s%cf`E>o+HRIycus*Y8ytvXz_zv@x>g{n(cugX7^zb*ev z-cf#U)L-XJl>Z{{ChsD@T~#0-As-^olo!i!xmvE3SIHIfk#dK8tbB}ofZQgZB%djt zCtogKC|@RDDPJvLEZ-#GCf_ALBtIzMC_gMeAt&Tvc|=|-|6P8c;(oF+F zGyG%xMf@ea8u#Lp@en>5cj9m3MqGt!aS1*TFTzFmNL+(Y#INGdDbL{B@!#+Vln3$0 zm8|?EHsuoKVdYNcD&>0Re&r}-LiHcYtI8LvU#kA3`i<%))z4So zRyMC5T-~d>UG=?D{}$Ev*L1AzQC(F%wpv|nsn%9ctR7x%ul7}+u2xn1t4pe7)#mC= z)r+flR4=ODS-r3NST$RHwE9-{V>Qp!JYMsAb)@>?nzw3RsrkI-*P6~XJ(9ZD45-Pf zDX6j4jH#JaGr2}rqpz7?Goxm0&H9>|HJfVQR!^zfTXVeTRL$8M3_DSCq~=0RW7Wf| zM^u>V0ab0yv#Li`PpWR!Jf`|u^{eVTRWsGQst;AKsM1vfRHdp;sw7phDpi%O8lg&4 zbyE#i<*FW1k5+|L3e^nN7S%Gqyp3pp| zc}~+x(^T`W<|EC=niiVYnr51BHQ#GGX& z`>nQxwvV=}Hc{J7E719) zS`1^fm$c982rZ?(p*^lWqJ2WwNcW14)V`p5M)#ubP2E?zDX#Z)AM1Y6eV}Wh>!eH8 zCF$Dhdg}V9hUJFEh8>2(hW&=E zh9ib8hSP>Kh7$(baNF>R@loR+hS!Zvjn5e0Ft#*)WBlCswXuWo-l%_DV|!yKV~Vkz zG0m7^OgDaH9Bk}i6dMbT5~JLxGFBUvMx9Y>v>7c%yU}BuY#e2rYMg0YZ=7XZZd_;F zXxw5vVx*0?jkU%z#@~&PnI1AVF*P>5Y9C14QKr+T3nrEMs_7x~EmOqwthtFf z!ThATsrd!-M|6=ZG?q%+8&NkETb(`EHf;VE%PjMEsHHHEvqb>EL$u)E&DAeETrX#<+kOT<+`P*^$*MQ)=#WW ztj}8eTT`uVtnIBC)^=8h^;c_0Yl_uxby{s!pS8Djhjo#4f%TO2X!Kf6o4Zy^;MH`?vOw?cdlF>_6ID*t^ zb&w9$K{*nfA2>gCe&%fH?Bq;#c663F`#SqO`#A?VGo6LbVNQe7?Q}R@&dJUR&RNdc z&Y8}+&iT%T&PC4c&MnSO&TY=lnxoDqUHhE3oDaGlc0J;H&h?b*S=ZC97hEs9-f?~8 z`rP${>u1+5uGX$ZS36frS9@1qSGp_1mFdcM?e03%Rp2UfRl6iExvSh&?qqjI z_fU79JKue8)W5)8>@IRocF%Cna?f|Kb+2)+cb{^faX;XB)YI7WyyqoPg6DNlQ_nk| zH$9(tKK1Eh|?>E!9*ae3UHah_?O>7J#Y6P_cUW1fSaU7mBE z^PWqd%br@#P0xMar@aq(DbLH^kGOFJs{dX8JN^X!=l-_-Fa3%BU;MrO>HaLg*k9@o`_K9>`X3EE5;)_3AnLxDYkn*lCxE^s06Ti})8tHGv0QLrp14a$S+pf0En zT7!za2<{B-4IT|14;~8=K{j|Ycp->kSA*AsH-fi< zwZYrL--3-okAxP8n}iZVpM-u1bqIA0bq@^=4G5)&28J?2S)pN}+)zoVFeDB+LjKUm z&}Y3i2?y9{6O*#$vu;;$=YOZvORfQa`Tk8QvOJ8nv$53pVBhLo^mziU`mHB8C?Qh zmUlVZrAb$R*YRCfc0JM+>(;2-JKYMq*}Bc{cC6b|5r21W_etIV=w8z!Ep+N0 zXCp294ez(G-=cmy`y~wcdO+5I1p|%`=$Brdu1!ClzAydFftS;t9r)eA&j-2(J~wFm zplgHEGXfdgGm?g2nN2dAWj>nuX=c;RcQS`$re>yPHjWf!dNN05&dyws8OeMq>%&`_ zSxd6IX0Ob;FS~X2j_l_m**VWg+&NUv=ea|36LX7lWw}%?le>TDufzHds~MIz?C`J$ zhpUF?4R;QoI(*0Qo5L>;zdihi5q(FzKjNnmD@LgE=H*?>dnmtA{s;Nh`BU<wCBK2e@XA@+-dVxM@t_{B(K$&9WErAeh3rRAlcm%S8uIr2)xUiNClSGKD>T>e_* zOnI>4^+-EuU+DtrNa=oQS6SQ2QI$<2_u+%^N_+!;9dDt2A_cqt$rgiv--_Q zLZp%Un!2&}J8hYEf_Ay~g!Zk-b!|6YrS78ss=l#7Y5&DzEKgEh-K!CGQ1wT7(+tcR>St?x(n zSqbYN>$|oDTdnnjNK@M%*4J(K+wyIxwz0PLw&S*&wlC~$?CtDU$I)9KMgqq5Azme3BVH$(5^oT1 z5(&gx#M{I>#Jj|M#QVet#D~O3#K*)Z#6O5niO-17i7$vRiLZ#SiEoH+iSLN-i64j` ziDtx4#LvVp1cv=eG$&dREs0h{YoZO2NVFx|5$%Z%L`R|%(V0jhl8F?e3(=M6Msz27 z5Iu=hq8E`y^d|ZceTjZVe_{ZUP7EXl5rc^gVhE8*WD(g!4v|X?C592hi4jB|kxvv5 zg+viiOo#|EQ9_gwWkfkqK}ZNGAtNe@Dnd>u2%Jz7)kF=UBGiP2&=NXAPZ$UzVIs_g zg|HGf!cI5{C*dO8gop4FKEh7~h#(OnMiQfl(Zm>HEHRE4PfQ>t5|fC@#1vvGF^!l` z%phhGvxwQm9AYjpkC;y^AQlpfh{ePbnESnOEhUx_%ZU}lN@5kUnpi`uCDsw^i4DX? zViU2M*g|Y2wh`Nj9mGyz7qOezL+mB?5&MY)#6jW^ahNzl93_qs$B7dJL68JR&;&!U z1V@C4lf)_FG;xMFOPnLl6Bme!#3kY~afP@_TqCX%H;9|WEg}LK)DpLe--zFdKM0Jx zkG!9JfP9dAh@&oci@+0zN@)PnO>`V3|`;!C6baEg$h#X92kVD8!GK$tqG#DoC7ElGS7lsUp>+ zhSZWeQcoI4BWWVdq=mGSHquTyNGItc-K2-~l0MQ;2FM^8B1e*=$kF5&ax6KH98XRl zCz6xM$>bDrDmjguPR<}_lC#L!QU-3>T&7`>PhM;>S^j3>RGBW^&Is))r5M1 zdXajGdYO8KdX;*OdYx)Yy+OT6B~WisZ&UA3?^5qk?^7R8A5tGtA5))D|DZmlKBGRT zzM#IOzM}4p`hQJ*Lw!qqM}1HIK>bKHqkf`(rhcJ*rJ7SMsFqYKsx{SyN~GFS?Wp!t z2dX30iRw%xQOQ&a)rIOxb)&jdJ*b{kD%Fchqk2<)sJ>J`sy{V=N~Z==gQ&q&1~r7r zq_U`NDu>FYhEl_*;nWB!kIJVCs6xP?h$^N;l$a`^N~tocoT{KCl$4TDl~ffarxX-U zDXD6zhEh>#N<(QW9i^uXl#wz~X39cYDH~;{9F&uCQEtjZc_|;|rvg-v3Q;4eQPgN^ z3^kS-M~$Z@P!p+1)MRQ3HIjNZfXy;m)b||rw&jDsYBFZ z>Iij|Iz}C*PEZ6zQWQl~48>9$6{b#7r>N7^8R{%`jyg|WxJT-Lk-9`(rmj#|scY1A z>IQX_xBs2D=_lwX>8I$Y>1XI? z>BjVP^z(ER`UUz$`X%~h`W5x+(nz{U)72zeT@IzeB%Ezem4Me?Wgoe?)&w ze?tF*{*=bB&*;zTFX%7nujsGoZ|HC7@96L8ALt+HX7o?=&-5?!uXJ;|1>KTvMYpEg z(1~JJFr#Bs!T+p}Wvs>27p)x(D5pPNjR%X>@P858apUNB5@((CPF* zdJsLB&Y*|TnRFJNP3O?L^iX;jJ)9mv=h69e0bNKJ(Z#fg7SknkDP2aF(-pLYmeMl1 zlCGlVw1UQIC0$L|&?;I@YiKR4qxH0bHqs{AOj~FxZKLh9gLcv`+D&_CFYTlKbbt=h zA$lY|iXKgmp~uqW=<)OfdLliEo=i`Hx!()dRC*dcot{C@q-W8y={fXVdLBKWUO+FT z7txF9CG=8y8NHldL9e7&(W~h-^jdlyy`J7cZ=^TTo9Qj|R(cz~o!&w3q<7J~={@vb zdLO-?K0qI&57CF|BlJ=F7=4^RK@&7dQ#4I8G)r@Im_A9LqE7<`XXvx^Ir=<(fxbvz zqA$}|=&STK`Z|4szDeJrBXli&oBoado&JNynEROfnFp8$nTME%nMTYb%%jX>%;U@x z%#+Mh%+t&>%(F~m<~inhrU~-`^CI&S^D^@a^D6Ti^E%U%d4qYANnqY$-e%rm-euln z-e*2wK4d;(K4v~){=t08e8zmve8GIle8qgte8YUpe8+sx{J{LkG-G~ZerA4Qer1|7 zEtr-}E2cHmhDl`FGVPf5Ob4bT(~0TKBr(ZM3e$z@%5-D8Gd-A|Oe)ihNn?65eVD#X zKc+u3fJtWtGJ}}GOa?QA$z-yad!hOmmd)faxy(>z7&Dw1!Q?UdOaW8K6fwn&h!Ha- zOes^wlrt5Kgpo2brjn^*`k4D`R8qjDvA9 zF2>Dx7%$^v{7ireG9hLpGm06_jA6zysP_Cxj~_G9)F_8;u0>}Txf>=*2p>{sm9>^JPU?04+<><{dZ zY%}&J_Gk7N_E)w!+k$P$wqjeeZP-M%E!&Q5&vsxtvYpt@Y!aKyrm$Vuu5359JKKZp z$)>Ws*fh2`+lTGT_GA0A1K4zSAUg;!7|dp{L)c6X?g61J2rW6RkJR>DeI8C%I#v2s?y;;fRbW@}g#t7bKUW6b^<$*oy1ONr?6AmY3y`% z20N3T#m;8uuyfgY?0j|syO3SPE@qdoOW9@Ya&`r~l3m5FX4kN5*>&uCb_2VS-NbHY zx3F8;ZR~b-2fLHq#qMVJuzT5k?0)tDdyqZE9%hfQN7-ZSarVSLQh$OaS&F4uhGkif z4YMcNQ|xK>411P6$DU^|uou}&>}B=}dzHP$UT1HxH`!Zkgso+7v%j&wvwyG{cOQ2@ z_W<`G_Yn6m*NA(Bdz5>Odz^cMdy;#KdzyQOdzNd=J;y!IHQ`?1UgTclUglomUgcin zUgw%}81@GDCYQjy#l6kF!@bMB$Gy*ez3bPocn_NlKYDLn)`

    l5xfHGo z*OlwWb?16;J-Jk_7njEM=K64bxqe)KZUC3g4de!KgSiZD2$#uaaoJoBm&*<1hH=BW z5nLXZ&lPZmToG5yi8wJ=!j*DmTsc?4NjNDd<0`o-PR=PfoKteuTn(q<)SQOXaym}W z88{?_dvvM}h&N(ZWXthTf?p8 z)^Y2(4cta<6StY$!foZYaof2a+)i#6x0~C;?dA4y`?&+$LGBQD7%(`(9p#R3$GH<6 z!I2!r(Hz6E9LI&ZliVrpG1+CDcm`n6iyDOgu8^hhP#EkhkJy3hEv1$LiPXp6YBJYfeC{W1}9`B3`xjL z$V$jg$PqrZ{^H9Q5)u-=dFwy(eIpF}?|$d#fByH4?v48YcfWh}pZUHKhW&THbM!y| z`$qRh{r~Q}SNBGS@hsv9#1V)i5Jw=6!2d=B?!)fK9>5;N9>N~R8exxMk7AEuk7G|@ zPhwADPh-zu&ti?S=dkCoCfEzui`Yxp%h)T}tJrJU>sV9l4eU)U0ecI38+!+P7kdwT zANv6N5c>%G82beK2lgrU8TL8$1@E;Q zz*=Ihu+~@`ED>vqwZqzD9k7mAC#*9DKmLWKU|lfyVJ@sY)&uK_rDDCXG^{t)2kVRV z!}?(MSFr-c{MUqa?-ndKG(DGm5WdDZ|s{RlC|CJo$HgN>v2*eTi z--190eqVbO{2uohd@Mc=ACFIf-`$=BzuP?ppNdbzr{gnl44aA1!e`@i@VW3C-t&Qv zy;k5W@m2V0d=0)9Ux%;9H{cubP55Sf3%(WK2EYHk1MGL=yYSuk9(*sp58r>6e{8J( z8+4-b@1izWwotZIwoeP!=kSl*LLB{Qi0={Kk5@vO+0QN|iEYl~NAB%Z@9R z%4%heQl(TYHA<~gr_?JAN~6-GG%GEToA__@`@7uZx;O%H1mXz95r`uYM<9+s9Dz6j zaRlNB#1V)i5Jw=6KpcTM0&xW52*eSHBM?U*jzAoNI0A75;t0eMh$9e3AdWyBfj9zj z1mXz95r`uYM<9+s9Dz6jaRlNB#1V)i5Jw=6KpcTM0&xW52*eSHBk;c!fqueojQ2%U z^#Ail=b9zYM*TJUb>1RFKA5LsvA6^>z1(n@X8c`ztVf*we~*CiU#f>f^;elG|3Z54 zUyAov(f(aocU2EnPxyblz2Lv`_Ez;#^;Pw2kWceSi%82zt4Qlen@D1$ZKPeKJ^a_7 zfBl*ypDdp!pYs1~K7Wy{dz%BmAHC&hVewlT|6IE~>8Zf7<`|BJh7UvygJKa-?#~pZ>B@1+#fX}R+L_7;%B6@}fZEy0@u;7#T!Q*p%8967s9c8n zIm$_>UxeD_sEwV++&@WTwf7P|Nwhnge{QXXF*X0;aj&9ySD?N8ACCOb=MmS$5r`uYM<9+s9Dz6jaRlNB z#1V)i5Jw=6KpcTM0&xW52*eS%7ZGSKZy|3f?+Kri^^&K_d&>*th4LbKv0Nk<%S+^? z@-lh3e1d$ce5icbU7w{@!Ds67|G?+!A;n0=D8*>S7{yq{IK_Cy1jR(fB*kRK6vb4< zG{tnq48=^vEX8ca9K~G4JjHy)0>whbBE@3G62(%*GR1Pm3dKsrD#dEW8pT@0I>ma$ z2E|6jCdFpO7R6S@HpOre8KE-~;0mVVZA;n?E5yer(F~xDk2?e1b z6_kQjFbY<|DZ+}Aic^ZyiZhC{igSwdiVKR1ic5-ywd)}7DDN)sfe>kqx4>KCt?<@( z8$1zji?_qu;~nshcqhCwo`fgkDR>vWE8Y$7j`zTO;;DEqJPq%S_rd$({qX+y06ZNZ zh!4UC;~Dr6JQL5tv+*1}7axib!-wM|@H{*pFTe}&BD@$E;bOc5FU8C7a=ZeU;8I+M zSK)G8f#bLmuf}U|6|TlLxE9ypdfb2;aT9LFEw~l8;db1CJB7b2-A3L=-c{c1u8KCC zW1Nd4@Si|HtI{<%CG-ur|HKIQLX2uvjY_3bt2F;3XE)?8vd(EBOXs=44WxC*4^$0B z(p*&ruO0RmRv4BkNJj|NpKCWjH5_RN@itj8w%IZCpFRI|iA-fu8C8b=B3hxU=$~5T zsq$3?NGtlMHvf7)7*_nJDEOZw|4zKPZ5)9(0&xW52*eSHBM?U*jzAoNI0A75;t0eM zh$9e3AdWyBfj9zj1mXz95r`uYM<9+s9Dz6jaRlNB#1V)i5Jw=6KpcTM0&xW52*eSH zBM?U*jzAoNI0A75;t0eMh$9e3AdWyBfj9#H^AR|$_F6G)p0REwekWr}HqxfW8VoCP z^mk#{NOuy@7-rOta*sBz^oVo|9rd$jO;KtoSk+xCL6Z%CU3s)S%GW^L=rzWDQ&&fg z1^E$eGRRruIAk*(wG-Uk?HDHJt)jL)fhTRN^j)2Gp3yd&o#^IFliYJqKJZtFb?5bA zq#x_MWA$gs4EfdC{w#;gv>4VMMxBhZnBrapmVI4n%QUyscnLgaB6&Z}F+GMI)idg7 zj9hmPNM{*Bz|C^ccF%ElbIt|HMLnywLN_?_`r^ z8g5<+^2Km3ahD+PQHJv{$3^ai?g;|cLyQ^5rIuyxVh786r^=VRSAtg#AWmCOxHh8v z?nGYUUJ6lGLqru{36p*mpFt~oF8^1shel2T3d3UlCiZIgCKTzo{rVemOk?ffFTdN@@*^&WiV}cFC-d+~CagA>aXDYdsK*YJVGHX<-a0DVtR}6! zO|uQHz-y}o!h+7%O)3Hg=Y;#|Nn!;bP z=hyvMzyQN`gMKsI`4JMvbc1dSa68?3fKDOYdw|;tcR?fsm~25Pjnu7(nMX8A^lA;6 zd7MVaWi(2iEXh0%qBgwtqxz=m$5<#K%D_k=+E+!912EQ6_hD!;Y#(Z;Sq?&f#C<5* zg71FlJHvXgIgh!IBa7JhtxRWlSstL@!@&V}9=zuAW1V)3W9rVh&mrpqz(Dp z2n;*xmO!OR&|O8dxuQS8M=gmQ1dPINQRJLsn3Z;0Ku@?&x+(V#*C}^8Z*?al2CXD> z3?}eRd7-4C@G#mk2|TZ3E*c`V^0w?TGM-*!MH!aW@9 z>#w$+_JSV6B$19h*8%$V!nx8@*V&TlEWv7wau4QkzNVK&jD|j*_0A!H zfCt9z4Yqw_Mv;Qtfp%APXom31mB_0L8rlQftn@c@qXmyl96u)(u ztBQ{h)z)2WKvQ>>d&ZnzgXUG?kqYRlO%bTS)I)(M9+4sIDo9 zkX->5ax}^aGY<4*$3gQ*u%4h*BHtR+rXi%NJxa*-QcV6D6s10+CHBp*rc~hJcBzpc z{9Rw@gDAchNhX+Y8|-?WC);QOY-2CRYCsm_(E2@RFnUa$WT5qYBn-0v)kB!668W12 ztwVp*V1V-~df7czLGE*$H2FMAt;1vUjELDAQ9Bn{IeE*$JVsS|zh??RM>nqvc)UP) zcrNM*d(~BqYIj4w3ay#T$fpL$9HtUmwPB=Z6nK=>!m}CaI=IL15pE(pM+27xqpBif zffB*Y7Q>0w25b-TdvhFb56_*G(J`k3lH)xSwX?uh4I_n26FjVA3Rq1vPxNeu+D`IJ z;^z^yzjJlgPX()KDBg6>49_8c1>e#ib!sA<^BStr1;atM~CKKH|8O}67b9g%f)an5uz;hv~qAdnm^;0*_L_w!a54W zGi-%txo4E=gnpH0De_;<`y95oTk>@lz;=^o9;(+y-ew|-x*50&5bvK~1~j`ZV7C*ZMEJ7jB#`tiCLcMBT3bB>`*m*~o~EVI}0b&A?$0!p0m z3P_wlEe8ZlkkdZ!8>Gp%)auvhSrqe-r@8gEWs+{R>9FUZXSIg0%s1v5+n}7x+8RR+ zL^vXh7L9xqxNO}Je&whPS-NA^)|wf%rOxICk1Nx5TbOOsy6#E>84G@-E++=JBdBf+ z$d77IcnA;GU>w*9z?bXfP_Zcv(sRs1LX_hmYsEiLFT%>_tc2dopAH=IKY~_g{m2db z(|3)4VYgv^S7UHH=Q#s5qae$Zo=cvyy!{2wTKzJV`9)rG8TzX{ZH=5a41}CcdBUD6 zAUn^?W5>3{toQ4nZ|S{>tZsX3plR;Cg}8p+s886CBJ9f>e5}^qtLj#u9iz^1w17T> zciREV+Lq1$L#dM{K;^~{th+V zI~1(P-4*YSK0ij23-k!Y596gnjd?NpJ6?BFg>?&9v=wkm;FiIC_tiJXqh8kw>QM@|Wq)ChVP0WB)!WN?yK|sX zdaJznU4B#fr|KS9&-2kaRp(g~V^{6XKyg)qOe;_tfvO*|u2+M+7mBC*lZ8G;Vg%X{ z!<%{9=CycDUaO$73lywJL_48%@lv-p1I~$vaTq$`E&*=W(ViUqm+RF7UZdeO*`}7p z>}#)g9okJ6(D=PR6#2L#z>g5}27ww0_b9ks$g)Z^8fnKuI|fnkCW8911}Dn|?;Q0p z;q>Z@d`C3MY@&A(@|(g(!LX@7QG(A7v|D=eyMD5Fns@3~MpLK1 zyRAjB7QniZ*w4c%KgFY33M0)#RdnmOp*K2s6Z5YAi+HPf?HXRUmhac2J{*JjRAG09 zPRliyP==>XS;AUj(AZm%-x9|Wct$k5E^9XOv7_=d%NF2pTN7oIm$VBKvOU6z*U1H|=XZwFe(#JQz*B9lu zPSd|Zu8W{1{d^uCxuG$-4Bx;Qu6`6HtoC#yAB5V$e_>N^Th|ZqW&VxLMOa1IzO28n z$oUIdG~(I2*6}c~JB&^;IMHBN=JI~S@A4l3d;yZ>L7RV!?S1?_q8 zNz{amD`78)Ez`Agpbl+gR$2}0=;o#wTDA40b-ucn?h34dm1v(9=uVjz8{4|%F>#~O zE8x`Fgd%j&;+n~(^?-%KHzekHmu;E=Cl_VJeV4R5Z4;bIh)|89l=3S~1>AV;HC+uK zdlJ~g6B}mQ#jNu+hw(RPw4hV_^gfNRgMCY|E9^*vZ!*{|hmnlP&kSwQYJzqYXniPa zc;Z>E0p)O<2H6&)#W%%d_1S#8)C(;IAhG*q*``8er@`$&QOQ&Me0#yCD?eNK>I#iC9<_B5{&_or zx4NvIh^R^b)N3;2T6awaJbGIwsLd4Lc;RH(30an+l{w8f!*>j*X_2YE=@4&LgP1dc zo83TCAGcvYr-989+f*LsIxAW;*S82+EP(c!K}1n2vTpeFMg#HsVP@)cf; zbSKf;U5V&bz9qh{+NHjAzO}wJz7@Rha@4QpIe41|1g;AsHq85eq4J9_48pCM|_77eN^x`2Gm(| zOSH3;(BiPh+8O2>+8fMhH5^Bl3()L0>QDH_ItSSakaghkm<+2h(YljgIYq{DRI?Sr zbB^>~K`SML*>`=?g#(mxQs(Kzl~uFzlR92a>ZflIS?+fgf+b;9Kgrc$X%st-oezmpPK` zmwi{jrlVoD_Ns5bez2>Z>#FuTNJc?!oeVQjUdIpuGld-80)=6xtv4LK&<^ShC|2;R z{6@^UrM8tkzMJ6bedaEFR;i7qBtvF{Q!1L_P2VkFzJPqhcg@vFcq6*)t3~lMQ8k+T z>t@u--x{cva1U_IG9~)k_*?KAcw+h6A-X*;>E!PKR7ZbjXqB3?)+Fe?`m35AFk-U* z7R=`m%p}F%Ss1g6f4QT(KjfYPlIg}PwjSUi(T%p=RQL3cbtI!1=J9ho2mJzmZ=Q25 z&~Ju$r-FU->IHp&xM_6$E{u!;+iAKqUfTgi9fG3t<|SDuP9NZ=!$zG|4>^$ejO~g<@@vaSn!q(RDqx2E$e1o z==VFyk#&*(hEUZNwsB~#iy^;cG?!w32Y&ZO{kp+k>Bz6Bx;WRgZ3OQ+34E8xFEXY% zd!zh=nsWaVh^9et;Zr>!tElxg;h8kWU&d>aQM^*biJ|pEB_$2K>c^??alghd2a5`n zagASsGOPkgwV!o~QC;^~qT612$1|Gi-X_mt5O@0~BH#IiD!=I0k7;l)bG5Fnnmwy&`PO#Jn z=hz^qlaU|cPA-50$O8V5e;LG!UQ3M2Bjfz%tv;gPl@?zDjM;Gy~01)KiyA5@YMS4YY%^TBG4e=f9)ae;p^ z^mW%fe`~;XA>wC48*$!3>kEFa%fHOO2WGN{$7Pn_b5S>zpW$5Xs>mWij$y0(D|v2R zeb0NZNBmlzgSSKf2LB5G8YJBa?IuJm_xmCL)&9MfE{2)@?!psgKWM{f^=L#8bd=p}sNWD+XN3O<*pR%Pk3S)1@VoL9FP|^0 z%`lRj^iMEwe#TGxPx~qVStPq9?Bv+d?xxQp+jD%}yS3wCXIui^1wZTeLoOGAzUsf? zzvjQ}zkZj`4gXF5NtjE-f6Lz=R>o8_qlsuXL~6lq8DI6y=G(xn;+hBK5UoX^e!O#h zy;|~;R)I8KYoOZ%5(E4A6}kmsae`lCeVkfTTfrapfO$!nfOG03tb#3uc7kRdc#lH} zUqEp>B29Z}J0YrrVA)xq;B6Fn%oBE_6!9cF2a~|AOJI(vH=Gj5fu4c6{OJOxO<-_? zlUM5M8t4{C4QOrM16>VA(Hw^aW`oT|eopoC-zcnZ%05NN<0cvv(C1HT2h6>9Af(aq zJUqdWZ8vMbKt^CdATuxs_<;cvjL;`gt4Rx3beAkWkWH^ZH^U*wV4f*GP=Kav7Y zRG&w*S81D@vjfAyD+lfXpgtnd!l*$fX}w(=Q{Sl0I26T>N=uBx0;>^TS%PeWkY}Br z4G^opX2;M`S#Dq)@?OP{(?i(7^&ZjwB8s@%o*$6jgwivgQX}?j_5+( zx)`|TD3*lBToRb%+G^--Ed$MA7_GZOX|B=qv6KWdVvxxMpKVqtiWQw9AB8|)kFio|HbvCDCR;e~>PD$= z^@3+qvIugt@e%{-wE>G@Q7BOL-tcqp34E@= z+us=h&XNXE;ba%~T-3@A9##C=66GBYA~=ED3fhr>8c)Gz1iPVuM>Gah)#wGON6>UZ z>tRb|PE0N-HXpC|LvMn$nqhAVr`QD;D=#J^_-qZTGA0ldqUE^8A$k`mfu0yw&`GImUibU||gB4lE8#M|`R9HnJqJG_X9dEU+SQ8=h!AP~{_zX)%@9 zifXV*h_g1ZEm8nD%J`kMCa^kCWM3DU!}A*g>v`LS+8$_}{W0~(<|}H45jKL|baPwK z${d@7_;=>FIk1J7&UZ+BYf(-^VdVXgC2e0NoE7j>eh^~??4E6b?P$d9;5QeYc)N95 zz^@9tqSubVKw~EC+;U^jm>I(7m0(|sY+RZO9-BQN+ZEUwSm4^tX94G!c^{%@qH|)u zAioJ~=OAzg1j(Vm;lM4l&m*=TR=rMQlp~)C({z*=EiEcjV|Vq6mi~Ga=#<$S`^p2v3nHw+`0RrNA|0w@Gt3a1~Ke z%@suHcrW;AQ0p4?^}u0%{2NGn1h|QOybHp)ItXQaieC*!!KN>N2DAv?LVmZQtqmLl z%}vBN2PELI? zf)1)|O%QTXcLsP6=`xDA;H9Va3kg; z=U^n}2?IaR7)%Zx1kGrt(^=tcZR~=)Pa!PwLEbIcGuQ)UH;`8q;NBIu?!p+Ui1G+K z#^>6gK5keH>-h+ZaS2tTH?kTINc9R%vZMtsnvOX81p5ZnE|RazKxB~yGb^_D59)14 z0|a0?0>;Y})NnV!%!pvN6C6cA*NZfZD8)qMWkaLxK+8 zCIk5l;60-?*~XtjLxVXW-J;0~4i6RqMe&)!S7VX&Xuqxi#6`Sk3x3@c2hH~W&P9OM zHS+*|74Cwy)){4yAI#(J3!oo?Xfd?I5S0sUVK5t7J$PJqt~Fj3b|!qihiH>C9b&VQ zt=5gU?7#gw<06{jY;$|zIT@W@DH^G_!QWsNln3ovInqRJqL&o7OFTxo+R13_9PKvL zR|Ko|xq5?*14PQeZY_Um!Dkae8Sq0_`%s zHfOw?-j8Y%^;d)DvbCjl3iwn3Z4Sy2PR5`WI3ujz*y}dvW`aB;vcc&J)&dsz&9STA)t<_9;TeFD2S*dM4l!Ii-k5aFC{dGKTmmZt!*)xmk7 zS+83dl<<|QvM&y9f)zA7I5#*8ji2l;(=I^Ns$lmRJfb7Ae3d0=PwYlkDeir!KZ7cg zX5Nc9cn0!)U8Xx5aBC4q2G>Bpzky~u;x+}>L%#v;9lUnr>1dp-!M=v|R;6V#NY^3F zMqUrQ+ogro5WThq5Af2nD5t3GMDQSzvCz_j&vAi@+8hIJJK$a&Jy((D5NZj*iVI$` zpA|S*g~8K72Jv?AONBjl7;#i^IpDj^c|CY4#(JcFltzXS-38KfLT)Qe=YhW#ykWE= zZRZBHKLff8k&D5rDAHVEw?{LMy@u*8Ba1p&bFkZFyTnVb2%7W4s8PFf!HWWaL9l@{ zDN?1)=Cg>z$m?R?42EHi-)JCj9;(y9SIWHn7WB2j+YPKbf>m3%4T3aL@N0vpmI4p& zKf%_aRxy122=#q?(6tes&rxZo&@F^dyHE$@cNu1Pg`eR#IK?{uO|+AVGe&jYA|JU0mX!6BJ(jyVK-){IVn!ZBDt%fUY%wi_<; zIb?)}fKL@K&kSXuNI9Ww=u6PflA2B%2ZKHrX`(Z&yM~2Y!Z|uLMmju(9uY(5h4Oi> z0Q#a(VW>DXMw0`x5g~aS<4~;_Q3Gw8z4h^1>PmPoc(MX@+M(um`Z`-#h=ICZg&N;N zPpMtJrd;r-k5bo5LKR3O@EiXCItdKSKYmqC~bd#KqdbiJ7nCn!&Qt&*55tgGB_C>T)#;Bjn{{UN&1` z-YlwfeMA@NnnO$v^rJ(!A|s*q@%Am8S#}&^v_?-s1LQx3*T8oyLI*5P;BtlW>m%OX zNBHxjPCnc=@DLYZDR!ue4l_vZm6ceVM2@?zSC@)3jCDNcxWdf zjT-7WF_ewwI2FaZY`W6mE#>a~?(lO%vmwT?25)*ZLbE`YhGsdy&;|B?Ul>8*xGl^| zjz(#v?QNceGPo13o>~a<+Q=&YjUgHA=LwcGftnv`UK_PN3L2YjWz0KAeT3M)1+wjd z)@w1!YblDhf{(ch}vcYlHHO&3G>0uhH%>tTrtP z4dhX%*AIpDzZgY>&oDv;(?Z}+0DmZC*Np%q_J)pyrm4GV+CkI>Fx&m1$(C)Q9iepo zO{zW)d}kz-SbGFK+k@pEJ|etN2vpQ^652fnLq{7(;jINmU4t^~Wo?V{-+h-w5|8FS zB;OvgqAKm;tq$B}o$p?148mCCUGiuSQ_qTzci%t*!-_Fgg_9W~1@%+F%-j1|0Uz_&r%r|X!uLa{1 zE9?mBk4ohPxDTquOMu+f7f`KXMF`m1{5B*h=b8;|$Wi^Y`k6wJsW3hiHFdH(UXJXu zNV*mIBKYAb&z>eul?q?|7_h3Gc-)sxAX{6Zx@XbQlW33$Hf3ckdu?u2pr=#WqSA`J zC8#dHiLC*QSk)d?o>8h)dYA;@P?&oL^_BJ-_(NIvJ`6C)^avA=ti@s_P6phYXvCBG zvg>QwS{!a_^jRenA`-nJ41udZ#=}gZrhWidLwda+?2GOQf!sjA;XDZWG6ZWyB|k4N z`-4m{7c#|x?{o1g7@k)E&Bp+xA5M9&A9r!CP!wfKK&@?vABXUBNv1@^O-6ni^5GoG zl${rb;vQ1z7}l;8LqQ_d#DKO_0O|u*e@~W+KyEl-vKwuZ{gKFr-5V(1K)BKYABJh8 zARK|=38=W^^QT=LM$Ww`9wIF%WGsZaOA zPZW*d>Af)T51vIErf7BhSmIz z@T?9pJ+YlJ`q5)??O_I}qQ@xI9Hk^nOuL~aELNoU;q+jwifl8{rHfYe{qwJpjq?nK z{3N7-Y)D_g_raUsGE)xFVD5oRGDs;Ij9Ng?C|zpGG+l&LZ=n8=3$z=`+)<9#(TRSk z7nr>Kd)xyF}p;$jx1`LZK`#8l1mM!B(1j6*i)GO1+YBvW0oo5ayfKfS=%5 zi)jRME0j{mXpKqs*CAhq)NZ{=jj7>U;5-DIyV0}($!r3DGlCxFFGdywMy~QLrmdzK zMeFjnnYJSyf!q$$&9V!q3_DFTRb|-*>G|9)lO6Ps2fg?_w7dOq?ahjTQOfJ40KOUL z;-kSl4({}BlMg?GEl5r79-!UM+h^LV@b)W^Pg)KSVCq2}$~2!#f|49E`LM$Zv<|hQ zi%^S1&rwWeP-{7&$PC48_+ZX4(+Qv+S2zT6Cjq|!>6}vNPRK0opox^S$ts4kHfC z-YOoN{N)}4o{H}N6M$~0a(iG(^9(_nI92=z#~O~?NjeUFX<2~(49N}yjtNV{y+C!T zD>WlZ8>Wv$v7L$(oQEMf0^Cxlna#AiVg$(S^t(qg_f3w4774X9jY2t$@r*_x%oA|v zGTp6c$Z0JdgIk>2Kra-`M}2+Rg5{o@T)E@3UIJEF_{?-4N;}R&o7O2-nKUt2 z5?0YB3c*KFe_<6mfgGpcYgBa}52fZ6j#u*azF?YwYG_rYMFsN3INPZ;_&Do3wRay! zQc`JEAddohr4n$P>0t&tUKR{K*y`+_5cWc_jE4XeY7R5^#+0dL5$14(D@(|3BGjR; zxsSP@AE&>0pm_kIRDtfI0E$G|IjGN6u&0S&MWf9J0g_!j)TJsb1|%jUO`6MMk@V)) zVI|QO>3Ku3+}Ps1FeYs;c4Q4gd~a;oj{`gr(c;ZR%%@lP&K?X{QzqDR)M_r_wgdZ7 z6+_Jl=6S`nD9tTE?*|q?*}S!4d~OQFNiyFpO@%NGu1TI|NC8$u1tS3)W`0_d4q?0N zje7JLOdD<<0nljkD02uJL(+;;p=Bo{nX#&P;}ASvMVScj1Qk9*)uU&c9i@|jHx==x zo0CwvXPBp%PvRCZ$vg#kX^>|fvSYK6>TcWrRHLD3Feqo{iY2@b=h444V7blnUX?L)2GUy(_+(G5IRe^qX!fwY4M=t)@;8AWj_%PGgl|Uv zR`WLVG|v?1HB(XV_m)ler+2`e&^EWDIFKg_vs!kVcbRu%8q5t*>U+#HQ0ra@^_&1L zXC`Vrdljku7}{qpgi^}xP~i|*A005Cf*1#Zdp^6N^bmxHk>n99e;@1Z4ur??{m(|N z;~0`KqSkf^`6m(Og!#DH17*38y-67xygB;&=aKXob8_KsoX2VNS@StWJ%xOKc^`Jc zd=a=ipe$y5f0wG-757EC8L$bU$!xfHZahm#D7KgnWB59@zrJ)}A4?Vg7_uzw zJQ=d7_;!G6bML%H3eE=2vvI4RQF1qL0?yArtzK|n#-bMPrO(8z7~U*UrRi4X=#`t( zp+EVjTCdFcWcpk`+1(YxQOX-oE3SdOpQ=)y2eQu<+^w|qXJ%GujW5k{*)2%M1+fQ0 zk8FVyr=XR_3ni|vL?ouvLmOx;q!m7+K(NoHz!RYDOMr=pvH_0*t!55Oc+9*ad#{v5 zu%J+2H_qbAw`17Dl7ahU8%%J>gL+XUq&Cnptf(JKbpZ0m z7NivRxAX-#0=2Cxs2+(*FKke?Eng`sQOk=$@nu@HWwhlUYO_NtW0i3;2JvrZamCe2 zY8ig@SPWRKl8P_&6*+?tZ7}8xfiN7EEnX#?U`a$+W6rItb0uK~OZ@5!Cl_)o)jheC zm5xf0gkrf-dcGWoLzz>7o@U7cdTL=Zz!|6wryyytimH@+v0w$Hw2SZ#JvJT1;~|Y< zmJ2GYu?@AWo@K)=BP`Q_djZ{-QI?T7pZ2udq35V(V=OO}+{R<+v6kCe6LQB{M&uR4 zU9y!;K=f%Sk4aeOM&ZP)!&zIExN^D^Uz6Kp%M=Tzs^wIWnW)SjrsEjd(8`vfy3VjL zD4YqLR;b|>XcLbM8QfZCy(R}^aSrHtj%A)@wq>p#KUmdvef6Gi>5Zw&bEZ@*RLL*G zaDQcXxENE)vyGMvfR|bpfWOSL3ADQ$piH=yc<2?`5N5$8yBtd{K>I54z<=(#R*-Kg zu*`(|A1o=fEJdv}tav$uMZixiF2=FsnCqe5$yM`<_b4syBGOX{$gG8{9Ii}|DYLA@ zu}UGluF@^+Ls_bT)(7>Y6#y+zG(D$q3AEEyIHp7CO>)i^KwW8Bjd;>)`lT#WYv~X1 zYfGyj?WUadmKu=WSGvZs0A5Omr;gxa{bC-(ImvNlw-NBo3T+!e+u_<`*@|eJEE^PF zwE}r#`uuBUckrFr2NJvCI)EhigI`j*0a|B5{!Ysw%U;0uSaw)4J-Yy^Q#9a9XDI4{ z9^8A@q8@u3r37meDvtpd*e5z64h*TuY(7cq|i94%yJxBjD;`g))?n%4q1VoA5HXHH0z5s<~Tc8wRCw$sfNy>Dei&fF%FdU7-c2{0Er{)^zJg>)5=zpv}9*LqR6jGaUSQYZ7QD*?KJV z5^7_c7otz!WeiYK5rV~`K~*=6;} z9@V3iAEME7Jn%ezEn^&(mUAvZHm{{*BBsr-PO{Ea@nN0ohskkPqViqJpAMWU)>+oc zNMe{WQiPz~$02;0brIsu1Aif0^Q{@yxe%sWZ=w594{0qx60?yH?XhSI+@n59Y6Vd5 zkitPlOAvpVbvX(bTbE*axkR#?V>JRL+nR;wD^MBwsM?QLDigT5)*NdAghfa)F+UH5 zGQZebVjbEFCx<{;VNED0hp^0AVXcI)N|9P=U8TU@R5kz28n_HCb2aE~&1-d64b(!o z(_XjMT4P-Yl=aqHB)!qP0fn28?~~eW-QtIDw{Ep=vp&h2ithRh=w*ALbrqtPG#;R* zr8}%o;oVh#x6`^CQdwHD%evRP2cW6YSN1{JhFZB-cE2j0LkK=-J%GX@DDz=!6mDn1 zMX6wgyzaR52$DF8V0}fAH&tT432#u=#+84@y-g*vmeW`oHtilTl z{UU}0I6iSJ9qu08!>jzj|#Ivj2{7e(G2i? zj2|j{08l~ELyXHgJyOAN2P;f~Ke0Zx?u6XT*4~hU1xr}1gDY){wB72oI;`vAjtel# zCRJ`y-Fw(mL2|d?K19HH*kW}7eiiHZc5WuB-7_3xb8b4+^SSjY$V>p8P0VwG6?Q0N z9z5OYmo_SOCJoXlP)47d1v1Te{P)pVR0`6{SYH4{S$iXW-a#$Tllc;HeX`z=2bzHB zhoDxnJAzwhZ`)9%6j`YJM{_O~j;P#T&RK6GyX=*Joj)~4?2uQh3YS!{2jJdNt0W~a z*uPh5+0*8qda#1Gp){sLIhK}%0JWEGQr=Y6eAX8)R*3;?W?O`Uy+Abz1H3XX6jA$N z-tOYHWjz(W-Y*&e^ntd%m-A9`RO#B%{%?P{<+QPX6&D681!s(663HVpD!192O&R)N35Rs?Nk~-Y&=1qh3#I}>g<(>x?15)>o9k! zv2C?&!`v-OUFB5wDsf&S`WB3DhH$-Y9r$Z))wab@I;m)bA}hzNRj^GMDlD3=oEMz~S}mUAChD@5Z>C?{R>4+E%ITT^1f`52>VN$!i$DfWwv0R?1O%MnDUB0M}_H zMl&k+ZRl<1Fs%vcWOB(RMMh3jcF&-VobpL!-OUXzs|V}^Tos;+n0^z&GZ>cRoV6WB zEv63B&fCJuFWVj~_xPkrR+evfSAlcIkDF6^8}LU8Z4GYMCPcZ8>33}TSTF5qJp^8* zXSzb)tUx}QO5DD4RDGqm%wnrXrQe0h`v6jDv=tZKvNhWt1Kbzii_O5j2lx|PCGNq# zRPO_|0j_p;N3m-IFtiofR9WI4>dPZh8g_{F!Y^if8chnl0!mwjTN+tg4(eS z;HOA#A=K;{f@V~hZG$0(6Jag3t%~htULJb$njWPo6>r`>8$P0bI%|RO12($ zxL- z%%ws9LV0n)8hqc|%MoXf^@}?}b(dtR7|bn$)<4KTSdkr~K(bV#J>CzmDM`TimD2ux z99i1uN}x3lwWrxj(Y;8;a=|!Ew~t2jBzsDS6p|4hSse_g&M-&mz#Y~B_vTg(RkR|d4hkUEt!wnO}J7|&O=#PJyT9T zh4RnHO;cI9c}m=5pk^R!zI~BGJ*?!v0MQoON0l{|8Wp}TKTlCE&@vI-4Xt?@KoRB3 z5xxZcrS@!l9>Q{v5Az`VVhqV?%z~1Y0=`+XO1_kfu~dml+Q)J+ z`ziYz%sKxWi8BiAEP~E~e?g(;LLaXK>>^wPl~EV^eclFSYvf!m*(YPp5GZ>+NKSz} z-B55Dr7}J5iv5YwpNFDR9hxW&ab1U~Ygr}5p%2F^(0$Ot(Tk*th zvX>#->!thrwMrW_+hY-Iv0G7kmNFZJCt(&?qsYjy+=%Oo?+sh*_X=G0b?DwMudpKt zr~O#r8qkgG96))Bs5MA?&jEgha9H7h#`~1Nz?fHF_LEv$?W7--QZU-iC~$iW7!QFe zA`Y+OvjD%eb1I4)N3tJqspHD5ZUJ zc5HtZ=toFR^QRZJ3TCaf?)Me|Q7 z5p#xikT?l7m5P%T>P*0AK>1UFvJOVd6bRRQB9T6l9b8dAq>(hBro+{U`(+;RhXL+& zebLzP2)C|sh{2SPf!00TQH$E($il+nCZ(^BP~wdSXpDk+(>k7ax@Z*8nhTOp+w@XL zBB@*$EvG`wCpc!I^5vruIuHEumE#@b9HEfs_?-00`0V>iZtx8aREDvtd?o=r*)b9P zdyq~F>Q4pvp?E}{2K19qmo#KM1_5^_j@ts#Ckm$mHp}rece-PSW20vZ=9voX(3)fb z#CqV@ZeO5u(?KhnJbO`^b0N-b6mJ~1Pp1lRX3qi2T%^NGs6JQ z&BF0vWCX|tv^-2PLYV8w2Y+jBwqpj^ojWM?0>>;|IygH4ZW)pi*#5MBX(m7@~#wxN5r60lv6>uSd;M(x%pQ{Apb_QA9Vb*WCoz0lq4mqQLv6CkOnEbv{i%s%kFu_o zo^#yFzEgPCQRk>CHbY%5I4(N+L#w~!SX}xDjq)!`hnAd<z}fWL6#-szZhEVLui{z!J2H#z3R9E@v^bhDb$*$AT8}cJqPxr za475HhU2DVR^D}`CNk}o<2K-T9W{{W9fu7@oO^&bI2ys<v>tV)m_G8-%R@cv7w@Z2<3zk>YUn=&a9FKZx zF5Dq>Cw(+KPQivlYo`>SI^2ln@Z-2J=5(|GQ~`B)0bv8=_)?K;1Pk}f@f_pfc*HmW zbqn_^1BoOUC7!Qrl!JE6RxJ94626m!Pm*sc#!sc zI76Jl&Nje%;xu|GY4yQSZ-oMFv@i@|5zbH*wLNz))DL*k&{z7ZWcn-c00c!jBOx4! z;238#ge+d+4gxsV8HYG>dV>|nn|gc)9Oh=uQH2SJ`ZOoenbe8zjR)sIew-BNP{32+ zx|^To9GDXgcXBW4wcAyrmN#xXj`;%h@uZSrekp-n>45ukgcMOr9pN1AoRd3Bk(vtq zWempd=Zywvio%0)BIg9BwQMY=jE8WtbD}d6+TSFV6wJjCHmCHS(tct<>$01s$OTu- z0C+mWXFF4o+(pg|r%~ak<^Npp z=)%R2-?scEIM<~(tgc$4>{2ar7Fw4(dlmO7Ou;irm{lRUJ7u?v*Pt;Y9JTrgus4)O_{;i&a6Nw`^xnMS)AB{F{oy%oGXF*JaaXY836U!fS~4*9K@dusq#>QT4!8E zGSsA@>`7L&bFH(+xz2f{q-U`?b0f%aa@MrS=^ul%Hv?XhH4dO1ps6{~mJWEfIQybL z0U9l8$lr=`*n#||1>2l`iXuST0`uMaid{e@OZPZ;J9j#lSZ+fPfL$t}?Q`x$@-n_3 zputLOmMO1yPf(uy!D1YM7>&wka1gLVIJLtl?14+yo{p=-o&c-`@O&a3Yqee02OURg z9Yoe4Cie(nM{zEtN;{c`YUGOnRtf5HLb=1xCzX<%z;WAU{h^bHe;CgoPJMv%5!Eas z7^Qy(q)&f94F8;D>{iKH=b(be`EyH)P&P5_{a~42N z<8Zr|Q>=Gh0^S_ZYDr#-a&Kgg*IiZ8Tn4eO0CpKJCu+UVp=F&x?NgTBsr2L_s`Rgc z^dl91(|H}Icms#`2DqwE4-(+AwvZ$kd5w-^SX3=SID#3AB0Ha~{&ai1K*uynyqJ zDRH3~Pn~WEUm&;+crTsxSVB(Q3G_=I)>(=yodZdM9frHo0`TL4D=Jx8UUqYFZ<66_ zp6e=!QP5v4NMj@5t{g?}HKo(JT0dw9G^GB_Nr8U`;v6hyoFvX$PPN@#Ri)k+wV!sL z3<(g6e{D>EIUmM3Z$pX$%I|@H7p|Ld-GZy#HDGKdYJWEX6P=Hc4VZ>>BukDcE5db;XFk%(LlpZlPNQ$9z9BjDl(z59GrzEht0%g{k30_`o>b7}33l}YN~7l% z=yEv9+mzoEY*(9;N4X>;JJuV;l2ZwBg(|Q&Rha)_tt2D;JK2ITs6DqmzL3Ro*dvp^d09k@eW{S-Y!xuRWz6#ihB73FynwcrF-EaJo{ z{Pwb`A)BHPH8mN>O6e3k6;YC0Lovrxu(>4BPinXy=1nzSMYH1^hq=;#%7X?+sJJj! zMc8PU1Ial%mG4gWD3^%m;@;G!xyGP4%2rnU?R$ za}@w(JGKUe3b)8rg5Y9>BFq1|U9K|EEcMH`3@|Gmam!s5h&S4=_sS(3i}k%OvmT&I zkjhiilld!MRj!4Q$_m$MKdUYCWOuG?wX52-#^Q;i`hh>>7lDhT%@E$8a#{ zex2(q-1jRLTS`{BYGEF-3#B<1%}BiI$-a=Y7bs!CGZpoK=SgZ^eX|Rc(QiifMo4j! zYlCYENIb|h7d%ClswcX)jX13>uI(_6ZN_=SoWZl*wbiu^@EvgNbhT9Ma_t7FuJ8z+ zNvFpd_C5iQsC=$i^~g0Bf${?II@cvEch&XMQxEWE zxURUaVJZnS6kN3DEV>JfheN5KL)dkYqCkf?TvJ;v1N|o8x8S<%nhE;63(#zQ?-~Gx zGeLCEW$2#kK2VwvRmPhURFd83dZ5rA0(3DGX34I{3Qd-U`68Y(Ji+vlX!Nxp%nZH> zK}E2Gbzjw@tu7mu_omX(w+dQl_3dxeLvh0y{sy{3TA zNxaZR_asE0?4F|HPebrjOqmTWU^<3ps-(OrMCZ&vw3S7BR6TxnCpmS_SJ22lN0Hg) zSMRwBU5>K|umy0N|K1%M?R((;IIhWUH?)R0=Eb*Xg*lZnAe81(xVN|&)sE+TJEku zVI`K?Rk{M8-eoJ@RS>R5)K$n|gM4_O1c&~)t$i)kU#13R)+uu95mf8m0O3ZsHn}&u zx45@r{x45Q6eOsmPrHsJBvaNlw_LU_k*L@jk3Xz-r|021>;n z>%Izpgy4tn2P&E@HM*?XO=Ug?s>zR2kbT7+mnBPB{OB-mbw6?U>$F0HlL@6|G@%jS zs)}b*!SKC5KTOU;?Hs^$exQU}`jZA90nNFfb_ZZQdG2=OdbXf2vivOQ@VcKZR@d(Z zl6vVrUh>TS6vu>hOqN$hKonk~!&5$(!(tkPLJItq&}vBc?)*(yLY+%Hyck)@R<{H( zL==|;-vd`0!rCQ6T6(p#V=~km;r+6Dv;+fGns*4bj-Gx};cZcf+NUK9+IOfQ4`xRl z@b>h>ffJ_U&g;;dypsJ?627?o71{uRA}~gy+6)Bj32xUh0LQjOW4;_as%0=>k#G%C zQN8KLVM@G;GeiaZa`%Rbew?9zB_YaaWCgu42`%oTr%3zBe$xIaeGt~CEu|$D#ZCK& zDSS}se?DG1D(&9fVYt-8A>08Lb%b9nMj~uf%V>o0HaCoarqyT4uKNZ7YNO zH4CS9+tdtk%tfJee87)Te7sIurWogSYh1Z_*@|H{$ zwU=Tq9zZ|HR&m;6j4#T0Ej{~av`~h%pi^AVsZ(&lw3wGCo{<`GMmgHbICk1pDZ8?$s)3t zEFnwDGP0bkAS=l#as|1PTt%)X*O1lZTC#>*N3JJp$qnR2aud0k+(K?8w~^b)9pp}O z7rC3F$H^1qN%9nVnmj|ECC`!P$qVE~vW~n&){~dX zE96!38hM?(LEa>9k+;b^YO=L59pL{?*Bp;EF$tR?VG?Nz6O4>*}=^&k? zi*%DM6R3&QBx*7>g_=rDqoz|csF~C(YBn{8noG^2=2Hu(h14P{gECT! zsU_4>Y8kbh%A~TWY$}J!rShnJs(>n_il}0$ges-VsB)@;s-&u@71T;<6}6gLLse62 zsTyh>wVtY_Hc%U>P1I&;3$>NnMs25dP&=tz)NX1IwU^pQ?WYb<2dP8UVd@BVlsZNo zr%q5OsZ-Qx>I`+3I!B$SE>IV#I_eTtPhF<2P*H+nTdPF^@o=_&rOj#%^WuxqrgK|8`XmjrhC%8=n%R$9ZHAM;dBJuhwe-Fqx;hX=z(-39YsgeF?1|Fh>oKN z(?jTZI)P54htf%OGMz%F(rI)$J&YbskDy1=qv+A}7peNFk=*jdHdMZ7Q zo=(r8XVSCi+4LNGE^m(WY;W%P17lg^^E=^Q$j&ZG0`0=kec zqKoMgx|A-X%jpWblCGjx&@1Ux^lEwyT}`j0Yv^_Kdb*b0KyRcs(VOWl^j3Nsy`A1c z@1%FpyXigjUV0zBpFThzqz}=D=_B+}`WStjK0%+PPtm98GxS;d9DSa?KwqTm=u31x zeVM*OU!||n*XbMdP5Ksno4!Ner5osbbR*qFH`Dj&2lPYw5&f8cLYrtaZK18SjkePc z+DW@;H{C)%rJvEy=@;}%nxrY3rWu;0Ihv;hTBIeqmG;nWbPpz&>B;nBLYUr6C=CX&c1~QRM6cf$FFtN-aCXN}*3}ND#1SXLg$|Nz#ObV0Aq%rBtFlIP2 zf*Hw-Vn#D#n6b<_W;`>2naE6HCNoo*smwHHIx~Zr$;@JAGjo`^%sgg3vw&I1EMhVk zBeR%U!YpN$G0T}uCX2~ta+q8ukI82Wm_nwADP~HTQl^Y4XDXOVrixj?tYlU(tC=-S zHM5qfVb(G0nObH8vys`vY-YAFTbXUlc4h~&li9`WX7(_9nSIQD<^Xe$Im8@hjxa}= zW6W{p1ap!(#hhl&FlU)_%z5SlbCIcIE;04YW#$TVmAS@TXKpYznOn?l<_>e0X<+U# zjZ72M%-m-lFb|nW%wy&WV`9vVg|RX=#?CkxC*xw=ObhdrdB!|vUNA2ilA#!yVHlR- z7@iRrk&&2I#>2ERJ=kEjC)|%BayOdqVE@v~@EH<0XVRP9$HlHnE z3)v#Jm@Q#T*)q1AtzawJDs~0Cl3m5FX4kOQ>{_;lUB|9xYuOF#Ms^dsncc!}Ww){0 z*&Xaob{D&w-NWu>_p$rg1METe5PO(C!X9OhvB%jH>`C?%dzwANo@LLm=h+MFMYfK; z#MZNy*(>Z-_8NPgy}{mOZ?U)8JM3MyfxX8zvQ2C=d!K#4K4c%UkJ%@zi8Zqp*2>yg zJL_Pbtc!KCE$mbF8T*`l!M&=C7 zVO%&D!S&(#a{aje+yHJM7s*9&(Oe7{%MIeGq{=DEN(V8hnvgIYnmOICt=Pqy;xjOC=SI=GMu5eemYut7226vOY#ogxaaCf-| z?jG03HF3?{eeMDGkbA^E=ALjS&dgajD`(^EoP%?6F3!!ha8J2s+;i>)_mU$yilaG( zV>yoFIe`;7iEHINTpQPe59WLFz4#EmHy_G}@!@;~--qwZ_v8EX1Neb_Bp<~`^D%rZ zKZuXx2lGStcs_woLKaL;IPv9r=llaN} z6n-i{ji1iX;Air)_}Tm%el9FXD^&626o#-bB2J%5?M!e8aD@z?nq{7wEAf1AI<-{l+ldwe6`#5eQz`3L+%{t^F}f5Mx1GjHLo zyp6Z>4&KSTcsJj|KjokC&-oYpOP=H@p5_^zdDx?YN z!Z2aDFhUq9j1oo*V}!B7IAOdnL6|5^5+(~%gsH+bVY)Cwm?_K>W(#wKxxzeQzOX=8 zC@c~(1f#H6SRyPHmI=#+Od(6i7IK7KAy3E`3WP$TNGKLcgi@hQC>JV(N})Wu z39E%QLbb3~s1eo)>xEijgRoK9By1M82wR11!ggVYuv6G2>=yP2dxd?%e&K*{P&gzU z7LEuEfI3=7G&Io6PbHaJyf^bo&6D|q$!e!x#a8?jHo-191gGE<+(L`+RCp#l7hVW21yY~{ zT3`fL-~?U}1W}NLR>32*2|dJMv8UKe3=w;ap<EaA=rZ`KSEzS|=iu1(z;sSA@xJb+pjpAZ)iMUi;CN38<#Vj#f%n@_N zJTYG^5DUd3u~;k-OT{v=T&xf)#VT=yxKdmtt`^sb)#6&QMqDSZ7i+~0;zn_kxLMpH zZWXtQ+r=H?PH~sGTihe=759nz#RK9&@sM~}JR%+ykBP^{6XHqnlz3V^Bc2t{iRZ-& z;zhAeyd>6(m&GgMRq>j5UA!UQ6mN;Q#XI6%u|d2iHi}JRvv^;8AU+fyiI2r6qDeH1 z7SSr&M7!t^ouW&0i!I_)@tOEsd?CIRNs$t1kr7#u6M0b(MNtx4MUU7f_KDI< zMCvVtN?}sC6e0DI`bzzz{?Y(xpcE-ZNzqb_6e|sq;-taS5Gh_tkP@Y#Qj(M`rAVn# znv^aLlZHzpq><7nX|yy(8Y_*H#!C~ViP9u#vNT1SDovB7OEaXI(kyAVG)I~%&6DO! z3#5h8A}K>MN{gi>(o$)ev|P%RvZQP&N6MA*qLOsW@(GGRoW(Pmv%@yrCri)X^*s5+9&Oo4oC;3 zL(*aCh;&psCLNbfNGGLJ(rM|8bXGbiotG|17o|Gsl2k8Umaa%wrEAi4>4tPux+UF~ z?nrl~2I-#EC^bpV(tYWH^iX;vJ(iwGCdn*WB&%eT?26B}rR(}1P{O#_++G!1AP&@`ZFK+}Mx0Zjv% z1~d(58qhSLX+YC}rU6X@ng%otXd2KoplLwUfTjUW1DXaj4QLwBG@xnVldl0&>nFbs zfBiLZd;a`oENorbx~g?gYg4PMwWW24C)qQ@v%s^^v&dugEc0Y}iao16)tBeEk;l>fhk;YNR(Z(^xvBq)6@x}?piN;CB$;K(hsm5u>>BbqxnZ{Yh z*~U4>2^lFF6Eg;8L}m2P$S@j>i;YW+OO4Bn%Z-`FEMvAY$Czu(Gv*r$jD^M`W3jQs zSZXXYmK!UKmBuRL3ge`V^o+?FsTp!@YlwBkdZL!tKx`y75u1rE#8zS(v7OjK>?C#( zyNNx-USc1ypEy7qBn}aWi6g{O;uvw9I6<5wP7$YxGsIcq9C4nwKwKp1h)YC0ahbS6 zTqUj%*NGd%P2v`Do47;VB^ro(L?h8eG!yrU2gF0-5%HLK@~5l&V~$ajQJir(BQj%9 zYXQ>6M@fG7uFrKR;oAMauKS|(y6(G)ZrxK`A>06$N7w!4MqPK}1YFoH+WEQer{O9D z853ML_W-_E*S!$J)ZL%!9ta`v_uaY&GP>@Q;UeK$#6nt-4!K>|{qzo9_ehZc`6i%m z2KpAjw*eh^sfQtii`?VQFU6NBglTX+3;T!eKdAmf_uqv411e8b*w?z7!r_X5tIyYz zGRo0R>)UZ1;5zW+u$#;T*9I$;K%Gwa_1bTBcdh>xikpf!Uhf|vkn{U`7x}3~5%{O% zxAwojz>byZh@k?L+%0GSeYhVAyH^24m z?|k>41Hbp)_kR%d!yoW$Lu)GiJ`3J!fv>(4^#))UNsS<}XhhTb`Mf zos*lFUr<<7TvA$AUQt=KV&$sUYpU1QtXp5ZVdJLFTefc7zGLUE-Fx=#+kfETp~FXx z9y@;GFPM5po>9glAUXm2eupHkgO0Ax@_y72$3%-E)uZXv;e{j!UA-zMx z!Xx_h?bm<6z{sfRnAkyagNMW?cpw@CD5O#MeI<+|b0Ky?TR-02eY~Ah@K)ra^-S#m7H>(gk0@{8zGN z>P7?*dcr__hWIS;cf{w2zbF15@pxSyB9RzMBoWC(3XuxdIh`0r3@1hqBZ*POXkrX8 zmKaBjCngXRiAls{VhS;pm_|$|W)L%pS;TB&4l$RQN6aS{5DSSv5Q~Tm!bmJ8mJmyc zWyEqKlgJ{ni5w!A$RqNJ0-}&8B8rI;qLe5j%83f1lBgn95G#pQ#A;#DKH5pe5e7x;7J0t3-F;6*SA1s(0M zG7K!irugl~fuHMq1{sB5y}MxwK;3oSz4jVHKL=udc=v7!-c23=WDg1Gy2~(dG1)&B zV1Y;p;JvLK?Ap$MsBvJq-#D;jnX#zII}Vg)w2uR|U5^7@*g3@x{?(5ID++`2g9{dU z%@~Y>#l-`w27(R4<6v;az(E^6U>tn+^^b}f6hCals4>%L&YnAeQGQ`@X+_og4V$*? z^p4oUUGmXb9$S+X*0)#x*p#%yDU-(a?VmJo;IeFE^~xRDHM!Q@6DNC5Sh9NI!j{%o zFP^>McKxXb#Gt{4c2`{LB&BOLEVCW5G1erPQxfMEGQ~ya8OdvIv6>+2Yn^zmq9-b`gRZz z^xGgStP2x@VuQv6jSNZ+iVPYbR7;SsB9zyMmxy2Jyeq`72Yo&$GiXWBMp!d03c3Sh z!ChD}aQKbi1KQ&nRp-8i@RVIx&zjuUkf}5Ytc=F0ISj6uwp#~W6?fXrLGNH z3*&&#y`%L&ccObYuXP5S4U-Z=G1c9=cO$y#I*vmM8w0Z0e~r`D1KN7v)3zRfIlrD2}9+;~8bzrnE zk656S*8^V<{6^rn1HTjaUf`DlzY_Sc+k4+j)BPqeMz@++sGF_xtq0!KzTlhI1Ek-2 zK!6dby4NV% z!K=;JZ(Mlv`0RO$#b#@0zIeH%<=L~kmoF*rzR@RmUue(1{re9d^zDnSt9`Q5^w9ad zX1!}i(}w#GZf?AN=cfJkohAqD3vGG~`(kFR744h3pFVy5;sr^faq=%w|Dj2VurK(I z_r>109)u~FwlDNH`$DH-o_`6=+2!+`{lt}S7rSjH9(HSnd3kN%E|{0U53}$(SZmw@ zn_L4chFVxnY=HTBv2Go#L|S0A;ep*9H(FPiU^PO6t+v6sgom}t1z4Fh!HUEV>kbjt zCg)+DBCkm5Va+111kS)3!U}5!63&yZ!a34ySQl_asxD1;pO~$C1?z)9!u(%8n>tB+ zB~U(n#-<3Q*0f%WS1 zfsW(Ahuj}m_Kn_pU#x38_i>#M?E47kBZ{4WqaA$PdBC5){`L+BM0WKF5u5<=RvT;K zd7-xnfa&*t+%^8U=%VRz;Sut)Pt*r8bj`!1P5xj}_N z-hG|G@2?H|#(UpkXKc_o*!S5&4AB2v{|kMn{+Igy z(ue5(Q{P+vKl)$mf203j{crWZ)Bj%ofBJ#?9{ON?Pkk@_uk`<+@2Bsp|8IS`zP~;~ zAExi4|CzqK{vY)})PF_)Px>F|zpM|^e_j6#{WtaB(tlh39sPIp|E&LaeW3n(`uFtT z*MC+2CH+tJ|Em9*{>S=%)BlV9NBW=W-}N})Gs9hm;V`j>sXff{<@r7Q!=xXk{oaEE zdH&zQAvq+87~b=R&f|lRt{m*YtsQvF^MN<62llMnS9{>#rp@xX@Q#zGKK19sHG9|Z z->~t}Vfozr#LitGaUA>@&xu2NhezNe@|f5-`FtEd9pHU*Ah=7u{7}KGH@cp$qF+S~ zef0_!7b9Mc8u_XNp3nLJGKH$*K=7lsGF~g)yd}u-E`-PU+X@jt0O+It0%s$yFz>uod$dXEMG5Om`;9PEK(;wFE&Is z5oY?6b@KCK>AEMxAUL705CJ+H@f+P|bxz_RbZ+7sx~Igqbn-dz7j^%r3(-IuCmz7hrcJ z1$I+D%JV`t*mJSNu8Zd0dK~E9^*G?&$mx0%M*mvO&^s%~J3b#Ebn^KWa`3be zIN!?^@@p3Ik^wfZ(PKo&lUJ`mkY#`i%c#!#LUI~v*VRJANB1fIZSBBY?h6$z_Psx_ zB1e8-pws&U>((8w-M?|u;Vnny_Xl8KXkYvL11GzDf8dj}Kc1m}e`1mH{zUf5+|~I7 zYinvc+QGdW;QfisN46eYf8xYP-52^xoR4^459xZGkZ=4Ow}pHHX+8jlKHNLlboma; zyf^Vb?fXLA^a1*B2FB=p&kJo2{CVK#^uG@Ld;RD2|4;u#eYD>Hc_D&$?|bq3-vlP; zhw2mcDf+d<7xc;czth(eg?jltLY;mKq1XG~BmAshet+OI`Z#@&-uoV5l>V#Vi`5?^ zhUg9YLHfaHcmGSUuP^WKe-(D_JMQlL_V#}OJO4j~9sM7}zWz^OkN>Bz!~Zkb` zfv)2D=)QUXYeM;9DWBL|KQHr^>w!0(6L)@IsC_*E?-RfEzQEhAhq|8celqq&{x<8c zoP&2UPf*s2pZ4{@+n*B?oxUs5sC-xC_kq6miPhf~5tQ$WC|2kr0$&TfX}D#$ZMb8& zYiKasGc+2S49$l7h6jd+hDV0Sh9?G-!ECS?tOlFGZg3c!2A9EYXfepyeIf9Rfe_$P z0KZSHU<%OD4u9P559k8B!_)jw4hZDO`hAb~2O>)Mz;K0#9v4+TGm0K3?ex45-V?%) z4dHi_I}ZY%xaWoNIMBW?w(@5SXNkExuSOC*6n-uA3k~d;`N)i@7Y~1$kw(; z-tTQJFDj{8xna|`9s3R(Id!HE-U4oPxL;D9_aDn&;`p}iJA=QQFn!9@f1VMT_`Lzq zQSS|cM~TFgFBBtID7umWtt!U?08d)Xtv(Z+mlts<*cu%SZ%7_a%k7F zhvzQTUtxq1&rdWzl`OUxZ5(*p?@D|p(EELI7M>Tf!*fDk*2&Kc&DDKP_kG=e>;6^uTX>Hk zL>CIr3-#B@-<1%EX}X^9+|oFBUTD5whi+^F}v654a4{^tXK zb4= zU;6T>QNw4?m_2H?{CTTa!EwW9Bu{!Z>eb{a(-ytTFv4@ANw0z@z4GDX;kne%l!(54 z21G?i#trWO3XTd!jvDo9!mJsyXD^)oYIr+;c4XX;PJC?~c-wK{Q~uoO+dePUai8Zk zIJ_+&zc2vK>wDiLka1WYbe`(_Q`Be1uY-Mq(A(|{ykTD`uxma#AMlOF9}upu4cxd1 z@pmBK4}a6X(3|*gy)X8LeWAdv`5(6a`}PMOJcMd~0{MP;7waKq-{VdEcU}Jv@#&<@ z<(G2v&}a0Ppzr8qF29qThrXS$r1}2(MHwrXE~{F+A_sq?uvY#^VV1FGZT9MQYgSdi ziT`OI2R{6JWbXxj(0gRD`q-d2IQMOTkE}?qeveGA9~Sf({b%*U>a7Yg*v$GZyU^9k?A=Li{+R{`MUH;dr{zw-eB zdi)ek{ry)4uf>#X|IF)wH@s)|5#J+vcnChJe5~!n`+q#$>ATana5msv!1;g+0T%=60xkvA2V4%g5^y!(TEKNkneYt*?`j-?Z@!_U z;Xto#hht-}3q+rf=++I!0c7yeCXm;|dqnbQsk?lC;FEh!+{OC?HSk@DBl35reeVyn zpA(<_oBsU)cz*zXkKr}Hzu^78(gyjQ`1sqO6Tj^|0LHmc-)}EO$Dm(c@IEM&=KWoY zuK69F8&1ecNK8*}e}5!lrHlvGtc$D--Wb1mOT7I4NIHC1BocjJDmeaFFe|(^S|Io0Q82L-1jYEclOHGXLf-hjc_S|S70pA-X2;chy1mXSOXaam2VgY;| zg6QzQQThD=`Fo>@@NI~N@O6l_L|r%UcO~>X-#C!0oD<96mDodgzboNACq4`(yD4x^ z{5$xr#P9zWzAN#T=L0YjeX!HciM4a$*ZmI9U)?!zuaNM*@(=L9@9)G7j)yTIB^^#` zM~@lj{c)V&F8Qzf*__VHw_rM!iA`a-w3PI5eEv8gSd5ioQ(8ST-j+WuNgfAc!$Ly) zf{Tm{?=yICJh=ChhJzcEp3(*XkC^|d9|zw4xl!$$SpKra>yBW(6Z;STuEYlq0xE9% z`@~+N{5~k~PcPYofSBE>DAN<9>zwaFfedhy@I{j{ChkcQc_Pd;KdLF6^zW4osh{qqx`GEYh zoPE%mASzn^P0siP)i07l{ShDkXs3$*k$;O+T_<@t04^jHTt9FTQPJRHOwkDm35orN z4EC}+@mmqU?~s&F`#A9S>w%h1?~#3+zfb*LU01(P{d4%P>6?F-@}G6;-=$Rl4$UuM ze+rg^@A`b;gGT{^=sXU<(eTIiC!axk-N!W!{F#5B7=33M{XX$N^gA?1jvPC7;>4*_ zXU?2Eci}=^U48wPE7z{wxN+;&ojVN;jg8IC4<0;v^yCTp85X6U+n<+=PjGz`OW zf*?sA&-?HH@nf-rf8pOJ-iv;R=J3&@$B&;pdHVF(v**uWym;x-<;z#EUcY|x=Iz^e z@7}xD)O7#;!-tO_n@sW#@i-hVm;6IK=yzynmgRX-Y;A38`$X3N<9(l8J12h2-zN_K z(Iz6x@GtPo%*Wt-_$z@wBa#gAuQR6@ei%5^kZ4FY{KAlC_zy$6;Xe(-48JrCH~g1j zgyC0)k%s>^j57R>VYK1bhB1cU7{(g@*D%iTTf=z6?+gj7I9|wHI`5k|MKs`77FdNRkap05kJM|r(C;C(C z-+u180-d+O?~!@O0jbr~_OXnEz2MvbeftlDpCN|Rfuxi)`L~M4!4Cva^M3UMeiG8l zkL}1mcmBe~x*4rOUJO3W`d~DymkluC9R}H{P~m*PeX`4jnnxk$m*Em$KqZtH2Sd*Q^DJ;~ney@zas01`j~2r6#e zd(~0*-de@I_a0TN)qL+IjEGVz;vfBZuYHqCF5EBoyZF4%dY>mYIeGT%1I0gyL?;GuX$yyZws`*lbw`$+P)(VrNs--*GKeyn1mahXpb zM4O6;;^Nx0nZ-ER1v?;U|M?sEGw`1&j(`UR%Aep{nFmaL>SMt?keE6%B{i5K4unMz;3sa@CYuW|h&fSfOl4LE@-R%)ExJv_a9%~KFq&s)K9u-+?o z>PY7?qa8+TMvfW7WpTL<4r6?Mfk(&L*~`gU$@P+H99T}ye7>`jQYMp`b%J@HH5Epw zV}XuwK^LDK7kx24=F$iEzlPfb^+1J%JD^|7b)*5v5UNkGCA1>+13h2{6Z(T%58FWh z$B7_s2=7yW0`USJVBVU#Ldpmr-v{)BQG+fp=05d~pj(V5;UwrXBOq`&;rdyO9lvqkA#QF5f@v)~Z^zxm1Zhwpv- zi`SEu#0M<=LhOJ*Q2Hgb!Z>m9ag!X>#l%cboH}DtVzMqdHFdIHpP7-Fm6d~wh`jvi z)4@Ej$g-}&^5v^R-stPK>q^}sJGbxHdBB{SayajBQE6Ywvu7^?eev?;>sPM3Gc6~B=GC+30TT^oiLowC2q}Z>qvC1fs;_@q|$)p;)2Nf*xZ5 zA<>CoR!c3O2P!;nng@XYe!PP=+Q9+L2BSxhPfZ=m9Shw2+VNVgHhTQ{=z^K!@%tYD zu7CXEC19%Hu3W_xu3aZKZwiHC`woS|bo~nOC)$$h*ZTPQoWB4b0VC8^4c`fJ|ycDb-uy5Ov0Q!>Iw*$3< zx`Qe~crVK?pk9!beIxr}pk~lOFpCTZvrT`(NKpG{IAJl+&qsiH#KwLR@GZ3`7a)!V zI;bbaAdDw8CO0A5kz14TX!T2EKV<@#V>kppp&z+FSwP?tc%TANEAmfdIjA}$1ij5v zU~W1DI-yMjHHh#oW^ShbWnKhtf)Bx$;0I=}w@4q*C(QxmtlESAXw@AbAdyXKf3e&* zOaJ#Cixv1{MV?S9=gse%75w6Z;8ew*Hcbj;`$#be4u#)tizWx9AF5`*kPalt8}NAm zuQmhn<;(jqEaV{2hd=B~X<-i}Cg+*z4w(H0MdrH8CO=b>eo}f`#+0l~Fc0M9PoGx2 z&a67^0nop(cpfnIU$}MPM$|#DH1^KT+jk$_yC3)P2|f=ze`Yxke8e88xG&|IGZxox z>Qn!t7yEC=&>eSII}XO)}R z8Jg~Y4YvpI`fhuG)_=^T=U+iCgD#KPLFe}yps)NL&@=un=okM0^oD;3`occ}o#dZ_ z4v#NDC-~Q(m;4*h(F~sl@H%tv&jYLR{*?B+LDjjRfIg1T19L&IczhmcXup-%#UAe! zzkqlXbe{hMbgswefeeD9ec3#)6?C8f6JZiqAIt+r(0zUcc`+HE2MBMh-n1nHFAtap zF!FX}Kd@g8AbUZ_`oqW(Q`h>Fi1F2N7~iHQ!KViM0u$|FWte70`uSEAH~H!uJ)TwXF6-C z+y^`04dk2h3&rE{f#l?rMR5r+85x;V42B%w7s}61#O;Auv**s8KYsz}$2((vv0tPh zw#Xz8Kpz_OJg^7Xhs`eEqbHM2-k6yJY7U$`|1kd2tSgz(l;}ciJ=$reDp?%F50L_E6rm>5JE#x_oEOoO$yK3m2wZ`h}L@{hGII zE~&e{yX5+h9Xo#F_@Advud|p3uKfw(#CL9kIQa$3>%Uhuabi$Mj)r@}fFDdERe;&R z-Nzp{2*Zl)feMeC>;XrAnuCMCqoatXQ1HSW9BCqvLLm~lyDR*Cd_>b79e4_V2ZyjQ z8f{^iqkHlFcW|J&uL0K|7FK-y4h|*P4{QnAco~mJ!>`}R|B}L60X$0X?k@f={=vaM zme=p^;sWfU8a@x~0kZs#N%ni0`uh$8y?yBf2H_#F1$H7fz*4~Vc3k%3Iy|m(<8}kC zbvqDF1G}NbFN6}Tf&XYCp#`BO(CQMas|ghU24;DW=)?5zC3Rf)}6*(jX(?kVEZrY$jERW zfb#&TvsXSJ=->TfWpU!~W*(T7nUyzf{(>dT)~wsOZO6VpPoKGP{nn!=&;EM(S@Xb& za`V7(^E{AVft)X$2hvt0XD6l3ot-=5@0p7>toY;Vu002i96O|Xu=&!xJ8#~?c>sz2 z!n{#94}8fy09-e?C&%Qv0e%{j@5cOqyNkdZxS|3!JOls+eh7aw@0mVW{2D)wp9}vm zzLk8kN zSq2UoJmh12>c6WvaSiv2z&L3&`$b@#wQ~9}th4rhocMdGvsPjKoMQiYg~v_)agY>c z&W)WbnMF=UHVUk>Ry;qHU(Z_AeHme&_Hy&Uf613A^9z*ZO_X1ERh%FF z|GqC{$@N?03zaMldT*Uk1;@+!GJe88{%L!F!Qu(=rBkLAPto{lD7;T&;lgF+b*~N; z9>*iSC2`)eI5&v%ntH5Q#Cfm(OTJI>DlHDafD2!|C5Vg#i?Wh8r{+&Dn6-FCb_K_` z6dzw&)BWR%VW0Nujg!K80L}wn;2-~P=7C{MHeaN1c6AR72CaQ#^chnOc)#8Si+cj5myoDS*)kcd+nv2+C7cJ~JW#Fk0G`@doc>-uwcT73 z6Hk1^UF4<*le669GzWi#CsG0z`X{!Mtjvm@&E9ly;xF_Me`Fn%DQ)gw)=?E7H_ZdE zA6GT^r`F72)?QYtP}dz`ZA^Ki2lFN1H+0v6HGRj?yZ_+fqsQSqKtx-SS!hW=s>NVsW*xL2`3JNfS{H4IGSLs~&kNB0WG7UF3Q;Lq zfZ)};YNK-yJ&O0EBA^Qp6kPvRJbp_1!0IVxe&2&pk~ zY?=C#;!!qpWm{AI+LCqUWfev&>WlnKzQq^E2OLJ?b+$lFU31A@lcpO8O6f+&n!jPi z$IZDSlXtPrRniU~a?Qv+Jh!w`SkW0*{1Vny)N6emULceCDd^dR z>%%LNRY*xcruE1M#MWjbvI*IYY(d%pgtj*JHtlSFwxQUxx9MQh(WaA4XPYiIU2VGA z{9@DH2D9m5)6?cxn_f1*+4QzKjr6hUYtzrBzs*n^2b*Cw!)-1jBW$iBBW*_6jJ9#K zxdkf1+%eUJ8Dlfn2CoS-&St#L1RJW&bA)C?w|R*$Z2m@=Hh5*22IPj|`&YL<48FmC z+sfjE=Fj7CLa#E|o){+JSP=RCgB2!LK9-zjPjSm_Xz3~F9^skC|JHcyMp68ijPBmm<;EE>YN7v{r|p|U&litm8=V|isv#f%a_W5FU*_! z@A_~_oh8_pu1fpT)o^|I%j+z`zHDDoU*nT<@ZY6m)gAp#IPR~>lCab1=0RT@T|f9+Ra8g=_-Hm>mAyKML~& z%z0Cmb}`Hsc)uUb|0G|a#>+t%FRxDjU`brN!qu_m1Kyvjc9;MA60c(MdA^l?B<6TH z>{nhd^n!`U>Ui8Gq2PyoSrcubszU){o=s^lAN6p8U7oubccSx%Oe4 z#AFqhK2#t;|EpbGnnL=`*u}8k*!yj2G z))UPEeM%Q1i%S1>m*h_(pg-MOWF2^a1L#d>&Ywg;ce<^hr`&eXjSkP}-witD?FGH^ z_Jb~S2aCJRA)s^KanLLKBFU%ls`r``*MMR*1J>b*8~)&r>m_*P;TSC@a#{4FYe#O0tV zg&q{<0(-^s&*V$Q-G)HDE5DVgnB{fBkGw7g{x0R<`E?*ge&auz-@e-l`Z2#>m+$zq z=f!glC!2_v>#8t*#_3wpZnnWiPE};NB%rh|Tn=uw+GI7` z^v?-jS816?jv!4z7f*8@c^omGm_S^IBodQ|I^sqog}51+OiUx{iRr`)VkU74F^gy* zW)pLWxx}f&JYqg^8gV*t2C;xRlZYU*iF1f^iSvl_iG{=k#D&B~#KpuV#HGY##O1^l z#FfNV#MQ(##I?kA#P!4t#NUY!3y-f|Gu6I z>Vsd{mj*HEgO=-QP6K_=yuN&JsT{OiPxG=^AKX&*`cLaq z)~BtDtj}1VwLWKk-ui;|Me9q}m#wc@U$wqweck$o^-b$r*0-(iSl_kA@xG1qG^Ofj zg*)YC9|$miu$d#sBs{{52bRH0@C@|C4?c*$fg_b3AeT`8t;@l^TlZ}{bnJr3EZUnoma13zy9Z$-TUv}y?=ktqeoAklw=~m z#`BPkU%syJe=P@BtTN>f7S98-=3H8O`TVU1GiTpkQZ)DMxuf%rpE!Es_=%#MU{T_H zTpzx2?V3p+zH{&1gNF}+f3Q?vhJD*3kuTQ=w*tF%ugR9hyQQy24p{BBDn!m(odMSC z9P%>IH}@@AUAPC>tjBq)% ze<3o09Q5rCCK!>6$U9(-<2LygqzM^c&4~AmUxXA;HUdo*Y^Cz&tHg_ey!rQdG5Gfv zgD1aO#YBS#uoG=6DvCGIGHqtv4eWv);0XBq4g4AS&$Rv!JeIlxOHTBy#7QPagE|wj zAWo8)Ix{6Tt{^5ItP-AOm<{^2=cVKMj& zv%twzcW}~Fef;G3gs7w>U2?t-#EUaB=I3V3%bu1ywJ;y7FHcL(%APxS-u%Mpi_Gm> zz9fJ0_D8P2hUZBpI}R9Md$}QgbxPY%69@Ovk#dF`jO*(%TFL) zK(@R!Ng)|w5a9yC2i?m{`j$I_zT%#QlZYJj1LuJ5;hTWOd%th_*NKy?AYMW;f%Q8Z z*uHC!oxpa^0haS>;!pOwfu-EZUT(hvQQNER$?e*E%GsY&XW~lR+M@a5~T^H1;vi?Go>q~7iAzNiV{aj zq@+;v6mz_n+^&Gq(!Q0wBakR+`vRnGyXByI#5&|9LZ>h&ObP>KQP>m?g-hX4_!I%f zi6W$kC}N6)BBjVEatZ-eP?QuEMNQFAv=nEG3&oYs4Z%Twnl$K+o0W1GTIikN86#z&>zvRXcx3O+7a!Bc0xO#ozd#n2mg~C z#O+;^6f{2qd$-u+1ro6$`B(CO#S+kb&~E;(*S~5tdF?vWm2WmZzW4f5a_&>dp&b0La?qaqZ*q{0vMF3p zPqIu7rlXc}5Ji;~8?+W$CI|H>l!Ili{(783SYtp}E~6xh?DgbP*^3E{wk#`9*ZfDPV{U_;o0)FbReS`ZE(t$=NQ z7})A0!f;@5<9Rc5!YSl5u*E%rb?!|#5B!b_!X<NK3*! z#Fl{P%`_x5B8(t767akk2H^$JUj+m&!fV6_cmv)dO2Qc{#5A?y57pxzB!3|I%DwTu z$RKo!7si33gldAytsnnW;rY{A2h3jrTXAUl|k_4cc%{)`v+m&v66Z_<~qEE8y4bqc%eRs z$PopiL{x|x(I8sH8F4{e5jVsg@jyHgFT@-1L3|NE#2*Pj0+Apj7zsf_kuW5@%Jjk3 zWOE(bpU8OrU=hfp<`PQe;O^2ow02~39`!4bN6jOY)SX$s&^Eh+fBCh5K6nG@ zfsH|==MEA|XijKH=tJm27)X!4O#MJEi(i z1$J|39N}yG2W6k|4@OPZJ4|*+iyAp{(jMu6bVNEKoslj`SEL*A3(_6IkRC`+4Wq| z`XT+10mwjP5Hc7Uf(%6*kYUJhWCSu28HJ2S9FZ}|SY#YB9+`kp5gI~A7zh(#A#8+$ za1kEDM+AryB1A-p7?B`SMD~Spa1Y2$xdSZA>mXz02FPu>3-VQNgPfFyAU_3{P5VHu z%5Gp)z5v-QZ-6dkG{uWwfy|d8AeAyoAjr?cc3HN-9BQ zg$H6Nvq0_)9-rw2Vl#FW1Vm@9fgBho5N~k-(G(el4&o?tKt9Y-Q%=raq=(5;K7zCW zah*gE$C(H6WTGf1OxZKXL9Wc7AUozA$dkcWo16hw?*&i|d^PBjvjS;O-U9Mw<^w6X z9?0cF5Z~DVDujO<@nZ1ruVe#zxUj^B`UxtVAmu6kbyng-0ty_0z-+pud z{zvjgzH}a_SkC>oK4nw&YbRe!i<)*qr#o%xOObvxH!CYU8_2=DY10?4Id%EYoH_I6 z6&5Z`wagn@g8NIiY%ae3-MdSQxgI-q{KRpPKe^7-1g=_ zpwTn{{=$a9ONe_6Z3!iEPysxJC30}M$qOhW$brA{Ebtk66L2}G1YXG{$OWr&R(q_9 ztjgrz87m(GALRE`N!|Dy0wZ1X|fHAdjR#m#d6jd>zQO8$c6;TebTK!P;R2H&*c{ z)9om>T*1ESb*aKu!M*xum#z=qtl*C=-u^;4IJ9CpSYe;8;yB9xBu-G>a`2OOF`h?W z>KAyw?pCQ^;Qjo`sf$$*cPtHuHg8V;^PZzdLI2^ax|C+-tSv0Bj9~&Q$hY`8lp$u zp9juc*|(Jw@V>Rvz>3RY!k6m9pQ^id^u4-kmU&YbKuqk@c~g(ytGi~IH}w`o)xLqe zDNysmlqv=LG8Fp-U|)v!>reiV`Z9d+JTQEO<2XDq7Uacpc>>@JlPXjit+R`pIThHU z!s8$*1W$@6&Iy?-ohMVcxtA@Vm9D^FgDf%5nqhc-DJfoH$^%43Ii}F)FzIgg9=3-V z7xe7WBSwrHKW-eI?&!$pISB-UyAk#|<^2WgWe5=9V zz&ZlF^jyh?J4&2qHz4uB=s%*q$6xy2J^Il+nj+$Av9tseJ5Ws`! zx%hHI>5xP3TKE{JOdV96JlFsCBla#Pwio0ONNv zM&icGVbv-{GIO|kRDDuy3#`P=wj}))F#Z5zs~JPTt#Vj(%3-+!tmm`JejEB7wxpb$ zVC({8w;3a6Pvx+Fu*4!G$U%U0-2y9TpDn3iKNttVIB3QwI8-^Tx|Uer7kwV!?6tru zIATjGJPO7!Fpirs3QtrHtAPa;{yWrz-{EQttinHSNy|@xaT<&wGsf~WmBVUS4h#Ry z7g%5|KW9r?e;$krU|cj~tiMz_EK4UW{+p)*tP~5Z^;c|3Td#s~4UFq%jIB2+ht