Capture first invalid body around shared physical solve

crates/skate-game/src/physics.rs

@@ -324,7 +324,8 @@ fn advance(
     ) {
         physics.failed = true;
         error!(
-            "{message}; tick={}; mapped_input={:?}; force_mode={}; board_axis_y={}; flags={:08x}/{:08x}/{:08x}/{:08x}/{:08x}",
+            "{message}; state={:?}; tick={}; mapped_input={:?}; force_mode={}; board_axis_y={}; flags={:08x}/{:08x}/{:08x}/{:08x}/{:08x}",
+            skater.player_state.current(),
             physics.ticks,
             input.0.actions(),
             skater.skeleton_input.force_mode,

crates/skate-game/src/physics/solve.rs

@@ -1,6 +1,7 @@
 //! One physical solve for the board, skater and original animation targets.
 //! The caller publishes forces and drive targets before entering this phase.
 mod assembly_contacts;
+mod diagnostics;
 use super::{GamePhysics, SkaterRuntime, colliders, skeleton_colliders};
 use skate_core::physics::{
     board::BodyId,
@@ -14,6 +15,8 @@ pub(super) fn advance(
     skater: &mut SkaterRuntime,
     truck_targets: [f32; 2],
 ) -> Result<(), String> {
+    let before = diagnostics::snapshot(physics, skater);
+    diagnostics::validate(&before, "before shared solve")?;
     let board_volumes = if skater.offboard.board_policy.volumes_enabled {
         colliders::world_volumes(&physics.board, &physics.settings)
     } else {
@@ -86,6 +89,11 @@ pub(super) fn advance(
             drives: &mut drives.rows,
         },
     );
+    if let Err(error) = diagnostics::validate(
+        &diagnostics::snapshot(physics, skater), "after shared solve",
+    ) {
+        return Err(format!("{error}; input_bodies={before:?}; contacts={contacts:?}; joints={joints:?}; drives={:?}", drives.rows));
+    }
     skater
         .skeleton
         .publish_physical_record(deck_frame(&physics.board));

crates/skate-game/src/physics/solve/diagnostics.rs

@@ -0,0 +1,35 @@
+//! Locate invalid physical values before they propagate into a rendered pose.
+use super::{GamePhysics, SkaterRuntime};
+use skate_core::physics::assembly::BodySnapshot;
+
+pub(super) fn snapshot(physics: &GamePhysics, skater: &SkaterRuntime) -> Vec<BodySnapshot> {
+    physics.board.bodies().iter()
+        .chain(std::iter::once(&physics.board.hook().body))
+        .chain(skater.skeleton.bodies())
+        .chain(skater.skeleton_drives.targets.bodies.iter())
+        .copied().collect()
+}
+
+pub(super) fn validate(bodies: &[BodySnapshot], stage: &str) -> Result<(), String> {
+    for (index, body) in bodies.iter().enumerate() {
+        let r = &body.rates;
+        for (name, v) in [
+            ("position", r.position),
+            ("linear_velocity", r.linear_velocity),
+            ("angular_velocity", r.angular_velocity),
+            ("force_acceleration", r.force_acceleration),
+            ("torque_acceleration", r.torque_acceleration),
+        ] {
+            if ![v.x, v.y, v.z].into_iter().all(f32::is_finite) {
+                return Err(format!("Non-finite {name} {stage}; reaction_body={index}; body={body:?}"));
+            }
+        }
+        let q = r.orientation;
+        if ![q.x, q.y, q.z, q.w].into_iter().all(f32::is_finite)
+            || !r.basis.columns.iter().flatten().all(|v| v.is_finite())
+            || !r.world_inverse_inertia.columns.iter().flatten().all(|v| v.is_finite()) {
+            return Err(format!("Non-finite orientation/inertia {stage}; reaction_body={index}; body={body:?}"));
+        }
+    }
+    Ok(())
+}