use super::godot_math::rotation;
use glam::{Affine3A, Vec3};
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum SpringCollider {
Sphere {
radius: f32,
inside: bool,
},
Capsule {
radius: f32,
height: f32,
inside: bool,
},
Plane,
}
impl SpringCollider {
pub fn collide(self, pose: Affine3A, bone_radius: f32, current: Vec3) -> Vec3 {
let origin = Vec3::from(pose.translation);
match self {
Self::Sphere { radius, inside } => sphere(origin, radius, inside, bone_radius, current),
Self::Capsule {
radius,
height,
inside,
} => {
let r = radius.max(0.);
let offset = pose.transform_vector3(Vec3::Y * (height.max(2. * r) * 0.5 - r));
let head = origin + offset;
let tail = origin - offset;
let v = tail - head;
let dot = (current - head).dot(v);
let d = v.length_squared();
if dot <= 0. {
sphere(head, r, inside, bone_radius, current)
} else if d == 0. {
current
} else if dot >= d {
sphere(tail, r, inside, bone_radius, current)
} else {
sphere(head + v * (dot / d), r, inside, bone_radius, current)
}
}
Self::Plane => {
let n = rotation(pose) * Vec3::Y;
let distance = (current - origin).dot(n) - bone_radius;
if distance > 0. {
current
} else {
current - n * distance
}
}
}
}
}
fn sphere(origin: Vec3, radius: f32, inside: bool, bone_radius: f32, current: Vec3) -> Vec3 {
let diff = current - origin;
let length = diff.length();
let r = if inside {
(radius - bone_radius).max(0.)
} else {
(radius + bone_radius).max(0.)
};
let distance = if inside { r - length } else { length - r };
if distance > 0. {
current
} else {
origin + diff.normalize_or_zero() * r
}
}