maliput/math/
mod.rs

1// BSD 3-Clause License
2//
3// Copyright (c) 2024, Woven by Toyota.
4// All rights reserved.
5//
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are met:
8//
9// * Redistributions of source code must retain the above copyright notice, this
10//   list of conditions and the following disclaimer.
11//
12// * Redistributions in binary form must reproduce the above copyright notice,
13//   this list of conditions and the following disclaimer in the documentation
14//   and/or other materials provided with the distribution.
15//
16// * Neither the name of the copyright holder nor the names of its
17//   contributors may be used to endorse or promote products derived from
18//   this software without specific prior written permission.
19//
20// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30
31/// A 3D vector.
32/// Wrapper around C++ implementation `maliput::math::Vector3`.
33///
34/// ## Example
35///
36/// ```rust, no_run
37/// use maliput::math::Vector3;
38///
39/// let v = Vector3::new(1.0, 2.0, 3.0);
40/// println!("v = {}", v);
41/// assert_eq!(v.x(), 1.0);
42/// assert_eq!(v.y(), 2.0);
43/// assert_eq!(v.z(), 3.0);
44/// assert_eq!(v.norm(), (1.0_f64.powi(2) + 2.0_f64.powi(2) + 3.0_f64.powi(2)).sqrt());
45/// assert_eq!(v.dot(&v), 1.0_f64.powi(2) + 2.0_f64.powi(2) + 3.0_f64.powi(2));
46/// assert_eq!(v.cross(&v), Vector3::new(0.0, 0.0, 0.0));
47/// ```
48pub struct Vector3 {
49    pub(crate) v: cxx::UniquePtr<maliput_sys::math::ffi::Vector3>,
50}
51
52impl Vector3 {
53    /// Create a new `Vector3` with the given `x`, `y`, and `z` components.
54    pub fn new(x: f64, y: f64, z: f64) -> Vector3 {
55        Vector3 {
56            v: maliput_sys::math::ffi::Vector3_new(x, y, z),
57        }
58    }
59    /// Get the `x` component of the `Vector3`.
60    pub fn x(&self) -> f64 {
61        self.v.x()
62    }
63    /// Get the `y` component of the `Vector3`.
64    pub fn y(&self) -> f64 {
65        self.v.y()
66    }
67    /// Get the `z` component of the `Vector3`.
68    pub fn z(&self) -> f64 {
69        self.v.z()
70    }
71    /// Get the norm of the `Vector3`.
72    pub fn norm(&self) -> f64 {
73        self.v.norm()
74    }
75    /// Normalize the `Vector3`.
76    pub fn normalize(&mut self) {
77        self.v.as_mut().expect("Unexpected error").normalize();
78    }
79    /// Get the dot product of the `Vector3` with another `Vector3`.
80    pub fn dot(&self, w: &Vector3) -> f64 {
81        maliput_sys::math::ffi::Vector3_dot(&self.v, &w.v)
82    }
83    /// Get the cross product of the `Vector3` with another `Vector3`.
84    pub fn cross(&self, w: &Vector3) -> Vector3 {
85        Vector3 {
86            v: maliput_sys::math::ffi::Vector3_cross(&self.v, &w.v),
87        }
88    }
89}
90
91impl PartialEq for Vector3 {
92    fn eq(&self, other: &Self) -> bool {
93        maliput_sys::math::ffi::Vector3_equals(&self.v, &other.v)
94    }
95}
96
97impl Eq for Vector3 {}
98
99impl std::fmt::Display for Vector3 {
100    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
101        write!(f, "{}", maliput_sys::math::ffi::Vector3_to_str(&self.v))
102    }
103}
104
105impl std::fmt::Debug for Vector3 {
106    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
107        f.debug_struct("Vector3")
108            .field("x", &self.x())
109            .field("y", &self.y())
110            .field("z", &self.z())
111            .finish()
112    }
113}
114
115/// A 4D vector.
116/// Wrapper around C++ implementation `maliput::math::Vector4`.
117///
118/// ## Example
119///
120/// ```rust, no_run
121/// use maliput::math::Vector4;
122///
123/// let v = Vector4::new(1.0, 2.0, 3.0, 4.0);
124/// println!("v = {}", v);
125/// assert_eq!(v.x(), 1.0);
126/// assert_eq!(v.y(), 2.0);
127/// assert_eq!(v.z(), 3.0);
128/// assert_eq!(v.w(), 4.0);
129/// assert_eq!(v.norm(), (1.0_f64.powi(2) + 2.0_f64.powi(2) + 3.0_f64.powi(2) + 4.0_f64.powi(2)).sqrt());
130/// ```
131pub struct Vector4 {
132    v: cxx::UniquePtr<maliput_sys::math::ffi::Vector4>,
133}
134
135impl Vector4 {
136    /// Create a new `Vector4` with the given `x`, `y`, `z` and `w` components.
137    pub fn new(x: f64, y: f64, z: f64, w: f64) -> Vector4 {
138        Vector4 {
139            v: maliput_sys::math::ffi::Vector4_new(x, y, z, w),
140        }
141    }
142    /// Get the `x` component of the `Vector4`.
143    pub fn x(&self) -> f64 {
144        self.v.x()
145    }
146    /// Get the `y` component of the `Vector4`.
147    pub fn y(&self) -> f64 {
148        self.v.y()
149    }
150    /// Get the `z` component of the `Vector4`.
151    pub fn z(&self) -> f64 {
152        self.v.z()
153    }
154    /// Get the `w` component of the `Vector4`.
155    pub fn w(&self) -> f64 {
156        self.v.w()
157    }
158    /// Get the norm of the `Vector4`.
159    pub fn norm(&self) -> f64 {
160        self.v.norm()
161    }
162    /// Get the dot product of the `Vector4` with another `Vector4`.
163    pub fn dot(&self, w: &Vector4) -> f64 {
164        maliput_sys::math::ffi::Vector4_dot(&self.v, &w.v)
165    }
166    /// Normalize the `Vector4`.
167    pub fn normalize(&mut self) {
168        self.v.as_mut().expect("Unexpected error").normalize();
169    }
170}
171
172impl PartialEq for Vector4 {
173    fn eq(&self, other: &Self) -> bool {
174        maliput_sys::math::ffi::Vector4_equals(&self.v, &other.v)
175    }
176}
177
178impl Eq for Vector4 {}
179
180impl std::fmt::Display for Vector4 {
181    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
182        write!(f, "{}", maliput_sys::math::ffi::Vector4_to_str(&self.v))
183    }
184}
185
186impl std::fmt::Debug for Vector4 {
187    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
188        f.debug_struct("Vector4")
189            .field("x", &self.x())
190            .field("y", &self.y())
191            .field("z", &self.z())
192            .field("w", &self.z())
193            .finish()
194    }
195}
196
197/// A 3x3 matrix.
198/// Wrapper around C++ implementation `maliput::math::Matrix3`.
199///
200/// ## Example
201///
202/// ```rust, no_run
203/// use maliput::math::Matrix3;
204/// use maliput::math::Vector3;
205///
206/// let row_1 = Vector3::new(1.0, 2.0, 3.0);
207/// let row_2 = Vector3::new(4.0, 5.0, 6.0);
208/// let row_3 = Vector3::new(7.0, 8.0, 9.0);
209/// let m = Matrix3::new(row_1, row_2, row_3);
210/// ```
211pub struct Matrix3 {
212    m: cxx::UniquePtr<maliput_sys::math::ffi::Matrix3>,
213}
214
215impl Matrix3 {
216    /// Create a new `Matrix3` with the given `row1`, `row2`, and `row3`.
217    pub fn new(row1: Vector3, row2: Vector3, row3: Vector3) -> Matrix3 {
218        Matrix3 {
219            m: maliput_sys::math::ffi::Matrix3_new(
220                row1.x(),
221                row1.y(),
222                row1.z(),
223                row2.x(),
224                row2.y(),
225                row2.z(),
226                row3.x(),
227                row3.y(),
228                row3.z(),
229            ),
230        }
231    }
232    /// Get the cofactor of the `Matrix3` at the given `row` and `col`.
233    pub fn cofactor(&self, row: usize, col: usize) -> f64 {
234        self.m.cofactor(row, col)
235    }
236    /// Get the cofactor matrix of the `Matrix3`.
237    pub fn cofactor_matrix(&self) -> Matrix3 {
238        Matrix3 {
239            m: maliput_sys::math::ffi::Matrix3_cofactor_matrix(&self.m),
240        }
241    }
242    /// Get the determinant of the `Matrix3`.
243    pub fn determinant(&self) -> f64 {
244        self.m.determinant()
245    }
246    /// Check if the `Matrix3` is singular.
247    pub fn is_singular(&self) -> bool {
248        self.m.is_singular()
249    }
250    /// Get the row at the given `index`.
251    pub fn row(&self, index: usize) -> Vector3 {
252        Vector3 {
253            v: maliput_sys::math::ffi::Matrix3_row(&self.m, index),
254        }
255    }
256    /// Get the column at the given `index`.
257    pub fn col(&self, index: usize) -> Vector3 {
258        Vector3 {
259            v: maliput_sys::math::ffi::Matrix3_col(&self.m, index),
260        }
261    }
262    /// Get the transpose of the `Matrix3`.
263    pub fn transpose(&self) -> Matrix3 {
264        Matrix3 {
265            m: maliput_sys::math::ffi::Matrix3_transpose(&self.m),
266        }
267    }
268    /// Get the adjoint of the `Matrix3`.
269    pub fn adjoint(&self) -> Matrix3 {
270        Matrix3 {
271            m: maliput_sys::math::ffi::Matrix3_adjoint(&self.m),
272        }
273    }
274    /// Get the inverse of the `Matrix3`.
275    pub fn inverse(&self) -> Matrix3 {
276        Matrix3 {
277            m: maliput_sys::math::ffi::Matrix3_inverse(&self.m),
278        }
279    }
280}
281
282impl PartialEq for Matrix3 {
283    fn eq(&self, other: &Self) -> bool {
284        maliput_sys::math::ffi::Matrix3_equals(&self.m, &other.m)
285    }
286}
287
288impl Eq for Matrix3 {}
289
290impl std::fmt::Display for Matrix3 {
291    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
292        write!(f, "{}", maliput_sys::math::ffi::Matrix3_to_str(&self.m))
293    }
294}
295
296impl std::fmt::Debug for Matrix3 {
297    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
298        f.debug_struct("Matrix3")
299            .field("row1", &self.row(0))
300            .field("row2", &self.row(1))
301            .field("row3", &self.row(2))
302            .finish()
303    }
304}
305
306/// A quaternion.
307/// Wrapper around C++ implementation `maliput::math::Quaternion`.
308pub struct Quaternion {
309    q: cxx::UniquePtr<maliput_sys::math::ffi::Quaternion>,
310}
311
312impl Quaternion {
313    /// Create a new `Quaternion` with the given `w`, `x`, `y`, and `z` components.
314    /// The `w` component is the real part of the quaternion.
315    /// The `x`, `y`, and `z` components are the imaginary parts of the quaternion.
316    pub fn new(w: f64, x: f64, y: f64, z: f64) -> Quaternion {
317        Quaternion {
318            q: maliput_sys::math::ffi::Quaternion_new(w, x, y, z),
319        }
320    }
321
322    /// Get the `w` component of the `Quaternion`.
323    pub fn w(&self) -> f64 {
324        self.q.w()
325    }
326
327    /// Get the `x` component of the `Quaternion`.
328    pub fn x(&self) -> f64 {
329        self.q.x()
330    }
331    /// Get the `y` component of the `Quaternion`.
332    pub fn y(&self) -> f64 {
333        self.q.y()
334    }
335    /// Get the `z` component of the `Quaternion`.
336    pub fn z(&self) -> f64 {
337        self.q.z()
338    }
339    /// Returns this quaternion Vector.
340    pub fn vec(&self) -> Vector3 {
341        Vector3 {
342            v: maliput_sys::math::ffi::Quaternion_vec(&self.q),
343        }
344    }
345    /// Returns this quaternion coefficients.
346    pub fn coeffs(&self) -> Vector4 {
347        Vector4 {
348            v: maliput_sys::math::ffi::Quaternion_coeffs(&self.q),
349        }
350    }
351    /// Get the dot product of the `Quaternion` with another `Quaternion`.
352    pub fn dot(&self, other: &Quaternion) -> f64 {
353        self.q.dot(&other.q)
354    }
355    /// Get the angular distance between the `Quaternion` and another `Quaternion`.
356    pub fn angular_distance(&self, other: &Quaternion) -> f64 {
357        self.q.AngularDistance(&other.q)
358    }
359    /// Get the norm of the `Quaternion`.
360    pub fn norm(&self) -> f64 {
361        self.q.norm()
362    }
363    /// Normalize the `Quaternion`.
364    pub fn normalize(&mut self) {
365        self.q.as_mut().expect("Unexpected error").normalize();
366    }
367    /// Get the squared norm of the `Quaternion`.
368    pub fn squared_norm(&self) -> f64 {
369        self.q.squared_norm()
370    }
371    /// Get the inverse of the `Quaternion`.
372    pub fn inverse(&self) -> Quaternion {
373        Quaternion {
374            q: maliput_sys::math::ffi::Quaternion_Inverse(&self.q),
375        }
376    }
377    /// Get the conjugate of the `Quaternion`.
378    pub fn conjugate(&self) -> Quaternion {
379        Quaternion {
380            q: maliput_sys::math::ffi::Quaternion_conjugate(&self.q),
381        }
382    }
383    /// Get the rotation matrix representation of the `Quaternion`.
384    pub fn to_rotation_matrix(&self) -> Matrix3 {
385        let q = maliput_sys::math::ffi::Quaternion_ToRotationMatrix(&self.q);
386        Matrix3 { m: q }
387    }
388    /// Apply the `Quaternion` to a `Vector3`.
389    pub fn transform_vector(&self, v: &Vector3) -> Vector3 {
390        let q = maliput_sys::math::ffi::Quaternion_TransformVector(&self.q, &v.v);
391        Vector3 { v: q }
392    }
393}
394
395impl PartialEq for Quaternion {
396    fn eq(&self, other: &Self) -> bool {
397        maliput_sys::math::ffi::Quaternion_equals(&self.q, &other.q)
398    }
399}
400
401impl Eq for Quaternion {}
402
403impl std::fmt::Display for Quaternion {
404    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
405        write!(f, "{}", maliput_sys::math::ffi::Quaternion_to_str(&self.q))
406    }
407}
408impl std::fmt::Debug for Quaternion {
409    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
410        f.debug_struct("Quaternion")
411            .field("w", &self.w())
412            .field("x", &self.x())
413            .field("y", &self.y())
414            .field("z", &self.z())
415            .finish()
416    }
417}
418
419/// This class represents the orientation between two arbitrary frames A and D
420/// associated with a Space-fixed (extrinsic) X-Y-Z rotation by "roll-pitch-yaw"
421/// angles `[r, p, y]`, which is equivalent to a Body-fixed (intrinsic) Z-Y-X
422/// rotation by "yaw-pitch-roll" angles `[y, p, r]`.  The rotation matrix `R_AD`
423/// associated with this roll-pitch-yaw `[r, p, y]` rotation sequence is equal
424/// to the matrix multiplication shown below.
425/// ```md, no_run
426///        ⎡cos(y) -sin(y)  0⎤   ⎡ cos(p)  0  sin(p)⎤   ⎡1      0        0 ⎤
427/// R_AD = ⎢sin(y)  cos(y)  0⎥ * ⎢     0   1      0 ⎥ * ⎢0  cos(r)  -sin(r)⎥
428///        ⎣    0       0   1⎦   ⎣-sin(p)  0  cos(p)⎦   ⎣0  sin(r)   cos(r)⎦
429///      =       R_AB          *        R_BC          *        R_CD
430/// ```
431///
432/// Wrapper of C++ implementation `maliput::math::RollPitchYaw`.
433pub struct RollPitchYaw {
434    rpy: cxx::UniquePtr<maliput_sys::math::ffi::RollPitchYaw>,
435}
436
437impl RollPitchYaw {
438    /// Create a new `RollPitchYaw` with the given `roll`, `pitch`, and `yaw` angles.
439    pub fn new(roll: f64, pitch: f64, yaw: f64) -> RollPitchYaw {
440        RollPitchYaw {
441            rpy: maliput_sys::math::ffi::RollPitchYaw_new(roll, pitch, yaw),
442        }
443    }
444    /// Get the roll angle of the `RollPitchYaw`.
445    pub fn roll_angle(&self) -> f64 {
446        self.rpy.roll_angle()
447    }
448    /// Get the pitch angle of the `RollPitchYaw`.
449    pub fn pitch_angle(&self) -> f64 {
450        self.rpy.pitch_angle()
451    }
452    /// Get the yaw angle of the `RollPitchYaw`.
453    pub fn yaw_angle(&self) -> f64 {
454        self.rpy.yaw_angle()
455    }
456    /// Get the vector of the `RollPitchYaw`.
457    pub fn vector(&self) -> Vector3 {
458        Vector3::new(self.rpy.vector().x(), self.rpy.vector().y(), self.rpy.vector().z())
459    }
460    /// Set the `RollPitchYaw` with the given `roll`, `pitch`, and `yaw` angles.
461    pub fn set(&mut self, roll: f64, pitch: f64, yaw: f64) {
462        maliput_sys::math::ffi::RollPitchYaw_set(self.rpy.as_mut().expect("Unexpected Error"), roll, pitch, yaw);
463    }
464    /// Set the `RollPitchYaw` from the given `Quaternion`.
465    pub fn set_from_quaternion(&mut self, q: &Quaternion) {
466        maliput_sys::math::ffi::RollPitchYaw_SetFromQuaternion(self.rpy.as_mut().expect("Unexpected Error"), &q.q);
467    }
468    /// Get the `Quaternion` representation of the `RollPitchYaw`.
469    pub fn to_quaternion(&self) -> Quaternion {
470        Quaternion {
471            q: maliput_sys::math::ffi::RollPitchYaw_ToQuaternion(&self.rpy),
472        }
473    }
474    /// Get the rotation matrix representation of the `RollPitchYaw`.
475    pub fn to_matrix(&self) -> Matrix3 {
476        Matrix3 {
477            m: maliput_sys::math::ffi::RollPitchYaw_ToMatrix(&self.rpy),
478        }
479    }
480    /// Get the rotation matrix representation of the `RollPitchYaw` with the given `rpy_dt`.
481    /// ## Description
482    /// Forms Ṙ, the ordinary derivative of the RotationMatrix `R` with respect
483    /// to an independent variable `t` (`t` usually denotes time) and `R` is the
484    /// RotationMatrix formed by `this` `RollPitchYaw`.  The roll-pitch-yaw angles
485    /// r, p, y are regarded as functions of `t` [i.e., r(t), p(t), y(t)].
486    /// The param `rpy_dt` Ordinary derivative of rpy with respect to an independent
487    /// variable `t` (`t` usually denotes time, but not necessarily).
488    /// Returns `Ṙ``, the ordinary derivative of `R` with respect to `t`, calculated
489    /// as `Ṙ = ∂R/∂r * ṙ + ∂R/∂p * ṗ + ∂R/∂y * ẏ`.  In other words, the returned
490    /// (i, j) element is `∂Rij/∂r * ṙ + ∂Rij/∂p * ṗ + ∂Rij/∂y * ẏ``.
491    ///
492    /// Note: Docs are taken from the C++ implementation.
493    /// ## Args
494    /// - `rpy_dt` Ordinary derivative of rpy with respect to an independent variable `t`.
495    /// ## Returns
496    /// Ordinary derivative of the RotationMatrix `R` with respect to an independent variable `t`.
497    pub fn calc_rotation_matrix_dt(&self, rpy_dt: &Vector3) -> Matrix3 {
498        Matrix3 {
499            m: maliput_sys::math::ffi::RollPitchYaw_CalcRotationMatrixDt(&self.rpy, &rpy_dt.v),
500        }
501    }
502}
503
504mod tests {
505    #[test]
506    fn vector3_new() {
507        let v = super::Vector3::new(1.0, 2.0, 3.0);
508        assert_eq!(v.x(), 1.0);
509        assert_eq!(v.y(), 2.0);
510        assert_eq!(v.z(), 3.0);
511    }
512
513    #[test]
514    fn vector3_equality() {
515        let v = super::Vector3::new(1.0, 2.0, 3.0);
516        let w = super::Vector3::new(1.0, 2.0, 3.0);
517        assert_eq!(v, w);
518        let z = super::Vector3::new(4.0, 5.0, 6.0);
519        assert_ne!(v, z);
520    }
521
522    #[test]
523    fn vector3_norm() {
524        let v = super::Vector3::new(1.0, 2.0, 3.0);
525        assert_eq!(v.norm(), (v.x() * v.x() + v.y() * v.y() + v.z() * v.z()).sqrt());
526    }
527
528    #[test]
529    fn vector3_normalize() {
530        let mut v = super::Vector3::new(1.0, 2.0, 3.0);
531        let norm = v.norm();
532        v.normalize();
533        assert_eq!(v.x(), 1.0 / norm);
534        assert_eq!(v.y(), 2.0 / norm);
535        assert_eq!(v.z(), 3.0 / norm);
536    }
537
538    #[test]
539    fn vector3_dot() {
540        let v = super::Vector3::new(1.0, 2.0, 3.0);
541        let w = super::Vector3::new(4.0, 5.0, 6.0);
542        assert_eq!(v.dot(&w), v.x() * w.x() + v.y() * w.y() + v.z() * w.z());
543    }
544
545    #[test]
546    fn vector3_cross() {
547        let v = super::Vector3::new(1.0, 2.0, 3.0);
548        let w = super::Vector3::new(4.0, 5.0, 6.0);
549        let cross = v.cross(&w);
550        assert_eq!(cross.x(), v.y() * w.z() - v.z() * w.y());
551        assert_eq!(cross.y(), v.z() * w.x() - v.x() * w.z());
552        assert_eq!(cross.z(), v.x() * w.y() - v.y() * w.x());
553    }
554
555    #[test]
556    fn vector4_new() {
557        let v = super::Vector4::new(1.0, 2.0, 3.0, 4.0);
558        assert_eq!(v.x(), 1.0);
559        assert_eq!(v.y(), 2.0);
560        assert_eq!(v.z(), 3.0);
561        assert_eq!(v.w(), 4.0);
562    }
563
564    #[test]
565    fn vector4_equality() {
566        let v = super::Vector4::new(1.0, 2.0, 3.0, 4.0);
567        let w = super::Vector4::new(1.0, 2.0, 3.0, 4.0);
568        assert_eq!(v, w);
569        let z = super::Vector4::new(4.0, 5.0, 6.0, 7.0);
570        assert_ne!(v, z);
571    }
572
573    #[test]
574    fn vector4_norm() {
575        let v = super::Vector4::new(1.0, 2.0, 3.0, 4.0);
576        assert_eq!(
577            v.norm(),
578            (v.x() * v.x() + v.y() * v.y() + v.z() * v.z() + v.w() * v.w()).sqrt()
579        );
580    }
581
582    #[test]
583    fn vector4_dot() {
584        let v = super::Vector4::new(1.0, 2.0, 3.0, 4.0);
585        let w = super::Vector4::new(4.0, 5.0, 6.0, 7.0);
586        assert_eq!(v.dot(&w), v.x() * w.x() + v.y() * w.y() + v.z() * w.z() + v.w() * w.w());
587    }
588
589    #[test]
590    fn vector4_normalize() {
591        let mut v = super::Vector4::new(1.0, 2.0, 3.0, 4.0);
592        let norm = v.norm();
593        v.normalize();
594        assert_eq!(v.x(), 1.0 / norm);
595        assert_eq!(v.y(), 2.0 / norm);
596        assert_eq!(v.z(), 3.0 / norm);
597        assert_eq!(v.w(), 4.0 / norm);
598    }
599
600    #[test]
601    fn matrix4_tests() {
602        let row_1 = super::Vector3::new(1.0, 2.0, 3.0);
603        let row_2 = super::Vector3::new(4.0, 5.0, 6.0);
604        let row_3 = super::Vector3::new(7.0, 8.0, 9.0);
605        let _m = super::Matrix3::new(row_1, row_2, row_3);
606        assert_eq!(_m.row(0).x(), 1.0);
607        assert_eq!(_m.col(2).z(), 9.0);
608        // TODO(francocipollone): Add tests for the rest of the API.
609    }
610
611    #[test]
612    fn quaternion_tests() {
613        let q = super::Quaternion::new(1.0, 2.0, 3.0, 4.0);
614        assert_eq!(q.w(), 1.0);
615        assert_eq!(q.x(), 2.0);
616        assert_eq!(q.y(), 3.0);
617        assert_eq!(q.z(), 4.0);
618        // TODO(francocipollone): Add tests for the rest of the API.
619    }
620
621    #[test]
622    fn roll_pitch_yaw_tests() {
623        let rpy = super::RollPitchYaw::new(1.0, 2.0, 3.0);
624        assert_eq!(rpy.roll_angle(), 1.0);
625        assert_eq!(rpy.pitch_angle(), 2.0);
626        assert_eq!(rpy.yaw_angle(), 3.0);
627        // TODO(francocipollone): Add tests for the rest of the API.
628    }
629}
maliput/math/mod.rs

maliput/math/
mod.rs
