diff --git a/src/lib.rs b/src/lib.rs index e57e0a1..6f21a43 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -38,6 +38,13 @@ pub struct RotationConstraint { pub roll: Range, } +// shorter version of an affine transformation?? +struct PoleTarget { + origin: Vec3, + tangent: Vec3, + normal: Vec3, +} + pub fn inverse_kinematics_solver_system( query: Query<(Entity, &IkConstraint)>, parents: Query<&Parent>, @@ -67,24 +74,46 @@ impl IkConstraint { joints.push(parents.get(joints[i])?.get()); } - let start = transforms - .get(joints[self.chain_length - 1])? - .1 - .translation(); - let end = transforms.get(self.target)?.1.translation(); - - let target = end; + let target = transforms.get(self.target)?.1.translation(); let normal = transforms.get(joints[0])?.0.translation; - let pole_target = None; - Self::solve_recursive(&joints[1..], normal, target, pole_target, transforms).map(drop) + let pole_target = if let Some(pole_target) = self.pole_target { + let start = transforms + .get(joints[self.chain_length])? + .1 + .translation(); + let pole_target = transforms.get(pole_target)?.1.translation(); + + let tangent = (target - start).normalize(); + let axis = (pole_target - start).cross(tangent); + let normal = tangent.cross(axis).normalize(); + + Some(PoleTarget { + origin: start, + tangent, + normal, + }) + } else { + None + }; + + for _ in 0..self.iterations { + Self::solve_recursive( + &joints[1..], + normal, + target, + pole_target.as_ref(), + transforms, + )?; + } + Ok(()) } fn solve_recursive( chain: &[Entity], normal: Vec3, target: Vec3, - pole_target: Option, + pole_target: Option<&PoleTarget>, transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>, ) -> Result { if chain.len() == 1 { @@ -95,21 +124,40 @@ impl IkConstraint { let (&transform, &global_transform) = transforms.get(chain[0])?; let parent_normal = transform.translation; - // determine absolute rotation and translation for this bone where the tail touches the + // determine absolute rotation and translation for this bone where the tail touches the // target. - let rotation = Quat::from_rotation_arc( - normal.normalize(), - // todo: constrain our own position to the pole plane maybe?? - (target - global_transform.translation()).normalize(), - ); + let mut from_position = global_transform.translation(); + if let Some(pole_target) = pole_target { + let on_pole = pole_target.origin + + (from_position - pole_target.origin).project_onto_normalized(pole_target.tangent); + let distance = from_position.distance(on_pole); + from_position = on_pole + pole_target.normal * distance; + } + let rotation = + Quat::from_rotation_arc(normal.normalize(), (target - from_position).normalize()); let translation = target - rotation.mul_vec3(normal); // recurse to target the parent towards the current translation - let parent_global_transform = - Self::solve_recursive(&chain[1..], parent_normal, translation, pole_target, transforms)?; + let parent_global_transform = Self::solve_recursive( + &chain[1..], + parent_normal, + translation, + pole_target, + transforms, + )?; // apply constraints on the way back from recursing let (mut transform, mut global_transform) = transforms.get_mut(chain[0]).unwrap(); + // if let Some(pole_target) = pole_target { + // let angle = -rotation.mul_vec3(normal).angle_between(pole_target.x_axis); + // let target = Quat::from_axis_angle(pole_target.z_axis, angle).mul_vec3(pole_target.x_axis); + + // rotation = Quat::from_rotation_arc( + // normal.normalize(), + // (target - global_transform.translation()).normalize(), + // ); + // } + transform.rotation = Quat::from_affine3(&parent_global_transform.affine().inverse()) * rotation; *global_transform = parent_global_transform.mul_transform(*transform);