#![doc = include_str!(".crate-docs.md")]
use std::{
borrow::Borrow,
collections::{HashMap, HashSet},
f32::consts::PI,
fmt::{Debug, Display},
ops::{Add, AddAssign, Deref, Div, Index, IndexMut, Mul, Neg, Sub, SubAssign},
sync::Arc,
vec::Vec,
};
pub mod animation;
#[cfg(feature = "cushy")]
pub mod cushy;
#[cfg(feature = "editor")]
pub mod editor;
#[cfg(feature = "serde")]
mod serde;
#[derive(Default, Clone, Copy, PartialEq, Debug)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub struct Coordinate {
pub x: f32,
pub y: f32,
}
impl Coordinate {
#[must_use]
pub const fn new(x: f32, y: f32) -> Self {
Self { x, y }
}
#[must_use]
pub fn magnitude(&self) -> f32 {
(self.x * self.x + self.y * self.y).sqrt()
}
#[must_use]
pub fn map(self, mut f: impl FnMut(f32) -> f32) -> Self {
Self {
x: f(self.x),
y: f(self.y),
}
}
#[must_use]
pub fn as_rotation(self) -> Rotation {
Rotation::radians(self.y.atan2(self.x))
}
#[must_use]
pub fn vector_to(self, other: Coordinate) -> Vector {
Vector::from(other - self)
}
}
impl Add for Coordinate {
type Output = Self;
fn add(self, rhs: Self) -> Self::Output {
Self {
x: self.x + rhs.x,
y: self.y + rhs.y,
}
}
}
impl Sub for Coordinate {
type Output = Self;
fn sub(self, rhs: Self) -> Self::Output {
Self {
x: self.x - rhs.x,
y: self.y - rhs.y,
}
}
}
impl Mul<f32> for Coordinate {
type Output = Self;
fn mul(self, rhs: f32) -> Self::Output {
Self {
x: self.x * rhs,
y: self.y * rhs,
}
}
}
impl Div<f32> for Coordinate {
type Output = Self;
fn div(self, rhs: f32) -> Self::Output {
Self {
x: self.x / rhs,
y: self.y / rhs,
}
}
}
#[derive(Debug, PartialEq, Clone, Copy, PartialOrd, Default)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub struct Angle(Rotation);
impl Angle {
pub const MIN: Self = Self(Rotation { radians: 0. });
pub const MAX: Self = Self(Rotation {
radians: PI * 2. - f32::EPSILON,
});
#[must_use]
pub fn radians(radians: f32) -> Self {
Self(Rotation::radians(radians).clamped())
}
#[must_use]
pub fn degrees(degrees: f32) -> Self {
Self::radians(degrees * PI / 180.0)
}
}
impl From<Angle> for Rotation {
fn from(value: Angle) -> Self {
value.0
}
}
impl From<Rotation> for Angle {
fn from(value: Rotation) -> Self {
Self(value.clamped())
}
}
impl Display for Angle {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
Display::fmt(&self.0, f)
}
}
#[derive(Clone, Copy, PartialEq, PartialOrd)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub struct Rotation {
radians: f32,
}
impl Rotation {
#[must_use]
pub const fn radians(radians: f32) -> Self {
Self { radians }
}
#[must_use]
pub fn degrees(degrees: f32) -> Self {
Self::radians(degrees * PI / 180.0)
}
#[must_use]
pub fn to_degrees(self) -> f32 {
self.radians * 180.0 / PI
}
#[must_use]
pub const fn to_radians(self) -> f32 {
self.radians
}
#[must_use]
pub fn clamped(mut self) -> Self {
const TWO_PI: f32 = PI * 2.0;
while self.radians >= TWO_PI {
self.radians -= TWO_PI;
}
while self.radians < 0. {
self.radians += TWO_PI;
}
self
}
#[must_use]
pub fn cos(self) -> f32 {
self.radians.cos()
}
#[must_use]
pub fn sin(self) -> f32 {
self.radians.sin()
}
}
impl Debug for Rotation {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
Display::fmt(self, f)
}
}
impl Display for Rotation {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(f, "{}°", self.to_degrees())
}
}
impl Default for Rotation {
fn default() -> Self {
Self { radians: 0. }
}
}
impl Add for Rotation {
type Output = Self;
fn add(self, rhs: Self) -> Self::Output {
Self::radians(self.radians + rhs.radians)
}
}
impl AddAssign for Rotation {
fn add_assign(&mut self, rhs: Self) {
self.radians = (*self + rhs).radians;
}
}
impl Sub for Rotation {
type Output = Self;
fn sub(self, rhs: Self) -> Self::Output {
Self::radians(self.radians - rhs.radians)
}
}
impl SubAssign for Rotation {
fn sub_assign(&mut self, rhs: Self) {
self.radians = (*self - rhs).radians;
}
}
impl Neg for Rotation {
type Output = Self;
fn neg(self) -> Self::Output {
Self::radians(-self.radians)
}
}
#[derive(Clone, Copy, PartialEq, Debug, Default)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub struct Vector {
pub magnitude: f32,
pub direction: Rotation,
}
impl Vector {
#[must_use]
pub const fn new(magnitude: f32, direction: Rotation) -> Self {
Self {
magnitude,
direction,
}
}
}
impl From<Vector> for Coordinate {
fn from(vec: Vector) -> Self {
Self {
x: vec.magnitude * vec.direction.cos(),
y: vec.magnitude * vec.direction.sin(),
}
}
}
impl From<Coordinate> for Vector {
fn from(pt: Coordinate) -> Self {
Self {
direction: pt.as_rotation(),
magnitude: pt.magnitude(),
}
}
}
impl Add for Vector {
type Output = Self;
fn add(self, rhs: Vector) -> Self::Output {
Vector::from(Coordinate::from(self) + Coordinate::from(rhs))
}
}
impl Add<Vector> for Coordinate {
type Output = Self;
fn add(self, rhs: Vector) -> Self::Output {
self + Coordinate::from(rhs)
}
}
impl Sub<Vector> for Coordinate {
type Output = Self;
fn sub(self, rhs: Vector) -> Self::Output {
self - Coordinate::from(rhs)
}
}
impl Add<Rotation> for Vector {
type Output = Self;
fn add(mut self, rhs: Rotation) -> Self::Output {
self.direction += rhs;
self
}
}
impl Sub<Rotation> for Vector {
type Output = Self;
fn sub(mut self, rhs: Rotation) -> Self::Output {
self.direction -= rhs;
self
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub enum BoneKind {
Rigid {
length: f32,
},
Jointed {
start_length: f32,
end_length: f32,
inverse: bool,
},
}
impl BoneKind {
#[must_use]
pub fn with_label(self, label: impl Into<String>) -> LabeledBoneKind {
LabeledBoneKind {
kind: self,
label: label.into(),
}
}
#[must_use]
pub fn full_length(&self) -> f32 {
match self {
BoneKind::Rigid { length } => *length,
BoneKind::Jointed {
start_length,
end_length,
..
} => *start_length + *end_length,
}
}
#[must_use]
pub fn is_inverse(&self) -> bool {
match self {
BoneKind::Rigid { .. } => false,
BoneKind::Jointed { inverse, .. } => *inverse,
}
}
pub fn set_inverse(&mut self, new_inverse: bool) {
let BoneKind::Jointed { inverse, .. } = self else {
return;
};
*inverse = new_inverse;
}
}
pub struct LabeledBoneKind {
pub kind: BoneKind,
pub label: String,
}
impl From<BoneKind> for LabeledBoneKind {
fn from(kind: BoneKind) -> Self {
kind.with_label(String::new())
}
}
#[derive(Clone, Eq, PartialEq, Hash, Debug)]
struct ArcString(Arc<String>);
impl PartialEq<str> for ArcString {
fn eq(&self, other: &str) -> bool {
&**self == other
}
}
impl Deref for ArcString {
type Target = str;
fn deref(&self) -> &Self::Target {
&self.0
}
}
impl Borrow<str> for ArcString {
fn borrow(&self) -> &str {
self
}
}
#[derive(Default, Debug, PartialEq)]
pub struct Skeleton {
bones: Vec<Bone>,
rotation: Rotation,
joints: Vec<Joint>,
connections: HashMap<BoneAxis, Vec<JointId>>,
generation: usize,
bones_by_label: HashMap<ArcString, BoneId>,
joints_by_label: HashMap<ArcString, JointId>,
}
impl Skeleton {
pub fn push_bone(&mut self, bone: impl Into<LabeledBoneKind>) -> BoneId {
let bone = bone.into();
let id = BoneId(u16::try_from(self.bones.len()).expect("too many bones"));
let label = if bone.label.is_empty() {
None
} else {
let label = ArcString(Arc::new(bone.label));
self.bones_by_label.insert(label.clone(), id);
Some(label)
};
self.bones.push(Bone {
id,
generation: self.generation,
label,
kind: bone.kind,
start: Coordinate::default(),
joint_pos: None,
end: Coordinate::default(),
desired_end: None,
entry_angle: Rotation::default(),
});
id
}
#[must_use]
pub fn bones(&self) -> &[Bone] {
&self.bones
}
#[must_use]
pub fn joints(&self) -> &[Joint] {
&self.joints
}
#[must_use]
pub fn bone(&self, id: BoneId) -> Option<&Bone> {
self.bones.get(id.index())
}
#[must_use]
pub fn bone_mut(&mut self, id: BoneId) -> Option<&mut Bone> {
self.bones.get_mut(id.index())
}
#[must_use]
pub fn joint(&self, id: JointId) -> Option<&Joint> {
self.joints.get(id.index())
}
#[must_use]
pub fn joint_mut(&mut self, id: JointId) -> Option<&mut Joint> {
self.joints.get_mut(id.index())
}
#[must_use]
pub fn connections_to(&self, axis: BoneAxis) -> Option<&[JointId]> {
self.connections.get(&axis).map(Vec::as_slice)
}
pub fn push_joint(&mut self, mut joint: Joint) -> JointId {
let id = JointId(u16::try_from(self.joints.len()).expect("too many joints"));
joint.id = id;
let bone_a = joint.bone_a;
let bone_b = joint.bone_b;
if let Some(label) = joint.label.clone() {
self.joints_by_label.insert(label, id);
}
self.joints.push(joint);
self.connections.entry(bone_a).or_default().push(id);
if bone_a != bone_b {
self.connections.entry(bone_b).or_default().push(id);
}
id
}
#[must_use]
pub fn find_joint_by_label(&self, label: &str) -> Option<JointId> {
self.joints_by_label.get(label).copied()
}
#[must_use]
pub fn find_bone_by_label(&self, label: &str) -> Option<BoneId> {
self.bones_by_label.get(label).copied()
}
pub fn set_translation(&mut self, translation: Coordinate) {
let bone = self.bones.first_mut().expect("root bone must be defined");
bone.start = translation;
}
#[must_use]
pub fn translation(&self) -> Coordinate {
self.bones.first().expect("root bone must be defined").start
}
pub fn set_rotation(&mut self, rotation: Rotation) {
self.rotation = rotation;
}
#[must_use]
pub const fn rotation(&self) -> Rotation {
self.rotation
}
pub fn solve(&mut self) {
if !self.bones.is_empty() {
self.generation = self.generation.wrapping_add(1);
self.solve_axis();
}
}
fn solve_axis(&mut self) {
let mut axis_solved = HashSet::new();
let root_bone = &mut self.bones[0];
let (end, mid, _) = determine_end_position(
root_bone.start,
root_bone.desired_end,
self.rotation,
Rotation::radians(0.),
&root_bone.kind,
);
root_bone.entry_angle = self.rotation;
root_bone.end = end;
root_bone.joint_pos = mid;
let angle = root_bone.final_vector().direction;
let mut to_solve = vec![
(
root_bone.id.axis_a(),
root_bone.start,
angle + Rotation::radians(PI),
),
(root_bone.id.axis_b(), root_bone.end, angle),
];
while let Some((axis, current_position, current_rotation)) = to_solve.pop() {
if !axis_solved.insert(axis) {
continue;
}
let Some(connections) = self.connections.get(&axis) else {
continue;
};
for joint_id in connections {
let joint = &mut self.joints[joint_id.index()];
let other_axis = joint.other_axis(axis);
let bone = &mut self.bones[other_axis.bone.index()];
if bone.generation == self.generation {
continue;
}
bone.generation = self.generation;
bone.entry_angle = current_rotation;
bone.start = current_position;
joint.calculated_position = current_position;
let (end, mid, angle_offset) = determine_end_position(
current_position,
bone.desired_end,
current_rotation,
joint.angle,
&bone.kind,
);
bone.entry_angle += angle_offset;
bone.end = end;
bone.joint_pos = mid;
to_solve.push((
other_axis.inverse(),
bone.end,
bone.final_vector().direction,
));
}
}
}
}
fn determine_end_position(
start: Coordinate,
desired_end: Option<Vector>,
current_rotation: Rotation,
joint_angle: Rotation,
bone: &BoneKind,
) -> (Coordinate, Option<Coordinate>, Rotation) {
let entry_angle = current_rotation + joint_angle;
match bone {
BoneKind::Rigid { length } => (
start + Vector::new(*length, entry_angle),
None,
Rotation::default(),
),
BoneKind::Jointed {
start_length,
end_length,
inverse,
} => {
if let Some(desired_end) = desired_end {
let desired_angle = desired_end.direction + entry_angle;
let distance = desired_end.magnitude;
let full_length = start_length + end_length;
let minimum_size = (start_length - end_length).abs();
let desired_length = if distance < minimum_size {
minimum_size
} else if distance > full_length {
full_length
} else {
distance
};
let end = start + Vector::new(desired_length, desired_angle);
let joint = get_third_point(
*inverse,
start,
desired_length,
desired_angle,
*start_length,
*end_length,
);
(end, Some(joint), joint_angle)
} else {
let joint = start + Vector::new(*start_length, entry_angle);
let end = joint + Vector::new(*end_length, entry_angle);
(end, Some(joint), joint_angle)
}
}
}
}
fn get_third_point(
inverse: bool,
start: Coordinate,
distance: f32,
hyp_angle: Rotation,
first: f32,
second: f32,
) -> Coordinate {
let hyp = distance;
let first_angle = ((first * first + hyp * hyp - second * second) / (2. * first * hyp)).acos();
if first_angle.is_nan() {
start + Vector::new(first, hyp_angle)
} else {
let first_angle = hyp_angle
- Rotation {
radians: if inverse { -first_angle } else { first_angle },
};
start + Vector::new(first, first_angle)
}
}
impl Index<BoneId> for Skeleton {
type Output = Bone;
fn index(&self, index: BoneId) -> &Self::Output {
&self.bones[index.index()]
}
}
impl IndexMut<BoneId> for Skeleton {
fn index_mut(&mut self, index: BoneId) -> &mut Self::Output {
&mut self.bones[index.index()]
}
}
impl Index<JointId> for Skeleton {
type Output = Joint;
fn index(&self, index: JointId) -> &Self::Output {
&self.joints[index.index()]
}
}
impl IndexMut<JointId> for Skeleton {
fn index_mut(&mut self, index: JointId) -> &mut Self::Output {
&mut self.joints[index.index()]
}
}
#[derive(Debug, Clone, Copy, Eq, PartialEq, Hash)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub struct BoneAxis {
pub bone: BoneId,
pub end: BoneEnd,
}
impl BoneAxis {
#[must_use]
pub const fn inverse(self) -> Self {
Self {
bone: self.bone,
end: self.end.inverse(),
}
}
}
#[derive(Debug, PartialEq)]
pub struct Bone {
id: BoneId,
generation: usize,
label: Option<ArcString>,
kind: BoneKind,
start: Coordinate,
joint_pos: Option<Coordinate>,
end: Coordinate,
desired_end: Option<Vector>,
entry_angle: Rotation,
}
impl Bone {
#[must_use]
pub const fn id(&self) -> BoneId {
self.id
}
#[must_use]
pub const fn is_root(&self) -> bool {
self.id.0 == 0
}
#[must_use]
pub const fn kind(&self) -> &BoneKind {
&self.kind
}
#[must_use]
pub fn kind_mut(&mut self) -> &mut BoneKind {
&mut self.kind
}
pub fn set_desired_end(&mut self, end: Option<Vector>) {
self.desired_end = end;
}
#[must_use]
pub const fn desired_end(&self) -> Option<Vector> {
self.desired_end
}
#[must_use]
pub const fn entry_angle(&self) -> Rotation {
self.entry_angle
}
#[must_use]
pub const fn start(&self) -> Coordinate {
self.start
}
#[must_use]
pub const fn end(&self) -> Coordinate {
self.end
}
#[must_use]
pub fn final_vector(&self) -> Vector {
let start = self.joint_pos.unwrap_or(self.start);
start.vector_to(self.end)
}
#[must_use]
pub const fn solved_joint(&self) -> Option<Coordinate> {
self.joint_pos
}
#[must_use]
pub fn label(&self) -> &str {
self.label.as_ref().map_or("", |s| s)
}
}
#[derive(Debug, PartialEq)]
pub struct Joint {
id: JointId,
label: Option<ArcString>,
bone_a: BoneAxis,
bone_b: BoneAxis,
calculated_position: Coordinate,
angle: Rotation,
}
impl Joint {
#[must_use]
pub const fn id(&self) -> JointId {
self.id
}
#[must_use]
pub const fn new(angle: Rotation, bone_a: BoneAxis, bone_b: BoneAxis) -> Self {
Self {
id: JointId(0),
label: None,
bone_a,
bone_b,
calculated_position: Coordinate::new(0., 0.),
angle,
}
}
#[must_use]
pub fn with_label(mut self, label: impl Into<String>) -> Self {
let label = label.into();
if !label.is_empty() {
self.label = Some(ArcString(Arc::new(label)));
}
self
}
#[must_use]
pub fn label(&self) -> &str {
self.label.as_ref().map_or("", |s| s)
}
#[must_use]
pub fn other_axis(&self, axis: BoneAxis) -> BoneAxis {
if self.bone_a == axis {
self.bone_b
} else {
debug_assert_eq!(self.bone_b, axis);
self.bone_a
}
}
pub fn set_angle(&mut self, angle: Rotation) {
self.angle = angle;
}
#[must_use]
pub const fn angle(&self) -> Rotation {
self.angle
}
}
#[derive(Debug, Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Hash)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub struct BoneId(u16);
impl BoneId {
#[must_use]
pub const fn axis_a(self) -> BoneAxis {
BoneAxis {
bone: self,
end: BoneEnd::A,
}
}
#[must_use]
pub const fn axis_b(self) -> BoneAxis {
BoneAxis {
bone: self,
end: BoneEnd::B,
}
}
#[must_use]
pub fn index(self) -> usize {
usize::from(self.0)
}
}
#[derive(Debug, Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Hash)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub struct JointId(u16);
impl JointId {
#[must_use]
pub fn index(self) -> usize {
usize::from(self.0)
}
}
#[derive(Debug, Clone, Copy, Eq, PartialEq, Hash)]
#[cfg_attr(feature = "serde", derive(::serde::Serialize, ::serde::Deserialize))]
pub enum BoneEnd {
A,
B,
}
impl BoneEnd {
#[must_use]
pub const fn inverse(self) -> Self {
match self {
Self::A => Self::B,
Self::B => Self::A,
}
}
}
#[test]
#[allow(clippy::cast_possible_truncation)]
fn rotation() {
assert_eq!(
(Rotation::degrees(90.) + Rotation::degrees(180.))
.clamped()
.to_degrees()
.round() as i32,
270,
);
assert_eq!(
(Rotation::degrees(90.) + Rotation::degrees(-180.))
.clamped()
.to_degrees()
.round() as i32,
270,
);
}