qm/
q.rs

1//! quaternion
2//!
3
4use num::Float;
5
6use crate::v::{TVector, v3::Vector3};
7use crate::m::{TMatrix, m4::{TM4, Matrix4}};
8
9/// TQuaternion
10pub trait TQuaternion<F: Float + std::fmt::Debug> {
11  /// constructor
12  fn from_axis_and_angle(v: &Vector3<F>, a: F) -> Self;
13  /// constructor
14  fn identity() -> Self;
15  /// conjugate
16  fn conjugate(&self) -> Self;
17  /// like as ref
18  fn me(&self) -> &Quaternion<F>;
19  /// to Matrix4 (qp = Q4x4 p4)
20  fn to_m4_left(&self) -> Matrix4<F>;
21  /// to Matrix4 (qp = P4x4 q4)
22  fn to_m4_right(&self) -> Matrix4<F>;
23  /// to Matrix4 rot
24  /// - q * r * q.conjugate()
25  /// - Matrix4::from_p_right(q.conjugate()) * Matrix4::from_q_left(q) * r
26  fn to_m4_rot(&self) -> Matrix4<F>;
27}
28
29/// Quaternion
30pub type Quaternion<F> = [F; 4];
31
32/// TQuaternion for Quaternion
33impl<F: Float + std::fmt::Debug + std::iter::Sum> TQuaternion<F> for Quaternion<F> {
34  /// constructor
35  fn from_axis_and_angle(v: &Vector3<F>, a: F) -> Self {
36    let t = a / <F>::from(2.0).unwrap();
37    let d = v.iter().map(|&p| p * p).sum::<F>().sqrt();
38    let n = v.iter().map(|&p| p / d).collect::<Vec<_>>();
39    [t.cos(), n[0] * t.sin(), n[1] * t.sin(), n[2] * t.sin()]
40  }
41  /// constructor
42  fn identity() -> Self {
43    Self::new(&(0..4).into_iter().map(|i|
44      <F>::from(if i == 0 { 1.0 } else { 0.0 }).unwrap()
45    ).collect::<Vec<_>>())
46  }
47  /// conjugate
48  fn conjugate(&self) -> Self {
49    [self[0], -self[1], -self[2], -self[3]]
50  }
51  /// like as ref
52  fn me(&self) -> &Quaternion<F> {
53    self
54  }
55  /// to Matrix4 (qp = Q4x4 p4)
56  fn to_m4_left(&self) -> Matrix4<F> {
57    Matrix4::<F>::from_q_left(self)
58  }
59  /// to Matrix4 (qp = P4x4 q4)
60  fn to_m4_right(&self) -> Matrix4<F> {
61    Matrix4::<F>::from_p_right(self)
62  }
63  /// to Matrix4 rot
64  /// - q * r * q.conjugate()
65  /// - Matrix4::from_p_right(q.conjugate()) * Matrix4::from_q_left(q) * r
66  fn to_m4_rot(&self) -> Matrix4<F> {
67    let m4p = Matrix4::from_p_right(&self.conjugate());
68    let m4q = Matrix4::from_q_left(self);
69    m4q.dot_m(&m4p) // m4p dot m4q
70  }
71}
qm/q.rs

qm/
q.rs
