From 60527c4e6bb6b7908071bbaf62ae6d5c92ae9eac Mon Sep 17 00:00:00 2001 From: ecker Date: Thu, 30 Apr 2026 22:44:32 -0500 Subject: [PATCH] more physics fixes (somewhat finished hulls, gravity-related jitter-bounces from position correction (removed it), fixed BVH bugs, islands/bodies always being awake, refit/rebuild strategies, non-flattened BVHs not working, spheres not rolling, etc) --- bin/data/config.json | 2 +- bin/data/entities/burger.json | 2 +- bin/data/entities/physics_prop.json | 5 +- bin/data/entities/prop.json | 5 +- bin/data/entities/scripts/player.lua | 2 +- .../scenes/sourceengine/mds_mcdonalds.json | 2 +- engine/inc/uf/utils/math/physics/impl.h | 19 ++- engine/inc/uf/utils/mesh/mesh.h | 5 +- engine/src/engine/ext/player/behavior.cpp | 8 +- engine/src/utils/math/physics/aabb.inl | 19 +-- engine/src/utils/math/physics/bvh.inl | 121 ++++++++------ engine/src/utils/math/physics/convexHull.inl | 11 +- engine/src/utils/math/physics/epa.inl | 151 +++++++++++++++++- engine/src/utils/math/physics/gjk.inl | 27 +--- engine/src/utils/math/physics/helpers.inl | 47 +++++- engine/src/utils/math/physics/impl.cpp | 31 ++-- engine/src/utils/math/physics/integration.inl | 95 ++++++++--- engine/src/utils/math/physics/solvers.inl | 27 +--- engine/src/utils/math/physics/triangle.inl | 88 +++------- 19 files changed, 409 insertions(+), 258 deletions(-) diff --git a/bin/data/config.json b/bin/data/config.json index 0a0820f6..8b32cd6c 100644 --- a/bin/data/config.json +++ b/bin/data/config.json @@ -117,7 +117,7 @@ "gui": true, "vsync": true, // vsync on vulkan side rather than engine-side "hdr": true, - "vxgi": true, + "vxgi": false, "culling": true, "bloom": true, "dof": true, diff --git a/bin/data/entities/burger.json b/bin/data/entities/burger.json index 9384aae8..af3855ad 100644 --- a/bin/data/entities/burger.json +++ b/bin/data/entities/burger.json @@ -1,7 +1,7 @@ { "type": "Object", "name": "Burger", - "ignore": false, + "ignore": true, "import": "/model.json", "assets": [ // "/burger/burger.glb" diff --git a/bin/data/entities/physics_prop.json b/bin/data/entities/physics_prop.json index a675afcb..d0f44117 100644 --- a/bin/data/entities/physics_prop.json +++ b/bin/data/entities/physics_prop.json @@ -6,12 +6,11 @@ "metadata": { "holdable": true, "physics": { - // "gravity": [ 0, -1, 0 ], "mass": 100, "inertia": false, // "type": "bounding box" - "type": "mesh" - // "type": "hull" + // "type": "mesh" + "type": "hull" } } } \ No newline at end of file diff --git a/bin/data/entities/prop.json b/bin/data/entities/prop.json index 9771eaec..31188987 100644 --- a/bin/data/entities/prop.json +++ b/bin/data/entities/prop.json @@ -6,11 +6,12 @@ "metadata": { "holdable": true, "physics": { - "mass": 100000, + // "gravity": [0,0,0], + "mass": 100, "inertia": false, "type": "bounding box" // "type": "mesh" - // "type": "mesh" + // "type": "hull" } } } \ No newline at end of file diff --git a/bin/data/entities/scripts/player.lua b/bin/data/entities/scripts/player.lua index 51cda1f5..69d17c6c 100644 --- a/bin/data/entities/scripts/player.lua +++ b/bin/data/entities/scripts/player.lua @@ -198,7 +198,7 @@ ent:bind( "tick", function(self) heldObject.uid = 0 heldObjectPhysicsState:enableGravity(true) - heldObjectPhysicsState:applyImpulse( flattenedTransform.forward * heldObjectPhysicsState:getMass() * 1000 ) + heldObjectPhysicsState:applyImpulse( flattenedTransform.forward * heldObjectPhysicsState:getMass() * 50 ) playSound("phys_launch"..math.random(1,4)) else diff --git a/bin/data/scenes/sourceengine/mds_mcdonalds.json b/bin/data/scenes/sourceengine/mds_mcdonalds.json index 35a1d902..faef9f93 100644 --- a/bin/data/scenes/sourceengine/mds_mcdonalds.json +++ b/bin/data/scenes/sourceengine/mds_mcdonalds.json @@ -15,7 +15,7 @@ "func_door_rotating_5568": { "action": "load", "payload": { "import": "/door.json", "metadata": { "angle":-1.570795, "normal": [1,0,0] } } }, "func_door_rotating_5584": { "action": "load", "payload": { "import": "/door.json", "metadata": { "angle":-1.570795, "normal": [1,0,0] } } }, - // "prop_physics_override_5813": { "action": "load", "payload": { "import": "/physics_prop.json" } }, + "prop_physics_override_5813": { "action": "load", "payload": { "import": "/physics_prop.json" } }, // regex matches "/^prop_physics_[^o]/": { "action": "load", "payload": { "import": "/prop.json" } }, diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index 4aaf9010..842273d1 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -170,8 +170,8 @@ namespace pod { float sleepTimer = 0.0f; int32_t islandID = -1; static constexpr float sleepThreshold = 0.5f; // seconds - static constexpr float linearSleepEpsilon = 0.1f; // m/s - static constexpr float angularSleepEpsilon = 0.1f; // rad/s + static constexpr float linearSleepEpsilon = 0.2f; // m/s + static constexpr float angularSleepEpsilon = 0.2f; // rad/s }; struct World; // forward declare @@ -211,7 +211,10 @@ namespace pod { pod::Vector3f normal; float penetration; - // Warm-start cached values + pod::Vector3f localA; + pod::Vector3f localB; + + // warm-start cached values int lifetime = 0; pod::Vector3f tangent = {}; float accumulatedNormalImpulse = 0.0f; @@ -241,7 +244,7 @@ namespace pod { bool warmupSolver = true; // cache manifold data to warm up the solver bool blockContactSolver = true; // use BlockNxN solvers (where N = number of contacts for a manifold) bool psgContactSolver = true; // use PSG contact solver - bool useGjk = true; // currently don't have a way to broadphase mesh => narrowphase tri via GJK + 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 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 @@ -262,7 +265,7 @@ namespace pod { // to-do: find possibly better values for this uint32_t solverIterations = 10; - float baumgarteCorrectionPercent = 0.4f; + float baumgarteCorrectionPercent = 0.01f; float baumgarteCorrectionSlop = 0.01f; uf::stl::unordered_map manifoldsCache; @@ -272,10 +275,12 @@ namespace pod { // to-do: tweak this to not be annoying pod::BVH::UpdatePolicy bvhUpdatePolicy = { + // triggers a refit .displacementThreshold = 0.25f, + // triggers a rebuild .overlapThreshold = 2.0f, - .dirtyRatioThreshold = 0.3f, - .maxFramesBeforeRebuild = 60, // * 10, // 10 seconds + .dirtyRatioThreshold = 0.3, + .maxFramesBeforeRebuild = 60, }; float groundedThreshold = 0.7f; // threshold before marking a body as grounded diff --git a/engine/inc/uf/utils/mesh/mesh.h b/engine/inc/uf/utils/mesh/mesh.h index f85887ac..203c3239 100644 --- a/engine/inc/uf/utils/mesh/mesh.h +++ b/engine/inc/uf/utils/mesh/mesh.h @@ -195,9 +195,10 @@ namespace uf { uf::stl::unordered_map attributes; const AttributeView& operator[](const uf::stl::string& name) const { - static AttributeView null{}; + //static AttributeView null{}; if ( auto it = attributes.find(name); it != attributes.end() ) return it->second; - return null; + UF_EXCEPTION("invalid view: {}", name); + //return null; } }; typedef uf::stl::vector views_t; diff --git a/engine/src/engine/ext/player/behavior.cpp b/engine/src/engine/ext/player/behavior.cpp index aa814d42..095a3de3 100644 --- a/engine/src/engine/ext/player/behavior.cpp +++ b/engine/src/engine/ext/player/behavior.cpp @@ -349,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 * ONE_OVER_SIXTY /*uf::physics::time::delta*/; + speed.rotate = metadata.movement.rotate * uf::physics::time::delta; speed.move = metadata.movement.move; speed.run = metadata.movement.run; speed.walk = metadata.movement.walk; @@ -466,7 +466,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( stats.walking ) { float factor = stats.floored ? 1.0f : speed.air; if ( stats.noclipped ) { - physicsBody.velocity += target * speed.move; + physicsBody.velocity += target * speed.move * ONE_OVER_SIXTY; } else { 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*/ ); } @@ -487,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 * ONE_OVER_SIXTY /*uf::physics::time::delta*/ * 4.0f; + if ( stats.floored && keys.jump && stats.noclipped ) transform.position += translator.up * metadata.movement.jump * uf::physics::time::delta * 4.0f; if ( keys.crouch ) { - if ( stats.noclipped ) transform.position -= translator.up * metadata.movement.jump * ONE_OVER_SIXTY /*uf::physics::time::delta*/ * 4.0f; + if ( stats.noclipped ) transform.position -= translator.up * metadata.movement.jump * uf::physics::time::delta * 4.0f; else { if ( !metadata.system.crouching ) stats.deltaCrouch = true; metadata.system.crouching = true; diff --git a/engine/src/utils/math/physics/aabb.inl b/engine/src/utils/math/physics/aabb.inl index 4141f0f2..7794c454 100644 --- a/engine/src/utils/math/physics/aabb.inl +++ b/engine/src/utils/math/physics/aabb.inl @@ -39,6 +39,9 @@ namespace { return bounds; } + FORCE_INLINE pod::AABB computeConvexHullAABB( const uf::Mesh::View& view, pod::AABB bounds = { { FLT_MAX, FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX, -FLT_MAX } } ) { + return ::computeConvexHullAABB( view, view["position"], bounds ); + } FORCE_INLINE pod::AABB mergeAabb( const pod::AABB& a, const pod::AABB& b ) { return { @@ -110,7 +113,8 @@ namespace { auto [ p1, p2 ] = ::getCapsuleSegment( body ); return ::computeSegmentAABB( p1, p2, body.collider.capsule.radius ); } break; - case pod::ShapeType::MESH: { + case pod::ShapeType::MESH: + case pod::ShapeType::CONVEX_HULL: { if ( body.collider.mesh.bvh && !body.collider.mesh.bvh->bounds.empty() ) return { transform.position + body.collider.mesh.bvh->bounds[0].min, @@ -120,18 +124,7 @@ namespace { pod::AABB bounds = { { FLT_MAX, FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX, -FLT_MAX } }; for ( const auto& view : meshData.buffer_views ) ::computeConvexHullAABB( view, view["position"], bounds ); return ::transformAabbToWorld( bounds, transform ); - } break; - case pod::ShapeType::CONVEX_HULL: { - if ( body.collider.convexHull.bvh && !body.collider.convexHull.bvh->bounds.empty() ) - return { - transform.position + body.collider.convexHull.bvh->bounds[0].min, - transform.position + body.collider.convexHull.bvh->bounds[0].max, - }; - const auto& meshData = *body.collider.convexHull.mesh; - pod::AABB bounds = { { FLT_MAX, FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX, -FLT_MAX } }; - for ( const auto& view : meshData.buffer_views ) ::computeConvexHullAABB( view, view["position"], bounds ); - return ::transformAabbToWorld( bounds, transform ); - } break; + } default: { } break; } diff --git a/engine/src/utils/math/physics/bvh.inl b/engine/src/utils/math/physics/bvh.inl index 6fb2d6a5..fa6115ef 100644 --- a/engine/src/utils/math/physics/bvh.inl +++ b/engine/src/utils/math/physics/bvh.inl @@ -282,7 +282,7 @@ namespace { // populate initial indices and bounds for ( size_t viewID = 0; viewID < hullCount; ++viewID ) { const auto& view = views[viewID]; - auto aabb = ::computeConvexHullAABB( view, view["position"] ); + auto aabb = ::computeConvexHullAABB( view ); bounds.emplace_back( aabb ); bvh.indices.emplace_back( viewID ); @@ -314,9 +314,29 @@ namespace { float oldRootArea = ::aabbSurfaceArea( bvh.bounds[0] ); // update/check each body + for ( auto i = 0; i < bvh.nodes.size(); ++i ) { + auto& node = bvh.nodes[i]; + if ( /*node.count*/ node.getCount() == 0 ) continue; + auto& body = *bodies[bvh.indices[node.start]]; + + auto& oldBounds = bvh.bounds[i]; + auto& newBounds = body.bounds; + + // compute displacement relative to size + pod::Vector3f oldCenter = ( oldBounds.min + oldBounds.max ) * 0.5f; + pod::Vector3f newCenter = ( newBounds.min + newBounds.max ) * 0.5f; + float displacement = uf::vector::distance( newCenter, oldCenter ); + + pod::Vector3f extent = oldBounds.max - oldBounds.min; + float size = std::max({extent.x, extent.y, extent.z, 1e-6f}); + + if ( displacement > policy.displacementThreshold * size ) ++dirtyCount; + } + /* for ( auto idx : bvh.indices ) { auto& body = *bodies[idx]; + // to-do: instead check against bounds in BVH pod::AABB oldBounds = body.bounds; body.bounds = ::computeAABB( body ); pod::AABB newBounds = body.bounds; @@ -334,11 +354,12 @@ namespace { // update nodes for ( auto i = 0; i < bvh.nodes.size(); ++i ) { auto& node = bvh.nodes[i]; - if ( /*node.count*/ node.getCount() == 0 ) continue; + if ( node.getCount() == 0 ) continue; auto& bound = bvh.bounds[i]; bound = bodies[bvh.indices[node.start]]->bounds; - for ( auto i = 1; i < node.getCount() /*node.count*/; ++i ) bound = ::mergeAabb( bound, bodies[bvh.indices[node.start + i]]->bounds ); + for ( auto i = 1; i < node.getCount(); ++i ) bound = ::mergeAabb( bound, bodies[bvh.indices[node.start + i]]->bounds ); } + */ float dirtyRatio = (float) dirtyCount / (float) bodies.size(); @@ -388,6 +409,8 @@ namespace { bound = ::mergeAabb(bvh.bounds[node.left], bvh.bounds[node.right]); } } + + if ( !bvh.flattened.empty() ) ::flattenBVH( bvh, 0 ); } // avoids creating a vector for bounds @@ -421,6 +444,8 @@ namespace { bvh.bounds[i] = ::mergeAabb( bvh.bounds[node.left], bvh.bounds[node.right] ); node.setAsleep( bvh.nodes[node.left].isAsleep() && bvh.nodes[node.right].isAsleep()); } + + if ( !bvh.flattened.empty() ) ::flattenBVH( bvh, 0 ); } void refitBVH( pod::BVH& bvh, const uf::Mesh& mesh ) { @@ -500,7 +525,8 @@ namespace { const auto& nodeA = bvh.nodes[nodeAID]; const auto& nodeB = bvh.nodes[nodeBID]; - if ( (nodeA.isAsleep() && nodeB.isAsleep()) || !::aabbOverlap( bvh.bounds[nodeAID], bvh.bounds[nodeBID] ) ) return; + if ( nodeA.isAsleep() && nodeB.isAsleep() ) return; + if ( !::aabbOverlap( bvh.bounds[nodeAID], bvh.bounds[nodeBID] ) ) return; if ( nodeA.getCount() > 0 && nodeB.getCount() > 0 ) { for ( auto i = 0; i < nodeA.getCount(); ++i ) { @@ -519,8 +545,7 @@ namespace { if ( nodeA.getCount() == 0 ) { ::traverseNodePair( bvh, nodeA.left, nodeBID, pairs ); ::traverseNodePair( bvh, nodeA.right, nodeBID, pairs ); - } - if ( nodeB.getCount() == 0 ) { + } else if ( nodeB.getCount() == 0 ) { ::traverseNodePair( bvh, nodeAID, nodeB.left, pairs ); ::traverseNodePair( bvh, nodeAID, nodeB.right, pairs ); } @@ -530,7 +555,8 @@ namespace { const auto& nodeA = bvhA.nodes[nodeAID]; const auto& nodeB = bvhB.nodes[nodeBID]; - if ( (nodeA.isAsleep() && nodeB.isAsleep()) || !::aabbOverlap( bvhA.bounds[nodeAID], bvhB.bounds[nodeBID] ) ) return; + if ( nodeA.isAsleep() && nodeB.isAsleep() ) return; + if ( !::aabbOverlap( bvhA.bounds[nodeAID], bvhB.bounds[nodeBID] ) ) return; if ( nodeA.getCount() > 0 && nodeB.getCount() > 0 ) { for ( auto i = 0; i < nodeA.getCount(); ++i ) { @@ -538,7 +564,7 @@ namespace { pod::BVH::index_t bodyA = bvhA.indices[nodeA.start + i]; pod::BVH::index_t bodyB = bvhB.indices[nodeB.start + j]; if ( bodyA == bodyB ) continue; - if ( bodyA > bodyB ) std::swap( bodyA, bodyB ); + //if ( bodyA > bodyB ) std::swap( bodyA, bodyB ); pairs.emplace_back(bodyA, bodyB); } @@ -621,6 +647,9 @@ namespace { } ::traverseNodePair( bvh, node.left, node.right, pairs ); + + ::traverseBVH( bvh, node.left, pairs ); + ::traverseBVH( bvh, node.right, pairs ); } void queryOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ) { @@ -649,7 +678,6 @@ namespace { if ( bvhA.nodes.empty() || bvhB.nodes.empty() ) return; outPairs.reserve(uf::physics::impl::settings.reserveCount); ::traverseNodePair(bvhA, 0, bvhB, 0, relTransform, outPairs); - UF_EXCEPTION("unimplemented"); ::postprocessPairs( outPairs ); } @@ -671,7 +699,8 @@ namespace { while ( !stack.empty() ) { pod::BVH::index_t idx = stack.top(); stack.pop(); auto& node = bvh.nodes[idx]; - if ( node.isAsleep() || !::aabbOverlap( bounds, bvh.bounds[idx] ) ) continue; + if ( node.isAsleep() ) continue; + if ( !::aabbOverlap( bounds, bvh.bounds[idx] ) ) continue; if ( node.getCount() > 0 ) { for ( auto i = 0; i < node.getCount(); ++i) outIndices.emplace_back(bvh.indices[node.start + i]); @@ -692,7 +721,8 @@ namespace { if ( nodeID == 0 ) outIndices.reserve(uf::physics::impl::settings.reserveCount); const auto& node = bvh.nodes[nodeID]; - if ( node.isAsleep() || !::aabbOverlap( bounds, bvh.bounds[nodeID] ) ) return; + if ( node.isAsleep() ) return; + if ( !::aabbOverlap( bounds, bvh.bounds[nodeID] ) ) return; if ( node.getCount() > 0 ) { for ( auto i = 0; i < node.getCount(); ++i ) outIndices.emplace_back(bvh.indices[node.start + i]); @@ -720,7 +750,8 @@ namespace { const auto& node = bvh.nodes[idx]; float tMin, tMax; - if ( node.isAsleep() || !::rayAabbIntersect( ray, bvh.bounds[idx], tMin, tMax ) ) continue; + //if ( node.isAsleep() ) continue; + if ( !::rayAabbIntersect( ray, bvh.bounds[idx], tMin, tMax ) ) continue; if ( tMin > maxDist ) continue; if ( node.getCount() > 0 ) { @@ -739,7 +770,8 @@ namespace { const auto& node = bvh.nodes[nodeID]; float tMin, tMax; - if ( node.isAsleep() || !::rayAabbIntersect( ray, bvh.bounds[nodeID], tMin, tMax ) ) return; + //if ( node.isAsleep() ) return; + if ( !::rayAabbIntersect( ray, bvh.bounds[nodeID], tMin, tMax ) ) return; if ( tMin > maxDist ) return; if ( node.getCount() > 0 ) { @@ -765,14 +797,14 @@ namespace { for ( pod::BVH::index_t a = 0; a < nodes.size(); ++a ) { const auto& nodeA = nodes[a]; - if ( nodeA.getCount() <= 0 || nodeA.isAsleep() ) continue; + if ( nodeA.getCount() <= 0 ) continue; const auto& boundsA = bounds[a]; pod::BVH::index_t b = a + 1; while ( b < nodes.size() ) { const auto& nodeB = nodes[b]; - if ( nodeB.isAsleep() || !::aabbOverlap( boundsA, bounds[b] ) ) { + if ( (nodeA.isAsleep() && nodeB.isAsleep()) || !::aabbOverlap( boundsA, bounds[b] ) ) { b = nodeB.skipIndex; continue; } @@ -820,7 +852,9 @@ namespace { const auto& nodeB = bvhB.flattened[b]; if ( nodeA.isAsleep() && nodeB.isAsleep() ) continue; - if ( !::aabbOverlap( bvhA.flatBounds[a], bvhB.flatBounds[b] ) ) continue; + if ( !::aabbOverlap( bvhA.flatBounds[a], bvhB.flatBounds[b] ) ) { + continue; + } bool isLeafA = (nodeA.getCount() > 0); bool isLeafB = (nodeB.getCount() > 0); @@ -885,11 +919,6 @@ namespace { if ( nodeA.isAsleep() && nodeB.isAsleep() ) continue; - /* - pod::AABB worldBoundsA = ::transformAabbToWorld( boundsA[a], tA ); - pod::AABB worldBoundsB = ::transformAabbToWorld( boundsB[b], tB ); - if ( !::aabbOverlap( worldBoundsA, worldBoundsB ) ) continue; - */ pod::AABB boundsB_in_A = ::transformAabbToWorld(boundsB[b], relTransform); if ( !::aabbOverlap( boundsA[a], boundsB_in_A ) ) continue; @@ -965,7 +994,7 @@ namespace { while ( idx < nodes.size() ) { const auto& node = nodes[idx]; float tMin, tMax; - if ( !node.isAsleep() && ::rayAabbIntersect( ray, bvh.flatBounds[idx], tMin, tMax ) && tMin <= maxDist ) { + if ( /*!node.isAsleep() &&*/ ::rayAabbIntersect( ray, bvh.flatBounds[idx], tMin, tMax ) && tMin <= maxDist ) { // leaf if ( node.getCount() > 0 ) { for ( auto i = 0; i < node.getCount(); ++i ) { @@ -1014,6 +1043,8 @@ namespace { } } }; + + // to-do: rewrite this, I'm pretty sure it's faulty void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector& bodies, uf::stl::vector& islands ) { UnionFind unionizer(bodies.size()); @@ -1039,13 +1070,6 @@ namespace { auto [ it, inserted ] = rootToIsland.try_emplace( root, (pod::BVH::index_t) islands.size()); if ( inserted ) islands.emplace_back(); - /* - if ( rootToIsland.find(root) == rootToIsland.end() ) { - rootToIsland[root] = (pod::BVH::index_t) islands.size(); - islands.emplace_back(); - } - */ - pod::BVH::index_t islandID = rootToIsland[root]; islands[islandID].indices.emplace_back( i ); } @@ -1070,34 +1094,29 @@ namespace { } } } - } - bool updateIsland( pod::Island& island, uf::stl::vector& bodies, float dt ) { - island.awake = false; + // update islands + for ( auto it = islands.begin(); it != islands.end(); ) { + auto& island = *it; + island.awake = false; - for ( auto idx : island.indices ) { - auto& body = *bodies[idx]; - if ( !body.activity.awake ) continue; - - float linSpeedSq = uf::vector::magnitude( body.velocity ); - float angSpeedSq = uf::vector::magnitude( body.angularVelocity ); - - if ( linSpeedSq < pod::Activity::linearSleepEpsilon && angSpeedSq < pod::Activity::angularSleepEpsilon) { - body.activity.sleepTimer += dt; - } else { - body.activity.sleepTimer = 0.0f; + // wake island if something is awake in it + for ( auto idx : island.indices ) { + auto& body = *bodies[idx]; + if ( !body.activity.awake ) continue; island.awake = true; } + + // update bodies within island + for ( auto idx : island.indices ) + (island.awake ? ::wakeBody : ::sleepBody)( *bodies[idx] ); - if ( body.activity.sleepTimer < pod::Activity::sleepThreshold ) { - island.awake = true; + // erase sleeping island + if ( !island.awake ) { + it = islands.erase(it); + } else { + ++it; } } - - // update bodies within island - for ( auto idx : island.indices ) - (island.awake ? ::wakeBody : ::sleepBody)( *bodies[idx] ); - - return island.awake; } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/convexHull.inl b/engine/src/utils/math/physics/convexHull.inl index 35d062a4..3fb2c858 100644 --- a/engine/src/utils/math/physics/convexHull.inl +++ b/engine/src/utils/math/physics/convexHull.inl @@ -19,12 +19,8 @@ namespace { pod::Simplex simplex; if ( !::gjk( hullView, body, simplex ) ) continue; - auto result = ::epa( hullView, body, simplex ); - - if ( !uf::vector::isValid(result.point) ) continue; - - manifold.points.emplace_back(result); + if ( !::generateClippingManifold( hullView, body, result, manifold ) ) continue; hit = true; } return hit; @@ -75,10 +71,7 @@ namespace { if ( !::gjk( viewA, viewB, simplex ) ) continue; auto result = ::epa( viewA, viewB, simplex ); - - if ( !uf::vector::isValid(result.point) ) continue; - - manifold.points.emplace_back(result); + if ( !::generateClippingManifold( viewA, viewB, result, manifold ) ) continue; hit = true; } return hit; diff --git a/engine/src/utils/math/physics/epa.inl b/engine/src/utils/math/physics/epa.inl index 970fbd4d..efaabda9 100644 --- a/engine/src/utils/math/physics/epa.inl +++ b/engine/src/utils/math/physics/epa.inl @@ -34,6 +34,153 @@ namespace { return face; }; + void getSupportFace( const pod::PhysicsBody& body, const pod::Vector3f& dir, pod::Vector3f outPoly[3], int& outCount ) { + outCount = 0; + const auto transform = ::getTransform(body); + pod::Vector3f localDir = uf::quaternion::rotate(uf::quaternion::inverse(transform.orientation), dir); + + switch (body.collider.type) { + case pod::ShapeType::TRIANGLE: { + outCount = 3; + bool hasTransform = ( body.transform != nullptr ); + FOR_EACH(3, { + outPoly[i] = hasTransform ? uf::transform::apply( transform, body.collider.triangle.points[i] ) : body.collider.triangle.points[i]; + }); + } break; + case pod::ShapeType::AABB: { + outCount = 4; + pod::Vector3f n = localDir; + pod::Vector3f absN = { std::fabs(n.x), std::fabs(n.y), std::fabs(n.z) }; + pod::Vector3f min = body.collider.aabb.min; + pod::Vector3f max = body.collider.aabb.max; + + // dominant axis + if ( absN.x > absN.y && absN.x > absN.z ) { + float x = (n.x > 0) ? max.x : min.x; + outPoly[0] = {x, min.y, max.z}; + outPoly[1] = {x, min.y, min.z}; + outPoly[2] = {x, max.y, min.z}; + outPoly[3] = {x, max.y, max.z}; + if ( n.x < 0 ) std::swap(outPoly[1], outPoly[3]); + } else if ( absN.y > absN.z ) { + float y = (n.y > 0) ? max.y : min.y; + outPoly[0] = {max.x, y, max.z}; + outPoly[1] = {max.x, y, min.z}; + outPoly[2] = {min.x, y, min.z}; + outPoly[3] = {min.x, y, max.z}; + if ( n.y < 0 ) std::swap(outPoly[1], outPoly[3]); + } else { + float z = (n.z > 0) ? max.z : min.z; + outPoly[0] = {min.x, max.y, z}; + outPoly[1] = {min.x, min.y, z}; + outPoly[2] = {max.x, min.y, z}; + outPoly[3] = {max.x, max.y, z}; + if (n.z < 0) std::swap(outPoly[1], outPoly[3]); + } + FOR_EACH(4, { + outPoly[i] = uf::transform::apply(transform, outPoly[i]); + }); + } break; + case pod::ShapeType::MESH: + case pod::ShapeType::CONVEX_HULL: { + if ( !body.collider.convexHull.mesh ) return; + const auto& mesh = *body.collider.convexHull.mesh; + auto selectedViewIdx = body.viewIndex; + + float bestDot = -FLT_MAX; + pod::Triangle bestTri; + + for ( auto viewIdx = 0; viewIdx < mesh.buffer_views.size(); ++viewIdx ) { + if ( 0 <= selectedViewIdx && selectedViewIdx != viewIdx ) continue; + + const auto& view = mesh.buffer_views[viewIdx]; + auto& indices = view["index"]; + auto& positions = view["position"]; + for ( size_t i = 0; i < view.index.count / 3; ++i ) { + pod::Triangle tri = ::fetchTriangle( view, indices, positions, i ); + pod::Vector3f normal = ::triangleNormal( tri ); + float d = uf::vector::dot( normal, localDir ); + if ( d > bestDot ) { + bestDot = d; + bestTri = tri; + } + } + } + outCount = 3; + FOR_EACH(3, { + outPoly[i] = uf::transform::apply(transform, bestTri.points[i]); + }); + } break; + // unsupported, fallback to single contact point + default: { + outCount = 0; + } break; + } + } + + bool generateClippingManifold( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Contact& contact, pod::Manifold& manifold ) { + if ( !uf::vector::isValid(contact.point) ) return false; + + auto& normal = contact.normal; + + pod::Vector3f polyA[4]; + pod::Vector3f polyB[4]; + int countA = 0; + int countB = 0; + ::getSupportFace(a, normal, polyA, countA); + ::getSupportFace(b, -normal, polyB, countB); + + if ( countA < 3 || countB < 3 ) { + if ( manifold.points.empty() ) manifold.points.emplace_back(contact); + return true; + } + + // to-do: reference face is the most perpendicular face + pod::Vector3f* refPoly = polyA; + pod::Vector3f* incPoly = polyB; + int refCount = countA; + int incCount = countB; + + pod::Vector3f clipBuffer1[8]; + pod::Vector3f clipBuffer2[8]; + int clipCount = incCount; + for ( auto i = 0; i < incCount; ++i ) clipBuffer1[i] = incPoly[i]; + + pod::Vector3f refNormal = uf::vector::normalize(uf::vector::cross(refPoly[1] - refPoly[0], refPoly[2] - refPoly[0])); + for ( auto i = 0; i < refCount; ++i ) { + pod::Vector3f edgeStart = refPoly[i]; + pod::Vector3f edgeEnd = refPoly[(i + 1) % refCount]; + pod::Vector3f edgeVector = edgeEnd - edgeStart; + + pod::Vector3f edgePlaneNormal = uf::vector::normalize(uf::vector::cross(edgeVector, refNormal)); + float edgePlaneOffset = uf::vector::dot(edgePlaneNormal, edgeStart); + + if (i % 2 == 0) { + clipCount = ::clipPolygonAgainstPlane(clipBuffer1, clipCount, edgePlaneNormal, edgePlaneOffset, clipBuffer2); + } else { + clipCount = ::clipPolygonAgainstPlane(clipBuffer2, clipCount, edgePlaneNormal, edgePlaneOffset, clipBuffer1); + } + } + + pod::Vector3f* finalPoly = (refCount % 2 == 0) ? clipBuffer1 : clipBuffer2; + float refOffset = uf::vector::dot(refNormal, refPoly[0]); + + for (int i = 0; i < clipCount; ++i) { + float distance = uf::vector::dot(finalPoly[i], refNormal) - refOffset; + // point is penetrating or touching + if ( distance <= EPS ) { + pod::Contact c; + c.point = finalPoly[i] - refNormal * (distance * 0.5f); + c.normal = normal; + c.penetration = -distance; + manifold.points.emplace_back(c); + } + } + + if ( manifold.points.empty() ) manifold.points.emplace_back(contact); + return true; + } + uf::stl::vector initialPolytope( const pod::Simplex& s ) { UF_ASSERT( s.pts.size() == 4 ); @@ -68,7 +215,7 @@ namespace { } // remove visible faces - for ( auto i = (int32_t) remove.size() - 1; i >= 0; --i ) { + for ( auto i = (int32_t) remove.size() - 1; i >= 0; --i ) { auto idx = remove[i]; faces[idx] = faces.back(); faces.pop_back(); @@ -116,7 +263,7 @@ namespace { auto contact = ( pA + pB ) * 0.5f; auto normal = f.normal; float penetration = uf::vector::dot( (pB - pA), normal ); - + // flip normal if ( penetration < 0.0f ) { f.normal = -f.normal; diff --git a/engine/src/utils/math/physics/gjk.inl b/engine/src/utils/math/physics/gjk.inl index abd4dc62..06bdf489 100644 --- a/engine/src/utils/math/physics/gjk.inl +++ b/engine/src/utils/math/physics/gjk.inl @@ -41,6 +41,7 @@ namespace { if ( d1 > d2 ) return tri.points[1]; return tri.points[2]; } break; + case pod::ShapeType::MESH: case pod::ShapeType::CONVEX_HULL: { const auto transform = ::getTransform( body ); const auto& mesh = *body.collider.convexHull.mesh; @@ -203,16 +204,10 @@ namespace { if ( uf::vector::dot( newPt.p, dir ) < 0 ) return false; // didn't pass origin, no collision // would invalidate the simplex if ( ::isDegenerate( simplex, newPt ) ) { - #if 1 // nudge direction with a small orthogonal component if ( fabs(dir.x) < fabs(dir.y) && fabs(dir.x) < fabs(dir.z) ) dir = uf::vector::normalize( pod::Vector3f{1,0,0} + dir * 0.01f ); else if ( fabs(dir.y) < fabs(dir.z) ) dir = uf::vector::normalize( pod::Vector3f{0,1,0} + dir * 0.01f ); else dir = uf::vector::normalize( pod::Vector3f{0,0,1} + dir * 0.01f ); - #else - // choose an alternate probe - static pod::Vector3f fallbackDirs[3] = { {1,0,0}, {0,1,0}, {0,0,1} }; - dir = fallbackDirs[ (it + simplex.pts.size()) % 3 ]; - #endif continue; // try again } // add new point to simplex @@ -221,27 +216,7 @@ namespace { if ( ::updateSimplex(simplex, dir) ) return true; // simplex contains origin, finished } - #if 1 return false; - #else - // if overlap detected but simplex ended at triangle, fix it, as EPA requires a tetrahedron: - if ( simplex.pts.size() == 3 ) { - // points - auto& A0 = simplex.pts[0].p; - auto& B0 = simplex.pts[1].p; - auto& C0 = simplex.pts[2].p; - // triangle normal - auto normal = uf::vector::normalize( uf::vector::cross( B0 - A0, C0 - A0 ) ); - - // try support in +normal - auto extra = ::supportMinkowskiDetailed( a, b, normal ); - float vol = fabs( uf::vector::dot( extra.p - A0, uf::vector::cross( B0 - A0, C0 - A0 ) ) ); - if ( vol < eps ) extra = ::supportMinkowskiDetailed( a, b, -normal ); // if still coplanar, try -normal - simplex.pts.emplace_back(extra); // force tetrahedron - } - - return !simplex.pts.empty(); - #endif } } diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index e194bd88..44207955 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -55,15 +55,17 @@ namespace { bool hullHull( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS ); // ugh + pod::AABB computeAABB( const pod::PhysicsBody& body ); FORCE_INLINE bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS ); pod::Vector3f aabbCenter( const pod::AABB& aabb ); pod::Vector3f getVertex( const uf::Mesh::View& view, const uf::Mesh::AttributeView& positions, size_t index ); - pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body, bool fast = false ); + pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body ); // to-do: define maxIterations as a setting bool gjk( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Simplex& simplex, int maxIterations = 20, float eps = EPS ); - pod::Contact epa( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Simplex& simplex, uint32_t maxIterations = 64, float eps = EPS ); bool gjk( const pod::Ray& ray, const pod::PhysicsBody& body, float maxDist, float& outT, pod::Vector3f& outNormal, float eps = EPS ); + pod::Contact epa( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Simplex& simplex, uint32_t maxIterations = 64, float eps = EPS ); + bool generateClippingManifold( const pod::PhysicsBody& a, const pod::PhysicsBody& b, const pod::Contact& contact, pod::Manifold& manifold ); void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices ); void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices, pod::BVH::index_t nodeID ); @@ -89,13 +91,14 @@ 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()); + if ( !wasAwake ) { + body.activity.sleepTimer = 0.0f; + } 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()); + bool wasAsleep = !body.activity.awake; body.activity.awake = false; body.velocity = pod::Vector3f{}; @@ -106,6 +109,9 @@ namespace { bool wasGrounded = body.activity.grounded; body.activity.grounded = false; + // update bounds + body.bounds = ::computeAABB( body ); + // already asleep if ( !body.activity.awake ) return; @@ -437,4 +443,33 @@ namespace { return best; } -} \ No newline at end of file + + int clipPolygonAgainstPlane( const pod::Vector3f* inPoly, int inCount, const pod::Vector3f& planeNormal, float planeOffset, pod::Vector3f* outPoly ) { + if (inCount == 0) return 0; + + int outCount = 0; + pod::Vector3f prevPoint = inPoly[inCount - 1]; + float prevDistance = uf::vector::dot(prevPoint, planeNormal) - planeOffset; + + for (int i = 0; i < inCount; ++i) { + pod::Vector3f currPoint = inPoly[i]; + float currDistance = uf::vector::dot(currPoint, planeNormal) - planeOffset; + + // If they cross the plane, compute the intersection point + if ((prevDistance * currDistance) < 0.0f) { + float t = prevDistance / (prevDistance - currDistance); + outPoly[outCount++] = prevPoint + (currPoint - prevPoint) * t; + } + + // If the current point is 'inside' or on the plane (distance <= 0), keep it + if (currDistance <= 0.0f) { + outPoly[outCount++] = currPoint; + } + + prevPoint = currPoint; + prevDistance = currDistance; + } + + return outCount; + } +} diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index a1462cb0..24c6ff6b 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -134,8 +134,9 @@ void uf::physics::impl::step( pod::World& world, float dt ) { manifolds.clear(); manifolds.reserve(uf::physics::impl::settings.reserveCount); - // sleeping island, skip - if ( !::updateIsland( island, bodies, dt ) ) continue; + // sleeping island, skip (asleep islands shouldn't ever be in here) + if ( !island.awake ) continue; + // iterate overlap pairs for ( auto& [ ia, ib ] : island.pairs ) { auto& a = *bodies[ia]; @@ -145,6 +146,9 @@ void uf::physics::impl::step( pod::World& world, float dt ) { // did not collide if ( !::generateContacts( a, b, manifold, dt ) ) continue; + // compute local points (for reprojection) + ::computeLocalContacts( manifold ); + // bodies with meshes already reorient the normal to the triangle's center // do not do it for meshes because it'll reorient to the mesh's origin // do not do it for planes @@ -174,9 +178,9 @@ void uf::physics::impl::step( pod::World& world, float dt ) { for ( auto& c : manifold.points ) { if ( std::fabs(uf::vector::dot(c.normal, pod::Vector3f{0,1,0})) > uf::physics::impl::settings.groundedThreshold ) { // only mark if contact point is below body - if ( c.point.y < getPosition(a).y ) a.activity.grounded = true; - if ( c.point.y < getPosition(b).y ) b.activity.grounded = true; - } + if ( c.point.y < ::getPosition(a).y ) a.activity.grounded = true; + if ( c.point.y < ::getPosition(b).y ) b.activity.grounded = true; + } } // store manifold @@ -186,11 +190,11 @@ void uf::physics::impl::step( pod::World& world, float dt ) { // pass manifolds to solver ::solveContacts( manifolds, dt ); // do position correction - ::solvePositions( manifolds, dt ); + // ::solvePositions( manifolds, dt ); // cache manifold positions if ( uf::physics::impl::settings.warmupSolver ) { - ::updateManifoldCache( manifolds, uf::physics::impl::settings.manifoldsCache ); - } + ::updateManifoldCache( manifolds, uf::physics::impl::settings.manifoldsCache ); + } } if ( uf::physics::impl::settings.warmupSolver ) ::pruneManifoldCache( uf::physics::impl::settings.manifoldsCache ); @@ -253,14 +257,6 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) { switch ( body.collider.type ) { case pod::ShapeType::AABB: { - /* - // old - pod::Vector3f extents = (body.collider.aabb.max - body.collider.aabb.min); - extents *= extents; // square it; - - body.inertiaTensor = extents * (body.mass / 12.0f); - body.inverseInertiaTensor = 1.0f / body.inertiaTensor; - */ 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); @@ -285,7 +281,8 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) { body.inertiaTensor = { Ixx, Iyy, Izz }; body.inverseInertiaTensor = { 1.0f/Ixx, 1.0f/Iyy, 1.0f/Izz }; } break; - case pod::ShapeType::MESH: { + case pod::ShapeType::MESH: + case pod::ShapeType::CONVEX_HULL: { const auto& bvh = *body.collider.mesh.bvh; pod::Matrix3f inertia = {}; diff --git a/engine/src/utils/math/physics/integration.inl b/engine/src/utils/math/physics/integration.inl index 2b6e233d..23232ca0 100644 --- a/engine/src/utils/math/physics/integration.inl +++ b/engine/src/utils/math/physics/integration.inl @@ -8,12 +8,16 @@ namespace { float angularTermB = 0.0f; if ( !a.isStatic ) { - pod::Vector3f crossA = uf::vector::cross(rA, n); - angularTermA = uf::vector::dot(uf::vector::cross(crossA * a.inverseInertiaTensor, rA), n); + auto invIa = ::computeWorldInverseInertia(a); + auto crossA = uf::vector::cross(rA, n); + auto I_crossA = uf::matrix::multiply(invIa, crossA); + angularTermA = uf::vector::dot(uf::vector::cross(I_crossA, rA), n); } if ( !b.isStatic ) { - pod::Vector3f crossB = uf::vector::cross(rB, n); - angularTermB = uf::vector::dot(uf::vector::cross(crossB * b.inverseInertiaTensor, rB), n); + auto invIb = ::computeWorldInverseInertia(b); + auto crossB = uf::vector::cross(rB, n); + auto I_crossB = uf::matrix::multiply(invIb, crossB); + angularTermB = uf::vector::dot(uf::vector::cross(I_crossB, rB), n); } float result = inverseMass + angularTermA + angularTermB; @@ -25,13 +29,13 @@ namespace { if ( !a.isStatic ) { a.velocity -= impulse * a.inverseMass; //a.angularVelocity -= (uf::vector::cross(rA, impulse)) * a.inverseInertiaTensor; - pod::Matrix3f invIa = computeWorldInverseInertia( a ); + pod::Matrix3f invIa = ::computeWorldInverseInertia( a ); a.angularVelocity -= uf::matrix::multiply( invIa, uf::vector::cross(rA, impulse) ); } if ( !b.isStatic ) { b.velocity += impulse * b.inverseMass; //b.angularVelocity += (uf::vector::cross(rB, impulse)) * b.inverseInertiaTensor; - pod::Matrix3f invIb = computeWorldInverseInertia( b ); + pod::Matrix3f invIb = ::computeWorldInverseInertia( b ); b.angularVelocity += uf::matrix::multiply( invIb, uf::vector::cross(rB, impulse) ); } } @@ -43,8 +47,8 @@ namespace { float angularSpeed2 = uf::vector::magnitude( body.angularVelocity ); if ( angularSpeed2 < EPS2 ) return; - body.angularVelocity += body.angularVelocity * body.mass * -rollingFriction * dt; - // body.angularVelocity *= -rollingFriction * dt; + //body.angularVelocity += body.angularVelocity * body.mass * -rollingFriction * dt; + body.angularVelocity *= std::max(0.0f, 1.0f - rollingFriction * dt); } void bindManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt = 0 ) { @@ -60,11 +64,9 @@ namespace { pod::Simplex simplex; if ( !::gjk(a,b,simplex) ) return false; - auto result = ::epa( a, b, simplex ); - - manifold.points.clear(); - manifold.points.emplace_back(result); + if ( !::generateClippingManifold( a, b, result, manifold ) ) return false; + return true; } @@ -125,6 +127,21 @@ namespace { return false; } + void computeLocalContacts( pod::Manifold& manifold ) { + if ( manifold.points.empty() ) return; + + auto& a = *manifold.a; + auto& b = *manifold.b; + + auto tA = ::getTransform( a ); + auto tB = ::getTransform( b ); + + for ( auto& c : manifold.points ) { + c.localA = uf::transform::applyInverse( tA, c.point - c.normal * (c.penetration * 0.5f) ); + c.localB = uf::transform::applyInverse( tB, c.point + c.normal * (c.penetration * 0.5f) ); + } + } + bool similarContact( const pod::Contact& a, const pod::Contact& b, float distSq = 1.0e-2f, float norm = 0.9f ) { return uf::vector::distanceSquared(a.point, b.point) < distSq && uf::vector::dot(a.normal, b.normal) > norm; } @@ -185,16 +202,54 @@ namespace { manifold.points = result; } - void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float decay = 0.35f ) { - for ( auto& c : current.points ) { - for ( auto& p : previous.points ) { - if ( !::similarContact( c, p ) ) continue; - c.accumulatedNormalImpulse = p.accumulatedNormalImpulse * decay; - c.accumulatedTangentImpulse = p.accumulatedTangentImpulse * decay; - c.lifetime = p.lifetime + 1; - break; + void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold = 0.1f, float separationThreshold = 0.1f, float decay = 0.85f ) { + auto& a = *current.a; + auto& b = *current.b; + + auto tA = ::getTransform( a ); + auto tB = ::getTransform( b ); + + uf::stl::vector merged = current.points; + + float distSqThresh = distanceThreshold * distanceThreshold; + for ( const auto& oldContact : previous.points ) { + // reproject point according to current transform + auto worldA = uf::transform::apply( tA, oldContact.localA ); + auto worldB = uf::transform::apply( tB, oldContact.localB ); + + auto delta = worldB - worldA; + auto normal = current.points.empty() ? oldContact.normal : current.points[0].normal; + float penetration = -uf::vector::dot( delta, normal ); + if ( penetration < -separationThreshold ) continue; + + pod::Vector3f projectedDelta = delta + normal * penetration; + float tangentialDriftSq = uf::vector::dot( projectedDelta, projectedDelta ); + if ( tangentialDriftSq > distSqThresh ) continue; + + pod::Contact validContact = oldContact; + validContact.point = (worldA + worldB) * 0.5f; + validContact.normal = normal; + validContact.penetration = penetration; + ++validContact.lifetime; + + validContact.accumulatedNormalImpulse *= decay; + validContact.accumulatedTangentImpulse *= decay; + + bool isDuplicate = false; + for ( auto& c : current.points ) { + if ( ::similarContact( validContact, c ) ) { + c.accumulatedNormalImpulse = validContact.accumulatedNormalImpulse; + c.accumulatedTangentImpulse = validContact.accumulatedTangentImpulse; + c.lifetime = validContact.lifetime; + isDuplicate = true; + break; + } } + + if ( !isDuplicate ) merged.emplace_back( validContact ); } + + current.points = merged; } void prepareManifoldCache( uf::stl::unordered_map& cache, const uf::stl::vector& islands, const uf::stl::vector& bodies ) { diff --git a/engine/src/utils/math/physics/solvers.inl b/engine/src/utils/math/physics/solvers.inl index a2072736..b9ebe443 100644 --- a/engine/src/utils/math/physics/solvers.inl +++ b/engine/src/utils/math/physics/solvers.inl @@ -69,8 +69,6 @@ namespace { ::applyImpulseTo(a, b, rA, rB, tangent * jt); } - - // ::positionCorrection(a, b, contact); } template @@ -121,18 +119,6 @@ namespace { K(i,i) += 1e-3f; } - #if 0 - pod::Vector3f relVelLinear = b.velocity - a.velocity; - for ( auto i = 0; i < N; i++ ) { - float vRel = uf::vector::dot( relVelLinear, manifold.points[i].normal ); - - float penetrationBias = std::max( manifold.points[i].penetration - uf::physics::impl::settings.baumgarteCorrectionSlop, 0.0f ) * ( uf::physics::impl::settings.baumgarteCorrectionPercent / dt ); - float cDot = vRel + penetrationBias; - - rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f; - lambda[i] = manifold.points[i].accumulatedNormalImpulse; // warmup - } - #endif for ( auto i = 0; i < N; i++ ) { auto& contact = manifold.points[i]; // full relative velocity, linear + angular @@ -140,7 +126,6 @@ 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,11 +133,7 @@ namespace { float maxPenetrationRecovery = 2.0f; // limit to 2 units per second 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; + rhs[i] = -vRel + penetrationBias; // RHS is magnitude of correction needed lambda[i] = contact.accumulatedNormalImpulse; } @@ -172,7 +153,6 @@ namespace { pod::Vector3f rB = manifold.points[i].point - pB; ::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] ); - // ::positionCorrection( a, b, manifold.points[i] ); } } @@ -218,7 +198,8 @@ 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 = 0; // (vn < -1.0f ? -e * vn : 0.0f) + penetrationBias; + float restitutionBias = (vn < -1.0f) ? -e * vn : 0.0f; + cc.bias = restitutionBias + penetrationBias; // effective mass (normal) pod::Vector3f rnA = uf::vector::cross( cc.rA, cc.normal ); @@ -256,7 +237,7 @@ namespace { // normal constraint float vn = uf::vector::dot( dv, cc.normal ); - float lambdaN = cc.effectiveMassN * (-(vn + cc.bias)); + float lambdaN = cc.effectiveMassN * (-vn + cc.bias); float oldImpulseN = cc.accumulatedNormalImpulse; cc.accumulatedNormalImpulse = std::max( oldImpulseN + lambdaN, 0.0f ); float dPn = cc.accumulatedNormalImpulse - oldImpulseN; diff --git a/engine/src/utils/math/physics/triangle.inl b/engine/src/utils/math/physics/triangle.inl index 07537fcf..7c89b881 100644 --- a/engine/src/utils/math/physics/triangle.inl +++ b/engine/src/utils/math/physics/triangle.inl @@ -91,6 +91,12 @@ namespace { }); return tri; } + + // for clean code, this would be preferable + // but this incurs two lookups every triangle fetch, and I doubt the optimizer will optimize that away, so explicitly passing attribute views is preferable + FORCE_INLINE pod::Triangle fetchTriangle( const uf::Mesh::View& view, size_t triID ) { + return ::fetchTriangle( view, view["index"], view["position"], triID ); + } pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ) { const auto& views = mesh.buffer_views; @@ -109,33 +115,24 @@ namespace { triBase += trisInView; } UF_ASSERT( view ); - - auto& positions = (*view)["position"]; - auto& indices = (*view)["index"]; - pod::TriangleWithNormal tri = { ::fetchTriangle( *view, indices, positions, triID ) }; + pod::TriangleWithNormal tri = { ::fetchTriangle( *view, triID ) }; tri.normal = uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0])); return tri; } // if body is a mesh, apply its transform to the triangles, else reorient the normal with respect to the body - pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body, bool fast ) { + pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body ) { auto tri = ::fetchTriangle( mesh, triID ); auto transform = ::getTransform( body ); if ( body.collider.type == pod::ShapeType::MESH ) { - if ( fast ) { - FOR_EACH(3, { - tri.points[i] += transform.position; - }); - } else { - FOR_EACH(3, { - tri.points[i] = uf::transform::apply( transform, tri.points[i] ); - }); - tri.normal = uf::quaternion::rotate( transform.orientation, tri.normal ); - } + FOR_EACH(3, { + tri.points[i] = uf::transform::apply( transform, tri.points[i] ); + }); + tri.normal = uf::quaternion::rotate( transform.orientation, tri.normal ); } else { #if REORIENT_NORMALS_ON_FETCH @@ -237,8 +234,6 @@ namespace { // triangle colliders namespace { - // i feel like it'd just be better to treat an AABB as a 12-triangle mesh and do a triangleTriangle collision instead of 3 + 1 + 3 * 3 axis tests - // but what do i know bool triangleAabbSAT( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = 1e-6f ) { const float eps2 = eps * eps; @@ -303,16 +298,6 @@ namespace { float planeDist = uf::vector::dot(triNormal, v0); if ( uf::vector::dot(bestAxis, triNormal) < 0.0f ) bestAxis = -bestAxis; pod::Vector3f contact = boxCenter - bestAxis * (boxHalf.x * fabs(bestAxis.x) + boxHalf.y * fabs(bestAxis.y) + boxHalf.z * fabs(bestAxis.z)); - - //pod::Vector3f contact = boxCenter - triNormal * planeDist; - - /* - float d = uf::vector::dot(triNormal, v0); - float dist = uf::vector::dot(triNormal, -boxCenter) - d; - pod::Vector3f contact = boxCenter - triNormal * dist; - - if ( uf::vector::dot(bestAxis, triNormal) < 0.0f ) bestAxis = -bestAxis; - */ manifold.points.emplace_back( pod::Contact{ contact, bestAxis, minOverlap } ); return true; @@ -387,12 +372,6 @@ namespace { }; if ( uf::vector::dot(bestAxis, ::triangleCenter(b) - ::triangleCenter(a)) < 0.0f ) bestAxis = -bestAxis; - /* - pod::Vector3f centroid{0,0,0}; - for ( auto i = 0; i < polyCount; i++ ) centroid += poly[i]; - centroid /= (float) polyCount; - if ( uf::vector::dot(bestAxis, centroid - ::triangleCenter(a)) < 0.0f ) bestAxis = -bestAxis; - */ for ( auto i = 0; i < 3; i++ ) { auto p0 = a.points[i]; @@ -412,37 +391,8 @@ namespace { return ( polyCount > 0 ); } - bool triangleAabbTri( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) { - // 8 corners - pod::Vector3f v[8] = { - {body.bounds.min.x, body.bounds.min.y, body.bounds.min.z}, - {body.bounds.max.x, body.bounds.min.y, body.bounds.min.z}, - {body.bounds.max.x, body.bounds.max.y, body.bounds.min.z}, - {body.bounds.min.x, body.bounds.max.y, body.bounds.min.z}, - {body.bounds.min.x, body.bounds.min.y, body.bounds.max.z}, - {body.bounds.max.x, body.bounds.min.y, body.bounds.max.z}, - {body.bounds.max.x, body.bounds.max.y, body.bounds.max.z}, - {body.bounds.min.x, body.bounds.max.y, body.bounds.max.z} - }; - - pod::TriangleWithNormal tris[12] = { - { {v[0], v[4], v[7]}, {-1,0,0} }, { {v[0], v[7], v[3]}, {-1,0,0} }, // left (x-) - { {v[1], v[5], v[6]}, {1,0,0} }, { {v[1], v[6], v[2]}, {1,0,0} }, // right (x+) - { {v[0], v[1], v[5]}, {0,-1,0} }, { {v[0], v[5], v[4]}, {0,-1,0} }, // bottom (y-) - { {v[3], v[2], v[6]}, {0,1,0} }, { {v[3], v[6], v[7]}, {0,1,0} }, // top (y+) - { {v[0], v[1], v[2]}, {0,0,-1} }, { {v[0], v[2], v[3]}, {0,0,-1} }, // back (z-) - { {v[4], v[5], v[6]}, {0,0,1} }, { {v[4], v[6], v[7]}, {0,0,1} }, // front (z+) - }; - - bool hit = false; - for ( auto& t : tris ) { - if ( ::triangleTriangle( tri, t, manifold, eps ) ) hit = true; - } - return hit; - } bool triangleAabb( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) { return ::triangleAabbSAT( tri, body, manifold, eps ); - //return ::triangleAabbTri( tri, body, manifold, eps ); } bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps ) { const auto& sphere = body; @@ -459,9 +409,11 @@ namespace { if ( dist2 > r * r ) return false; float dist = std::sqrt(dist2); + auto triNormal = ::triangleNormal( tri ); auto contact = ( center + closest ) * 0.5f; - auto normal = ( dist > eps ) ? ( delta / dist ) : ::triangleNormal( tri ); + auto normal = ( dist > eps ) ? ( delta / dist ) : triNormal; float penetration = r - dist; + if ( uf::vector::dot( normal, triNormal ) > 0.707f ) normal = triNormal; #if REORIENT_NORMALS_ON_CONTACT if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal; @@ -534,9 +486,11 @@ namespace { auto delta = ( closestSeg - closest ); // to-do: properly derive the contact information + auto triNormal = ::triangleNormal( tri ); auto contact = closest; // ( closestSeg + closest ) * 0.5f; - auto normal = ( dist > eps ) ? ( delta / dist ) : ::triangleNormal( tri ); + auto normal = ( dist > eps ) ? ( delta / dist ) : triNormal; float penetration = r - dist; + if ( uf::vector::dot( normal, triNormal ) > 0.707f ) normal = triNormal; #if REORIENT_NORMALS_ON_CONTACT if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal; @@ -573,12 +527,8 @@ namespace { pod::Simplex simplex; if ( !::gjk( tri, hull, simplex ) ) return false; - auto result = ::epa( tri, hull, simplex ); - - if ( !uf::vector::isValid(result.point) ) return false; - - manifold.points.emplace_back( result ); + if ( !::generateClippingManifold( tri, hull, result, manifold ) ) return false; return true; }