correct position is found for ik solving
This commit is contained in:
parent
124b9a2612
commit
f48906853d
76
src/lib.rs
76
src/lib.rs
|
@ -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);
|
||||||
|
|
Loading…
Reference in New Issue