Added skin_mesh example, made debug_lines optional

Cargo.toml

@@ -9,4 +9,7 @@ license = "MIT OR Apache-2.0"
 
 [dependencies]
 bevy = "0.8.1"
-bevy_prototype_debug_lines = { version = "0.8.1", features = ["3d"] }
+bevy_prototype_debug_lines = { version = "0.8.1", features = ["3d"], optional = true }
+
+[features]
+debug_lines = ["bevy_prototype_debug_lines"]

examples/skin_mesh.rs

@@ -0,0 +1,217 @@
+use bevy::prelude::*;
+use bevy_mod_inverse_kinematics::*;
+
+#[derive(Component)]
+pub struct ManuallyTarget(Vec4);
+
+fn main() {
+    App::new()
+        .insert_resource(WindowDescriptor {
+            width: 800.0,
+            height: 600.0,
+            ..default()
+        })
+        .add_plugins(DefaultPlugins)
+        .add_plugin(InverseKinematicsPlugin)
+        .add_startup_system(setup)
+        .add_system(setup_ik)
+        .add_system(manually_target)
+        .run();
+}
+
+fn setup(
+    mut commands: Commands,
+    assets: Res<AssetServer>,
+    mut meshes: ResMut<Assets<Mesh>>,
+    mut materials: ResMut<Assets<StandardMaterial>>,
+) {
+    commands
+        .spawn_bundle(SpatialBundle::default())
+        .with_children(|parent| {
+            parent.spawn_bundle(Camera3dBundle {
+                transform: Transform::from_xyz(-0.5, 1.5, 2.5)
+                    .looking_at(Vec3::new(0.0, 1.0, 0.0), Vec3::Y),
+                projection: bevy::render::camera::Projection::Perspective(PerspectiveProjection {
+                    fov: std::f32::consts::FRAC_PI_4,
+                    aspect_ratio: 1.0,
+                    near: 0.1,
+                    far: 100.0,
+                }),
+                ..default()
+            });
+        });
+
+    let size = 30.0;
+    commands.spawn_bundle(DirectionalLightBundle {
+        directional_light: DirectionalLight {
+            color: Color::WHITE,
+            illuminance: 10000.0,
+            shadows_enabled: true,
+            shadow_projection: OrthographicProjection {
+                left: -size,
+                right: size,
+                bottom: -size,
+                top: size,
+                near: -size,
+                far: size,
+                ..default()
+            },
+            ..default()
+        },
+        transform: Transform::from_xyz(-8.0, 8.0, 8.0).looking_at(Vec3::ZERO, Vec3::Y),
+        ..default()
+    });
+
+    commands.spawn_bundle(PbrBundle {
+        mesh: meshes.add(Mesh::from(shape::Plane { size: 5.0 })),
+        material: materials.add(StandardMaterial {
+            base_color: Color::WHITE,
+            ..default()
+        }),
+        ..default()
+    });
+
+    commands.spawn_bundle(SceneBundle {
+        scene: assets.load("skin.gltf#Scene0"),
+        transform: Transform::from_xyz(0.0, 0.0, 0.0),
+        ..default()
+    });
+}
+
+fn setup_ik(
+    mut commands: Commands,
+    mut meshes: ResMut<Assets<Mesh>>,
+    mut materials: ResMut<Assets<StandardMaterial>>,
+    added_query: Query<(Entity, &Parent), Added<AnimationPlayer>>,
+    children: Query<&Children>,
+    names: Query<&Name>,
+) {
+    // Use the presence of `AnimationPlayer` to determine the root entity of the skeleton.
+    for (entity, _parent) in added_query.iter() {
+        // Try to get the entity for the right hand joint.
+        let right_hand = find_entity(
+            &EntityPath {
+                parts: vec![
+                    "Pelvis".into(),
+                    "Spine1".into(),
+                    "Spine2".into(),
+                    "Collar.R".into(),
+                    "UpperArm.R".into(),
+                    "ForeArm.R".into(),
+                    "Hand.R".into(),
+                ],
+            },
+            entity,
+            &children,
+            &names,
+        )
+        .unwrap();
+
+        let target = commands
+            .spawn_bundle(PbrBundle {
+                transform: Transform::from_xyz(0.3, 0.8, 0.2),
+                mesh: meshes.add(Mesh::from(shape::Icosphere {
+                    radius: 0.05,
+                    subdivisions: 1,
+                })),
+                material: materials.add(StandardMaterial {
+                    base_color: Color::RED,
+                    ..default()
+                }),
+                ..default()
+            })
+            .insert(ManuallyTarget(Vec4::new(0.0, 0.0, 1.0, 0.3)))
+            .id();
+
+        let pole_target = commands
+            .spawn_bundle(PbrBundle {
+                transform: Transform::from_xyz(-1.0, 0.4, -0.2),
+                mesh: meshes.add(Mesh::from(shape::Icosphere {
+                    radius: 0.05,
+                    subdivisions: 1,
+                })),
+                material: materials.add(StandardMaterial {
+                    base_color: Color::GREEN,
+                    ..default()
+                }),
+                ..default()
+            })
+            .id();
+
+        // Add an IK constraint to the right hand, using the targets that were created earlier.
+        commands.entity(right_hand).insert(IkConstraint {
+            chain_length: 2,
+            iterations: 20,
+            target,
+            pole_target: Some(pole_target),
+            pole_angle: -std::f32::consts::FRAC_PI_2,
+        });
+    }
+}
+
+fn find_entity(
+    path: &EntityPath,
+    root: Entity,
+    children: &Query<&Children>,
+    names: &Query<&Name>,
+) -> Result<Entity, ()> {
+    let mut current_entity = root;
+
+    for part in path.parts.iter() {
+        let mut found = false;
+        if let Ok(children) = children.get(current_entity) {
+            for child in children.iter() {
+                if let Ok(name) = names.get(*child) {
+                    if name == part {
+                        // Found a children with the right name, continue to the next part
+                        current_entity = *child;
+                        found = true;
+                        break;
+                    }
+                }
+            }
+        }
+        if !found {
+            warn!("Entity not found for path {:?} on part {:?}", path, part);
+            return Err(());
+        }
+    }
+
+    Ok(current_entity)
+}
+
+fn manually_target(
+    camera_query: Query<(&Camera, &GlobalTransform)>,
+    mut target_query: Query<(&ManuallyTarget, &mut Transform)>,
+    mut cursor: EventReader<CursorMoved>,
+) {
+    let (camera, transform) = camera_query.single();
+
+    if let Some(event) = cursor.iter().last() {
+        let view = transform.compute_matrix();
+        let (viewport_min, viewport_max) = camera.logical_viewport_rect().unwrap();
+        let screen_size = camera.logical_target_size().unwrap();
+        let viewport_size = viewport_max - viewport_min;
+        let adj_cursor_pos =
+            event.position - Vec2::new(viewport_min.x, screen_size.y - viewport_max.y);
+
+        let projection = camera.projection_matrix();
+        let far_ndc = projection.project_point3(Vec3::NEG_Z).z;
+        let near_ndc = projection.project_point3(Vec3::Z).z;
+        let cursor_ndc = (adj_cursor_pos / viewport_size) * 2.0 - Vec2::ONE;
+        let ndc_to_world: Mat4 = view * projection.inverse();
+        let near = ndc_to_world.project_point3(cursor_ndc.extend(near_ndc));
+        let far = ndc_to_world.project_point3(cursor_ndc.extend(far_ndc));
+        let ray_direction = far - near;
+
+        for (&ManuallyTarget(plane), mut transform) in target_query.iter_mut() {
+            let normal = plane.truncate();
+            let d = plane.w;
+            let denom = normal.dot(ray_direction);
+            if denom.abs() > 0.0001 {
+                let t = (normal * d - near).dot(normal) / denom;
+                transform.translation = near + ray_direction * t;
+            }
+        }
+    }
+}

src/lib.rs

@@ -1,7 +1,8 @@
 use bevy::ecs::query::QueryEntityError;
 use bevy::prelude::*;
 use bevy::transform::TransformSystem;
-use bevy_prototype_debug_lines::{DebugLinesPlugin, DebugLines};
+#[cfg(feature = "debug_lines")]
+use bevy_prototype_debug_lines::{DebugLines, DebugLinesPlugin};
 
 pub struct InverseKinematicsPlugin;
 
@@ -32,10 +33,16 @@ pub fn inverse_kinematics_solver_system(
     query: Query<(Entity, &IkConstraint)>,
     parents: Query<&Parent>,
     mut transforms: Query<(&mut Transform, &mut GlobalTransform)>,
-    mut debug_lines: ResMut<DebugLines>,
+    #[cfg(feature = "debug_lines")] mut debug_lines: ResMut<DebugLines>,
 ) {
     for (entity, constraint) in query.iter() {
-        if let Err(e) = constraint.solve(entity, &parents, &mut transforms, &mut debug_lines) {
+        if let Err(e) = constraint.solve(
+            entity,
+            &parents,
+            &mut transforms,
+            #[cfg(feature = "debug_lines")]
+            &mut debug_lines,
+        ) {
             bevy::log::warn!("Failed to solve IK constraint: {e}");
         }
     }
@@ -47,7 +54,7 @@ impl IkConstraint {
         entity: Entity,
         parents: &Query<&Parent>,
         transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>,
-        debug_lines: &mut DebugLines,
+        #[cfg(feature = "debug_lines")] debug_lines: &mut DebugLines,
     ) -> Result<(), QueryEntityError> {
         if self.chain_length == 0 {
             return Ok(());
@@ -87,6 +94,7 @@ impl IkConstraint {
                 target,
                 pole_target.as_ref(),
                 transforms,
+                #[cfg(feature = "debug_lines")]
                 debug_lines,
             )?;
 
@@ -104,7 +112,7 @@ impl IkConstraint {
         target: Vec3,
         pole_target: Option<&PoleTarget>,
         transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>,
-        debug_lines: &mut DebugLines,
+        #[cfg(feature = "debug_lines")] debug_lines: &mut DebugLines,
     ) -> Result<GlobalTransform, QueryEntityError> {
         let (&transform, &global_transform) = transforms.get(chain[0])?;
         if chain.len() == 1 {
@@ -121,10 +129,7 @@ impl IkConstraint {
             let distance = on_pole.distance(global_transform.translation());
             let from_position = on_pole + pt.normal * distance;
 
-            let base = Quat::from_rotation_arc(
+            let base = Quat::from_rotation_arc(normal.normalize(), Vec3::Z);
-                normal.normalize(),
-                Vec3::Z,
-            );
 
             let forward = (target - from_position).normalize();
             let up = forward.cross(pt.normal).normalize();
@@ -136,14 +141,19 @@ impl IkConstraint {
             Quat::from_rotation_arc(
                 normal.normalize(),
                 (target - global_transform.translation()).normalize(),
-            ).normalize()
+            )
+            .normalize()
         };
         let translation = target - rotation.mul_vec3(normal);
 
-        debug_lines.line_colored(translation, translation+rotation.mul_vec3(Vec3::X * 0.1), 0.0, Color::RED);
+        #[cfg(feature = "debug_lines")]
-        debug_lines.line_colored(translation, translation+rotation.mul_vec3(Vec3::Y * 0.1), 0.0, Color::GREEN);
+        {
-        debug_lines.line_colored(translation, translation+rotation.mul_vec3(Vec3::Z * 0.1), 0.0, Color::BLUE);
+            let p = translation;
-        debug_lines.line_colored(translation, translation+rotation.mul_vec3(normal), 0.0, Color::YELLOW);
+            debug_lines.line_colored(p, p + rotation.mul_vec3(Vec3::X * 0.1), 0.0, Color::RED);
+            debug_lines.line_colored(p, p + rotation.mul_vec3(Vec3::Y * 0.1), 0.0, Color::GREEN);
+            debug_lines.line_colored(p, p + rotation.mul_vec3(Vec3::Z * 0.1), 0.0, Color::BLUE);
+            debug_lines.line_colored(p, p + rotation.mul_vec3(normal), 0.0, Color::YELLOW);
+        }
 
         // recurse to target the parent towards the current translation
         let parent_global_transform = Self::solve_recursive(
@@ -152,17 +162,25 @@ impl IkConstraint {
             translation,
             pole_target,
             transforms,
+            #[cfg(feature = "debug_lines")]
             debug_lines,
         )?;
 
         // apply constraints on the way back from recursing
         let (mut transform, mut global_transform) = transforms.get_mut(chain[0]).unwrap();
-        transform.rotation =
+        transform.rotation = Quat::from_affine3(&parent_global_transform.affine())
-            Quat::from_affine3(&parent_global_transform.affine()).inverse().normalize() * rotation;
+            .inverse()
+            .normalize()
+            * rotation;
         *global_transform = parent_global_transform.mul_transform(*transform);
 
-        debug_lines.line_gradient(global_transform.translation(), global_transform.translation() + rotation.mul_vec3(normal), 0.0, Color::VIOLET, Color::BLACK);
+        #[cfg(feature = "debug_lines")]
-        debug_lines.line_gradient(global_transform.translation(), global_transform.mul_vec3(normal), 0.0, Color::BLACK, Color::CYAN);
+        debug_lines.line_colored(
+            global_transform.translation(),
+            global_transform.mul_vec3(normal),
+            0.0,
+            Color::CYAN,
+        );
 
         Ok(*global_transform)
     }
@@ -170,6 +188,7 @@ impl IkConstraint {
 
 impl Plugin for InverseKinematicsPlugin {
     fn build(&self, app: &mut App) {
+        #[cfg(feature = "debug_lines")]
         app.add_plugin(DebugLinesPlugin::default());
 
         // app.add_system_to_stage(