feat: add simple inverse-kinematics solver for multibodies (#632)

* feat: add a simple jacobian-based inverse-kinematics implementation for multibodies

* feat: add 2d inverse kinematics example

* feat: make forward_kinematics auto-fix the root’s degrees of freedom

* feat: add 3d inverse kinematics example

* chore: update changelog

* chore: clippy fixes

* chore: more clippy fixes

* fix tests

3 files changed, 72 insertions, 5 deletions

diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index cb24d7e..36c9199 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -25,6 +25,7 @@ mod camera3d;
 mod debug_render;
 mod graphics;
 pub mod harness;
+mod mouse;
 pub mod objects;
 pub mod physics;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
diff --git a/src_testbed/mouse.rs b/src_testbed/mouse.rs
new file mode 100644
index 0000000..05b69e2
--- /dev/null
+++ b/src_testbed/mouse.rs
@@ -0,0 +1,46 @@
+use crate::math::Point;
+use bevy::prelude::*;
+use bevy::window::PrimaryWindow;
+
+#[derive(Component)]
+pub struct MainCamera;
+
+#[derive(Default, Copy, Clone, Debug, Resource)]
+pub struct SceneMouse {
+    #[cfg(feature = "dim2")]
+    pub point: Option<Point<f32>>,
+    #[cfg(feature = "dim3")]
+    pub ray: Option<(Point<f32>, crate::math::Vector<f32>)>,
+}
+
+pub fn track_mouse_state(
+    mut scene_mouse: ResMut<SceneMouse>,
+    windows: Query<&Window, With<PrimaryWindow>>,
+    camera: Query<(&GlobalTransform, &Camera), With<MainCamera>>,
+) {
+    if let Ok(window) = windows.get_single() {
+        for (camera_transform, camera) in camera.iter() {
+            if let Some(cursor) = window.cursor_position() {
+                let ndc_cursor = ((cursor / Vec2::new(window.width(), window.height()) * 2.0)
+                    - Vec2::ONE)
+                    * Vec2::new(1.0, -1.0);
+                let ndc_to_world =
+                    camera_transform.compute_matrix() * camera.projection_matrix().inverse();
+                let ray_pt1 =
+                    ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
+
+                #[cfg(feature = "dim2")]
+                {
+                    scene_mouse.point = Some(Point::new(ray_pt1.x, ray_pt1.y));
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    let ray_pt2 =
+                        ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
+                    let ray_dir = ray_pt2 - ray_pt1;
+                    scene_mouse.ray = Some((na::Vector3::from(ray_pt1).into(), ray_dir.into()));
+                }
+            }
+        }
+    }
+}
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 6ddc949..836fc1f 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -9,8 +9,8 @@ use bevy::prelude::*;
 use crate::debug_render::{DebugRenderPipelineResource, RapierDebugRenderPlugin};
 use crate::physics::{DeserializedPhysicsSnapshot, PhysicsEvents, PhysicsSnapshot, PhysicsState};
 use crate::plugin::TestbedPlugin;
-use crate::ui;
 use crate::{graphics::GraphicsManager, harness::RunState};
+use crate::{mouse, ui};
 
 use na::{self, Point2, Point3, Vector3};
 #[cfg(feature = "dim3")]
@@ -135,7 +135,7 @@ pub struct TestbedState {
     pub solver_type: RapierSolverType,
     pub physx_use_two_friction_directions: bool,
     pub snapshot: Option<PhysicsSnapshot>,
-    nsteps: usize,
+    pub nsteps: usize,
     camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button.
 }
 
@@ -161,6 +161,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
     camera_transform: GlobalTransform,
     camera: &'a mut OrbitCamera,
     keys: &'a ButtonInput<KeyCode>,
+    mouse: &'a SceneMouse,
 }
 
 pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -400,6 +401,7 @@ impl TestbedApp {
                     brightness: 0.3,
                     ..Default::default()
                 })
+                .init_resource::<mouse::SceneMouse>()
                 .add_plugins(DefaultPlugins.set(window_plugin))
                 .add_plugins(OrbitCameraPlugin)
                 .add_plugins(WireframePlugin)
@@ -419,7 +421,9 @@ impl TestbedApp {
                 .insert_resource(self.builders)
                 .insert_non_send_resource(self.plugins)
                 .add_systems(Update, update_testbed)
-                .add_systems(Update, egui_focus);
+                .add_systems(Update, egui_focus)
+                .add_systems(Update, track_mouse_state);
+
             init(&mut app);
             app.run();
         }
@@ -472,6 +476,15 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
     pub fn keys(&self) -> &ButtonInput<KeyCode> {
         self.keys
     }
+
+    pub fn mouse(&self) -> &SceneMouse {
+        self.mouse
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn camera_fwd_dir(&self) -> Vector<f32> {
+        (self.camera_transform * -Vec3::Z).normalize().into()
+    }
 }
 
 impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -1047,7 +1060,8 @@ fn setup_graphics_environment(mut commands: Commands) {
         .insert(OrbitCamera {
             rotate_sensitivity: 0.05,
             ..OrbitCamera::default()
-        });
+        })
+        .insert(MainCamera);
 }
 
 #[cfg(feature = "dim2")]
@@ -1078,7 +1092,8 @@ fn setup_graphics_environment(mut commands: Commands) {
             zoom: 100.0,
             pan_sensitivity: 0.02,
             ..OrbitCamera::default()
-        });
+        })
+        .insert(MainCamera);
 }
 
 fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera>) {
@@ -1091,12 +1106,14 @@ fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera>
     }
 }
 
+use crate::mouse::{track_mouse_state, MainCamera, SceneMouse};
 use bevy::window::PrimaryWindow;
 
 fn update_testbed(
     mut commands: Commands,
     windows: Query<&Window, With<PrimaryWindow>>,
     // mut pipelines: ResMut<Assets<RenderPipelineDescriptor>>,
+    mouse: Res<SceneMouse>,
     mut meshes: ResMut<Assets<Mesh>>,
     mut materials: ResMut<Assets<BevyMaterial>>,
     builders: ResMut<SceneBuilders>,
@@ -1126,6 +1143,7 @@ fn update_testbed(
             camera_transform: *cameras.single().1,
             camera: &mut cameras.single_mut().2,
             keys: &keys,
+            mouse: &mouse,
         };
 
         let mut testbed = Testbed {
@@ -1216,6 +1234,7 @@ fn update_testbed(
                 camera_transform: *cameras.single().1,
                 camera: &mut cameras.single_mut().2,
                 keys: &keys,
+                mouse: &mouse,
             };
 
             let mut testbed = Testbed {
@@ -1389,6 +1408,7 @@ fn update_testbed(
                     camera_transform: *cameras.single().1,
                     camera: &mut cameras.single_mut().2,
                     keys: &keys,
+                    mouse: &mouse,
                 };
                 harness.step_with_graphics(Some(&mut testbed_graphics));