angle to cartesian conversion

This is a first step towards fixing how `right` and `forward` combine
into a 3D angle. Currently, it is quite broken.
Actual rotation matrix multiplication follows later with its own tests.

bouncy_instructor/src/intern/geom/angle3d.rs

@@ -1,4 +1,5 @@
 use super::SignedAngle;
+use crate::keypoints::Cartesian3d;
 
 /// A direction in 3D space.
 #[derive(Clone, Copy, PartialEq, Debug)]
@@ -14,6 +15,25 @@ impl Angle3d {
         Self { azimuth, polar }
     }
 
+    /// Combines two rotations to one and returns the spherical coordinate of the final result.
+    ///
+    /// The combination is done rotationally, first applying the
+    /// forward angles (pitch) and then the sideward angle (yaw).
+    pub(crate) fn from_rotations(forward: SignedAngle, right: SignedAngle) -> Self {
+        let right = *right;
+        let forward = *forward;
+
+        // The multiplication of the rotation matrices, multiplied by (0,0,1) simplifies to this.
+        let cartesian = Cartesian3d::new(
+            -forward.cos() * right.sin(),
+            forward.cos() * right.cos(),
+            -forward.sin(),
+        );
+        // Now use cartesian to spherical conversion
+        // (note: could these two steps be combined for better precision and performance?)
+        Self::from(cartesian)
+    }
+
     pub(crate) const ZERO: Self = Angle3d {
         azimuth: SignedAngle::ZERO,
         polar: SignedAngle::ZERO,
@@ -52,3 +72,35 @@ impl Angle3d {
         }
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::test_utils::assert_angle_3d_eq;
+
+    #[test]
+    fn test_combine_angles() {
+        // check_combine_angle((forward, right), (azimuth, polar));
+        check_combine_angle((90.0, 0.0), (0.0, 90.0));
+        check_combine_angle((120.0, 0.0), (0.0, 120.0));
+        check_combine_angle((30.0, 0.0), (0.0, 30.0));
+        check_combine_angle((-30.0, 0.0), (180.0, 30.0));
+        check_combine_angle((0.0, 90.0), (90.0, 90.0));
+        check_combine_angle((0.0, -90.0), (270.0, 90.0));
+        check_combine_angle((0.0, 45.0), (90.0, 45.0));
+        check_combine_angle((0.1, 45.0), (89.86, 45.0));
+        check_combine_angle((45.0, 90.0), (45.0, 90.0));
+        check_combine_angle((45.0, 45.0), (35.26, 60.0));
+        check_combine_angle((45.0, 135.0), (35.26, 120.0));
+        check_combine_angle((45.0, -45.0), (-35.26, 60.0));
+        check_combine_angle((45.0, 180.0), (0.0, 135.0));
+    }
+
+    #[track_caller]
+    fn check_combine_angle((forward, right): (f32, f32), (azimuth, polar): (f32, f32)) {
+        let expected = Angle3d::degree(azimuth, polar);
+        let angle =
+            Angle3d::from_rotations(SignedAngle::degree(forward), SignedAngle::degree(right));
+        assert_angle_3d_eq(expected, angle);
+    }
+}

bouncy_instructor/src/intern/geom/cartesian.rs

@@ -1,7 +1,13 @@
-use super::SignedAngle;
+use super::{Angle3d, SignedAngle};
 use crate::keypoints::Cartesian3d;
 
 impl Cartesian3d {
+    const ZERO: Self = Cartesian3d {
+        x: 0.0,
+        y: 0.0,
+        z: 0.0,
+    };
+
     /// The polar angle is measured against the y-axis, which goes from the
     /// ground to the sky.
     ///
@@ -57,12 +63,74 @@ impl Cartesian3d {
         // note 2: what about Math.acos() instead of wasm ?
         SignedAngle(dx.signum() * (dz / r).acos())
     }
+
+    #[allow(dead_code)]
+    pub(crate) fn length(&self) -> f32 {
+        (self.x.powi(2) + self.y.powi(2) + self.z.powi(2)).sqrt()
+    }
+}
+
+impl From<Cartesian3d> for Angle3d {
+    fn from(p: Cartesian3d) -> Self {
+        Self::new(
+            Cartesian3d::ZERO.azimuth(p),
+            Cartesian3d::ZERO.polar_angle(p),
+        )
+    }
+}
+
+impl From<Angle3d> for Cartesian3d {
+    fn from(angle: Angle3d) -> Self {
+        let x = -angle.polar.sin() * angle.azimuth.sin();
+        let y = angle.polar.cos();
+        let z = -angle.polar.sin() * angle.azimuth.cos();
+        Self { x, y, z }
+    }
+}
+
+impl std::ops::Add<Cartesian3d> for Cartesian3d {
+    type Output = Self;
+
+    fn add(self, rhs: Cartesian3d) -> Self::Output {
+        Self {
+            x: self.x + rhs.x,
+            y: self.y + rhs.y,
+            z: self.z + rhs.z,
+        }
+    }
+}
+
+impl std::ops::Sub<Cartesian3d> for Cartesian3d {
+    type Output = Self;
+
+    fn sub(self, rhs: Cartesian3d) -> Self::Output {
+        Self {
+            x: self.x - rhs.x,
+            y: self.y - rhs.y,
+            z: self.z - rhs.z,
+        }
+    }
+}
+
+impl std::ops::Mul<f32> for Cartesian3d {
+    type Output = Self;
+
+    fn mul(self, rhs: f32) -> Self::Output {
+        Self {
+            x: self.x * rhs,
+            y: self.y * rhs,
+            z: self.z * rhs,
+        }
+    }
 }
 
 #[cfg(test)]
 mod tests {
+    use std::f32::consts::FRAC_1_SQRT_2;
+
     use super::*;
-    use crate::test_utils::assert_angle_eq;
+    use crate::intern::geom::Angle3d;
+    use crate::test_utils::{assert_angle_eq, assert_cartesian_eq};
 
     #[test]
     fn test_cartesian_to_angle() {
@@ -100,4 +168,48 @@ mod tests {
             origin.polar_angle(cartesian),
         );
     }
+
+    #[test]
+    fn test_angle_to_cartesian() {
+        // azimuth, polar, (x,y,z)
+        check_angle_to_cartesian(0.0, 0.0, (0.0, 1.0, 0.0));
+        check_angle_to_cartesian(-90.0, 90.0, (1.0, 0.0, 0.0));
+        check_angle_to_cartesian(90.0, 90.0, (-1.0, 0.0, 0.0));
+        check_angle_to_cartesian(0.0, 180.0, (0.0, -1.0, 0.0));
+        check_angle_to_cartesian(180.0, 90.0, (0.0, 0.0, 1.0));
+        check_angle_to_cartesian(0.0, 90.0, (0.0, 0.0, -1.0));
+        check_angle_to_cartesian(0.0, 45.0, (0.0, FRAC_1_SQRT_2, -FRAC_1_SQRT_2));
+    }
+
+    #[track_caller]
+    fn check_angle_to_cartesian(azimuth: f32, polar: f32, expected_cartesian: (f32, f32, f32)) {
+        let (x, y, z) = expected_cartesian;
+        let want = Cartesian3d::new(x, y, z);
+
+        let angle = Angle3d::degree(azimuth, polar);
+        let actual = Cartesian3d::from(angle);
+        assert_cartesian_eq(want, actual);
+    }
+
+    #[test]
+    fn test_cartesian_to_angle_and_back() {
+        check_cartesian_to_angle_and_back(1.0, 0.0, 0.0);
+        check_cartesian_to_angle_and_back(0.0, 1.0, 0.0);
+        check_cartesian_to_angle_and_back(0.0, 0.0, 1.0);
+        check_cartesian_to_angle_and_back(1.0, 1.0, 1.0);
+        check_cartesian_to_angle_and_back(1.0, -1.0, 1.0);
+        check_cartesian_to_angle_and_back(0.0, 1.0, -1.0);
+        check_cartesian_to_angle_and_back(-3.2, 7.1, -1.1);
+        check_cartesian_to_angle_and_back(0.0, 0.0, 0.0);
+    }
+
+    #[track_caller]
+    fn check_cartesian_to_angle_and_back(x: f32, y: f32, z: f32) {
+        let start = Cartesian3d::new(x, y, z);
+        let origin = Cartesian3d::new(0.0, 0.0, 0.0);
+        let angle = Angle3d::new(origin.azimuth(start), origin.polar_angle(start));
+
+        let end = Cartesian3d::from(angle) * start.length();
+        assert_cartesian_eq(start, end);
+    }
 }

bouncy_instructor/src/intern/geom/signed_angle.rs

@@ -29,6 +29,7 @@ impl SignedAngle {
         self
     }
 
+    #[allow(dead_code)]
     pub(crate) fn abs(mut self) -> Self {
         self.0 = self.0.abs();
         self

bouncy_instructor/src/intern/pose_db.rs

@@ -39,6 +39,7 @@ pub(crate) struct LimbPositionDatabase {
 #[derive(Clone, Copy)]
 pub(crate) struct LimbIndex(usize);
 
+#[derive(Debug)]
 pub(crate) enum AddPoseError {
     MissingMirror(String),
 }

bouncy_instructor/src/intern/pose_score.rs

@@ -73,6 +73,10 @@ impl AngleTarget {
         self.weight
     }
 
+    pub(crate) fn angle(&self) -> Angle3d {
+        self.angle
+    }
+
     /// Mirrors left/right, doesn't affect up/down or forward/backward
     pub(crate) fn x_mirror(&self) -> AngleTarget {
         Self {

bouncy_instructor/src/intern/skeleton_3d.rs

@@ -26,6 +26,13 @@ pub(crate) struct Skeleton3d {
 }
 
 impl Skeleton3d {
+    pub(crate) fn new(limb_angles: Vec<Angle3d>, azimuth_correction: SignedAngle) -> Self {
+        Self {
+            limb_angles,
+            azimuth_correction,
+        }
+    }
+
     pub(crate) fn from_keypoints(kp: &Keypoints) -> Self {
         let mut limb_angles = STATE.with(|state| {
             state
@@ -35,7 +42,7 @@ impl Skeleton3d {
                 .map(|(_index, limb)| limb.to_angle(kp))
                 .collect::<Vec<_>>()
         });
-        // Shoulder defines where he person is looking
+        // Shoulder defines where the person is looking
         let shoulder_angle = kp.left.shoulder.azimuth(kp.right.shoulder);
 
         // Rotate skelton to face north.
@@ -44,10 +51,7 @@ impl Skeleton3d {
             angle.azimuth = angle.azimuth - azimuth_correction;
         }
 
-        Self {
-            limb_angles,
-            azimuth_correction,
-        }
+        Self::new(limb_angles, azimuth_correction)
     }
 
     pub(crate) fn angles(&self) -> &[Angle3d] {

bouncy_instructor/src/public/pose_file.rs

@@ -22,7 +22,7 @@ pub(crate) struct PoseFile {
 /// This includes the exact desired position range and a name.
 /// This is the format for external files and loaded in at runtime.
 /// It is converted to a [`crate::pose::Pose`] for computations.
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug, PartialEq)]
 pub(crate) struct Pose {
     pub name: String,
     #[serde(default, skip_serializing_if = "Vec::is_empty")]
@@ -32,7 +32,7 @@ pub(crate) struct Pose {
 }
 
 /// Describes a desired angle of a limb defined by start and end point.
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug, PartialEq, Clone)]
 pub(crate) struct LimbPosition {
     pub limb: Limb,
     pub weight: f32,
@@ -53,7 +53,7 @@ pub(crate) struct LimbPosition {
     pub tolerance: u8,
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Clone)]
 pub(crate) struct BodyPoint {
     pub side: BodySide,
     pub part: BodyPart,
@@ -63,7 +63,7 @@ pub(crate) struct BodyPoint {
 ///
 /// Custom points are maximally expressive but also verbose. Any limb that's
 /// used frequently should probably be included in the pre-defined list.
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Clone)]
 pub(crate) enum Limb {
     /// knee to ankle
     LeftShin,
@@ -95,13 +95,13 @@ pub(crate) enum Limb {
     },
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Clone)]
 pub(crate) enum BodySide {
     Left,
     Right,
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Clone)]
 pub(crate) enum BodyPart {
     Shoulder,
     Hip,
@@ -119,7 +119,6 @@ pub enum ParseFileError {
     VersionMismatch { expected: u16, found: u16 },
     #[error("parsing pose file failed, {0}")]
     RonError(#[from] ron::error::SpannedError),
-    // TODO: unit test
     #[error("unknown pose reference `{0}`")]
     UnknownPoseReference(String),
 }

bouncy_instructor/src/test_utils.rs

@@ -1,6 +1,7 @@
 //! Utilities for unit tests.
 
-use crate::intern::geom::{SignedAngle, Angle3d};
+use crate::intern::geom::{Angle3d, SignedAngle};
+use crate::keypoints::Cartesian3d;
 
 #[track_caller]
 pub(crate) fn assert_float_angle_eq(expected: f32, actual: SignedAngle) {
@@ -27,8 +28,13 @@ pub(crate) fn assert_angle_3d_eq(expected: Angle3d, actual: Angle3d) {
     assert_angle_eq(expected.polar, actual.polar);
 }
 
-
 #[track_caller]
+pub(crate) fn assert_cartesian_eq(expected: Cartesian3d, actual: Cartesian3d) {
+    assert!(float_eq(expected.x, actual.x), "{expected:?} == {actual:?}");
+    assert!(float_eq(expected.y, actual.y), "{expected:?} == {actual:?}");
+    assert!(float_eq(expected.z, actual.z), "{expected:?} == {actual:?}");
+}
+
 pub(crate) fn float_eq(expected: f32, actual: f32) -> bool {
     // first try strict equality
     if expected == actual {
@@ -45,4 +51,4 @@ pub(crate) fn float_eq(expected: f32, actual: f32) -> bool {
     }
     // fall back to absolute tolerance if expectation is 0.0
     actual.abs() < 1e-6
-}
+}