implement pole targetting

This commit is contained in:
Bram Buurlage 2022-09-17 18:29:21 +02:00
parent f48906853d
commit d38a17e812
2 changed files with 52 additions and 47 deletions

View File

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

View File

@ -1,8 +1,7 @@
use std::ops::Range;
use bevy::ecs::query::QueryEntityError; use bevy::ecs::query::QueryEntityError;
use bevy::prelude::*; use bevy::prelude::*;
use bevy::transform::TransformSystem; use bevy::transform::TransformSystem;
use bevy_prototype_debug_lines::{DebugLinesPlugin, DebugLines};
pub struct InverseKinematicsPlugin; pub struct InverseKinematicsPlugin;
@ -22,36 +21,21 @@ pub struct IkConstraint {
pub pole_angle: f32, pub pole_angle: f32,
} }
#[derive(Component)]
pub struct CopyLocationConstraint {
/// Target entity. The target must have a `Transform` and `GlobalTransform`.
pub target: Entity,
}
#[derive(Component)]
pub struct RotationConstraint {
/// The allowable range for yaw rotation.
pub yaw: Range<f32>,
/// The allowable range for pitch rotation.
pub pitch: Range<f32>,
/// The allowable range for roll rotation.
pub roll: Range<f32>,
}
// shorter version of an affine transformation??
struct PoleTarget { struct PoleTarget {
origin: Vec3, origin: Vec3,
tangent: Vec3, tangent: Vec3,
normal: Vec3, normal: Vec3,
angle: f32,
} }
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>,
mut transforms: Query<(&mut Transform, &mut GlobalTransform)>, mut transforms: Query<(&mut Transform, &mut GlobalTransform)>,
mut debug_lines: ResMut<DebugLines>,
) { ) {
for (entity, constraint) in query.iter() { for (entity, constraint) in query.iter() {
if let Err(e) = constraint.solve(entity, &parents, &mut transforms) { if let Err(e) = constraint.solve(entity, &parents, &mut transforms, &mut debug_lines) {
bevy::log::warn!("Failed to solve IK constraint: {e}"); bevy::log::warn!("Failed to solve IK constraint: {e}");
} }
} }
@ -63,6 +47,7 @@ impl IkConstraint {
entity: Entity, entity: Entity,
parents: &Query<&Parent>, parents: &Query<&Parent>,
transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>, transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>,
debug_lines: &mut DebugLines,
) -> Result<(), QueryEntityError> { ) -> Result<(), QueryEntityError> {
if self.chain_length == 0 { if self.chain_length == 0 {
return Ok(()); return Ok(());
@ -78,10 +63,7 @@ impl IkConstraint {
let normal = transforms.get(joints[0])?.0.translation; let normal = transforms.get(joints[0])?.0.translation;
let pole_target = if let Some(pole_target) = self.pole_target { let pole_target = if let Some(pole_target) = self.pole_target {
let start = transforms let start = transforms.get(joints[self.chain_length])?.1.translation();
.get(joints[self.chain_length])?
.1
.translation();
let pole_target = transforms.get(pole_target)?.1.translation(); let pole_target = transforms.get(pole_target)?.1.translation();
let tangent = (target - start).normalize(); let tangent = (target - start).normalize();
@ -92,20 +74,27 @@ impl IkConstraint {
origin: start, origin: start,
tangent, tangent,
normal, normal,
angle: self.pole_angle,
}) })
} else { } else {
None None
}; };
for _ in 0..self.iterations { for _ in 0..self.iterations {
Self::solve_recursive( let result = Self::solve_recursive(
&joints[1..], &joints[1..],
normal, normal,
target, target,
pole_target.as_ref(), pole_target.as_ref(),
transforms, transforms,
debug_lines,
)?; )?;
if result.mul_vec3(normal).distance_squared(target) < 0.001 {
return Ok(());
} }
}
Ok(()) Ok(())
} }
@ -115,28 +104,47 @@ impl IkConstraint {
target: Vec3, target: Vec3,
pole_target: Option<&PoleTarget>, pole_target: Option<&PoleTarget>,
transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>, transforms: &mut Query<(&mut Transform, &mut GlobalTransform)>,
debug_lines: &mut DebugLines,
) -> Result<GlobalTransform, QueryEntityError> { ) -> Result<GlobalTransform, QueryEntityError> {
let (&transform, &global_transform) = transforms.get(chain[0])?;
if chain.len() == 1 { if chain.len() == 1 {
let (_, &global_transform) = transforms.get(chain[0])?;
return Ok(global_transform); return Ok(global_transform);
} }
let (&transform, &global_transform) = transforms.get(chain[0])?;
let parent_normal = transform.translation; 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. // target.
let mut from_position = global_transform.translation(); let rotation = if let Some(pt) = pole_target {
if let Some(pole_target) = pole_target { let on_pole = pt.origin
let on_pole = pole_target.origin + (global_transform.translation() - pt.origin).project_onto_normalized(pt.tangent);
+ (from_position - pole_target.origin).project_onto_normalized(pole_target.tangent); let distance = on_pole.distance(global_transform.translation());
let distance = from_position.distance(on_pole); let from_position = on_pole + pt.normal * distance;
from_position = on_pole + pole_target.normal * distance;
} let base = Quat::from_rotation_arc(
let rotation = normal.normalize(),
Quat::from_rotation_arc(normal.normalize(), (target - from_position).normalize()); Vec3::Z,
);
let forward = (target - from_position).normalize();
let up = forward.cross(pt.normal);
let right = up.cross(forward);
let orientation = Mat3::from_cols(right, up, forward) * Mat3::from_rotation_z(pt.angle);
(Quat::from_mat3(&orientation) * base).normalize()
} else {
Quat::from_rotation_arc(
normal.normalize(),
(target - global_transform.translation()).normalize(),
).normalize()
};
let translation = target - rotation.mul_vec3(normal); let translation = target - rotation.mul_vec3(normal);
debug_lines.line_colored(translation, translation+rotation.mul_vec3(Vec3::X * 0.1), 0.0, Color::RED);
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);
debug_lines.line_colored(translation, translation+rotation.mul_vec3(normal), 0.0, Color::YELLOW);
// recurse to target the parent towards the current translation // recurse to target the parent towards the current translation
let parent_global_transform = Self::solve_recursive( let parent_global_transform = Self::solve_recursive(
&chain[1..], &chain[1..],
@ -144,30 +152,26 @@ impl IkConstraint {
translation, translation,
pole_target, pole_target,
transforms, transforms,
debug_lines,
)?; )?;
// 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().normalize() * rotation;
*global_transform = parent_global_transform.mul_transform(*transform); *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);
debug_lines.line_gradient(global_transform.translation(), global_transform.mul_vec3(normal), 0.0, Color::BLACK, Color::CYAN);
Ok(*global_transform) Ok(*global_transform)
} }
} }
impl Plugin for InverseKinematicsPlugin { impl Plugin for InverseKinematicsPlugin {
fn build(&self, app: &mut App) { fn build(&self, app: &mut App) {
app.add_plugin(DebugLinesPlugin::default());
app.add_system_to_stage( app.add_system_to_stage(
CoreStage::PostUpdate, CoreStage::PostUpdate,
inverse_kinematics_solver_system.after(TransformSystem::TransformPropagate), inverse_kinematics_solver_system.after(TransformSystem::TransformPropagate),