Add system that asserts physics components are finite (#909)

# Objective

A lot of people (me included) have had to track down NaNs or infinities that causes avian internals to crash, and they can be difficult to track down because they cause crashes a lot later than they are introduced.

## Solution

Add a system when running with debug assertions that checks relevant components during `PhysicsSystems::First`.

For bugs in Avian internals we can potentially also schedule this system in other places.

### Questions
* I placed it in `PhysicsSchedulePlugin` because it feels more general purpose than the rest of `PhysicsDebugPlugin` and because it needs `self.schedule`, but I'm not sure if that's the best location for it?
* We could make the system use `assert!` instead of `debug_assert!` and make it runnable outside of `debug_assertions`?

## Testing

I modified one of the examples to introduce an infinity during Update and saw that it was caught.

Author: kristoff3r

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