correct position is found for ik solving

This commit is contained in:
Bram Buurlage 2022-09-16 09:12:18 +02:00
parent 124b9a2612
commit f48906853d
1 changed files with 66 additions and 18 deletions

View File

@ -38,6 +38,13 @@ pub struct RotationConstraint {
pub roll: Range<f32>, pub roll: Range<f32>,
} }
// shorter version of an affine transformation??
struct PoleTarget {
origin: Vec3,
tangent: Vec3,
normal: Vec3,
}
pub fn inverse_kinematics_solver_system( pub fn inverse_kinematics_solver_system(
query: Query<(Entity, &IkConstraint)>, query: Query<(Entity, &IkConstraint)>,
parents: Query<&Parent>, parents: Query<&Parent>,
@ -67,24 +74,46 @@ impl IkConstraint {
joints.push(parents.get(joints[i])?.get()); joints.push(parents.get(joints[i])?.get());
} }
let target = transforms.get(self.target)?.1.translation();
let normal = transforms.get(joints[0])?.0.translation;
let pole_target = if let Some(pole_target) = self.pole_target {
let start = transforms let start = transforms
.get(joints[self.chain_length - 1])? .get(joints[self.chain_length])?
.1 .1
.translation(); .translation();
let end = transforms.get(self.target)?.1.translation(); let pole_target = transforms.get(pole_target)?.1.translation();
let target = end; let tangent = (target - start).normalize();
let normal = transforms.get(joints[0])?.0.translation; let axis = (pole_target - start).cross(tangent);
let pole_target = None; let normal = tangent.cross(axis).normalize();
Self::solve_recursive(&joints[1..], normal, target, pole_target, transforms).map(drop) 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( fn solve_recursive(
chain: &[Entity], chain: &[Entity],
normal: Vec3, normal: Vec3,
target: Vec3, target: Vec3,
pole_target: Option<Vec3>, pole_target: Option<&PoleTarget>,
transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>, transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>,
) -> Result<GlobalTransform, QueryEntityError> { ) -> Result<GlobalTransform, QueryEntityError> {
if chain.len() == 1 { if chain.len() == 1 {
@ -97,19 +126,38 @@ impl IkConstraint {
// 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. // target.
let rotation = Quat::from_rotation_arc( let mut from_position = global_transform.translation();
normal.normalize(), if let Some(pole_target) = pole_target {
// todo: constrain our own position to the pole plane maybe?? let on_pole = pole_target.origin
(target - global_transform.translation()).normalize(), + (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); let translation = target - rotation.mul_vec3(normal);
// recurse to target the parent towards the current translation // recurse to target the parent towards the current translation
let parent_global_transform = let parent_global_transform = Self::solve_recursive(
Self::solve_recursive(&chain[1..], parent_normal, translation, pole_target, transforms)?; &chain[1..],
parent_normal,
translation,
pole_target,
transforms,
)?;
// apply constraints on the way back from recursing // apply constraints on the way back from recursing
let (mut transform, mut global_transform) = transforms.get_mut(chain[0]).unwrap(); 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 = transform.rotation =
Quat::from_affine3(&parent_global_transform.affine().inverse()) * rotation; Quat::from_affine3(&parent_global_transform.affine().inverse()) * rotation;
*global_transform = parent_global_transform.mul_transform(*transform); *global_transform = parent_global_transform.mul_transform(*transform);