use crate::na::{Translation3, UnitQuaternion};
use crate::{Iso3, Vector3};
use serde::{Deserialize, Serialize};
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
pub struct XyzWpr {
pub x: f64,
pub y: f64,
pub z: f64,
pub w: f64,
pub p: f64,
pub r: f64,
}
impl XyzWpr {
pub fn new(x: f64, y: f64, z: f64, w: f64, p: f64, r: f64) -> Self {
XyzWpr { x, y, z, w, p, r }
}
pub fn approx_eq(&self, other: &XyzWpr, epsilon: f64) -> bool {
let m0 = Iso3::from(self).to_matrix();
let m1 = Iso3::from(other).to_matrix();
let diff = m0 - m1;
diff.amax() < epsilon
}
pub fn to_array(&self) -> [f64; 6] {
[self.x, self.y, self.z, self.w, self.p, self.r]
}
}
impl std::fmt::Display for XyzWpr {
fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(
f,
"XyzWpr {{ x: {}, y: {}, z: {}, w: {}, p: {}, r: {} }}",
self.x, self.y, self.z, self.w, self.p, self.r
)
}
}
impl From<[f64; 6]> for XyzWpr {
fn from(value: [f64; 6]) -> Self {
XyzWpr::new(value[0], value[1], value[2], value[3], value[4], value[5])
}
}
impl From<&Iso3> for XyzWpr {
fn from(isometry: &Iso3) -> Self {
let translation = isometry.translation.vector;
let rotation = isometry.rotation;
let (w, p, r) = rotation.euler_angles();
XyzWpr::new(
translation.x,
translation.y,
translation.z,
w.to_degrees(),
p.to_degrees(),
r.to_degrees(),
)
}
}
impl From<&XyzWpr> for Iso3 {
fn from(value: &XyzWpr) -> Self {
let translation = Vector3::new(value.x, value.y, value.z);
let rotation = UnitQuaternion::from_euler_angles(
value.w.to_radians(),
value.p.to_radians(),
value.r.to_radians(),
);
Iso3::from_parts(Translation3::from(translation), rotation)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::na::{Matrix4, try_convert};
use approx::assert_relative_eq;
use test_case::test_case;
#[test_case(151.8, 677.1, 277.0, 119.2, -0.2, -179.6)]
#[test_case(451.0, 265.4, -242.9, 179.8, -0.0, 90.1)]
#[test_case(450.9, 62.5, -242.5, -179.9, 0.0, 90.4)]
#[test_case(508.7, -253.6, -197.2, -179.3, -0.4, 82.2)]
#[test_case(-413.8635232282, 263.1863811434, 291.9820746748, -96.8338333769, -35.3450905433, 179.9682178248)]
#[test_case(-149.1668440821, 59.3543506644, 543.2838251943, -120.8798246395, 16.7001441168, 99.0130388728)]
#[test_case(31.0642288750, -26.7743169615, 37.9006405971, -39.6352782596, -2.9214064154, -59.8943255222)]
#[test_case(33.5138781088, 96.1164814542, -300.9032245106, -90.3561134473, 24.0876647389, -5.3733485621)]
#[test_case(77.8658952620, 100.2923343848, -127.6541936118, -171.7171410357, -14.6451288996, 99.8404139441)]
#[test_case(-70.6389144769, 22.3217390074, 18.1798498130, 23.2569815201, 57.7013782986, -140.4311743532)]
#[test_case(604.2872268462, -20.9333741686, 30.4479677368, 114.8155367373, 31.6840511268, -151.5220050073)]
#[test_case(77.4016768659, -28.4720503689, -19.7908480100, 85.1619317507, 53.8517426985, 34.2917400153)]
#[test_case(471.9320306822, -102.6726730677, 588.5120154719, -49.5782300627, -31.1318310910, -84.4135682965)]
#[test_case(-577.6445766741, -216.2517731052, 240.9460890915, -63.0050045001, 75.3203125033, -172.1668871906)]
#[test_case(411.6803610302, 331.0434986878, -361.9951518329, -63.7156683382, 45.0090193165, 75.3251405039)]
#[test_case(5.4890201453, -26.5744088848, -99.6628429743, -179.9170519309, 12.2738553488, -139.1333357015)]
#[test_case(-1.7434467919, -14.7633944063, -79.9344894886, 39.4698348110, 28.9693530939, -103.4665369616)]
#[test_case(-49.7118498703, 20.3765636776, 8.4422995188, -68.5366961539, -27.7591729450, 35.2043221840)]
#[test_case(59.5794643289, 6.7548483372, -88.8341095469, -146.2807856754, 1.0526398756, -16.7799423973)]
fn xyzwpr_round_trip(x: f64, y: f64, z: f64, w: f64, p: f64, r: f64) {
let xyzwpr = XyzWpr::new(x, y, z, w, p, r);
let iso = Iso3::from(&xyzwpr);
let xyzwpr_converted = XyzWpr::from(&iso);
assert_relative_eq!(xyzwpr.x, xyzwpr_converted.x, epsilon = 1e-8);
assert_relative_eq!(xyzwpr.y, xyzwpr_converted.y, epsilon = 1e-8);
assert_relative_eq!(xyzwpr.z, xyzwpr_converted.z, epsilon = 1e-8);
assert_relative_eq!(xyzwpr.w, xyzwpr_converted.w, epsilon = 1e-8);
assert_relative_eq!(xyzwpr.p, xyzwpr_converted.p, epsilon = 1e-8);
assert_relative_eq!(xyzwpr.r, xyzwpr_converted.r, epsilon = 1e-8);
}
#[test_case((-0.9999695385, -0.0003588828, -0.0077970028, 151.8000000000, -0.0069812178, 0.4878690427, 0.8728889161, 677.1000000000, 0.0034906514, 0.8729167591, -0.4878566869, 277.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (151.8000000000, 677.1000000000, 277.0000000000, 119.2000000000, -0.2000000000, -179.6000000000))]
#[test_case((-0.0017453284, 0.9999923846, 0.0034906461, 451.0000000000, 0.9999984769, 0.0017453177, 0.0000060923, 265.4000000000, -0.0000000000, 0.0034906514, -0.9999939077, -242.9000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (451.0000000000, 265.4000000000, -242.9000000000, 179.8000000000, -0.0000000000, 90.1000000000))]
#[test_case((-0.0069812603, 0.9999741077, -0.0017452858, 450.9000000000, 0.9999756307, 0.0069812497, -0.0000121846, 62.5000000000, 0.0000000000, -0.0017453284, -0.9999984769, -242.5000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (450.9000000000, 62.5000000000, -242.5000000000, -179.9000000000, 0.0000000000, 90.4000000000))]
#[test_case((0.1357122651, 0.9906854758, -0.0111565722, 508.7000000000, 0.9907236966, -0.1356209430, 0.0085741896, -253.6000000000, 0.0069812603, -0.0122167031, -0.9999010022, -197.2000000000, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (508.7000000000, -253.6000000000, -197.2000000000, -179.3000000000, -0.4000000000, 82.2000000000))]
#[test_case((-0.8156824504, -0.5743236360, -0.0693866068, -413.8635232282, 0.0004524620, 0.1193088953, -0.9928570807, 263.1863811434, 0.5784997281, -0.8098874913, -0.0970583124, 291.9820746748, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-413.8635232282, 263.1863811434, 291.9820746748, -96.8338333769, -35.3450905433, 179.9682178248))]
#[test_case((-0.1500516217, 0.5455385155, -0.8245436550, -149.1668440821, 0.9459952737, -0.1631791693, -0.2801169415, 59.3543506644, -0.2873629291, -0.8220464019, -0.4915915582, 543.2838251943, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-149.1668440821, 59.3543506644, 543.2838251943, -120.8798246395, 16.7001441168, 99.0130388728))]
#[test_case((0.5009445358, 0.6825401810, 0.5321592557, 31.0642288750, -0.8639774486, 0.3581642754, 0.3539227601, -26.7743169615, 0.0509660705, -0.6370692687, 0.7691197609, 37.9006405971, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (31.0642288750, -26.7743169615, 37.9006405971, -39.6352782596, -2.9214064154, -59.8943255222))]
#[test_case((0.9089103503, -0.4069146210, 0.0911178708, 33.5138781088, -0.0854907806, 0.0320310501, 0.9958239494, 96.1164814542, -0.4081339260, -0.9129044326, -0.0056740956, -300.9032245106, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (33.5138781088, 96.1164814542, -300.9032245106, -90.3561134473, 24.0876647389, -5.3733485621))]
#[test_case((-0.1653518888, 0.9687851465, -0.1847000075, 77.8658952620, 0.9532759246, 0.2050088803, 0.2218927005, 100.2923343848, 0.2528314941, -0.1393796933, -0.9574181619, -127.6541936118, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (77.8658952620, 100.2923343848, -127.6541936118, -171.7171410357, -14.6451288996, 99.8404139441))]
#[test_case((-0.4118951499, 0.3279601759, -0.8501673415, -70.6389144769, -0.3403719856, -0.9208299694, -0.1903131075, 22.3217390074, -0.8452746873, 0.2109841002, 0.4909138545, 18.1798498130, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-70.6389144769, 22.3217390074, 18.1798498130, 23.2569815201, 57.7013782986, -140.4311743532))]
#[test_case((-0.7479917678, -0.6191725989, -0.2390263753, 604.2872268462, -0.4057545090, 0.1415967662, 0.9029471935, -20.9333741686, -0.5252347986, 0.7723830970, -0.3571452893, 30.4479677368, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (604.2872268462, -20.9333741686, 30.4479677368, 114.8155367373, 31.6840511268, -151.5220050073))]
#[test_case((0.4873440333, 0.6172398073, 0.6176656163, 77.4016768659, 0.3323406191, 0.5230063131, -0.7848656633, -28.4720503689, -0.8074933474, 0.5877749712, 0.0497501464, -19.7908480100, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (77.4016768659, -28.4720503689, -19.7908480100, 85.1619317507, 53.8517426985, 34.2917400153))]
#[test_case((0.0833272687, 0.6836449300, 0.7250421891, 471.9320306822, -0.8519144952, -0.3286045615, 0.4077508246, -102.6726730677, 0.5170089546, -0.6516507130, 0.5550253049, 588.5120154719, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (471.9320306822, -102.6726730677, 588.5120154719, -49.5782300627, -31.1318310910, -84.4135682965))]
#[test_case((-0.2510504585, 0.9157805028, -0.3135597837, -577.6445766741, -0.0345374583, -0.3322023607, -0.9425755967, -216.2517731052, -0.9673576543, -0.2258044778, 0.1150282859, 240.9460890915, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-577.6445766741, -216.2517731052, 240.9460890915, -63.0050045001, 75.3203125033, -172.1668871906))]
#[test_case((0.1791056335, -0.5890186234, -0.7880217213, 411.6803610302, 0.6839325663, -0.5012295385, 0.5300992308, 331.0434986878, -0.7072180829, -0.6338974767, 0.3130759848, -361.9951518329, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (411.6803610302, 331.0434986878, -361.9951518329, -63.7156683382, 45.0090193165, 75.3251405039))]
#[test_case((-0.7389487926, -0.6540675080, 0.1617107816, 5.4890201453, -0.6393453686, 0.7564348576, 0.1379992968, -26.5744088848, -0.2125845282, -0.0014146255, -0.9771416567, -99.6628429743, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (5.4890201453, -26.5744088848, -99.6628429743, -179.9170519309, 12.2738553488, -139.1333357015))]
#[test_case((-0.2037395422, 0.6790363200, -0.7052658187, -1.7434467919, -0.8508250650, -0.4791894336, -0.2155787453, -14.7633944063, -0.4843417261, 0.5561359212, 0.6753709570, -79.9344894886, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-1.7434467919, -14.7633944063, -79.9344894886, 39.4698348110, 28.9693530939, -103.4665369616))]
#[test_case((0.7230637290, 0.1432361537, -0.6757678952, -49.7118498703, 0.5101470443, 0.5488670300, 0.6621895322, 20.3765636776, 0.4657561992, -0.8235462268, 0.3237943410, 8.4422995188, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (-49.7118498703, 20.3765636776, 8.4422995188, -68.5366961539, -27.7591729450, 35.2043221840))]
#[test_case((0.9572590453, -0.2498925755, 0.1456324858, 59.5794643289, -0.2886479285, -0.7934076632, 0.5358979878, 6.7548483372, -0.0183709982, -0.5550297115, -0.8316276365, -88.8341095469, 0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000), (59.5794643289, 6.7548483372, -88.8341095469, -146.2807856754, 1.0526398756, -16.7799423973))]
#[allow(clippy::type_complexity)]
fn xyzwpr_to_isometry(
mf: (
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
f64,
),
pf: (f64, f64, f64, f64, f64, f64),
) {
let m = Matrix4::new(
mf.0, mf.1, mf.2, mf.3, mf.4, mf.5, mf.6, mf.7, mf.8, mf.9, mf.10, mf.11, mf.12, mf.13,
mf.14, mf.15,
);
let p = XyzWpr::new(pf.0, pf.1, pf.2, pf.3, pf.4, pf.5);
let t = Iso3::from(&p);
let m_ = t.to_matrix();
assert_relative_eq!(m, m_, epsilon = 1e-8);
}
#[test]
fn xyzwpr_to_isometry_single() {
let x = XyzWpr::new(
151.8000000000,
677.1000000000,
277.0000000000,
119.2000000000,
-0.2000000000,
-179.6000000000,
);
let e = Matrix4::new(
-0.9999695385116514,
-0.0003588828397861686,
-0.007797002751219406,
151.8,
-0.006981217765734895,
0.48786904270864234,
0.8728889160512124,
677.1,
0.003490651415223732,
0.8729167591297927,
-0.4878566869307391,
277.0,
0.0,
0.0,
0.0,
1.0,
);
let m = Iso3::from(&x).to_matrix();
assert_relative_eq!(m, e, epsilon = 1e-8);
}
#[test]
fn isometry_to_xyzwpr_single() {
let _x = XyzWpr::new(
151.8000000000,
677.1000000000,
277.0000000000,
119.2000000000,
-0.2000000000,
-179.6000000000,
);
let m = Matrix4::new(
-0.9999695385116514,
-0.0003588828397861686,
-0.007797002751219406,
151.8,
-0.006981217765734895,
0.48786904270864234,
0.8728889160512124,
677.1,
0.003490651415223732,
0.8729167591297927,
-0.4878566869307391,
277.0,
0.0,
0.0,
0.0,
1.0,
);
let iso: Iso3 = try_convert(m).unwrap();
let p = XyzWpr::from(&iso);
let pf = (
151.8000000000,
677.1000000000,
277.0000000000,
119.2000000000,
-0.2000000000,
-179.6000000000,
);
assert_relative_eq!(p.x, pf.0, epsilon = 1e-5);
assert_relative_eq!(p.y, pf.1, epsilon = 1e-5);
assert_relative_eq!(p.z, pf.2, epsilon = 1e-5);
assert_relative_eq!(p.w, pf.3, epsilon = 1e-5);
assert_relative_eq!(p.p, pf.4, epsilon = 1e-5);
assert_relative_eq!(p.r, pf.5, epsilon = 1e-5);
}
}