implement pole targetting
This commit is contained in:
parent
f48906853d
commit
d38a17e812
|
@ -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"] }
|
||||||
|
|
98
src/lib.rs
98
src/lib.rs
|
@ -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),
|
||||||
|
|
Loading…
Reference in New Issue