scirs2_spatial/transform/rotation.rs
1//! Rotation class for 3D rotations
2//!
3//! This module provides a `Rotation` class that supports various rotation representations,
4//! including quaternions, rotation matrices, Euler angles, and axis-angle representations.
5
6use crate::error::{SpatialError, SpatialResult};
7use scirs2_core::ndarray::{array, Array1, Array2, ArrayView1, ArrayView2};
8use scirs2_core::numeric::Float;
9use std::f64::consts::PI;
10use std::fmt;
11
12/// Supported Euler angle conventions
13#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
14pub enum EulerConvention {
15 /// Intrinsic rotation around X, then Y, then Z
16 Xyz,
17 /// Intrinsic rotation around Z, then Y, then X
18 Zyx,
19 /// Intrinsic rotation around X, then Y, then X
20 Xyx,
21 /// Intrinsic rotation around X, then Z, then X
22 Xzx,
23 /// Intrinsic rotation around Y, then X, then Y
24 Yxy,
25 /// Intrinsic rotation around Y, then Z, then Y
26 Yzy,
27 /// Intrinsic rotation around Z, then X, then Z
28 Zxz,
29 /// Intrinsic rotation around Z, then Y, then Z
30 Zyz,
31}
32
33impl fmt::Display for EulerConvention {
34 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
35 match self {
36 EulerConvention::Xyz => write!(f, "xyz"),
37 EulerConvention::Zyx => write!(f, "zyx"),
38 EulerConvention::Xyx => write!(f, "xyx"),
39 EulerConvention::Xzx => write!(f, "xzx"),
40 EulerConvention::Yxy => write!(f, "yxy"),
41 EulerConvention::Yzy => write!(f, "yzy"),
42 EulerConvention::Zxz => write!(f, "zxz"),
43 EulerConvention::Zyz => write!(f, "zyz"),
44 }
45 }
46}
47
48impl EulerConvention {
49 /// Parse a string into an EulerConvention
50 pub fn from_str(s: &str) -> SpatialResult<Self> {
51 match s.to_lowercase().as_str() {
52 "xyz" => Ok(EulerConvention::Xyz),
53 "zyx" => Ok(EulerConvention::Zyx),
54 "xyx" => Ok(EulerConvention::Xyx),
55 "xzx" => Ok(EulerConvention::Xzx),
56 "yxy" => Ok(EulerConvention::Yxy),
57 "yzy" => Ok(EulerConvention::Yzy),
58 "zxz" => Ok(EulerConvention::Zxz),
59 "zyz" => Ok(EulerConvention::Zyz),
60 _ => Err(SpatialError::ValueError(format!(
61 "Invalid Euler convention: {s}"
62 ))),
63 }
64 }
65}
66
67/// Rotation representation for 3D rotations
68///
69/// This class provides a convenient way to represent and manipulate 3D rotations.
70/// It supports multiple representations including quaternions, rotation matrices,
71/// Euler angles (in various conventions), and axis-angle representation.
72///
73/// Rotations can be composed, inverted, and applied to vectors.
74///
75/// # Examples
76///
77/// ```
78/// use scirs2_spatial::transform::Rotation;
79/// use scirs2_core::ndarray::array;
80///
81/// // Create a rotation from a quaternion [w, x, y, z]
82/// let quat = array![std::f64::consts::FRAC_1_SQRT_2, std::f64::consts::FRAC_1_SQRT_2, 0.0, 0.0]; // 90 degree rotation around X
83/// let rot = Rotation::from_quat(&quat.view()).unwrap();
84///
85/// // Apply the rotation to a vector
86/// let vec = array![0.0, 1.0, 0.0];
87/// let rotated = rot.apply(&vec.view()).unwrap();
88///
89/// // Verify the result (should be approximately [0, 0, 1])
90/// assert!((rotated[0]).abs() < 1e-10);
91/// assert!((rotated[1]).abs() < 1e-10);
92/// assert!((rotated[2] - 1.0).abs() < 1e-10);
93///
94/// // Convert to other representations
95/// let matrix = rot.as_matrix();
96/// let euler = rot.as_euler("xyz").unwrap();
97/// let axis_angle = rot.as_rotvec();
98/// ```
99#[derive(Clone, Debug)]
100pub struct Rotation {
101 /// Quaternion representation [w, x, y, z] where w is the scalar part
102 quat: Array1<f64>,
103}
104
105// Helper function to create an array from values
106#[allow(dead_code)]
107fn euler_array(x: f64, y: f64, z: f64) -> Array1<f64> {
108 array![x, y, z]
109}
110
111// Helper function to create a rotation from Euler angles
112#[allow(dead_code)]
113fn rotation_from_euler(x: f64, y: f64, z: f64, convention: &str) -> SpatialResult<Rotation> {
114 let angles = euler_array(x, y, z);
115 let angles_view = angles.view();
116 Rotation::from_euler(&angles_view, convention)
117}
118
119impl Rotation {
120 /// Create a new rotation from a quaternion [w, x, y, z]
121 ///
122 /// # Arguments
123 ///
124 /// * `quat` - A quaternion in the format [w, x, y, z] where w is the scalar part
125 ///
126 /// # Returns
127 ///
128 /// A `SpatialResult` containing the rotation if valid, or an error if the quaternion is invalid
129 ///
130 /// # Examples
131 ///
132 /// ```
133 /// use scirs2_spatial::transform::Rotation;
134 /// use scirs2_core::ndarray::array;
135 ///
136 /// // Create a quaternion for a 90-degree rotation around the x-axis
137 /// let quat = array![std::f64::consts::FRAC_1_SQRT_2, std::f64::consts::FRAC_1_SQRT_2, 0.0, 0.0];
138 /// let rot = Rotation::from_quat(&quat.view()).unwrap();
139 /// ```
140 pub fn from_quat(quat: &ArrayView1<f64>) -> SpatialResult<Self> {
141 if quat.len() != 4 {
142 return Err(SpatialError::DimensionError(format!(
143 "Quaternion must have 4 elements, got {}",
144 quat.len()
145 )));
146 }
147
148 // Normalize the quaternion
149 let norm = (quat.iter().map(|&x| x * x).sum::<f64>()).sqrt();
150
151 if norm < 1e-10 {
152 return Err(SpatialError::ComputationError(
153 "Quaternion has near-zero norm".into(),
154 ));
155 }
156
157 let normalized_quat = quat.to_owned() / norm;
158
159 Ok(Rotation {
160 quat: normalized_quat,
161 })
162 }
163
164 /// Create a rotation from a rotation matrix
165 ///
166 /// # Arguments
167 ///
168 /// * `matrix` - A 3x3 orthogonal rotation matrix
169 ///
170 /// # Returns
171 ///
172 /// A `SpatialResult` containing the rotation if valid, or an error if the matrix is invalid
173 ///
174 /// # Examples
175 ///
176 /// ```
177 /// use scirs2_spatial::transform::Rotation;
178 /// use scirs2_core::ndarray::array;
179 ///
180 /// // Create a rotation matrix for a 90-degree rotation around the z-axis
181 /// let matrix = array![
182 /// [0.0, -1.0, 0.0],
183 /// [1.0, 0.0, 0.0],
184 /// [0.0, 0.0, 1.0]
185 /// ];
186 /// let rot = Rotation::from_matrix(&matrix.view()).unwrap();
187 /// ```
188 pub fn from_matrix(matrix: &ArrayView2<'_, f64>) -> SpatialResult<Self> {
189 if matrix.shape() != [3, 3] {
190 return Err(SpatialError::DimensionError(format!(
191 "Matrix must be 3x3, got {:?}",
192 matrix.shape()
193 )));
194 }
195
196 // Check if the matrix is approximately orthogonal
197 let det = matrix[[0, 0]]
198 * (matrix[[1, 1]] * matrix[[2, 2]] - matrix[[1, 2]] * matrix[[2, 1]])
199 - matrix[[0, 1]] * (matrix[[1, 0]] * matrix[[2, 2]] - matrix[[1, 2]] * matrix[[2, 0]])
200 + matrix[[0, 2]] * (matrix[[1, 0]] * matrix[[2, 1]] - matrix[[1, 1]] * matrix[[2, 0]]);
201
202 if (det - 1.0).abs() > 1e-6 {
203 return Err(SpatialError::ValueError(format!(
204 "Matrix is not orthogonal, determinant = {det}"
205 )));
206 }
207
208 // Convert rotation matrix to quaternion using method from:
209 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
210
211 let mut quat = Array1::zeros(4);
212 let m = matrix;
213
214 let trace = m[[0, 0]] + m[[1, 1]] + m[[2, 2]];
215
216 if trace > 0.0 {
217 let s = 0.5 / (trace + 1.0).sqrt();
218 quat[0] = 0.25 / s;
219 quat[1] = (m[[2, 1]] - m[[1, 2]]) * s;
220 quat[2] = (m[[0, 2]] - m[[2, 0]]) * s;
221 quat[3] = (m[[1, 0]] - m[[0, 1]]) * s;
222 } else if m[[0, 0]] > m[[1, 1]] && m[[0, 0]] > m[[2, 2]] {
223 let s = 2.0 * (1.0 + m[[0, 0]] - m[[1, 1]] - m[[2, 2]]).sqrt();
224 quat[0] = (m[[2, 1]] - m[[1, 2]]) / s;
225 quat[1] = 0.25 * s;
226 quat[2] = (m[[0, 1]] + m[[1, 0]]) / s;
227 quat[3] = (m[[0, 2]] + m[[2, 0]]) / s;
228 } else if m[[1, 1]] > m[[2, 2]] {
229 let s = 2.0 * (1.0 + m[[1, 1]] - m[[0, 0]] - m[[2, 2]]).sqrt();
230 quat[0] = (m[[0, 2]] - m[[2, 0]]) / s;
231 quat[1] = (m[[0, 1]] + m[[1, 0]]) / s;
232 quat[2] = 0.25 * s;
233 quat[3] = (m[[1, 2]] + m[[2, 1]]) / s;
234 } else {
235 let s = 2.0 * (1.0 + m[[2, 2]] - m[[0, 0]] - m[[1, 1]]).sqrt();
236 quat[0] = (m[[1, 0]] - m[[0, 1]]) / s;
237 quat[1] = (m[[0, 2]] + m[[2, 0]]) / s;
238 quat[2] = (m[[1, 2]] + m[[2, 1]]) / s;
239 quat[3] = 0.25 * s;
240 }
241
242 // Make w the first element
243 quat = array![quat[0], quat[1], quat[2], quat[3]];
244
245 // Normalize the quaternion
246 let norm = (quat.iter().map(|&x| x * x).sum::<f64>()).sqrt();
247 quat /= norm;
248
249 Ok(Rotation { quat })
250 }
251
252 /// Create a rotation from Euler angles
253 ///
254 /// # Arguments
255 ///
256 /// * `angles` - A 3-element array of Euler angles (in radians)
257 /// * `convention` - The Euler angle convention (e.g., "xyz", "zyx")
258 ///
259 /// # Returns
260 ///
261 /// A `SpatialResult` containing the rotation if valid, or an error if the angles are invalid
262 ///
263 /// # Examples
264 ///
265 /// ```
266 /// use scirs2_spatial::transform::Rotation;
267 /// use scirs2_core::ndarray::array;
268 /// use std::f64::consts::PI;
269 ///
270 /// // Create a rotation using Euler angles in the XYZ convention
271 /// let angles = array![PI/2.0, 0.0, 0.0]; // 90 degrees around X
272 /// let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
273 /// ```
274 pub fn from_euler(angles: &ArrayView1<f64>, convention: &str) -> SpatialResult<Self> {
275 if angles.len() != 3 {
276 return Err(SpatialError::DimensionError(format!(
277 "Euler _angles must have 3 elements, got {}",
278 angles.len()
279 )));
280 }
281
282 let conv = EulerConvention::from_str(convention)?;
283 let mut quat = Array1::zeros(4);
284
285 // Compute quaternions for individual rotations
286 let _angles = angles.as_slice().unwrap();
287 let a = angles[0] / 2.0;
288 let b = angles[1] / 2.0;
289 let c = angles[2] / 2.0;
290
291 let ca = a.cos();
292 let sa = a.sin();
293 let cb = b.cos();
294 let sb = b.sin();
295 let cc = c.cos();
296 let sc = c.sin();
297
298 // Construct quaternion based on convention
299 match conv {
300 EulerConvention::Xyz => {
301 // Intrinsic rotation around X, then Y, then Z
302 // Quaternion multiplication order: Qz * Qy * Qx
303 quat[0] = cc * cb * ca + sc * sb * sa;
304 quat[1] = cc * cb * sa - sc * sb * ca;
305 quat[2] = cc * sb * ca + sc * cb * sa;
306 quat[3] = sc * cb * ca - cc * sb * sa;
307 }
308 EulerConvention::Zyx => {
309 // Intrinsic rotation around Z, then Y, then X
310 // For ZYX: angles[0] = Z, angles[1] = Y, angles[2] = X
311 // So: ca = cos(Z/2), sa = sin(Z/2)
312 // cb = cos(Y/2), sb = sin(Y/2)
313 // cc = cos(X/2), sc = sin(X/2)
314 // Quaternion multiplication order: Qx * Qy * Qz
315 // Formula: Qx(cc,sc) * Qy(cb,sb) * Qz(ca,sa)
316 quat[0] = cc * cb * ca - sc * sb * sa;
317 quat[1] = cc * sb * sa + sc * cb * ca;
318 quat[2] = cc * sb * ca - sc * cb * sa;
319 quat[3] = cc * cb * sa + sc * sb * ca;
320 }
321 EulerConvention::Xyx => {
322 quat[0] = ca * cb * cc - sa * cb * sc;
323 quat[1] = sa * cb * cc + ca * cb * sc;
324 quat[2] = ca * sb * sc + sa * sb * cc;
325 quat[3] = sa * sb * sc - ca * sb * cc;
326 }
327 EulerConvention::Xzx => {
328 quat[0] = ca * cb * cc - sa * cb * sc;
329 quat[1] = sa * cb * cc + ca * cb * sc;
330 quat[2] = sa * sb * sc - ca * sb * cc;
331 quat[3] = ca * sb * sc + sa * sb * cc;
332 }
333 EulerConvention::Yxy => {
334 quat[0] = ca * cb * cc - sa * cb * sc;
335 quat[1] = ca * sb * sc + sa * sb * cc;
336 quat[2] = sa * cb * cc + ca * cb * sc;
337 quat[3] = sa * sb * sc - ca * sb * cc;
338 }
339 EulerConvention::Yzy => {
340 quat[0] = ca * cb * cc - sa * cb * sc;
341 quat[1] = sa * sb * sc - ca * sb * cc;
342 quat[2] = sa * cb * cc + ca * cb * sc;
343 quat[3] = ca * sb * sc + sa * sb * cc;
344 }
345 EulerConvention::Zxz => {
346 quat[0] = ca * cb * cc - sa * cb * sc;
347 quat[1] = ca * sb * sc + sa * sb * cc;
348 quat[2] = sa * sb * sc - ca * sb * cc;
349 quat[3] = sa * cb * cc + ca * cb * sc;
350 }
351 EulerConvention::Zyz => {
352 quat[0] = ca * cb * cc - sa * cb * sc;
353 quat[1] = sa * sb * sc - ca * sb * cc;
354 quat[2] = ca * sb * sc + sa * sb * cc;
355 quat[3] = sa * cb * cc + ca * cb * sc;
356 }
357 }
358
359 // Normalize the quaternion
360 let norm = (quat.iter().map(|&x| x * x).sum::<f64>()).sqrt();
361 quat /= norm;
362
363 Ok(Rotation { quat })
364 }
365
366 /// Create a rotation from an axis-angle representation (rotation vector)
367 ///
368 /// # Arguments
369 ///
370 /// * `rotvec` - A 3-element array representing the rotation axis and angle
371 /// (axis is the unit vector in the direction of the array, and the angle is the
372 /// magnitude of the array in radians)
373 ///
374 /// # Returns
375 ///
376 /// A `SpatialResult` containing the rotation if valid, or an error if the rotation vector is invalid
377 ///
378 /// # Examples
379 ///
380 /// ```
381 /// use scirs2_spatial::transform::Rotation;
382 /// use scirs2_core::ndarray::array;
383 /// use std::f64::consts::PI;
384 ///
385 /// // Create a rotation for a 90-degree rotation around the x-axis
386 /// let rotvec = array![PI/2.0, 0.0, 0.0];
387 /// let rot = Rotation::from_rotvec(&rotvec.view()).unwrap();
388 /// ```
389 pub fn from_rotvec(rotvec: &ArrayView1<f64>) -> SpatialResult<Self> {
390 if rotvec.len() != 3 {
391 return Err(SpatialError::DimensionError(format!(
392 "Rotation vector must have 3 elements, got {}",
393 rotvec.len()
394 )));
395 }
396
397 // Compute the angle (magnitude of the rotation vector)
398 let angle = (rotvec[0] * rotvec[0] + rotvec[1] * rotvec[1] + rotvec[2] * rotvec[2]).sqrt();
399
400 let mut quat = Array1::zeros(4);
401
402 if angle < 1e-10 {
403 // For zero rotation, use the identity quaternion
404 quat[0] = 1.0;
405 quat[1] = 0.0;
406 quat[2] = 0.0;
407 quat[3] = 0.0;
408 } else {
409 // Extract the normalized axis
410 let x = rotvec[0] / angle;
411 let y = rotvec[1] / angle;
412 let z = rotvec[2] / angle;
413
414 // Convert axis-angle to quaternion
415 let half_angle = angle / 2.0;
416 let s = half_angle.sin();
417
418 quat[0] = half_angle.cos();
419 quat[1] = x * s;
420 quat[2] = y * s;
421 quat[3] = z * s;
422 }
423
424 Ok(Rotation { quat })
425 }
426
427 /// Convert the rotation to a rotation matrix
428 ///
429 /// # Returns
430 ///
431 /// A 3x3 rotation matrix
432 ///
433 /// # Examples
434 ///
435 /// ```
436 /// use scirs2_spatial::transform::Rotation;
437 /// use scirs2_core::ndarray::array;
438 /// use std::f64::consts::PI;
439 ///
440 /// let angles = array![0.0, 0.0, PI/2.0];
441 /// let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
442 /// let matrix = rot.as_matrix();
443 /// // Should approximately be a 90 degree rotation around Z
444 /// ```
445 pub fn as_matrix(&self) -> Array2<f64> {
446 let mut matrix = Array2::zeros((3, 3));
447
448 let q = &self.quat;
449 let w = q[0];
450 let x = q[1];
451 let y = q[2];
452 let z = q[3];
453
454 // Fill the rotation matrix using quaternion to matrix conversion
455 matrix[[0, 0]] = 1.0 - 2.0 * (y * y + z * z);
456 matrix[[0, 1]] = 2.0 * (x * y - w * z);
457 matrix[[0, 2]] = 2.0 * (x * z + w * y);
458
459 matrix[[1, 0]] = 2.0 * (x * y + w * z);
460 matrix[[1, 1]] = 1.0 - 2.0 * (x * x + z * z);
461 matrix[[1, 2]] = 2.0 * (y * z - w * x);
462
463 matrix[[2, 0]] = 2.0 * (x * z - w * y);
464 matrix[[2, 1]] = 2.0 * (y * z + w * x);
465 matrix[[2, 2]] = 1.0 - 2.0 * (x * x + y * y);
466
467 matrix
468 }
469
470 /// Convert the rotation to Euler angles
471 ///
472 /// # Arguments
473 ///
474 /// * `convention` - The Euler angle convention to use (e.g., "xyz", "zyx")
475 ///
476 /// # Returns
477 ///
478 /// A `SpatialResult` containing a 3-element array of Euler angles (in radians),
479 /// or an error if the convention is invalid
480 ///
481 /// # Examples
482 ///
483 /// ```
484 /// use scirs2_spatial::transform::Rotation;
485 /// use scirs2_core::ndarray::array;
486 /// use std::f64::consts::PI;
487 ///
488 /// let rot = Rotation::from_rotvec(&array![PI/2.0, 0.0, 0.0].view()).unwrap();
489 /// let angles = rot.as_euler("xyz").unwrap();
490 /// // Should be approximately [PI/2, 0, 0]
491 /// ```
492 pub fn as_euler(&self, convention: &str) -> SpatialResult<Array1<f64>> {
493 let conv = EulerConvention::from_str(convention)?;
494 let matrix = self.as_matrix();
495
496 let mut angles = Array1::zeros(3);
497
498 match conv {
499 EulerConvention::Xyz => {
500 // For intrinsic XYZ: R = Rz(c) * Ry(b) * Rx(a)
501 // Extract angles using arctan2 to handle singularities
502 angles[1] = (-matrix[[2, 0]]).asin();
503
504 if angles[1].cos().abs() > 1e-6 {
505 // Not in gimbal lock
506 angles[0] = matrix[[2, 1]].atan2(matrix[[2, 2]]);
507 angles[2] = matrix[[1, 0]].atan2(matrix[[0, 0]]);
508 } else {
509 // Gimbal lock case
510 angles[0] = 0.0;
511 if matrix[[2, 0]] < 0.0 {
512 // sin(beta) = 1, beta = pi/2
513 angles[2] = (-matrix[[0, 1]]).atan2(matrix[[1, 1]]);
514 } else {
515 // sin(beta) = -1, beta = -pi/2
516 angles[2] = matrix[[0, 1]].atan2(matrix[[1, 1]]);
517 }
518 }
519 }
520 EulerConvention::Zyx => {
521 // For intrinsic ZYX: First rotate around Z, then around new Y, then around new X
522 // This is equivalent to extrinsic rotations in reverse order: Rx * Ry * Rz
523 // The combined matrix has elements:
524 // R[0,2] = sin(Y)
525 // So we need to extract Y from arcsin(R[0,2]), not arcsin(-R[0,2])
526 angles[1] = matrix[[0, 2]].asin();
527
528 if angles[1].cos().abs() > 1e-6 {
529 // Not in gimbal lock
530 // R[0,1]/cos(Y) = -sin(Z), R[0,0]/cos(Y) = cos(Z)
531 angles[0] = (-matrix[[0, 1]]).atan2(matrix[[0, 0]]);
532 // R[1,2]/cos(Y) = -sin(X), R[2,2]/cos(Y) = cos(X)
533 angles[2] = (-matrix[[1, 2]]).atan2(matrix[[2, 2]]);
534 } else {
535 // Gimbal lock case
536 angles[0] = 0.0;
537 if matrix[[0, 2]] > 0.0 {
538 // sin(Y) = 1, Y = pi/2
539 angles[2] = matrix[[1, 0]].atan2(matrix[[1, 1]]);
540 } else {
541 // sin(Y) = -1, Y = -pi/2
542 angles[2] = (-matrix[[1, 0]]).atan2(matrix[[1, 1]]);
543 }
544 }
545 }
546 EulerConvention::Xyx => {
547 angles[1] = (matrix[[0, 0]].abs()).acos();
548
549 if angles[1].abs() < 1e-6 || (angles[1] - PI).abs() < 1e-6 {
550 // Gimbal lock case
551 angles[0] = 0.0;
552 // In gimbal lock, only the sum or difference of angles[0] and angles[2] matters
553 angles[2] = (matrix[[1, 2]]).atan2(matrix[[1, 1]]);
554 } else {
555 angles[0] = (matrix[[2, 0]]).atan2(matrix[[1, 0]]);
556 angles[2] = (matrix[[0, 2]]).atan2(-matrix[[0, 1]]);
557 }
558 }
559 EulerConvention::Xzx => {
560 angles[1] = (matrix[[0, 0]].abs()).acos();
561
562 if angles[1].abs() < 1e-6 || (angles[1] - PI).abs() < 1e-6 {
563 // Gimbal lock case
564 angles[0] = 0.0;
565 angles[2] = (matrix[[1, 2]]).atan2(matrix[[1, 1]]);
566 } else {
567 angles[0] = (matrix[[2, 0]]).atan2(-matrix[[1, 0]]);
568 angles[2] = (matrix[[0, 2]]).atan2(matrix[[0, 1]]);
569 }
570 }
571 EulerConvention::Yxy => {
572 angles[1] = (matrix[[1, 1]].abs()).acos();
573
574 if angles[1].abs() < 1e-6 || (angles[1] - PI).abs() < 1e-6 {
575 // Gimbal lock case
576 angles[0] = 0.0;
577 angles[2] = (matrix[[0, 2]]).atan2(matrix[[0, 0]]);
578 } else {
579 angles[0] = (matrix[[0, 1]]).atan2(matrix[[2, 1]]);
580 angles[2] = (matrix[[1, 0]]).atan2(-matrix[[1, 2]]);
581 }
582 }
583 EulerConvention::Yzy => {
584 angles[1] = (matrix[[1, 1]].abs()).acos();
585
586 if angles[1].abs() < 1e-6 || (angles[1] - PI).abs() < 1e-6 {
587 // Gimbal lock case
588 angles[0] = 0.0;
589 angles[2] = (matrix[[0, 2]]).atan2(matrix[[0, 0]]);
590 } else {
591 angles[0] = (matrix[[0, 1]]).atan2(-matrix[[2, 1]]);
592 angles[2] = (matrix[[1, 0]]).atan2(matrix[[1, 2]]);
593 }
594 }
595 EulerConvention::Zxz => {
596 angles[1] = (matrix[[2, 2]].abs()).acos();
597
598 if angles[1].abs() < 1e-6 || (angles[1] - PI).abs() < 1e-6 {
599 // Gimbal lock case
600 angles[0] = 0.0;
601 angles[2] = (matrix[[0, 1]]).atan2(matrix[[0, 0]]);
602 } else {
603 angles[0] = (matrix[[0, 2]]).atan2(matrix[[1, 2]]);
604 angles[2] = (matrix[[2, 0]]).atan2(-matrix[[2, 1]]);
605 }
606 }
607 EulerConvention::Zyz => {
608 angles[1] = (matrix[[2, 2]].abs()).acos();
609
610 if angles[1].abs() < 1e-6 || (angles[1] - PI).abs() < 1e-6 {
611 // Gimbal lock case
612 angles[0] = 0.0;
613 angles[2] = (matrix[[0, 1]]).atan2(matrix[[0, 0]]);
614 } else {
615 angles[0] = (matrix[[1, 2]]).atan2(-matrix[[0, 2]]);
616 angles[2] = (matrix[[2, 1]]).atan2(matrix[[2, 0]]);
617 }
618 }
619 }
620
621 Ok(angles)
622 }
623
624 /// Convert the rotation to an axis-angle representation (rotation vector)
625 ///
626 /// # Returns
627 ///
628 /// A 3-element array representing the rotation axis and angle (axis is the
629 /// unit vector in the direction of the array, and the angle is the magnitude
630 /// of the array in radians)
631 ///
632 /// # Examples
633 ///
634 /// ```
635 /// use scirs2_spatial::transform::Rotation;
636 /// use scirs2_core::ndarray::array;
637 /// use std::f64::consts::PI;
638 ///
639 /// let angles = array![PI/2.0, 0.0, 0.0];
640 /// let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
641 /// let rotvec = rot.as_rotvec();
642 /// // Should be approximately [PI/2, 0, 0]
643 /// ```
644 pub fn as_rotvec(&self) -> Array1<f64> {
645 let q = &self.quat;
646 let angle = 2.0 * q[0].acos();
647
648 let mut rotvec = Array1::zeros(3);
649
650 // Handle the case of zero rotation
651 if angle < 1e-10 {
652 return rotvec;
653 }
654
655 // Extract the axis
656 let sin_half_angle = (1.0 - q[0] * q[0]).sqrt();
657
658 if sin_half_angle < 1e-10 {
659 // If sin(angle/2) is close to zero, rotation is close to 0 or 2π
660 return rotvec;
661 }
662
663 // Normalize the axis
664 let axis_x = q[1] / sin_half_angle;
665 let axis_y = q[2] / sin_half_angle;
666 let axis_z = q[3] / sin_half_angle;
667
668 // Compute the rotation vector
669 rotvec[0] = axis_x * angle;
670 rotvec[1] = axis_y * angle;
671 rotvec[2] = axis_z * angle;
672
673 rotvec
674 }
675
676 /// Get the quaternion representation [w, x, y, z]
677 ///
678 /// # Returns
679 ///
680 /// A 4-element array representing the quaternion (w, x, y, z)
681 ///
682 /// # Examples
683 ///
684 /// ```
685 /// use scirs2_spatial::transform::Rotation;
686 /// use scirs2_core::ndarray::array;
687 ///
688 /// let angles = array![0.0, 0.0, 0.0];
689 /// let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
690 /// let quat = rot.as_quat();
691 /// // Should be [1, 0, 0, 0] (identity rotation)
692 /// ```
693 pub fn as_quat(&self) -> Array1<f64> {
694 self.quat.clone()
695 }
696
697 /// Get the inverse of the rotation
698 ///
699 /// # Returns
700 ///
701 /// A new Rotation that is the inverse of this one
702 ///
703 /// # Examples
704 ///
705 /// ```
706 /// use scirs2_spatial::transform::Rotation;
707 /// use scirs2_core::ndarray::array;
708 /// use std::f64::consts::PI;
709 ///
710 /// let angles = array![0.0, 0.0, PI/4.0];
711 /// let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
712 /// let rot_inv = rot.inv();
713 /// ```
714 pub fn inv(&self) -> Rotation {
715 // For unit quaternions, the inverse is the conjugate
716 let mut quat_inv = self.quat.clone();
717 quat_inv[1] = -quat_inv[1];
718 quat_inv[2] = -quat_inv[2];
719 quat_inv[3] = -quat_inv[3];
720
721 Rotation { quat: quat_inv }
722 }
723
724 /// Apply the rotation to a vector or array of vectors
725 ///
726 /// # Arguments
727 ///
728 /// * `vec` - A 3-element vector or a 2D array of 3-element vectors to rotate
729 ///
730 /// # Returns
731 ///
732 /// The rotated vector or array of vectors
733 ///
734 /// # Examples
735 ///
736 /// ```
737 /// use scirs2_spatial::transform::Rotation;
738 /// use scirs2_core::ndarray::array;
739 /// use std::f64::consts::PI;
740 ///
741 /// let angles = array![0.0, 0.0, PI/2.0];
742 /// let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
743 /// let vec = array![1.0, 0.0, 0.0];
744 /// let rotated = rot.apply(&vec.view());
745 /// // Should be approximately [0, 1, 0]
746 /// ```
747 pub fn apply(&self, vec: &ArrayView1<f64>) -> SpatialResult<Array1<f64>> {
748 if vec.len() != 3 {
749 return Err(SpatialError::DimensionError(
750 "Vector must have 3 elements".to_string(),
751 ));
752 }
753
754 // Convert to matrix and apply
755 let matrix = self.as_matrix();
756 Ok(matrix.dot(vec))
757 }
758
759 /// Apply the rotation to multiple vectors
760 ///
761 /// # Arguments
762 ///
763 /// * `vecs` - A 2D array of vectors (each row is a 3-element vector)
764 ///
765 /// # Returns
766 ///
767 /// A 2D array of rotated vectors
768 ///
769 /// # Examples
770 ///
771 /// ```
772 /// use scirs2_spatial::transform::Rotation;
773 /// use scirs2_core::ndarray::array;
774 /// use std::f64::consts::PI;
775 ///
776 /// let angles = array![0.0, 0.0, PI/2.0];
777 /// let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
778 /// let vecs = array![[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]];
779 /// let rotated = rot.apply_multiple(&vecs.view());
780 /// // First row should be approximately [0, 1, 0]
781 /// // Second row should be approximately [-1, 0, 0]
782 /// ```
783 pub fn apply_multiple(&self, vecs: &ArrayView2<'_, f64>) -> SpatialResult<Array2<f64>> {
784 if vecs.ncols() != 3 {
785 return Err(SpatialError::DimensionError(
786 "Each vector must have 3 elements".to_string(),
787 ));
788 }
789
790 let matrix = self.as_matrix();
791 Ok(vecs.dot(&matrix.t()))
792 }
793
794 /// Combine this rotation with another (apply the other rotation after this one)
795 ///
796 /// # Arguments
797 ///
798 /// * `other` - The other rotation to combine with
799 ///
800 /// # Returns
801 ///
802 /// A new rotation that represents the composition of the two rotations
803 ///
804 /// # Examples
805 ///
806 /// ```
807 /// use scirs2_spatial::transform::Rotation;
808 /// use scirs2_core::ndarray::array;
809 /// use std::f64::consts::PI;
810 ///
811 /// // Rotate 90 degrees around X, then 90 degrees around Y
812 /// let angles1 = array![PI/2.0, 0.0, 0.0];
813 /// let rot1 = Rotation::from_euler(&angles1.view(), "xyz").unwrap();
814 /// let angles2 = array![0.0, PI/2.0, 0.0];
815 /// let rot2 = Rotation::from_euler(&angles2.view(), "xyz").unwrap();
816 /// let combined = rot1.compose(&rot2);
817 /// ```
818 pub fn compose(&self, other: &Rotation) -> Rotation {
819 // Quaternion multiplication
820 let w1 = self.quat[0];
821 let x1 = self.quat[1];
822 let y1 = self.quat[2];
823 let z1 = self.quat[3];
824
825 let w2 = other.quat[0];
826 let x2 = other.quat[1];
827 let y2 = other.quat[2];
828 let z2 = other.quat[3];
829
830 let result_quat = array![
831 w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
832 w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
833 w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
834 w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
835 ];
836
837 // Normalize the resulting quaternion
838 let norm = (result_quat.iter().map(|&x| x * x).sum::<f64>()).sqrt();
839 let normalized_quat = result_quat / norm;
840
841 Rotation {
842 quat: normalized_quat,
843 }
844 }
845
846 /// Create an identity rotation (no rotation)
847 ///
848 /// # Returns
849 ///
850 /// A new Rotation that represents no rotation
851 ///
852 /// # Examples
853 ///
854 /// ```
855 /// use scirs2_spatial::transform::Rotation;
856 /// use scirs2_core::ndarray::array;
857 ///
858 /// let identity = Rotation::identity();
859 /// let vec = array![1.0, 2.0, 3.0];
860 /// let rotated = identity.apply(&vec.view());
861 /// // Should still be [1.0, 2.0, 3.0]
862 /// ```
863 pub fn identity() -> Rotation {
864 let quat = array![1.0, 0.0, 0.0, 0.0];
865 Rotation { quat }
866 }
867
868 /// Create a random uniform rotation
869 ///
870 /// # Returns
871 ///
872 /// A new random Rotation uniformly distributed over the rotation space
873 ///
874 /// # Examples
875 ///
876 /// ```
877 /// use scirs2_spatial::transform::Rotation;
878 ///
879 /// let random_rot = Rotation::random();
880 /// ```
881 pub fn random() -> Rotation {
882 use scirs2_core::random::Rng;
883 let mut rng = scirs2_core::random::rng();
884
885 // Generate random quaternion using method from:
886 // http://planning.cs.uiuc.edu/node198.html
887 let u1 = rng.gen_range(0.0..1.0);
888 let u2 = rng.gen_range(0.0..1.0);
889 let u3 = rng.gen_range(0.0..1.0);
890
891 let sqrt_u1 = u1.sqrt();
892 let sqrt_1_minus_u1 = (1.0 - u1).sqrt();
893 let two_pi_u2 = 2.0 * PI * u2;
894 let two_pi_u3 = 2.0 * PI * u3;
895
896 let quat = array![
897 sqrt_1_minus_u1 * (two_pi_u2 / 2.0).sin(),
898 sqrt_1_minus_u1 * (two_pi_u2 / 2.0).cos(),
899 sqrt_u1 * (two_pi_u3 / 2.0).sin(),
900 sqrt_u1 * (two_pi_u3 / 2.0).cos()
901 ];
902
903 Rotation { quat }
904 }
905
906 /// Spherical linear interpolation (SLERP) between two rotations
907 ///
908 /// # Arguments
909 ///
910 /// * `other` - The target rotation to interpolate towards
911 /// * `t` - Interpolation parameter (0.0 = this rotation, 1.0 = other rotation)
912 ///
913 /// # Returns
914 ///
915 /// A new rotation interpolated between this and the other rotation
916 ///
917 /// # Examples
918 ///
919 /// ```
920 /// use scirs2_spatial::transform::Rotation;
921 /// use scirs2_core::ndarray::array;
922 /// use std::f64::consts::PI;
923 ///
924 /// let rot1 = Rotation::identity();
925 /// let rot2 = Rotation::from_euler(&array![0.0, 0.0, PI/2.0].view(), "xyz").unwrap();
926 /// let interpolated = rot1.slerp(&rot2, 0.5);
927 /// ```
928 pub fn slerp(&self, other: &Rotation, t: f64) -> Rotation {
929 // Ensure t is in [0, 1]
930 let t = t.clamp(0.0, 1.0);
931
932 if t == 0.0 {
933 return self.clone();
934 }
935 if t == 1.0 {
936 return other.clone();
937 }
938
939 let q1 = &self.quat;
940 let mut q2 = other.quat.clone();
941
942 // Compute dot product
943 let mut dot = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
944
945 // If dot product is negative, use -q2 to take the shorter path
946 if dot < 0.0 {
947 q2 *= -1.0;
948 dot = -dot;
949 }
950
951 // If the quaternions are very close, use linear interpolation
952 if dot > 0.9995 {
953 let result = q1 * (1.0 - t) + &q2 * t;
954 let norm = (result.iter().map(|&x| x * x).sum::<f64>()).sqrt();
955 return Rotation {
956 quat: result / norm,
957 };
958 }
959
960 // Calculate the angle between quaternions
961 let theta_0 = dot.acos();
962 let sin_theta_0 = theta_0.sin();
963
964 // Use correct SLERP formula
965 let s0 = ((1.0 - t) * theta_0).sin() / sin_theta_0;
966 let s1 = (t * theta_0).sin() / sin_theta_0;
967
968 let result = q1 * s0 + &q2 * s1;
969 Rotation { quat: result }
970 }
971
972 /// Calculate the angular distance between two rotations
973 ///
974 /// # Arguments
975 ///
976 /// * `other` - The other rotation to calculate distance to
977 ///
978 /// # Returns
979 ///
980 /// The angular distance in radians (always positive, in range [0, π])
981 ///
982 /// # Examples
983 ///
984 /// ```
985 /// use scirs2_spatial::transform::Rotation;
986 /// use scirs2_core::ndarray::array;
987 /// use std::f64::consts::PI;
988 ///
989 /// # fn example() -> Result<(), Box<dyn std::error::Error>> {
990 /// let rot1 = Rotation::identity();
991 /// let rot2 = Rotation::from_euler(&array![0.0, 0.0, PI/2.0].view(), "xyz")?;
992 /// let distance = rot1.angular_distance(&rot2);
993 /// assert!((distance - PI/2.0).abs() < 1e-10);
994 /// # Ok(())
995 /// # }
996 /// ```
997 pub fn angular_distance(&self, other: &Rotation) -> f64 {
998 let q1 = &self.quat;
999 let q2 = &other.quat;
1000
1001 // Compute dot product
1002 let dot = (q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]).abs();
1003
1004 // Clamp to avoid numerical issues with acos
1005 let dot_clamped = dot.min(1.0);
1006
1007 // Return the angular distance
1008 2.0 * dot_clamped.acos()
1009 }
1010}
1011
1012#[cfg(test)]
1013mod tests {
1014 use super::*;
1015 use approx::assert_relative_eq;
1016
1017 #[test]
1018 fn test_rotation_identity() {
1019 let identity = Rotation::identity();
1020 let vec = array![1.0, 2.0, 3.0];
1021 let rotated = identity.apply(&vec.view()).unwrap();
1022
1023 assert_relative_eq!(rotated[0], vec[0], epsilon = 1e-10);
1024 assert_relative_eq!(rotated[1], vec[1], epsilon = 1e-10);
1025 assert_relative_eq!(rotated[2], vec[2], epsilon = 1e-10);
1026 }
1027
1028 #[test]
1029 fn test_rotation_from_quat() {
1030 // A quaternion for 90 degrees rotation around X axis
1031 let quat = array![
1032 std::f64::consts::FRAC_1_SQRT_2,
1033 std::f64::consts::FRAC_1_SQRT_2,
1034 0.0,
1035 0.0
1036 ];
1037 let rot = Rotation::from_quat(&quat.view()).unwrap();
1038
1039 let vec = array![0.0, 1.0, 0.0];
1040 let rotated = rot.apply(&vec.view()).unwrap();
1041
1042 // Should rotate [0, 1, 0] to [0, 0, 1]
1043 assert_relative_eq!(rotated[0], 0.0, epsilon = 1e-10);
1044 assert_relative_eq!(rotated[1], 0.0, epsilon = 1e-10);
1045 assert_relative_eq!(rotated[2], 1.0, epsilon = 1e-10);
1046 }
1047
1048 #[test]
1049 fn test_rotation_from_matrix() {
1050 // A rotation matrix for 90 degrees rotation around Z axis
1051 let matrix = array![[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]];
1052 let rot = Rotation::from_matrix(&matrix.view()).unwrap();
1053
1054 let vec = array![1.0, 0.0, 0.0];
1055 let rotated = rot.apply(&vec.view()).unwrap();
1056
1057 // Should rotate [1, 0, 0] to [0, 1, 0]
1058 assert_relative_eq!(rotated[0], 0.0, epsilon = 1e-10);
1059 assert_relative_eq!(rotated[1], 1.0, epsilon = 1e-10);
1060 assert_relative_eq!(rotated[2], 0.0, epsilon = 1e-10);
1061 }
1062
1063 #[test]
1064 fn test_rotation_from_euler() {
1065 // 90 degrees rotation around X axis
1066 let angles = array![PI / 2.0, 0.0, 0.0];
1067 let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
1068
1069 let vec = array![0.0, 1.0, 0.0];
1070 let rotated = rot.apply(&vec.view()).unwrap();
1071
1072 // Should rotate [0, 1, 0] to [0, 0, 1]
1073 assert_relative_eq!(rotated[0], 0.0, epsilon = 1e-10);
1074 assert_relative_eq!(rotated[1], 0.0, epsilon = 1e-10);
1075 assert_relative_eq!(rotated[2], 1.0, epsilon = 1e-10);
1076 }
1077
1078 #[test]
1079 fn test_rotation_from_rotvec() {
1080 // 90 degrees rotation around X axis
1081 let rotvec = array![PI / 2.0, 0.0, 0.0];
1082 let rot = Rotation::from_rotvec(&rotvec.view()).unwrap();
1083
1084 let vec = array![0.0, 1.0, 0.0];
1085 let rotated = rot.apply(&vec.view()).unwrap();
1086
1087 // Should rotate [0, 1, 0] to [0, 0, 1]
1088 assert_relative_eq!(rotated[0], 0.0, epsilon = 1e-10);
1089 assert_relative_eq!(rotated[1], 0.0, epsilon = 1e-10);
1090 assert_relative_eq!(rotated[2], 1.0, epsilon = 1e-10);
1091 }
1092
1093 #[test]
1094 fn test_rotation_compose() {
1095 // 90 degrees rotation around X, then 90 degrees around Y
1096 let rot_x = rotation_from_euler(PI / 2.0, 0.0, 0.0, "xyz").unwrap();
1097 let rot_y = rotation_from_euler(0.0, PI / 2.0, 0.0, "xyz").unwrap();
1098
1099 let composed = rot_x.compose(&rot_y);
1100
1101 let vec = array![0.0, 0.0, 1.0];
1102 let rotated = composed.apply(&vec.view()).unwrap();
1103
1104 // Should rotate [0, 0, 1] to [1, 0, 0]
1105 assert_relative_eq!(rotated[0], 1.0, epsilon = 1e-10);
1106 assert_relative_eq!(rotated[1], 0.0, epsilon = 1e-10);
1107 assert_relative_eq!(rotated[2], 0.0, epsilon = 1e-10);
1108 }
1109
1110 #[test]
1111 fn test_rotation_inv() {
1112 // 45 degrees rotation around Z axis
1113 let rot = rotation_from_euler(0.0, 0.0, PI / 4.0, "xyz").unwrap();
1114 let rot_inv = rot.inv();
1115
1116 let vec = array![1.0, 0.0, 0.0];
1117 let rotated = rot.apply(&vec.view()).unwrap();
1118 let rotated_back = rot_inv.apply(&rotated.view()).unwrap();
1119
1120 // Should get back the original vector
1121 assert_relative_eq!(rotated_back[0], vec[0], epsilon = 1e-10);
1122 assert_relative_eq!(rotated_back[1], vec[1], epsilon = 1e-10);
1123 assert_relative_eq!(rotated_back[2], vec[2], epsilon = 1e-10);
1124 }
1125
1126 #[test]
1127 fn test_rotation_conversions() {
1128 // Create a rotation and verify conversion between representations
1129 let angles = array![PI / 4.0, PI / 6.0, PI / 3.0];
1130 let rot = Rotation::from_euler(&angles.view(), "xyz").unwrap();
1131
1132 // Convert to matrix and back to Euler
1133 let matrix = rot.as_matrix();
1134 let rot_from_matrix = Rotation::from_matrix(&matrix.view()).unwrap();
1135 let _angles_back = rot_from_matrix.as_euler("xyz").unwrap();
1136
1137 // Allow for different but equivalent representations
1138 let vec = array![1.0, 2.0, 3.0];
1139 let rotated1 = rot.apply(&vec.view()).unwrap();
1140 let rotated2 = rot_from_matrix.apply(&vec.view()).unwrap();
1141
1142 assert_relative_eq!(rotated1[0], rotated2[0], epsilon = 1e-10);
1143 assert_relative_eq!(rotated1[1], rotated2[1], epsilon = 1e-10);
1144 assert_relative_eq!(rotated1[2], rotated2[2], epsilon = 1e-10);
1145
1146 // Convert to axis-angle and back
1147 let rotvec = rot.as_rotvec();
1148 let rot_from_rotvec = Rotation::from_rotvec(&rotvec.view()).unwrap();
1149 let rotated3 = rot_from_rotvec.apply(&vec.view()).unwrap();
1150
1151 assert_relative_eq!(rotated1[0], rotated3[0], epsilon = 1e-10);
1152 assert_relative_eq!(rotated1[1], rotated3[1], epsilon = 1e-10);
1153 assert_relative_eq!(rotated1[2], rotated3[2], epsilon = 1e-10);
1154 }
1155
1156 #[test]
1157 fn test_slerp_interpolation() {
1158 // Test SLERP between identity and 90-degree Z rotation
1159 let rot1 = Rotation::identity();
1160 let rot2 = rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").unwrap();
1161
1162 // Test endpoints
1163 let slerp_0 = rot1.slerp(&rot2, 0.0);
1164 let slerp_1 = rot1.slerp(&rot2, 1.0);
1165
1166 let vec = array![1.0, 0.0, 0.0];
1167 let result_0 = slerp_0.apply(&vec.view()).unwrap();
1168 let result_1 = slerp_1.apply(&vec.view()).unwrap();
1169
1170 // At t=0, should be same as rot1 (identity)
1171 assert_relative_eq!(result_0[0], 1.0, epsilon = 1e-10);
1172 assert_relative_eq!(result_0[1], 0.0, epsilon = 1e-10);
1173
1174 // At t=1, should be same as rot2 (90 degree rotation)
1175 assert_relative_eq!(result_1[0], 0.0, epsilon = 1e-10);
1176 assert_relative_eq!(result_1[1], 1.0, epsilon = 1e-10);
1177
1178 // Test midpoint
1179 let slerp_mid = rot1.slerp(&rot2, 0.5);
1180 let result_mid = slerp_mid.apply(&vec.view()).unwrap();
1181
1182 // Should be roughly 45 degrees
1183 let expected_angle = PI / 4.0;
1184 assert_relative_eq!(result_mid[0], expected_angle.cos(), epsilon = 1e-10);
1185 assert_relative_eq!(result_mid[1], expected_angle.sin(), epsilon = 1e-10);
1186 }
1187
1188 #[test]
1189 fn test_angular_distance() {
1190 let rot1 = Rotation::identity();
1191 let rot2 = rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").unwrap();
1192 let rot3 = rotation_from_euler(0.0, 0.0, PI, "xyz").unwrap();
1193
1194 // Distance from identity to 90-degree rotation should be PI/2
1195 let dist1 = rot1.angular_distance(&rot2);
1196 assert_relative_eq!(dist1, PI / 2.0, epsilon = 1e-10);
1197
1198 // Distance from identity to 180-degree rotation should be PI
1199 let dist2 = rot1.angular_distance(&rot3);
1200 assert_relative_eq!(dist2, PI, epsilon = 1e-10);
1201
1202 // Distance from rotation to itself should be 0
1203 let dist3 = rot1.angular_distance(&rot1);
1204 assert_relative_eq!(dist3, 0.0, epsilon = 1e-10);
1205 }
1206}