quaternion_wrapper/lib.rs
1//! This is a wrapper for the `quaternion-core` crate.
2//!
3//! Provides quaternion operations and interconversion with several attitude representations.
4//! Operator overloading allows implementation similar to mathematical expressions.
5//!
6//! The supported operations are listed in the table below:
7//!
8//! | ↓Left / Right→ | QuaternionWrapper | Vector3Wrapper | ScalarWrapper |
9//! |:---------------------:|:--------------------------------|:--------------------------|:-------------------|
10//! | __QuaternionWrapper__ | `+`, `-`, `*`, `+=`, `-=`, `*=` | `+`, `-`, `*` | `+`, `-`, `*`, `/` |
11//! | __Vector3Wrapper__ | `+`, `-`, `*` | `+`, `-`, `*`, `+=`, `-=` | `+`, `-`, `*`, `/` |
12//! | __ScalarWrapper__ | `+`, `-`, `*` | `+`, `-`, `*` | `+`, `-`, `*`, `/`, `+=`, `-=`, `*=`, `/=` |
13//!
14//! ## Versor
15//!
16//! Versor refers to a Quaternion representing a rotation, the norm of which is 1.
17//!
18//! The documentation for this crate basically writes Versor instead of Unit Quaternion,
19//! but the difference in usage is not clear.
20//! Please think Versor = Unit Quaternion.
21
22#![no_std]
23#[cfg(feature = "std")]
24extern crate std;
25
26use num_traits::{Float, FloatConst};
27use quaternion_core as quat;
28pub use quat::{Vector3, Quaternion, DCM, RotationType, RotationSequence};
29
30mod ops;
31
32#[derive(Debug, Clone, Copy)]
33pub struct QuaternionWrapper<T>(pub Quaternion<T>);
34
35/// Treated as Pure Quaternion.
36///
37/// `QuaternionWrapper = ScalarWrapper + Vector3Wrapper`
38#[derive(Debug, Clone, Copy)]
39pub struct Vector3Wrapper<T>(pub Vector3<T>);
40
41/// Treated as Real Quaternion.
42///
43/// `QuaternionWrapper = ScalarWrapper + Vector3Wrapper`
44#[derive(Debug, Clone, Copy)]
45pub struct ScalarWrapper<T>(pub T);
46
47// ------------------------ Quaternion ------------------------ //
48impl<T: Float + FloatConst> QuaternionWrapper<T> {
49 /// Create a new Quaternion.
50 ///
51 /// # Examples
52 ///
53 /// ```
54 /// # use quaternion_wrapper::QuaternionWrapper;
55 /// let q = QuaternionWrapper::new( (1.0, [0.0; 3]) );
56 ///
57 /// // Or it could be written like this
58 /// let q = QuaternionWrapper( (1.0, [0.0; 3]) );
59 /// ```
60 #[inline]
61 pub fn new(q: Quaternion<T>) -> Self {
62 Self(q)
63 }
64
65 /// Create a new Identity Quaternion
66 ///
67 /// # Examples
68 ///
69 /// ```
70 /// # use quaternion_wrapper::QuaternionWrapper;
71 /// let q: QuaternionWrapper<f64> = QuaternionWrapper::new_identity();
72 ///
73 /// let p: QuaternionWrapper<f64> = QuaternionWrapper::new((1.0, [0.0; 3]));
74 ///
75 /// assert_eq!(q.unwrap(), p.unwrap());
76 /// ```
77 #[inline]
78 pub fn new_identity() -> Self {
79 Self( (T::one(), [T::zero(); 3]) )
80 }
81
82 /// Returns the `Quaternion<T>`.
83 #[inline]
84 pub fn unwrap(self) -> Quaternion<T> {
85 self.0
86 }
87
88 /// Returns the scalar part of a quaternion.
89 #[inline]
90 pub fn get_scalar_part(self) -> ScalarWrapper<T> {
91 ScalarWrapper( (self.0).0 )
92 }
93
94 /// Returns the vector part of a quaternion.
95 #[inline]
96 pub fn get_vector_part(self) -> Vector3Wrapper<T> {
97 Vector3Wrapper( (self.0).1 )
98 }
99
100 /// Generate Versor by specifying rotation `angle`\[rad\] and `axis` vector.
101 ///
102 /// The `axis` vector does not have to be a unit vector.
103 ///
104 /// If you enter a zero vector, it returns an identity quaternion.
105 ///
106 /// # Examples
107 ///
108 /// ```
109 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
110 /// # let PI = std::f64::consts::PI;
111 /// // Generates a quaternion representing the
112 /// // rotation of π/2[rad] around the y-axis.
113 /// let q = QuaternionWrapper::from_axis_angle([0.0, 1.0, 0.0], PI/2.0);
114 ///
115 /// // Rotate the point.
116 /// let r = q.point_rotation( Vector3Wrapper([2.0, 2.0, 0.0]) );
117 ///
118 /// // Check if the calculation is correct.
119 /// let diff = Vector3Wrapper([0.0, 2.0, -2.0]) - r;
120 /// for val in diff.unwrap() {
121 /// assert!( val.abs() < 1e-12 );
122 /// }
123 /// ```
124 #[inline]
125 pub fn from_axis_angle(axis: Vector3<T>, angle: T) -> Self {
126 Self( quat::from_axis_angle(axis, angle) )
127 }
128
129 /// Convert a DCM to a Versor representing
130 /// the `q v q*` rotation (Point Rotation - Frame Fixed).
131 ///
132 /// When convert to a DCM representing `q* v q` rotation
133 /// (Frame Rotation - Point Fixed) to a Versor, do the following:
134 ///
135 /// ```
136 /// # use quaternion_wrapper::QuaternionWrapper;
137 /// # let dcm = QuaternionWrapper((1.0, [0.0; 3])).to_dcm();
138 /// let q = QuaternionWrapper::from_dcm(dcm).conj();
139 /// ```
140 ///
141 /// # Examples
142 ///
143 /// ```
144 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
145 /// # let PI = std::f64::consts::PI;
146 /// // Make these as you like.
147 /// let v = Vector3Wrapper([1.0, 0.5, -8.0]);
148 /// let q = QuaternionWrapper::from_axis_angle([0.2, 1.0, -2.0], PI/4.0);
149 ///
150 /// // --- Point rotation --- //
151 /// {
152 /// let m = q.to_dcm();
153 /// let q_check = QuaternionWrapper::from_dcm(m);
154 ///
155 /// let diff = (q - q_check).unwrap();
156 /// assert!( diff.0.abs() < 1e-12 );
157 /// assert!( diff.1[0].abs() < 1e-12 );
158 /// assert!( diff.1[1].abs() < 1e-12 );
159 /// assert!( diff.1[2].abs() < 1e-12 );
160 /// }
161 ///
162 /// // --- Frame rotation --- //
163 /// {
164 /// let m = q.conj().to_dcm();
165 /// let q_check = QuaternionWrapper::from_dcm(m).conj();
166 ///
167 /// let diff = (q - q_check).unwrap();
168 /// assert!( diff.0.abs() < 1e-12 );
169 /// assert!( diff.1[0].abs() < 1e-12 );
170 /// assert!( diff.1[1].abs() < 1e-12 );
171 /// assert!( diff.1[2].abs() < 1e-12 );
172 /// }
173 /// ```
174 #[inline]
175 pub fn from_dcm(m: DCM<T>) -> Self {
176 Self( quat::from_dcm(m) )
177 }
178
179 /// Convert Euler angles to Versor.
180 ///
181 /// The type of rotation (Intrinsic or Extrinsic) is specified by `RotationType` enum,
182 /// and the rotation sequence (XZX, XYZ, ...) is specified by `RotationSequence` enum.
183 ///
184 /// Each element of `angles` should be specified in the range: `[-2π, 2π]`.
185 ///
186 /// Sequences: `angles[0]` ---> `angles[1]` ---> `angles[2]`
187 ///
188 /// # Examples
189 ///
190 /// ```
191 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
192 /// # let PI = std::f64::consts::PI;
193 /// use quaternion_core::{RotationType::*, RotationSequence::XYZ};
194 ///
195 /// let angles = [PI/6.0, 1.6*PI, -PI/4.0];
196 /// let v = Vector3Wrapper([1.0, 0.5, -0.4]);
197 ///
198 /// // Quaternions representing rotation around each axis.
199 /// let x = QuaternionWrapper::from_axis_angle([1.0, 0.0, 0.0], angles[0]);
200 /// let y = QuaternionWrapper::from_axis_angle([0.0, 1.0, 0.0], angles[1]);
201 /// let z = QuaternionWrapper::from_axis_angle([0.0, 0.0, 1.0], angles[2]);
202 ///
203 /// // ---- Intrinsic (X-Y-Z) ---- //
204 /// // These represent the same rotation.
205 /// let q_in = x * y * z;
206 /// let e2q_in = QuaternionWrapper::from_euler_angles(Intrinsic, XYZ, angles);
207 /// // Confirmation
208 /// let a_in = q_in.point_rotation(v);
209 /// let b_in = e2q_in.point_rotation(v);
210 /// let diff_in = (a_in - b_in).unwrap();
211 /// assert!( diff_in[0].abs() < 1e-12 );
212 /// assert!( diff_in[1].abs() < 1e-12 );
213 /// assert!( diff_in[2].abs() < 1e-12 );
214 ///
215 /// // ---- Extrinsic (X-Y-Z) ---- //
216 /// // These represent the same rotation.
217 /// let q_ex = z * y * x;
218 /// let e2q_ex = QuaternionWrapper::from_euler_angles(Extrinsic, XYZ, angles);
219 /// // Confirmation
220 /// let a_ex = q_ex.point_rotation(v);
221 /// let b_ex = e2q_ex.point_rotation(v);
222 /// let diff_ex = (a_ex - b_ex).unwrap();
223 /// assert!( diff_ex[0].abs() < 1e-12 );
224 /// assert!( diff_ex[1].abs() < 1e-12 );
225 /// assert!( diff_ex[2].abs() < 1e-12 );
226 /// ```
227 #[inline]
228 pub fn from_euler_angles(rt: RotationType, rs: RotationSequence, angles: Vector3<T>) -> Self {
229 Self( quat::from_euler_angles(rt, rs, angles) )
230 }
231
232 /// Calculate the rotation `axis` (unit vector) and the rotation `angle`\[rad\]
233 /// around the `axis` from the Versor.
234 ///
235 /// If identity quaternion is entered, `angle` returns zero and
236 /// the `axis` returns a zero vector.
237 ///
238 /// Range of `angle`: `(-π, π]`
239 #[inline]
240 pub fn to_axis_angle(self) -> (Vector3Wrapper<T>, ScalarWrapper<T>) {
241 let f = quat::to_axis_angle(self.0);
242 ( Vector3Wrapper(f.0), ScalarWrapper(f.1) )
243 }
244
245 /// Convert a Versor to a DCM representing
246 /// the `q v q*` rotation (Point Rotation - Frame Fixed).
247 ///
248 /// When convert to a DCM representing the
249 /// `q* v q` rotation (Frame Rotation - Point Fixed), do the following:
250 ///
251 /// ```
252 /// # use quaternion_wrapper::QuaternionWrapper;
253 /// # let q = QuaternionWrapper::<f64>::new_identity();
254 /// let dcm = q.conj().to_dcm();
255 /// ```
256 ///
257 /// # Examples
258 ///
259 /// ```
260 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
261 /// # let PI = std::f64::consts::PI;
262 /// // Make these as you like.
263 /// let v = Vector3Wrapper([1.0, 0.5, -8.0]);
264 /// let q = QuaternionWrapper::from_axis_angle([0.2, 1.0, -2.0], PI/4.0);
265 ///
266 /// // --- Point rotation --- //
267 /// {
268 /// let m = q.to_dcm();
269 ///
270 /// let rm = v.matrix_product(m);
271 /// let rq = q.point_rotation(v);
272 /// let diff = (rm - rq).unwrap();
273 /// assert!( diff[0].abs() < 1e-12 );
274 /// assert!( diff[1].abs() < 1e-12 );
275 /// assert!( diff[2].abs() < 1e-12 );
276 /// }
277 ///
278 /// // --- Frame rotation --- //
279 /// {
280 /// let m = q.conj().to_dcm();
281 ///
282 /// let rm = v.matrix_product(m);
283 /// let rq = q.frame_rotation(v);
284 /// let diff = (rm - rq).unwrap();
285 /// assert!( diff[0].abs() < 1e-12 );
286 /// assert!( diff[1].abs() < 1e-12 );
287 /// assert!( diff[2].abs() < 1e-12 );
288 /// }
289 /// ```
290 #[inline]
291 pub fn to_dcm(self) -> DCM<T> {
292 quat::to_dcm(self.0)
293 }
294
295 /// Convert Versor to Euler angles.
296 ///
297 /// The type of rotation (Intrinsic or Extrinsic) is specified by `RotationType` enum,
298 /// and the rotation sequence (XZX, XYZ, ...) is specified by `RotationSequence` enum.
299 ///
300 /// ```
301 /// # use quaternion_wrapper::{RotationType::Intrinsic, RotationSequence::XYZ, QuaternionWrapper};
302 /// # let q = QuaternionWrapper::<f64>::new_identity();
303 /// let angles = q.to_euler_angles(Intrinsic, XYZ);
304 /// ```
305 ///
306 /// Sequences: `angles[0]` ---> `angles[1]` ---> `angles[2]`
307 ///
308 /// # Singularity
309 ///
310 /// ## RotationType::Intrinsic
311 ///
312 /// For Proper Euler angles (ZXZ, XYX, YZY, ZYZ, XZX, YXY), the singularity is reached
313 /// when the sine of the second rotation angle is 0 (angle = 0, ±π, ...), and for
314 /// Tait-Bryan angles (XYZ, YZX, ZXY, XZY, ZYX, YXZ), the singularity is reached when
315 /// the cosine of the second rotation angle is 0 (angle = ±π/2).
316 ///
317 /// At the singularity, the third rotation angle is set to 0\[rad\].
318 ///
319 /// ## RotationType::Extrinsic
320 ///
321 /// As in the case of Intrinsic rotation, for Proper Euler angles, the singularity occurs
322 /// when the sine of the second rotation angle is 0 (angle = 0, ±π, ...), and for
323 /// Tait-Bryan angles, the singularity occurs when the cosine of the second rotation angle
324 /// is 0 (angle = ±π/2).
325 ///
326 /// At the singularity, the first rotation angle is set to 0\[rad\].
327 ///
328 /// # Examples
329 ///
330 /// Depending on the rotation angle of each axis, it may not be possible to recover the
331 /// same rotation angle as the original. However, they represent the same rotation in 3D space.
332 ///
333 /// ```
334 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
335 /// # let PI = std::f64::consts::PI;
336 /// use quaternion_wrapper::{RotationType::*, RotationSequence::XYZ};
337 ///
338 /// let angles = Vector3Wrapper([PI/6.0, PI/4.0, PI/3.0]);
339 ///
340 /// // ---- Intrinsic (X-Y-Z) ---- //
341 /// let q_in = QuaternionWrapper::from_euler_angles(Intrinsic, XYZ, angles.unwrap());
342 /// let e_in = q_in.to_euler_angles(Intrinsic, XYZ);
343 /// let diff = (angles - e_in).unwrap();
344 /// assert!( diff[0].abs() < 1e-12 );
345 /// assert!( diff[1].abs() < 1e-12 );
346 /// assert!( diff[2].abs() < 1e-12 );
347 ///
348 /// // ---- Extrinsic (X-Y-Z) ---- //
349 /// let q_ex = QuaternionWrapper::from_euler_angles(Extrinsic, XYZ, angles.unwrap());
350 /// let e_ex = q_ex.to_euler_angles(Extrinsic, XYZ);
351 /// let diff = (angles - e_ex).unwrap();
352 /// assert!( diff[0].abs() < 1e-12 );
353 /// assert!( diff[1].abs() < 1e-12 );
354 /// assert!( diff[2].abs() < 1e-12 );
355 /// ```
356 #[inline]
357 pub fn to_euler_angles(self, rt: RotationType, rs: RotationSequence) -> Vector3Wrapper<T> {
358 Vector3Wrapper( quat::to_euler_angles(rt, rs, self.0) )
359 }
360
361 /// Calculate the versor to rotate from vector `a` to vector `b` (Without singularity!).
362 ///
363 /// This function calculates `q` satisfying `b = q.point_rotation(a)`
364 /// when `a.norm() = b.norm()`.
365 /// If `a.norm() > 0` and `b.norm() > 0`, then `q` can be calculated with good
366 /// accuracy no matter what the positional relationship between `a` and `b` is.
367 ///
368 /// If you enter a zero vector either `a` or `b`, it returns `None`.
369 ///
370 /// # Example
371 ///
372 /// ```
373 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
374 /// let a = Vector3Wrapper::<f64>::new([1.5, -0.5, 0.2]);
375 /// let b = Vector3Wrapper::<f64>::new([0.1, 0.6, 1.0]);
376 ///
377 /// let q = QuaternionWrapper::rotate_a_to_b(a, b).unwrap();
378 /// let b_check = q.point_rotation(a);
379 ///
380 /// let cross = b.cross(b_check).unwrap();
381 /// assert!( cross[0].abs() < 1e-12 );
382 /// assert!( cross[1].abs() < 1e-12 );
383 /// assert!( cross[2].abs() < 1e-12 );
384 /// ```
385 #[inline]
386 pub fn rotate_a_to_b(a: Vector3Wrapper<T>, b: Vector3Wrapper<T>) -> Option<Self> {
387 let result = quat::rotate_a_to_b(a.0, b.0);
388 if result.is_some() {
389 Some( Self( result.unwrap() ) )
390 } else {
391 None
392 }
393 }
394
395 /// Calculate the versor to rotate from vector `a` to vector `b` by the shortest path.
396 ///
397 /// The parameter `t` adjusts the amount of movement from `a` to `b`.
398 /// When `t = 1`, `a` moves completely to position `b`.
399 ///
400 /// The algorithm used in this function is less accurate when `a` and `b` are parallel.
401 /// Therefore, it is better to use the `rotate_a_to_b(a, b)` function when `t = 1` and
402 /// the rotation axis is not important.
403 ///
404 /// If you enter a zero vector either `a` or `b`, it returns `None`.
405 #[inline]
406 pub fn rotate_a_to_b_shortest(a: Vector3Wrapper<T>, b: Vector3Wrapper<T>, t: T) -> Option<Self> {
407 let result = quat::rotate_a_to_b_shortest(a.0, b.0, t);
408 if result.is_some() {
409 Some( Self( result.unwrap() ) )
410 } else {
411 None
412 }
413 }
414
415 /// Calculate the sum of each element of Quaternion.
416 ///
417 /// # Examples
418 ///
419 /// ```
420 /// # use quaternion_wrapper::QuaternionWrapper;
421 /// let q = QuaternionWrapper::<f64>::new( (1.0, [2.0, 3.0, 4.0]) );
422 ///
423 /// assert!( (10.0 - q.sum().unwrap()).abs() < 1e-12 );
424 /// ```
425 #[inline]
426 pub fn sum(self) -> ScalarWrapper<T> {
427 ScalarWrapper( quat::sum(self.0) )
428 }
429
430 /// Calculate `s*self + b`
431 ///
432 /// If the `fma` feature is enabled, the FMA calculation is performed using the `mul_add` method.
433 /// If not enabled, it's computed by unfused multiply-add (s*a + b).
434 #[inline]
435 pub fn scale_add(self, s: ScalarWrapper<T>, b: QuaternionWrapper<T>) -> Self {
436 Self( quat::scale_add(s.0, self.0, b.0) )
437 }
438
439 /// Hadamard product of Quaternion.
440 ///
441 /// Calculate `self ∘ other`
442 #[inline]
443 pub fn hadamard(self, other: QuaternionWrapper<T>) -> Self {
444 Self( quat::hadamard(self.0, other.0) )
445 }
446
447 /// Hadamard product and Addiction of Quaternion.
448 ///
449 /// Calculate `a ∘ b + c`
450 ///
451 /// If the `fma` feature is enabled, the FMA calculation is performed using the `mul_add` method.
452 /// If not enabled, it's computed by unfused multiply-add (s*a + b).
453 #[inline]
454 pub fn hadamard_add(self, b: QuaternionWrapper<T>, c: QuaternionWrapper<T>) -> Self {
455 Self( quat::hadamard_add(self.0, b.0, c.0) )
456 }
457
458 /// Dot product of the quaternion.
459 #[inline]
460 pub fn dot(self, other: QuaternionWrapper<T>) -> ScalarWrapper<T> {
461 ScalarWrapper( quat::dot(self.0, other.0) )
462 }
463
464 /// Calcurate the L2 norm of the quaternion.
465 #[inline]
466 pub fn norm(self) -> ScalarWrapper<T> {
467 ScalarWrapper( quat::norm(self.0) )
468 }
469
470 /// Normalization of quaternion.
471 ///
472 /// # Examples
473 ///
474 /// ```
475 /// # use quaternion_wrapper::QuaternionWrapper;
476 /// // This norm is not 1.
477 /// let q = QuaternionWrapper::<f64>::new( (1.0, [2.0, 3.0, 4.0]) );
478 /// assert!( (1.0 - q.norm().unwrap()).abs() > 1e-12 );
479 ///
480 /// // Now that normalized, this norm is 1!
481 /// let q_n = q.normalize();
482 /// assert!( (1.0 - q_n.norm().unwrap()).abs() < 1e-12 );
483 /// ```
484 #[inline]
485 pub fn normalize(self) -> Self {
486 Self( quat::normalize(self.0) )
487 }
488
489 /// Returns the conjugate of quaternion.
490 #[inline]
491 pub fn conj(self) -> Self {
492 Self( quat::conj(self.0) )
493 }
494
495 /// Calculate the inverse of quaternion.
496 ///
497 /// # Examples
498 ///
499 /// ```
500 /// # use quaternion_wrapper::QuaternionWrapper;
501 /// let q = QuaternionWrapper::<f64>::new( (1.0, [2.0, 3.0, 4.0]) );
502 ///
503 /// // Identity quaternion
504 /// let id = (q * q.inv()).unwrap(); // = (q.inv() * q).unwrap()
505 ///
506 /// assert!( (id.0 - 1.0).abs() < 1e-12 );
507 /// assert!( id.1[0].abs() < 1e-12 );
508 /// assert!( id.1[1].abs() < 1e-12 );
509 /// assert!( id.1[2].abs() < 1e-12 );
510 /// ```
511 #[inline]
512 pub fn inv(self) -> Self {
513 Self( quat::inv(self.0) )
514 }
515
516 /// Exponential function of quaternion
517 #[inline]
518 pub fn exp(self) -> Self {
519 Self( quat::exp(self.0) )
520 }
521
522 /// Natural logarithm of quaternion.
523 #[inline]
524 pub fn ln(self) -> Self {
525 Self( quat::ln(self.0) )
526 }
527
528 /// Natural logarithm of versor.
529 ///
530 /// If it is guaranteed to be a versor, it is less computationally
531 /// expensive than the `.ln()` method.
532 ///
533 /// Only the vector part is returned since the real part is always zero.
534 #[inline]
535 pub fn ln_versor(self) -> Vector3Wrapper<T> {
536 Vector3Wrapper( quat::ln_versor(self.0) )
537 }
538
539 /// Power function of quaternion.
540 #[inline]
541 pub fn pow(self, t: T) -> Self {
542 Self( quat::pow(self.0, t) )
543 }
544
545 /// Power function of versor.
546 ///
547 /// If it is guaranteed to be a versor, it is less computationally
548 /// expensive than the `.pow()` method.
549 #[inline]
550 pub fn pow_versor(self, t: T) -> Self {
551 Self( quat::pow_versor(self.0, t) )
552 }
553
554 /// Rotation of vector (Point Rotation - Frame Fixed)
555 /// Rotation of point (Point Rotation - Frame Fixed)
556 ///
557 /// `q v q* (||q|| = 1)`
558 ///
559 /// Since it is implemented with an optimized formula,
560 /// it can be calculated with the amount of operations shown in the table below:
561 ///
562 /// | Operation | Num |
563 /// |:------------:|:---:|
564 /// | Multiply | 18 |
565 /// | Add/Subtract | 12 |
566 ///
567 /// # Example
568 ///
569 /// ```
570 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
571 /// # let PI = std::f64::consts::PI;
572 /// // Make these as you like.
573 /// let v = Vector3Wrapper([1.0, 0.5, -8.0]);
574 /// let q = QuaternionWrapper::from_axis_angle([0.2, 1.0, -2.0], PI);
575 ///
576 /// let r = q.point_rotation(v);
577 ///
578 /// // This makes a lot of wasted calculations.
579 /// let r_check = (q * v * q.conj()).get_vector_part();
580 ///
581 /// let diff = (r - r_check).unwrap();
582 /// assert!( diff[0].abs() < 1e-12 );
583 /// assert!( diff[1].abs() < 1e-12 );
584 /// assert!( diff[2].abs() < 1e-12 );
585 /// ```
586 #[inline]
587 pub fn point_rotation(self, v: Vector3Wrapper<T>) -> Vector3Wrapper<T> {
588 Vector3Wrapper( quat::point_rotation(self.0, v.0) )
589 }
590
591 /// Rotation of frame (Frame Rotation - Point Fixed)
592 ///
593 /// `q* v q (||q|| = 1)`
594 ///
595 /// Since it is implemented with an optimized formula,
596 /// it can be calculated with the amount of operations shown in the table below:
597 ///
598 /// | Operation | Num |
599 /// |:------------:|:---:|
600 /// | Multiply | 18 |
601 /// | Add/Subtract | 12 |
602 ///
603 /// # Example
604 ///
605 /// ```
606 /// # use quaternion_wrapper::{QuaternionWrapper, Vector3Wrapper};
607 /// # let PI = std::f64::consts::PI;
608 /// // Make these as you like.
609 /// let v = Vector3Wrapper([1.0, 0.5, -8.0]);
610 /// let q = QuaternionWrapper::from_axis_angle([0.2, 1.0, -2.0], PI);
611 ///
612 /// let r = q.frame_rotation(v);
613 ///
614 /// // This makes a lot of wasted calculations.
615 /// let r_check = (q.conj() * v * q).get_vector_part();
616 ///
617 /// let diff = (r - r_check).unwrap();
618 /// assert!( diff[0].abs() < 1e-12 );
619 /// assert!( diff[1].abs() < 1e-12 );
620 /// assert!( diff[2].abs() < 1e-12 );
621 /// ```
622 #[inline]
623 pub fn frame_rotation(self, v: Vector3Wrapper<T>) -> Vector3Wrapper<T> {
624 Vector3Wrapper( quat::frame_rotation(self.0, v.0) )
625 }
626
627 /// Lerp (Linear interpolation)
628 ///
629 /// Generate a quaternion that interpolate the shortest path from `self` to `other`
630 /// (The norm of `self` and `other` must be 1).
631 /// The argument `t (0 <= t <= 1)` is the interpolation parameter.
632 ///
633 /// Normalization is not performed internally because
634 /// it increases the computational complexity.
635 #[inline]
636 pub fn lerp(self, other: QuaternionWrapper<T>, t: T) -> Self {
637 Self( quat::lerp(self.0, other.0, t) )
638 }
639
640 /// Slerp (Spherical linear interpolation)
641 ///
642 /// Generate a quaternion that interpolate the shortest path from `self` to `other`.
643 /// The argument `t(0 <= t <= 1)` is the interpolation parameter.
644 ///
645 /// The norm of `self` and `other` must be 1 (Versor).
646 #[inline]
647 pub fn slerp(self, other: QuaternionWrapper<T>, t: T) -> Self {
648 Self( quat::slerp(self.0, other.0, t) )
649 }
650}
651
652// ------------------------- Vector3 ------------------------- //
653impl<T: Float> Vector3Wrapper<T> {
654 /// Create a new Vector3Wrapper
655 #[inline]
656 pub fn new(v: Vector3<T>) -> Self {
657 Self(v)
658 }
659
660 /// Returns the `Vector3<T>`
661 #[inline]
662 pub fn unwrap(self) -> Vector3<T> {
663 self.0
664 }
665
666 /// Product of Vector3Wrapper and DCM.
667 ///
668 /// `m * self`
669 #[inline]
670 pub fn matrix_product(self, m: DCM<T>) -> Self {
671 Self( quat::matrix_product(m, self.0) )
672 }
673
674 /// Sum of the element of the vector.
675 #[inline]
676 pub fn sum(self) -> ScalarWrapper<T> {
677 ScalarWrapper( quat::sum(self.0) )
678 }
679
680 /// Calculate `s*self + b`
681 ///
682 /// If the `fma` feature is enabled, the FMA calculation is performed using the `mul_add` method.
683 /// If not enabled, it's computed by unfused multiply-add (s*a + b).
684 #[inline]
685 pub fn scale_add(self, s: ScalarWrapper<T>, b: Vector3Wrapper<T>) -> Self {
686 Self( quat::scale_add(s.0, self.0, b.0) )
687 }
688
689 /// Hadamard product of vector.
690 ///
691 /// Calculate `a ∘ b`
692 #[inline]
693 pub fn hadamard(self, other: Vector3Wrapper<T>) -> Self {
694 Self( quat::hadamard(self.0, other.0) )
695 }
696
697 /// Hadamard product and Addiction of Vector.
698 ///
699 /// Calculate `a ∘ b + c`
700 ///
701 /// If the `fma` feature is enabled, the FMA calculation is performed using the `mul_add` method.
702 /// If not enabled, it's computed by unfused multiply-add (s*a + b).
703 #[inline]
704 pub fn hadamard_add(self, b: Vector3Wrapper<T>, c: Vector3Wrapper<T>) -> Self {
705 Self( quat::hadamard_add(self.0, b.0, c.0) )
706 }
707
708 /// Dot product of the vector.
709 #[inline]
710 pub fn dot(self, other: Vector3Wrapper<T>) -> ScalarWrapper<T> {
711 ScalarWrapper( quat::dot(self.0, other.0) )
712 }
713
714 /// Cross product of the vector.
715 ///
716 /// `self × other`
717 #[inline]
718 pub fn cross(self, other: Vector3Wrapper<T>) -> Self {
719 Self( quat::cross(self.0, other.0) )
720 }
721
722 /// Calcurate the L2 norm of the vector.
723 #[inline]
724 pub fn norm(self) -> ScalarWrapper<T> {
725 ScalarWrapper( quat::norm(self.0) )
726 }
727
728 /// Normalization of vector.
729 ///
730 /// If you enter a zero vector, it returns a zero vector.
731 ///
732 /// # Examples
733 ///
734 /// ```
735 /// # use quaternion_wrapper::Vector3Wrapper;
736 /// // This norm is not 1.
737 /// let v = Vector3Wrapper::<f64>::new([1.0, 2.0, 3.0]);
738 /// assert!( (1.0 - v.norm().unwrap()).abs() > 1e-12 );
739 ///
740 /// // Now that normalized, this norm is 1!
741 /// let v_n = v.normalize();
742 /// assert!( (1.0 - v_n.norm().unwrap()).abs() < 1e-12 );
743 /// ```
744 #[inline]
745 pub fn normalize(self) -> Self {
746 Self( quat::normalize(self.0) )
747 }
748
749 /// Calculate the inverse of pure quaternion.
750 ///
751 /// # Examples
752 ///
753 /// ```
754 /// # use quaternion_wrapper::Vector3Wrapper;
755 /// let v = Vector3Wrapper::<f64>::new( [1.0, 2.0, 3.0] );
756 ///
757 /// // Identity quaternion
758 /// let id = (v * v.inv()).unwrap(); // = (v.inv() * v).unwrap()
759 ///
760 /// assert!( (id.0 - 1.0).abs() < 1e-12 );
761 /// assert!( id.1[0].abs() < 1e-12 );
762 /// assert!( id.1[1].abs() < 1e-12 );
763 /// assert!( id.1[2].abs() < 1e-12 );
764 /// ```
765 #[inline]
766 pub fn inv(self) -> Self {
767 Self( quat::inv(self.0) )
768 }
769
770 /// Exponential function of vector.
771 #[inline]
772 pub fn exp(self) -> QuaternionWrapper<T> {
773 QuaternionWrapper( quat::exp(self.0) )
774 }
775}
776
777// --------------------- Scalar -------------------- //
778impl<T: Float> ScalarWrapper<T> {
779 /// Create a new ScalarWrapper
780 #[inline]
781 pub fn new(s: T) -> Self {
782 Self(s)
783 }
784
785 /// Returns the `T`
786 #[inline]
787 pub fn unwrap(self) -> T {
788 self.0
789 }
790}