franklinblanco/bevy_mod_inverse_kinematics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

correct position is found for ik solving

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
src/lib.rs
View File

@ -38,6 +38,13 @@ pub struct RotationConstraint {
pub roll: Range<f32>,
}
// 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 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
.get(joints[self.chain_length - 1])?
.get(joints[self.chain_length])?
.1
.translation();
let end = transforms.get(self.target)?.1.translation();
let pole_target = transforms.get(pole_target)?.1.translation();
let target = end;
let normal = transforms.get(joints[0])?.0.translation;
let pole_target = None;
let tangent = (target - start).normalize();
let axis = (pole_target - start).cross(tangent);
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(
chain: &[Entity],
normal: Vec3,
target: Vec3,
pole_target: Option<Vec3>,
pole_target: Option<&PoleTarget>,
transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>,
) -> Result<GlobalTransform, QueryEntityError> {
if chain.len() == 1 {
@ -97,19 +126,38 @@ impl IkConstraint {
// 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);