From fc36c771675c91c850fd93a64fd8a9bc146d04ed Mon Sep 17 00:00:00 2001 From: ecker Date: Sun, 26 Apr 2026 20:27:14 -0500 Subject: [PATCH] physics fixes --- Makefile | 4 +- bin/data/config.json | 20 ++--- engine/inc/uf/utils/math/physics/impl.h | 6 +- engine/src/engine/ext/player/behavior.cpp | 22 ++--- engine/src/ext/vulkan/graphic.cpp | 3 - engine/src/ext/vulkan/shader.cpp | 7 +- engine/src/utils/math/physics/bvh.inl | 30 +++++-- engine/src/utils/math/physics/epa.inl | 4 +- engine/src/utils/math/physics/gjk.inl | 4 +- engine/src/utils/math/physics/helpers.inl | 86 ++++++++++--------- engine/src/utils/math/physics/impl.cpp | 8 +- engine/src/utils/math/physics/integration.inl | 16 ++-- engine/src/utils/math/physics/ray.inl | 8 +- engine/src/utils/math/physics/solvers.inl | 7 +- engine/src/utils/math/physics/tests.inl | 42 ++++----- engine/src/utils/math/physics/triangle.inl | 6 +- 16 files changed, 150 insertions(+), 123 deletions(-) diff --git a/Makefile b/Makefile index 186f6608..f22dd4a5 100644 --- a/Makefile +++ b/Makefile @@ -90,8 +90,8 @@ ifneq (,$(findstring ffx:sdk,$(REQ_DEPS))) ifneq (,$(findstring vulkan,$(REQ_DEPS))) FLAGS += -DUF_USE_FFX_SDK=3 #INCS += -I./dep/include/ffx_fsr2/ - DEPS += -lffx_fsr3_x64drel -lffx_fsr2_x64drel -lffx_fsr3upscaler_x64drel -lffx_frameinterpolation_x64drel -lffx_opticalflow_x64drel -lffx_backend_vk_x64drel -lamd_fidelityfx_vkdrel - #DEPS += -lffx_fsr3_x64 -lffx_fsr2_x64 -lffx_fsr3upscaler_x64 -lffx_frameinterpolation_x64 -lffx_opticalflow_x64 -lffx_backend_vk_x64 -lamd_fidelityfx_vk + #DEPS += -lffx_fsr3_x64drel -lffx_fsr2_x64drel -lffx_fsr3upscaler_x64drel -lffx_frameinterpolation_x64drel -lffx_opticalflow_x64drel -lffx_backend_vk_x64drel -lamd_fidelityfx_vkdrel + DEPS += -lffx_fsr3_x64 -lffx_fsr2_x64 -lffx_fsr3upscaler_x64 -lffx_frameinterpolation_x64 -lffx_opticalflow_x64 -lffx_backend_vk_x64 -lamd_fidelityfx_vk endif endif ifneq (,$(findstring vulkan,$(REQ_DEPS))) diff --git a/bin/data/config.json b/bin/data/config.json index 1ae30133..6910bc75 100644 --- a/bin/data/config.json +++ b/bin/data/config.json @@ -66,9 +66,9 @@ "vulkan": { "version": 1.3, "validation": { - "enabled": false, - "messages": false, - "checkpoints": false, + "enabled": true, + "messages": true, + "checkpoints": true, "filters": [ // "0xe5d1743c" // VUID-vkCmdDispatch-None-02699 (problem when using VXGI) (seems to be fixed?) // ,"0x6714bd0c" // VUID-vkCmdDispatch-format-07753 (for some dumb shit) (seems to be fixed?) @@ -101,7 +101,7 @@ "experimental": { "rebuild on tick begin": false, "batch queue submissions": true, - "dedicated thread": true, // mostly works + "dedicated thread": false, // mostly works "memory budget": false, "register render modes": true, "skip render on rebuild": false @@ -109,7 +109,7 @@ "invariant": { "default stage buffers": true, "default defer buffer destroy": true, - "default command buffer immediate": false, + "default command buffer immediate": true, "multithreaded recording": true }, "pipelines": { @@ -117,10 +117,10 @@ "gui": true, "vsync": true, // vsync on vulkan side rather than engine-side "hdr": true, - "vxgi": true, - "culling": true, - "bloom": true, - "dof": true, + "vxgi": false, + "culling": false, + "bloom": false, + "dof": false, "rt": false, "fsr": false, "postProcess": false // "postProcess.chromab" // false @@ -363,7 +363,7 @@ "render modes": { "gui": true, "deferred": true }, "limiters": { "deltaTime": 5, - "framerate": 0 // "auto" // for some reason drops to 60 + "framerate": "auto" // "auto" // for some reason drops to 60 }, "threads": { "workers" : "auto", diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index cddad46d..dd932bd7 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -161,8 +161,8 @@ namespace pod { float sleepTimer = 0.0f; int32_t islandID = -1; static constexpr float sleepThreshold = 0.5f; // seconds - static constexpr float linearSleepEpsilon = 0.01f; // m/s - static constexpr float angularSleepEpsilon = 0.01f; // rad/s + static constexpr float linearSleepEpsilon = 0.1f; // m/s + static constexpr float angularSleepEpsilon = 0.1f; // rad/s }; struct World; // forward declare @@ -232,7 +232,7 @@ namespace pod { bool blockContactSolver = true; // use BlockNxN solvers (where N = number of contacts for a manifold) bool psgContactSolver = true; // use PSG contact solver bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK - bool fixedStep = true; // run physics simulation with a fixed delta time (with accumulation), rather than rely on actual engine deltatime + bool fixedStep = false; // run physics simulation with a fixed delta time (with accumulation), rather than rely on actual engine deltatime uint32_t substeps = 4; // number of substeps per frame tick uint32_t reserveCount = 32; // amount of elements to reserve for vectors used in this system, to-do: have it tie to a memory pool allocator diff --git a/engine/src/engine/ext/player/behavior.cpp b/engine/src/engine/ext/player/behavior.cpp index df7702bc..aa814d42 100644 --- a/engine/src/engine/ext/player/behavior.cpp +++ b/engine/src/engine/ext/player/behavior.cpp @@ -19,6 +19,8 @@ #include "../scene/behavior.h" // #include "../../gui/manager/behavior.h" +#define ONE_OVER_SIXTY 0.016666f + UF_BEHAVIOR_REGISTER_CPP(ext::PlayerBehavior) UF_BEHAVIOR_TRAITS_CPP(ext::PlayerBehavior, ticks = true, renders = false, multithread = true) #define this (&self) @@ -94,8 +96,8 @@ void ext::PlayerBehavior::initialize( uf::Object& self ) { const pod::Vector2ui deadZone{0, 0}; if ( (payload.mouse.delta.x == 0 && payload.mouse.delta.y == 0) || !metadata.system.control ) return; - if (abs(payload.mouse.delta.x) > deadZone.x) metadata.mouse.accum.x += payload.mouse.delta.x * uf::physics::time::delta / payload.window.size.x; - if (abs(payload.mouse.delta.y) > deadZone.y) metadata.mouse.accum.y += payload.mouse.delta.y * uf::physics::time::delta / payload.window.size.y; + if (abs(payload.mouse.delta.x) > deadZone.x) metadata.mouse.accum.x += payload.mouse.delta.x * (ONE_OVER_SIXTY)/* uf::physics::time::delta*/ / payload.window.size.x; + if (abs(payload.mouse.delta.y) > deadZone.y) metadata.mouse.accum.y += payload.mouse.delta.y * (ONE_OVER_SIXTY)/* uf::physics::time::delta*/ / payload.window.size.y; }); #endif #endif @@ -347,7 +349,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( physicsBody.gravity == pod::Vector3f{0,0,0} ) stats.noclipped = true; { - speed.rotate = metadata.movement.rotate * uf::physics::time::delta; + speed.rotate = metadata.movement.rotate * ONE_OVER_SIXTY /*uf::physics::time::delta*/; speed.move = metadata.movement.move; speed.run = metadata.movement.run; speed.walk = metadata.movement.walk; @@ -466,7 +468,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( stats.noclipped ) { physicsBody.velocity += target * speed.move; } else { - physicsBody.velocity += target * std::clamp( speed.move * factor - uf::vector::dot( physicsBody.velocity, target ), 0.0f, speed.move * 10 * uf::physics::time::delta ); + physicsBody.velocity += target * std::clamp( speed.move * factor - uf::vector::dot( physicsBody.velocity, target ), 0.0f, speed.move * 10 * ONE_OVER_SIXTY /*uf::physics::time::delta*/ ); } auto dot = uf::vector::dot( transform.forward, target ); @@ -474,7 +476,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { // auto cross = uf::vector::normalize( uf::vector::cross( transform.forward, target ) ); // auto axis = cross == pod::Vector3f{0, 0, 0} ? transform.up : cross; auto axis = transform.up; - float angle = uf::vector::signedAngle( transform.forward, target, axis ) * uf::physics::time::delta * 4; // speed.rotate; + float angle = uf::vector::signedAngle( transform.forward, target, axis ) * ONE_OVER_SIXTY /*uf::physics::time::delta*/ * 4; // speed.rotate; if ( physicsBody.object ) uf::physics::impl::applyRotation( physicsBody, axis, angle ); else uf::transform::rotate( transform, axis, angle ); @@ -485,9 +487,9 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { TIMER(0.0625, stats.floored && keys.jump && !stats.noclipped ) { physicsBody.velocity += translator.up * metadata.movement.jump; } - if ( stats.floored && keys.jump && stats.noclipped ) transform.position += translator.up * metadata.movement.jump * uf::physics::time::delta * 4.0f; + if ( stats.floored && keys.jump && stats.noclipped ) transform.position += translator.up * metadata.movement.jump * ONE_OVER_SIXTY /*uf::physics::time::delta*/ * 4.0f; if ( keys.crouch ) { - if ( stats.noclipped ) transform.position -= translator.up * metadata.movement.jump * uf::physics::time::delta * 4.0f; + if ( stats.noclipped ) transform.position -= translator.up * metadata.movement.jump * ONE_OVER_SIXTY /*uf::physics::time::delta*/ * 4.0f; else { if ( !metadata.system.crouching ) stats.deltaCrouch = true; metadata.system.crouching = true; @@ -504,8 +506,8 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { const auto& mouseDelta = uf::inputs::kbm::states::Mouse; bool shouldnt = (mouseDelta.x == 0 && mouseDelta.y == 0) || !metadata.system.control || metadata.camera.fixed; if ( !shouldnt ) { - if (abs(mouseDelta.x) > deadZone.x) metadata.mouse.accum.x += mouseDelta.x * uf::physics::time::delta; - if (abs(mouseDelta.y) > deadZone.y) metadata.mouse.accum.y += mouseDelta.y * uf::physics::time::delta; + if (abs(mouseDelta.x) > deadZone.x) metadata.mouse.accum.x += mouseDelta.x * (ONE_OVER_SIXTY)/* uf::physics::time::delta*/; + if (abs(mouseDelta.y) > deadZone.y) metadata.mouse.accum.y += mouseDelta.y * (ONE_OVER_SIXTY)/* uf::physics::time::delta*/; } } #endif @@ -564,7 +566,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { } { if ( physicsBody.object ) uf::physics::impl::setVelocity( physicsBody, physicsBody.velocity ); else - transform.position += physicsBody.velocity * uf::physics::time::delta; + transform.position += physicsBody.velocity * ONE_OVER_SIXTY /*uf::physics::time::delta*/; // if ( uf::vector::magnitude( physicsBody.velocity ) > 1.0e-6 ) UF_MSG_DEBUG("Velocity: {}", uf::vector::toString( physicsBody.velocity )); } diff --git a/engine/src/ext/vulkan/graphic.cpp b/engine/src/ext/vulkan/graphic.cpp index f296772a..b85ba3e4 100644 --- a/engine/src/ext/vulkan/graphic.cpp +++ b/engine/src/ext/vulkan/graphic.cpp @@ -397,9 +397,6 @@ void ext::vulkan::Pipeline::record( const Graphic& graphic, VkCommandBuffer comm } void ext::vulkan::Pipeline::record( const Graphic& graphic, const GraphicDescriptor& descriptor, VkCommandBuffer commandBuffer, size_t pass, size_t draw, size_t offset ) const { auto shaders = getShaders( graphic.material.shaders, descriptor.pipeline ); - for ( auto i = 0; i < shaders.size(); ++i ) { - // UF_MSG_DEBUG("{} | {}: {}", descriptor.pipeline, i, shaders[i]->filename); - } // create dynamic offset ranges static thread_local uf::stl::vector dynamicOffsets; diff --git a/engine/src/ext/vulkan/shader.cpp b/engine/src/ext/vulkan/shader.cpp index fc42393a..3deaf5c9 100644 --- a/engine/src/ext/vulkan/shader.cpp +++ b/engine/src/ext/vulkan/shader.cpp @@ -15,6 +15,8 @@ #define VK_DEBUG_VALIDATION_MESSAGE(...)\ //VK_VALIDATION_MESSAGE(__VA_ARGS__); +#define IS_DYNAMIC(name) name == "UBO_d" + #define UF_SHADER_PARSE_AS_JSON 0 #if UF_SHADER_PARSE_AS_JSON ext::json::Value ext::vulkan::definitionToJson(/*const*/ ext::json::Value& definition ) { @@ -519,7 +521,10 @@ void ext::vulkan::Shader::initialize( ext::vulkan::Device& device, const uf::stl }; if ( descriptorType == VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER_DYNAMIC ) { - metadata.dynamicRanges.emplace_back( bufferSize ); + if ( IS_DYNAMIC(name) ) { + UF_MSG_DEBUG("Registered dynamic UBO: {}", name); + } + metadata.dynamicRanges.emplace_back( IS_DYNAMIC(name) ? bufferSize : 0 ); } } break; case VK_DESCRIPTOR_TYPE_STORAGE_BUFFER: diff --git a/engine/src/utils/math/physics/bvh.inl b/engine/src/utils/math/physics/bvh.inl index 42c0e823..6f58bbbe 100644 --- a/engine/src/utils/math/physics/bvh.inl +++ b/engine/src/utils/math/physics/bvh.inl @@ -83,7 +83,7 @@ namespace { float bestCost = std::numeric_limits::infinity(); for ( auto axis = 0; axis < 3; ++axis ) { - if ( extent[axis] < EPS(1e-6f) ) continue; + if ( extent[axis] < EPS ) continue; for ( auto i = 0; i < numBins; i++ ) { bins[i].count = 0; bins[i].bounds = {}; @@ -819,7 +819,9 @@ namespace { // union all pairs for ( auto& [a, b] : pairs ) { - unionizer.unite(a, b); + if ( !bodies[a]->isStatic && !bodies[b]->isStatic ) { + unionizer.unite(a, b); + } } // map root to island index @@ -830,9 +832,11 @@ namespace { islands.reserve(bodies.size()); for ( auto i = 0; i < bodies.size(); i++ ) { + if ( bodies[i]->isStatic ) continue; + pod::BVH::index_t root = unionizer.find(i); - if (rootToIsland.find(root) == rootToIsland.end()) { + if ( rootToIsland.find(root) == rootToIsland.end() ) { rootToIsland[root] = (pod::BVH::index_t) islands.size(); islands.emplace_back(); } @@ -846,9 +850,19 @@ namespace { // do not insert these pairs if they're non-colliding if ( !::shouldCollide( *bodies[a], *bodies[b] ) ) continue; + // just in case + pod::BVH::index_t dynamicIndex = bodies[a]->isStatic ? b : a; + if ( bodies[a]->isStatic && bodies[b]->isStatic ) continue; + pod::BVH::index_t root = unionizer.find(a); - pod::BVH::index_t islandID = rootToIsland[root]; - islands[islandID].pairs.emplace(a, b); + if ( rootToIsland.find(root) != rootToIsland.end() ) { + pod::BVH::index_t islandID = rootToIsland[root]; + islands[islandID].pairs.emplace(a, b); + + if ( bodies[a]->activity.awake || bodies[b]->activity.awake ) { + ::wakeBody( *bodies[dynamicIndex] ); + } + } } } @@ -859,10 +873,10 @@ namespace { auto& body = *bodies[idx]; if ( !body.activity.awake ) continue; - float linSpeed = uf::vector::norm( body.velocity ); - float angSpeed = uf::vector::norm( body.angularVelocity ); + float linSpeedSq = uf::vector::magnitude( body.velocity ); + float angSpeedSq = uf::vector::magnitude( body.angularVelocity ); - if ( linSpeed < pod::Activity::linearSleepEpsilon && angSpeed < pod::Activity::angularSleepEpsilon) { + if ( linSpeedSq < pod::Activity::linearSleepEpsilon && angSpeedSq < pod::Activity::angularSleepEpsilon) { body.activity.sleepTimer += dt; } else { body.activity.sleepTimer = 0.0f; diff --git a/engine/src/utils/math/physics/epa.inl b/engine/src/utils/math/physics/epa.inl index c41c70da..f9784544 100644 --- a/engine/src/utils/math/physics/epa.inl +++ b/engine/src/utils/math/physics/epa.inl @@ -17,7 +17,7 @@ namespace { const auto& C = s.pts[2].p; const auto& D = s.pts[3].p; - return fabs( uf::vector::dot( (B - A), uf::vector::cross(C - A, D - A) ) ) > EPS(1e-6f); + return fabs( uf::vector::dot( (B - A), uf::vector::cross(C - A, D - A) ) ) > EPS; } pod::Face makeFace( const pod::SupportPoint& a, const pod::SupportPoint& b, const pod::SupportPoint& c ) { @@ -80,7 +80,7 @@ namespace { } } - pod::Contact epa( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Simplex& simplex, uint32_t maxIterations = 64, float eps = EPS(1.0e-4f) ) { + pod::Contact epa( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Simplex& simplex, uint32_t maxIterations = 64, float eps = EPS ) { UF_ASSERT( ::isValidSimplex(simplex) ); auto faces = ::initialPolytope(simplex); diff --git a/engine/src/utils/math/physics/gjk.inl b/engine/src/utils/math/physics/gjk.inl index 8e6011d9..136f93c2 100644 --- a/engine/src/utils/math/physics/gjk.inl +++ b/engine/src/utils/math/physics/gjk.inl @@ -123,7 +123,7 @@ namespace { return false; } - bool isDegenerate( const pod::Simplex& s, const pod::SupportPoint& newPt, float eps = EPS(1.0e-6f) ) { + bool isDegenerate( const pod::Simplex& s, const pod::SupportPoint& newPt, float eps = EPS ) { const float eps2 = eps * eps; // compare to existing for ( auto& sp : s.pts ) { @@ -150,7 +150,7 @@ namespace { return false; } - bool gjk( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Simplex& simplex, int maxIterations = 20, float eps = EPS(1e-6f) ) { + bool gjk( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Simplex& simplex, int maxIterations = 20, float eps = EPS ) { const float eps2 = eps * eps; auto dir = ::getPosition( b ) - ::getPosition( a ); if ( uf::vector::magnitude( dir ) < eps2 ) dir = {1,0,0}; // fallback direction diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index 3105fd65..50db6cb8 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -6,44 +6,44 @@ // forward declare namespace { - bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool aabbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool aabbPlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool aabbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool aabbMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool aabbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool aabbPlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool aabbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool aabbMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); - bool sphereSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool sphereAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool spherePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool sphereCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool sphereMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool sphereSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool sphereAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool spherePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool sphereCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool sphereMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); - bool planeAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool planeSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool planePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool planeCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool planeMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool planeAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool planeSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool planePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool planeCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool planeMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); - bool capsuleCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool capsuleAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool capsulePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool capsuleSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool capsuleMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool capsuleCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool capsuleAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool capsulePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool capsuleSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool capsuleMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); - bool meshAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool meshSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool meshPlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool meshCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool meshMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool meshAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool meshSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool meshPlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool meshCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); + bool meshMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); - bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); - bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold, float eps = EPS ); + bool triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS ); + bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS ); + bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS ); + bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS ); // ugh - FORCE_INLINE bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS(1.0e-6f) ); + FORCE_INLINE bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS ); pod::Vector3f aabbCenter( const pod::AABB& aabb ); @@ -69,10 +69,15 @@ namespace { // marks a body as asleep void wakeBody( pod::PhysicsBody& body ) { + bool wasAwake = body.activity.awake; + if ( !wasAwake ) UF_MSG_DEBUG("name={} waking up", body.object->getName()); + body.activity.awake = true; body.activity.sleepTimer = 0.0f; } void sleepBody( pod::PhysicsBody& body ) { + if ( body.activity.awake ) UF_MSG_DEBUG("name={} sleeping", body.object->getName()); + body.activity.awake = false; body.velocity = pod::Vector3f{}; body.angularVelocity = pod::Vector3f{}; @@ -86,13 +91,14 @@ namespace { if ( !body.activity.awake ) return; // check if body is moving - float linSpeed = uf::vector::norm( body.velocity ); - float angSpeed = uf::vector::norm( body.angularVelocity ); + float linSpeedSq = uf::vector::magnitude( body.velocity ); + float angSpeedSq = uf::vector::magnitude( body.angularVelocity ); // body is nearly still - if ( linSpeed < pod::Activity::linearSleepEpsilon && angSpeed < pod::Activity::angularSleepEpsilon ) { + if ( linSpeedSq < pod::Activity::linearSleepEpsilon && angSpeedSq < pod::Activity::angularSleepEpsilon ) { body.activity.sleepTimer += dt; float threshold = pod::Activity::sleepThreshold; + if ( wasGrounded ) threshold *= 0.25f; if ( body.activity.sleepTimer > threshold ) ::sleepBody( body ); } @@ -133,7 +139,7 @@ namespace { // normalizes the delta between two bodies / contacts by the distance (as it was already computed) if non-zero // a lot of collider v colliders use this semantic - pod::Vector3f normalizeDelta( const pod::Vector3f& delta, float dist, float eps = EPS(1.0e-6), const pod::Vector3f& fallback = pod::Vector3f{0,1,0} ) { + pod::Vector3f normalizeDelta( const pod::Vector3f& delta, float dist, float eps = EPS, const pod::Vector3f& fallback = pod::Vector3f{0,1,0} ) { return ( dist > eps ) ? delta / dist : fallback; } @@ -149,7 +155,7 @@ namespace { return a + ab * t; } - std::pair closestSegmentSegment( const pod::Vector3f& A, const pod::Vector3f& B, const pod::Vector3f& C, const pod::Vector3f& D, float eps = EPS(1e-6f) ) { + std::pair closestSegmentSegment( const pod::Vector3f& A, const pod::Vector3f& B, const pod::Vector3f& C, const pod::Vector3f& D, float eps = EPS ) { auto u = B - A; auto v = D - C; auto w = A - C; @@ -195,7 +201,7 @@ namespace { return { A + u * sc, C + v * tc }; } - pod::Vector3f closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box, float eps = EPS(1e-6f) ) { + pod::Vector3f closestPointSegmentAabb( const pod::Vector3f& p1, const pod::Vector3f& p2, const pod::AABB& box, float eps = EPS ) { const float eps2 = eps * eps; // AABB center and half extents auto c = ( box.min + box.max ) * 0.5f; @@ -282,11 +288,11 @@ namespace { return ::interpolateWithBarycentric( bary, points[0], points[1], points[2] ); } - bool pointInTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, float eps = EPS(1.0e-6f) ) { + bool pointInTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c, float eps = EPS ) { auto bary = ::computeBarycentric( p, a, b, c, false ); return ( bary.x >= -eps && bary.y >= -eps && bary.z >= -eps ); } - bool pointInTriangle( const pod::Vector3f& p, const pod::Triangle& tri, float eps = EPS(1.0e-6f) ) { + bool pointInTriangle( const pod::Vector3f& p, const pod::Triangle& tri, float eps = EPS ) { auto bary = ::computeBarycentric( p, tri, false ); return ( bary.x >= -eps && bary.y >= -eps && bary.z >= -eps ); } @@ -347,7 +353,7 @@ namespace { return uf::vector::normalize( uf::vector::dot( n, ::getPosition( b ) - ::getPosition( a ) ) < 0.0f ? -n : n ); } - float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri, float eps = EPS(1.0e-6f) ) { + float segmentTriangleDistanceSq( const pod::Vector3f& p0, const pod::Vector3f& p1, const pod::Triangle& tri, pod::Vector3f& outSeg, pod::Vector3f& outTri, float eps = EPS ) { float best = std::numeric_limits::max(); auto n = uf::vector::cross( tri.points[1]-tri.points[0], tri.points[2]-tri.points[0] ); diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index 7ecf208c..31fff4e7 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -4,8 +4,8 @@ #include #include -#define EPS(x) 1.0e-6f -#define EPS2 (EPS(1.0e-6) * EPS(1.0e-6)) +#define EPS 1.0e-6f +#define EPS2 (EPS * EPS) #define ASSERT_COLLIDER_TYPES( A, B ) UF_ASSERT( a.collider.type == pod::ShapeType::A && b.collider.type == pod::ShapeType::B ); #define UF_PHYSICS_TEST 0 @@ -259,7 +259,7 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) { pod::Vector3f dims = (body.collider.aabb.max - body.collider.aabb.min); pod::Vector3f dimsSq = dims * dims; body.inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f); - body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS(1.0e-6f), EPS(1.0e-6f), EPS(1.0e-6f) } ); + body.inertiaTensor = uf::vector::max( body.inertiaTensor, { EPS, EPS, EPS } ); body.inverseInertiaTensor = 1.0f / body.inertiaTensor; } break; case pod::ShapeType::SPHERE: { @@ -295,7 +295,7 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) { totalVolume += extents.x * extents.y * extents.z; } - if ( totalVolume < EPS(1.0e-6) ) { + if ( totalVolume < EPS ) { body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; } else { diff --git a/engine/src/utils/math/physics/integration.inl b/engine/src/utils/math/physics/integration.inl index 2f49430b..88d2017d 100644 --- a/engine/src/utils/math/physics/integration.inl +++ b/engine/src/utils/math/physics/integration.inl @@ -17,7 +17,7 @@ namespace { } float result = inverseMass + angularTermA + angularTermB; - if (result < EPS(1e-8f)) result = 1.0f; // prevent divide by zero + if (result < EPS) result = 1.0f; // prevent divide by zero return result; } @@ -242,16 +242,17 @@ namespace { void snapVelocity( pod::PhysicsBody& body, float dt, float threshold = 0.01f ) { if ( !body.activity.grounded || !body.activity.awake ) return; + float thresholdSq = threshold * threshold; // snap velocity if body is grounded and nearly still - float linSpeed = uf::vector::norm( body.velocity ); - float angSpeed = uf::vector::norm( body.angularVelocity ); + float linSpeedSq = uf::vector::magnitude( body.velocity ); + float angSpeedSq = uf::vector::magnitude( body.angularVelocity ); // cancel out vertical component - if ( fabs(body.velocity.y) < threshold ) body.velocity.y = 0.0f; + if ( fabs(body.velocity.y) < thresholdSq ) body.velocity.y = 0.0f; // cancel out velocity entirely - if ( linSpeed < threshold ) body.velocity = {}; + if ( linSpeedSq < thresholdSq ) body.velocity = {}; // cancel out rotational velocity entirely - if ( angSpeed < threshold ) body.angularVelocity = {}; + if ( angSpeedSq < thresholdSq ) body.angularVelocity = {}; } // baumgarte position correction @@ -267,7 +268,7 @@ namespace { float invMassA = ( a.isStatic ? 0.0f : a.inverseMass ); float invMassB = ( b.isStatic ? 0.0f : b.inverseMass ); float totalInvMass = invMassA + invMassB; - if ( totalInvMass <= EPS(1e-8f) ) return; + if ( totalInvMass <= EPS ) return; // apply correction vector pod::Vector3f correction = contact.normal * (penetration / totalInvMass) * uf::physics::impl::settings.baumgarteCorrectionPercent; @@ -278,7 +279,6 @@ namespace { void integrate( pod::PhysicsBody& body, float dt ) { - // only integrate awake and dynamic bodies if ( !body.activity.awake || body.isStatic || body.mass == 0 ) return; diff --git a/engine/src/utils/math/physics/ray.inl b/engine/src/utils/math/physics/ray.inl index 5ea89686..2e9b4ccc 100644 --- a/engine/src/utils/math/physics/ray.inl +++ b/engine/src/utils/math/physics/ray.inl @@ -1,5 +1,5 @@ namespace { - bool rayTriangleIntersect( const pod::Ray& ray, const pod::Triangle& tri, float& t, float& u, float& v, float eps = EPS(1.0e-6f) ) { + bool rayTriangleIntersect( const pod::Ray& ray, const pod::Triangle& tri, float& t, float& u, float& v, float eps = EPS ) { auto edge1 = tri.points[1] - tri.points[0]; auto edge2 = tri.points[2] - tri.points[0]; auto h = uf::vector::cross( ray.direction, edge2 ); @@ -20,7 +20,7 @@ namespace { return ( t > eps ); } - bool rayAabbIntersect( const pod::Ray& ray, const pod::AABB& box, float& tMin, float& tMax, float eps = EPS(1.0e-6f) ) { + bool rayAabbIntersect( const pod::Ray& ray, const pod::AABB& box, float& tMin, float& tMax, float eps = EPS ) { tMin = 0.0f; tMax = FLT_MAX; @@ -108,7 +108,7 @@ namespace { rayHit.contact.penetration = t; return true; } - bool rayPlane( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit, float eps = EPS(1e-6f) ) { + bool rayPlane( const pod::Ray& ray, const pod::PhysicsBody& body, pod::RayQuery& rayHit, float eps = EPS ) { auto& normal = body.collider.plane.normal; float offset = body.collider.plane.offset; @@ -146,7 +146,7 @@ namespace { float b = dd * uf::vector::dot(m,n) - nd*md; float discr = b*b - a*c; - if ( fabs(a) < EPS(1.0e-6f) || discr < 0.0f ) return false; + if ( fabs(a) < EPS || discr < 0.0f ) return false; float t = (-b - std::sqrt(discr)) / a; // nearer hit diff --git a/engine/src/utils/math/physics/solvers.inl b/engine/src/utils/math/physics/solvers.inl index d6bd6e81..a2072736 100644 --- a/engine/src/utils/math/physics/solvers.inl +++ b/engine/src/utils/math/physics/solvers.inl @@ -140,6 +140,7 @@ namespace { pod::Vector3f vB = b.velocity + uf::vector::cross( b.angularVelocity, contact.point - pB ); float vRel = uf::vector::dot((vB - vA), contact.normal); + /* // penetration bias with clamp float penetrationBias = std::max(contact.penetration - uf::physics::impl::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::impl::settings.baumgarteCorrectionPercent / dt); penetrationBias = std::min(penetrationBias, 2.0f / dt); // clamp @@ -148,9 +149,11 @@ namespace { if ( penetrationBias > maxPenetrationRecovery ) penetrationBias = maxPenetrationRecovery; float cDot = vRel + penetrationBias; - rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f; // RHS is magnitude of correction needed lambda[i] = contact.accumulatedNormalImpulse; + */ + rhs[i] = (vRel < 0.0f) ? -vRel : 0.0f; + lambda[i] = contact.accumulatedNormalImpulse; } residual = rhs - uf::matrix::multiply( K, lambda ); @@ -215,7 +218,7 @@ namespace { // restitution bias + baumgarte float e = std::min( a.material.restitution, b.material.restitution ); float penetrationBias = std::max( c.penetration - uf::physics::impl::settings.baumgarteCorrectionSlop, 0.0f ) * (uf::physics::impl::settings.baumgarteCorrectionPercent / dt); - cc.bias = (vn < -1.0f ? -e * vn : 0.0f) + penetrationBias; + cc.bias = 0; // (vn < -1.0f ? -e * vn : 0.0f) + penetrationBias; // effective mass (normal) pod::Vector3f rnA = uf::vector::cross( cc.rA, cc.normal ); diff --git a/engine/src/utils/math/physics/tests.inl b/engine/src/utils/math/physics/tests.inl index 766f5902..7ff7267a 100644 --- a/engine/src/utils/math/physics/tests.inl +++ b/engine/src/utils/math/physics/tests.inl @@ -45,7 +45,7 @@ TEST(SphereSphere_Collision, { bool collided = sphereSphere(bodyA, bodyB, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); - EXPECT_NEAR(m.points[0].penetration, 0.5f, EPS(1e-4f)); + EXPECT_NEAR(m.points[0].penetration, 0.5f, EPS); }) TEST(AabbAabb_Collision, { @@ -81,7 +81,7 @@ TEST(RaySphere_Hit, { pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); EXPECT_TRUE(hit.hit); - EXPECT_NEAR(hit.contact.penetration, 4.0f, EPS(1e-4f)); + EXPECT_NEAR(hit.contact.penetration, 4.0f, EPS); }) TEST(SphereSphere_NoCollision, { @@ -138,7 +138,7 @@ TEST(SpherePlane_Collision, { bool collided = planeSphere(bodyB, bodyA, m); EXPECT_TRUE(collided); EXPECT_TRUE(!m.points.empty()); - EXPECT_NEAR(m.points[0].penetration, 0.5f, EPS(1e-4f)); + EXPECT_NEAR(m.points[0].penetration, 0.5f, EPS); }) TEST(SpherePlane_NoCollision, { @@ -308,7 +308,7 @@ TEST(SphereSphere_TouchingButNotOverlapping, { bool collided = sphereSphere(bodyA, bodyB, m); EXPECT_TRUE(collided); // should count as a collision - EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS(1e-6f)); + EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS); }) // expects being inside the body will set the hit to where the origin is @@ -326,7 +326,7 @@ TEST(RaySphere_OriginInside, { auto q = uf::physics::impl::rayCast(ray, world, 100.0f); EXPECT_TRUE(q.hit); - EXPECT_NEAR(q.contact.penetration, 0.0f, EPS(1e-6f)); + EXPECT_NEAR(q.contact.penetration, 0.0f, EPS); }) #endif @@ -944,13 +944,13 @@ TEST(TriangleTriangle_Collision_SimpleOverlap, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS); EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); if ( !m.points.empty() ) { EXPECT_GE(m.points[0].penetration, 0.0f); - EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS(1e-6f)); + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); } }) @@ -976,7 +976,7 @@ TEST(TriangleTriangle_Collision_CoplanarOverlap, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS); EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); @@ -1004,13 +1004,13 @@ TEST(TriangleTriangle_Collision_TouchingEdge, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleTriangle(bodyA, bodyB, m, EPS); // Should still report as collision (tangent contact) EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); if(!m.points.empty()) { - EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS(1e-6f)); + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); } }) @@ -1033,7 +1033,7 @@ TEST(TriangleAabb_Collision_OverlapCenter, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleAabb(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleAabb(bodyA, bodyB, m, EPS); EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); @@ -1061,7 +1061,7 @@ TEST(TriangleAabb_Collision_NoOverlap, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleAabb(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleAabb(bodyA, bodyB, m, EPS); EXPECT_FALSE(collided); }) @@ -1085,7 +1085,7 @@ TEST(TrianglePlane_Collision_BelowPlane, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::trianglePlane(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::trianglePlane(bodyA, bodyB, m, EPS); EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); @@ -1113,7 +1113,7 @@ TEST(TrianglePlane_NoCollision_AbovePlane, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::trianglePlane(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::trianglePlane(bodyA, bodyB, m, EPS); EXPECT_FALSE(collided); }) @@ -1137,13 +1137,13 @@ TEST(TriangleSphere_Collision_Tangent, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleSphere(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleSphere(bodyA, bodyB, m, EPS); // At tangency: considered collision EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); if(!m.points.empty()) { - EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS(1e-6f)); + EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS); } }) @@ -1170,7 +1170,7 @@ TEST(TriangleCapsule_Collision_Overlap, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS); EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); @@ -1201,7 +1201,7 @@ TEST(TriangleCapsule_Collision_NoOverlap, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS); EXPECT_FALSE(collided); }) @@ -1227,13 +1227,13 @@ TEST(TriangleCapsule_Collision_Tangent, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS); // At tangency, should still count as collision (penetration ≈ 0) EXPECT_TRUE(collided); EXPECT_FALSE(m.points.empty()); if(!m.points.empty()) { - EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS(1e-6f)); + EXPECT_NEAR(m.points[0].penetration, 0.0f, EPS); } }) @@ -1260,7 +1260,7 @@ TEST(TriangleCapsule_Collision_EdgeAlignment, { bodyB.bounds = ::computeAABB( bodyB ); pod::Manifold m; - bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS(1e-6f)); + bool collided = ::triangleCapsule(bodyA, bodyB, m, EPS); EXPECT_TRUE(collided); }) \ No newline at end of file diff --git a/engine/src/utils/math/physics/triangle.inl b/engine/src/utils/math/physics/triangle.inl index e833e28c..92211e1c 100644 --- a/engine/src/utils/math/physics/triangle.inl +++ b/engine/src/utils/math/physics/triangle.inl @@ -15,7 +15,7 @@ namespace { //return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] ); } - bool triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b, float eps = EPS(1e-6f) ) { + bool triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b, float eps = EPS ) { const float eps2 = eps * eps; auto boxA = ::computeTriangleAABB( a ); auto boxB = ::computeTriangleAABB( b ); @@ -148,7 +148,7 @@ namespace { return tri; } - bool computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1, float eps = EPS(1e-6f) ) { + bool computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1, float eps = EPS ) { int intersections = 0; pod::Vector3f intersectionBuffers[6]; @@ -280,7 +280,7 @@ namespace { if ( triMin > r || triMax < -r ) return false; // separating axis // compute overlap depth - float overlap = std::min(triMax, r) - std::max(triMin, -r); + float overlap = std::min(triMax + r, r - triMin); if ( overlap < minOverlap ) { minOverlap = overlap; bestAxis = n;