use glam::{Affine3A, Quat, Vec3};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BonePose {
pub translation: Vec3,
pub rotation: Quat,
pub scale: Vec3,
}
impl Default for BonePose {
fn default() -> Self {
Self {
translation: Vec3::ZERO,
rotation: Quat::IDENTITY,
scale: Vec3::ONE,
}
}
}
impl BonePose {
pub fn affine(self) -> Affine3A {
super::godot_math::pose_affine(self.scale, self.rotation, self.translation)
}
}
pub struct SkeletonView<'a> {
pub parents: &'a [Option<u16>],
pub rest: &'a [BonePose],
pub local: &'a mut [BonePose],
pub root_global: Affine3A,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct ChainIndices {
len: u8,
bones: [u16; 32],
}
impl ChainIndices {
pub fn as_slice(&self) -> &[u16] {
&self.bones[..self.len as usize]
}
pub fn len(&self) -> usize {
self.len as usize
}
pub fn is_empty(&self) -> bool {
self.len == 0
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum ChainError {
TooLong,
NotDescendant,
InvalidIndex,
Cycle,
}
impl SkeletonView<'_> {
pub fn validate(&self) -> Result<(), ChainError> {
let n = self.local.len();
if self.parents.len() != n || self.rest.len() != n || n > u16::MAX as usize {
return Err(ChainError::InvalidIndex);
}
for i in 0..n {
let mut p = Some(i as u16);
for step in 0..=n {
if let Some(j) = p {
if j as usize >= n {
return Err(ChainError::InvalidIndex);
}
if step == n {
return Err(ChainError::Cycle);
}
p = self.parents[j as usize];
} else {
break;
}
}
}
Ok(())
}
pub fn chain(&self, root: u16, end: u16) -> Result<ChainIndices, ChainError> {
if root as usize >= self.local.len() || end as usize >= self.local.len() {
return Err(ChainError::InvalidIndex);
}
let mut chain = ChainIndices {
len: 0,
bones: [0; 32],
};
let mut i = end;
loop {
if chain.as_slice().contains(&i) {
return Err(ChainError::Cycle);
}
if chain.len() == 32 {
return Err(ChainError::TooLong);
}
chain.bones[chain.len()] = i;
chain.len += 1;
if i == root {
break;
}
i = self
.parents
.get(i as usize)
.copied()
.flatten()
.ok_or(ChainError::NotDescendant)?;
if i as usize >= self.local.len() {
return Err(ChainError::InvalidIndex);
}
}
let len = chain.len();
chain.bones[..len].reverse();
Ok(chain)
}
pub(crate) fn pose_from(&self, i: usize, poses: &[BonePose]) -> Affine3A {
let mut result = poses.get(i).copied().unwrap_or_default().affine();
let mut p = self.parents.get(i).copied().flatten();
for _ in 0..poses.len() {
let Some(j) = p else { return result };
let Some(pose) = poses.get(j as usize) else {
return result;
};
result = super::godot_math::affine_mul(pose.affine(), result);
p = self.parents.get(j as usize).copied().flatten();
}
result
}
pub fn skeleton_pose(&self, i: u16) -> Affine3A {
self.pose_from(i as usize, self.local)
}
pub fn global_pose(&self, i: u16) -> Affine3A {
self.root_global * self.skeleton_pose(i)
}
pub fn set_global_rotation(&mut self, i: u16, rotation: Quat) {
let parent = self
.parents
.get(i as usize)
.copied()
.flatten()
.map_or(self.root_global, |p| self.global_pose(p));
if let Some(local) = self.local.get_mut(i as usize) {
local.rotation = (super::godot_math::rotation(parent).inverse() * rotation).normalize();
}
}
pub(crate) fn set_skeleton_rotation(&mut self, i: u16, q: Quat) {
let parent = self.parents[i as usize].map_or(Quat::IDENTITY, |p| {
super::godot_math::rotation(self.skeleton_pose(p))
});
self.local[i as usize].rotation = (parent.inverse() * q).normalize();
}
}
pub fn apply_influence(input: &[BonePose], output: &mut [BonePose], influence: f32) {
let t = if influence.is_finite() {
influence.clamp(0., 1.)
} else {
0.
};
for (a, b) in input.iter().zip(output.iter_mut()) {
if t == 0. {
*b = *a
} else if t < 1. {
b.translation = a.translation.lerp(b.translation, t);
b.rotation = a.rotation.slerp(b.rotation, t).normalize();
b.scale = a.scale.lerp(b.scale, t);
}
}
}