diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml new file mode 100644 index 0000000..e9f2f13 --- /dev/null +++ b/.github/FUNDING.yml @@ -0,0 +1 @@ +github: [ecton] \ No newline at end of file diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml new file mode 100644 index 0000000..534bd74 --- /dev/null +++ b/.github/workflows/docs.yml @@ -0,0 +1,28 @@ +name: Docs + +on: [push, pull_request] + +jobs: + docs: + runs-on: ubuntu-latest + steps: + - name: Install Rust + uses: hecrj/setup-rust-action@v1 + with: + rust-version: nightly + + - uses: actions/checkout@v3 + - name: Generate Docs + run: | + cargo +nightly doc --no-deps --all-features --workspace + + - name: Deploy Docs + if: github.ref == 'refs/heads/main' + uses: JamesIves/github-pages-deploy-action@releases/v4 + with: + branch: gh-pages + folder: target/doc/ + git-config-name: kl-botsu + git-config-email: botsu@khonsulabs.com + target-folder: /main/ + clean: true diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml new file mode 100644 index 0000000..a9cc410 --- /dev/null +++ b/.github/workflows/rust.yml @@ -0,0 +1,33 @@ +name: Tests + +on: [push, pull_request] + +jobs: + test: + runs-on: ubuntu-latest + timeout-minutes: 30 + steps: + - uses: actions/checkout@v3 + + - name: Install Rust + uses: hecrj/setup-rust-action@v1 + + - name: Run clippy + run: | + cargo clippy + + - name: Run unit tests + run: | + cargo test + + build-msrv: + name: Test on MSRV + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Install Rust + uses: hecrj/setup-rust-action@v1 + with: + rust-version: 1.65.0 + - name: Run unit tests + run: cargo test diff --git a/Cargo.lock b/Cargo.lock index 76bcd94..ac5da4f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -624,6 +624,7 @@ checksum = "96a6ac251f4a2aca6b3f91340350eab87ae57c3f127ffeb585e92bd336717991" [[package]] name = "cushy" version = "0.3.0" +source = "git+https://github.com/khonsulabs/cushy#6726855ed06ed0f0b03157363b4cecb3869ba500" dependencies = [ "ahash", "alot", @@ -649,6 +650,7 @@ dependencies = [ [[package]] name = "cushy-macros" version = "0.3.0" +source = "git+https://github.com/khonsulabs/cushy#6726855ed06ed0f0b03157363b4cecb3869ba500" dependencies = [ "attribute-derive", "manyhow 0.11.3", @@ -891,6 +893,13 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "aa9a19cbb55df58761df49b23516a86d432839add4af60fc256da840f66ed35b" +[[package]] +name = "funnybones" +version = "0.1.0" +dependencies = [ + "cushy", +] + [[package]] name = "gethostname" version = "0.4.3" @@ -2532,13 +2541,6 @@ version = "0.3.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "38b58827f4464d87d377d175e90bf58eb00fd8716ff0a62f80356b5e61555d0d" -[[package]] -name = "skeleton" -version = "0.1.0" -dependencies = [ - "cushy", -] - [[package]] name = "skrifa" version = "0.20.0" diff --git a/Cargo.toml b/Cargo.toml index 75e606a..de1f913 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,26 @@ [package] -name = "skeleton" +name = "funnybones" version = "0.1.0" edition = "2021" +[features] + +[[example]] +name = "toy" +required-features = ["cushy"] + [dependencies] -cushy = { path = "../cushy" } +cushy = { git = "https://github.com/khonsulabs/cushy", optional = true } + +[dev-dependencies] +cushy = { git = "https://github.com/khonsulabs/cushy" } + +[lints.clippy] +pedantic = { level = "warn", priority = -1 } +module_name_repetitions = "allow" +missing_errors_doc = "allow" +missing_panics_doc = "allow" + +[lints.rust] +unsafe_code = "forbid" +missing_docs = "warn" diff --git a/README.md b/README.md new file mode 100644 index 0000000..613e23a --- /dev/null +++ b/README.md @@ -0,0 +1,43 @@ +# FunnyBones + +A simple 2D kinematics library for Rust + +## Motivation and Goals + +When looking at the libraries that support inverse kinematics in Rust in 2024, +there are several fairly mature solutions that focus on 3D and robotics. For +someone interested in 2D only, a lot of these libraries seemed like overkill for +basic 2D games. + +This library implements a simplified forward and inverse kinematics model that +only uses basic trigonometry and can be solved in one pass across the bone +structure with no smoothing algorithms necessary. The initial implementation of +this library was under 600 lines of code with no required dependencies. + +## How FunnyBones works + +FunnyBones has two main concepts: joints and bones. + +- *Joints* are used to connect a specific end of one bone to a specific end of +another bone. Each joint can be assigned an angle which is applied as *forward +kinematics* to create the angle using the two associated bones. +- *Bones* are one-dimensional line segments that have a required length. Bones + can have a *desired position* for the end of the bone positioned furthest from + the skeleton root. If the desired position is set, it is applied as *inverse + kinematics*. + + In FunnyBones, bones come in two varieties: + + - *Rigid* bones are a single line segment of a fixed length. An example of a + rigid bone in a simple human skeleton might be a single bone representing + the spine. + - *Flexible* bones are two line segments of fixed lengths that bend and rotate + automatically (ignoring the connecting joint's angle) to ensure that both + leg segments are always the correct length. An example of a flexible bone in + a simple human skeleton might be a leg or an arm. + +A `Skeleton` is a collection of joints and bones. The first bone pushed is +considered the root bone. When solving for updated positions, the algorithm +starts by evaluating all joints connected to both ends of the root bone and +continues until all reachable bones have been evaluated. The algorithm is +single-pass and produces stable results. \ No newline at end of file diff --git a/examples/toy.rs b/examples/toy.rs new file mode 100644 index 0000000..2ec0178 --- /dev/null +++ b/examples/toy.rs @@ -0,0 +1,232 @@ +//! A basic 2d humanoid skeleton with sliders powered by Cushy. +use core::f32; +use std::ops::RangeInclusive; + +use cushy::{ + figures::{ + units::{Lp, Px}, + IntoComponents, IntoSigned, Point, + }, + kludgine::{ + shapes::{PathBuilder, Shape, StrokeOptions}, + DrawableExt, Origin, + }, + styles::Color, + value::{Dynamic, DynamicRead, Source}, + widget::MakeWidget, + widgets::{slider::Slidable, Canvas}, + Run, +}; +use funnybones::{BoneId, BoneKind, JointId, Rotation, Skeleton, Vector}; + +fn main() { + let mut skeleton = Skeleton::default(); + let spine = skeleton.push_bone(BoneKind::Rigid { length: 3. }, "spine"); + let r_hip = skeleton.push_bone(BoneKind::Rigid { length: 0.5 }, "r_hip"); + let r_leg = skeleton.push_bone( + BoneKind::Jointed { + start_length: 1.5, + end_length: 1.5, + inverse: true, + }, + "r_leg", + ); + let r_foot = skeleton.push_bone(BoneKind::Rigid { length: 0.5 }, "r_foot"); + let l_hip = skeleton.push_bone(BoneKind::Rigid { length: 0.5 }, "l_hip"); + let l_leg = skeleton.push_bone( + BoneKind::Jointed { + start_length: 1.5, + end_length: 1.5, + inverse: false, + }, + "l_leg", + ); + let l_foot = skeleton.push_bone(BoneKind::Rigid { length: 0.5 }, "l_foot"); + let r_shoulder = skeleton.push_bone(BoneKind::Rigid { length: 0.5 }, "r_shoulder"); + let r_arm = skeleton.push_bone( + BoneKind::Jointed { + start_length: 1.0, + end_length: 1.0, + inverse: true, + }, + "r_arm", + ); + let r_hand = skeleton.push_bone(BoneKind::Rigid { length: 0.3 }, "r_hand"); + let l_shoulder = skeleton.push_bone(BoneKind::Rigid { length: 0.5 }, "l_shoulder"); + let l_arm = skeleton.push_bone( + BoneKind::Jointed { + start_length: 1.0, + end_length: 1.0, + inverse: false, + }, + "l_arm", + ); + let l_hand = skeleton.push_bone(BoneKind::Rigid { length: 0.3 }, "l_hand"); + let head = skeleton.push_bone(BoneKind::Rigid { length: 0.5 }, "head"); + + let neck = skeleton.push_joint(Rotation::degrees(180.), spine.axis_b(), head.axis_a()); + // Create some width for the legs to be spaced. + skeleton.push_joint(Rotation::degrees(90.), spine.axis_a(), l_hip.axis_a()); + skeleton.push_joint(Rotation::degrees(-90.), spine.axis_a(), r_hip.axis_a()); + skeleton.push_joint(Rotation::degrees(90.), l_hip.axis_b(), l_leg.axis_a()); + let l_ankle_id = skeleton.push_joint(Rotation::degrees(-90.), l_leg.axis_b(), l_foot.axis_a()); + skeleton.push_joint(Rotation::degrees(-90.), r_hip.axis_b(), r_leg.axis_a()); + let r_ankle_id = skeleton.push_joint(Rotation::degrees(90.), r_leg.axis_b(), r_foot.axis_a()); + + skeleton.push_joint(Rotation::degrees(90.), spine.axis_b(), l_shoulder.axis_a()); + skeleton.push_joint(Rotation::degrees(-90.), spine.axis_b(), r_shoulder.axis_a()); + let l_arm_socket = + skeleton.push_joint(Rotation::degrees(90.), l_shoulder.axis_b(), l_arm.axis_a()); + let l_wrist_id = skeleton.push_joint(Rotation::degrees(-175.), l_arm.axis_b(), l_hand.axis_a()); + let r_arm_socket = + skeleton.push_joint(Rotation::degrees(-90.), r_shoulder.axis_b(), r_arm.axis_a()); + let r_wrist_id = skeleton.push_joint(Rotation::degrees(175.), r_arm.axis_b(), r_hand.axis_a()); + + let skeleton = Dynamic::new(skeleton); + + Canvas::new({ + let skeleton = skeleton.clone(); + move |context| { + let mut s = skeleton.lock(); + s.prevent_notifications(); + s.solve(); + + let center = Point::from(context.gfx.size().into_signed()) / 2; + + let scale = Px::new(50); + for (bone, color) in [ + (spine, Color::RED), + (r_hip, Color::DARKBLUE), + (r_leg, Color::BLUE), + (r_foot, Color::LIGHTBLUE), + (l_hip, Color::DARKGREEN), + (l_leg, Color::GREEN), + (l_foot, Color::LIGHTGREEN), + (r_shoulder, Color::DARKGOLDENROD), + (r_arm, Color::GOLDENROD), + (r_hand, Color::LIGHTGOLDENRODYELLOW), + (l_shoulder, Color::DARKMAGENTA), + (l_arm, Color::MAGENTA), + (l_hand, Color::LIGHTPINK), + (head, Color::YELLOW), + ] { + let start = s[bone].start().to_vec::>().map(|d| scale * d); + let end = s[bone].end().to_vec::>().map(|d| scale * d); + if let Some(joint) = s[bone].solved_joint() { + let joint = joint.to_vec::>().map(|d| scale * d); + context.gfx.draw_shape( + PathBuilder::new(start) + .line_to(joint) + .build() + .stroke(StrokeOptions::px_wide(1).colored(color)) + .translate_by(center), + ); + context.gfx.draw_shape( + PathBuilder::new(joint) + .line_to(end) + .build() + .stroke(StrokeOptions::px_wide(1).colored(color)) + .translate_by(center), + ); + } else { + context.gfx.draw_shape( + PathBuilder::new(start) + .line_to(end) + .build() + .stroke(StrokeOptions::px_wide(1).colored(color)) + .translate_by(center), + ); + } + + if let Some(handle) = s[bone].desired_end() { + let handle = handle.to_vec::>().map(|d| scale * d); + context.gfx.draw_shape( + Shape::filled_circle(Px::new(3), Color::WHITE, Origin::Center) + .translate_by(handle + center), + ); + } + } + + drop(s); + + context.redraw_when_changed(&skeleton); + } + }) + .expand() + .and( + bone_widget("Lower Left Leg", &skeleton, l_leg, 0.5..=3.0, 0.5..=3.0) + .and(joint_widget("Left Ankle", &skeleton, l_ankle_id)) + .and(bone_widget( + "Lower Right Leg", + &skeleton, + r_leg, + -3.0..=-0.5, + 0.5..=3.0, + )) + .and(joint_widget("Right Ankle", &skeleton, r_ankle_id)) + .and(joint_widget("Left Shoulder", &skeleton, l_arm_socket)) + .and(joint_widget("Left Wrist", &skeleton, l_wrist_id)) + .and(joint_widget("Right Shoulder", &skeleton, r_arm_socket)) + .and(joint_widget("Right Wrist", &skeleton, r_wrist_id)) + .and(joint_widget("Neck", &skeleton, neck)) + .into_rows() + .pad() + .width(Lp::inches(2)), + ) + .into_columns() + .expand() + .run() + .unwrap(); +} + +fn joint_widget(label: &str, skeleton: &Dynamic, joint: JointId) -> impl MakeWidget { + let angle = Dynamic::new(skeleton.read()[joint].angle()); + angle + .for_each({ + let skeleton = skeleton.clone(); + move |degrees| { + skeleton.lock()[joint].set_angle(*degrees); + } + }) + .persist(); + let angle_slider = angle.slider_between(Rotation::degrees(0.), Rotation::degrees(359.9)); + + label.and(angle_slider).into_rows().contain() +} + +fn bone_widget( + label: &str, + skeleton: &Dynamic, + bone: BoneId, + x: RangeInclusive, + y: RangeInclusive, +) -> impl MakeWidget { + let bone_y = Dynamic::new(skeleton.lock()[bone].desired_end().unwrap_or_default().y); + let bone_x = Dynamic::new(skeleton.lock()[bone].desired_end().unwrap_or_default().x); + + bone_y + .for_each({ + let skeleton = skeleton.clone(); + move |y| { + let mut skeleton = skeleton.lock(); + let current_end = skeleton[bone].desired_end().unwrap_or_default(); + skeleton[bone].set_desired_end(Some(Vector::new(current_end.x, *y))); + } + }) + .persist(); + bone_x + .for_each({ + let skeleton = skeleton.clone(); + move |x| { + let mut skeleton = skeleton.lock(); + let current_end = skeleton[bone].desired_end().unwrap_or_default(); + skeleton[bone].set_desired_end(Some(Vector::new(*x, current_end.y))); + } + }) + .persist(); + label + .and(bone_x.slider_between(*x.start(), *x.end())) + .and(bone_y.slider_between(*y.start(), *y.end())) + .into_rows() + .contain() +} diff --git a/src/main.rs b/src/lib.rs similarity index 51% rename from src/main.rs rename to src/lib.rs index 66437f8..749c9a0 100644 --- a/src/main.rs +++ b/src/lib.rs @@ -1,719 +1,661 @@ -use std::{ - collections::{HashMap, HashSet}, - f32::consts::PI, - fmt::{Debug, Display}, - ops::{Add, Index, IndexMut, Neg, RangeInclusive, Sub}, -}; - -use cushy::{ - animation::{LinearInterpolate, PercentBetween}, - figures::{ - units::{Lp, Px}, - IntoSigned, Point, - }, - kludgine::{ - shapes::{PathBuilder, Shape, StrokeOptions}, - DrawableExt, Origin, - }, - styles::Color, - value::{Dynamic, DynamicRead, Source}, - widget::MakeWidget, - widgets::{slider::Slidable, Canvas}, - Run, -}; - -#[derive(Clone, Copy, PartialEq, PartialOrd)] -pub struct Rotation { - radians: f32, -} - -impl Rotation { - pub fn radians(radians: f32) -> Self { - Self { radians }.normalized() - } - - pub fn degrees(degrees: f32) -> Self { - Self::radians(degrees * PI / 180.0) - } - - pub fn to_degrees(&self) -> f32 { - self.radians * 180.0 / PI - } - - fn normalized(mut self) -> Self { - while self.radians > PI { - self.radians -= PI * 2.0; - } - while self.radians < -PI { - self.radians += PI * 2.0; - } - self - } -} - -impl PercentBetween for Rotation { - fn percent_between(&self, min: &Self, max: &Self) -> cushy::animation::ZeroToOne { - self.radians.percent_between(&min.radians, &max.radians) - } -} - -impl LinearInterpolate for Rotation { - fn lerp(&self, target: &Self, percent: f32) -> Self { - Self { - radians: self.radians.lerp(&target.radians, percent), - } - } -} - -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 Sub for Rotation { - type Output = Self; - - fn sub(self, rhs: Self) -> Self::Output { - Self { - radians: self.radians - rhs.radians, - } - } -} - -impl Neg for Rotation { - type Output = Self; - - fn neg(self) -> Self::Output { - Self { - radians: self.radians, - } - } -} - -#[derive(Debug, Clone, Copy, PartialEq)] -pub enum Bone { - Rigid { - length: f32, - }, - Jointed { - start_length: f32, - end_length: f32, - inverse: bool, - }, -} - -#[derive(Default, Debug)] -pub struct Skeleton { - initial_joint: Option, - bones: Vec, - joints: Vec, - connections: HashMap>, - generation: usize, -} - -impl Skeleton { - pub fn push_bone(&mut self, bone: Bone, label: &'static str) -> BoneId { - let id = BoneId(u8::try_from(self.bones.len()).expect("too many bones")); - if id == BoneId(0) { - let joint = self.push_joint(Rotation::default(), id.axis_a(), id.axis_a()); - self.initial_joint = Some(joint); - self.connections.insert(id.axis_a(), vec![joint]); - } - self.bones.push(SkeletonBone { - generation: self.generation, - label, - kind: bone, - start: Point::default(), - joint_pos: None, - end: Point::default(), - desired_end: None, - }); - id - } - - pub fn push_joint(&mut self, angle: Rotation, bone_a: BoneAxis, bone_b: BoneAxis) -> JointId { - let id = JointId(u8::try_from(self.joints.len()).expect("too many joints")); - self.joints.push(SkeletonJoint { - bone_a, - bone_b, - angle, - calculated_position: Point::default(), - }); - self.connections.entry(bone_a).or_default().push(id); - if bone_a != bone_b { - self.connections.entry(bone_b).or_default().push(id); - } - id - } - - pub fn set_translation(&mut self, translation: Point) { - let bone = self.bones.first_mut().expect("root bone must be defined"); - bone.start = translation; - } - - pub fn translation(&self) -> Point { - self.bones.first().expect("root bone must be defined").start - } - - pub fn set_rotation(&mut self, rotation: Rotation) { - let joint = self.initial_joint.expect("root bone must be defined"); - let joint = &mut self[joint]; - joint.angle = rotation; - } - - pub fn rotation(&self) -> Rotation { - let joint = self.initial_joint.expect("root bone must be defined"); - self[joint].angle - } - - pub fn solve(&mut self) { - if !self.bones.is_empty() { - self.generation = self.generation.wrapping_add(1); - self.solve_axis(BoneId(0).axis_a()); - } - } - - fn solve_axis(&mut self, axis: BoneAxis) { - let mut axis_solved = HashSet::new(); - let mut to_solve = vec![(axis, None, Rotation::default(), false)]; - while let Some((axis, current_position, current_rotation, inverse_root)) = to_solve.pop() { - if !axis_solved.insert(axis) { - continue; - } - - let Some(connections) = self.connections.get(&axis) else { - continue; - }; - - println!( - "Solving {}:{:?} at {current_position:?} - {current_rotation} - {inverse_root}", - self.bones[usize::from(axis.bone.0)].label, - axis.end - ); - - for joint_id in connections { - let joint = &mut self.joints[usize::from(joint_id.0)]; - let other_axis = joint.other_axis(axis); - let bone = &mut self.bones[usize::from(other_axis.bone.0)]; - if bone.generation == self.generation { - // We store connections in both directions, which means we - // can visit bones twice. We want to ensure we only follow - // each bone a single time. - continue; - } - bone.generation = self.generation; - println!( - " -> {joint_id:?} -> {}:{:?} ({})", - bone.label, other_axis.end, joint.angle - ); - joint.calculated_position = if let Some(current_position) = current_position { - bone.start = current_position; - current_position - } else { - debug_assert_eq!(axis.bone.0, 0); - bone.start - }; - - let angle = if inverse_root { - Rotation::radians(PI) - joint.angle - } else { - joint.angle - }; - - let mut next_rotation = (current_rotation + angle).normalized(); - let (end, mid) = determine_end_position( - joint.calculated_position, - bone.desired_end, - next_rotation, - &bone.kind, - ); - bone.end = end; - bone.joint_pos = mid; - if let Some(mid) = mid { - let final_delta = end - mid; - let rotation = Rotation::radians(final_delta.y.atan2(final_delta.x)); - // TODO I don't know why rotating by 90 degrees fixes - // everything here. It feels like atan2 should be giving us - // the correct rotation, or the correction amount should be - // driven by an input angle, but a fixed correction amount - // seems to be the correct answer. Without this, a joint - // angle of 0 sticks out at a perpendicular angle. - next_rotation = (rotation + Rotation::radians(PI / 2.)).normalized(); - } - - if axis == BoneId(0).axis_a() && other_axis == axis { - // The first joint doesn't have any real connection, so we - // must manually traverse the other side of the root bone. - to_solve.push(( - axis.bone.axis_b(), - Some(self.bones[0].end), - current_rotation, - true, - )); - } else { - to_solve.push((other_axis.inverse(), Some(bone.end), next_rotation, true)); - } - } - } - } -} - -fn next_point(mut point: Point, angle: Rotation, length: f32) -> Point { - point.x += length * angle.radians.sin(); - point.y -= length * angle.radians.cos(); - point -} - -fn determine_end_position( - start: Point, - desired_end: Option>, - angle: Rotation, - bone: &Bone, -) -> (Point, Option>) { - match bone { - Bone::Rigid { length } => (next_point(start, angle, *length), None), - Bone::Jointed { - start_length, - end_length, - inverse, - } => { - if let Some(desired_end) = desired_end { - let delta = desired_end - start; - let full_length = start_length + end_length; - let distance = delta.magnitude(); - 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 desired_angle = Rotation::radians(delta.y.atan2(delta.x) + PI / 2.); - let end = if desired_length != distance { - // We need to cap the end point along this sloped line - next_point(start, desired_angle, desired_length) - } else { - // The end position is valid - desired_end - }; - - let joint = get_third_point( - *inverse, - start, - desired_length, - desired_angle, - *start_length, - *end_length, - ); - - (end, Some(joint)) - } else { - let joint = next_point(start, angle, *start_length); - let end = next_point(joint, angle, *end_length); - (end, Some(joint)) - } - } - } -} - -fn get_third_point( - inverse: bool, - start: Point, - distance: f32, - hyp_angle: Rotation, - first: f32, - second: f32, -) -> Point { - let hyp = distance; - let first_angle = ((first * first + hyp * hyp - second * second) / (2. * first * hyp)).acos(); - if first_angle.is_nan() { - next_point(start, hyp_angle, first) - } else { - let first_angle = hyp_angle - - Rotation { - radians: if inverse { -first_angle } else { first_angle }, - }; - next_point(start, first_angle, first) - } -} - -impl Index for Skeleton { - type Output = SkeletonBone; - - fn index(&self, index: BoneId) -> &Self::Output { - &self.bones[usize::from(index.0)] - } -} - -impl IndexMut for Skeleton { - fn index_mut(&mut self, index: BoneId) -> &mut Self::Output { - &mut self.bones[usize::from(index.0)] - } -} - -impl Index for Skeleton { - type Output = SkeletonJoint; - - fn index(&self, index: JointId) -> &Self::Output { - &self.joints[usize::from(index.0)] - } -} - -impl IndexMut for Skeleton { - fn index_mut(&mut self, index: JointId) -> &mut Self::Output { - &mut self.joints[usize::from(index.0)] - } -} - -#[derive(Debug, Clone, Copy, Eq, PartialEq, Hash)] -pub struct BoneAxis { - pub bone: BoneId, - pub end: BoneEnd, -} - -impl BoneAxis { - pub fn inverse(self) -> Self { - Self { - bone: self.bone, - end: self.end.inverse(), - } - } -} - -#[derive(Debug)] -pub struct SkeletonBone { - generation: usize, - label: &'static str, - kind: Bone, - start: Point, - joint_pos: Option>, - end: Point, - desired_end: Option>, -} - -impl SkeletonBone { - pub fn set_desired_end(&mut self, end: Option>) { - self.desired_end = end; - } -} - -#[derive(Debug)] -pub struct SkeletonJoint { - bone_a: BoneAxis, - bone_b: BoneAxis, - calculated_position: Point, - angle: Rotation, -} - -impl SkeletonJoint { - 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; - } -} - -#[derive(Debug, Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Hash)] -pub struct BoneId(u8); - -impl BoneId { - pub const fn axis_a(self) -> BoneAxis { - BoneAxis { - bone: self, - end: BoneEnd::A, - } - } - - pub const fn axis_b(self) -> BoneAxis { - BoneAxis { - bone: self, - end: BoneEnd::B, - } - } -} - -#[derive(Debug, Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Hash)] -pub struct JointId(u8); - -#[derive(Debug, Clone, Copy, Eq, PartialEq, Hash)] -pub enum BoneEnd { - A, - B, -} - -impl BoneEnd { - pub fn inverse(self) -> Self { - match self { - Self::A => Self::B, - Self::B => Self::A, - } - } -} - -fn main() { - let mut skeleton = Skeleton::default(); - let spine = skeleton.push_bone(Bone::Rigid { length: 3. }, "spine"); - let r_hip = skeleton.push_bone(Bone::Rigid { length: 0.5 }, "r_hip"); - let r_leg = skeleton.push_bone( - Bone::Jointed { - start_length: 1.5, - end_length: 1.5, - inverse: true, - }, - "r_leg", - ); - let r_foot = skeleton.push_bone(Bone::Rigid { length: 0.5 }, "r_foot"); - let l_hip = skeleton.push_bone(Bone::Rigid { length: 0.5 }, "l_hip"); - let l_leg = skeleton.push_bone( - Bone::Jointed { - start_length: 1.5, - end_length: 1.5, - inverse: false, - }, - "l_leg", - ); - let l_foot = skeleton.push_bone(Bone::Rigid { length: 0.5 }, "l_foot"); - let r_shoulder = skeleton.push_bone(Bone::Rigid { length: 0.5 }, "r_shoulder"); - let r_arm = skeleton.push_bone( - Bone::Jointed { - start_length: 1.0, - end_length: 1.0, - inverse: true, - }, - "r_arm", - ); - let r_hand = skeleton.push_bone(Bone::Rigid { length: 0.3 }, "r_hand"); - let l_shoulder = skeleton.push_bone(Bone::Rigid { length: 0.5 }, "l_shoulder"); - let l_arm = skeleton.push_bone( - Bone::Jointed { - start_length: 1.0, - end_length: 1.0, - inverse: false, - }, - "l_arm", - ); - let l_hand = skeleton.push_bone(Bone::Rigid { length: 0.3 }, "l_hand"); - let head = skeleton.push_bone(Bone::Rigid { length: 0.5 }, "head"); - - let neck = skeleton.push_joint(Rotation::degrees(180.), spine.axis_b(), head.axis_a()); - // Create some width for the legs to be spaced. - skeleton.push_joint(Rotation::degrees(90.), spine.axis_a(), l_hip.axis_a()); - skeleton.push_joint(Rotation::degrees(-90.), spine.axis_a(), r_hip.axis_a()); - skeleton.push_joint(Rotation::degrees(90.), l_hip.axis_b(), l_leg.axis_a()); - let l_ankle_id = skeleton.push_joint(Rotation::degrees(-90.), l_leg.axis_b(), l_foot.axis_a()); - skeleton.push_joint(Rotation::degrees(-90.), r_hip.axis_b(), r_leg.axis_a()); - let r_ankle_id = skeleton.push_joint(Rotation::degrees(90.), r_leg.axis_b(), r_foot.axis_a()); - - skeleton.push_joint(Rotation::degrees(90.), spine.axis_b(), l_shoulder.axis_a()); - skeleton.push_joint(Rotation::degrees(-90.), spine.axis_b(), r_shoulder.axis_a()); - let l_arm_socket = - skeleton.push_joint(Rotation::degrees(90.), l_shoulder.axis_b(), l_arm.axis_a()); - let l_wrist_id = skeleton.push_joint(Rotation::degrees(-175.), l_arm.axis_b(), l_hand.axis_a()); - let r_arm_socket = - skeleton.push_joint(Rotation::degrees(-90.), r_shoulder.axis_b(), r_arm.axis_a()); - let r_wrist_id = skeleton.push_joint(Rotation::degrees(175.), r_arm.axis_b(), r_hand.axis_a()); - - let skeleton = Dynamic::new(skeleton); - - Canvas::new({ - let skeleton = skeleton.clone(); - move |context| { - let mut s = skeleton.lock(); - s.prevent_notifications(); - s.solve(); - - let center = Point::from(context.gfx.size().into_signed()) / 2; - - let scale = Px::new(50); - for (bone, color) in [ - (spine, Color::RED), - (r_hip, Color::DARKBLUE), - (r_leg, Color::BLUE), - (r_foot, Color::LIGHTBLUE), - (l_hip, Color::DARKGREEN), - (l_leg, Color::GREEN), - (l_foot, Color::LIGHTGREEN), - (r_shoulder, Color::DARKGOLDENROD), - (r_arm, Color::GOLDENROD), - (r_hand, Color::LIGHTGOLDENRODYELLOW), - (l_shoulder, Color::DARKMAGENTA), - (l_arm, Color::MAGENTA), - (l_hand, Color::LIGHTPINK), - (head, Color::YELLOW), - ] { - let start = s[bone].start.map(|d| scale * d); - let end = s[bone].end.map(|d| scale * d); - if let Some(joint) = s[bone].joint_pos { - let joint = joint.map(|d| scale * d); - context.gfx.draw_shape( - PathBuilder::new(start) - .line_to(joint) - .build() - .stroke(StrokeOptions::px_wide(1).colored(color)) - .translate_by(center), - ); - context.gfx.draw_shape( - PathBuilder::new(joint) - .line_to(end) - .build() - .stroke(StrokeOptions::px_wide(1).colored(color)) - .translate_by(center), - ); - } else { - context.gfx.draw_shape( - PathBuilder::new(start) - .line_to(end) - .build() - .stroke(StrokeOptions::px_wide(1).colored(color)) - .translate_by(center), - ); - } - - if let Some(handle) = s[bone].desired_end { - let handle = handle.map(|d| scale * d); - context.gfx.draw_shape( - Shape::filled_circle(Px::new(3), Color::WHITE, Origin::Center) - .translate_by(handle + center), - ); - } - } - - drop(s); - - context.redraw_when_changed(&skeleton); - } - }) - .expand() - .and( - bone_widget("Lower Left Leg", &skeleton, l_leg, 0.5..=3.0, 0.5..=3.0) - .and(joint_widget("Left Ankle", &skeleton, l_ankle_id)) - .and(bone_widget( - "Lower Right Leg", - &skeleton, - r_leg, - -3.0..=-0.5, - 0.5..=3.0, - )) - .and(joint_widget("Right Ankle", &skeleton, r_ankle_id)) - .and(joint_widget("Left Shoulder", &skeleton, l_arm_socket)) - .and(joint_widget("Left Wrist", &skeleton, l_wrist_id)) - .and(joint_widget("Right Shoulder", &skeleton, r_arm_socket)) - .and(joint_widget("Right Wrist", &skeleton, r_wrist_id)) - .and(joint_widget("Neck", &skeleton, neck)) - .into_rows() - .pad() - .width(Lp::inches(2)), - ) - .into_columns() - .expand() - .run() - .unwrap(); -} - -fn joint_widget(label: &str, skeleton: &Dynamic, joint: JointId) -> impl MakeWidget { - let angle = Dynamic::new(skeleton.read()[joint].angle); - angle - .for_each({ - let skeleton = skeleton.clone(); - move |degrees| { - skeleton.lock()[joint].set_angle(*degrees); - } - }) - .persist(); - let angle_slider = angle.slider_between(Rotation::degrees(-180.), Rotation::degrees(180.)); - - label.and(angle_slider).into_rows().contain() -} - -fn bone_widget( - label: &str, - skeleton: &Dynamic, - bone: BoneId, - x: RangeInclusive, - y: RangeInclusive, -) -> impl MakeWidget { - let bone_y = Dynamic::new(skeleton.lock()[bone].desired_end.unwrap_or_default().y); - let bone_x = Dynamic::new(skeleton.lock()[bone].desired_end.unwrap_or_default().x); - - bone_y - .for_each({ - let skeleton = skeleton.clone(); - move |y| { - let mut skeleton = skeleton.lock(); - let current_end = skeleton[bone].desired_end.unwrap_or_default(); - skeleton[bone].set_desired_end(Some(Point::new(current_end.x, *y))); - } - }) - .persist(); - bone_x - .for_each({ - let skeleton = skeleton.clone(); - move |x| { - let mut skeleton = skeleton.lock(); - let current_end = skeleton[bone].desired_end.unwrap_or_default(); - skeleton[bone].set_desired_end(Some(Point::new(*x, current_end.y))); - } - }) - .persist(); - label - .and(bone_x.slider_between(*x.start(), *x.end())) - .and(bone_y.slider_between(*y.start(), *y.end())) - .into_rows() - .contain() -} - -#[test] -fn rotation() { - assert_eq!( - (Rotation::degrees(90.) + Rotation::degrees(180.)) - .normalized() - .to_degrees() - .round() as i32, - -90, - ); - assert_eq!( - (Rotation::degrees(90.) + Rotation::degrees(-180.)) - .normalized() - .to_degrees() - .round() as i32, - -90, - ); - // assert_eq!( - // (Rotation::degrees(90.) + Rotation::degrees(-180.)) - // .normalized() - // .to_degrees(), - // -90. - // ); -} +#![doc = include_str!("../README.md")] + +use std::{ + collections::{HashMap, HashSet}, + f32::consts::PI, + fmt::{Debug, Display}, + ops::{Add, Index, IndexMut, Neg, Sub}, +}; + +/// A two dimensionsional offset/measurement. +#[derive(Default, Clone, Copy, PartialEq, Debug)] +pub struct Vector { + /// The x-axis component of this vector. + pub x: f32, + /// The y-axis component of this vector. + pub y: f32, +} + +impl Vector { + /// Returns a new vector from the x and y values. + #[must_use] + pub const fn new(x: f32, y: f32) -> Self { + Self { x, y } + } + + /// Returns the magnitude of this vector. + #[must_use] + pub fn magnitude(&self) -> f32 { + (self.x * self.x + self.y * self.y).sqrt() + } + + /// Returns the result of mapping `x` and `y` to `f`. + #[must_use] + pub fn map(self, mut f: impl FnMut(f32) -> f32) -> Self { + Self { + x: f(self.x), + y: f(self.y), + } + } +} + +impl Sub for Vector { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self { + x: self.x - rhs.x, + y: self.y - rhs.y, + } + } +} + +#[cfg(feature = "cushy")] +impl cushy::figures::IntoComponents for Vector { + fn into_components(self) -> (f32, f32) { + (self.x, self.y) + } +} + +#[cfg(feature = "cushy")] +impl cushy::figures::FromComponents for Vector { + fn from_components(components: (f32, f32)) -> Self { + Self::new(components.0, components.1) + } +} + +/// A value representing a rotation between no rotation and a full rotation. +#[derive(Clone, Copy, PartialEq, PartialOrd)] +pub struct Rotation { + radians: f32, +} + +impl Rotation { + /// Returns a rotation representing the given radians. + #[must_use] + pub fn radians(radians: f32) -> Self { + Self { radians }.normalized() + } + + /// Returns a rotation representing the given degrees. + #[must_use] + pub fn degrees(degrees: f32) -> Self { + Self::radians(degrees * PI / 180.0) + } + + /// Returns this rotation represented in degrees. + /// + /// This value will always be greater than or equal to 0 and will always be + /// less than 360.0. + #[must_use] + pub fn to_degrees(self) -> f32 { + self.radians * 180.0 / PI + } + + /// Returns this rotation represented in radians. + /// + /// This value will always be greater than or equal to 0 and will always be + /// less than `2π`. + #[must_use] + pub const fn to_radians(self) -> f32 { + self.radians + } + + fn normalized(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 + } +} + +#[cfg(feature = "cushy")] +impl cushy::animation::PercentBetween for Rotation { + fn percent_between(&self, min: &Self, max: &Self) -> cushy::animation::ZeroToOne { + self.radians.percent_between(&min.radians, &max.radians) + } +} + +#[cfg(feature = "cushy")] +impl cushy::animation::LinearInterpolate for Rotation { + fn lerp(&self, target: &Self, percent: f32) -> Self { + Self { + radians: self.radians.lerp(&target.radians, percent), + } + } +} + +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 Sub for Rotation { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self::radians(self.radians - rhs.radians) + } +} + +impl Neg for Rotation { + type Output = Self; + + fn neg(self) -> Self::Output { + Self::radians(-self.radians) + } +} + +/// A representation of a bone structure inside of a [`Skeleton`]. +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum BoneKind { + /// A single bone of a fixed length. + Rigid { + /// The length of the bone. + length: f32, + }, + /// Two bones connected with a joint that automatically adjusts its angle as + /// needed. + Jointed { + /// The length of the bone connected closest to the root of the + /// skeleton. + start_length: f32, + /// The length of the bone connected furthes from the root of the + /// skeleton. + end_length: f32, + /// The bend of the simulated joint always goes in one of two + /// directions. This boolean toggles which direction the bend goes in. + inverse: bool, + }, +} + +/// A collection of [`Bone`]s. connected by [`Joint`]s. +#[derive(Default, Debug)] +pub struct Skeleton { + initial_joint: Option, + bones: Vec, + joints: Vec, + connections: HashMap>, + generation: usize, +} + +impl Skeleton { + /// Creates a new [`Bone`] into the skeleton. Returns the unique id of the + /// created bone. + /// + /// The first bone pushed is considered the root of the skeleton. All other + /// bones must be connected to the root directly or indirectly through + /// [`Joint`]s. + pub fn push_bone(&mut self, bone: BoneKind, label: &'static str) -> BoneId { + let id = BoneId(u8::try_from(self.bones.len()).expect("too many bones")); + if id == BoneId(0) { + let joint = self.push_joint(Rotation::default(), id.axis_a(), id.axis_a()); + self.initial_joint = Some(joint); + self.connections.insert(id.axis_a(), vec![joint]); + } + self.bones.push(Bone { + generation: self.generation, + label, + kind: bone, + start: Vector::default(), + joint_pos: None, + end: Vector::default(), + desired_end: None, + }); + id + } + + /// Creates a new [`Joint`] in the skeleton, connecting two bones together + /// by their [axis](BoneAxis). Returns the unique id of the created joint. + pub fn push_joint(&mut self, angle: Rotation, bone_a: BoneAxis, bone_b: BoneAxis) -> JointId { + let id = JointId(u8::try_from(self.joints.len()).expect("too many joints")); + self.joints.push(Joint { + bone_a, + bone_b, + angle, + calculated_position: Vector::default(), + }); + self.connections.entry(bone_a).or_default().push(id); + if bone_a != bone_b { + self.connections.entry(bone_b).or_default().push(id); + } + id + } + + /// Sets a translation to be applied to the entire skeleton. + pub fn set_translation(&mut self, translation: Vector) { + let bone = self.bones.first_mut().expect("root bone must be defined"); + bone.start = translation; + } + + /// Returns the translation applied to the entire skeleton. + #[must_use] + pub fn translation(&self) -> Vector { + self.bones.first().expect("root bone must be defined").start + } + + /// Sets a base rotation to apply to the entire skeleton. + pub fn set_rotation(&mut self, rotation: Rotation) { + let joint = self.initial_joint.expect("root bone must be defined"); + let joint = &mut self[joint]; + joint.angle = rotation; + } + + /// Returns the base rotation being applied to the entire skeleton. + #[must_use] + pub fn rotation(&self) -> Rotation { + let joint = self.initial_joint.expect("root bone must be defined"); + self[joint].angle + } + + /// Updates the solved positions of all bones in this skeleton that are + /// connected either directly or indirectly to the root bone via [`Joint`]s. + pub fn solve(&mut self) { + if !self.bones.is_empty() { + self.generation = self.generation.wrapping_add(1); + self.solve_axis(BoneId(0).axis_a()); + } + } + + fn solve_axis(&mut self, axis: BoneAxis) { + let mut axis_solved = HashSet::new(); + let mut to_solve = vec![(axis, None, Rotation::default(), false)]; + while let Some((axis, current_position, current_rotation, inverse_root)) = to_solve.pop() { + if !axis_solved.insert(axis) { + continue; + } + + let Some(connections) = self.connections.get(&axis) else { + continue; + }; + + println!( + "Solving {}:{:?} at {current_position:?} - {current_rotation} - {inverse_root}", + self.bones[usize::from(axis.bone.0)].label, + axis.end + ); + + for joint_id in connections { + let joint = &mut self.joints[usize::from(joint_id.0)]; + let other_axis = joint.other_axis(axis); + let bone = &mut self.bones[usize::from(other_axis.bone.0)]; + if bone.generation == self.generation { + // We store connections in both directions, which means we + // can visit bones twice. We want to ensure we only follow + // each bone a single time. + continue; + } + bone.generation = self.generation; + println!( + " -> {joint_id:?} -> {}:{:?} ({})", + bone.label, other_axis.end, joint.angle + ); + joint.calculated_position = if let Some(current_position) = current_position { + bone.start = current_position; + current_position + } else { + debug_assert_eq!(axis.bone.0, 0); + bone.start + }; + + let angle = if inverse_root { + Rotation::radians(PI) - joint.angle + } else { + joint.angle + }; + + let mut next_rotation = current_rotation + angle; + let (end, mid) = determine_end_position( + joint.calculated_position, + bone.desired_end, + next_rotation, + &bone.kind, + ); + bone.end = end; + bone.joint_pos = mid; + if let Some(mid) = mid { + let final_delta = end - mid; + let rotation = Rotation::radians(final_delta.y.atan2(final_delta.x)); + // TODO I don't know why rotating by 90 degrees fixes + // everything here. It feels like atan2 should be giving us + // the correct rotation, or the correction amount should be + // driven by an input angle, but a fixed correction amount + // seems to be the correct answer. Without this, a joint + // angle of 0 sticks out at a perpendicular angle. + next_rotation = rotation + Rotation::radians(PI / 2.); + } + + if axis == BoneId(0).axis_a() && other_axis == axis { + // The first joint doesn't have any real connection, so we + // must manually traverse the other side of the root bone. + to_solve.push(( + axis.bone.axis_b(), + Some(self.bones[0].end), + current_rotation, + true, + )); + } else { + to_solve.push((other_axis.inverse(), Some(bone.end), next_rotation, true)); + } + } + } + } +} + +fn next_point(mut point: Vector, angle: Rotation, length: f32) -> Vector { + point.x += length * angle.radians.sin(); + point.y -= length * angle.radians.cos(); + point +} + +fn determine_end_position( + start: Vector, + desired_end: Option, + angle: Rotation, + bone: &BoneKind, +) -> (Vector, Option) { + match bone { + BoneKind::Rigid { length } => (next_point(start, angle, *length), None), + BoneKind::Jointed { + start_length, + end_length, + inverse, + } => { + if let Some(desired_end) = desired_end { + let delta = desired_end - start; + let desired_angle = Rotation::radians(delta.y.atan2(delta.x) + PI / 2.); + let distance = delta.magnitude(); + let full_length = start_length + end_length; + let minimum_size = (start_length - end_length).abs(); + let (capped, desired_length) = if distance < minimum_size { + (true, minimum_size) + } else if distance > full_length { + (true, full_length) + } else { + (false, distance) + }; + + let end = if capped { + // We need to cap the end point along this sloped line + next_point(start, desired_angle, desired_length) + } else { + // The end position is valid + desired_end + }; + + let joint = get_third_point( + *inverse, + start, + desired_length, + desired_angle, + *start_length, + *end_length, + ); + + (end, Some(joint)) + } else { + let joint = next_point(start, angle, *start_length); + let end = next_point(joint, angle, *end_length); + (end, Some(joint)) + } + } + } +} + +fn get_third_point( + inverse: bool, + start: Vector, + distance: f32, + hyp_angle: Rotation, + first: f32, + second: f32, +) -> Vector { + let hyp = distance; + let first_angle = ((first * first + hyp * hyp - second * second) / (2. * first * hyp)).acos(); + if first_angle.is_nan() { + next_point(start, hyp_angle, first) + } else { + let first_angle = hyp_angle + - Rotation { + radians: if inverse { -first_angle } else { first_angle }, + }; + next_point(start, first_angle, first) + } +} + +impl Index for Skeleton { + type Output = Bone; + + fn index(&self, index: BoneId) -> &Self::Output { + &self.bones[usize::from(index.0)] + } +} + +impl IndexMut for Skeleton { + fn index_mut(&mut self, index: BoneId) -> &mut Self::Output { + &mut self.bones[usize::from(index.0)] + } +} + +impl Index for Skeleton { + type Output = Joint; + + fn index(&self, index: JointId) -> &Self::Output { + &self.joints[usize::from(index.0)] + } +} + +impl IndexMut for Skeleton { + fn index_mut(&mut self, index: JointId) -> &mut Self::Output { + &mut self.joints[usize::from(index.0)] + } +} + +/// A specific end of a specific bone. +#[derive(Debug, Clone, Copy, Eq, PartialEq, Hash)] +pub struct BoneAxis { + /// The unique id of the bone of this axis. + pub bone: BoneId, + /// The end of the bone being referenced. + pub end: BoneEnd, +} + +impl BoneAxis { + /// Returns the opposite axis on the same bone. + #[must_use] + pub const fn inverse(self) -> Self { + Self { + bone: self.bone, + end: self.end.inverse(), + } + } +} + +/// A bone in a [`Skeleton`]. +#[derive(Debug)] +pub struct Bone { + generation: usize, + label: &'static str, + kind: BoneKind, + start: Vector, + joint_pos: Option, + end: Vector, + desired_end: Option, +} + +impl Bone { + /// Sets the location to aim the end of this bone towards. + /// + /// The end of the bone that is aimed is the end that is furthest from the + /// root of the skeleton. + /// + /// This setting only impacts [`BoneKind::Jointed`] bones. + pub fn set_desired_end(&mut self, end: Option) { + self.desired_end = end; + } + + /// Returns the location this bone is being aimed towards. + #[must_use] + pub const fn desired_end(&self) -> Option { + self.desired_end + } + + /// Returns the solved start position of this bone. + #[must_use] + pub const fn start(&self) -> Vector { + self.start + } + + /// Returns the solved end position of this bone. + #[must_use] + pub const fn end(&self) -> Vector { + self.end + } + + /// If this is a [`BoneKind::Jointed`] bone, returns the solved position of + /// the joint. + #[must_use] + pub const fn solved_joint(&self) -> Option { + self.joint_pos + } + + /// Returns the label this bone was created with. + #[must_use] + pub fn label(&self) -> &str { + self.label + } +} + +/// A connection between two bones. +#[derive(Debug)] +pub struct Joint { + bone_a: BoneAxis, + bone_b: BoneAxis, + calculated_position: Vector, + angle: Rotation, +} + +impl Joint { + /// Given `axis` is one of the two connections in this joint, return the + /// other axis. + /// + /// # Panics + /// + /// This function has a debug assertion that ensures that `axis` is one of + /// the bones in this joint. + #[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 + } + } + + /// Sets the angle to form between these joints. + /// + /// This setting is ignored if the bone furthest from the root of the joint + /// is a [`BoneKind::Jointed`] bone. + pub fn set_angle(&mut self, angle: Rotation) { + self.angle = angle; + } + + /// Returns the rotation of this joint. + #[must_use] + pub const fn angle(&self) -> Rotation { + self.angle + } +} + +/// The unique ID of a [`Bone`] in a [`Skeleton`]. +#[derive(Debug, Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub struct BoneId(u8); + +impl BoneId { + /// Returns the first axis of this bone. + #[must_use] + pub const fn axis_a(self) -> BoneAxis { + BoneAxis { + bone: self, + end: BoneEnd::A, + } + } + + /// Returns the second axis of this bone. + #[must_use] + pub const fn axis_b(self) -> BoneAxis { + BoneAxis { + bone: self, + end: BoneEnd::B, + } + } +} + +/// The unique ID of a [`Joint`] in a [`Skeleton`]. +#[derive(Debug, Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub struct JointId(u8); + +/// A specific end of a [`Bone`]. +#[derive(Debug, Clone, Copy, Eq, PartialEq, Hash)] +pub enum BoneEnd { + /// The first end of a bone. + A, + /// The second end of a bone. + B, +} + +impl BoneEnd { + /// Returns the opposite end of `self`. + #[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.)) + .normalized() + .to_degrees() + .round() as i32, + 270, + ); + assert_eq!( + (Rotation::degrees(90.) + Rotation::degrees(-180.)) + .normalized() + .to_degrees() + .round() as i32, + 270, + ); +}