1#![allow(dead_code)]
4
5use std::f32::consts::PI;
6
7#[allow(dead_code)]
9pub struct JointOrientV2 {
10 pub name: String,
11 pub euler_xyz: [f32; 3],
12 pub quaternion: [f32; 4],
13 pub rotation_order: RotOrder,
14}
15
16#[allow(dead_code)]
17pub enum RotOrder {
18 Xyz,
19 Xzy,
20 Yxz,
21 Yzx,
22 Zxy,
23 Zyx,
24}
25
26#[allow(dead_code)]
27pub struct JointOrientV2Export {
28 pub joints: Vec<JointOrientV2>,
29}
30
31fn euler_to_quat_xyz(e: [f32; 3]) -> [f32; 4] {
32 let (cx, sx) = ((e[0] * 0.5).cos(), (e[0] * 0.5).sin());
33 let (cy, sy) = ((e[1] * 0.5).cos(), (e[1] * 0.5).sin());
34 let (cz, sz) = ((e[2] * 0.5).cos(), (e[2] * 0.5).sin());
35 let w = cx * cy * cz + sx * sy * sz;
36 let x = sx * cy * cz - cx * sy * sz;
37 let y = cx * sy * cz + sx * cy * sz;
38 let z = cx * cy * sz - sx * sy * cz;
39 [x, y, z, w]
40}
41
42#[allow(dead_code)]
43pub fn new_joint_orient_v2_export() -> JointOrientV2Export {
44 JointOrientV2Export { joints: vec![] }
45}
46
47#[allow(dead_code)]
48pub fn add_joint_orient(
49 export: &mut JointOrientV2Export,
50 name: &str,
51 euler_deg: [f32; 3],
52 order: RotOrder,
53) {
54 let euler_rad = [
55 euler_deg[0] * PI / 180.0,
56 euler_deg[1] * PI / 180.0,
57 euler_deg[2] * PI / 180.0,
58 ];
59 let q = euler_to_quat_xyz(euler_rad);
60 export.joints.push(JointOrientV2 {
61 name: name.to_string(),
62 euler_xyz: euler_deg,
63 quaternion: q,
64 rotation_order: order,
65 });
66}
67
68#[allow(dead_code)]
69pub fn joint_orient_count(export: &JointOrientV2Export) -> usize {
70 export.joints.len()
71}
72
73#[allow(dead_code)]
74pub fn find_joint_orient<'a>(
75 export: &'a JointOrientV2Export,
76 name: &str,
77) -> Option<&'a JointOrientV2> {
78 export.joints.iter().find(|j| j.name == name)
79}
80
81#[allow(dead_code)]
82pub fn quaternion_is_unit(q: [f32; 4]) -> bool {
83 let len = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
84 (len - 1.0).abs() < 0.01
85}
86
87#[allow(dead_code)]
88pub fn joint_orient_to_json(j: &JointOrientV2) -> String {
89 format!(
90 "{{\"name\":\"{}\",\"euler\":[{},{},{}],\"quat\":[{},{},{},{}]}}",
91 j.name,
92 j.euler_xyz[0],
93 j.euler_xyz[1],
94 j.euler_xyz[2],
95 j.quaternion[0],
96 j.quaternion[1],
97 j.quaternion[2],
98 j.quaternion[3]
99 )
100}
101
102#[allow(dead_code)]
103pub fn joint_orient_export_to_json(export: &JointOrientV2Export) -> String {
104 format!("{{\"joint_count\":{}}}", export.joints.len())
105}
106
107#[allow(dead_code)]
108pub fn validate_joint_orients(export: &JointOrientV2Export) -> bool {
109 export
110 .joints
111 .iter()
112 .all(|j| !j.name.is_empty() && quaternion_is_unit(j.quaternion))
113}
114
115#[allow(dead_code)]
116pub fn rot_order_name(order: &RotOrder) -> &'static str {
117 match order {
118 RotOrder::Xyz => "xyz",
119 RotOrder::Xzy => "xzy",
120 RotOrder::Yxz => "yxz",
121 RotOrder::Yzx => "yzx",
122 RotOrder::Zxy => "zxy",
123 RotOrder::Zyx => "zyx",
124 }
125}
126
127#[allow(dead_code)]
128pub fn identity_joint_orient(name: &str) -> JointOrientV2 {
129 JointOrientV2 {
130 name: name.to_string(),
131 euler_xyz: [0.0; 3],
132 quaternion: [0.0, 0.0, 0.0, 1.0],
133 rotation_order: RotOrder::Xyz,
134 }
135}
136
137#[cfg(test)]
138mod tests {
139 use super::*;
140
141 #[test]
142 fn test_add_joint_orient() {
143 let mut e = new_joint_orient_v2_export();
144 add_joint_orient(&mut e, "spine", [0.0, 0.0, 0.0], RotOrder::Xyz);
145 assert_eq!(joint_orient_count(&e), 1);
146 }
147
148 #[test]
149 fn test_identity_quaternion_is_unit() {
150 let j = identity_joint_orient("test");
151 assert!(quaternion_is_unit(j.quaternion));
152 }
153
154 #[test]
155 fn test_rotated_quaternion_is_unit() {
156 let mut e = new_joint_orient_v2_export();
157 add_joint_orient(&mut e, "hip", [45.0, 0.0, 0.0], RotOrder::Xyz);
158 let j = find_joint_orient(&e, "hip").expect("should succeed");
159 assert!(quaternion_is_unit(j.quaternion));
160 }
161
162 #[test]
163 fn test_find_joint_found() {
164 let mut e = new_joint_orient_v2_export();
165 add_joint_orient(&mut e, "shoulder", [0.0, 90.0, 0.0], RotOrder::Yxz);
166 assert!(find_joint_orient(&e, "shoulder").is_some());
167 }
168
169 #[test]
170 fn test_find_joint_missing() {
171 let e = new_joint_orient_v2_export();
172 assert!(find_joint_orient(&e, "knee").is_none());
173 }
174
175 #[test]
176 fn test_validate_valid() {
177 let mut e = new_joint_orient_v2_export();
178 add_joint_orient(&mut e, "j1", [0.0, 0.0, 0.0], RotOrder::Xyz);
179 assert!(validate_joint_orients(&e));
180 }
181
182 #[test]
183 fn test_to_json() {
184 let j = identity_joint_orient("test");
185 let json = joint_orient_to_json(&j);
186 assert!(json.contains("test"));
187 }
188
189 #[test]
190 fn test_rot_order_name() {
191 assert_eq!(rot_order_name(&RotOrder::Xyz), "xyz");
192 assert_eq!(rot_order_name(&RotOrder::Zyx), "zyx");
193 }
194
195 #[test]
196 fn test_export_to_json() {
197 let mut e = new_joint_orient_v2_export();
198 add_joint_orient(&mut e, "j1", [0.0; 3], RotOrder::Xyz);
199 let j = joint_orient_export_to_json(&e);
200 assert!(j.contains("joint_count"));
201 }
202
203 #[test]
204 fn test_90deg_x_quat() {
205 let q = euler_to_quat_xyz([PI / 2.0, 0.0, 0.0]);
206 assert!(quaternion_is_unit(q));
207 assert!((q[0] - (PI / 4.0).sin()).abs() < 0.01);
208 }
209}