qm/
lib.rs

1#![doc(html_root_url = "https://docs.rs/quaternion-matrix/0.1.4")]
2//! quaternion matrix for Rust
3//!
4
5pub mod q;
6pub mod m;
7pub mod v;
8
9use num::Float;
10
11/// check equal with precision
12pub fn prec_eq_f<F: Float>(s: F, e: F, d: F) -> bool {
13  (s - d).abs() < e
14}
15
16/// check equal with precision
17pub fn prec_eq<F: Float>(s: &[F], e: F, d: &[F]) -> bool {
18  for i in 0..s.len() { if !prec_eq_f(s[i], e, d[i]) { return false; } }
19  true
20}
21
22/// preq_eq macro
23#[macro_export]
24macro_rules! prec_eq {
25  ($qs: expr, $e: expr, $f: ident, $qe: expr) => {
26    $qs.prec_eq($e, &Quaternion::<$f>::new($qe))
27  }
28}
29
30/// assert_pe macro
31#[macro_export]
32macro_rules! assert_pe {
33  ($qs: expr, $e: expr, $f: ident, $qe: expr) => {
34    assert!(prec_eq!($qs, $e, $f, $qe))
35  }
36}
37
38/// check_q macro
39#[macro_export]
40macro_rules! check_qaa {
41  ($f: ident, $v: expr, $r: expr, $qe: expr) => {{
42    let q = Quaternion::<$f>::from_axis_and_angle($v, $r);
43    assert_pe!(q, 1e-6, $f, $qe);
44    q
45  }}
46}
47
48/// test with [-- --nocapture] or [-- --show-output]
49#[cfg(test)]
50mod tests {
51  // use super::*;
52  use crate::v::{TVector, v3::Vector3, v4::Vector4};
53  use crate::q::{TQuaternion, Quaternion};
54  use crate::m::{TMatrix, m3::Matrix3, m4::Matrix4};
55
56  /// test Vector3
57  #[test]
58  fn test_vector3() {
59    let v32 = Vector3::<f32>::new(&vec![1.0, 2.0, 3.0]);
60    let v64 = Vector3::<f64>::new(&vec![1.0, 2.0, 3.0]);
61    assert_eq!(v32, [1.0, 2.0, 3.0]);
62    assert_eq!(v64, [1.0, 2.0, 3.0]);
63    assert_eq!(v32.dot(&v32), 14.0);
64    assert_eq!(v64.dot(&v64), 14.0);
65    let i32 = Matrix3::<f32>::identity();
66    let i64 = Matrix3::<f64>::identity();
67    assert_eq!(v32.dot_mv(&i32).to_vec(), v32.to_vec()); // i32 dot v32
68    assert_eq!(v64.dot_mv(&i64).to_vec(), v64.to_vec()); // i64 dot v64
69  }
70
71  /// test Vector4
72  #[test]
73  fn test_vector4() {
74    let v32 = Vector4::<f32>::new(&vec![1.0, 2.0, 3.0, 4.0]);
75    let v64 = Vector4::<f64>::new(&vec![1.0, 2.0, 3.0, 4.0]);
76    assert_eq!(v32, [1.0, 2.0, 3.0, 4.0]);
77    assert_eq!(v64, [1.0, 2.0, 3.0, 4.0]);
78    assert_eq!(v32.dot(&v32), 30.0);
79    assert_eq!(v64.dot(&v64), 30.0);
80    let i32 = Matrix4::<f32>::identity();
81    let i64 = Matrix4::<f64>::identity();
82    assert_eq!(v32.dot_mv(&i32).to_vec(), v32.to_vec()); // i32 dot v32
83    assert_eq!(v64.dot_mv(&i64).to_vec(), v64.to_vec()); // i64 dot v64
84  }
85
86  /// test Vector3 cross
87  #[test]
88  fn test_vector3_cross() {
89    let a32 = Vector3::<f32>::new(&vec![1.0, 0.0, 1.0]);
90    let a64 = Vector3::<f64>::new(&vec![1.0, 0.0, 1.0]);
91    let b32 = Vector3::<f32>::new(&vec![0.0, 1.0, 1.0]);
92    let b64 = Vector3::<f64>::new(&vec![0.0, 1.0, 1.0]);
93    assert_eq!(a32.cross(&a32), [0.0, 0.0, 0.0]);
94    assert_eq!(a64.cross(&a64), [0.0, 0.0, 0.0]);
95    assert_eq!(a32.cross(&b32), [-1.0, -1.0, 1.0]);
96    assert_eq!(a64.cross(&b64), [-1.0, -1.0, 1.0]);
97    assert_eq!(b32.cross(&a32), [1.0, 1.0, -1.0]);
98    assert_eq!(b64.cross(&a64), [1.0, 1.0, -1.0]);
99  }
100
101  /// test Vector4 cross (TODO: skip now)
102  #[test]
103  fn test_vector4_cross() {
104/*
105    let a32 = Vector4::<f32>::new(&vec![1.0, 0.0, 1.0, 1.0]);
106    let a64 = Vector4::<f64>::new(&vec![1.0, 0.0, 1.0, 1.0]);
107    let b32 = Vector4::<f32>::new(&vec![0.0, 1.0, 1.0, 1.0]);
108    let b64 = Vector4::<f64>::new(&vec![0.0, 1.0, 1.0, 1.0]);
109    assert_eq!(a32.cross(&a32), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
110    assert_eq!(a64.cross(&a64), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
111    assert_eq!(a32.cross(&b32), [-1.0, -1.0, 1.0, 0.0, 1.0, -1.0, 0.0, 0.0]);
112    assert_eq!(a64.cross(&b64), [-1.0, -1.0, 1.0, 0.0, 1.0, -1.0, 0.0, 0.0]);
113*/
114  }
115
116  /// test Quaternion
117  #[test]
118  fn test_quaternion() {
119    let qi32 = Quaternion::<f32>::identity();
120    let qi64 = Quaternion::<f64>::identity();
121    let qi32c = qi32.conjugate();
122    let qi64c = qi64.conjugate();
123    let q32 = Quaternion::<f32>::new(&vec![1.0, 0.0, 0.0, 0.0]);
124    let q64 = Quaternion::<f64>::new(&vec![1.0, 0.0, 0.0, 0.0]);
125    let q32m = Quaternion::<f32>::new(&vec![1.0, 0.0, 0.0, 0.0000009]);
126    let q64m = Quaternion::<f64>::new(&vec![1.0, 0.0, 0.0, 0.0000009]);
127    let q32p = Quaternion::<f32>::new(&vec![1.0, 0.0, 0.0, 1e-6]);
128    let q64p = Quaternion::<f64>::new(&vec![1.0, 0.0, 0.0, 1e-6]);
129    assert_eq!(q32, [1.0, 0.0, 0.0, 0.0]);
130    assert_eq!(q64, [1.0, 0.0, 0.0, 0.0]);
131    assert_eq!(qi32, [1.0, 0.0, 0.0, 0.0]);
132    assert_eq!(qi64, [1.0, 0.0, 0.0, 0.0]);
133    assert_eq!(q32, qi32);
134    assert_eq!(q64, qi64);
135    assert_eq!(qi32c, [1.0, 0.0, 0.0, 0.0]);
136    assert_eq!(qi64c, [1.0, 0.0, 0.0, 0.0]);
137    assert_eq!(qi32.prec_eq(1e-6, &q32m), true);
138    assert_eq!(qi64.prec_eq(1e-6, &q64m), true);
139    assert_eq!(qi32.prec_eq(1e-6, &q32p), false);
140    assert_eq!(qi64.prec_eq(1e-6, &q64p), false);
141    assert_eq!(qi32.to_vec(), [1.0, 0.0, 0.0, 0.0]);
142    assert_eq!(qi64.to_vec(), [1.0, 0.0, 0.0, 0.0]);
143  }
144
145  /// test Quaternion rot
146  #[test]
147  fn test_quaternion_rot() {
148    let pi32 = std::f32::consts::PI;
149    let pi64 = std::f64::consts::PI;
150    let t32 = (0..=4).into_iter().map(|i|
151      i as f32 * pi32 / 2.0).collect::<Vec<_>>();
152    let t64 = (0..=4).into_iter().map(|i|
153      i as f64 * pi64 / 2.0).collect::<Vec<_>>();
154    let ax32 = Vector3::<f32>::new(&vec![1.0, 0.0, 0.0]);
155    let ay32 = Vector3::<f32>::new(&vec![0.0, 1.0, 0.0]);
156    let az32 = Vector3::<f32>::new(&vec![0.0, 0.0, 1.0]);
157    let ax64 = Vector3::<f64>::new(&vec![1.0, 0.0, 0.0]);
158    let ay64 = Vector3::<f64>::new(&vec![0.0, 1.0, 0.0]);
159    let az64 = Vector3::<f64>::new(&vec![0.0, 0.0, 1.0]);
160    let r32 = 0.70710678f32;
161    let r64 = 0.70710678f64;
162
163    // cos = 1, sin = 0
164    // cos = r, sin = r
165    // cos = 0, sin = 1
166    // cos = -r, sin = r
167    // cos = -1, sin = 0
168    let q32t = vec![
169      vec![
170        check_qaa!(f32, &ax32, t32[0], &vec![1.0, 0.0, 0.0, 0.0]),
171        check_qaa!(f32, &ax32, t32[1], &vec![r32, r32, 0.0, 0.0]),
172        check_qaa!(f32, &ax32, t32[2], &vec![0.0, 1.0, 0.0, 0.0]),
173        check_qaa!(f32, &ax32, t32[3], &vec![-r32, r32, 0.0, 0.0]),
174        check_qaa!(f32, &ax32, t32[4], &vec![-1.0, 0.0, 0.0, 0.0])],
175      vec![
176        check_qaa!(f32, &ay32, t32[0], &vec![1.0, 0.0, 0.0, 0.0]),
177        check_qaa!(f32, &ay32, t32[1], &vec![r32, 0.0, r32, 0.0]),
178        check_qaa!(f32, &ay32, t32[2], &vec![0.0, 0.0, 1.0, 0.0]),
179        check_qaa!(f32, &ay32, t32[3], &vec![-r32, 0.0, r32, 0.0]),
180        check_qaa!(f32, &ay32, t32[4], &vec![-1.0, 0.0, 0.0, 0.0])],
181      vec![
182        check_qaa!(f32, &az32, t32[0], &vec![1.0, 0.0, 0.0, 0.0]),
183        check_qaa!(f32, &az32, t32[1], &vec![r32, 0.0, 0.0, r32]),
184        check_qaa!(f32, &az32, t32[2], &vec![0.0, 0.0, 0.0, 1.0]),
185        check_qaa!(f32, &az32, t32[3], &vec![-r32, 0.0, 0.0, r32]),
186        check_qaa!(f32, &az32, t32[4], &vec![-1.0, 0.0, 0.0, 0.0])]];
187    let q64t = vec![
188      vec![
189        check_qaa!(f64, &ax64, t64[0], &vec![1.0, 0.0, 0.0, 0.0]),
190        check_qaa!(f64, &ax64, t64[1], &vec![r64, r64, 0.0, 0.0]),
191        check_qaa!(f64, &ax64, t64[2], &vec![0.0, 1.0, 0.0, 0.0]),
192        check_qaa!(f64, &ax64, t64[3], &vec![-r64, r64, 0.0, 0.0]),
193        check_qaa!(f64, &ax64, t64[4], &vec![-1.0, 0.0, 0.0, 0.0])],
194      vec![
195        check_qaa!(f64, &ay64, t64[0], &vec![1.0, 0.0, 0.0, 0.0]),
196        check_qaa!(f64, &ay64, t64[1], &vec![r64, 0.0, r64, 0.0]),
197        check_qaa!(f64, &ay64, t64[2], &vec![0.0, 0.0, 1.0, 0.0]),
198        check_qaa!(f64, &ay64, t64[3], &vec![-r64, 0.0, r64, 0.0]),
199        check_qaa!(f64, &ay64, t64[4], &vec![-1.0, 0.0, 0.0, 0.0])],
200      vec![
201        check_qaa!(f64, &az64, t64[0], &vec![1.0, 0.0, 0.0, 0.0]),
202        check_qaa!(f64, &az64, t64[1], &vec![r64, 0.0, 0.0, r64]),
203        check_qaa!(f64, &az64, t64[2], &vec![0.0, 0.0, 0.0, 1.0]),
204        check_qaa!(f64, &az64, t64[3], &vec![-r64, 0.0, 0.0, r64]),
205        check_qaa!(f64, &az64, t64[4], &vec![-1.0, 0.0, 0.0, 0.0])]];
206
207    let i32 = Matrix4::<f32>::identity();
208    let i64 = Matrix4::<f64>::identity();
209    let qm32 = q32t.iter().map(|qxyz| qxyz.iter().map(|q|
210      q.to_m4_rot()).collect::<Vec<_>>()).collect::<Vec<_>>();
211    let qm64 = q64t.iter().map(|qxyz| qxyz.iter().map(|q|
212      q.to_m4_rot()).collect::<Vec<_>>()).collect::<Vec<_>>();
213
214    assert!(qm32[0][0].prec_eq(1e-6, &i32));
215    assert!(qm32[0][1].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
216      vec![1.0, 0.0, 0.0, 0.0],
217      vec![0.0, 0.0, -1.0, 0.0], // yz (0, -sin pi/2)
218      vec![0.0, 1.0, 0.0, 0.0], // yz (sin pi/2, 0)
219      vec![0.0, 0.0, 0.0, 1.0]])));
220    assert!(qm32[0][2].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
221      vec![1.0, 0.0, 0.0, 0.0],
222      vec![0.0, -1.0, 0.0, 0.0], // yz (cos pi, 0)
223      vec![0.0, 0.0, -1.0, 0.0], // yz (0, cos pi)
224      vec![0.0, 0.0, 0.0, 1.0]])));
225    assert!(qm32[0][3].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
226      vec![1.0, 0.0, 0.0, 0.0],
227      vec![0.0, 0.0, 1.0, 0.0], // yz (0, -sin 3pi/2)
228      vec![0.0, -1.0, 0.0, 0.0], // yz (sin 3pi/2, 0)
229      vec![0.0, 0.0, 0.0, 1.0]])));
230    assert!(qm32[0][4].prec_eq(1e-6, &i32));
231
232    assert!(qm32[1][0].prec_eq(1e-6, &i32));
233    assert!(qm32[1][1].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
234      vec![0.0, 0.0, 1.0, 0.0], // zx (0, -sin pi/2)
235      vec![0.0, 1.0, 0.0, 0.0],
236      vec![-1.0, 0.0, 0.0, 0.0], // zx (sin pi/2, 0)
237      vec![0.0, 0.0, 0.0, 1.0]])));
238    assert!(qm32[1][2].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
239      vec![-1.0, 0.0, 0.0, 0.0], // zx (cos pi, 0)
240      vec![0.0, 1.0, 0.0, 0.0],
241      vec![0.0, 0.0, -1.0, 0.0], // zx (0, cos pi)
242      vec![0.0, 0.0, 0.0, 1.0]])));
243    assert!(qm32[1][3].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
244      vec![0.0, 0.0, -1.0, 0.0], // zx (0, -sin 3pi/2)
245      vec![0.0, 1.0, 0.0, 0.0],
246      vec![1.0, 0.0, 0.0, 0.0], // zx (sin 3pi/2, 0)
247      vec![0.0, 0.0, 0.0, 1.0]])));
248    assert!(qm32[1][4].prec_eq(1e-6, &i32));
249
250    assert!(qm32[2][0].prec_eq(1e-6, &i32));
251    assert!(qm32[2][1].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
252      vec![0.0, -1.0, 0.0, 0.0], // xy (0, -sin pi/2)
253      vec![1.0, 0.0, 0.0, 0.0], // xy (sin pi/2, 0)
254      vec![0.0, 0.0, 1.0, 0.0],
255      vec![0.0, 0.0, 0.0, 1.0]])));
256    assert!(qm32[2][2].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
257      vec![-1.0, 0.0, 0.0, 0.0], // xy (cos pi, 0)
258      vec![0.0, -1.0, 0.0, 0.0], // xy (0, cos pi)
259      vec![0.0, 0.0, 1.0, 0.0],
260      vec![0.0, 0.0, 0.0, 1.0]])));
261    assert!(qm32[2][3].prec_eq(1e-6, &Matrix4::<f32>::new(&vec![
262      vec![0.0, 1.0, 0.0, 0.0], // xy (0, -sin 3pi/2)
263      vec![-1.0, 0.0, 0.0, 0.0], // xy (sin 3pi/2, 0)
264      vec![0.0, 0.0, 1.0, 0.0],
265      vec![0.0, 0.0, 0.0, 1.0]])));
266    assert!(qm32[2][4].prec_eq(1e-6, &i32));
267
268    assert!(qm64[0][0].prec_eq(1e-6, &i64));
269    assert!(qm64[0][4].prec_eq(1e-6, &i64));
270
271    assert!(qm64[1][0].prec_eq(1e-6, &i64));
272    assert!(qm64[1][4].prec_eq(1e-6, &i64));
273
274    assert!(qm64[2][0].prec_eq(1e-6, &i64));
275    assert!(qm64[2][4].prec_eq(1e-6, &i64));
276  }
277
278  /// test Matrix3
279  #[test]
280  fn test_matrix3() {
281    let i32 = Matrix3::<f32>::identity();
282    let i64 = Matrix3::<f64>::identity();
283    let m32 = Matrix3::<f32>::new(&vec![
284      vec![1.0, 0.0, 0.0],
285      vec![0.0, 1.0, 0.0],
286      vec![0.0, 0.0, 1.0]]);
287    let m64 = Matrix3::<f64>::new(&vec![
288      vec![1.0, 0.0, 0.0],
289      vec![0.0, 1.0, 0.0],
290      vec![0.0, 0.0, 1.0]]);
291    let u32 = Matrix3::<f32>::new(&vec![
292      vec![1.0, 2.0, 1.0],
293      vec![2.0, 1.0, 0.0],
294      vec![1.0, 1.0, 2.0]]);
295    let u64 = Matrix3::<f64>::new(&vec![
296      vec![1.0, 2.0, 1.0],
297      vec![2.0, 1.0, 0.0],
298      vec![1.0, 1.0, 2.0]]);
299    let v32 = Matrix3::<f32>::new(&vec![
300      vec![-0.4, 0.6, 0.2],
301      vec![0.8, -0.2, -0.4],
302      vec![-0.2, -0.2, 0.6]]);
303    let v64 = Matrix3::<f64>::new(&vec![
304      vec![-0.4, 0.6, 0.2],
305      vec![0.8, -0.2, -0.4],
306      vec![-0.2, -0.2, 0.6]]);
307    assert_eq!(m32, [
308      [1.0, 0.0, 0.0],
309      [0.0, 1.0, 0.0],
310      [0.0, 0.0, 1.0]]);
311    assert_eq!(m64, [
312      [1.0, 0.0, 0.0],
313      [0.0, 1.0, 0.0],
314      [0.0, 0.0, 1.0]]);
315    assert_eq!(m32, i32);
316    assert_eq!(m64, i64);
317    assert!(m32.dot_m(&m32).prec_eq(1e-6, &i32));
318    assert!(m64.dot_m(&m64).prec_eq(1e-6, &i64));
319    assert!(v32.dot_m(&u32).prec_eq(1e-6, &i32)); // u32 dot v32
320    assert!(v64.dot_m(&u64).prec_eq(1e-6, &i64)); // u64 dot v64
321
322    assert!(i32.inv(1e-6).expect("det").prec_eq(1e-6, &i32));
323    assert!(i64.inv(1e-6).expect("det").prec_eq(1e-6, &i64));
324    assert!(u32.inv(1e-6).expect("det").prec_eq(1e-6, &v32));
325    assert!(u64.inv(1e-6).expect("det").prec_eq(1e-6, &v64));
326    assert!(v32.inv(1e-6).expect("det").prec_eq(1e-6, &u32));
327    assert!(v64.inv(1e-6).expect("det").prec_eq(1e-6, &u64));
328  }
329
330  /// test Matrix4
331  #[test]
332  fn test_matrix4() {
333    let i32 = Matrix4::<f32>::identity();
334    let i64 = Matrix4::<f64>::identity();
335    let m32 = Matrix4::<f32>::new(&vec![
336      vec![1.0, 0.0, 0.0, 0.0],
337      vec![0.0, 1.0, 0.0, 0.0],
338      vec![0.0, 0.0, 1.0, 0.0],
339      vec![0.0, 0.0, 0.0, 1.0]]);
340    let m64 = Matrix4::<f64>::new(&vec![
341      vec![1.0, 0.0, 0.0, 0.0],
342      vec![0.0, 1.0, 0.0, 0.0],
343      vec![0.0, 0.0, 1.0, 0.0],
344      vec![0.0, 0.0, 0.0, 1.0]]);
345    let u32 = Matrix4::<f32>::new(&vec![
346      vec![3.0, 1.0, 1.0, 2.0],
347      vec![5.0, 1.0, 3.0, 4.0],
348      vec![2.0, 0.0, 1.0, 0.0],
349      vec![1.0, 3.0, 2.0, 1.0]]);
350    let u64 = Matrix4::<f64>::new(&vec![
351      vec![3.0, 1.0, 1.0, 2.0],
352      vec![5.0, 1.0, 3.0, 4.0],
353      vec![2.0, 0.0, 1.0, 0.0],
354      vec![1.0, 3.0, 2.0, 1.0]]);
355    let v32 = Matrix4::<f32>::new(&vec![
356      vec![0.5, -0.22727273, 0.36363637, -0.09090909],
357      vec![0.5, -0.31818182, -0.09090909, 0.27272728],
358      vec![-1.0, 0.45454547, 0.27272728, 0.18181819],
359      vec![0.0, 0.27272728, -0.63636364, -0.09090909]]);
360    let v64 = Matrix4::<f64>::new(&vec![
361      vec![0.5, -0.22727272727, 0.36363636364, -0.09090909091],
362      vec![0.5, -0.31818181818, -0.09090909091, 0.27272727273],
363      vec![-1.0, 0.45454545455, 0.27272727273, 0.18181818182],
364      vec![0.0, 0.27272727273, -0.63636363636, -0.09090909091]]);
365    assert_eq!(m32, [
366      [1.0, 0.0, 0.0, 0.0],
367      [0.0, 1.0, 0.0, 0.0],
368      [0.0, 0.0, 1.0, 0.0],
369      [0.0, 0.0, 0.0, 1.0]]);
370    assert_eq!(m64, [
371      [1.0, 0.0, 0.0, 0.0],
372      [0.0, 1.0, 0.0, 0.0],
373      [0.0, 0.0, 1.0, 0.0],
374      [0.0, 0.0, 0.0, 1.0]]);
375    assert_eq!(m32, i32);
376    assert_eq!(m64, i64);
377    assert!(m32.dot_m(&m32).prec_eq(1e-6, &i32));
378    assert!(m64.dot_m(&m64).prec_eq(1e-6, &i64));
379    assert!(v32.dot_m(&u32).prec_eq(1e-6, &i32)); // u32 dot v32
380    assert!(v64.dot_m(&u64).prec_eq(1e-6, &i64)); // u64 dot v64
381
382    assert!(i32.inv(1e-6).expect("det").prec_eq(1e-6, &i32));
383    assert!(i64.inv(1e-6).expect("det").prec_eq(1e-6, &i64));
384    assert!(u32.inv(1e-6).expect("det").prec_eq(1e-6, &v32));
385    assert!(u64.inv(1e-6).expect("det").prec_eq(1e-6, &v64));
386    assert!(v32.inv(1e-6).expect("det").prec_eq(1e-6, &u32));
387    assert!(v64.inv(1e-6).expect("det").prec_eq(1e-6, &u64));
388  }
389}
qm/lib.rs

qm/
lib.rs
