bevy_fabrik 0.1.0

IK solver for Bevy using FABRIK algorithm
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

use bevy::prelude::*;

use crate::util::*;

// This will likely be useful more directly when Bevy supports trait queries.
pub(crate) trait Constraint {
    fn apply(&self, swing: Quat, twist: Quat) -> (Quat, Quat);
}


/// Constrains the swing of a joint rotation by a given angle in radians.
#[derive(Component, Clone, Reflect)]
pub struct SwingConstraint(pub f32);

impl Constraint for SwingConstraint {
    fn apply(&self, swing: Quat, twist: Quat) -> (Quat, Quat) {
        (Quat::constrain(swing, self.0), twist)
    }
}

/// Constrains the twist of a joint rotation by a given angle in radians.
#[derive(Component, Clone, Reflect)]
pub struct TwistConstraint(pub f32);

impl Constraint for TwistConstraint {
    fn apply(&self, swing: Quat, twist: Quat) -> (Quat, Quat) {
        (swing, Quat::constrain(twist, self.0))
    }
}

pub(crate) fn rotate_and_constrain(
    direction: Vec3,
    normal: Vec3,
    transform: GlobalTransform,
    constraints: Vec<&dyn Constraint>,
) -> GlobalTransform {
    let mut result = transform.compute_transform();

    let rotation_local = result.rotation.inverse() * Quat::from_rotation_arc(normal, direction);
    let (mut twist, mut swing) = rotation_local.decompose(Vec3::NEG_Z);

    for constraint in constraints {
        (swing, twist) = constraint.apply(swing, twist);
    }

    result.rotation = result.rotation * swing * twist;
    result.into()
}