use super::{
SecondaryDirection,
godot_math::{arc, matrix_quat, qmul, qnorm, qx, vcross, vdot, vnorm},
};
use glam::{Mat3, Quat, Vec3};
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum JointLimit {
Cone {
angle: f32,
right_axis: SecondaryDirection,
right_axis_vector: Vec3,
rotation_offset: Quat,
},
}
impl JointLimit {
pub fn solve(self, forward: Vec3, direction: Vec3) -> Vec3 {
self.solve_direction(forward, direction) * direction.length()
}
pub(crate) fn solve_direction(self, forward: Vec3, direction: Vec3) -> Vec3 {
let Self::Cone {
angle,
right_axis,
right_axis_vector,
rotation_offset,
} = self;
let len = direction.length();
if len < 1e-5 {
return vnorm(direction);
}
let y = vnorm(forward);
if y == Vec3::ZERO {
return vnorm(direction);
}
let x = vnorm(right_axis.vector(right_axis_vector));
let base = if x == Vec3::ZERO || x.dot(y).abs() > 1. - 1e-5 {
arc(Vec3::Y, y)
} else {
let z = vnorm(vcross(x, y));
let x = vnorm(vcross(y, z));
matrix_quat(Mat3::from_cols(x, y, z))
};
let offset = if rotation_offset.length_squared() > 1e-10 {
qnorm(rotation_offset)
} else {
Quat::IDENTITY
};
let space = qnorm(qmul(base, offset));
let dir = qx(space.inverse(), vnorm(direction));
let current = vcross(dir, Vec3::Y).length().atan2(vdot(dir, Vec3::Y));
let maximum = angle.clamp(0., std::f32::consts::TAU) * 0.5;
if current <= maximum {
return qx(space, dir);
}
let projection = dir - Vec3::Y * dir.y;
let axis = if projection.length_squared() > 1e-5 {
vnorm(vcross(Vec3::Y, vnorm(projection)))
} else if ((current as f64) - std::f64::consts::PI).abs()
< 1e-5 * (current as f64).abs().max(1.)
{
Vec3::NEG_Z
} else {
vnorm(vcross(Vec3::Y, dir))
};
if axis == Vec3::ZERO {
return vnorm(direction);
}
qx(
space,
vnorm(qx(Quat::from_axis_angle(axis, maximum), Vec3::Y)),
)
}
}