diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index 0c604691..87396b96 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -157,6 +157,7 @@ namespace pod { struct Activity { bool awake = true; + bool grounded = false; float sleepTimer = 0.0f; int32_t islandID = -1; static constexpr float sleepThreshold = 0.5f; // seconds diff --git a/engine/src/engine/ext/player/behavior.cpp b/engine/src/engine/ext/player/behavior.cpp index cd8d04b4..df7702bc 100644 --- a/engine/src/engine/ext/player/behavior.cpp +++ b/engine/src/engine/ext/player/behavior.cpp @@ -283,6 +283,10 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { //if ( physicsBody.velocity.y < 0.0f ) physicsBody.velocity.y = 0.0f; } #else + if ( physicsBody.activity.grounded ) { + stats.floored = true; + } + /* pod::Vector3f origin = transform.position + metadata.movement.floored.feet; pod::Vector3f direction = metadata.movement.floored.floor; pod::RayQuery query = uf::physics::impl::rayCast( pod::Ray{origin, direction}, physicsBody, 1.0f ); @@ -292,6 +296,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { stats.floored = true; //if ( physicsBody.velocity.y < 0.0f ) physicsBody.velocity.y = 0.0f; } + */ #endif } #if 0 diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index c10cf14e..b4fef2bf 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -75,6 +75,9 @@ namespace { body.angularVelocity = pod::Vector3f{}; } void updateActivity( pod::PhysicsBody& body, float dt ) { + // reset grounded state + body.activity.grounded = false; + // already asleep if ( !body.activity.awake ) return; @@ -85,7 +88,9 @@ namespace { // body is nearly still if ( linSpeed < pod::Activity::linearSleepEpsilon && angSpeed < pod::Activity::angularSleepEpsilon ) { body.activity.sleepTimer += dt; - if ( body.activity.sleepTimer > pod::Activity::sleepThreshold ) ::sleepBody( body ); + float threshold = pod::Activity::sleepThreshold; + if ( body.activity.grounded ) threshold *= 0.25f; + if ( body.activity.sleepTimer > threshold ) ::sleepBody( body ); } // body is moving, reset timer else ::wakeBody( body ); diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index ea95cc84..1b6b4239 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -44,6 +44,8 @@ namespace { .dirtyRatioThreshold = 0.3f, .maxFramesBeforeRebuild = 60, // * 10, // 10 seconds }; + + float groundedThreshold = 0.7f; // threshold before marking a body as grounded } #define EPS(x) x // 1.0e-6f @@ -130,7 +132,6 @@ void uf::physics::impl::step( pod::World& world, float dt ) { ++::frameCounter; for ( auto* body : bodies ) { - if ( !body->activity.awake ) continue; ::integrate( *body, dt ); } @@ -198,6 +199,14 @@ void uf::physics::impl::step( pod::World& world, float dt ) { // wake up bodies if ( a.activity.awake && !b.activity.awake ) ::wakeBody( b ); if ( b.activity.awake && !a.activity.awake ) ::wakeBody( a ); + // mark as grounded + for ( auto& c : manifold.points ) { + if ( std::fabs(uf::vector::dot(c.normal, pod::Vector3f{0,1,0})) > ::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; + } + } // store manifold manifolds.emplace_back(manifold); @@ -210,6 +219,11 @@ void uf::physics::impl::step( pod::World& world, float dt ) { // cache manifold positions if ( ::warmupSolver ) ::storeManifolds( manifolds, ::manifoldsCache ); } + + for ( auto* b : bodies ) { + if ( b->isStatic ) continue; + ::snapVelocity( *b, dt ); + } } void uf::physics::impl::setMass( pod::PhysicsBody& body, float mass ) { diff --git a/engine/src/utils/math/physics/integration.inl b/engine/src/utils/math/physics/integration.inl index ed50e521..7758a2b3 100644 --- a/engine/src/utils/math/physics/integration.inl +++ b/engine/src/utils/math/physics/integration.inl @@ -221,6 +221,22 @@ namespace { ::warmupContacts( a, b, contact, dt ); } } + + // snap velocity for grounded bodies + void snapVelocity( pod::PhysicsBody& body, float dt, float threshold = 0.01f ) { + if ( !body.activity.grounded || !body.activity.awake ) return; + + // snap velocity if body is grounded and nearly still + float linSpeed = uf::vector::norm( body.velocity ); + float angSpeed = uf::vector::norm( body.angularVelocity ); + + // cancel out vertical component + if ( fabs(body.velocity.y) < threshold ) body.velocity.y = 0.0f; + // cancel out velocity entirely + if ( linSpeed < threshold ) body.velocity = {}; + // cancel out rotational velocity entirely + if ( angSpeed < threshold ) body.angularVelocity = {}; + } // baumgarte position correction void positionCorrection( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& contact ) { @@ -239,13 +255,16 @@ namespace { // apply correction vector pod::Vector3f correction = contact.normal * (penetration / totalInvMass) * ::baumgarteCorrectionPercent; + if ( !a.isStatic ) a.transform->position -= correction * invMassA; if ( !b.isStatic ) b.transform->position += correction * invMassB; } + void integrate( pod::PhysicsBody& body, float dt ) { - // only integrate dynamic bodies - if ( body.isStatic || body.mass == 0 ) return; + + // only integrate awake and dynamic bodies + if ( !body.activity.awake || body.isStatic || body.mass == 0 ) return; auto& world = *body.world; @@ -253,11 +272,13 @@ namespace { pod::Vector3f acceleration = body.forceAccumulator * body.inverseMass; acceleration += uf::physics::impl::getGravity( body ); // apply gravity body.velocity += acceleration * dt; - body.transform/*.reference*/->position += body.velocity * dt; // angular integration body.angularVelocity += body.torqueAccumulator * body.inverseInertiaTensor * dt; + // update position + body.transform/*.reference*/->position += body.velocity * dt; + // update orientation if ( uf::vector::magnitude( body.angularVelocity ) > EPS(1.0e-8f) ) { pod::Quaternion<> dq = uf::quaternion::axisAngle(uf::vector::normalize(body.angularVelocity), uf::vector::magnitude(body.angularVelocity)*dt); diff --git a/engine/src/utils/math/physics/solvers.inl b/engine/src/utils/math/physics/solvers.inl index 9e694107..9625e513 100644 --- a/engine/src/utils/math/physics/solvers.inl +++ b/engine/src/utils/math/physics/solvers.inl @@ -300,6 +300,7 @@ namespace { weight += w; } s.normal = weight > 0.0f ? uf::vector::normalize(s.normal) : pod::Vector3f{0,1,0}; + ::positionCorrection( *m.a, *m.b, s ); } }