use super::{ChainIndices, SkeletonView, godot_math::*};
use glam::{Affine3A, Quat, Vec3};
#[derive(Clone, Debug, PartialEq)]
pub struct BakedPath {
pub points: Vec<Vec3>,
pub tilts: Vec<f32>,
pub interval: f32,
}
impl BakedPath {
#[cfg(feature = "bevy")]
pub fn from_curve(curve: &impl bevy_math::curve::Curve<Vec3>, interval: f32) -> Self {
let domain = curve.domain();
let mut result = Self {
points: Vec::new(),
tilts: Vec::new(),
interval,
};
let start = domain.start();
let end = domain.end();
if !start.is_finite()
|| !end.is_finite()
|| !interval.is_finite()
|| interval <= 0.
|| end <= start
{
return result;
}
let Some(mut previous) = curve.sample(start) else {
return result;
};
if !previous.is_finite() {
return result;
}
result.points.push(previous);
let mut remaining = interval;
for i in 1..=16384 {
let Some(point) = curve.sample(start + (end - start) * (i as f32 / 16384.)) else {
continue;
};
if !point.is_finite() {
return Self {
points: Vec::new(),
tilts: Vec::new(),
interval,
};
}
let mut segment = point - previous;
let mut length = segment.length();
while length >= remaining {
let next = previous + segment * (remaining / length);
result.points.push(next);
previous = next;
segment = point - previous;
length = segment.length();
remaining = interval;
}
remaining -= length;
previous = point;
}
if result
.points
.last()
.is_some_and(|p| p.distance(previous) > 1e-6)
{
result.points.push(previous)
}
result.tilts.resize(result.points.len(), 0.);
result
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct SplineSettings {
pub tilt_enabled: bool,
pub tilt_fade_in: i32,
pub tilt_fade_out: i32,
}
impl Default for SplineSettings {
fn default() -> Self {
Self {
tilt_enabled: true,
tilt_fade_in: 1,
tilt_fade_out: 1,
}
}
}
fn nearest(origin: Vec3, length_sq: f64, points: &[Vec3], first: usize) -> (usize, f64) {
let mut i = first.min(points.len() - 1);
while i < points.len() {
if origin.distance_squared(points[i]) as f64 >= length_sq {
break;
}
i += 1;
}
if i == 0 {
(first, 0.)
} else if i >= points.len() {
(i, 1.)
} else {
let a = origin.distance_squared(points[i - 1]) as f64;
let b = origin.distance_squared(points[i]) as f64;
(
i - 1,
if b == a {
0.
} else {
(length_sq - a) / (b - a)
},
)
}
}
pub fn process_spline_ik(
sk: &mut SkeletonView<'_>,
chain: &ChainIndices,
path: &BakedPath,
path_to_skeleton: Affine3A,
settings: &SplineSettings,
) {
if sk.validate().is_err()
|| chain.len() < 2
|| path.points.is_empty()
|| path.tilts.len() != path.points.len()
|| path.points.iter().any(|p| !p.is_finite())
|| path.tilts.iter().any(|t| !t.is_finite())
|| chain
.as_slice()
.iter()
.any(|&i| i as usize >= sk.local.len())
{
return;
}
let n = chain.len();
let mut points = [Vec3::ZERO; 32];
let mut forward = [Vec3::ZERO; 32];
let mut lengths = [0.; 32];
let mut accum = [0_f64; 32];
let mut twists = [0_f64; 32];
let mut total = 0.;
for (i, &bone) in chain.as_slice().iter().enumerate() {
points[i] = Vec3::from(sk.skeleton_pose(bone).translation);
if i + 1 < n {
let axis = sk.local[chain.as_slice()[i + 1] as usize].translation;
forward[i] = axis.normalize_or_zero();
lengths[i] = axis.length();
total += lengths[i] as f64;
}
accum[i] = total;
}
let start_point = path_to_skeleton.transform_point3(path.points[0]);
let start_vector = start_point - points[0];
let start_dist = start_vector.length() as f64;
let chain_start = accum[..n]
.iter()
.position(|&l| l >= start_dist)
.unwrap_or(n - 1);
let denom_start = if settings.tilt_fade_in > 0 {
(settings.tilt_fade_in - 1).clamp(chain_start as i32, n as i32)
} else {
-1
};
let mut fade_in = 0.;
for i in ((denom_start - settings.tilt_fade_in + 1).max(0)..=denom_start).rev() {
fade_in += lengths.get(i as usize).copied().unwrap_or(0.) as f64;
}
let point_last = path.points.len() - 1;
let end_point = path_to_skeleton.transform_point3(path.points[point_last]);
let mut last_nearest = 0;
let mut last_next = 0;
let mut last_t = 0.;
let mut ended = 0;
let mut end_vector = Vec3::ZERO;
let mut end_length = 0.;
let mut fade_out = 0.;
let mut i = 0;
while i < n - 1 {
let len = lengths[i];
if len < 1e-5 {
i += 1;
continue;
}
let fitting = i == chain_start;
if ended > 0 {
points[i + 1] = limit_length(points[i], points[i] + end_vector, len);
if settings.tilt_enabled {
twists[i] = if settings.tilt_fade_out < 0 {
0.
} else if settings.tilt_fade_out == 0 {
path.tilts[point_last] as f64
} else {
if ended == 1 {
end_length = points[i + 1].distance(end_point) as f64
} else {
end_length += len as f64
}
let damping = if fade_out == 0. {
1.
} else {
(end_length / fade_out).clamp(0., 1.)
};
let initial = if ended == 1 {
path.tilts[last_nearest] as f64
+ (path.tilts[last_next] as f64 - path.tilts[last_nearest] as f64)
* last_t
} else {
path.tilts[point_last] as f64
};
ended = 2;
initial * (1. - damping)
};
}
i += 1;
continue;
}
if path.points.len() == 1 || i <= chain_start {
let update = !fitting || path.points.len() == 1;
if update {
points[i + 1] = limit_length(points[i], points[i] + start_vector, len)
}
if settings.tilt_enabled {
twists[i] = if settings.tilt_fade_in < 0 {
0.
} else if settings.tilt_fade_in == 0 {
path.tilts[0] as f64
} else {
let damping = if fade_in == 0. {
1.
} else {
(points[i].distance(start_point) as f64 / fade_in).clamp(0., 1.)
};
path.tilts[0] as f64 * (1. - damping)
};
}
if update {
i += 1;
continue;
}
}
let (mut sample, mut t) = nearest(
path_to_skeleton.inverse().transform_point3(points[i]),
(len * len) as f64,
&path.points,
last_nearest,
);
if sample >= path.points.len() {
if i == 0 {
sample = path.points.len().saturating_sub(2);
t = 1.;
} else {
let chain_end = (points[i] - points[i - 1]).normalize_or_zero();
let path_end = (end_point - points[i]).normalize_or_zero();
let mut distance_to_last = 0_f64;
let mut path_length = 0_f64;
for j in 1..path.points.len() {
let d = path.points[j].distance(path.points[j - 1]) as f64;
path_length += d;
if j <= last_nearest {
distance_to_last += d;
}
}
let next_dist = if last_next > last_nearest {
path.points[last_next].distance(path.points[last_nearest]) as f64
} else {
0.
};
let rest_length = path_length - distance_to_last - next_dist * last_t;
let t = (rest_length / len as f64).clamp(0., 1.) as f32;
end_vector = chain_end.lerp(path_end, t);
if settings.tilt_fade_out > 0 {
let start =
(n as i32 - 1 - settings.tilt_fade_out).clamp(0, last_nearest as i32);
for e in start..start + settings.tilt_fade_out {
fade_out += lengths.get(e as usize).copied().unwrap_or(0.) as f64;
}
}
ended = 1;
continue;
}
}
let next = sample.min(point_last);
points[i + 1] = limit_length(
points[i],
path_to_skeleton
.transform_point3(path.points[sample].lerp(path.points[next], t as f32)),
len,
);
if !fitting {
twists[i] = path.tilts[last_nearest] as f64
+ (path.tilts[last_next] as f64 - path.tilts[last_nearest] as f64) * last_t;
}
last_nearest = sample;
last_next = next;
last_t = t;
i += 1;
}
let mut parent = sk.parents[chain.as_slice()[0] as usize]
.map_or(Quat::IDENTITY, |p| rotation(sk.skeleton_pose(p)));
let mut parent_twist = 0.;
for i in 0..n - 1 {
if lengths[i] < 1e-5 {
continue;
}
let bone = chain.as_slice()[i];
let input = rotation(sk.local[bone as usize].affine());
let grest = (parent * input).normalize();
let to =
(grest.inverse() * (points[i + 1] - points[i]).normalize_or_zero()).normalize_or_zero();
let mut q = swing(arc(forward[i], to), forward[i]);
if settings.tilt_enabled {
q *= Quat::from_axis_angle(forward[i], (twists[i] - parent_twist) as f32);
parent_twist = twists[i];
}
let local = (input * q).normalize();
if local.is_finite() {
sk.local[bone as usize].rotation = local;
parent = (parent * local).normalize();
}
}
}