From 7bd4b00514941532e9a0fba9566cc297518d1e9d Mon Sep 17 00:00:00 2001 From: ecker Date: Wed, 20 May 2026 22:32:34 -0500 Subject: [PATCH] NGS position solver fixed because of a pesky minus sign --- bin/data/config.json | 23 ++++++++----- bin/data/entities/ball.json | 2 +- bin/data/entities/scripts/door.lua | 4 +-- bin/data/entities/scripts/player.lua | 2 +- engine/inc/uf/utils/math/physics/impl.h | 31 ++++++++--------- engine/src/engine/ext/ext.cpp | 25 +++++++++----- engine/src/engine/graph/graph.cpp | 4 +-- engine/src/engine/object/behavior.cpp | 1 - engine/src/ext/lua/usertypes/physics.cpp | 2 +- engine/src/utils/math/physics/common.cpp | 4 +++ engine/src/utils/math/physics/impl.cpp | 33 ++++++++++--------- engine/src/utils/math/physics/integration.cpp | 2 +- engine/src/utils/math/physics/solvers.cpp | 8 ++--- .../src/utils/math/physics/solvers/block.cpp | 2 +- .../math/physics/solvers/iterativeImpulse.cpp | 2 +- 15 files changed, 81 insertions(+), 64 deletions(-) diff --git a/bin/data/config.json b/bin/data/config.json index 3a782bff..6e6e1ad6 100644 --- a/bin/data/config.json +++ b/bin/data/config.json @@ -336,19 +336,24 @@ "discord": { "enabled": false } }, "physics": { - "warmup solver": true, - "block solver": false, - "resolve block true": true, - "correction percent": 0.2, - "correction slop": 0.01, - "max correction": 0.01, - "gjk": false, + "solvers": { + "warmup": true, + "block": true, + "resolve invalid": true, + "iterations": 10, + "gjk": false + }, + "correction": { + "ngs": true, + "percent": 0.2, + "slop": 0.01, // 0.005 + "max": 0.01 // 0.2 + }, "debug draw": { "dynamic": true }, "fixed step": true, - "substeps": 4, - "solver iterations": 10 + "substeps": 4 }, "audio": { "mute": false, diff --git a/bin/data/entities/ball.json b/bin/data/entities/ball.json index 8dd33b02..f51e42d7 100644 --- a/bin/data/entities/ball.json +++ b/bin/data/entities/ball.json @@ -4,7 +4,7 @@ "SoundEmitterBehavior" ], "transform": { - "position": [ 0, 3, 0 ], + "position": [ -0.574743, 2.3547, -5.05161 ], "rotation": { "axis": [ 0, 1, 0 ], "angle": 0 diff --git a/bin/data/entities/scripts/door.lua b/bin/data/entities/scripts/door.lua index 5ba0b188..5772db16 100644 --- a/bin/data/entities/scripts/door.lua +++ b/bin/data/entities/scripts/door.lua @@ -111,9 +111,9 @@ ent:addHook( "entity:Use.%UID%", function( payload ) local delta = transform.position - userTransform.position local side = normal:dot(delta) if side > 0 then - polarity = 1 - elseif side < 0 then polarity = -1 + elseif side < 0 then + polarity = 1 end end elseif state == 2 --[[or state == 1]] then diff --git a/bin/data/entities/scripts/player.lua b/bin/data/entities/scripts/player.lua index b4f0150d..f4a36876 100644 --- a/bin/data/entities/scripts/player.lua +++ b/bin/data/entities/scripts/player.lua @@ -25,7 +25,7 @@ local heldObject = { uid = 0, distance = 0, smoothSpeed = 4, - scrollSpeed = 16, + scrollSpeed = 32, momentum = Vector3f(0,0,0), rotate = false, } diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index 1c72faba..8cd47cb8 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -200,25 +200,25 @@ namespace pod { float inverseMass = 1.0f; // for fast division int32_t viewIndex = -1; // -1 means it's not an aliased view - /*alignas(16)*/ pod::Vector3f offset = {}; + pod::Vector3f offset = {}; - /*alignas(16)*/ pod::Vector3f velocity = {}; - /*alignas(16)*/ pod::Vector3f pseudoVelocity = {}; - /*alignas(16)*/ pod::Vector3f forceAccumulator = {}; + pod::Vector3f velocity = {}; + pod::Vector3f forceAccumulator = {}; - /*alignas(16)*/ pod::Vector3f angularVelocity = {}; - /*alignas(16)*/ pod::Vector3f pseudoAngularVelocity = {}; - /*alignas(16)*/ pod::Vector3f torqueAccumulator = {}; + pod::Vector3f angularVelocity = {}; + pod::Vector3f torqueAccumulator = {}; - /*alignas(16)*/ pod::Vector3f inertiaTensor = { 1, 1, 1 }; - /*alignas(16)*/ pod::Vector3f inverseInertiaTensor = { 1, 1, 1 }; + pod::Vector3f pseudoVelocity = {}; + pod::Vector3f pseudoAngularVelocity = {}; - /*alignas(16)*/ pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity + pod::Vector3f inverseInertiaTensor = { 1, 1, 1 }; - /*alignas(16)*/ pod::AABB bounds; - /*alignas(16)*/ pod::Collider collider; - /*alignas(16)*/ pod::PhysicsMaterial material; - /*alignas(16)*/ pod::Activity activity; + pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity + + pod::AABB bounds; + pod::Collider collider; + pod::PhysicsMaterial material; + pod::Activity activity; }; struct Contact { @@ -231,7 +231,7 @@ namespace pod { // warm-start cached values int lifetime = 0; - pod::Vector3f tangent = {}; + pod::Vector3f tangent; float accumulatedNormalImpulse = 0.0f; float accumulatedTangentImpulse = 0.0f; float accumulatedPseudoImpulse = 0.0f; @@ -287,6 +287,7 @@ namespace pod { // to-do: find possibly better values for this uint32_t solverIterations = 10; + bool ngsPositionSolver = false; float baumgarteCorrectionPercent = 0.2f; float baumgarteCorrectionSlop = 0.005f; float maxLinearCorrection = 0.2f; diff --git a/engine/src/engine/ext/ext.cpp b/engine/src/engine/ext/ext.cpp index 77b3e48e..348314af 100644 --- a/engine/src/engine/ext/ext.cpp +++ b/engine/src/engine/ext/ext.cpp @@ -201,19 +201,26 @@ void UF_API uf::load( ext::json::Value& json ) { // Physics settings { auto& configEnginePhysicsJson = json["engine"]["physics"]; - uf::physics::settings.warmupSolver = configEnginePhysicsJson["warmup solver"].as(uf::physics::settings.warmupSolver); - uf::physics::settings.blockContactSolver = configEnginePhysicsJson["block solver"].as(uf::physics::settings.blockContactSolver); - uf::physics::settings.resolveBlockContact = configEnginePhysicsJson["resolve block solver"].as(uf::physics::settings.resolveBlockContact); - uf::physics::settings.useGjk = configEnginePhysicsJson["gjk"].as(uf::physics::settings.useGjk); uf::physics::settings.async = configEnginePhysicsJson["async"].as(uf::physics::settings.async); uf::physics::settings.timestep = configEnginePhysicsJson["timestep"].as(uf::physics::settings.timestep); uf::physics::settings.fixedStep = configEnginePhysicsJson["fixed step"].as(uf::physics::settings.fixedStep); uf::physics::settings.substeps = configEnginePhysicsJson["substeps"].as(uf::physics::settings.substeps); - uf::physics::settings.solverIterations = configEnginePhysicsJson["solver iterations"].as(uf::physics::settings.solverIterations); - uf::physics::settings.baumgarteCorrectionPercent = configEnginePhysicsJson["correction percent"].as(uf::physics::settings.baumgarteCorrectionPercent); - uf::physics::settings.baumgarteCorrectionSlop = configEnginePhysicsJson["correction slop"].as(uf::physics::settings.baumgarteCorrectionSlop); - uf::physics::settings.maxLinearCorrection = configEnginePhysicsJson["max correction"].as(uf::physics::settings.maxLinearCorrection); - + + auto& configEnginePhysicsSolverJson = configEnginePhysicsJson["solvers"]; + if ( ext::json::isObject( configEnginePhysicsSolverJson ) ) { + uf::physics::settings.useGjk = configEnginePhysicsSolverJson["gjk"].as(uf::physics::settings.useGjk); + uf::physics::settings.blockContactSolver = configEnginePhysicsSolverJson["block"].as(uf::physics::settings.blockContactSolver); + uf::physics::settings.warmupSolver = configEnginePhysicsSolverJson["warmup"].as(uf::physics::settings.warmupSolver); + uf::physics::settings.resolveBlockContact = configEnginePhysicsSolverJson["resolve invalid"].as(uf::physics::settings.resolveBlockContact); + uf::physics::settings.solverIterations = configEnginePhysicsSolverJson["iterations"].as(uf::physics::settings.solverIterations); + } + auto& configEnginePhysicsCorrectionJson = configEnginePhysicsJson["correction"]; + if ( ext::json::isObject( configEnginePhysicsCorrectionJson ) ) { + uf::physics::settings.ngsPositionSolver = configEnginePhysicsCorrectionJson["ngs"].as(uf::physics::settings.ngsPositionSolver); + uf::physics::settings.baumgarteCorrectionPercent = configEnginePhysicsCorrectionJson["percent"].as(uf::physics::settings.baumgarteCorrectionPercent); + uf::physics::settings.baumgarteCorrectionSlop = configEnginePhysicsCorrectionJson["slop"].as(uf::physics::settings.baumgarteCorrectionSlop); + uf::physics::settings.maxLinearCorrection = configEnginePhysicsCorrectionJson["max"].as(uf::physics::settings.maxLinearCorrection); + } auto& configEnginePhysicsDebugDrawJson = configEnginePhysicsJson["debug draw"]; if ( ext::json::isObject( configEnginePhysicsDebugDrawJson ) ) { if ( configEnginePhysicsDebugDrawJson["static"].as() ) uf::physics::settings.debugDraw |= pod::Collider::CATEGORY_STATIC; diff --git a/engine/src/engine/graph/graph.cpp b/engine/src/engine/graph/graph.cpp index 95296fed..6108e181 100644 --- a/engine/src/engine/graph/graph.cpp +++ b/engine/src/engine/graph/graph.cpp @@ -1335,7 +1335,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent ) physicsBody.material.staticFriction = phyziks["friction"].as(physicsBody.material.staticFriction); physicsBody.material.restitution = phyziks["restitution"].as(physicsBody.material.restitution); - physicsBody.inertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inertiaTensor ); + physicsBody.inverseInertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inverseInertiaTensor ); physicsBody.gravity = uf::vector::decode( phyziks["gravity"], physicsBody.gravity ); auto center = uf::vector::decode( phyziks["center"], pod::Vector3f{} ); @@ -2007,7 +2007,7 @@ void uf::graph::reload( pod::Graph& graph, pod::Node& node ) { physicsBody.material.staticFriction = phyziks["friction"].as(physicsBody.material.staticFriction); physicsBody.material.restitution = phyziks["restitution"].as(physicsBody.material.restitution); - physicsBody.inertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inertiaTensor ); + physicsBody.inverseInertiaTensor = uf::vector::decode( phyziks["inertia"], physicsBody.inverseInertiaTensor ); physicsBody.gravity = uf::vector::decode( phyziks["gravity"], physicsBody.gravity ); auto center = uf::vector::decode( phyziks["center"], pod::Vector3f{} ); diff --git a/engine/src/engine/object/behavior.cpp b/engine/src/engine/object/behavior.cpp index d9de086d..30bca59c 100644 --- a/engine/src/engine/object/behavior.cpp +++ b/engine/src/engine/object/behavior.cpp @@ -196,7 +196,6 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) { physicsBody.angularVelocity = uf::vector::decode( metadataJsonPhysics["angularVelocity"], physicsBody.angularVelocity ); if ( metadataJsonPhysics["inertia"].is() && !metadataJsonPhysics["inertia"].as() ) { - physicsBody.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; physicsBody.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; } } diff --git a/engine/src/ext/lua/usertypes/physics.cpp b/engine/src/ext/lua/usertypes/physics.cpp index 914cd37b..81c975e2 100644 --- a/engine/src/ext/lua/usertypes/physics.cpp +++ b/engine/src/ext/lua/usertypes/physics.cpp @@ -30,7 +30,7 @@ namespace binds { } std::tuple rayCast( pod::PhysicsBody& self, const pod::Vector3f& center, const pod::Vector3f& direction ) { - pod::RayQuery query = uf::physics::rayCast( pod::Ray{center, direction}, self, uf::vector::norm( direction ) ); + pod::RayQuery query = uf::physics::rayCast( pod::Ray{center, uf::vector::normalize( direction )}, self, uf::vector::norm( direction ) ); uf::Object* object = query.hit ? query.body->object : NULL; float depth = query.hit ? query.contact.penetration : -1; return std::make_tuple( object, depth ); diff --git a/engine/src/utils/math/physics/common.cpp b/engine/src/utils/math/physics/common.cpp index a1696eae..9ce89a0b 100644 --- a/engine/src/utils/math/physics/common.cpp +++ b/engine/src/utils/math/physics/common.cpp @@ -85,6 +85,10 @@ pod::PhysicsBody impl::physicsBodyTriView( const pod::TriangleWithNormal triangl pod::PhysicsBody view = body; view.collider.type = pod::ShapeType::TRIANGLE; view.collider.triangle = triangle; + // calculate normal if needed + if ( uf::vector::magnitude( view.collider.triangle.normal ) < 0.001f ) { + view.collider.triangle.normal = impl::triangleNormal( (const pod::Triangle&) triangle ); + } // assume triangle is already transformed view.offset = {}; view.transform = NULL; diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index a5d1bc4e..e7866131 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -259,30 +259,30 @@ pod::Vector3f uf::physics::getGravity( pod::PhysicsBody& body ) { void uf::physics::updateInertia( pod::PhysicsBody& body ) { if ( body.isStatic || body.mass <= 0 ) { - body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; return; } + pod::Vector3f inertiaTensor = {}; switch ( body.collider.type ) { case pod::ShapeType::AABB: { 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, EPS, EPS } ); - body.inverseInertiaTensor = 1.0f / body.inertiaTensor; + inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f); + inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } ); + body.inverseInertiaTensor = 1.0f / inertiaTensor; } break; case pod::ShapeType::OBB: { pod::Vector3f dims = body.collider.obb.extent * 2.0f; 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, EPS, EPS } ); - body.inverseInertiaTensor = 1.0f / body.inertiaTensor; + inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f); + inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } ); + body.inverseInertiaTensor = 1.0f / inertiaTensor; } break; case pod::ShapeType::SPHERE: { float I = 0.4f * body.mass * body.collider.sphere.radius * body.collider.sphere.radius; float invI = 1.0f / I; - body.inertiaTensor = { I, I, I }; + inertiaTensor = { I, I, I }; body.inverseInertiaTensor = { invI, invI, invI }; } break; case pod::ShapeType::CAPSULE: { @@ -294,7 +294,7 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) { float Iyy = 0.5f * m * r * r; float Izz = Ixx; - body.inertiaTensor = { Ixx, Iyy, Izz }; + inertiaTensor = { Ixx, Iyy, Izz }; body.inverseInertiaTensor = { 1.0f/Ixx, 1.0f/Iyy, 1.0f/Izz }; } break; case pod::ShapeType::MESH: @@ -304,9 +304,9 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) { #if 1 pod::Vector3f dims = (body.bounds.max - body.bounds.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, EPS, EPS } ); - body.inverseInertiaTensor = 1.0f / body.inertiaTensor; + inertiaTensor = pod::Vector3f{ dimsSq.y + dimsSq.z, dimsSq.x + dimsSq.z, dimsSq.x + dimsSq.y } * (body.mass / 12.0f); + inertiaTensor = uf::vector::max( inertiaTensor, { EPS, EPS, EPS } ); + body.inverseInertiaTensor = 1.0f / inertiaTensor; #else pod::Matrix3f inertia = {}; float totalVolume = 0.0f; @@ -321,7 +321,7 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) { } if ( totalVolume < EPS ) { - body.inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; + inertiaTensor = { FLT_MAX, FLT_MAX, FLT_MAX }; body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; } else { // accumulate inertia @@ -353,8 +353,8 @@ void uf::physics::updateInertia( pod::PhysicsBody& body ) { inertia += Ibox + pat; } - body.inertiaTensor = { inertia(0,0), inertia(1,1), inertia(2,2) }; - body.inverseInertiaTensor = 1.0f / body.inertiaTensor; + inertiaTensor = { inertia(0,0), inertia(1,1), inertia(2,2) }; + body.inverseInertiaTensor = 1.0f / inertiaTensor; } #endif } break; @@ -514,6 +514,9 @@ pod::PhysicsBody& uf::physics::create( pod::World& world, uf::Object& object, co auto& body = uf::physics::create( world, object, mass, offset ); body.collider.type = pod::ShapeType::TRIANGLE; body.collider.triangle = tri; + if ( uf::vector::magnitude( body.collider.triangle.normal ) < 0.001f ) { + body.collider.triangle.normal = impl::triangleNormal( (const pod::Triangle&) tri ); + } body.bounds = impl::computeAABB( body ); uf::physics::updateInertia( body ); return body; diff --git a/engine/src/utils/math/physics/integration.cpp b/engine/src/utils/math/physics/integration.cpp index fc893990..5078152a 100644 --- a/engine/src/utils/math/physics/integration.cpp +++ b/engine/src/utils/math/physics/integration.cpp @@ -470,7 +470,7 @@ void impl::integrate( pod::PhysicsBody& body, float dt ) { } // pseudo-impulse position correction - { + if ( !uf::physics::settings.ngsPositionSolver ) { body.transform->position += body.pseudoVelocity * dt; float pseudoAngularSpeed2 = uf::vector::magnitude( body.pseudoAngularVelocity ); diff --git a/engine/src/utils/math/physics/solvers.cpp b/engine/src/utils/math/physics/solvers.cpp index 39f0bf3f..09fe7468 100644 --- a/engine/src/utils/math/physics/solvers.cpp +++ b/engine/src/utils/math/physics/solvers.cpp @@ -14,10 +14,9 @@ void impl::solveContacts( uf::stl::vector& manifolds, float dt ) for ( auto i = 0; i < uf::physics::settings.solverIterations; ++i ) for ( auto& manifold : manifolds ) impl::resolveManifold( *manifold.a, *manifold.b, manifold, dt ); } void impl::solvePositions( uf::stl::vector& manifolds, float dt, uint32_t iterations ) { - if ( true || uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return; + if ( !uf::physics::settings.ngsPositionSolver ) return; + if ( uf::physics::settings.baumgarteCorrectionPercent <= 0 ) return; for ( auto i = 0; i < iterations; ++i ) { - float minSeparation = 0.0f; - for ( auto& manifold : manifolds ) { auto& a = *manifold.a; auto& b = *manifold.b; @@ -32,8 +31,7 @@ void impl::solvePositions( uf::stl::vector& manifolds, float dt, pod::Vector3f worldA = tA.position + rA; pod::Vector3f worldB = tB.position + rB; - float penetration = -uf::vector::dot( worldB - worldA, c.normal ); - minSeparation = std::min( minSeparation, -penetration ); + float penetration = uf::vector::dot( worldB - worldA, c.normal ); float C = std::clamp( penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f, uf::physics::settings.maxLinearCorrection ); if ( C <= 0.0f ) continue; diff --git a/engine/src/utils/math/physics/solvers/block.cpp b/engine/src/utils/math/physics/solvers/block.cpp index 6cb3a65c..5a11982f 100644 --- a/engine/src/utils/math/physics/solvers/block.cpp +++ b/engine/src/utils/math/physics/solvers/block.cpp @@ -119,7 +119,7 @@ namespace impl { impl::applyImpulseTo( a, b, rA, rB, manifold.points[i].normal * dLambda[i] ); } // pseudo impulse - { + if ( !uf::physics::settings.ngsPositionSolver ) { float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt); penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt); diff --git a/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp b/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp index 481ad8ad..ae0cfd91 100644 --- a/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp +++ b/engine/src/utils/math/physics/solvers/iterativeImpulse.cpp @@ -31,7 +31,7 @@ void impl::iterativeImpulseSolver( pod::PhysicsBody& a, pod::PhysicsBody& b, pod impl::applyImpulseTo(a, b, rA, rB, contact.normal * jn); } // pseudo impulse - { + if ( !uf::physics::settings.ngsPositionSolver ) { float penetrationBias = std::max(contact.penetration - uf::physics::settings.baumgarteCorrectionSlop, 0.0f) * (uf::physics::settings.baumgarteCorrectionPercent / dt); penetrationBias = std::min(penetrationBias, uf::physics::settings.maxLinearCorrection / dt);