Add system that asserts physics components are finite

src/schedule/mod.rs

@@ -117,6 +117,12 @@ impl Plugin for PhysicsSchedulePlugin {
             PhysicsSchedule,
             update_last_physics_tick.after(PhysicsStepSystems::Last),
         );
+
+        #[cfg(debug_assertions)]
+        app.add_systems(
+            schedule,
+            assert_components_finite.in_set(PhysicsSystems::First),
+        );
     }
 }
 
@@ -283,3 +289,33 @@ fn update_last_physics_tick(
 ) {
     last_physics_tick.0 = system_change_tick.this_run();
 }
+
+/// Debug system that checks for NaNs and infinities in Avian components and
+/// reports the last location they were written to.
+#[cfg(debug_assertions)]
+fn assert_components_finite(
+    pos_query: Query<(Entity, Ref<Position>)>,
+    lin_vel_query: Query<(Entity, Ref<LinearVelocity>)>,
+    ang_vel_query: Query<(Entity, Ref<AngularVelocity>)>,
+) {
+    macro_rules! assert_finite {
+        ($ent:expr, $val:ident, $ty:ty) => {
+            debug_assert!(
+                $val.is_finite(),
+                "NaN or infinity found in Avian component: type={} entity={} location='{}' (enable feature \"bevy/track_location\" if location is empty)",
+                stringify!($ty),
+                $ent,
+                $val.changed_by()
+            );
+        };
+    }
+    for (entity, position) in pos_query {
+        assert_finite!(entity, position, Position);
+    }
+    for (entity, velocity) in lin_vel_query {
+        assert_finite!(entity, velocity, LinearVelocity);
+    }
+    for (entity, velocity) in ang_vel_query {
+        assert_finite!(entity, velocity, AngularVelocity);
+    }
+}