added grounded state, attempted to try and snap velocities for grounded bodies with small enough velocities (but it doesn't seem to prevent jittering bodies that resolve collision-from-gravity......)

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

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

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 );

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 ) {

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);

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 );
 			}
 		}