From fe10395ce86a2b6e12dfde6671d08919dc67a05c Mon Sep 17 00:00:00 2001 From: ecker Date: Fri, 1 May 2026 11:51:24 -0500 Subject: [PATCH] organized physics system --- .../math/physics/{ => broadphase}/bvh.inl | 111 ------- .../utils/math/physics/broadphase/island.inl | 110 +++++++ engine/src/utils/math/physics/helpers.inl | 3 +- engine/src/utils/math/physics/impl.cpp | 28 +- .../math/physics/{ => narrowphase}/aabb.inl | 0 .../physics/{ => narrowphase}/capsule.inl | 0 .../physics/{ => narrowphase}/convexHull.inl | 0 .../math/physics/{ => narrowphase}/epa.inl | 0 .../math/physics/{ => narrowphase}/gjk.inl | 0 .../math/physics/{ => narrowphase}/mesh.inl | 0 .../math/physics/{ => narrowphase}/plane.inl | 0 .../math/physics/{ => narrowphase}/ray.inl | 0 .../math/physics/{ => narrowphase}/sphere.inl | 0 .../physics/{ => narrowphase}/triangle.inl | 0 engine/src/utils/math/physics/solvers.inl | 270 +----------------- .../src/utils/math/physics/solvers/block.inl | 96 +++++++ .../math/physics/solvers/iterativeImpulse.inl | 73 +++++ engine/src/utils/math/physics/solvers/psg.inl | 100 +++++++ 18 files changed, 402 insertions(+), 389 deletions(-) rename engine/src/utils/math/physics/{ => broadphase}/bvh.inl (89%) create mode 100644 engine/src/utils/math/physics/broadphase/island.inl rename engine/src/utils/math/physics/{ => narrowphase}/aabb.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/capsule.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/convexHull.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/epa.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/gjk.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/mesh.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/plane.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/ray.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/sphere.inl (100%) rename engine/src/utils/math/physics/{ => narrowphase}/triangle.inl (100%) create mode 100644 engine/src/utils/math/physics/solvers/block.inl create mode 100644 engine/src/utils/math/physics/solvers/iterativeImpulse.inl create mode 100644 engine/src/utils/math/physics/solvers/psg.inl diff --git a/engine/src/utils/math/physics/bvh.inl b/engine/src/utils/math/physics/broadphase/bvh.inl similarity index 89% rename from engine/src/utils/math/physics/bvh.inl rename to engine/src/utils/math/physics/broadphase/bvh.inl index fa6115ef..a6c3831a 100644 --- a/engine/src/utils/math/physics/bvh.inl +++ b/engine/src/utils/math/physics/broadphase/bvh.inl @@ -1008,115 +1008,4 @@ namespace { } } } -} - -namespace { - struct UnionFind { - uf::stl::vector parent; - uf::stl::vector rank; - - UnionFind( pod::BVH::index_t n ) { - parent.resize(n); - rank.resize(n, 0); - - for ( auto i = 0; i < n; i++ ) - parent[i] = i; - } - - pod::BVH::index_t find( pod::BVH::index_t x ) { - if ( parent[x] != x ) parent[x] = find(parent[x]); - return parent[x]; - } - - void unite( pod::BVH::index_t a, pod::BVH::index_t b ) { - pod::BVH::index_t rootA = find(a); - pod::BVH::index_t rootB = find(b); - - if ( rootA == rootB ) return; - - // union by rank - if ( rank[rootA] < rank[rootB] ) parent[rootA] = rootB; - else if ( rank[rootA] > rank[rootB] ) parent[rootB] = rootA; - else { - parent[rootB] = rootA; - rank[rootA]++; - } - } - }; - - // 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()); - - // union all pairs - for ( auto& [a, b] : pairs ) { - if ( !bodies[a]->isStatic && !bodies[b]->isStatic ) { - unionizer.unite(a, b); - } - } - - // map root to island index - static thread_local uf::stl::unordered_map rootToIsland; - rootToIsland.clear(); - - islands.clear(); - 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); - - auto [ it, inserted ] = rootToIsland.try_emplace( root, (pod::BVH::index_t) islands.size()); - if ( inserted ) islands.emplace_back(); - - pod::BVH::index_t islandID = rootToIsland[root]; - islands[islandID].indices.emplace_back( i ); - } - - // collect pairs per island - for ( auto& [a, b] : pairs ) { - // 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); - if ( rootToIsland.find(root) != rootToIsland.end() ) { - pod::BVH::index_t islandID = rootToIsland[root]; - - islands[islandID].pairs.emplace_back(a, b); - - if ( bodies[a]->activity.awake || bodies[b]->activity.awake ) { - ::wakeBody( *bodies[dynamicIndex] ); - } - } - } - - // update islands - for ( auto it = islands.begin(); it != islands.end(); ) { - auto& island = *it; - island.awake = false; - - // 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] ); - - // erase sleeping island - if ( !island.awake ) { - it = islands.erase(it); - } else { - ++it; - } - } - } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/broadphase/island.inl b/engine/src/utils/math/physics/broadphase/island.inl new file mode 100644 index 00000000..990dffd3 --- /dev/null +++ b/engine/src/utils/math/physics/broadphase/island.inl @@ -0,0 +1,110 @@ +namespace { + struct UnionFind { + uf::stl::vector parent; + uf::stl::vector rank; + + UnionFind( pod::BVH::index_t n ) { + parent.resize(n); + rank.resize(n, 0); + + for ( auto i = 0; i < n; i++ ) + parent[i] = i; + } + + pod::BVH::index_t find( pod::BVH::index_t x ) { + if ( parent[x] != x ) parent[x] = find(parent[x]); + return parent[x]; + } + + void unite( pod::BVH::index_t a, pod::BVH::index_t b ) { + pod::BVH::index_t rootA = find(a); + pod::BVH::index_t rootB = find(b); + + if ( rootA == rootB ) return; + + // union by rank + if ( rank[rootA] < rank[rootB] ) parent[rootA] = rootB; + else if ( rank[rootA] > rank[rootB] ) parent[rootB] = rootA; + else { + parent[rootB] = rootA; + rank[rootA]++; + } + } + }; + + // 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()); + + // union all pairs + for ( auto& [a, b] : pairs ) { + if ( !bodies[a]->isStatic && !bodies[b]->isStatic ) { + unionizer.unite(a, b); + } + } + + // map root to island index + static thread_local uf::stl::unordered_map rootToIsland; + rootToIsland.clear(); + + islands.clear(); + 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); + + auto [ it, inserted ] = rootToIsland.try_emplace( root, (pod::BVH::index_t) islands.size()); + if ( inserted ) islands.emplace_back(); + + pod::BVH::index_t islandID = rootToIsland[root]; + islands[islandID].indices.emplace_back( i ); + } + + // collect pairs per island + for ( auto& [a, b] : pairs ) { + // 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); + if ( rootToIsland.find(root) != rootToIsland.end() ) { + pod::BVH::index_t islandID = rootToIsland[root]; + + islands[islandID].pairs.emplace_back(a, b); + + if ( bodies[a]->activity.awake || bodies[b]->activity.awake ) { + ::wakeBody( *bodies[dynamicIndex] ); + } + } + } + + // update islands + for ( auto it = islands.begin(); it != islands.end(); ) { + auto& island = *it; + island.awake = false; + + // 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] ); + + // erase sleeping island + if ( !island.awake ) { + it = islands.erase(it); + } else { + ++it; + } + } + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index 44207955..1bfbd279 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -1,10 +1,11 @@ +// to-do: clean this mess #define REVERSE_COLLIDER( a, b, fun )\ auto start = manifold.points.size();\ if ( !::fun( b, a, manifold, eps ) ) return false;\ for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\ return true; -// forward declare +// forward declare (to-do: properly handle this) namespace { 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 ); diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index 24c6ff6b..2689da22 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -10,17 +10,23 @@ #define UF_PHYSICS_TEST 0 #include "helpers.inl" -#include "aabb.inl" -#include "sphere.inl" -#include "plane.inl" -#include "capsule.inl" -#include "triangle.inl" -#include "mesh.inl" -#include "convexHull.inl" -#include "ray.inl" -#include "bvh.inl" -#include "gjk.inl" -#include "epa.inl" + +#include "narrowphase/aabb.inl" +#include "narrowphase/sphere.inl" +#include "narrowphase/plane.inl" +#include "narrowphase/capsule.inl" +#include "narrowphase/triangle.inl" +#include "narrowphase/mesh.inl" +#include "narrowphase/convexHull.inl" +#include "narrowphase/ray.inl" + + +#include "narrowphase/gjk.inl" +#include "narrowphase/epa.inl" + +#include "broadphase/bvh.inl" +#include "broadphase/island.inl" + #include "integration.inl" #include "solvers.inl" diff --git a/engine/src/utils/math/physics/aabb.inl b/engine/src/utils/math/physics/narrowphase/aabb.inl similarity index 100% rename from engine/src/utils/math/physics/aabb.inl rename to engine/src/utils/math/physics/narrowphase/aabb.inl diff --git a/engine/src/utils/math/physics/capsule.inl b/engine/src/utils/math/physics/narrowphase/capsule.inl similarity index 100% rename from engine/src/utils/math/physics/capsule.inl rename to engine/src/utils/math/physics/narrowphase/capsule.inl diff --git a/engine/src/utils/math/physics/convexHull.inl b/engine/src/utils/math/physics/narrowphase/convexHull.inl similarity index 100% rename from engine/src/utils/math/physics/convexHull.inl rename to engine/src/utils/math/physics/narrowphase/convexHull.inl diff --git a/engine/src/utils/math/physics/epa.inl b/engine/src/utils/math/physics/narrowphase/epa.inl similarity index 100% rename from engine/src/utils/math/physics/epa.inl rename to engine/src/utils/math/physics/narrowphase/epa.inl diff --git a/engine/src/utils/math/physics/gjk.inl b/engine/src/utils/math/physics/narrowphase/gjk.inl similarity index 100% rename from engine/src/utils/math/physics/gjk.inl rename to engine/src/utils/math/physics/narrowphase/gjk.inl diff --git a/engine/src/utils/math/physics/mesh.inl b/engine/src/utils/math/physics/narrowphase/mesh.inl similarity index 100% rename from engine/src/utils/math/physics/mesh.inl rename to engine/src/utils/math/physics/narrowphase/mesh.inl diff --git a/engine/src/utils/math/physics/plane.inl b/engine/src/utils/math/physics/narrowphase/plane.inl similarity index 100% rename from engine/src/utils/math/physics/plane.inl rename to engine/src/utils/math/physics/narrowphase/plane.inl diff --git a/engine/src/utils/math/physics/ray.inl b/engine/src/utils/math/physics/narrowphase/ray.inl similarity index 100% rename from engine/src/utils/math/physics/ray.inl rename to engine/src/utils/math/physics/narrowphase/ray.inl diff --git a/engine/src/utils/math/physics/sphere.inl b/engine/src/utils/math/physics/narrowphase/sphere.inl similarity index 100% rename from engine/src/utils/math/physics/sphere.inl rename to engine/src/utils/math/physics/narrowphase/sphere.inl diff --git a/engine/src/utils/math/physics/triangle.inl b/engine/src/utils/math/physics/narrowphase/triangle.inl similarity index 100% rename from engine/src/utils/math/physics/triangle.inl rename to engine/src/utils/math/physics/narrowphase/triangle.inl diff --git a/engine/src/utils/math/physics/solvers.inl b/engine/src/utils/math/physics/solvers.inl index b9ebe443..f1f16c25 100644 --- a/engine/src/utils/math/physics/solvers.inl +++ b/engine/src/utils/math/physics/solvers.inl @@ -1,270 +1,8 @@ +#include "./solvers/iterativeImpulse.inl" +#include "./solvers/block.inl" +#include "./solvers/psg.inl" + namespace { - void iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Contact& contact, float dt ) { - // relative positions from centers to contact point - pod::Vector3f rA = contact.point - ::getPosition( a, true ); - pod::Vector3f rB = contact.point - ::getPosition( b, true ); - - // relative velocity at contact - pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA); - pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB); - pod::Vector3f rv = vB - vA; - - // normal contact velocity - float velAlongNormal = uf::vector::dot(rv, contact.normal); - float velTolerance = 0; // -1.0e3f; - if ( velAlongNormal > velTolerance ) return; // if separating, no impulse - - // compute restitution (bounce) - float e = std::min(a.material.restitution, b.material.restitution); - - // nullify restitution if velocity is small enough - if ( fabs(velAlongNormal) < 1.0f) e = 0.0f; - - // effective inverse mass along normal - float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal); - - // normal impulse scalar - float jn = -(1.0f + e) * velAlongNormal; - jn /= invMassN; - if ( uf::physics::impl::settings.warmupSolver ) { - float jnOld = contact.accumulatedNormalImpulse; - float jnNew = std::max(0.0f, jnOld + jn); - float jnDelta = jnNew - jnOld; - contact.accumulatedNormalImpulse = jnNew; - jn = jnDelta; - } - - ::applyImpulseTo(a, b, rA, rB, contact.normal * jn); - - // tangent direction - pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal); - float tangentMag2 = uf::vector::magnitude(tangent); - if ( tangentMag2 > EPS2 ) { - tangent /= std::sqrt( tangentMag2 ); - - // effective mass along tangent - float invMassT = ::computeEffectiveMass(a, b, rA, rB, tangent); - - // tangential relative velocity - float vt = uf::vector::dot(rv, tangent); - - // required tangential impulse to cancel tangent velocity - float jt = -vt / invMassT; - - // friction coefficients - float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction); - float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction); - - if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally - - if ( uf::physics::impl::settings.warmupSolver ) { - float maxFriction = mu_s * contact.accumulatedNormalImpulse; - float jtOld = contact.accumulatedTangentImpulse; - float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction)); - float jtDelta = jtNew - jtOld; - contact.accumulatedTangentImpulse = jtNew; - contact.tangent = tangent; - jt = jtDelta; - } - - ::applyImpulseTo(a, b, rA, rB, tangent * jt); - } - } - - template - void blockNxNSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { - pod::Matrix K = {}; - pod::Vector rhs = {}; - pod::Vector lambda = {}; - pod::Vector residual = {}; - - // precompute inverse masses - float invMassA = ( a.isStatic ? 0.0f : a.inverseMass ); - float invMassB = ( b.isStatic ? 0.0f : b.inverseMass ); - - pod::Matrix3f invIa = computeWorldInverseInertia( a ); - pod::Matrix3f invIb = computeWorldInverseInertia( b ); - - auto pA = ::getPosition( a, true ); - auto pB = ::getPosition( b, true ); - - for ( auto i = 0; i < N; i++ ) { - pod::Vector3f rA_i = manifold.points[i].point - pA; - pod::Vector3f rB_i = manifold.points[i].point - pB; - pod::Vector3f n_i = manifold.points[i].normal; - - for ( auto j = 0; j < N; j++ ) { - pod::Vector3f rA_j = manifold.points[j].point - pA; - pod::Vector3f rB_j = manifold.points[j].point - pB; - pod::Vector3f n_j = manifold.points[j].normal; - - float termLinear = (invMassA + invMassB) * uf::vector::dot(n_i, n_j); - - // angular parts - pod::Vector3f raXnj = uf::vector::cross(rA_j, n_j); - pod::Vector3f rbXnj = uf::vector::cross(rB_j, n_j); - - - pod::Vector3f Ia_raXnj = uf::matrix::multiply( invIa, raXnj ); - pod::Vector3f Ib_rbXnj = uf::matrix::multiply( invIb, rbXnj ); - - pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA_i); - pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB_i); - - float termAngular = uf::vector::dot(n_i, crossA + crossB); - - K(i,j) = termLinear + termAngular; - } - - K(i,i) += 1e-3f; - } - - for ( auto i = 0; i < N; i++ ) { - auto& contact = manifold.points[i]; - // full relative velocity, linear + angular - pod::Vector3f vA = a.velocity + uf::vector::cross( a.angularVelocity, contact.point - pA ); - 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 - - float maxPenetrationRecovery = 2.0f; // limit to 2 units per second - if ( penetrationBias > maxPenetrationRecovery ) penetrationBias = maxPenetrationRecovery; - - rhs[i] = -vRel + penetrationBias; // RHS is magnitude of correction needed - lambda[i] = contact.accumulatedNormalImpulse; - } - - residual = rhs - uf::matrix::multiply( K, lambda ); - pod::Matrix Kinv = uf::matrix::inverse( K ); - pod::Vector dLambda = uf::matrix::multiply( Kinv, residual ); - - for ( auto i = 0; i < N; i++ ) { - float newLambda = std::max(lambda[i] + dLambda[i], 0.0f); - dLambda[i] = newLambda - lambda[i]; - lambda[i] = newLambda; - manifold.points[i].accumulatedNormalImpulse = newLambda; - } - - for ( auto i = 0; i < N; i++ ) { - pod::Vector3f rA = manifold.points[i].point - pA; - pod::Vector3f rB = manifold.points[i].point - pB; - - ::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] ); - } - } - - void block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { - return ::blockNxNSolver<2>( a, b, manifold, dt ); - } - void block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { - return ::blockNxNSolver<3>( a, b, manifold, dt ); - } - void block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { - return ::blockNxNSolver<4>( a, b, manifold, dt ); - } - - struct ContactCache { - pod::Vector3f normal; - pod::Vector3f tangent; - pod::Vector3f rA, rB; - float bias; - float effectiveMassN; - float effectiveMassT; - float accumulatedNormalImpulse; - float accumulatedTangentImpulse; - }; - - void blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { - const uint32_t count = std::min( (uint32_t) manifold.points.size(), (uint32_t) 4 ); - - // precompute contact caches - ::ContactCache cache[4]; - for ( auto i = 0; i < count; i++ ) { - auto& c = manifold.points[i]; - auto& cc = cache[i]; - - cc.normal = c.normal; - cc.tangent = ::computeTangent( c.normal ); - cc.rA = c.point - ::getPosition( a, true ); - cc.rB = c.point - ::getPosition( b, true ); - - // relative velocity along normal - pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); - float vn = uf::vector::dot( dv, cc.normal ); - - // 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); - 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 ); - pod::Vector3f rnB = uf::vector::cross( cc.rB, cc.normal ); - float Kn = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) + - uf::vector::dot( uf::vector::cross( rnA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rnB * b.inverseInertiaTensor, cc.rB ), cc.normal ); - cc.effectiveMassN = (Kn > 0.0f) ? 1.0f / Kn : 0.0f; - - // effective mass (tangent) - pod::Vector3f rtA = uf::vector::cross( cc.rA, cc.tangent ); - pod::Vector3f rtB = uf::vector::cross( cc.rB, cc.tangent ); - float Kt = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) + - uf::vector::dot( uf::vector::cross( rtA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rtB * b.inverseInertiaTensor, cc.rB ), cc.tangent ); - cc.effectiveMassT = ( Kt > 0.0f ) ? ( 1.0f / Kt ) : 0.0f; - - // warm start - #if 1 - cc.accumulatedNormalImpulse = c.accumulatedNormalImpulse; - cc.accumulatedTangentImpulse = c.accumulatedTangentImpulse; - - // apply warm-start impulses - pod::Vector3f P = cc.normal * cc.accumulatedNormalImpulse + cc.tangent * cc.accumulatedTangentImpulse; - - ::applyImpulseTo(a, b, cc.rA, cc.rB, P); - #endif - } - - // iterative PGS - for ( auto iter = 0; iter < uf::physics::impl::settings.solverIterations; iter++ ) { - for ( auto i = 0; i < count; i++ ) { - auto& cc = cache[i]; - - // relative velocity - pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); - - // normal constraint - float vn = uf::vector::dot( dv, cc.normal ); - float lambdaN = cc.effectiveMassN * (-vn + cc.bias); - float oldImpulseN = cc.accumulatedNormalImpulse; - cc.accumulatedNormalImpulse = std::max( oldImpulseN + lambdaN, 0.0f ); - float dPn = cc.accumulatedNormalImpulse - oldImpulseN; - - ::applyImpulseTo( a, b, cc.rA, cc.rB, cc.normal * dPn ); - - // friction constraint - dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); - float vt = uf::vector::dot( dv, cc.tangent ); - float lambdaT = cc.effectiveMassT * (-vt); - float maxFriction = ( a.material.dynamicFriction + b.material.dynamicFriction ) * 0.5f * cc.accumulatedNormalImpulse; - - float oldImpulseT = cc.accumulatedTangentImpulse; - cc.accumulatedTangentImpulse = std::clamp( oldImpulseT + lambdaT, -maxFriction, maxFriction ); - float dPt = cc.accumulatedTangentImpulse - oldImpulseT; - - ::applyImpulseTo( a, b, cc.rA, cc.rB, cc.tangent * dPt ); - } - } - - // store impulses back into manifold - for ( auto i = 0; i < count; i++ ) { - manifold.points[i].accumulatedNormalImpulse = cache[i].accumulatedNormalImpulse; - manifold.points[i].accumulatedTangentImpulse = cache[i].accumulatedTangentImpulse; - } - } - void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { if ( uf::physics::impl::settings.blockContactSolver ) { if ( manifold.points.size() == 2 ) return ::block2x2Solver( a, b, manifold, dt ); diff --git a/engine/src/utils/math/physics/solvers/block.inl b/engine/src/utils/math/physics/solvers/block.inl new file mode 100644 index 00000000..c21528c3 --- /dev/null +++ b/engine/src/utils/math/physics/solvers/block.inl @@ -0,0 +1,96 @@ +namespace { + template + void blockNxNSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { + pod::Matrix K = {}; + pod::Vector rhs = {}; + pod::Vector lambda = {}; + pod::Vector residual = {}; + + // precompute inverse masses + float invMassA = ( a.isStatic ? 0.0f : a.inverseMass ); + float invMassB = ( b.isStatic ? 0.0f : b.inverseMass ); + + pod::Matrix3f invIa = computeWorldInverseInertia( a ); + pod::Matrix3f invIb = computeWorldInverseInertia( b ); + + auto pA = ::getPosition( a, true ); + auto pB = ::getPosition( b, true ); + + for ( auto i = 0; i < N; i++ ) { + pod::Vector3f rA_i = manifold.points[i].point - pA; + pod::Vector3f rB_i = manifold.points[i].point - pB; + pod::Vector3f n_i = manifold.points[i].normal; + + for ( auto j = 0; j < N; j++ ) { + pod::Vector3f rA_j = manifold.points[j].point - pA; + pod::Vector3f rB_j = manifold.points[j].point - pB; + pod::Vector3f n_j = manifold.points[j].normal; + + float termLinear = (invMassA + invMassB) * uf::vector::dot(n_i, n_j); + + // angular parts + pod::Vector3f raXnj = uf::vector::cross(rA_j, n_j); + pod::Vector3f rbXnj = uf::vector::cross(rB_j, n_j); + + + pod::Vector3f Ia_raXnj = uf::matrix::multiply( invIa, raXnj ); + pod::Vector3f Ib_rbXnj = uf::matrix::multiply( invIb, rbXnj ); + + pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA_i); + pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB_i); + + float termAngular = uf::vector::dot(n_i, crossA + crossB); + + K(i,j) = termLinear + termAngular; + } + + K(i,i) += 1e-3f; + } + + for ( auto i = 0; i < N; i++ ) { + auto& contact = manifold.points[i]; + // full relative velocity, linear + angular + pod::Vector3f vA = a.velocity + uf::vector::cross( a.angularVelocity, contact.point - pA ); + 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 + + float maxPenetrationRecovery = 2.0f; // limit to 2 units per second + if ( penetrationBias > maxPenetrationRecovery ) penetrationBias = maxPenetrationRecovery; + + rhs[i] = -vRel + penetrationBias; // RHS is magnitude of correction needed + lambda[i] = contact.accumulatedNormalImpulse; + } + + residual = rhs - uf::matrix::multiply( K, lambda ); + pod::Matrix Kinv = uf::matrix::inverse( K ); + pod::Vector dLambda = uf::matrix::multiply( Kinv, residual ); + + for ( auto i = 0; i < N; i++ ) { + float newLambda = std::max(lambda[i] + dLambda[i], 0.0f); + dLambda[i] = newLambda - lambda[i]; + lambda[i] = newLambda; + manifold.points[i].accumulatedNormalImpulse = newLambda; + } + + for ( auto i = 0; i < N; i++ ) { + pod::Vector3f rA = manifold.points[i].point - pA; + pod::Vector3f rB = manifold.points[i].point - pB; + + ::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] ); + } + } + + void block2x2Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { + return ::blockNxNSolver<2>( a, b, manifold, dt ); + } + void block3x3Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { + return ::blockNxNSolver<3>( a, b, manifold, dt ); + } + void block4x4Solver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { + return ::blockNxNSolver<4>( a, b, manifold, dt ); + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/solvers/iterativeImpulse.inl b/engine/src/utils/math/physics/solvers/iterativeImpulse.inl new file mode 100644 index 00000000..1015653d --- /dev/null +++ b/engine/src/utils/math/physics/solvers/iterativeImpulse.inl @@ -0,0 +1,73 @@ +namespace { + void iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Contact& contact, float dt ) { + // relative positions from centers to contact point + pod::Vector3f rA = contact.point - ::getPosition( a, true ); + pod::Vector3f rB = contact.point - ::getPosition( b, true ); + + // relative velocity at contact + pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA); + pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB); + pod::Vector3f rv = vB - vA; + + // normal contact velocity + float velAlongNormal = uf::vector::dot(rv, contact.normal); + float velTolerance = 0; // -1.0e3f; + if ( velAlongNormal > velTolerance ) return; // if separating, no impulse + + // compute restitution (bounce) + float e = std::min(a.material.restitution, b.material.restitution); + + // nullify restitution if velocity is small enough + if ( fabs(velAlongNormal) < 1.0f) e = 0.0f; + + // effective inverse mass along normal + float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal); + + // normal impulse scalar + float jn = -(1.0f + e) * velAlongNormal; + jn /= invMassN; + if ( uf::physics::impl::settings.warmupSolver ) { + float jnOld = contact.accumulatedNormalImpulse; + float jnNew = std::max(0.0f, jnOld + jn); + float jnDelta = jnNew - jnOld; + contact.accumulatedNormalImpulse = jnNew; + jn = jnDelta; + } + + ::applyImpulseTo(a, b, rA, rB, contact.normal * jn); + + // tangent direction + pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal); + float tangentMag2 = uf::vector::magnitude(tangent); + if ( tangentMag2 > EPS2 ) { + tangent /= std::sqrt( tangentMag2 ); + + // effective mass along tangent + float invMassT = ::computeEffectiveMass(a, b, rA, rB, tangent); + + // tangential relative velocity + float vt = uf::vector::dot(rv, tangent); + + // required tangential impulse to cancel tangent velocity + float jt = -vt / invMassT; + + // friction coefficients + float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction); + float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction); + + if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally + + if ( uf::physics::impl::settings.warmupSolver ) { + float maxFriction = mu_s * contact.accumulatedNormalImpulse; + float jtOld = contact.accumulatedTangentImpulse; + float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction)); + float jtDelta = jtNew - jtOld; + contact.accumulatedTangentImpulse = jtNew; + contact.tangent = tangent; + jt = jtDelta; + } + + ::applyImpulseTo(a, b, rA, rB, tangent * jt); + } + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/solvers/psg.inl b/engine/src/utils/math/physics/solvers/psg.inl new file mode 100644 index 00000000..0588c243 --- /dev/null +++ b/engine/src/utils/math/physics/solvers/psg.inl @@ -0,0 +1,100 @@ +namespace { + struct ContactCache { + pod::Vector3f normal; + pod::Vector3f tangent; + pod::Vector3f rA, rB; + float bias; + float effectiveMassN; + float effectiveMassT; + float accumulatedNormalImpulse; + float accumulatedTangentImpulse; + }; + + void blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ) { + const uint32_t count = std::min( (uint32_t) manifold.points.size(), (uint32_t) 4 ); + + // precompute contact caches + ::ContactCache cache[4]; + for ( auto i = 0; i < count; i++ ) { + auto& c = manifold.points[i]; + auto& cc = cache[i]; + + cc.normal = c.normal; + cc.tangent = ::computeTangent( c.normal ); + cc.rA = c.point - ::getPosition( a, true ); + cc.rB = c.point - ::getPosition( b, true ); + + // relative velocity along normal + pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); + float vn = uf::vector::dot( dv, cc.normal ); + + // 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); + 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 ); + pod::Vector3f rnB = uf::vector::cross( cc.rB, cc.normal ); + float Kn = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) + + uf::vector::dot( uf::vector::cross( rnA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rnB * b.inverseInertiaTensor, cc.rB ), cc.normal ); + cc.effectiveMassN = (Kn > 0.0f) ? 1.0f / Kn : 0.0f; + + // effective mass (tangent) + pod::Vector3f rtA = uf::vector::cross( cc.rA, cc.tangent ); + pod::Vector3f rtB = uf::vector::cross( cc.rB, cc.tangent ); + float Kt = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) + + uf::vector::dot( uf::vector::cross( rtA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rtB * b.inverseInertiaTensor, cc.rB ), cc.tangent ); + cc.effectiveMassT = ( Kt > 0.0f ) ? ( 1.0f / Kt ) : 0.0f; + + // warm start + #if 1 + cc.accumulatedNormalImpulse = c.accumulatedNormalImpulse; + cc.accumulatedTangentImpulse = c.accumulatedTangentImpulse; + + // apply warm-start impulses + pod::Vector3f P = cc.normal * cc.accumulatedNormalImpulse + cc.tangent * cc.accumulatedTangentImpulse; + + ::applyImpulseTo(a, b, cc.rA, cc.rB, P); + #endif + } + + // iterative PGS + for ( auto iter = 0; iter < uf::physics::impl::settings.solverIterations; iter++ ) { + for ( auto i = 0; i < count; i++ ) { + auto& cc = cache[i]; + + // relative velocity + pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); + + // normal constraint + float vn = uf::vector::dot( dv, cc.normal ); + float lambdaN = cc.effectiveMassN * (-vn + cc.bias); + float oldImpulseN = cc.accumulatedNormalImpulse; + cc.accumulatedNormalImpulse = std::max( oldImpulseN + lambdaN, 0.0f ); + float dPn = cc.accumulatedNormalImpulse - oldImpulseN; + + ::applyImpulseTo( a, b, cc.rA, cc.rB, cc.normal * dPn ); + + // friction constraint + dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); + float vt = uf::vector::dot( dv, cc.tangent ); + float lambdaT = cc.effectiveMassT * (-vt); + float maxFriction = ( a.material.dynamicFriction + b.material.dynamicFriction ) * 0.5f * cc.accumulatedNormalImpulse; + + float oldImpulseT = cc.accumulatedTangentImpulse; + cc.accumulatedTangentImpulse = std::clamp( oldImpulseT + lambdaT, -maxFriction, maxFriction ); + float dPt = cc.accumulatedTangentImpulse - oldImpulseT; + + ::applyImpulseTo( a, b, cc.rA, cc.rB, cc.tangent * dPt ); + } + } + + // store impulses back into manifold + for ( auto i = 0; i < count; i++ ) { + manifold.points[i].accumulatedNormalImpulse = cache[i].accumulatedNormalImpulse; + manifold.points[i].accumulatedTangentImpulse = cache[i].accumulatedTangentImpulse; + } + } +} \ No newline at end of file