use crate::scene::{Quat, Vec3};
use super::AnimationInterpolation;
pub(super) fn sample_vec3(
times: &[f32],
values: &[Vec3],
interpolation: AnimationInterpolation,
time_seconds: f32,
) -> Option<Vec3> {
if times.is_empty() || values.is_empty() {
return None;
}
if interpolation == AnimationInterpolation::CubicSpline {
return sample_cubic_vec3(times, values, time_seconds);
}
if time_seconds <= times[0] {
return values.first().copied();
}
if time_seconds >= *times.last()? {
return values.last().copied();
}
for index in 0..times.len().saturating_sub(1) {
let start = times[index];
let end = times[index + 1];
if time_seconds <= end {
let left = *values.get(index)?;
let right = *values.get(index + 1)?;
return Some(match interpolation {
AnimationInterpolation::Step => left,
AnimationInterpolation::Linear => {
let amount = ((time_seconds - start) / (end - start)).clamp(0.0, 1.0);
lerp_vec3(left, right, amount)
}
AnimationInterpolation::CubicSpline => unreachable!("handled before loop"),
});
}
}
values.last().copied()
}
pub(super) fn sample_quat(
times: &[f32],
values: &[Quat],
interpolation: AnimationInterpolation,
time_seconds: f32,
) -> Option<Quat> {
if times.is_empty() || values.is_empty() {
return None;
}
if interpolation == AnimationInterpolation::CubicSpline {
return sample_cubic_quat(times, values, time_seconds);
}
if time_seconds <= times[0] {
return values.first().copied().map(normalize_quat);
}
if time_seconds >= *times.last()? {
return values.last().copied().map(normalize_quat);
}
for index in 0..times.len().saturating_sub(1) {
let start = times[index];
let end = times[index + 1];
if time_seconds <= end {
let left = normalize_quat(*values.get(index)?);
let right = normalize_quat(*values.get(index + 1)?);
return Some(match interpolation {
AnimationInterpolation::Step => left,
AnimationInterpolation::Linear => {
let amount = ((time_seconds - start) / (end - start)).clamp(0.0, 1.0);
slerp_quat(left, right, amount)
}
AnimationInterpolation::CubicSpline => unreachable!("handled before loop"),
});
}
}
values.last().copied().map(normalize_quat)
}
pub(super) fn sample_weights(
times: &[f32],
values: &[Vec<f32>],
interpolation: AnimationInterpolation,
time_seconds: f32,
) -> Option<Vec<f32>> {
if times.is_empty() || values.is_empty() {
return None;
}
if interpolation == AnimationInterpolation::CubicSpline {
return sample_cubic_weights(times, values, time_seconds);
}
if time_seconds <= times[0] {
return values.first().cloned();
}
if time_seconds >= *times.last()? {
return values.last().cloned();
}
for index in 0..times.len().saturating_sub(1) {
let start = times[index];
let end = times[index + 1];
if time_seconds <= end {
let left = values.get(index)?;
let right = values.get(index + 1)?;
if interpolation == AnimationInterpolation::Step {
return Some(left.clone());
}
let amount = ((time_seconds - start) / (end - start)).clamp(0.0, 1.0);
return Some(
left.iter()
.zip(right)
.map(|(left, right)| left + (right - left) * amount)
.collect(),
);
}
}
values.last().cloned()
}
fn lerp_vec3(left: Vec3, right: Vec3, amount: f32) -> Vec3 {
Vec3::new(
left.x + (right.x - left.x) * amount,
left.y + (right.y - left.y) * amount,
left.z + (right.z - left.z) * amount,
)
}
fn sample_cubic_vec3(times: &[f32], values: &[Vec3], time_seconds: f32) -> Option<Vec3> {
if values.len() < times.len().saturating_mul(3) {
return None;
}
if time_seconds <= times[0] {
return values.get(1).copied();
}
if time_seconds >= *times.last()? {
return values.get((times.len() - 1) * 3 + 1).copied();
}
for index in 0..times.len().saturating_sub(1) {
let start = times[index];
let end = times[index + 1];
if time_seconds <= end {
let amount = ((time_seconds - start) / (end - start)).clamp(0.0, 1.0);
return Some(cubic_vec3(
*values.get(index * 3 + 1)?,
*values.get(index * 3 + 2)?,
*values.get((index + 1) * 3)?,
*values.get((index + 1) * 3 + 1)?,
end - start,
amount,
));
}
}
values.get((times.len() - 1) * 3 + 1).copied()
}
fn sample_cubic_quat(times: &[f32], values: &[Quat], time_seconds: f32) -> Option<Quat> {
if values.len() < times.len().saturating_mul(3) {
return None;
}
if time_seconds <= times[0] {
return values.get(1).copied().map(normalize_quat);
}
if time_seconds >= *times.last()? {
return values
.get((times.len() - 1) * 3 + 1)
.copied()
.map(normalize_quat);
}
for index in 0..times.len().saturating_sub(1) {
let start = times[index];
let end = times[index + 1];
if time_seconds <= end {
let amount = ((time_seconds - start) / (end - start)).clamp(0.0, 1.0);
return Some(normalize_quat(cubic_quat(
*values.get(index * 3 + 1)?,
*values.get(index * 3 + 2)?,
*values.get((index + 1) * 3)?,
*values.get((index + 1) * 3 + 1)?,
end - start,
amount,
)));
}
}
values
.get((times.len() - 1) * 3 + 1)
.copied()
.map(normalize_quat)
}
fn sample_cubic_weights(times: &[f32], values: &[Vec<f32>], time_seconds: f32) -> Option<Vec<f32>> {
if values.len() < times.len().saturating_mul(3) {
return None;
}
if time_seconds <= times[0] {
return values.get(1).cloned();
}
if time_seconds >= *times.last()? {
return values.get((times.len() - 1) * 3 + 1).cloned();
}
for index in 0..times.len().saturating_sub(1) {
let start = times[index];
let end = times[index + 1];
if time_seconds <= end {
let amount = ((time_seconds - start) / (end - start)).clamp(0.0, 1.0);
return Some(cubic_weights(
values.get(index * 3 + 1)?,
values.get(index * 3 + 2)?,
values.get((index + 1) * 3)?,
values.get((index + 1) * 3 + 1)?,
end - start,
amount,
));
}
}
values.get((times.len() - 1) * 3 + 1).cloned()
}
fn cubic_vec3(
p0: Vec3,
out_tangent0: Vec3,
in_tangent1: Vec3,
p1: Vec3,
delta_seconds: f32,
amount: f32,
) -> Vec3 {
let components = cubic_components(
[p0.x, p0.y, p0.z],
[out_tangent0.x, out_tangent0.y, out_tangent0.z],
[in_tangent1.x, in_tangent1.y, in_tangent1.z],
[p1.x, p1.y, p1.z],
delta_seconds,
amount,
);
Vec3::new(components[0], components[1], components[2])
}
fn cubic_quat(
p0: Quat,
out_tangent0: Quat,
in_tangent1: Quat,
p1: Quat,
delta_seconds: f32,
amount: f32,
) -> Quat {
let components = cubic_components(
[p0.x, p0.y, p0.z, p0.w],
[
out_tangent0.x,
out_tangent0.y,
out_tangent0.z,
out_tangent0.w,
],
[in_tangent1.x, in_tangent1.y, in_tangent1.z, in_tangent1.w],
[p1.x, p1.y, p1.z, p1.w],
delta_seconds,
amount,
);
Quat::from_xyzw(components[0], components[1], components[2], components[3])
}
fn cubic_weights(
p0: &[f32],
out_tangent0: &[f32],
in_tangent1: &[f32],
p1: &[f32],
delta_seconds: f32,
amount: f32,
) -> Vec<f32> {
p0.iter()
.zip(out_tangent0)
.zip(in_tangent1)
.zip(p1)
.map(|(((p0, out_tangent0), in_tangent1), p1)| {
cubic_scalar(*p0, *out_tangent0, *in_tangent1, *p1, delta_seconds, amount)
})
.collect()
}
fn cubic_components<const N: usize>(
p0: [f32; N],
out_tangent0: [f32; N],
in_tangent1: [f32; N],
p1: [f32; N],
delta_seconds: f32,
amount: f32,
) -> [f32; N] {
std::array::from_fn(|index| {
cubic_scalar(
p0[index],
out_tangent0[index],
in_tangent1[index],
p1[index],
delta_seconds,
amount,
)
})
}
fn cubic_scalar(
p0: f32,
out_tangent0: f32,
in_tangent1: f32,
p1: f32,
delta_seconds: f32,
amount: f32,
) -> f32 {
let t2 = amount * amount;
let t3 = t2 * amount;
let h00 = 2.0 * t3 - 3.0 * t2 + 1.0;
let h10 = t3 - 2.0 * t2 + amount;
let h01 = -2.0 * t3 + 3.0 * t2;
let h11 = t3 - t2;
h00 * p0 + h10 * delta_seconds * out_tangent0 + h01 * p1 + h11 * delta_seconds * in_tangent1
}
fn normalize_quat(value: Quat) -> Quat {
let length =
(value.x * value.x + value.y * value.y + value.z * value.z + value.w * value.w).sqrt();
if length <= f32::EPSILON || !length.is_finite() {
return Quat::IDENTITY;
}
Quat::from_xyzw(
value.x / length,
value.y / length,
value.z / length,
value.w / length,
)
}
fn slerp_quat(left: Quat, right: Quat, amount: f32) -> Quat {
let mut right = right;
let mut dot = left.x * right.x + left.y * right.y + left.z * right.z + left.w * right.w;
if dot < 0.0 {
dot = -dot;
right = Quat::from_xyzw(-right.x, -right.y, -right.z, -right.w);
}
if dot > 0.9995 {
return normalize_quat(Quat::from_xyzw(
left.x + (right.x - left.x) * amount,
left.y + (right.y - left.y) * amount,
left.z + (right.z - left.z) * amount,
left.w + (right.w - left.w) * amount,
));
}
let theta_0 = dot.acos();
let theta = theta_0 * amount;
let sin_theta = theta.sin();
let sin_theta_0 = theta_0.sin();
let left_scale = theta.cos() - dot * sin_theta / sin_theta_0;
let right_scale = sin_theta / sin_theta_0;
normalize_quat(Quat::from_xyzw(
left.x * left_scale + right.x * right_scale,
left.y * left_scale + right.y * right_scale,
left.z * left_scale + right.z * right_scale,
left.w * left_scale + right.w * right_scale,
))
}