├── .gitignore ├── Cargo.toml ├── examples ├── inverse.rs ├── solve_linear_system.rs ├── nice_square_example.rs ├── for_each_example.rs ├── dual_quaternion_screw_parameters.rs ├── dual_quaternion_transform_point.rs ├── eigenvalues.rs ├── quaternions.rs ├── dual_quaternions.rs └── qr_example.rs ├── src ├── errors.rs ├── traits.rs ├── lib.rs ├── utils.rs ├── slices_methods.rs ├── vector2.rs ├── vector4.rs ├── vector5.rs ├── vector3.rs ├── vector6.rs ├── matrix2x2.rs ├── quaternion.rs └── matrix3x3.rs ├── benches └── bench_inverse.rs └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | Cargo.lock 3 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "static-math" 3 | version = "0.2.3" 4 | authors = ["elsuizo "] 5 | edition = "2018" 6 | description = "Fast mathematical operations with static arrays, without `unsafe` code" 7 | repository = "https://github.com/elsuizo/static-math" 8 | documentation = "https://docs.rs/static-math/" 9 | license = "MIT" 10 | keywords = ["linear-algebra", "matrix-operations", "quaternions", "robotics", "dual-quaternions"] 11 | categories = ["science"] 12 | 13 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 14 | [dependencies] 15 | num = { version = "0.4", default-features = false} 16 | 17 | [features] 18 | default = ["num/std"] 19 | no-std = ["num/libm"] 20 | 21 | [dev-dependencies] 22 | criterion = "0.3" 23 | 24 | [[bench]] 25 | name = "bench_inverse" 26 | harness = false 27 | -------------------------------------------------------------------------------- /examples/inverse.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file inverse.rs 3 | // 4 | // @date 06/25/20 19:32:54 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence: 13 | // This program is free software: you can redistribute it and/or modify 14 | // it under the terms of the GNU General Public License as published by 15 | // the Free Software Foundation, either version 3 of the License, or (at 16 | // your option) any later version. 17 | // 18 | // This program is distributed in the hope that it will be useful, but 19 | // WITHOUT ANY WARRANTY; without even the implied warranty of 20 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 21 | // General Public License for more details. 22 | // 23 | // You should have received a copy of the GNU General Public License 24 | //-------------------------------------------------------------------------- 25 | use static_math::{M66, m66_new}; 26 | use static_math::traits::LinearAlgebra; 27 | 28 | fn main() { 29 | 30 | let m66 = m66_new!( 1.0, 1.0, 3.0, 4.0, 9.0, 3.0; 31 | 10.0, 10.0, 1.0, 2.0, 2.0, 5.0; 32 | 2.0, 9.0, 6.0, 10.0, 10.0, 9.0; 33 | 10.0, 9.0, 9.0, 7.0, 3.0, 6.0; 34 | 7.0, 6.0, 6.0, 2.0, 9.0, 5.0; 35 | 3.0, 8.0, 1.0, 4.0, 1.0, 5.0); 36 | 37 | if let Some(inv) = m66.inverse() { 38 | println!("inverse: {}", inv); 39 | println!("check: {}", inv * m66); 40 | } 41 | } 42 | 43 | 44 | -------------------------------------------------------------------------------- /examples/solve_linear_system.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file solve_linear_system.rs 3 | // 4 | // @date 06/27/20 22:43:12 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence: 13 | // This program is free software: you can redistribute it and/or modify 14 | // it under the terms of the GNU General Public License as published by 15 | // the Free Software Foundation, either version 3 of the License, or (at 16 | // your option) any later version. 17 | // 18 | // This program is distributed in the hope that it will be useful, but 19 | // WITHOUT ANY WARRANTY; without even the implied warranty of 20 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 21 | // General Public License for more details. 22 | // 23 | // You should have received a copy of the GNU General Public License 24 | //-------------------------------------------------------------------------- 25 | use static_math::{M66, V6, m66_new}; 26 | use static_math::traits::LinearAlgebra; 27 | 28 | fn main() { 29 | 30 | let a = m66_new!( 1.0, 1.0, 3.0, 4.0, 9.0, 3.0; 31 | 10.0, 10.0, 1.0, 2.0, 2.0, 5.0; 32 | 2.0, 9.0, 6.0, 10.0, 10.0, 9.0; 33 | 10.0, 9.0, 9.0, 7.0, 3.0, 6.0; 34 | 7.0, 6.0, 6.0, 2.0, 9.0, 5.0; 35 | 3.0, 8.0, 1.0, 4.0, 1.0, 5.0); 36 | 37 | let b = V6::new_from(0.0, 1.0, 3.0, 0.0, 1.0, 2.0); 38 | 39 | if let Some(inv) = a.inverse() { 40 | let solution = inv * b; 41 | println!("the solution is: {}", solution); 42 | println!("verification: a * solution = b?: {}", a * solution); 43 | println!("b: {}", b); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /examples/nice_square_example.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file nice_square_example.rs 3 | // 4 | // @date 09/17/20 13:11:50 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use static_math::{M22, m22_new}; 32 | // i see this tweet and i want prove that 33 | // https://twitter.com/FlammableMaths/status/1306303462607654914 34 | fn main() { 35 | 36 | let m = m22_new!(3, 4; 37 | 6, 8); 38 | let m_square = m * m; 39 | println!("m: {:}", m); 40 | println!("m_square: {:}", m_square); 41 | } 42 | 43 | -------------------------------------------------------------------------------- /src/errors.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file errors.rs 3 | // 4 | // @date 06/01/20 22:20:11 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //-------------------------------------------------------------------------- 31 | // use std::fmt; 32 | // use std::error; 33 | // TODO(elsuizo:2020-08-11): if we need the eigenvalues 34 | pub type Result = ::core::result::Result; 35 | 36 | /// Errors from Vectors 37 | #[derive(Debug)] 38 | pub enum VectorErrors { 39 | /// the norm cannot be zero 40 | Norm2IsZero, 41 | } 42 | 43 | #[derive(Debug)] 44 | pub enum EigenvalueError { 45 | MaxIterations, 46 | } 47 | -------------------------------------------------------------------------------- /examples/for_each_example.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file for_each_example.rs 3 | // 4 | // @date 09/03/20 15:08:39 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use static_math::{M66, m66_new}; 32 | 33 | // Apply the closure f to all the elements of the matrix 34 | fn main() { 35 | 36 | let m66 = m66_new!( 1.0, 1.0, 3.0, 4.0, 9.0, 3.0; 37 | 10.0, 10.0, 1.0, 2.0, 2.0, 5.0; 38 | 2.0, 9.0, 6.0, 10.0, 10.0, 9.0; 39 | 10.0, 9.0, 9.0, 7.0, 3.0, 6.0; 40 | 7.0, 6.0, 6.0, 2.0, 9.0, 5.0; 41 | 3.0, 8.0, 1.0, 4.0, 1.0, 5.0); 42 | 43 | let result = m66.for_each(|element: f32| element.cos() + element.sin()); 44 | println!("result: {:}", result); 45 | } 46 | -------------------------------------------------------------------------------- /examples/dual_quaternion_screw_parameters.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file dual_quaternion_screw_parameters.rs 3 | // 4 | // @date 05/22/21 20:49:24 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2021> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use static_math::{Quaternion, DualQuaternion, V3}; 32 | 33 | // In this example we rotate around the z axis 45 degrees and translate along the z axis 34 | // 7 units, so the l vector should be z_hat, d should be 7, theta should be 45 35 | // degrees and m should be zero because the moment is zero 36 | fn main() { 37 | let v = V3::z_axis() * 45f32.to_radians(); 38 | let q = Quaternion::rotation_norm_encoded(&v); 39 | let trans = V3::new_from(0.0, 0.0, 7.0); 40 | let dq_full = DualQuaternion::new_from_rot_trans(&q, &trans); 41 | let (l, m, theta, d) = dq_full.get_screw_parameters(); 42 | println!("l: {}", l); 43 | println!("m: {}", m); 44 | println!("theta: {}", theta.to_degrees()); 45 | println!("d: {}", d); 46 | } 47 | 48 | 49 | -------------------------------------------------------------------------------- /examples/dual_quaternion_transform_point.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file dual_quaternion_transform_point.rs 3 | // 4 | // @date 05/20/21 19:39:58 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2021> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use static_math::{DualQuaternion, Quaternion, V3, V4}; 32 | use static_math::transformations::{homogeneous_from_quaternion}; 33 | 34 | // In this example we transform a point with a homogeneous matrix in SE(3) and with 35 | // a DualQuaternion to obtain the same result 36 | // This representation is more compact since there are only 8 elements instead of the 16 of the 37 | // homogeneous matrix 38 | fn main() { 39 | 40 | let q = Quaternion::from_euler_angles(10f32.to_radians(), 30f32.to_radians(), 45f32.to_radians()); 41 | let t = homogeneous_from_quaternion(&q, &V3::new_from(1.0, 2.0, 3.0)); 42 | 43 | let p = V4::new_from(1.0, 2.0, 3.0, 0.0); 44 | let expected = t * p; 45 | 46 | let p = V3::new_from(1.0, 2.0, 3.0); 47 | let dq = DualQuaternion::new_from_homogeneous(&t); 48 | let result = dq.transform_point(&p); 49 | 50 | println!("expected: {}", expected); 51 | println!("result: {}", result); 52 | } 53 | 54 | -------------------------------------------------------------------------------- /src/traits.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file traits.rs 3 | // 4 | // @date 06/01/20 22:19:00 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //-------------------------------------------------------------------------- 31 | /// Generic Trait for Matrix operations and Linear Algebra methods 32 | pub trait LinearAlgebra { 33 | /// get the rows of the matrix 34 | fn rows(&self) -> usize; 35 | 36 | /// get the columns of the matrix 37 | fn cols(&self) -> usize; 38 | 39 | /// get the overal shape of the matrix 40 | fn shape(&self) -> (usize, usize) { 41 | (self.rows(), self.cols()) 42 | } 43 | 44 | /// transpose dimentions of the matrix 45 | fn transpose(&self) -> Self 46 | where 47 | Self: Sized; 48 | 49 | /// get the trace of the matrix 50 | fn trace(&self) -> T; 51 | 52 | /// compute the euclidean norm of the matrix 53 | fn norm2(&self) -> T; 54 | 55 | /// compute the determinant of the matrix 56 | fn det(&self) -> T; 57 | 58 | /// compute the inverse of the matrix 59 | fn inverse(&self) -> Option 60 | where 61 | Self: Sized; 62 | 63 | /// compute the QR factorization of the matrix(if has inverse) 64 | fn qr(&self) -> Option<(Self, Self)> 65 | where 66 | Self: Sized; 67 | 68 | } 69 | -------------------------------------------------------------------------------- /examples/eigenvalues.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file eigenvalues.rs 3 | // 4 | // @date 08/11/20 12:00:53 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2021> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | 32 | /// Example for calculate the real eigenvalues of a Matrix 33 | /// This is a implementation of the amazing lecture of the profesor 34 | /// Gilbert Strang: https://youtu.be/d32WV1rKoVk 35 | 36 | use static_math::{M33, m33_new}; 37 | use static_math::slices_methods::check_elements; 38 | use static_math::traits::LinearAlgebra; 39 | 40 | fn convert_to_similar(m: &mut M33) { 41 | if let Some((q, r)) = m.qr() { 42 | *m = r * q; 43 | } 44 | } 45 | 46 | fn main() { 47 | 48 | let mut m = m33_new!(5.0, 2.0, 0.0; 49 | 2.0, 5.0, 0.0; 50 | -3.0, 4.0, 6.0); 51 | 52 | let mut result = false; 53 | let mut counter = 0; 54 | let max_iterations = 200; 55 | 56 | while !result && counter < max_iterations { 57 | convert_to_similar(&mut m); 58 | let lower = m.get_lower_triangular(); 59 | result = check_elements(&lower, 1e-9); 60 | counter += 1; 61 | } 62 | 63 | let eigenvalues = m.get_diagonal(); 64 | println!("number of iterations: {:}", counter); 65 | println!("eigenvalues: {:}", eigenvalues); 66 | 67 | } 68 | 69 | -------------------------------------------------------------------------------- /examples/quaternions.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file quaternions.rs 3 | // 4 | // @date 09/11/20 11:48:54 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use static_math::{V3, Quaternion}; 32 | use static_math::vector3::X_AXIS; 33 | // In this example we rotate the x axis around the z axis 360 degrees 34 | // to obtain the x axis again, but the rotation is via a composition of 35 | // rotations of 90 degrees 36 | fn main() { 37 | 38 | // vector to rotate: x axis: [1, 0, 0] 39 | let x = X_AXIS; 40 | // quaternion represent the rotation around the z axis 90 degrees, the angle 41 | // is encoded in the vector norm: [0, 0, 90] 42 | let v = V3::z_axis() * 90f32.to_radians(); 43 | let q = Quaternion::rotation_norm_encoded(&v); 44 | let r = q * q * q * q * x; 45 | println!("r: {:}", r); 46 | //------------------------------------------------------------------------- 47 | // Quaternions and euler angles 48 | //------------------------------------------------------------------------- 49 | let q = Quaternion::from_euler_angles(0.1, 0.2, 0.3); 50 | println!("q: {}", q); 51 | let euler_angles = q.to_euler_angles(); 52 | // this would have to give the same value :) 53 | println!("euler_angles: {:?}", euler_angles); 54 | } 55 | -------------------------------------------------------------------------------- /benches/bench_inverse.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file bench_inverse.rs 3 | // 4 | // @date 06/07/20 20:33:40 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // Some benchs for speed reference 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //-------------------------------------------------------------------------- 31 | use criterion::{criterion_group, criterion_main, Criterion}; 32 | 33 | use static_math::traits::LinearAlgebra; 34 | use static_math::matrix6x6::M66; 35 | use static_math::matrix4x4::M44; 36 | 37 | fn inverse6x6_test() { 38 | 39 | let m = M66::::new([ 40 | [1.0, 1.0, 3.0, 4.0, 9.0, 3.0], 41 | [10.0, 10.0, 1.0, 2.0, 2.0, 5.0], 42 | [2.0, 9.0, 6.0, 10.0, 10.0, 9.0], 43 | [10.0, 9.0, 9.0, 7.0, 3.0, 6.0], 44 | [7.0, 6.0, 6.0, 2.0, 9.0, 5.0], 45 | [3.0, 8.0, 1.0, 4.0, 1.0, 5.0], 46 | ]); 47 | 48 | if let Some(_result) = m.inverse() { 49 | } 50 | } 51 | 52 | fn inverse4x4_test() { 53 | 54 | let m = M44::::new([ 55 | [1.0, 1.0, 1.0, -1.0], 56 | [1.0, 1.0, -1.0, 1.0], 57 | [1.0, -1.0, 1.0, 1.0], 58 | [-1.0, 1.0, 1.0, 1.0], 59 | ]); 60 | 61 | if let Some(_result) = m.inverse() { 62 | } 63 | } 64 | 65 | pub fn criterion_benchmark(c: &mut Criterion) { 66 | c.bench_function("inverse 6x6", |b| b.iter(|| inverse6x6_test())); 67 | c.bench_function("inverse 4x4", |b| b.iter(|| inverse4x4_test())); 68 | } 69 | 70 | criterion_group!(benches, criterion_benchmark); 71 | criterion_main!(benches); 72 | -------------------------------------------------------------------------------- /examples/dual_quaternions.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file dual_quaternions.rs 3 | // 4 | // @date 05/19/21 15:14:54 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2021> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use static_math::{DualQuaternion, Quaternion, V3}; 32 | use static_math::transformations::{euler_to_rotation, homogeneous_from_quaternion}; 33 | 34 | // this example show some of the most common methods and function to work with `DualQuaternion`s 35 | fn main() { 36 | let rot = euler_to_rotation(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians(), None); 37 | let q = Quaternion::from_euler_angles(10f32.to_radians(), 10f32.to_radians(), 10f32.to_radians()); 38 | // create a DualQuaternion and back again 39 | let t = homogeneous_from_quaternion(&q, &V3::new_from(1.0, 2.0, 3.0)); 40 | let double = DualQuaternion::new_from_homogeneous(&t).to_homogeneous(); 41 | 42 | let t_pure = DualQuaternion::new_from_translation(&V3::x_axis()); 43 | let r_pure = DualQuaternion::new_from_rotation(&q); 44 | 45 | let normal = DualQuaternion::new_from_rot_trans(&q, &V3::x_axis()); 46 | let combined = t_pure * r_pure; 47 | 48 | let r_pure2 = DualQuaternion::new_from_rotation_matrix(&rot); 49 | let combined2 = t_pure * r_pure2; 50 | 51 | println!("t: {}", t); 52 | println!("double: {}", double); 53 | 54 | println!("normal: {}", normal); 55 | println!("combined: {}", combined); 56 | println!("combined2: {}", combined2); 57 | } 58 | 59 | 60 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file lib.rs 3 | // 4 | // @date 06/18/20 11:26:55 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2021> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //-------------------------------------------------------------------------- 31 | // TODO(elsuizo:2021-05-16): fill this section 32 | //! 33 | //! static-math 34 | //! 35 | //! Safe and fast mathematical operations with static arrays in Rust programming language 36 | //! 37 | #![cfg_attr(all(feature = "no-std"), no_std)] 38 | #![deny(unsafe_code)] 39 | pub mod dual_quaternion; 40 | pub mod errors; 41 | pub mod matrix2x2; 42 | pub mod matrix3x3; 43 | pub mod matrix4x4; 44 | pub mod matrix5x5; 45 | pub mod matrix6x6; 46 | pub mod quaternion; 47 | pub mod slices_methods; 48 | pub mod traits; 49 | pub mod transformations; 50 | pub mod utils; 51 | pub mod vector2; 52 | pub mod vector3; 53 | pub mod vector4; 54 | pub mod vector5; 55 | pub mod vector6; 56 | 57 | //------------------------------------------------------------------------- 58 | // export types 59 | //------------------------------------------------------------------------- 60 | pub use dual_quaternion::DualQuaternion; 61 | pub use matrix2x2::M22; 62 | pub use matrix3x3::M33; 63 | pub use matrix4x4::M44; 64 | pub use matrix5x5::M55; 65 | pub use matrix6x6::M66; 66 | pub use quaternion::Quaternion; 67 | pub use vector2::V2; 68 | pub use vector2::V2_ZEROS; 69 | pub use vector3::V3; 70 | pub use vector3::V3_ZEROS; 71 | pub use vector3::X_AXIS; 72 | pub use vector3::Y_AXIS; 73 | pub use vector3::Z_AXIS; 74 | pub use vector4::V4; 75 | pub use vector4::V4_ZEROS; 76 | pub use vector5::V5; 77 | pub use vector5::V5_ZEROS; 78 | pub use vector6::V6; 79 | pub use vector6::V6_ZEROS; 80 | -------------------------------------------------------------------------------- /examples/qr_example.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file qr_example.rs 3 | // 4 | // @date 06/25/20 19:30:44 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence: 13 | // This program is free software: you can redistribute it and/or modify 14 | // it under the terms of the GNU General Public License as published by 15 | // the Free Software Foundation, either version 3 of the License, or (at 16 | // your option) any later version. 17 | // 18 | // This program is distributed in the hope that it will be useful, but 19 | // WITHOUT ANY WARRANTY; without even the implied warranty of 20 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 21 | // General Public License for more details. 22 | // 23 | // You should have received a copy of the GNU General Public License 24 | //-------------------------------------------------------------------------- 25 | use static_math::{M22, M33, M44, M55, M66, m22_new, m33_new, m44_new, m55_new, m66_new}; 26 | use static_math::traits::LinearAlgebra; 27 | 28 | fn main() { 29 | 30 | let m22 = m22_new!(1.0, 2.0; 31 | 3.0, 4.0); 32 | 33 | let m33 = m33_new!(10.0, -1.0, 2.0; 34 | 3.0, 3.0, 1.0; 35 | 1.0, 2.0, 5.0); 36 | 37 | let m44 = m44_new!( 0.0, 1.0, 2.0, 3.0; 38 | 4.0, 0.0, 1.0, 7.0; 39 | 10.0, 9.0, 10.0, 11.0; 40 | 12.0, 13.0, 14.0, 15.0); 41 | 42 | let m55 = m55_new!(10.0, 1.0, 7.0, 1.0, 5.0; 43 | 2.0, 4.0, 8.0, 3.0, 2.0; 44 | 5.0, 1.0, 2.0, 9.0, 10.0; 45 | 6.0, 9.0, 1.0, 7.0, 3.0; 46 | 1.0, 8.0, 8.0, 10.0, 5.0); 47 | 48 | let m66 = m66_new!( 1.0, 1.0, 3.0, 4.0, 9.0, 3.0; 49 | 10.0, 10.0, 1.0, 2.0, 2.0, 5.0; 50 | 2.0, 9.0, 6.0, 10.0, 10.0, 9.0; 51 | 10.0, 9.0, 9.0, 7.0, 3.0, 6.0; 52 | 7.0, 6.0, 6.0, 2.0, 9.0, 5.0; 53 | 3.0, 8.0, 1.0, 4.0, 1.0, 5.0); 54 | 55 | if let Some((q, r)) = m22.qr() { 56 | println!("q: {}", q); 57 | println!("r: {}", r); 58 | println!("m22: {}", q * r); 59 | println!("q.det(): {}", q.det()); 60 | } 61 | 62 | if let Some((q, r)) = m33.qr() { 63 | println!("q: {}", q); 64 | println!("r: {}", r); 65 | println!("m33: {}", q * r); 66 | println!("q.det(): {}", q.det()); 67 | } 68 | 69 | if let Some((q, r)) = m44.qr() { 70 | println!("q: {}", q); 71 | println!("r: {}", r); 72 | println!("m44: {}", q * r); 73 | println!("q.det(): {}", q.det()); 74 | } 75 | 76 | if let Some((q, r)) = m55.qr() { 77 | println!("q: {}", q); 78 | println!("r: {}", r); 79 | println!("m55: {}", q * r); 80 | println!("q.det(): {}", q.det()); 81 | } 82 | 83 | if let Some((q, r)) = m66.qr() { 84 | println!("q: {}", q); 85 | println!("r: {}", r); 86 | println!("m66: {}", q * r); 87 | println!("q.det(): {}", q.det()); 88 | } 89 | 90 | } 91 | -------------------------------------------------------------------------------- /src/utils.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file utils.rs 3 | // 4 | // @date 06/02/20 11:06:12 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use crate::dual_quaternion::DualQuaternion; 32 | use crate::matrix3x3::M33; 33 | use crate::matrix4x4::M44; 34 | use crate::traits::LinearAlgebra; 35 | use num::Float; 36 | // NOTE(elsuizo:2020-06-02): the following function 37 | // is a translation of the implementation that is here: 38 | // https://floating-point-gui.de/errors/comparison/ 39 | // 40 | /// a comparison function for floating point values 41 | /// 42 | pub fn nearly_equal(a: T, b: T, epsilon: T) -> bool { 43 | let abs_a = a.abs(); 44 | let abs_b = b.abs(); 45 | let abs_diff = (a - b).abs(); 46 | let zero = T::zero(); 47 | // short-cut, handles infinity 48 | if a == b { 49 | true 50 | } else if a == zero || b == zero || (abs_a + abs_b < T::min_value()) { 51 | // a or b is zero or both are extremely close to it 52 | // relative error is less meaningful here 53 | abs_diff < epsilon 54 | } else { 55 | abs_diff / T::min(abs_a + abs_b, T::max_value()) < epsilon 56 | } 57 | } 58 | 59 | #[inline(always)] 60 | pub fn nearly_zero(a: T) -> bool { 61 | nearly_equal(a, T::zero(), T::epsilon()) 62 | } 63 | 64 | /// utility function to compare vectors of Floats 65 | pub fn compare_vecs(v1: &[T], v2: &[T], epsilon: T) -> bool { 66 | v1.iter() 67 | .zip(v2) 68 | .map(|(a, b)| nearly_equal(*a, *b, epsilon)) 69 | .all(|x| x) 70 | } 71 | 72 | pub fn compare_floats(num1: T, num2: T, tol: T) -> bool { 73 | Float::abs(num1 - num2) < tol 74 | } 75 | 76 | /// utility function to verify if a Matrix is a propper rotation matrix 77 | pub fn is_rotation(r: M33) -> bool { 78 | let r2 = r * r; 79 | nearly_equal(r.det(), T::one(), T::epsilon()) && nearly_equal(r2.det(), T::one(), T::epsilon()) 80 | } 81 | 82 | pub fn is_rotation_h(r: M44) -> bool { 83 | let r2 = r * r; 84 | let eps = T::from(1e-6).unwrap(); 85 | nearly_equal(r.det(), T::one(), eps) && nearly_equal(r2.det(), T::one(), eps) 86 | } 87 | 88 | pub fn compare_dual_quaternions( 89 | a: DualQuaternion, 90 | b: DualQuaternion, 91 | epsilon: T, 92 | ) -> bool { 93 | nearly_equal(a.real().real(), b.real().real(), epsilon) 94 | && nearly_equal(a.real().imag()[0], b.real().imag()[0], epsilon) 95 | && nearly_equal(a.real().imag()[1], b.real().imag()[1], epsilon) 96 | && nearly_equal(a.real().imag()[2], b.real().imag()[2], epsilon) 97 | && nearly_equal(a.dual().real(), b.dual().real(), epsilon) 98 | && nearly_equal(a.dual().imag()[0], b.dual().imag()[0], epsilon) 99 | && nearly_equal(a.dual().imag()[1], b.dual().imag()[1], epsilon) 100 | && nearly_equal(a.dual().imag()[2], b.dual().imag()[2], epsilon) 101 | } 102 | 103 | //------------------------------------------------------------------------- 104 | // tests 105 | //------------------------------------------------------------------------- 106 | #[cfg(test)] 107 | mod test_utils { 108 | use super::*; 109 | #[test] 110 | fn test_nearly_equal() { 111 | let a = 0.15 + 0.15; 112 | let b = 0.1 + 0.2; 113 | assert_eq!(nearly_equal(a, b, 1e-10), true); 114 | } 115 | 116 | #[test] 117 | fn test_zero_signs() { 118 | let a = 0.0; 119 | let b = -0.0; 120 | assert_eq!(nearly_equal(a, b, 1e-10), true); 121 | } 122 | 123 | #[test] 124 | fn test_one_zero() { 125 | let b = -0.0000000005561918225744; 126 | let a = 0.0; 127 | assert_eq!(nearly_equal(a, b, 1e-8), true); 128 | } 129 | } 130 | -------------------------------------------------------------------------------- /src/slices_methods.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file slices_methods.rs 3 | // 4 | // @date 06/24/20 21:48:21 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use crate::utils::nearly_equal; 32 | use num::{Float, Num, Signed}; 33 | 34 | // TODO(elsuizo:2020-08-31): implement the Display trait for this type 35 | #[derive(Copy, Clone, Debug, PartialEq)] 36 | pub struct MaxMin { 37 | pub max: (T, usize), 38 | pub min: (T, usize), 39 | } 40 | 41 | /// generic function to fin min, max values and the position in a slice 42 | pub fn find_max_min(slice: &[T]) -> MaxMin { 43 | let mut max = &slice[0]; 44 | let mut min = &slice[0]; 45 | 46 | let mut max_pos: usize = 0; 47 | let mut min_pos: usize = 0; 48 | 49 | for (index, element) in slice.iter().enumerate().skip(1) { 50 | if element < min { 51 | min = element; 52 | min_pos = index; 53 | } 54 | if element > max { 55 | max = element; 56 | max_pos = index; 57 | } 58 | } 59 | 60 | MaxMin { 61 | max: (*max, max_pos), 62 | min: (*min, min_pos), 63 | } 64 | } 65 | 66 | /// calculate the inf-norm of the slice 67 | pub fn norm_inf(slice: &[T]) -> T { 68 | let max_min = find_max_min(slice); 69 | max_min.max.0 70 | } 71 | 72 | /// calculate the l-norm of the slice 73 | pub fn norm_l(slice: &[T]) -> T { 74 | slice.iter().map(|element| element.abs()).sum() 75 | } 76 | 77 | /// calculate the euclidean norm of the slice 78 | pub fn norm2(slice: &[T]) -> T { 79 | slice.iter().fold(T::zero(), |n, &i| (i * i) + n).sqrt() 80 | } 81 | 82 | /// calculate the dot product of two slices 83 | pub fn dot(slice1: &[T], slice2: &[T]) -> T { 84 | assert!(slice1.len() == slice2.len()); 85 | slice1.iter().zip(slice2).map(|(&a, &b)| a * b).sum() 86 | } 87 | 88 | // NOTE(elsuizo:2021-04-15): this function assume that the slice is not zero 89 | // we use safely in the QR algorithm because known that the vector is not zero 90 | /// normalize the slice 91 | pub fn normalize(slice: &mut [T]) { 92 | let n = norm2(slice); 93 | slice.iter_mut().for_each(|element| { 94 | *element = *element / n; 95 | }) 96 | } 97 | 98 | /// project x in the direction of y 99 | pub fn project_x_over_y(x: &[T], y: &[T]) -> T { 100 | dot(x, y) / dot(y, y) 101 | } 102 | 103 | pub fn check_elements(v: &[T], tol: T) -> bool { 104 | let mut result = false; 105 | for num in v.iter() { 106 | result |= nearly_equal(*num, T::zero(), tol); 107 | } 108 | result 109 | } 110 | 111 | //------------------------------------------------------------------------- 112 | // tests 113 | //------------------------------------------------------------------------- 114 | #[cfg(test)] 115 | mod test_slides_methods { 116 | 117 | use crate::slices_methods::*; 118 | use crate::vector3::V3; 119 | 120 | #[test] 121 | fn find_max_min_test() { 122 | let v = V3::new([1, 10, 37]); 123 | 124 | let result = find_max_min(&*v); 125 | 126 | let expected = MaxMin { 127 | max: (37, 2), 128 | min: (1, 0), 129 | }; 130 | 131 | assert_eq!(result, expected); 132 | } 133 | 134 | #[test] 135 | fn dot_tests() { 136 | let v1 = V3::new([1, 1, 1]); 137 | let v2 = V3::new([1, 1, 3]); 138 | 139 | let result = dot(&*v1, &*v2); 140 | let expected = 5; 141 | 142 | assert_eq!(result, expected); 143 | } 144 | 145 | #[test] 146 | fn normalize_test() { 147 | let mut v1 = V3::new([1.0, 1.0, 1.0]); 148 | normalize(&mut *v1); 149 | 150 | let expected = V3::new([0.5773502691896258, 0.5773502691896258, 0.5773502691896258]); 151 | 152 | assert_eq!( 153 | &v1[..], 154 | &expected[..], 155 | "\nExpected\n{:?}\nfound\n{:?}", 156 | &v1[..], 157 | &expected[..] 158 | ); 159 | } 160 | } 161 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![MIT](https://img.shields.io/badge/license-MIT-blue.svg) 2 | [![Documentation](https://docs.rs/static-math/badge.svg)](https://docs.rs/static-math) 3 | [![crates.io](https://img.shields.io/crates/v/static-math.svg)](https://crates.io/crates/static-math) 4 | 5 | # Static Math in Rust programming language 6 | 7 | "*Simple things should be simple, complex things should be possible*" Alan Kay. 8 | 9 | - This crate take advantage of the static arrays in Rust for fast operations in 10 | stack memory. 11 | 12 | - We use a tuple to indexing elements: `m[(i, j)]` allowing nice interface with the `match` feature of Rust 13 | 14 | - You could index the rows of the matrix with simply: `m[i]` 15 | 16 | - No `unsafe` code :ballot_box_with_check: 17 | 18 | - Could be optimize more with the use of SIMD 19 | 20 | - This crate could be used in an `no-std` environment. 21 | 22 | by enabling the feature `no-std`, for example in your `Cargo.toml`: 23 | 24 | ```toml 25 | [dependencies.static-math] 26 | default-features = false 27 | version = "0.2.0" 28 | features = ["no-std"] 29 | ``` 30 | 31 | - You can visualize the matrices 32 | 33 | ```text 34 | inverse: 35 | |-0.54 0.58 0.67 -0.08 -0.17 -1.18| 36 | |2.16 -1.53 -2.44 0.44 0.32 3.77| 37 | |0.21 -0.42 -0.39 0.15 0.20 0.62| 38 | |0.70 -0.24 -0.53 0.20 -0.21 0.73| 39 | |0.85 -0.47 -0.83 0.11 0.11 1.20| 40 | |-3.91 2.47 4.17 -0.87 -0.31 -6.08| 41 | ``` 42 | 43 | - The determinant of the matrices are evaluated "in-place" without loops and code 44 | bifurcations 45 | 46 | - The use cases can be: Robotics, Game programming, Simulations ...etc. 47 | 48 | The matrix types `Mnn` (where `n=2..6`) implements the Methods from the 49 | `LinearAlgebra` trait: 50 | 51 | - `det()`: Determinant of the matrix 52 | - `inverse()`: Inverse of the matrix 53 | - `qr()`: QR decomposition of the matrix 54 | - `norm2()`: norm of the matrix 55 | - `transpose()`: transpose of the matrix 56 | - `trace()`: trace of the matrix 57 | - `shape()`: shape of the matrix 58 | 59 | - We have implemented `Quaternion`s (and all the most used methods) 60 | - We have implemented `DualQuaternion`s (and all the most used methods in Robotics and graphics like *Screw Linear Interpolation*) 61 | - We have implemented in the `transformations.rs` module a wide variety of functions used in Robotics (which conforms to the screw theory) 62 | 63 | ## Benchmarks 64 | 65 | Using the criterion crate: 66 | 67 | https://github.com/bheisler/criterion.rs 68 | 69 | run with: `cargo bench` 70 | 71 | Others benches comparing the performance with others crates are in this repo: https://github.com/bitshifter/mathbench-rs 72 | 73 | *NOTE*: this is the only crate that not have unsafe code 74 | 75 | with the following results: 76 | 77 | 78 | | benchmark | glam | cgmath | nalgebra | euclid | vek | pathfinder | static-math | ultraviolet | 79 | |--------------------------------|-----------------|-----------------|-----------------|----------------|-----------------|-----------------|-----------------|-----------------| 80 | | euler 2d x10000 | 16.23 us | 16.13 us | __9.954 us__ | 16.18 us | 16.2 us | 10.42 us | __9.97 us__ | 16.17 us | 81 | | euler 3d x10000 | __15.95 us__ | 32.11 us | 32.13 us | 32.13 us | 32.13 us | __16.27 us__ | 32.16 us | 32.11 us | 82 | | matrix2 determinant | __2.0386 ns__ | 2.0999 ns | 2.1018 ns | N/A | 2.0997 ns | 2.0987 ns | 2.0962 ns | 2.1080 ns | 83 | | matrix2 inverse | __2.8226 ns__ | 8.4418 ns | 7.6303 ns | N/A | N/A | 3.3459 ns | 9.4636 ns | 5.8796 ns | 84 | | matrix2 mul matrix2 | __2.6036 ns__ | 5.0007 ns | 4.8172 ns | N/A | 9.3814 ns | __2.5516 ns__ | 4.7274 ns | 4.9428 ns | 85 | | matrix2 mul vector2 x1 | 2.4904 ns | 2.6144 ns | 2.8714 ns | N/A | 4.2139 ns | __2.0839 ns__ | 2.8873 ns | 2.6250 ns | 86 | | matrix2 mul vector2 x100 | 227.5271 ns | 243.3579 ns | 265.1698 ns | N/A | 400.6940 ns | __219.7127 ns__ | 267.8780 ns | 243.9880 ns | 87 | | matrix2 return self | __2.4235 ns__ | 2.8841 ns | 2.8756 ns | N/A | 2.8754 ns | __2.4147 ns__ | 2.8717 ns | 2.8697 ns | 88 | | matrix2 transpose | __2.2887 ns__ | 3.0645 ns | 7.9154 ns | N/A | 2.9635 ns | N/A | 3.0637 ns | 3.0652 ns | 89 | | matrix3 determinant | 3.9129 ns | __3.8107 ns__ | __3.8191 ns__ | N/A | __3.8180 ns__ | N/A | __3.8151 ns__ | 8.9368 ns | 90 | | matrix3 inverse | 17.5373 ns | 18.6931 ns | __12.3183 ns__ | N/A | N/A | N/A | 12.8195 ns | 21.9098 ns | 91 | | matrix3 mul matrix3 | 9.9578 ns | 13.3648 ns | 7.8154 ns | N/A | 35.5802 ns | N/A | __6.4938 ns__ | 10.0527 ns | 92 | | matrix3 mul vector3 x1 | 4.8090 ns | 4.9339 ns | __4.5046 ns__ | N/A | 12.5518 ns | N/A | 4.8002 ns | 4.8118 ns | 93 | | matrix3 mul vector3 x100 | __0.4836 us__ | __0.4808 us__ | __0.4755 us__ | N/A | 1.247 us | N/A | __0.4816 us__ | __0.4755 us__ | 94 | | matrix3 return self | __5.4421 ns__ | __5.4469 ns__ | __5.4526 ns__ | N/A | __5.4656 ns__ | N/A | __5.4718 ns__ | __5.4043 ns__ | 95 | | matrix3 transpose | __9.9567 ns__ | __10.0794 ns__ | 10.9704 ns | N/A | __9.9257 ns__ | N/A | 10.7350 ns | 10.5334 ns | 96 | | matrix4 determinant | __6.2050 ns__ | 11.1041 ns | 69.2549 ns | 17.1809 ns | 18.5233 ns | N/A | 16.5331 ns | 8.2704 ns | 97 | | matrix4 inverse | __16.4386 ns__ | 47.0674 ns | 71.8174 ns | 64.1356 ns | 284.3703 ns | N/A | 52.6993 ns | 41.1780 ns | 98 | | matrix4 mul matrix4 | __7.7715 ns__ | 26.7308 ns | 8.6500 ns | 10.4414 ns | 86.1501 ns | N/A | 21.7985 ns | 26.8056 ns | 99 | | matrix4 mul vector4 x1 | __3.0303 ns__ | 7.7400 ns | 3.4091 ns | N/A | 21.0968 ns | N/A | 6.2971 ns | 6.2537 ns | 100 | | matrix4 mul vector4 x100 | __0.6136 us__ | 0.9676 us | __0.627 us__ | N/A | 2.167 us | N/A | 0.7893 us | 0.8013 us | 101 | | matrix4 return self | 7.1741 ns | __6.8838 ns__ | 7.5030 ns | N/A | 7.0410 ns | N/A | __6.7768 ns__ | 6.9508 ns | 102 | | matrix4 transpose | __6.6826 ns__ | 12.4966 ns | 15.3265 ns | N/A | 12.6386 ns | N/A | 15.2657 ns | 12.3396 ns | 103 | | ray-sphere intersection x10000 | 56.2 us | 55.7 us | __15.32 us__ | 55.45 us | 56.02 us | N/A | N/A | 50.94 us | 104 | | rotation3 inverse | __2.3113 ns__ | 3.1752 ns | 3.3292 ns | 3.3311 ns | 3.1808 ns | N/A | 8.7109 ns | 3.6535 ns | 105 | | rotation3 mul rotation3 | __3.6584 ns__ | 7.5255 ns | 7.4808 ns | 8.1393 ns | 14.1636 ns | N/A | 6.8044 ns | 7.6386 ns | 106 | | rotation3 mul vector3 x1 | __6.4950 ns__ | 7.6808 ns | 7.5784 ns | 7.5746 ns | 18.2547 ns | N/A | 7.2727 ns | 8.9732 ns | 107 | | rotation3 mul vector3 x100 | __0.6465 us__ | 0.7844 us | 0.7573 us | 0.7533 us | 1.769 us | N/A | 0.7317 us | 0.9416 us | 108 | | rotation3 return self | __2.4928 ns__ | 2.8740 ns | 2.8687 ns | N/A | 2.8724 ns | N/A | 4.7868 ns | 2.8722 ns | 109 | | transform point2 x1 | 2.7854 ns | 2.8878 ns | 4.4207 ns | 2.8667 ns | 11.9427 ns | __2.3601 ns__ | N/A | 4.1770 ns | 110 | | transform point2 x100 | 0.3316 us | 0.3574 us | 0.4445 us | __0.3008 us__ | 1.212 us | 0.3184 us | N/A | 0.4332 us | 111 | | transform point3 x1 | __2.9619 ns__ | 10.6812 ns | 6.1037 ns | 7.7051 ns | 13.2607 ns | 3.0934 ns | N/A | 6.8419 ns | 112 | | transform point3 x100 | __0.6095 us__ | 1.27 us | 0.8064 us | 0.7674 us | 1.446 us | __0.6189 us__ | N/A | 0.8899 us | 113 | | transform vector2 x1 | __2.4944 ns__ | N/A | 3.7174 ns | 2.6273 ns | 11.9424 ns | N/A | N/A | 3.0458 ns | 114 | | transform vector2 x100 | 0.3125 us | N/A | 0.3871 us | __0.2817 us__ | 1.213 us | N/A | N/A | 0.3649 us | 115 | | transform vector3 x1 | __2.8091 ns__ | 7.7343 ns | 5.5064 ns | 4.4810 ns | 15.4097 ns | N/A | N/A | 4.8819 ns | 116 | | transform vector3 x100 | __0.6035 us__ | 0.9439 us | 0.7573 us | 0.6327 us | 1.63 us | N/A | N/A | 0.6703 us | 117 | | transform2 inverse | __9.0256 ns__ | N/A | 12.2614 ns | 9.4803 ns | N/A | __8.9047 ns__ | N/A | N/A | 118 | | transform2 mul transform2 | 4.5111 ns | N/A | 8.1434 ns | 5.8677 ns | N/A | __3.8513 ns__ | N/A | N/A | 119 | | transform2 return self | __4.1707 ns__ | N/A | 5.4356 ns | 4.2775 ns | N/A | __4.1117 ns__ | N/A | N/A | 120 | | transform3 inverse | __10.9869 ns__ | N/A | 71.4437 ns | 56.0136 ns | N/A | 23.0392 ns | N/A | N/A | 121 | | transform3 mul transform3d | __6.5903 ns__ | N/A | 8.5673 ns | 10.1802 ns | N/A | 7.6587 ns | N/A | N/A | 122 | | transform3 return self | __7.1828 ns__ | N/A | __7.2619 ns__ | __7.2407 ns__ | N/A | __7.3214 ns__ | N/A | N/A | 123 | | vector3 cross | __2.4257 ns__ | 3.6842 ns | 3.7945 ns | 3.6821 ns | 3.8323 ns | N/A | 3.8622 ns | 3.6927 ns | 124 | | vector3 dot | __2.1055 ns__ | 2.3179 ns | 2.3174 ns | 2.3190 ns | 2.3195 ns | N/A | 2.3204 ns | 2.3160 ns | 125 | | vector3 length | __2.5020 ns__ | __2.5002 ns__ | 2.5986 ns | __2.5013 ns__ | __2.5021 ns__ | N/A | __2.5036 ns__ | __2.5017 ns__ | 126 | | vector3 normalize | __4.0454 ns__ | 5.8411 ns | 8.4069 ns | 8.0679 ns | 8.8137 ns | N/A | N/A | 5.8440 ns | 127 | | vector3 return self | __2.4087 ns__ | 3.1021 ns | 3.1061 ns | N/A | 3.1052 ns | N/A | 3.1136 ns | 3.1071 ns | 128 | 129 | 130 | ## TODOS: 131 | 132 | - [X] `Quaternion` type and methods 133 | - [ ] `expm()`: Exponential matrix implementation 134 | - [X] Eigenvalues 135 | - [X] QR decomposition 136 | 137 | 138 | -------------------------------------------------------------------------------- /src/vector2.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file vector2.rs 3 | // 4 | // @date 06/01/20 22:37:02 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use core::fmt; 32 | use core::ops::{Add, AddAssign, Div, Mul, Neg, Sub, SubAssign}; 33 | use core::ops::{Deref, DerefMut}; 34 | use num::{Float, Num, Signed, Zero}; 35 | 36 | use crate::errors::VectorErrors; 37 | use crate::matrix2x2::*; 38 | use crate::slices_methods::{norm_inf, norm_l}; 39 | 40 | //------------------------------------------------------------------------- 41 | // code 42 | //------------------------------------------------------------------------- 43 | #[derive(Copy, Clone, Debug, PartialEq)] 44 | pub struct V2([T; 2]); 45 | 46 | impl V2 { 47 | /// create a new V2 from a static array 48 | pub const fn new(input: [T; 2]) -> Self { 49 | Self(input) 50 | } 51 | 52 | /// create a new V2 from raw numbers 53 | pub const fn new_from(a: T, b: T) -> Self { 54 | Self::new([a, b]) 55 | } 56 | } 57 | 58 | impl V2 { 59 | /// create a V2 with all elements zero 60 | pub fn zeros() -> Self { 61 | ::zero() 62 | } 63 | 64 | /// create a V2 with all elements one 65 | pub fn ones() -> Self { 66 | let one = T::one(); 67 | Self::new([one, one]) 68 | } 69 | } 70 | 71 | impl V2 { 72 | pub fn norm_inf(&self) -> T { 73 | norm_inf(&**self) 74 | } 75 | } 76 | 77 | impl V2 { 78 | pub fn norm_l(&self) -> T { 79 | norm_l(&**self) 80 | } 81 | } 82 | 83 | impl Neg for V2 { 84 | type Output = Self; 85 | 86 | #[inline] 87 | fn neg(self) -> Self { 88 | Self::new_from(-self[0], -self[1]) 89 | } 90 | } 91 | 92 | impl V2 { 93 | /// calculate the euclidean norm of the V2 94 | pub fn norm2(&self) -> T { 95 | T::sqrt(self[0] * self[0] + self[1] * self[1]) 96 | } 97 | 98 | /// normalize the current V2 and return a new one 99 | pub fn normalize(&self) -> Result { 100 | let n = self.norm2(); 101 | if n != T::zero() { 102 | // this method return a new fresh and clean vector :) 103 | let mut result = Self::zeros(); 104 | for i in 0..self.len() { 105 | result[i] = self[i] / n; 106 | } 107 | Ok(result) 108 | } else { 109 | Err(VectorErrors::Norm2IsZero) 110 | } 111 | } 112 | } 113 | 114 | impl Mul for V2 { 115 | type Output = T; 116 | 117 | #[inline] 118 | fn mul(self, rhs: Self) -> T { 119 | self[0] * rhs[0] + self[1] * rhs[1] 120 | } 121 | } 122 | 123 | // TODO(elsuizo:2020-05-01): missing constant * V2 124 | // V2 * constant 125 | impl Mul for V2 { 126 | type Output = V2; 127 | 128 | #[inline] 129 | fn mul(self, rhs: T) -> V2 { 130 | Self::new_from(self[0] * rhs, self[1] * rhs) 131 | } 132 | } 133 | 134 | // V2 / const 135 | impl Div for V2 { 136 | type Output = Self; 137 | 138 | #[inline] 139 | fn div(self, rhs: T) -> Self::Output { 140 | Self::new_from(self[0] / rhs, self[1] / rhs) 141 | } 142 | } 143 | 144 | // FIXME(elsuizo:2020-06-19): this code dont compile i dont known why 145 | //impl Mul> for T { 146 | // type Output = V2; 147 | // 148 | // fn mul(self, rhs: V2) -> V2 { 149 | // let a0 = self * rhs[0]; 150 | // let a1 = self * rhs[1]; 151 | // V2::new([a0, a1]) 152 | // } 153 | //} 154 | 155 | impl Mul> for f32 { 156 | type Output = V2; 157 | 158 | #[inline] 159 | fn mul(self, rhs: V2) -> V2 { 160 | V2::new_from(self * rhs[0], self * rhs[1]) 161 | } 162 | } 163 | 164 | // V2 * M22 165 | impl Mul> for V2 { 166 | type Output = V2; 167 | 168 | #[inline] 169 | fn mul(self, rhs: M22) -> V2 { 170 | Self::new_from( 171 | self[0] * rhs[(0, 0)] + self[1] * rhs[(1, 0)], 172 | self[0] * rhs[(0, 1)] + self[1] * rhs[(1, 1)], 173 | ) 174 | } 175 | } 176 | 177 | // V2 - V2 178 | impl Sub for V2 { 179 | type Output = Self; 180 | 181 | #[inline] 182 | fn sub(self, rhs: Self) -> Self { 183 | Self::new_from(self[0] - rhs[0], self[1] - rhs[1]) 184 | } 185 | } 186 | 187 | // V2 -= V2 188 | impl SubAssign for V2 { 189 | #[inline] 190 | fn sub_assign(&mut self, other: Self) { 191 | *self = *self - other 192 | } 193 | } 194 | 195 | // V2 + V2 196 | impl Add for V2 { 197 | type Output = Self; 198 | #[inline] 199 | fn add(self, rhs: Self) -> Self { 200 | Self::new_from(self[0] + rhs[0], self[1] + rhs[1]) 201 | } 202 | } 203 | 204 | // V2 += V2 205 | impl AddAssign for V2 { 206 | fn add_assign(&mut self, other: Self) { 207 | *self = *self + other 208 | } 209 | } 210 | 211 | // Zero trait 212 | impl Zero for V2 { 213 | #[inline] 214 | fn zero() -> V2 { 215 | Self::new_from(T::zero(), T::zero()) 216 | } 217 | 218 | fn is_zero(&self) -> bool { 219 | *self == V2::zero() 220 | } 221 | } 222 | 223 | impl Deref for V2 { 224 | type Target = [T; 2]; 225 | #[inline] 226 | fn deref(&self) -> &Self::Target { 227 | &self.0 228 | } 229 | } 230 | 231 | impl DerefMut for V2 { 232 | #[inline] 233 | fn deref_mut(&mut self) -> &mut Self::Target { 234 | &mut self.0 235 | } 236 | } 237 | 238 | impl From<[T; 2]> for V2 { 239 | fn from(data: [T; 2]) -> V2 { 240 | V2(data) 241 | } 242 | } 243 | //------------------------------------------------------------------------- 244 | // Display impl 245 | //------------------------------------------------------------------------- 246 | impl fmt::Display for V2 { 247 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 248 | writeln!(dest, "[{0:^3.2} {1:^3.2}]", self[0], self[1]) 249 | } 250 | } 251 | //------------------------------------------------------------------------- 252 | // constants 253 | //------------------------------------------------------------------------- 254 | pub const V2_ZEROS: V2 = V2::new_from(0.0, 0.0); 255 | pub const X_AXIS: V2 = V2::new_from(1.0, 0.0); 256 | pub const Y_AXIS: V2 = V2::new_from(0.0, 1.0); 257 | 258 | //------------------------------------------------------------------------- 259 | // tests 260 | //------------------------------------------------------------------------- 261 | #[cfg(test)] 262 | mod vector2_test { 263 | 264 | use crate::vector2::V2; 265 | 266 | #[test] 267 | fn create_vector2_test() { 268 | let v = V2::new([1.0, 2.0]); 269 | assert_eq!(v[0], 1.0); 270 | assert_eq!(v[1], 2.0); 271 | } 272 | 273 | #[test] 274 | fn zero_vector2_test() { 275 | let result: V2 = V2::zeros(); 276 | let expected = V2::new([0.0, 0.0]); 277 | assert_eq!( 278 | &result[..], 279 | &expected[..], 280 | "\nExpected\n{:?}\nfound\n{:?}", 281 | &result[..], 282 | &expected[..] 283 | ); 284 | } 285 | 286 | #[test] 287 | fn product_test() { 288 | let v1 = V2::new([1.0, 2.0]); 289 | let v2 = V2::new([3.0, 4.0]); 290 | let result = v1 * v2; 291 | let expected = 11.0; 292 | assert_eq!(result, expected); 293 | } 294 | 295 | #[test] 296 | fn add_test() { 297 | let v1 = V2::new([1.0, 2.0]); 298 | let v2 = V2::new([3.0, 4.0]); 299 | let result = v1 + v2; 300 | let expected = V2::new([4.0, 6.0]); 301 | assert_eq!( 302 | &result[..], 303 | &expected[..], 304 | "\nExpected\n{:?}\nfound\n{:?}", 305 | &result[..], 306 | &expected[..] 307 | ); 308 | } 309 | 310 | #[test] 311 | fn sub_test() { 312 | let v1 = V2::new([1.0, 2.0]); 313 | let v2 = V2::new([2.0, 3.0]); 314 | let expected = V2::new([-1.0, -1.0]); 315 | let result = v1 - v2; 316 | 317 | assert_eq!( 318 | &result[..], 319 | &expected[..], 320 | "\nExpected\n{:?}\nfound\n{:?}", 321 | &result[..], 322 | &expected[..] 323 | ); 324 | } 325 | 326 | #[test] 327 | fn mul_const_rhs() { 328 | let v1 = V2::new_from(1.0, 2.0); 329 | let result = 2.0 * v1; 330 | let expected = V2::new_from(2.0, 4.0); 331 | assert_eq!( 332 | &result[..], 333 | &expected[..], 334 | "\nExpected\n{:?}\nfound\n{:?}", 335 | &result[..], 336 | &expected[..] 337 | ); 338 | } 339 | 340 | #[test] 341 | fn mul_const() { 342 | let v1 = V2::new_from(1.0, 2.0); 343 | let result = v1 * 10.0; 344 | let expected = V2::new_from(10.0, 20.0); 345 | assert_eq!( 346 | &result[..], 347 | &expected[..], 348 | "\nExpected\n{:?}\nfound\n{:?}", 349 | &result[..], 350 | &expected[..] 351 | ); 352 | } 353 | 354 | #[test] 355 | fn norm2_test() { 356 | let v1 = V2::new_from(1.0, 2.0); 357 | let expected = 2.23606797749979; 358 | let result = v1.norm2(); 359 | assert_eq!(result, expected); 360 | } 361 | 362 | #[test] 363 | fn normalize_test() { 364 | let result = V2::new([1.0, 1.0]).normalize().unwrap(); 365 | let expected = V2::new([0.7071067811865475, 0.7071067811865475]); 366 | assert_eq!( 367 | &result[..], 368 | &expected[..], 369 | "\nExpected\n{:?}\nfound\n{:?}", 370 | &result[..], 371 | &expected[..] 372 | ); 373 | } 374 | 375 | #[test] 376 | fn sub_assigment_test() { 377 | let mut result = V2::new_from(1.0, 3.0); 378 | let v2 = V2::new_from(3.0, 3.0); 379 | let expected = V2::new_from(-2.0, 0.0); 380 | result -= v2; 381 | assert_eq!( 382 | &result[..], 383 | &expected[..], 384 | "\nExpected\n{:?}\nfound\n{:?}", 385 | &result[..], 386 | &expected[..] 387 | ); 388 | } 389 | 390 | #[test] 391 | fn add_assigment_test() { 392 | let mut result = V2::new_from(1.0, 3.0); 393 | let v2 = V2::new_from(3.0, 3.0); 394 | let expected = V2::new_from(4.0, 6.0); 395 | result += v2; 396 | assert_eq!( 397 | &result[..], 398 | &expected[..], 399 | "\nExpected\n{:?}\nfound\n{:?}", 400 | &result[..], 401 | &expected[..] 402 | ); 403 | } 404 | 405 | #[test] 406 | fn norm_inf_test() { 407 | let v = V2::new_from(1, 10); 408 | let result = v.norm_inf(); 409 | let expected = 10; 410 | assert_eq!(result, expected); 411 | } 412 | 413 | #[test] 414 | fn norm_l_test() { 415 | let v = V2::new_from(-1, 1); 416 | let result = v.norm_l(); 417 | let expected = 2; 418 | assert_eq!(result, expected); 419 | } 420 | } 421 | -------------------------------------------------------------------------------- /src/vector4.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file vector4.rs 3 | // 4 | // @date 06/02/20 20:08:45 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use core::fmt; 32 | use num::{Float, Zero, Num, Signed}; 33 | use core::ops::{Deref, DerefMut}; 34 | 35 | use core::ops::{Add, Sub, Div, Mul, SubAssign, AddAssign, Neg}; 36 | use core::ops::BitAnd; 37 | use crate::slices_methods::{norm_inf, norm_l}; 38 | use crate::errors::VectorErrors; 39 | use crate::matrix4x4::M44; 40 | 41 | //------------------------------------------------------------------------- 42 | // code 43 | //------------------------------------------------------------------------- 44 | #[derive(Copy, Clone, Debug, PartialEq)] 45 | pub struct V4([T; 4]); 46 | 47 | impl V4 { 48 | /// create a new V4 from a static array 49 | pub const fn new(input: [T; 4]) -> Self { 50 | Self(input) 51 | } 52 | 53 | /// create a new V4 from raw numbers 54 | pub const fn new_from(a: T, b: T, c: T, d: T) -> Self { 55 | Self::new([a, b, c, d]) 56 | } 57 | } 58 | 59 | impl V4 { 60 | /// create a V4 with all elements zeros 61 | pub fn zeros() -> V4 { 62 | as Zero>::zero() 63 | } 64 | 65 | /// create a V4 with all elements one 66 | pub fn ones() -> Self { 67 | let one = T::one(); 68 | Self::new([one, one, one, one]) 69 | } 70 | 71 | } 72 | 73 | impl BitAnd for V4 { 74 | type Output = Self; 75 | fn bitand(self, rhs: Self) -> Self::Output { 76 | Self::new_from(self[0] & rhs[0], self[1] & rhs[1], self[2] & rhs[2], self[3] & rhs[3]) 77 | } 78 | } 79 | 80 | // norm inf 81 | impl V4 { 82 | pub fn norm_inf(&self) -> T { 83 | norm_inf(&**self) 84 | } 85 | } 86 | 87 | // norm l 88 | impl V4 { 89 | pub fn norm_l(&self) -> T { 90 | norm_l(&**self) 91 | } 92 | } 93 | 94 | // -V4 95 | impl Neg for V4 { 96 | type Output = Self; 97 | 98 | #[inline] 99 | fn neg(self) -> Self { 100 | V4::new_from(-self[0], -self[1], -self[2], -self[3]) 101 | } 102 | } 103 | 104 | impl V4 { 105 | /// calculate the euclidean norm of the V4 106 | #[inline] 107 | pub fn norm2(&self) -> T { 108 | T::sqrt(self[0] * self[0] + self[1] * self[1] + self[2] * self[2] + self[3] * self[3]) 109 | } 110 | 111 | /// normalize the current vector and return a new one 112 | pub fn normalize(&mut self) -> Result { 113 | let n = self.norm2(); 114 | if n != T::zero() { 115 | // this method return a new fresh and clean vector :) 116 | let mut result = Self::zeros(); 117 | for i in 0..self.len() { 118 | result[i] = self[i] / n; 119 | } 120 | Ok(result) 121 | } else { 122 | Err(VectorErrors::Norm2IsZero) 123 | } 124 | } 125 | } 126 | 127 | // V4 * V4(dot product) 128 | impl Mul for V4 { 129 | type Output = T; 130 | 131 | #[inline] 132 | fn mul(self, rhs: Self) -> T { 133 | self[0] * rhs[0] + self[1] * rhs[1] + self[2] * rhs[2] + self[3] * rhs[3] 134 | } 135 | } 136 | 137 | // V4 * const 138 | impl Mul for V4 { 139 | type Output = V4; 140 | 141 | #[inline(always)] 142 | fn mul(self, rhs: T) -> V4 { 143 | Self::new_from(self[0] * rhs, self[1] * rhs, self[2] * rhs, self[3] * rhs) 144 | } 145 | } 146 | 147 | // NOTE(elsuizo:2021-08-03): this should panics if rhs == zero 148 | // V4 / const 149 | impl Div for V4 { 150 | type Output = Self; 151 | 152 | #[inline(always)] 153 | fn div(self, rhs: T) -> Self::Output { 154 | Self::new_from(self[0] / rhs, self[1] / rhs, self[2] / rhs, self[3] / rhs) 155 | } 156 | } 157 | 158 | // f32 * V4 159 | impl Mul> for f32 { 160 | type Output = V4; 161 | 162 | #[inline] 163 | fn mul(self, rhs: V4) -> V4 { 164 | V4::new_from(self * rhs[0], self * rhs[1], self * rhs[2], self * rhs[3]) 165 | } 166 | } 167 | 168 | // V4 * M44 169 | impl Mul> for V4 { 170 | type Output = V4; 171 | 172 | #[inline] 173 | fn mul(self, rhs: M44) -> V4 { 174 | Self::new_from( 175 | self[0] * rhs[(0, 0)] + self[1] * rhs[(1, 0)] + self[2] * rhs[(2, 0)] + self[3] * rhs[(3, 0)], 176 | self[0] * rhs[(0, 1)] + self[1] * rhs[(1, 1)] + self[2] * rhs[(2, 1)] + self[3] * rhs[(3, 1)], 177 | self[0] * rhs[(0, 2)] + self[1] * rhs[(1, 2)] + self[2] * rhs[(2, 2)] + self[3] * rhs[(3, 2)], 178 | self[0] * rhs[(0, 3)] + self[1] * rhs[(1, 3)] + self[2] * rhs[(2, 3)] + self[3] * rhs[(3, 3)], 179 | ) 180 | } 181 | } 182 | 183 | // V4 - V4 184 | impl Sub for V4 { 185 | type Output = Self; 186 | 187 | #[inline] 188 | fn sub(self, rhs: Self) -> Self { 189 | V4::new_from(self[0] - rhs[0], self[1] - rhs[1], self[2] - rhs[2], self[3] - rhs[3]) 190 | } 191 | } 192 | 193 | // V4 -= V4 194 | impl SubAssign for V4 { 195 | fn sub_assign(&mut self, other: Self) { 196 | *self = *self - other 197 | } 198 | } 199 | 200 | // V4 + V4 201 | impl Add for V4 { 202 | type Output = Self; 203 | 204 | #[inline] 205 | fn add(self, rhs: Self) -> Self { 206 | V4::new_from(self[0] + rhs[0], self[1] + rhs[1], self[2] + rhs[2], self[3] + rhs[3]) 207 | } 208 | } 209 | 210 | // V4 += V4 211 | impl AddAssign for V4 { 212 | #[inline] 213 | fn add_assign(&mut self, other: Self) { 214 | *self = *self + other 215 | } 216 | } 217 | 218 | impl Zero for V4 { 219 | #[inline] 220 | fn zero() -> V4 { 221 | V4::new([T::zero(); 4]) 222 | } 223 | 224 | fn is_zero(&self) -> bool { 225 | *self == V4::zero() 226 | } 227 | } 228 | 229 | impl Deref for V4 { 230 | type Target = [T; 4]; 231 | #[inline] 232 | fn deref(&self) -> &Self::Target { 233 | &self.0 234 | } 235 | } 236 | 237 | impl DerefMut for V4 { 238 | #[inline] 239 | fn deref_mut(&mut self) -> &mut Self::Target { 240 | &mut self.0 241 | } 242 | } 243 | 244 | impl From<[T; 4]> for V4 { 245 | fn from(data: [T; 4]) -> V4 { 246 | V4(data) 247 | } 248 | } 249 | 250 | 251 | //------------------------------------------------------------------------- 252 | // Display impl 253 | //------------------------------------------------------------------------- 254 | impl fmt::Display for V4 { 255 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 256 | writeln!(dest, "[{0:^3.2} {1:^3.2} {2:^3.2} {3:^3.2}]", self[0], self[1], self[2], self[3]) 257 | } 258 | } 259 | 260 | //------------------------------------------------------------------------- 261 | // constants 262 | //------------------------------------------------------------------------- 263 | /// constant `V4` zeros 264 | pub const V4_ZEROS: V4 = V4::new_from(0.0, 0.0, 0.0, 0.0); 265 | 266 | //------------------------------------------------------------------------- 267 | // tests 268 | //------------------------------------------------------------------------- 269 | #[cfg(test)] 270 | mod vector4_test { 271 | use crate::matrix4x4::M44; 272 | use crate::vector4::V4; 273 | 274 | #[test] 275 | fn vector4_creation_test() { 276 | let v = V4::new([1, 1, 1, 1]); 277 | assert_eq!(v[0], 1); 278 | assert_eq!(v[1], 1); 279 | assert_eq!(v[2], 1); 280 | assert_eq!(v[3], 1); 281 | } 282 | 283 | #[test] 284 | fn vector4_zeros_test() { 285 | let result: V4 = V4::zeros(); 286 | let expected = V4::new([0.0, 0.0, 0.0, 0.0]); 287 | assert_eq!( 288 | &result[..], 289 | &expected[..], 290 | "\nExpected\n{:?}\nfound\n{:?}", 291 | &result[..], 292 | &expected[..] 293 | ); 294 | } 295 | 296 | #[test] 297 | fn vector4_add_test() { 298 | let v1 = V4::new([1.0, 2.0, 3.0, 4.0]); 299 | let v2 = V4::new([5.0, 6.0, 7.0, 8.0]); 300 | let result = v1 + v2; 301 | let expected = V4::new([6.0, 8.0, 10.0, 12.0]); 302 | assert_eq!( 303 | &result[..], 304 | &expected[..], 305 | "\nExpected\n{:?}\nfound\n{:?}", 306 | &result[..], 307 | &expected[..] 308 | ); 309 | } 310 | 311 | #[test] 312 | fn sub_test() { 313 | let v1 = V4::new([1.0, 2.0, 3.0, 4.0]); 314 | let v2 = V4::new([5.0, 6.0, 7.0, 8.0]); 315 | let result = v1 - v2; 316 | let expected = V4::new([-4.0, -4.0, -4.0, -4.0]); 317 | assert_eq!( 318 | &result[..], 319 | &expected[..], 320 | "\nExpected\n{:?}\nfound\n{:?}", 321 | &result[..], 322 | &expected[..] 323 | ); 324 | } 325 | 326 | #[test] 327 | fn vector4_product_test() { 328 | let v1 = V4::new([1.0, 2.0, 3.0, 4.0]); 329 | let v2 = V4::new([5.0, 6.0, 7.0, 8.0]); 330 | let result = v1 * v2; 331 | let expected = 70.0; 332 | assert_eq!(result, expected); 333 | } 334 | 335 | #[test] 336 | fn vector4_norm_test() { 337 | let v1 = V4::new([1.0, 2.0, 3.0, 4.0]); 338 | let result = v1.norm2(); 339 | let expected = 5.477225575051661; 340 | assert_eq!(result, expected); 341 | } 342 | 343 | #[test] 344 | fn mul_const_rhs() { 345 | let v = V4::new([1.0, 2.0, 3.0, 4.0]); 346 | let result = 2.0 * v; 347 | let expected = V4::new([2.0, 4.0, 6.0, 8.0]); 348 | assert_eq!( 349 | &result[..], 350 | &expected[..], 351 | "\nExpected\n{:?}\nfound\n{:?}", 352 | &result[..], 353 | &expected[..] 354 | ); 355 | } 356 | 357 | #[test] 358 | fn mul_const() { 359 | let v = V4::new([1.0, 2.0, 3.0, 4.0]); 360 | let result = v * 2.0; 361 | let expected = V4::new([2.0, 4.0, 6.0, 8.0]); 362 | assert_eq!( 363 | &result[..], 364 | &expected[..], 365 | "\nExpected\n{:?}\nfound\n{:?}", 366 | &result[..], 367 | &expected[..] 368 | ); 369 | } 370 | 371 | #[test] 372 | fn vector4_mul_matrix4x4_test() { 373 | let m = M44::new([ 374 | [1.0, 2.0, 3.0, 4.0], 375 | [5.0, 6.0, 7.0, 8.0], 376 | [9.0, 10.0, 11.0, 12.0], 377 | [13.0, 14.0, 15.0, 16.0], 378 | ]); 379 | 380 | let v1 = V4::new([1.0, 2.0, 3.0, 4.0]); 381 | let result = v1 * m; 382 | let expected = V4::new([90.0, 100.0, 110.0, 120.0]); 383 | assert_eq!( 384 | &result[..], 385 | &expected[..], 386 | "\nExpected\n{:?}\nfound\n{:?}", 387 | &result[..], 388 | &expected[..] 389 | ); 390 | } 391 | 392 | #[test] 393 | fn normalize_test() { 394 | let result = V4::new([1.0, 1.0, 1.0, 1.0]).normalize().unwrap(); 395 | let expected = V4::new([0.5, 0.5, 0.5, 0.5]); 396 | assert_eq!( 397 | &result[..], 398 | &expected[..], 399 | "\nExpected\n{:?}\nfound\n{:?}", 400 | &result[..], 401 | &expected[..] 402 | ); 403 | } 404 | 405 | #[test] 406 | fn sub_assigment_test() { 407 | let mut result = V4::new([1.0, 2.0, 3.0, 4.0]); 408 | let v = V4::new([5.0, 6.0, 7.0, 8.0]); 409 | let expected = V4::new([-4.0, -4.0, -4.0, -4.0]); 410 | result -= v; 411 | assert_eq!( 412 | &result[..], 413 | &expected[..], 414 | "\nExpected\n{:?}\nfound\n{:?}", 415 | &result[..], 416 | &expected[..] 417 | ); 418 | } 419 | 420 | #[test] 421 | fn add_assigment_test() { 422 | let mut result = V4::new_from(1.0, 2.0, 3.0, 4.0); 423 | let v = V4::new_from(5.0, 6.0, 7.0, 8.0); 424 | let expected = V4::new_from(6.0, 8.0, 10.0, 12.0); 425 | result += v; 426 | assert_eq!( 427 | &result[..], 428 | &expected[..], 429 | "\nExpected\n{:?}\nfound\n{:?}", 430 | &result[..], 431 | &expected[..] 432 | ); 433 | } 434 | 435 | #[test] 436 | fn norm_inf_test() { 437 | let v = V4::new_from(10, 100, -9, 0); 438 | let result = v.norm_inf(); 439 | let expected = 100; 440 | assert_eq!(result, expected); 441 | } 442 | 443 | #[test] 444 | fn norm_l_test() { 445 | let v = V4::new_from(1, -1, 1, -1); 446 | let result = v.norm_l(); 447 | let expected = 4; 448 | assert_eq!(result, expected); 449 | } 450 | 451 | #[test] 452 | fn product_lhs_f32_test() { 453 | let v = V4::new_from(1.0f32, 2.0, 3.0, 4.0); 454 | let result = 2.0f32 * v; 455 | let expected = V4::new_from(2.0, 4.0, 6.0, 8.0); 456 | assert_eq!(result, expected); 457 | } 458 | } 459 | -------------------------------------------------------------------------------- /src/vector5.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file vector5.rs 3 | // 4 | // @date 06/02/20 20:49:59 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use core::fmt; 32 | use num::{Float, Zero, Num, Signed}; 33 | use core::ops::{Deref, DerefMut}; 34 | 35 | use core::ops::{Add, Sub, Div, Mul, SubAssign, AddAssign, Neg}; 36 | 37 | use crate::slices_methods::{norm_inf, norm_l}; 38 | use crate::errors::VectorErrors; 39 | use crate::matrix5x5::M55; 40 | //------------------------------------------------------------------------- 41 | // code 42 | //------------------------------------------------------------------------- 43 | #[derive(Copy, Clone, Debug, PartialEq)] 44 | pub struct V5([T; 5]); 45 | 46 | impl V5 { 47 | /// create a new V5 from a static array 48 | pub const fn new(input: [T; 5]) -> Self { 49 | Self(input) 50 | } 51 | 52 | /// create a new V5 from raw numbers 53 | pub const fn new_from(num1: T, num2: T, num3: T, num4: T, num5: T) -> Self { 54 | Self::new([num1, num2, num3, num4, num5]) 55 | } 56 | } 57 | 58 | impl V5 { 59 | /// create a V5 with all elements zeros 60 | pub fn zeros() -> Self { 61 | as Zero>::zero() 62 | } 63 | 64 | /// create a V5 with all elements one 65 | pub fn ones() -> Self { 66 | let one = T::one(); 67 | Self::new([one, one, one, one, one]) 68 | } 69 | } 70 | 71 | impl V5 { 72 | pub fn norm_inf(&self) -> T { 73 | norm_inf(&**self) 74 | } 75 | } 76 | 77 | impl V5 { 78 | pub fn norm_l(&self) -> T { 79 | norm_l(&**self) 80 | } 81 | } 82 | 83 | impl Neg for V5 { 84 | type Output = Self; 85 | 86 | #[inline] 87 | fn neg(self) -> Self { 88 | Self::new_from(-self[0], -self[1], -self[2], -self[3], -self[4]) 89 | } 90 | } 91 | 92 | impl V5 { 93 | /// calculate the euclidean norm of the V5 94 | #[inline] 95 | pub fn norm2(&self) -> T { 96 | T::sqrt(self[0] * self[0] + self[1] * self[1] + self[2] * self[2] + self[3] * self[3] + self[4] * self[4]) 97 | } 98 | 99 | /// normalize the current V5 and return a new one 100 | pub fn normalize(&mut self) -> Result { 101 | let n = self.norm2(); 102 | if n != T::zero() { 103 | // this method return a new fresh and clean vector :) 104 | let mut result = Self::zeros(); 105 | for i in 0..self.len() { 106 | result[i] = self[i] / n; 107 | } 108 | Ok(result) 109 | } else { 110 | Err(VectorErrors::Norm2IsZero) 111 | } 112 | } 113 | } 114 | 115 | // V5 * V5(dot product) 116 | impl Mul for V5 { 117 | type Output = T; 118 | 119 | #[inline] 120 | fn mul(self, rhs: Self) -> T { 121 | self[0] * rhs[0] + self[1] * rhs[1] + self[2] * rhs[2] + self[3] * rhs[3] + self[4] * rhs[4] 122 | } 123 | } 124 | 125 | // V5 * constant 126 | impl Mul for V5 { 127 | type Output = V5; 128 | 129 | #[inline] 130 | fn mul(self, rhs: T) -> V5 { 131 | Self::new_from(self[0] * rhs, self[1] * rhs, self[2] * rhs, self[3] * rhs, self[4] * rhs) 132 | } 133 | } 134 | 135 | // V5 / const 136 | impl Div for V5 { 137 | type Output = Self; 138 | 139 | fn div(self, rhs: T) -> Self::Output { 140 | Self::new_from(self[0] / rhs, self[1] / rhs, self[2] / rhs, self[3] / rhs, self[4] / rhs) 141 | } 142 | } 143 | // f32 * V5 144 | impl Mul> for f32 { 145 | type Output = V5; 146 | 147 | #[inline] 148 | fn mul(self, rhs: V5) -> V5 { 149 | V5::new_from(self * rhs[0], self * rhs[1], self * rhs[2], self * rhs[3], self * rhs[4]) 150 | } 151 | } 152 | 153 | // V5 * M55 154 | impl Mul> for V5 { 155 | type Output = V5; 156 | 157 | #[inline] 158 | fn mul(self, rhs: M55) -> V5 { 159 | Self::new_from( 160 | rhs[(0, 0)] * self[0] + rhs[(1, 0)] * self[1] + rhs[(2, 0)] * self[2] + rhs[(3, 0)] * self[3] + rhs[(4, 0)] * self[4], 161 | rhs[(0, 1)] * self[0] + rhs[(1, 1)] * self[1] + rhs[(2, 1)] * self[2] + rhs[(3, 1)] * self[3] + rhs[(4, 1)] * self[4], 162 | rhs[(0, 2)] * self[0] + rhs[(1, 2)] * self[1] + rhs[(2, 2)] * self[2] + rhs[(3, 2)] * self[3] + rhs[(4, 2)] * self[4], 163 | rhs[(0, 3)] * self[0] + rhs[(1, 3)] * self[1] + rhs[(2, 3)] * self[2] + rhs[(3, 3)] * self[3] + rhs[(4, 3)] * self[4], 164 | rhs[(0, 4)] * self[0] + rhs[(1, 4)] * self[1] + rhs[(2, 4)] * self[2] + rhs[(3, 4)] * self[3] + rhs[(4, 4)] * self[4], 165 | ) 166 | } 167 | } 168 | 169 | // V5 - V5 170 | impl Sub for V5 { 171 | type Output = Self; 172 | 173 | #[inline] 174 | fn sub(self, rhs: Self) -> Self { 175 | Self::new_from(self[0] - rhs[0], self[1] - rhs[1], self[2] - rhs[2], self[3] - rhs[3], self[4] - rhs[4]) 176 | } 177 | } 178 | 179 | // V5 -= V5 180 | impl SubAssign for V5 { 181 | #[inline] 182 | fn sub_assign(&mut self, other: Self) { 183 | *self = *self - other 184 | } 185 | } 186 | 187 | // V5 + V5 188 | impl Add for V5 { 189 | type Output = Self; 190 | #[inline] 191 | fn add(self, rhs: Self) -> Self { 192 | Self::new_from(self[0] + rhs[0], self[1] + rhs[1], self[2] + rhs[2], self[3] + rhs[3], self[4] + rhs[4]) 193 | } 194 | } 195 | 196 | // V5 += V5 197 | impl AddAssign for V5 { 198 | #[inline] 199 | fn add_assign(&mut self, other: Self) { 200 | *self = *self + other 201 | } 202 | } 203 | 204 | impl Zero for V5 { 205 | fn zero() -> V5 { 206 | Self::new([T::zero(); 5]) 207 | } 208 | 209 | fn is_zero(&self) -> bool { 210 | *self == V5::zero() 211 | } 212 | } 213 | 214 | impl Deref for V5 { 215 | type Target = [T; 5]; 216 | #[inline] 217 | fn deref(&self) -> &Self::Target { 218 | &self.0 219 | } 220 | } 221 | 222 | impl DerefMut for V5 { 223 | #[inline] 224 | fn deref_mut(&mut self) -> &mut Self::Target { 225 | &mut self.0 226 | } 227 | } 228 | 229 | impl From<[T; 5]> for V5 { 230 | fn from(data: [T; 5]) -> V5 { 231 | V5(data) 232 | } 233 | } 234 | 235 | //------------------------------------------------------------------------- 236 | // Display impl 237 | //------------------------------------------------------------------------- 238 | impl fmt::Display for V5 { 239 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 240 | writeln!(dest, "[{0:^3.2} {1:^3.2} {2:^3.2} {3:^3.2} {4:^3.2}]", self[0], self[1], self[2], self[3], self[4]) 241 | } 242 | } 243 | 244 | //------------------------------------------------------------------------- 245 | // constants 246 | //------------------------------------------------------------------------- 247 | /// constant `V5` zeros 248 | pub const V5_ZEROS: V5 = V5::new_from(0.0, 0.0, 0.0, 0.0, 0.0); 249 | 250 | //------------------------------------------------------------------------- 251 | // tests 252 | //------------------------------------------------------------------------- 253 | #[cfg(test)] 254 | mod vector5_test { 255 | 256 | use crate::matrix5x5::M55; 257 | use crate::vector5::V5; 258 | 259 | #[test] 260 | fn vector5_creation_test() { 261 | let v = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 262 | assert_eq!(v[0], 1.0); 263 | assert_eq!(v[1], 2.0); 264 | assert_eq!(v[2], 3.0); 265 | assert_eq!(v[3], 4.0); 266 | assert_eq!(v[4], 5.0); 267 | } 268 | 269 | #[test] 270 | fn vector5_zeros_test() { 271 | let result: V5 = V5::zeros(); 272 | let expected = V5::new([0.0, 0.0, 0.0, 0.0, 0.0]); 273 | assert_eq!( 274 | &result[..], 275 | &expected[..], 276 | "\nExpected\n{:?}\nfound\n{:?}", 277 | &result[..], 278 | &expected[..] 279 | ); 280 | } 281 | 282 | #[test] 283 | fn vector5_add_test() { 284 | let v1 = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 285 | let v2 = V5::new([6.0, 7.0, 8.0, 9.0, 10.0]); 286 | let result = v1 + v2; 287 | let expected = V5::new([7.0, 9.0, 11.0, 13.0, 15.0]); 288 | assert_eq!( 289 | &result[..], 290 | &expected[..], 291 | "\nExpected\n{:?}\nfound\n{:?}", 292 | &result[..], 293 | &expected[..] 294 | ); 295 | } 296 | 297 | #[test] 298 | fn sub_test() { 299 | let v1 = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 300 | let v2 = V5::new([6.0, 7.0, 8.0, 9.0, 10.0]); 301 | let result = v1 - v2; 302 | let expected = V5::new([-5.0, -5.0, -5.0, -5.0, -5.0]); 303 | assert_eq!( 304 | &result[..], 305 | &expected[..], 306 | "\nExpected\n{:?}\nfound\n{:?}", 307 | &result[..], 308 | &expected[..] 309 | ); 310 | } 311 | 312 | #[test] 313 | fn vector5_mul_test() { 314 | let v1 = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 315 | let v2 = V5::new([6.0, 7.0, 8.0, 9.0, 10.0]); 316 | let result = v1 * v2; 317 | let expected = 130.0; 318 | assert_eq!(result, expected); 319 | } 320 | 321 | #[test] 322 | fn vector5_norm_test() { 323 | let v1 = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 324 | let result = v1.norm2(); 325 | let expected = 7.416198487095663; 326 | assert_eq!(result, expected); 327 | } 328 | 329 | #[test] 330 | fn mul_const_rhs() { 331 | let v = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 332 | let result = 2.0 * v; 333 | let expected = V5::new([2.0, 4.0, 6.0, 8.0, 10.0]); 334 | assert_eq!( 335 | &result[..], 336 | &expected[..], 337 | "\nExpected\n{:?}\nfound\n{:?}", 338 | &result[..], 339 | &expected[..] 340 | ); 341 | } 342 | 343 | #[test] 344 | fn mul_const() { 345 | let v = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 346 | let result = v * 2.0; 347 | let expected = V5::new([2.0, 4.0, 6.0, 8.0, 10.0]); 348 | assert_eq!( 349 | &result[..], 350 | &expected[..], 351 | "\nExpected\n{:?}\nfound\n{:?}", 352 | &result[..], 353 | &expected[..] 354 | ); 355 | } 356 | 357 | #[test] 358 | fn vector5_mul_matrix5x5_test() { 359 | let v1 = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 360 | let m = M55::new([ 361 | [10.0, 1.0, 7.0, 1.0, 5.0], 362 | [2.0, 4.0, 8.0, 3.0, 2.0], 363 | [5.0, 1.0, 2.0, 9.0, 10.0], 364 | [6.0, 9.0, 9.0, 7.0, 3.0], 365 | [1.0, 8.0, 8.0, 10.0, 5.0], 366 | ]); 367 | let result = v1 * m; 368 | let expected = V5::new([58.0, 88.0, 105.0, 112.0, 76.0]); 369 | assert_eq!( 370 | &result[..], 371 | &expected[..], 372 | "\nExpected\n{:?}\nfound\n{:?}", 373 | &result[..], 374 | &expected[..] 375 | ); 376 | } 377 | 378 | #[test] 379 | fn normalize_test() { 380 | let result = V5::new([1.0, 1.0, 1.0, 1.0, 1.0]).normalize().unwrap(); 381 | let expected = V5::new([0.4472135954999579, 0.4472135954999579, 0.4472135954999579, 0.4472135954999579, 0.4472135954999579]); 382 | assert_eq!( 383 | &result[..], 384 | &expected[..], 385 | "\nExpected\n{:?}\nfound\n{:?}", 386 | &result[..], 387 | &expected[..] 388 | ); 389 | } 390 | 391 | #[test] 392 | fn sub_assigment_test() { 393 | let mut result = V5::new([1.0, 2.0, 3.0, 4.0, 5.0]); 394 | let v = V5::new([0.0, 1.0, 2.0, 3.0, 4.0]); 395 | let expected = V5::new([1.0, 1.0, 1.0, 1.0, 1.0]); 396 | result -= v; 397 | assert_eq!( 398 | &result[..], 399 | &expected[..], 400 | "\nExpected\n{:?}\nfound\n{:?}", 401 | &result[..], 402 | &expected[..] 403 | ); 404 | } 405 | 406 | #[test] 407 | fn add_assigment_test() { 408 | let mut result = V5::new_from(1.0, 2.0, 3.0, 4.0, 5.0); 409 | let v = V5::new_from(0.0, 1.0, 2.0, 3.0, 4.0); 410 | let expected = V5::new_from(1.0, 3.0, 5.0, 7.0, 9.0); 411 | result += v; 412 | assert_eq!( 413 | &result[..], 414 | &expected[..], 415 | "\nExpected\n{:?}\nfound\n{:?}", 416 | &result[..], 417 | &expected[..] 418 | ); 419 | } 420 | 421 | #[test] 422 | fn norm_inf_test() { 423 | let v = V5::new_from(1, 2, 30, 91, 10); 424 | let result = v.norm_inf(); 425 | let expected = 91; 426 | assert_eq!(result, expected); 427 | } 428 | 429 | #[test] 430 | fn norm_l_test() { 431 | let v = V5::new_from(-1, 1, -1, 1, -1); 432 | let result = v.norm_l(); 433 | let expected = 5; 434 | assert_eq!(result, expected); 435 | } 436 | } 437 | 438 | -------------------------------------------------------------------------------- /src/vector3.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file vector3.rs 3 | // 4 | // @date 06/02/20 18:50:41 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use core::fmt; 32 | use core::ops::{Deref, DerefMut}; 33 | use num::{Float, Num, Signed, Zero}; 34 | 35 | use crate::slices_methods::{norm_inf, norm_l}; 36 | use core::ops::{Add, AddAssign, Div, Mul, Neg, Sub, SubAssign}; 37 | 38 | use crate::errors::VectorErrors; 39 | use crate::matrix3x3::M33; 40 | //------------------------------------------------------------------------- 41 | // code 42 | //------------------------------------------------------------------------- 43 | #[derive(Copy, Clone, Debug, PartialEq)] 44 | pub struct V3([T; 3]); 45 | 46 | impl V3 { 47 | /// create a new V3 from a static array 48 | pub const fn new(input: [T; 3]) -> Self { 49 | Self(input) 50 | } 51 | 52 | /// create a new V3 from raw numbers 53 | pub const fn new_from(a: T, b: T, c: T) -> Self { 54 | Self::new([a, b, c]) 55 | } 56 | } 57 | 58 | impl V3 { 59 | /// create a V3 with all elements zero 60 | #[inline(always)] 61 | pub fn zeros() -> Self { 62 | ::zero() 63 | } 64 | 65 | /// create a V3 with all elements one 66 | pub fn ones() -> Self { 67 | let one = T::one(); 68 | Self::new_from(one, one, one) 69 | } 70 | 71 | /// calculate the cross product 72 | #[inline] 73 | pub fn cross(&self, rhs: Self) -> Self { 74 | Self::new_from( 75 | self[1] * rhs[2] - self[2] * rhs[1], 76 | self[2] * rhs[0] - self[0] * rhs[2], 77 | self[0] * rhs[1] - self[1] * rhs[0], 78 | ) 79 | } 80 | 81 | /// create a unitary x axis vector 82 | pub fn x_axis() -> Self { 83 | let one = T::one(); 84 | let zero = T::zero(); 85 | Self::new_from(one, zero, zero) 86 | } 87 | 88 | /// create a unitary y axis vector 89 | pub fn y_axis() -> Self { 90 | let one = T::one(); 91 | let zero = T::zero(); 92 | Self::new_from(zero, one, zero) 93 | } 94 | 95 | /// create a unitary z axis vector 96 | pub fn z_axis() -> Self { 97 | let one = T::one(); 98 | let zero = T::zero(); 99 | Self::new_from(zero, zero, one) 100 | } 101 | } 102 | 103 | impl V3 { 104 | pub fn norm_inf(&self) -> T { 105 | norm_inf(&**self) 106 | } 107 | } 108 | 109 | impl V3 { 110 | pub fn norm_l(&self) -> T { 111 | norm_l(&**self) 112 | } 113 | } 114 | 115 | impl Neg for V3 { 116 | type Output = Self; 117 | 118 | #[inline(always)] 119 | fn neg(self) -> Self { 120 | Self::new_from(-self[0], -self[1], -self[2]) 121 | } 122 | } 123 | 124 | impl V3 { 125 | /// calculate the euclidean norm of the V3 126 | #[inline(always)] 127 | pub fn norm2(&self) -> T { 128 | T::sqrt(self[0] * self[0] + self[1] * self[1] + self[2] * self[2]) 129 | } 130 | 131 | /// normalize the current V3 132 | pub fn normalize(&self) -> Result { 133 | let n = self.norm2(); 134 | if n != T::zero() { 135 | // this method return a new fresh and clean vector :) 136 | let mut result = Self::zeros(); 137 | for i in 0..self.len() { 138 | result[i] = self[i] / n; 139 | } 140 | Ok(result) 141 | } else { 142 | Err(VectorErrors::Norm2IsZero) 143 | } 144 | } 145 | } 146 | 147 | //------------------------------------------------------------------------- 148 | // maths basic operations 149 | //------------------------------------------------------------------------- 150 | 151 | // V3 * const 152 | impl Mul for V3 { 153 | type Output = Self; 154 | 155 | #[inline(always)] 156 | fn mul(self, rhs: T) -> Self::Output { 157 | Self::new_from(self[0] * rhs, self[1] * rhs, self[2] * rhs) 158 | } 159 | } 160 | 161 | // V3 / const 162 | impl Div for V3 { 163 | type Output = Self; 164 | 165 | #[inline(always)] 166 | fn div(self, rhs: T) -> Self::Output { 167 | Self::new_from(self[0] / rhs, self[1] / rhs, self[2] / rhs) 168 | } 169 | } 170 | 171 | // FIXME(elsuizo:2020-06-19): this is a hack 172 | impl Mul> for f32 { 173 | type Output = V3; 174 | 175 | #[inline(always)] 176 | fn mul(self, rhs: V3) -> Self::Output { 177 | V3::new_from(rhs[0] * self, rhs[1] * self, rhs[2] * self) 178 | } 179 | } 180 | 181 | // f64 * V3<64> 182 | impl Mul> for f64 { 183 | type Output = V3; 184 | 185 | #[inline(always)] 186 | fn mul(self, rhs: V3) -> Self::Output { 187 | V3::new_from(rhs[0] * self, rhs[1] * self, rhs[2] * self) 188 | } 189 | } 190 | 191 | // V3 * V3 192 | impl Mul for V3 { 193 | type Output = T; 194 | 195 | #[inline(always)] 196 | fn mul(self, rhs: Self) -> T { 197 | self[0] * rhs[0] + self[1] * rhs[1] + self[2] * rhs[2] 198 | } 199 | } 200 | 201 | // V3 * M33 202 | impl Mul> for V3 { 203 | type Output = V3; 204 | 205 | #[inline] 206 | fn mul(self, rhs: M33) -> V3 { 207 | Self::new_from( 208 | rhs[(0, 0)] * self[0] + rhs[(0, 1)] * self[1] + rhs[(0, 2)] * self[2], 209 | rhs[(1, 0)] * self[0] + rhs[(1, 1)] * self[1] + rhs[(1, 2)] * self[2], 210 | rhs[(2, 0)] * self[0] + rhs[(2, 1)] * self[1] + rhs[(2, 2)] * self[2]) 211 | } 212 | } 213 | 214 | // V3 - V3 215 | impl Sub for V3 { 216 | type Output = Self; 217 | 218 | #[inline(always)] 219 | fn sub(self, rhs: Self) -> Self { 220 | Self::new_from(self[0] - rhs[0], self[1] - rhs[1], self[2] - rhs[2]) 221 | } 222 | } 223 | 224 | // V3 -= V3 225 | impl SubAssign for V3 { 226 | #[inline(always)] 227 | fn sub_assign(&mut self, other: Self) { 228 | *self = *self - other 229 | } 230 | } 231 | 232 | // V3 + V3 233 | impl Add for V3 { 234 | type Output = Self; 235 | 236 | #[inline(always)] 237 | fn add(self, rhs: Self) -> Self { 238 | Self::new_from(self[0] + rhs[0], self[1] + rhs[1], self[2] + rhs[2]) 239 | } 240 | } 241 | 242 | // V3 += V3 243 | impl AddAssign for V3 { 244 | #[inline(always)] 245 | fn add_assign(&mut self, other: Self) { 246 | *self = *self + other 247 | } 248 | } 249 | 250 | // impl the Zero trait 251 | impl Zero for V3 { 252 | #[inline(always)] 253 | fn zero() -> V3 { 254 | Self::new_from(T::zero(), T::zero(), T::zero()) 255 | } 256 | 257 | fn is_zero(&self) -> bool { 258 | *self == V3::zero() 259 | } 260 | } 261 | 262 | impl Deref for V3 { 263 | type Target = [T; 3]; 264 | #[inline] 265 | fn deref(&self) -> &Self::Target { 266 | &self.0 267 | } 268 | } 269 | 270 | impl DerefMut for V3 { 271 | #[inline] 272 | fn deref_mut(&mut self) -> &mut Self::Target { 273 | &mut self.0 274 | } 275 | } 276 | 277 | impl From<[T; 3]> for V3 { 278 | fn from(data: [T; 3]) -> V3 { 279 | V3(data) 280 | } 281 | } 282 | 283 | //------------------------------------------------------------------------- 284 | // Display impl 285 | //------------------------------------------------------------------------- 286 | impl fmt::Display for V3 { 287 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 288 | writeln!( 289 | dest, 290 | "[{0:^3.2} {1:^3.2} {2:^3.2}]", 291 | self[0], self[1], self[2] 292 | ) 293 | } 294 | } 295 | 296 | //------------------------------------------------------------------------- 297 | // constants 298 | //------------------------------------------------------------------------- 299 | /// constant `x` axis 300 | pub const X_AXIS: V3 = V3::new_from(1.0, 0.0, 0.0); 301 | /// constant `y` axis 302 | pub const Y_AXIS: V3 = V3::new_from(0.0, 1.0, 0.0); 303 | /// constant `z` axis 304 | pub const Z_AXIS: V3 = V3::new_from(0.0, 0.0, 1.0); 305 | /// constant `V3` zeros 306 | pub const V3_ZEROS: V3 = V3::new_from(0.0, 0.0, 0.0); 307 | //------------------------------------------------------------------------- 308 | // tests 309 | //------------------------------------------------------------------------- 310 | #[cfg(test)] 311 | mod vector3_test { 312 | 313 | use crate::vector3::V3; 314 | 315 | #[test] 316 | fn create_vector3_test() { 317 | let v = V3::new([1.0, 1.0, 1.0]); 318 | assert_eq!(v[0], 1.0); 319 | assert_eq!(v[1], 1.0); 320 | assert_eq!(v[2], 1.0); 321 | } 322 | 323 | #[test] 324 | fn zero_vector3_test() { 325 | let result: V3 = V3::zeros(); 326 | let expected = V3::new([0.0, 0.0, 0.0]); 327 | assert_eq!( 328 | &result[..], 329 | &expected[..], 330 | "\nExpected\n{:?}\nfound\n{:?}", 331 | &result[..], 332 | &expected[..] 333 | ); 334 | } 335 | 336 | #[test] 337 | fn product_test() { 338 | let v1 = V3::new([1.0, 2.0, 3.0]); 339 | let v2 = V3::new([4.0, 5.0, 6.0]); 340 | let result = v1 * v2; 341 | let expected = 32.0; 342 | assert_eq!(result, expected); 343 | } 344 | 345 | #[test] 346 | fn add_test() { 347 | let v1 = V3::new([1.0, 2.0, 3.0]); 348 | let v2 = V3::new([4.0, 5.0, 6.0]); 349 | let result = v1 + v2; 350 | let expected = V3::new([5.0, 7.0, 9.0]); 351 | assert_eq!( 352 | &result[..], 353 | &expected[..], 354 | "\nExpected\n{:?}\nfound\n{:?}", 355 | &result[..], 356 | &expected[..] 357 | ); 358 | } 359 | 360 | #[test] 361 | fn norm2_test() { 362 | let v1 = V3::new([1.0, 2.0, 3.0]); 363 | let expected = 3.7416573867739413; 364 | let result = v1.norm2(); 365 | assert_eq!(result, expected); 366 | } 367 | 368 | #[test] 369 | fn mul_const_rhs() { 370 | let v = V3::new([1.0, 2.0, 3.0]); 371 | let result: V3 = 2.0 * v; 372 | let expected = V3::new([2.0, 4.0, 6.0]); 373 | assert_eq!( 374 | &result[..], 375 | &expected[..], 376 | "\nExpected\n{:?}\nfound\n{:?}", 377 | &result[..], 378 | &expected[..] 379 | ); 380 | } 381 | 382 | #[test] 383 | fn mul_const() { 384 | let v = V3::new([1.0, 2.0, 3.0]); 385 | let result = v * 2.0; 386 | let expected = V3::new([2.0, 4.0, 6.0]); 387 | assert_eq!( 388 | &result[..], 389 | &expected[..], 390 | "\nExpected\n{:?}\nfound\n{:?}", 391 | &result[..], 392 | &expected[..] 393 | ); 394 | } 395 | 396 | #[test] 397 | fn sub_test() { 398 | let v1 = V3::new([1.0, 1.0, 1.0]); 399 | let v2 = V3::new([2.0, 3.0, 4.0]); 400 | let result = v1 - v2; 401 | let expected = V3::new([-1.0, -2.0, -3.0]); 402 | assert_eq!( 403 | &result[..], 404 | &expected[..], 405 | "\nExpected\n{:?}\nfound\n{:?}", 406 | &result[..], 407 | &expected[..] 408 | ); 409 | } 410 | 411 | #[test] 412 | fn normalize_test() { 413 | let result = V3::new([1.0, 1.0, 1.0]).normalize().unwrap(); 414 | let expected = V3::new([0.5773502691896258, 0.5773502691896258, 0.5773502691896258]); 415 | assert_eq!( 416 | &result[..], 417 | &expected[..], 418 | "\nExpected\n{:?}\nfound\n{:?}", 419 | &result[..], 420 | &expected[..] 421 | ); 422 | } 423 | 424 | #[test] 425 | fn cross_test() { 426 | let x = V3::new([1.0, 0.0, 0.0]); 427 | let y = V3::new([0.0, 1.0, 0.0]); 428 | 429 | let result = x.cross(y); 430 | // z 431 | let expected = V3::new([0.0, 0.0, 1.0]); 432 | assert_eq!( 433 | &result[..], 434 | &expected[..], 435 | "\nExpected\n{:?}\nfound\n{:?}", 436 | &result[..], 437 | &expected[..] 438 | ); 439 | } 440 | 441 | #[test] 442 | fn sub_assigment_test() { 443 | let mut result = V3::new([1.0, 2.0, 3.0]); 444 | let v = V3::new([4.0, 5.0, 6.0]); 445 | let expected = V3::new([-3.0, -3.0, -3.0]); 446 | result -= v; 447 | assert_eq!( 448 | &result[..], 449 | &expected[..], 450 | "\nExpected\n{:?}\nfound\n{:?}", 451 | &result[..], 452 | &expected[..] 453 | ); 454 | } 455 | 456 | #[test] 457 | fn add_assigment_test() { 458 | let mut result = V3::new_from(1.0, 2.0, 3.0); 459 | let v = V3::new_from(4.0, 5.0, 6.0); 460 | let expected = V3::new_from(5.0, 7.0, 9.0); 461 | result += v; 462 | assert_eq!( 463 | &result[..], 464 | &expected[..], 465 | "\nExpected\n{:?}\nfound\n{:?}", 466 | &result[..], 467 | &expected[..] 468 | ); 469 | } 470 | 471 | #[test] 472 | fn norm_inf_test() { 473 | let v = V3::new_from(1, -10, 73); 474 | let result = v.norm_inf(); 475 | let expected = 73; 476 | assert_eq!(result, expected); 477 | } 478 | 479 | #[test] 480 | fn norm_l_test() { 481 | let v = V3::new_from(1, -1, 1); 482 | let result = v.norm_l(); 483 | let expected = 3; 484 | assert_eq!(result, expected); 485 | } 486 | } 487 | -------------------------------------------------------------------------------- /src/vector6.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file vector6.rs 3 | // 4 | // @date 06/02/20 21:07:05 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use core::fmt; 32 | use num::{Float, Zero, Num, Signed}; 33 | use core::ops::{Deref, DerefMut}; 34 | 35 | use core::ops::{Add, Sub, Div, Mul, SubAssign, AddAssign, Neg}; 36 | 37 | use crate::slices_methods::{norm_inf, norm_l}; 38 | use crate::errors::VectorErrors; 39 | use crate::matrix6x6::M66; 40 | 41 | //------------------------------------------------------------------------- 42 | // code 43 | //------------------------------------------------------------------------- 44 | #[derive(Copy, Clone, Debug, PartialEq)] 45 | pub struct V6([T; 6]); 46 | 47 | impl V6 { 48 | /// create a new V6 from a static array 49 | pub const fn new(input: [T; 6]) -> Self { 50 | V6(input) 51 | } 52 | 53 | /// create a new V6 from raw numbers 54 | pub const fn new_from(num1: T, num2: T, num3: T, num4: T, num5: T, num6: T) -> Self { 55 | Self::new([num1, num2, num3, num4, num5, num6]) 56 | } 57 | } 58 | 59 | impl V6 { 60 | /// create a V6 with all elements zeros 61 | pub fn zeros() -> Self { 62 | as Zero>::zero() 63 | } 64 | 65 | /// create a V6 with all elements one 66 | pub fn ones() -> Self { 67 | let one = T::one(); 68 | Self::new([one, one, one, one, one, one]) 69 | } 70 | } 71 | 72 | impl V6 { 73 | pub fn norm_inf(&self) -> T { 74 | norm_inf(&**self) 75 | } 76 | } 77 | 78 | impl V6 { 79 | pub fn norm_l(&self) -> T { 80 | norm_l(&**self) 81 | } 82 | } 83 | 84 | impl Neg for V6 { 85 | type Output = Self; 86 | 87 | #[inline] 88 | fn neg(self) -> Self { 89 | Self::new_from(-self[0], -self[1], -self[2], -self[3], -self[4], -self[5]) 90 | } 91 | } 92 | 93 | impl V6 { 94 | /// calculate the euclidean norm of the V6 95 | #[inline] 96 | pub fn norm2(&self) -> T { 97 | T::sqrt(self[0] * self[0] + self[1] * self[1] + self[2] * self[2] 98 | + self[3] * self[3] + self[4] * self[4] + self[5] * self[5]) 99 | } 100 | 101 | /// normalize the current V6 and return a new one 102 | pub fn normalize(&mut self) -> Result { 103 | let n = self.norm2(); 104 | if n != T::zero() { 105 | // this method return a new fresh and clean vector :) 106 | let mut result = Self::zeros(); 107 | for i in 0..self.len() { 108 | result[i] = self[i] / n; 109 | } 110 | Ok(result) 111 | } else { 112 | Err(VectorErrors::Norm2IsZero) 113 | } 114 | } 115 | } 116 | 117 | // V6 * constant 118 | impl Mul for V6 { 119 | type Output = V6; 120 | 121 | #[inline] 122 | fn mul(self, rhs: T) -> V6 { 123 | Self::new_from(self[0] * rhs, self[1] * rhs, self[2] * rhs, 124 | self[3] * rhs, self[4] * rhs, self[5] * rhs) 125 | } 126 | } 127 | 128 | // V6 / const 129 | impl Div for V6 { 130 | type Output = Self; 131 | 132 | #[inline] 133 | fn div(self, rhs: T) -> Self::Output { 134 | Self::new_from(self[0] / rhs, self[1] / rhs, self[2] / rhs, 135 | self[3] / rhs, self[4] / rhs, self[5] / rhs) 136 | } 137 | } 138 | 139 | // f32 * V6 140 | impl Mul> for f32 { 141 | type Output = V6; 142 | 143 | #[inline] 144 | fn mul(self, rhs: V6) -> V6 { 145 | V6::new_from(self * rhs[0], self * rhs[1], self * rhs[2], 146 | self * rhs[3], self * rhs[4], self * rhs[5]) 147 | } 148 | } 149 | 150 | // V6 * V6(dot product) 151 | impl Mul for V6 { 152 | type Output = T; 153 | 154 | #[inline] 155 | fn mul(self, rhs: Self) -> T { 156 | self[0] * rhs[0] + self[1] * rhs[1] + self[2] * rhs[2] + self[3] * rhs[3] 157 | + self[4] * rhs[4] + self[5] * rhs[5] 158 | } 159 | } 160 | 161 | // V6 * M66 162 | impl Mul> for V6 { 163 | type Output = V6; 164 | 165 | fn mul(self, rhs: M66) -> V6 { 166 | let a_00 = rhs[(0, 0)]; 167 | let a_01 = rhs[(0, 1)]; 168 | let a_02 = rhs[(0, 2)]; 169 | let a_03 = rhs[(0, 3)]; 170 | let a_04 = rhs[(0, 4)]; 171 | let a_05 = rhs[(0, 5)]; 172 | let a_10 = rhs[(1, 0)]; 173 | let a_11 = rhs[(1, 1)]; 174 | let a_12 = rhs[(1, 2)]; 175 | let a_13 = rhs[(1, 3)]; 176 | let a_14 = rhs[(1, 4)]; 177 | let a_15 = rhs[(1, 5)]; 178 | let a_20 = rhs[(2, 0)]; 179 | let a_21 = rhs[(2, 1)]; 180 | let a_22 = rhs[(2, 2)]; 181 | let a_23 = rhs[(2, 3)]; 182 | let a_24 = rhs[(2, 4)]; 183 | let a_25 = rhs[(2, 5)]; 184 | let a_30 = rhs[(3, 0)]; 185 | let a_31 = rhs[(3, 1)]; 186 | let a_32 = rhs[(3, 2)]; 187 | let a_33 = rhs[(3, 3)]; 188 | let a_34 = rhs[(3, 4)]; 189 | let a_35 = rhs[(3, 5)]; 190 | let a_40 = rhs[(4, 0)]; 191 | let a_41 = rhs[(4, 1)]; 192 | let a_42 = rhs[(4, 2)]; 193 | let a_43 = rhs[(4, 3)]; 194 | let a_44 = rhs[(4, 4)]; 195 | let a_45 = rhs[(4, 5)]; 196 | let a_50 = rhs[(5, 0)]; 197 | let a_51 = rhs[(5, 1)]; 198 | let a_52 = rhs[(5, 2)]; 199 | let a_53 = rhs[(5, 3)]; 200 | let a_54 = rhs[(5, 4)]; 201 | let a_55 = rhs[(5, 5)]; 202 | 203 | let v0 = self[0]; 204 | let v1 = self[1]; 205 | let v2 = self[2]; 206 | let v3 = self[3]; 207 | let v4 = self[4]; 208 | let v5 = self[5]; 209 | 210 | V6::new([ 211 | a_00 * v0 + a_10 * v1 + a_20 * v2 + a_30 * v3 + a_40 * v4 + a_50 * v5, 212 | a_01 * v0 + a_11 * v1 + a_21 * v2 + a_31 * v3 + a_41 * v4 + a_51 * v5, 213 | a_02 * v0 + a_12 * v1 + a_22 * v2 + a_32 * v3 + a_42 * v4 + a_52 * v5, 214 | a_03 * v0 + a_13 * v1 + a_23 * v2 + a_33 * v3 + a_43 * v4 + a_53 * v5, 215 | a_04 * v0 + a_14 * v1 + a_24 * v2 + a_34 * v3 + a_44 * v4 + a_54 * v5, 216 | a_05 * v0 + a_15 * v1 + a_25 * v2 + a_35 * v3 + a_45 * v4 + a_55 * v5, 217 | ]) 218 | } 219 | } 220 | 221 | // V6 - V6 222 | impl Sub for V6 { 223 | type Output = Self; 224 | 225 | #[inline] 226 | fn sub(self, rhs: Self) -> Self { 227 | Self::new_from(self[0] - rhs[0], self[1] - rhs[1], self[2] - rhs[2], 228 | self[3] - rhs[3], self[4] - rhs[4], self[5] - rhs[5]) 229 | } 230 | } 231 | 232 | // V6 -= V6 233 | impl SubAssign for V6 { 234 | #[inline] 235 | fn sub_assign(&mut self, other: Self) { 236 | *self = *self - other 237 | } 238 | } 239 | 240 | // V6 + V6 241 | impl Add for V6 { 242 | type Output = Self; 243 | 244 | #[inline] 245 | fn add(self, rhs: Self) -> Self { 246 | Self::new_from(self[0] + rhs[0], self[1] + rhs[1], self[2] + rhs[2], 247 | self[3] + rhs[3], self[4] + rhs[4], self[5] + rhs[5]) 248 | } 249 | } 250 | 251 | // V6 += V6 252 | impl AddAssign for V6 { 253 | #[inline] 254 | fn add_assign(&mut self, other: Self) { 255 | *self = *self + other 256 | } 257 | } 258 | 259 | // impl Zero trait 260 | impl Zero for V6 { 261 | fn zero() -> V6 { 262 | V6::new([T::zero(); 6]) 263 | } 264 | 265 | fn is_zero(&self) -> bool { 266 | *self == V6::zero() 267 | } 268 | } 269 | 270 | impl Deref for V6 { 271 | type Target = [T; 6]; 272 | #[inline] 273 | fn deref(&self) -> &Self::Target { 274 | &self.0 275 | } 276 | } 277 | 278 | impl DerefMut for V6 { 279 | #[inline] 280 | fn deref_mut(&mut self) -> &mut Self::Target { 281 | &mut self.0 282 | } 283 | } 284 | 285 | impl From<[T; 6]> for V6 { 286 | fn from(data: [T; 6]) -> V6 { 287 | V6(data) 288 | } 289 | } 290 | 291 | //------------------------------------------------------------------------- 292 | // Display impl 293 | //------------------------------------------------------------------------- 294 | impl fmt::Display for V6 { 295 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 296 | writeln!(dest, "[{0:^3.2} {1:^3.2} {2:^3.2} {3:^3.2} {4:^3.2} {5:^3.2}]", 297 | self[0], self[1], self[2], self[3], self[4], self[5]) 298 | } 299 | } 300 | 301 | //------------------------------------------------------------------------- 302 | // constants 303 | //------------------------------------------------------------------------- 304 | /// constant `V6` zeros 305 | pub const V6_ZEROS: V6 = V6::new_from(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); 306 | 307 | //------------------------------------------------------------------------- 308 | // tests 309 | //------------------------------------------------------------------------- 310 | #[cfg(test)] 311 | mod vector6_tests { 312 | 313 | use crate::matrix6x6::M66; 314 | use crate::vector6::V6; 315 | 316 | #[test] 317 | fn vector6_creation_test() { 318 | let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 319 | assert_eq!(v[0], 1.0); 320 | assert_eq!(v[1], 2.0); 321 | assert_eq!(v[2], 3.0); 322 | assert_eq!(v[3], 4.0); 323 | assert_eq!(v[4], 5.0); 324 | assert_eq!(v[5], 6.0); 325 | } 326 | 327 | #[test] 328 | fn vector6_add_test() { 329 | let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 330 | let result = v + v; 331 | let expected = V6::new([2.0, 4.0, 6.0, 8.0, 10.0, 12.0]); 332 | assert_eq!( 333 | &result[..], 334 | &expected[..], 335 | "\nExpected\n{:?}\nfound\n{:?}", 336 | &result[..], 337 | &expected[..] 338 | ); 339 | } 340 | 341 | #[test] 342 | fn sub_test() { 343 | let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 344 | let result = v - v; 345 | let expected = V6::new([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]); 346 | assert_eq!( 347 | &result[..], 348 | &expected[..], 349 | "\nExpected\n{:?}\nfound\n{:?}", 350 | &result[..], 351 | &expected[..] 352 | ); 353 | } 354 | 355 | #[test] 356 | fn mul_const_rhs() { 357 | let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 358 | let result = 2.0 * v; 359 | let expected = V6::new([2.0, 4.0, 6.0, 8.0, 10.0, 12.0]); 360 | assert_eq!( 361 | &result[..], 362 | &expected[..], 363 | "\nExpected\n{:?}\nfound\n{:?}", 364 | &result[..], 365 | &expected[..] 366 | ); 367 | } 368 | 369 | #[test] 370 | fn mul_const() { 371 | let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 372 | let result = v * 2.0; 373 | let expected = V6::new([2.0, 4.0, 6.0, 8.0, 10.0, 12.0]); 374 | assert_eq!( 375 | &result[..], 376 | &expected[..], 377 | "\nExpected\n{:?}\nfound\n{:?}", 378 | &result[..], 379 | &expected[..] 380 | ); 381 | } 382 | 383 | #[test] 384 | fn product_test() { 385 | let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 386 | let result = v * v; 387 | let expected = 91.0; 388 | assert_eq!(result, expected); 389 | } 390 | #[test] 391 | fn product_matrix6x6_test() { 392 | let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 393 | 394 | let m = M66::new([ 395 | [0.0, 1.0, 2.0, 3.0, 4.0, 5.0], 396 | [6.0, 7.0, 8.0, 9.0, 10.0, 11.0], 397 | [12.0, 13.0, 14.0, 15.0, 16.0, 17.0], 398 | [18.0, 19.0, 20.0, 21.0, 22.0, 23.0], 399 | [24.0, 25.0, 26.0, 27.0, 28.0, 29.0], 400 | [30.0, 31.0, 32.0, 33.0, 34.0, 35.0], 401 | ]); 402 | let result = v * m; 403 | let expected = V6::new([420.0, 441.0, 462.0, 483.0, 504.0, 525.0]); 404 | assert_eq!( 405 | &result[..], 406 | &expected[..], 407 | "\nExpected\n{:?}\nfound\n{:?}", 408 | &result[..], 409 | &expected[..] 410 | ); 411 | } 412 | 413 | #[test] 414 | fn normalize_test() { 415 | let result = V6::new([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]).normalize().unwrap(); 416 | let expected = V6::new([0.4082482904638631, 0.4082482904638631, 0.4082482904638631, 0.4082482904638631, 0.4082482904638631, 0.4082482904638631]); 417 | assert_eq!( 418 | &result[..], 419 | &expected[..], 420 | "\nExpected\n{:?}\nfound\n{:?}", 421 | &result[..], 422 | &expected[..] 423 | ); 424 | } 425 | 426 | #[test] 427 | fn sub_assigment_test() { 428 | let mut result = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); 429 | let v = V6::new([0.0, 1.0, 2.0, 3.0, 4.0, 5.0]); 430 | let expected = V6::new([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]); 431 | result -= v; 432 | assert_eq!( 433 | &result[..], 434 | &expected[..], 435 | "\nExpected\n{:?}\nfound\n{:?}", 436 | &result[..], 437 | &expected[..] 438 | ); 439 | } 440 | 441 | #[test] 442 | fn add_assigment_test() { 443 | let mut result = V6::new_from(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); 444 | let v = V6::new_from(0.0, 1.0, 2.0, 3.0, 4.0, 5.0); 445 | let expected = V6::new_from(1.0, 3.0, 5.0, 7.0, 9.0, 11.0); 446 | result += v; 447 | assert_eq!( 448 | &result[..], 449 | &expected[..], 450 | "\nExpected\n{:?}\nfound\n{:?}", 451 | &result[..], 452 | &expected[..] 453 | ); 454 | } 455 | 456 | #[test] 457 | fn norm_inf_test() { 458 | let v = V6::new_from(1, 10, -10, 10, 100, 3); 459 | let result = v.norm_inf(); 460 | let expected = 100; 461 | assert_eq!(result, expected); 462 | } 463 | 464 | #[test] 465 | fn norm_l_test() { 466 | let v = V6::new_from(1, -1, 1, -1, 1, -1); 467 | let result = v.norm_l(); 468 | let expected = 6; 469 | assert_eq!(result, expected); 470 | } 471 | } 472 | -------------------------------------------------------------------------------- /src/matrix2x2.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file matrix2x2.rs 3 | // 4 | // @date 06/01/20 22:14:25 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | // imports 32 | #![macro_use] 33 | use core::fmt; 34 | use core::ops::{Add, AddAssign, Div, Mul, Neg, Sub, SubAssign}; 35 | use core::ops::{Deref, DerefMut, Index, IndexMut}; 36 | 37 | use crate::slices_methods::*; 38 | use crate::traits::LinearAlgebra; 39 | use crate::utils::nearly_zero; 40 | use crate::vector2::*; 41 | use num::{Float, Num, One, Signed, Zero}; 42 | 43 | /// A static Matrix of 2x2 shape 44 | #[derive(Copy, Clone, Debug, PartialEq)] 45 | pub struct M22([[T; 2]; 2]); 46 | 47 | impl M22 { 48 | #[inline(always)] 49 | pub const fn new(data_input: [[T; 2]; 2]) -> Self { 50 | Self(data_input) 51 | } 52 | 53 | #[inline(always)] 54 | pub const fn new_from(a: T, b: T, c: T, d: T) -> Self { 55 | Self::new([[a, b], [c, d]]) 56 | } 57 | 58 | #[inline(always)] 59 | pub const fn rows(&self) -> usize { 60 | self.0.len() 61 | } 62 | 63 | #[inline(always)] 64 | pub const fn cols(&self) -> usize { 65 | self.rows() 66 | } 67 | } 68 | 69 | impl LinearAlgebra for M22 { 70 | fn rows(&self) -> usize { 71 | self.0.len() 72 | } 73 | 74 | fn cols(&self) -> usize { 75 | self.rows() 76 | } 77 | 78 | #[inline(always)] 79 | fn det(&self) -> T { 80 | (self[(0, 0)] * self[(1, 1)]) - (self[(1, 0)] * self[(0, 1)]) 81 | } 82 | 83 | #[inline(always)] 84 | fn transpose(&self) -> M22 { 85 | M22::new([[self[(0, 0)], self[(1, 0)]], [self[(0, 1)], self[(1, 1)]]]) 86 | } 87 | 88 | #[inline(always)] 89 | fn trace(&self) -> T { 90 | self[(0, 0)] + self[(1, 1)] 91 | } 92 | 93 | #[inline(always)] 94 | fn norm2(&self) -> T { 95 | T::sqrt( 96 | self[(0, 0)] * self[(0, 0)] 97 | + self[(0, 1)] * self[(0, 1)] 98 | + self[(1, 0)] * self[(1, 0)] 99 | + self[(1, 1)] * self[(1, 1)], 100 | ) 101 | } 102 | 103 | #[inline(always)] 104 | fn inverse(&self) -> Option { 105 | let det = self.det(); 106 | if !nearly_zero(det) { 107 | let det_recip = det.recip(); 108 | Some(M22::new([ 109 | [self[(1, 1)] * det_recip, -self[(0, 1)] * det_recip], 110 | [-self[(1, 0)] * det_recip, self[(0, 0)] * det_recip], 111 | ])) 112 | } else { 113 | None 114 | } 115 | } 116 | 117 | /// Calculate de QR factorization of the M22 via gram-schmidt 118 | /// orthogonalization process 119 | fn qr(&self) -> Option<(Self, Self)> { 120 | if !nearly_zero(self.det()) { 121 | let cols = self.get_cols(); 122 | let mut q: [V2; 2] = *M22::zeros().get_cols(); 123 | for i in 0..q.len() { 124 | let mut q_tilde = cols[i]; 125 | for elem in q.iter().take(i) { 126 | q_tilde -= *elem * project_x_over_y(&*cols[i], &**elem); 127 | } 128 | normalize(&mut *q_tilde); 129 | q[i] = q_tilde; 130 | } 131 | let basis = V2::new_from(q[0], q[1]); 132 | let q = M22::new_from_vecs(basis); 133 | let r = q.transpose() * (*self); 134 | Some((q, r)) 135 | } else { 136 | None 137 | } 138 | } 139 | } 140 | 141 | impl M22 { 142 | /// contruct identity matrix 143 | pub fn identity() -> M22 { 144 | as One>::one() 145 | } 146 | 147 | /// construct the matrix with all zeros 148 | pub fn zeros() -> M22 { 149 | as Zero>::zero() 150 | } 151 | 152 | /// transform the matrix to a flatten vector 153 | pub fn as_vec(&self) -> [T; 4] { 154 | let mut result = [T::zero(); 4]; 155 | for (index, element) in self.iter().flatten().enumerate() { 156 | result[index] = *element; 157 | } 158 | result 159 | } 160 | 161 | /// construct the matrix from columns-vectors 162 | pub fn new_from_vecs(cols: V2>) -> Self { 163 | let mut result = Self::zeros(); 164 | 165 | for i in 0..result.cols() { 166 | result[(i, 0)] = cols[0][i]; 167 | result[(i, 1)] = cols[1][i]; 168 | } 169 | result 170 | } 171 | 172 | /// get the diagonal of the matrix 173 | pub fn get_diagonal(&self) -> V2 { 174 | let mut result = V2::zeros(); 175 | let mut index: usize = 0; 176 | for i in 0..self.rows() { 177 | for j in 0..self.cols() { 178 | if i == j { 179 | result[index] = self[(i, j)]; 180 | index += 1; 181 | } 182 | } 183 | } 184 | result 185 | } 186 | 187 | } 188 | 189 | // M22 * V2 190 | impl Mul> for M22 { 191 | type Output = V2; 192 | 193 | #[inline(always)] 194 | fn mul(self, rhs: V2) -> V2 { 195 | V2::new_from( 196 | self[(0, 0)] * rhs[0] + self[(0, 1)] * rhs[1], 197 | self[(1, 0)] * rhs[0] + self[(1, 1)] * rhs[1], 198 | ) 199 | } 200 | } 201 | 202 | // M22 + M22 203 | impl Add for M22 { 204 | type Output = Self; 205 | 206 | #[inline(always)] 207 | fn add(self, rhs: Self) -> Self { 208 | Self::new([ 209 | [self[(0, 0)] + rhs[(0, 0)], self[(0, 1)] + rhs[(0, 1)]], 210 | [self[(1, 0)] + rhs[(1, 0)], self[(1, 1)] + rhs[(1, 1)]], 211 | ]) 212 | } 213 | } 214 | 215 | // M22 += M22 216 | impl AddAssign for M22 { 217 | #[inline(always)] 218 | fn add_assign(&mut self, other: Self) { 219 | *self = *self + other 220 | } 221 | } 222 | 223 | // M22 - M22 224 | impl Sub for M22 { 225 | type Output = Self; 226 | 227 | #[inline(always)] 228 | fn sub(self, rhs: Self) -> Self { 229 | Self::new([ 230 | [self[(0, 0)] - rhs[(0, 0)], self[(0, 1)] - rhs[(0, 1)]], 231 | [self[(1, 0)] - rhs[(1, 0)], self[(1, 1)] - rhs[(1, 1)]], 232 | ]) 233 | } 234 | } 235 | 236 | // M22 -= M22 237 | impl SubAssign for M22 { 238 | #[inline] 239 | fn sub_assign(&mut self, other: Self) { 240 | *self = *self - other 241 | } 242 | } 243 | 244 | impl M22 { 245 | /// get the rows of the matrix as a vectors 246 | pub fn get_rows(self) -> V2> { 247 | let mut r0 = V2::zeros(); 248 | let mut r1 = V2::zeros(); 249 | 250 | for j in 0..self.rows() { 251 | r0[j] = self[(0, j)]; 252 | r1[j] = self[(1, j)] 253 | } 254 | 255 | V2::new([r0, r1]) 256 | } 257 | 258 | /// get the columns of the matrix as a vectors 259 | pub fn get_cols(self) -> V2> { 260 | let mut c0 = V2::zeros(); 261 | let mut c1 = V2::zeros(); 262 | 263 | for i in 0..self.cols() { 264 | c0[i] = self[(i, 0)]; 265 | c1[i] = self[(i, 1)] 266 | } 267 | 268 | V2::new([c0, c1]) 269 | } 270 | 271 | /// Applies `f` of each element in the M22 272 | pub fn for_each(&self, f: impl Fn(T) -> T) -> Self { 273 | let mut result = Self::zeros(); 274 | for i in 0..self.rows() { 275 | for j in 0..self.cols() { 276 | result[(i, j)] = f(self[(i, j)]); 277 | } 278 | } 279 | result 280 | } 281 | } 282 | 283 | impl M22 { 284 | /// compute the LU factorization 285 | pub fn lu(&self) -> (Self, T, V2) { 286 | const N: usize = 2; 287 | let tiny = T::from(1e-40).unwrap(); 288 | let mut indx: V2 = V2::zeros(); 289 | let mut lu = *self; 290 | let mut vv = V2::zeros(); 291 | let mut d = T::one(); 292 | let mut big = T::zero(); 293 | for i in 0..N { 294 | for j in 0..N { 295 | let temp = T::abs(lu[(i, j)]); 296 | if temp > big { 297 | big = temp; 298 | } 299 | } 300 | if big == T::zero() { 301 | panic!("the matrix should be non zero") 302 | } 303 | vv[i] = big.recip(); 304 | } 305 | for k in 0..N { 306 | big = T::zero(); 307 | let mut i_max = k; 308 | for i in k..N { 309 | let temp = vv[i] * T::abs(lu[(i, k)]); 310 | if temp > big { 311 | big = temp; 312 | i_max = i; 313 | } 314 | } 315 | // TODO(elsuizo:2021-08-13): cargo clippy this 316 | if k != i_max { 317 | for j in 0..N { 318 | // std::mem::swap(lu[(i_max, j)], &mut lu[(k, j)]); 319 | let temp = lu[(i_max, j)]; 320 | lu[(i_max, j)] = lu[(k, j)]; 321 | lu[(k, j)] = temp; 322 | } 323 | d = -d; 324 | vv[i_max] = vv[k]; 325 | } 326 | indx[k] = i_max; 327 | if lu[(k, k)] == T::zero() { 328 | lu[(k, k)] = tiny; 329 | } 330 | for i in (k + 1)..N { 331 | lu[(i, k)] = lu[(i, k)] / lu[(k, k)]; 332 | for j in (k + 1)..N { 333 | lu[(i, j)] = lu[(i, j)] - lu[(i, k)] * lu[(k, j)]; 334 | } 335 | } 336 | } 337 | let det = d * lu[(0, 0)] * lu[(1, 1)]; 338 | // return 339 | (lu, det, indx) 340 | } 341 | } 342 | 343 | // NOTE(elsuizo:2020-06-10): maybe an error here is better 344 | impl M22 { 345 | /// calculate the real eigen values for the matrix 346 | pub fn real_eigenvals(&self) -> Option> { 347 | let tau = self.trace(); 348 | let delta = self.det(); 349 | let tau_2 = tau * tau; 350 | let four = T::from(4)?; 351 | let discr = tau_2 - four * delta; 352 | if discr < T::zero() { 353 | None 354 | } else { 355 | let two = T::from(2)?; 356 | let lambda2 = (tau - T::sqrt(discr)) / two; 357 | let lambda1 = (tau + T::sqrt(discr)) / two; 358 | Some(V2::new([lambda1, lambda2])) 359 | } 360 | } 361 | } 362 | 363 | // FIXME(elsuizo:2020-06-19): this is a hack 364 | // f32 * M22 365 | impl Mul> for f32 { 366 | type Output = M22; 367 | 368 | #[inline] 369 | fn mul(self, rhs: M22) -> M22 { 370 | M22::new([ 371 | [rhs[(0, 0)] * self, rhs[(0, 1)] * self], 372 | [rhs[(1, 0)] * self, rhs[(1, 1)] * self], 373 | ]) 374 | } 375 | } 376 | 377 | // M22 * constant 378 | impl Mul for M22 { 379 | type Output = M22; 380 | 381 | #[inline(always)] 382 | fn mul(self, rhs: T) -> M22 { 383 | Self::new([ 384 | [self[(0, 0)] * rhs, self[(0, 1)] * rhs], 385 | [self[(1, 0)] * rhs, self[(1, 1)] * rhs], 386 | ]) 387 | } 388 | } 389 | 390 | // M22 / constant 391 | impl Div for M22 { 392 | type Output = Self; 393 | 394 | fn div(self, rhs: T) -> Self::Output { 395 | Self::new([ 396 | [self[(0, 0)] / rhs, self[(0, 1)] / rhs], 397 | [self[(1, 0)] / rhs, self[(1, 1)] / rhs], 398 | ]) 399 | } 400 | } 401 | 402 | // M22 * M22 403 | impl Mul for M22 { 404 | type Output = Self; 405 | 406 | #[inline(always)] 407 | fn mul(self, rhs: Self) -> Self { 408 | let a1 = self[(0, 0)]; 409 | let b1 = self[(0, 1)]; 410 | let c1 = self[(1, 0)]; 411 | let d1 = self[(1, 1)]; 412 | 413 | let a2 = rhs[(0, 0)]; 414 | let b2 = rhs[(0, 1)]; 415 | let c2 = rhs[(1, 0)]; 416 | let d2 = rhs[(1, 1)]; 417 | 418 | Self::new([ 419 | [a1 * a2 + b1 * c2, a1 * b2 + b1 * d2], 420 | [c1 * a2 + d1 * c2, c1 * b2 + d1 * d2], 421 | ]) 422 | } 423 | } 424 | 425 | // -M22 426 | impl Neg for M22 { 427 | type Output = Self; 428 | 429 | #[inline(always)] 430 | fn neg(self) -> Self { 431 | Self::new([ 432 | [-self[(0, 0)], -self[(0, 1)]], 433 | [-self[(1, 0)], -self[(1, 1)]], 434 | ]) 435 | } 436 | } 437 | 438 | impl Zero for M22 { 439 | #[inline(always)] 440 | fn zero() -> M22 { 441 | M22::new([[T::zero(); 2]; 2]) 442 | } 443 | 444 | fn is_zero(&self) -> bool { 445 | *self == M22::zero() 446 | } 447 | } 448 | 449 | impl One for M22 { 450 | /// Create an identity matrix 451 | fn one() -> M22 { 452 | let one = T::one(); 453 | let zero = T::zero(); 454 | M22::new([[one, zero], [zero, one]]) 455 | } 456 | } 457 | 458 | impl Deref for M22 { 459 | type Target = [[T; 2]; 2]; 460 | #[inline] 461 | fn deref(&self) -> &Self::Target { 462 | &self.0 463 | } 464 | } 465 | 466 | impl DerefMut for M22 { 467 | #[inline] 468 | fn deref_mut(&mut self) -> &mut Self::Target { 469 | &mut self.0 470 | } 471 | } 472 | 473 | impl From<[[T; 2]; 2]> for M22 { 474 | fn from(data: [[T; 2]; 2]) -> M22 { 475 | M22(data) 476 | } 477 | } 478 | 479 | //------------------------------------------------------------------------- 480 | // index rows 481 | //------------------------------------------------------------------------- 482 | impl Index for M22 { 483 | type Output = [T; 2]; 484 | #[inline(always)] 485 | fn index(&self, index: usize) -> &[T; 2] { 486 | &self.0[index] 487 | } 488 | } 489 | 490 | impl IndexMut for M22 { 491 | #[inline(always)] 492 | fn index_mut(&mut self, index: usize) -> &mut [T; 2] { 493 | &mut self.0[index] 494 | } 495 | } 496 | 497 | //------------------------------------------------------------------------- 498 | // index elements 499 | //------------------------------------------------------------------------- 500 | impl Index<(usize, usize)> for M22 { 501 | type Output = T; 502 | #[inline(always)] 503 | fn index(&self, index: (usize, usize)) -> &T { 504 | &self.0[index.0][index.1] 505 | } 506 | } 507 | 508 | impl IndexMut<(usize, usize)> for M22 { 509 | #[inline(always)] 510 | fn index_mut(&mut self, index: (usize, usize)) -> &mut T { 511 | &mut self.0[index.0][index.1] 512 | } 513 | } 514 | 515 | //------------------------------------------------------------------------- 516 | // macros 517 | //------------------------------------------------------------------------- 518 | #[macro_export] 519 | macro_rules! m22_new { 520 | ($($first_row:expr),* ; $($second_row:expr),*) => { 521 | M22::new([[$($first_row),*], [$($second_row),*]]) 522 | } 523 | } 524 | 525 | //------------------------------------------------------------------------- 526 | // Display for M22 527 | //------------------------------------------------------------------------- 528 | impl fmt::Display for M22 { 529 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 530 | println!(); 531 | writeln!(dest, "|{0:^3.2} {1:^3.2}|", self[(0, 0)], self[(0, 1)])?; 532 | writeln!(dest, "|{0:^3.2} {1:^3.2}|", self[(1, 0)], self[(1, 1)]) 533 | } 534 | } 535 | 536 | //------------------------------------------------------------------------- 537 | // constants 538 | //------------------------------------------------------------------------- 539 | // TODO(elsuizo): no se si me sirve esto pero lo podriamos dejar 540 | pub const M22_ZEROS: M22 = m22_new!(0.0, 0.0; 0.0, 0.0); 541 | pub const M22_IDENT: M22 = m22_new!(1.0, 0.0; 0.0, 1.0); 542 | //------------------------------------------------------------------------- 543 | // testing 544 | //------------------------------------------------------------------------- 545 | 546 | #[cfg(test)] 547 | mod test_matrix2x2 { 548 | use crate::matrix2x2::M22; 549 | use crate::traits::LinearAlgebra; 550 | use crate::utils::{compare_vecs, nearly_equal}; 551 | use crate::vector2::V2; 552 | 553 | const EPS: f32 = 1e-7; 554 | 555 | #[test] 556 | fn create_m22_floats() { 557 | let matrix = M22::new([[0.0, 1.0], [2.0, 3.0]]); 558 | assert_eq!(matrix[(0, 0)], 0.0); 559 | assert_eq!(matrix[(0, 1)], 1.0); 560 | assert_eq!(matrix[(1, 0)], 2.0); 561 | assert_eq!(matrix[(1, 1)], 3.0); 562 | } 563 | 564 | #[test] 565 | fn create_m22_test() { 566 | let m = m22_new!(0.0, 1.0; 567 | 2.0, 3.0); 568 | 569 | assert_eq!(m[(0, 0)], 0.0); 570 | assert_eq!(m[(0, 1)], 1.0); 571 | assert_eq!(m[(1, 0)], 2.0); 572 | assert_eq!(m[(1, 1)], 3.0); 573 | } 574 | 575 | #[test] 576 | fn create_m22_ints() { 577 | let m = M22::new([[0, 1], [2, 3]]); 578 | assert_eq!(m[(0, 0)], 0); 579 | assert_eq!(m[(0, 1)], 1); 580 | assert_eq!(m[(1, 0)], 2); 581 | assert_eq!(m[(1, 1)], 3); 582 | } 583 | 584 | #[test] 585 | fn create_identity_floats() { 586 | let expected = M22::new([[1.0, 0.0], [0.0, 1.0]]); 587 | let result: M22 = M22::identity(); 588 | assert_eq!(result.as_vec(), expected.as_vec()); 589 | } 590 | 591 | #[test] 592 | fn create_identity_ints() { 593 | let expected = M22::new([[1, 0], [0, 1]]); 594 | let result: M22 = M22::identity(); 595 | assert_eq!(result.as_vec(), expected.as_vec()); 596 | } 597 | 598 | #[test] 599 | fn add_m22_floats() { 600 | let m1 = M22::new([[1.0, 2.0], [3.0, 4.0]]); 601 | let m2 = M22::new([[5.0, 6.0], [7.0, 8.0]]); 602 | let expected = M22::new([[6.0, 8.0], [10.0, 12.0]]); 603 | let result = m1 + m2; 604 | assert_eq!(result.as_vec(), expected.as_vec()); 605 | } 606 | 607 | #[test] 608 | fn sub_test() { 609 | let m1 = m22_new!(1.0, 2.0; 610 | 3.0, 4.0); 611 | let m2 = m22_new!(5.0, 6.0; 612 | 7.0, 8.0); 613 | let expected = m22_new!( -4.0, -4.0; 614 | -4.0, -4.0); 615 | let result = m1 - m2; 616 | assert_eq!(result.as_vec(), expected.as_vec()); 617 | } 618 | 619 | #[test] 620 | fn add_m22_ints() { 621 | let m1 = M22::new([[1, 2], [3, 4]]); 622 | let m2 = M22::new([[5, 6], [7, 8]]); 623 | let expected = M22::new([[6, 8], [10, 12]]); 624 | let result = m1 + m2; 625 | assert_eq!(result.as_vec(), expected.as_vec()); 626 | } 627 | 628 | #[test] 629 | fn test_determinant() { 630 | let m1 = M22::new([[1.0, 2.0], [1.0, 2.0]]); 631 | let result = m1.det(); 632 | let expected = 0.0; 633 | assert_eq!(result, expected); 634 | } 635 | 636 | #[test] 637 | fn product_with_vector2_rhs_test() { 638 | let m1 = M22::new([[1.0, 2.0], [3.0, 4.0]]); 639 | let v = V2::new([1.0, 2.0]); 640 | 641 | let result = m1 * v; 642 | let expected = V2::new([5.0, 11.0]); 643 | assert_eq!( 644 | &result[..], 645 | &expected[..], 646 | "\nExpected\n{:?}\nfound\n{:?}", 647 | &result[..], 648 | &expected[..] 649 | ); 650 | } 651 | 652 | #[test] 653 | fn product_with_matrix2x2_rhs_test() { 654 | let v = V2::new([1.0, 2.0]); 655 | let m1 = M22::new([[1.0, 2.0], [3.0, 4.0]]); 656 | let result = v * m1; 657 | let expected = V2::new([7.0, 10.0]); 658 | assert_eq!( 659 | &result[..], 660 | &expected[..], 661 | "\nExpected\n{:?}\nfound\n{:?}", 662 | &result[..], 663 | &expected[..] 664 | ); 665 | } 666 | 667 | #[test] 668 | fn inverse_test() { 669 | // NOTE(elsuizo:2020-06-02): no se si conviene asi o poner el numero 670 | // directamente 671 | use super::test_matrix2x2::EPS; 672 | let m1 = M22::new([[1.0, 2.0], [3.0, 4.0]]); 673 | let expected = M22::new([[-2.0, 1.0], [1.5, -0.5]]); 674 | if let Some(result) = m1.inverse() { 675 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 676 | } 677 | } 678 | 679 | #[test] 680 | fn inverse_fail() { 681 | let m1 = M22::new([[1.0, 2.0], [1.0, 2.0]]); 682 | let result = m1.inverse(); 683 | assert!(result.is_none()) 684 | } 685 | 686 | #[test] 687 | fn get_columns_test() { 688 | let m1 = m22_new!(1.0, 2.0; 689 | 3.0, 4.0); 690 | let result = m1.get_cols(); 691 | 692 | let expected1 = V2::new([1.0, 3.0]); 693 | let expected2 = V2::new([2.0, 4.0]); 694 | let expected = V2::new([expected1, expected2]); 695 | assert_eq!( 696 | &result[..], 697 | &expected[..], 698 | "\nExpected\n{:?}\nfound\n{:?}", 699 | &result[..], 700 | &expected[..] 701 | ); 702 | } 703 | 704 | #[test] 705 | fn get_rows_test() { 706 | let m1 = m22_new!(1.0, 2.0; 707 | 3.0, 4.0); 708 | let result = m1.get_rows(); 709 | 710 | let expected1 = V2::new([1.0, 2.0]); 711 | let expected2 = V2::new([3.0, 4.0]); 712 | let expected = V2::new([expected1, expected2]); 713 | assert_eq!( 714 | &result[..], 715 | &expected[..], 716 | "\nExpected\n{:?}\nfound\n{:?}", 717 | &result[..], 718 | &expected[..] 719 | ); 720 | } 721 | 722 | #[test] 723 | fn new_from_vecs_test() { 724 | let expected = m22_new!(1.0, 2.0; 725 | 3.0, 4.0); 726 | 727 | let cols = expected.get_cols(); 728 | 729 | let result = M22::new_from_vecs(cols); 730 | 731 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 732 | } 733 | 734 | #[test] 735 | fn qr_test() { 736 | let expected = m22_new!(10.0, 2.0; 737 | 3.0, -4.0); 738 | if let Some((q, r)) = expected.qr() { 739 | let result = q * r; 740 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 741 | assert!(nearly_equal(q.det().abs(), 1.0, EPS)); 742 | } 743 | } 744 | 745 | #[test] 746 | fn get_diagonal() { 747 | let m = m22_new!(10.0, 2.0; 748 | 3.0, -4.0); 749 | let result = m.get_diagonal(); 750 | let expected = V2::new([10.0, -4.0]); 751 | assert_eq!( 752 | &result[..], 753 | &expected[..], 754 | "\nExpected\n{:?}\nfound\n{:?}", 755 | &result[..], 756 | &expected[..] 757 | ); 758 | } 759 | 760 | #[test] 761 | fn for_each_test() { 762 | let m = m22_new!(10.0, 2.0; 763 | 3.0, -4.0); 764 | let result = m.for_each(|element| element + 37.0); 765 | let expected = m22_new!(47.0, 39.0; 766 | 40.0, 33.0); 767 | 768 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 769 | } 770 | } 771 | -------------------------------------------------------------------------------- /src/quaternion.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file quaternions.rs 3 | // 4 | // @date 08/29/20 20:26:13 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | use core::fmt; 32 | use core::ops::{Mul, Add, Sub, Neg, Div}; 33 | use num::{Num, Float, Signed, Zero, One}; 34 | use num::traits::FloatConst; 35 | use crate::vector3::*; 36 | use crate::vector4::V4; 37 | use crate::matrix3x3::M33; 38 | use crate::transformations::rotation_to_euler; 39 | use crate::utils::nearly_zero; 40 | use crate::traits::LinearAlgebra; 41 | 42 | /// Quaternion type 43 | #[derive(Copy, Debug, Clone, PartialEq)] 44 | #[repr(C)] 45 | pub struct Quaternion { 46 | /// Scalar part 47 | q0: T, 48 | /// Imaginary part 49 | q: V3, 50 | /// flag to signaling if the Quaternion is normalized 51 | normalized: bool 52 | } 53 | 54 | impl Quaternion { 55 | 56 | /// Construct a new Quaternion from a number(real part) and a vector(imag part) 57 | #[inline(always)] 58 | pub const fn new(q0: T, q: V3) -> Self { 59 | Self{q0, q, normalized: false} 60 | } 61 | 62 | /// Construct a new Quaternion from raw four numbers 63 | #[inline(always)] 64 | pub const fn new_from(q0: T, q1: T, q2: T, q3: T) -> Self { 65 | Self{q0, q: V3::new_from(q1, q2, q3), normalized: false} 66 | } 67 | } 68 | 69 | impl Quaternion { 70 | /// dot product 71 | pub fn dot(&self, rhs: Self) -> T { 72 | self.q0 * rhs.q0 + self.q * rhs.q 73 | } 74 | 75 | /// get the real part 76 | pub fn real(&self) -> T { 77 | self.q0 78 | } 79 | 80 | /// get the imaginary part 81 | pub fn imag(&self) -> V3 { 82 | self.q 83 | } 84 | 85 | /// construct a unit Quaternion 86 | pub fn one() -> Quaternion { 87 | as One>::one() 88 | } 89 | 90 | /// construct a zero Quaternion 91 | pub fn zero() -> Self { 92 | as Zero>::zero() 93 | } 94 | 95 | /// construct a pure "real" Quaternion 96 | pub fn new_real(q0: T) -> Self { 97 | Self{q0, q: V3::zeros(), normalized: false} 98 | } 99 | 100 | /// construct a pure "imaginary" Quaternion 101 | pub fn new_imag(q: &V3) -> Self { 102 | Self{q0: T::zero(), q: *q, normalized: false} 103 | } 104 | 105 | /// calculate the abs2 of the Quaternion 106 | pub fn abs2(&self) -> T { 107 | self.q0 * self.q0 + self.q[0] * self.q[0] + self.q[1] * self.q[1] + self.q[2] * self.q[2] 108 | } 109 | 110 | /// Construct a new Quternion from a V4 111 | pub fn new_from_vec(v: &V4) -> Self { 112 | Self{q0: v[0], q: V3::new_from(v[1], v[2], v[3]), normalized: false} 113 | } 114 | 115 | /// convert the Quaternion to a rotation matrix 116 | pub fn to_rotation(&self) -> M33 { 117 | let (q0, q) = (self.real(), self.imag()); 118 | let q0_s = q0 * q0; 119 | let (q1, q2, q3) = (q[0], q[1], q[2]); 120 | let q1_s = q1 * q1; 121 | let q2_s = q2 * q2; 122 | let q3_s = q3 * q3; 123 | let two = T::one() + T::one(); 124 | 125 | m33_new!(q0_s + q1_s - q2_s - q3_s, two * q1 * q2 - two * q0 * q3, two * q1 * q3 + two * q0 * q2; 126 | two * q1 * q2 + two * q0 * q3, q0_s - q1_s + q2_s - q3_s, two * q2 * q3 - two * q0 * q1; 127 | two * q1 * q3 - two * q0 * q2, two * q2 * q3 + two * q0 * q1, q0_s - q1_s - q2_s + q3_s) 128 | } 129 | } 130 | 131 | // create the zero Quaternion 132 | impl Zero for Quaternion { 133 | fn zero() -> Self { 134 | Self::new(T::zero(), V3::zeros()) 135 | } 136 | 137 | fn is_zero(&self) -> bool { 138 | *self == Quaternion::zero() 139 | } 140 | } 141 | 142 | // create the unit Quaternion 143 | impl One for Quaternion { 144 | /// Create an identity Quaternion 145 | fn one() -> Self { 146 | let one = T::one(); 147 | Self{q0: one, q: V3::zeros(), normalized: true} 148 | } 149 | } 150 | 151 | // q + q 152 | impl Add for Quaternion { 153 | type Output = Self; 154 | #[inline] 155 | fn add(self, rhs: Self) -> Self { 156 | Self::new(self.q0 + rhs.q0, self.q + rhs.q) 157 | } 158 | } 159 | 160 | // q - q 161 | impl Sub for Quaternion { 162 | type Output = Self; 163 | #[inline] 164 | fn sub(self, rhs: Self) -> Self { 165 | Self::new(self.q0 - rhs.q0, self.q - rhs.q) 166 | } 167 | } 168 | 169 | // q / const 170 | impl Div for Quaternion { 171 | type Output = Self; 172 | 173 | #[inline] 174 | fn div(self, rhs: T) -> Self::Output { 175 | Self::new(self.q0 / rhs, self.q / rhs) 176 | } 177 | } 178 | 179 | // q / q 180 | #[allow(clippy::suspicious_arithmetic_impl)] 181 | impl Div for Quaternion { 182 | type Output = Self; 183 | 184 | fn div(self, rhs: Self) -> Self::Output { 185 | self * rhs.inverse().unwrap() 186 | } 187 | } 188 | 189 | // q * q 190 | impl Mul for Quaternion { 191 | type Output = Self; 192 | 193 | #[inline(always)] 194 | fn mul(self, rhs: Self) -> Self::Output { 195 | Self::new(self.q0 * rhs.q0 - self.q * rhs.q, rhs.q * self.q0 + self.q * rhs.q0 + self.q.cross(rhs.q)) 196 | } 197 | } 198 | 199 | // NOTE(elsuizo:2020-09-10): this implementation comes from this nice simplification 200 | // https://fgiesen.wordpress.com/2019/02/09/rotating-a-single-vector-using-a-quaternion/ 201 | // from: Fabian “ryg” Giesen 202 | impl Mul> for Quaternion { 203 | type Output = V3; 204 | #[inline(always)] 205 | fn mul(self, rhs: V3) -> Self::Output { 206 | let one = T::one(); 207 | let two = one + one; 208 | let t = (self.q * two).cross(rhs); 209 | rhs + t * self.q0 + self.q.cross(t) 210 | } 211 | } 212 | 213 | // q * const 214 | impl Mul for Quaternion { 215 | type Output = Quaternion; 216 | fn mul(self, rhs: T) -> Self::Output { 217 | Self {q0: self.q0 * rhs, q: self.q * rhs, normalized: false} 218 | } 219 | } 220 | 221 | // -q 222 | impl Neg for Quaternion { 223 | type Output = Self; 224 | #[inline] 225 | fn neg(self) -> Self { 226 | Self::new(-self.q0, -self.q) 227 | } 228 | } 229 | 230 | // *Quaternion (conjugate) 231 | impl Quaternion { 232 | #[inline] 233 | pub fn conj(&self) -> Self { 234 | Self::new(self.q0, -self.q) 235 | } 236 | } 237 | 238 | impl Quaternion { 239 | 240 | // TODO(elsuizo:2021-08-05): test all the cases for this method 241 | /// convert a rotation matrix M33 to a Quaternion 242 | pub fn from_rotation(rot: &M33) -> Self { 243 | let m = rot.transpose(); 244 | let zero = T::zero(); 245 | let one = T::one(); 246 | let half = one / (one + one); 247 | if m[(2, 2)] < zero { 248 | if m[(0, 0)] > m[(1, 1)] { 249 | let t = one + m[(0, 0)] - m[(1, 1)] - m[(2, 2)]; 250 | let q = Self::new_from(m[(1, 2)] - m[(2, 1)], t, m[(0, 1)] + m[(1, 0)], m[(2, 0)] + m[(0, 2)]); 251 | q * half / t.sqrt() 252 | } else { 253 | let t = one - m[(0, 0)] + m[(1, 1)] - m[(2, 2)]; 254 | let q = Self::new_from(m[(2, 0)] - m[(0, 2)], m[(0, 1)] + m[(1, 0)], t, m[(1, 2)] + m[(2, 1)]); 255 | q * half / t.sqrt() 256 | } 257 | } else if m[(0, 0)] < -m[(1, 1)] { 258 | let t = one - m[(0, 0)] - m[(1, 1)] + m[(2, 2)]; 259 | let q = Self::new_from(m[(0, 1)]-m[(1, 0)], m[(2, 0)] + m[(0, 2)], m[(1, 2)] + m[(2, 1)], t); 260 | q * half / t.sqrt() 261 | } else { 262 | let t = one + m[(0, 0)] + m[(1, 1)] + m[(2, 2)]; 263 | let q = Self::new_from(t, m[(1, 2)]-m[(2, 1)], m[(2, 0)] - m[(0, 2)], m[(0, 1)] - m[(1, 0)]); 264 | q * half / t.sqrt() 265 | } 266 | } 267 | } 268 | 269 | impl Quaternion { 270 | /// the euclidean norm of the Quaternion 271 | pub fn norm2(&self) -> T { 272 | self.dot(*self).sqrt() 273 | } 274 | 275 | /// normalize the Quaternion only if necessary 276 | pub fn normalize(&self) -> Option { 277 | if self.normalized { 278 | Some(*self) 279 | } else { 280 | let norm_sqr = self.norm2(); 281 | if !nearly_zero(norm_sqr) { 282 | let mut result = *self / norm_sqr; 283 | result.normalized = true; 284 | Some(result) 285 | } else { 286 | None 287 | } 288 | } 289 | } 290 | 291 | /// get the norm of the "imaginary" part 292 | pub fn abs_imag(&self) -> T { 293 | self.imag().norm2() 294 | } 295 | 296 | /// generate a Quaternion that represents a rotation of a angle `theta` 297 | /// around the axis(normalized) `v` 298 | pub fn rotation(theta: T, vector: &V3) -> Self { 299 | let one = T::one(); 300 | let two = one + one; 301 | let normalized = vector.normalize().expect("the input has to be a non zero vector"); 302 | let (sin, cos) = (theta / two).sin_cos(); 303 | let q0 = cos; 304 | let q = normalized * sin; 305 | Self{q0, q, normalized: true} 306 | } 307 | 308 | /// generate a Quaternion that represents a rotation of a angle `theta` 309 | /// around the axis(normalized) `v`, the angle `theta` is encoded in the 310 | /// norm of the vector `v` 311 | pub fn rotation_norm_encoded(v: &V3) -> Self { 312 | let one = T::one(); 313 | let two = T::from(2.0).unwrap(); 314 | let theta = v.norm2(); 315 | if !nearly_zero(theta) { 316 | let (s, c) = (theta / two).sin_cos(); 317 | Self{q0: c, q: *v * (s / theta), normalized: true} 318 | } else { 319 | Self::new(one, V3::zeros()) 320 | } 321 | } 322 | 323 | /// create a quaternion that represents the rotation from a Euler angles 324 | /// with the roll-pitch-yay convention 325 | pub fn from_euler_angles(yay: T, pitch: T, roll: T) -> Self { 326 | let one = T::one(); 327 | let two = one + one; 328 | let (roll_sin, roll_cos) = (roll / two).sin_cos(); 329 | let (pitch_sin, pitch_cos) = (pitch / two).sin_cos(); 330 | let (yay_sin, yay_cos) = (yay / two).sin_cos(); 331 | let q0 = roll_cos * pitch_cos * yay_cos + roll_sin * pitch_sin * yay_sin; 332 | let q1 = roll_sin * pitch_cos * yay_cos - roll_cos * pitch_sin * yay_sin; 333 | let q2 = roll_cos * pitch_sin * yay_cos + roll_sin * pitch_cos * yay_sin; 334 | let q3 = roll_cos * pitch_cos * yay_sin - roll_sin * pitch_sin * yay_cos; 335 | 336 | Self{q0, q: V3::new_from(q1, q2, q3), normalized: true} 337 | } 338 | 339 | /// get the angle of representation from this Quaternion 340 | pub fn get_angle(&self) -> T { 341 | let one = T::one(); 342 | let two = one + one; 343 | let n = self.q.norm2(); 344 | 345 | two * T::atan2(n, self.q0) 346 | } 347 | 348 | /// get the axis of rotation from which this Quaternion represent 349 | pub fn get_axis(&self) -> Option> { 350 | let qn = self.normalize()?; 351 | let s = T::sin(qn.get_angle() / T::from(2.0)?); 352 | (s.abs() > T::epsilon()).then(|| qn.q / s) 353 | } 354 | 355 | /// combine the two previous methods: `get_axis` and `get_angle` 356 | pub fn axis_angle(&self) -> (Option>, T) { 357 | (self.get_axis(), self.get_angle()) 358 | } 359 | 360 | // TODO(elsuizo:2021-05-20): this epsilon comparison could be wrong maybe we need a 361 | // nearly_equal here 362 | /// normalize the Quaternion 363 | pub fn normalize_q(&self) -> Self { 364 | let a = self.dot(*self); 365 | if a > T::epsilon() { 366 | let mut result = *self / a.sqrt(); 367 | result.normalized = true; 368 | result 369 | } else { 370 | Self {q0: T::zero(), q: V3::x_axis(), normalized: true} 371 | } 372 | } 373 | 374 | fn normalize_a(&self) -> (Self, T) { 375 | if self.normalized { 376 | return (*self, T::one()) 377 | } 378 | let a = self.norm2(); 379 | let mut result = *self / a; 380 | result.normalized = true; 381 | (result, a) 382 | } 383 | 384 | /// get the argument of the Quaternion 385 | pub fn argq(&self) -> Self { 386 | let result = Quaternion::new(T::zero(), self.q); 387 | result.normalize_q() 388 | } 389 | 390 | /// exponential function apply to the current Quaternion 391 | pub fn exp(&self) -> Self { 392 | let real = self.real(); 393 | let real_exp = T::exp(real); 394 | let mut scale = real_exp; 395 | let imag_norm = self.abs_imag(); 396 | 397 | if imag_norm > T::epsilon() { 398 | scale = scale * (T::sin(imag_norm) / imag_norm); 399 | } 400 | 401 | Self {q0: real_exp * T::cos(imag_norm), q: self.q * scale, normalized: self.norm2() < T::epsilon()} 402 | } 403 | 404 | /// natural logaritmic function apply to the current Quaternion 405 | pub fn ln(&self) -> Self { 406 | let (q_norm, a) = self.normalize_a(); 407 | let real = q_norm.real(); 408 | let mut imag_norm = q_norm.abs_imag(); 409 | let arg_angle = T::atan2(imag_norm, real); 410 | if imag_norm > T::epsilon() { 411 | imag_norm = arg_angle / imag_norm; 412 | Self {q0: T::ln(a), q: q_norm.q * imag_norm, normalized: false} 413 | } else { 414 | Self {q0: T::ln(a), q: V3::new_from(arg_angle, T::zero(), T::zero()), normalized: false} 415 | } 416 | } 417 | 418 | /// sqrt function apply to the current Quaternion 419 | pub fn sqrt(&self) -> Self { 420 | let one = T::one(); 421 | let two = one + one; 422 | (self.ln() * (one / two)).exp() 423 | } 424 | 425 | /// power the current Quaternion to the rhs argument 426 | pub fn pow(&self, rhs: Self) -> Self { 427 | (rhs * self.ln()).exp() 428 | } 429 | 430 | // TODO(elsuizo:2021-04-24): maybe here its better a error for the corner cases 431 | 432 | /// Spherical Linear Interpolation between two Quaternions 433 | /// this implementation follow this implementations: 434 | /// 435 | /// 436 | /// 437 | /// Function arguments: 438 | /// 439 | /// `a`: Quaternion(normalized) 440 | /// 441 | /// `b`: Quaternion(normalized) 442 | /// 443 | /// `t`: Float in the closed interval [0.0, 1.0] 444 | /// 445 | pub fn slerp(a: Self, b: Self, t: T) -> Self { 446 | let one = T::one(); 447 | let mut result = Quaternion::zero(); 448 | // calculate the angle betwen two unit Quaternions via dot product 449 | let mut cos_half_theta = a.dot(b); 450 | // if a = b or a = -b then theta(the angle between) = 0 then we can return a 451 | if cos_half_theta.abs() >= one { 452 | return a 453 | } 454 | let mut reverse_a = false; 455 | // allways follow the shortest path 456 | if cos_half_theta < T::zero() { 457 | reverse_a = true; 458 | cos_half_theta = -cos_half_theta; 459 | } 460 | let half_theta = T::acos(cos_half_theta); 461 | let sin_half_theta = T::sqrt(one - cos_half_theta * cos_half_theta); 462 | // TODO(elsuizo:2021-04-24): maybe here the comparison could be with epsilon 463 | if sin_half_theta.abs() < T::from(0.001).unwrap() { 464 | if !reverse_a { 465 | result.q0 = (one - t) * a.q0 + t * b.q0; 466 | result.q[0] = (one - t) * a.q[0] + t * b.q[0]; 467 | result.q[1] = (one - t) * a.q[1] + t * b.q[2]; 468 | result.q[2] = (one - t) * a.q[2] + t * b.q[1]; 469 | } 470 | return result 471 | } 472 | let aux1 = T::sin((one - t) * half_theta) / sin_half_theta; 473 | let aux2 = T::sin(t * half_theta) / sin_half_theta; 474 | // this part handle the correct orientation 475 | if !reverse_a { 476 | result.q0 = aux1 * a.q0 + aux2 * b.q0; 477 | result.q = a.q * aux1 + b.q * aux2; 478 | } else { 479 | result.q0 = aux1 * a.q0 - aux2 * b.q0; 480 | result.q = a.q * aux1 - b.q * aux2; 481 | } 482 | result 483 | } 484 | 485 | /// Calculate the instantaneous Quaternion derivative representing a Quaternion rotating at 486 | /// rate given by a vector rate 487 | /// 488 | /// Function arguments: 489 | /// 490 | /// `rate`: V3 491 | /// 492 | pub fn derivative(&self, rate: &V3) -> Self { 493 | let one = T::one(); 494 | let two = one + one; 495 | Self::new_imag(rate) * (one / two) * (*self) 496 | } 497 | } 498 | 499 | impl Quaternion { 500 | /// Calculate the inverse of the Quaternion 501 | pub fn inverse(&self) -> Option { 502 | if !self.normalized { 503 | let norm_sqr = self.abs2(); 504 | if !nearly_zero(norm_sqr) { 505 | Some(self.conj() / norm_sqr) 506 | } else { 507 | None 508 | } 509 | } else { 510 | Some(self.conj()) 511 | } 512 | } 513 | 514 | /// sin function apply to the current Quaternion 515 | pub fn sin(&self) -> Self { 516 | let one = T::one(); 517 | let two = one + one; 518 | let l = self.argq(); 519 | ((*self * l).exp() - (*self * -l).exp())/ (l * two) 520 | } 521 | 522 | /// cos function apply to the current Quaternion 523 | pub fn cos(&self) -> Self { 524 | let one = T::one(); 525 | let two = one + one; 526 | let l = self.argq(); 527 | ((*self * l).exp() + (*self * -l).exp()) / two 528 | } 529 | } 530 | 531 | impl Quaternion { 532 | /// get the euler angles from the Quaternion 533 | pub fn to_euler_angles(&self) -> (T, T, T) { 534 | rotation_to_euler(&self.to_rotation()) 535 | } 536 | } 537 | 538 | // convert from array to Quaternion 539 | impl From<[T; 4]> for Quaternion { 540 | fn from(data: [T; 4]) -> Quaternion { 541 | Quaternion::new_from(data[0], data[1], data[2], data[3]) 542 | } 543 | } 544 | 545 | //------------------------------------------------------------------------- 546 | // Display for Quaternion 547 | //------------------------------------------------------------------------- 548 | impl fmt::Display for Quaternion { 549 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 550 | write!(dest, "q0: {0:^3.2}, q:{1:^3.2}", self.q0, self.q) 551 | } 552 | } 553 | 554 | //------------------------------------------------------------------------- 555 | // tests 556 | //------------------------------------------------------------------------- 557 | #[cfg(test)] 558 | mod test_quaternion { 559 | use crate::vector3::V3; 560 | use crate::quaternion::Quaternion; 561 | use crate::utils::{nearly_equal}; 562 | use crate::utils::compare_vecs; 563 | use crate::transformations::{rotx, roty}; 564 | 565 | // NOTE(elsuizo:2021-04-23): this could be more small but the rotation accumulates error in 566 | // sucesives runs 567 | const EPS: f32 = 1e-6; 568 | 569 | #[test] 570 | fn quaternion_creation_test() { 571 | let q = Quaternion::new(0, V3::ones()); 572 | 573 | let expected = V3::new([1, 1, 1]); 574 | assert_eq!(q.q0, 0); 575 | assert_eq!( 576 | &q.q[..], 577 | &expected[..], 578 | "\nExpected\n{:?}\nfound\n{:?}", 579 | &q.q[..], 580 | &expected[..] 581 | ); 582 | } 583 | 584 | #[test] 585 | fn quaternion_product_test() { 586 | let a = Quaternion::new(1, V3::ones()); 587 | let b = Quaternion::new(1, V3::ones()); 588 | let result = a * b; 589 | 590 | assert_eq!(result.q0, -2); 591 | assert_eq!(result.q[0], 2); 592 | assert_eq!(result.q[1], 2); 593 | assert_eq!(result.q[2], 2); 594 | 595 | let q1 = Quaternion::new(1, V3::ones()); 596 | let q2 = q1.conj(); 597 | 598 | let result = q1 * q2; 599 | let expected = Quaternion::new(q1.dot(q1), V3::zeros()); 600 | 601 | assert_eq!(result.q0, expected.q0); 602 | assert_eq!(result.q[0], expected.q[0]); 603 | assert_eq!(result.q[1], expected.q[1]); 604 | assert_eq!(result.q[2], expected.q[2]); 605 | } 606 | 607 | #[test] 608 | fn quaternion_conj() { 609 | let a = Quaternion::new(1, V3::ones()); 610 | let result = a.conj(); 611 | assert_eq!(result.q0, 1); 612 | assert_eq!(result.q[0], -1); 613 | assert_eq!(result.q[1], -1); 614 | assert_eq!(result.q[2], -1); 615 | 616 | 617 | let a_float = Quaternion::new(1.0, V3::ones()); 618 | let result_float = a_float.conj(); 619 | assert_eq!(result_float.q0, 1.0); 620 | assert_eq!(result_float.q[0], -1.0); 621 | assert_eq!(result_float.q[1], -1.0); 622 | assert_eq!(result_float.q[2], -1.0); 623 | } 624 | 625 | // NOTE(elsuizo:2021-04-14): we assume all the values of the angles in radians!!! 626 | #[test] 627 | fn rotate_vec() { 628 | let q1 = Quaternion::rotation(90.0f32.to_radians(), &V3::new_from(0.0, 0.0, 1.0)); 629 | let x = V3::new_from(1.0, 0.0, 0.0); 630 | // rotate x around z 90 degrees 631 | let result = q1 * x; 632 | let expected = V3::new_from(0.0, 1.0, 0.0); 633 | assert!(nearly_equal(result[0], expected[0], EPS)); 634 | assert!(nearly_equal(result[1], expected[1], EPS)); 635 | assert!(nearly_equal(result[2], expected[2], EPS)); 636 | } 637 | 638 | #[test] 639 | fn rotate_vec_composition_360() { 640 | let q1 = Quaternion::rotation(90.0f32.to_radians(), &V3::new_from(0.0, 0.0, 1.0)); 641 | let x = V3::new_from(1.0, 0.0, 0.0); 642 | // rotate x around z (90 * 4 = 360) degrees 643 | let result = q1 * q1 * q1 * q1 * x; 644 | assert!(nearly_equal(result[0], x[0], EPS)); 645 | assert!(nearly_equal(result[1], x[1], EPS)); 646 | assert!(nearly_equal(result[2], x[2], EPS)); 647 | } 648 | 649 | #[test] 650 | fn rotate_vec_angle_encode() { 651 | let q = Quaternion::rotation_norm_encoded(&V3::new_from(0.0, 0.0, 90.0f32.to_radians())); 652 | let x = V3::x_axis(); 653 | let result = q * x; 654 | let expected = V3::new_from(0.0, 1.0, 0.0); 655 | assert!(nearly_equal(result[0], expected[0], EPS)); 656 | assert!(nearly_equal(result[1], expected[1], EPS)); 657 | assert!(nearly_equal(result[2], expected[2], EPS)); 658 | } 659 | 660 | #[test] 661 | fn convert_rotation_test() { 662 | let q = Quaternion::rotation_norm_encoded(&V3::new_from(0.0, 0.0, 90.0f32.to_radians())); 663 | let x = V3::x_axis(); 664 | // rotate the x around z axis 360 degrees 665 | let expected = q * q * q * q * x; 666 | // convert the quaternion to a rotation matrix 667 | let m = q.to_rotation(); 668 | // rotate the x around z axis 360 degrees with the rotation matrix 669 | let result = m * m * m * m * x; 670 | 671 | assert!(nearly_equal(result[0], expected[0], EPS)); 672 | assert!(nearly_equal(result[1], expected[1], EPS)); 673 | assert!(nearly_equal(result[2], expected[2], EPS)); 674 | } 675 | 676 | #[test] 677 | fn inverse_test() { 678 | let q = Quaternion::new_from(1.0, 1.0, 1.0, 10.0); 679 | if let Some(inv) = q.inverse() { 680 | let result = q * inv; 681 | let expected = Quaternion::one(); 682 | assert!(nearly_equal(result.q0, expected.q0, EPS)); 683 | assert!(nearly_equal(result.q[0], expected.q[0], EPS)); 684 | assert!(nearly_equal(result.q[1], expected.q[1], EPS)); 685 | assert!(nearly_equal(result.q[2], expected.q[2], EPS)); 686 | } 687 | } 688 | 689 | #[test] 690 | fn division_test() { 691 | let q = Quaternion::new_from(10.0, 3.0, 7.0, 1.0); 692 | let result = q / q; 693 | let expected = Quaternion::one(); 694 | assert!(nearly_equal(result.q0, expected.q0, EPS)); 695 | assert!(nearly_equal(result.q[0], expected.q[0], EPS)); 696 | assert!(nearly_equal(result.q[1], expected.q[1], EPS)); 697 | assert!(nearly_equal(result.q[2], expected.q[2], EPS)); 698 | } 699 | 700 | #[test] 701 | fn euler_and_quaternions() { 702 | let expected = (0.1, 0.2, 0.3); 703 | let q = Quaternion::from_euler_angles(expected.0, expected.1, expected.2); 704 | let result = q.to_euler_angles(); 705 | assert!(nearly_equal(result.0, expected.0, EPS)); 706 | assert!(nearly_equal(result.1, expected.1, EPS)); 707 | assert!(nearly_equal(result.2, expected.2, EPS)); 708 | } 709 | 710 | #[test] 711 | fn slerp_test() { 712 | let a = Quaternion::rotation(1.78, &V3::new_from(1.0, 2.0, 3.0)); 713 | let b = Quaternion::rotation(1.78, &V3::x_axis()); 714 | let result = Quaternion::slerp(a, b, 0.3); 715 | // NOTE(elsuizo:2021-04-24): this result is from julia language 716 | let expected = Quaternion::new_from(0.6995922116669001, 0.42947374679735195, 0.31677365769795535, 0.475160486546933); 717 | assert!(nearly_equal(result.q0, expected.q0, EPS)); 718 | assert!(nearly_equal(result.q[0], expected.q[0], EPS)); 719 | assert!(nearly_equal(result.q[1], expected.q[1], EPS)); 720 | assert!(nearly_equal(result.q[2], expected.q[2], EPS)); 721 | } 722 | 723 | // NOTE(elsuizo:2021-08-05): convert to Quaternion and back to rotation 724 | #[test] 725 | fn to_rotation_test() { 726 | let expected = rotx(20f32.to_radians()) * roty(30f32.to_radians()); 727 | let q = Quaternion::from_rotation(&expected); 728 | let result = q.to_rotation(); 729 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 730 | } 731 | } 732 | -------------------------------------------------------------------------------- /src/matrix3x3.rs: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------- 2 | // @file matrix3x3.rs 3 | // 4 | // @date 06/02/20 18:41:39 5 | // @author Martin Noblia 6 | // @email mnoblia@disroot.org 7 | // 8 | // @brief 9 | // 10 | // @detail 11 | // 12 | // Licence MIT: 13 | // Copyright <2020> 14 | // 15 | // Permission is hereby granted, free of charge, to any person obtaining a copy 16 | // of this software and associated documentation files (the "Software"), to deal 17 | // in the Software without restriction, including without limitation the rights 18 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | // copies of the Software, and to permit persons to whom the Software is 20 | // furnished to do so, subject to the following conditions: 21 | // 22 | // The above copyright notice and this permission notice shall be included in 23 | // all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED 24 | // "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT 25 | // LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 26 | // PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 27 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 28 | // ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 29 | // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 30 | //------------------------------------------------------------------------- 31 | #![macro_use] 32 | use core::fmt; 33 | use core::ops::{Add, AddAssign, Div, Mul, Neg, Sub, SubAssign}; 34 | use core::ops::{Deref, DerefMut, Index, IndexMut}; 35 | 36 | use crate::traits::LinearAlgebra; 37 | use crate::utils::nearly_zero; 38 | use num::{Float, Num, One, Signed, Zero}; 39 | 40 | use crate::slices_methods::*; 41 | use crate::vector3::*; 42 | //------------------------------------------------------------------------- 43 | // code 44 | //------------------------------------------------------------------------- 45 | 46 | /// A static matrix of 3x3 shape 47 | #[derive(Copy, Clone, Debug, PartialEq)] 48 | pub struct M33([[T; 3]; 3]); 49 | 50 | impl M33 { 51 | pub const fn new(data_input: [[T; 3]; 3]) -> M33 { 52 | M33(data_input) 53 | } 54 | 55 | pub const fn rows(&self) -> usize { 56 | self.0.len() 57 | } 58 | 59 | pub const fn cols(&self) -> usize { 60 | self.rows() 61 | } 62 | } 63 | 64 | impl LinearAlgebra for M33 { 65 | fn rows(&self) -> usize { 66 | self.0.len() 67 | } 68 | 69 | fn cols(&self) -> usize { 70 | self.rows() 71 | } 72 | 73 | #[inline(always)] 74 | fn transpose(&self) -> M33 { 75 | M33::new([ 76 | [self[(0, 0)], self[(1, 0)], self[(2, 0)]], 77 | [self[(0, 1)], self[(1, 1)], self[(2, 1)]], 78 | [self[(0, 2)], self[(1, 2)], self[(2, 2)]], 79 | ]) 80 | } 81 | 82 | #[inline(always)] 83 | fn trace(&self) -> T { 84 | self[(0, 0)] + self[(1, 1)] + self[(2, 2)] 85 | } 86 | 87 | fn norm2(&self) -> T { 88 | T::sqrt( 89 | self[(0, 0)] * self[(0, 0)] 90 | + self[(1, 0)] * self[(1, 0)] 91 | + self[(2, 0)] * self[(2, 0)] 92 | + self[(0, 1)] * self[(0, 1)] 93 | + self[(1, 1)] * self[(1, 1)] 94 | + self[(2, 1)] * self[(2, 1)] 95 | + self[(0, 2)] * self[(0, 2)] 96 | + self[(1, 2)] * self[(1, 2)] 97 | + self[(2, 2)] * self[(2, 2)], 98 | ) 99 | } 100 | 101 | /// Calculate the determinant of the matrix 102 | #[inline(always)] 103 | fn det(&self) -> T { 104 | self[(0, 0)] * (self[(1, 1)] * self[(2, 2)] - self[(2, 1)] * self[(1, 2)]) 105 | - self[(0, 1)] * (self[(1, 0)] * self[(2, 2)] - self[(1, 2)] * self[(2, 0)]) 106 | + self[(0, 2)] * (self[(1, 0)] * self[(2, 1)] - self[(1, 1)] * self[(2, 0)]) 107 | } 108 | 109 | /// Calculate the inverse 110 | #[inline(always)] 111 | fn inverse(&self) -> Option { 112 | let det = self.det(); 113 | if !nearly_zero(det) { 114 | let invdet = det.recip(); 115 | let mut res = M33::zero(); 116 | res[(0, 0)] = (self[(1, 1)] * self[(2, 2)] - self[(2, 1)] * self[(1, 2)]) * invdet; 117 | res[(0, 1)] = (self[(0, 2)] * self[(2, 1)] - self[(0, 1)] * self[(2, 2)]) * invdet; 118 | res[(0, 2)] = (self[(0, 1)] * self[(1, 2)] - self[(0, 2)] * self[(1, 1)]) * invdet; 119 | res[(1, 0)] = (self[(1, 2)] * self[(2, 0)] - self[(1, 0)] * self[(2, 2)]) * invdet; 120 | res[(1, 1)] = (self[(0, 0)] * self[(2, 2)] - self[(0, 2)] * self[(2, 0)]) * invdet; 121 | res[(1, 2)] = (self[(1, 0)] * self[(0, 2)] - self[(0, 0)] * self[(1, 2)]) * invdet; 122 | res[(2, 0)] = (self[(1, 0)] * self[(2, 1)] - self[(2, 0)] * self[(1, 1)]) * invdet; 123 | res[(2, 1)] = (self[(2, 0)] * self[(0, 1)] - self[(0, 0)] * self[(2, 1)]) * invdet; 124 | res[(2, 2)] = (self[(0, 0)] * self[(1, 1)] - self[(1, 0)] * self[(0, 1)]) * invdet; 125 | Some(res) 126 | } else { 127 | None 128 | } 129 | } 130 | 131 | /// Calculate de QR factorization of the M33 via gram-schmidt 132 | /// orthogonalization process 133 | fn qr(&self) -> Option<(Self, Self)> { 134 | if !nearly_zero(self.det()) { 135 | let cols = self.get_cols(); 136 | let mut q: [V3; 3] = *M33::zeros().get_cols(); 137 | for i in 0..q.len() { 138 | let mut q_tilde = cols[i]; 139 | for elem in q.iter().take(i) { 140 | q_tilde -= *elem * project_x_over_y(&*cols[i], &**elem); 141 | } 142 | normalize(&mut *q_tilde); 143 | q[i] = q_tilde; 144 | } 145 | let basis = V3::new_from(q[0], q[1], q[2]); 146 | let q = M33::new_from_vecs(basis); 147 | let r = q.transpose() * (*self); 148 | Some((q, r)) 149 | } else { 150 | None 151 | } 152 | } 153 | } 154 | 155 | impl M33 { 156 | /// compute the LU factorization 157 | pub fn lu(&self) -> (Self, T, V3) { 158 | const N: usize = 3; 159 | let tiny = T::from(1e-40).unwrap(); 160 | let mut indx: V3 = V3::zeros(); 161 | let mut lu = *self; 162 | let mut vv = V3::zeros(); 163 | let mut d = T::one(); 164 | let mut big = T::zero(); 165 | for i in 0..N { 166 | for j in 0..N { 167 | let temp = T::abs(lu[(i, j)]); 168 | if temp > big { 169 | big = temp; 170 | } 171 | } 172 | if big == T::zero() { 173 | panic!("the matrix should be non zero") 174 | } 175 | vv[i] = big.recip(); 176 | } 177 | for k in 0..N { 178 | big = T::zero(); 179 | let mut i_max = k; 180 | for i in k..N { 181 | let temp = vv[i] * T::abs(lu[(i, k)]); 182 | if temp > big { 183 | big = temp; 184 | i_max = i; 185 | } 186 | } 187 | if k != i_max { 188 | for j in 0..N { 189 | let temp = lu[(i_max, j)]; 190 | lu[(i_max, j)] = lu[(k, j)]; 191 | lu[(k, j)] = temp; 192 | } 193 | d = -d; 194 | vv[i_max] = vv[k]; 195 | } 196 | indx[k] = i_max; 197 | if lu[(k, k)] == T::zero() { 198 | lu[(k, k)] = tiny; 199 | } 200 | for i in (k + 1)..N { 201 | lu[(i, k)] = lu[(i, k)] / lu[(k, k)]; 202 | for j in (k + 1)..N { 203 | lu[(i, j)] = lu[(i, j)] - lu[(i, k)] * lu[(k, j)]; 204 | } 205 | } 206 | } 207 | let det = d * lu[(0, 0)] * lu[(1, 1)] * lu[(2, 2)]; 208 | // return 209 | (lu, det, indx) 210 | } 211 | } 212 | 213 | impl M33 { 214 | /// contruct identity matrix 215 | pub fn identity() -> M33 { 216 | as One>::one() 217 | } 218 | 219 | /// construct the matrix with all zeros 220 | pub fn zeros() -> M33 { 221 | as Zero>::zero() 222 | } 223 | 224 | /// transform the matrix to a flatten vector 225 | pub fn as_vec(&self) -> [T; 9] { 226 | let mut result = [T::zero(); 9]; 227 | for (index, element) in self.iter().flatten().enumerate() { 228 | result[index] = *element; 229 | } 230 | result 231 | } 232 | 233 | /// construct the matrix from columns-vectors 234 | pub fn new_from_vecs(cols: V3>) -> Self { 235 | let mut result = Self::zeros(); 236 | 237 | for i in 0..result.cols() { 238 | result[(i, 0)] = cols[0][i]; 239 | result[(i, 1)] = cols[1][i]; 240 | result[(i, 2)] = cols[2][i]; 241 | } 242 | result 243 | } 244 | 245 | /// get the diagonal of the matrix 246 | pub fn get_diagonal(&self) -> V3 { 247 | let mut result = V3::zeros(); 248 | let mut index: usize = 0; 249 | for i in 0..self.rows() { 250 | for j in 0..self.cols() { 251 | if i == j { 252 | result[index] = self[(i, j)]; 253 | index += 1; 254 | } 255 | } 256 | } 257 | result 258 | } 259 | 260 | pub fn get_upper_triagular(&self) -> [T; 3] { 261 | let zero = T::zero(); 262 | let mut result: [T; 3] = [zero, zero, zero]; 263 | let mut index = 0; 264 | for i in 0..self.rows() { 265 | for j in 0..self.cols() { 266 | if i < j { 267 | result[index] = self[(i, j)]; 268 | index += 1; 269 | } 270 | } 271 | } 272 | result 273 | } 274 | 275 | pub fn get_lower_triangular(&self) -> [T; 3] { 276 | let zero = T::zero(); 277 | let mut result: [T; 3] = [zero, zero, zero]; 278 | let mut index = 0; 279 | for i in 0..self.rows() { 280 | for j in 0..self.cols() { 281 | if i > j { 282 | result[index] = self[(i, j)]; 283 | index += 1; 284 | } 285 | } 286 | } 287 | result 288 | } 289 | 290 | /// Applies `f` of each element in the M33 291 | pub fn for_each(&self, f: impl Fn(T) -> T) -> Self { 292 | let mut result = Self::zeros(); 293 | for i in 0..self.rows() { 294 | for j in 0..self.cols() { 295 | result[(i, j)] = f(self[(i, j)]); 296 | } 297 | } 298 | result 299 | } 300 | } 301 | 302 | impl M33 { 303 | /// get the rows of the matrix as a vectors 304 | pub fn get_rows(self) -> V3> { 305 | let mut r0 = V3::zeros(); 306 | let mut r1 = V3::zeros(); 307 | let mut r2 = V3::zeros(); 308 | 309 | for j in 0..self.rows() { 310 | r0[j] = self[(0, j)]; 311 | r1[j] = self[(1, j)]; 312 | r2[j] = self[(2, j)]; 313 | } 314 | V3::new([r0, r1, r2]) 315 | } 316 | 317 | /// get the columns of the matrix as a vectors 318 | pub fn get_cols(self) -> V3> { 319 | let mut c0 = V3::zeros(); 320 | let mut c1 = V3::zeros(); 321 | let mut c2 = V3::zeros(); 322 | 323 | for i in 0..self.rows() { 324 | c0[i] = self[(i, 0)]; 325 | c1[i] = self[(i, 1)]; 326 | c2[i] = self[(i, 2)]; 327 | } 328 | V3::new([c0, c1, c2]) 329 | } 330 | } 331 | 332 | impl Add for M33 { 333 | type Output = Self; 334 | 335 | #[inline(always)] 336 | fn add(self, rhs: Self) -> Self { 337 | M33::new([ 338 | [ 339 | self[(0, 0)] + rhs[(0, 0)], 340 | self[(0, 1)] + rhs[(0, 1)], 341 | self[(0, 2)] + rhs[(0, 2)], 342 | ], 343 | [ 344 | self[(1, 0)] + rhs[(1, 0)], 345 | self[(1, 1)] + rhs[(1, 1)], 346 | self[(1, 2)] + rhs[(1, 2)], 347 | ], 348 | [ 349 | self[(2, 0)] + rhs[(2, 0)], 350 | self[(2, 1)] + rhs[(2, 1)], 351 | self[(2, 2)] + rhs[(2, 2)], 352 | ], 353 | ]) 354 | } 355 | } 356 | 357 | // M33 += M33 358 | impl AddAssign for M33 { 359 | fn add_assign(&mut self, other: Self) { 360 | *self = *self + other 361 | } 362 | } 363 | 364 | // M33 - M33 365 | impl Sub for M33 { 366 | type Output = Self; 367 | 368 | fn sub(self, rhs: Self) -> Self { 369 | M33::new([ 370 | [ 371 | self[(0, 0)] - rhs[(0, 0)], 372 | self[(0, 1)] - rhs[(0, 1)], 373 | self[(0, 2)] - rhs[(0, 2)], 374 | ], 375 | [ 376 | self[(1, 0)] - rhs[(1, 0)], 377 | self[(1, 1)] - rhs[(1, 1)], 378 | self[(1, 2)] - rhs[(1, 2)], 379 | ], 380 | [ 381 | self[(2, 0)] - rhs[(2, 0)], 382 | self[(2, 1)] - rhs[(2, 1)], 383 | self[(2, 2)] - rhs[(2, 2)], 384 | ], 385 | ]) 386 | } 387 | } 388 | 389 | // M33 -= M33 390 | impl SubAssign for M33 { 391 | fn sub_assign(&mut self, other: Self) { 392 | *self = *self - other 393 | } 394 | } 395 | 396 | // M3 * V3 397 | impl Mul> for M33 { 398 | type Output = V3; 399 | 400 | #[inline(always)] 401 | fn mul(self, rhs: V3) -> V3 { 402 | let a_00 = self[(0, 0)]; 403 | let a_01 = self[(0, 1)]; 404 | let a_02 = self[(0, 2)]; 405 | let a_10 = self[(1, 0)]; 406 | let a_11 = self[(1, 1)]; 407 | let a_12 = self[(1, 2)]; 408 | let a_20 = self[(2, 0)]; 409 | let a_21 = self[(2, 1)]; 410 | let a_22 = self[(2, 2)]; 411 | 412 | let v0 = rhs[0]; 413 | let v1 = rhs[1]; 414 | let v2 = rhs[2]; 415 | V3::new([ 416 | a_00 * v0 + a_01 * v1 + a_02 * v2, 417 | a_10 * v0 + a_11 * v1 + a_12 * v2, 418 | a_20 * v0 + a_21 * v1 + a_22 * v2, 419 | ]) 420 | } 421 | } 422 | 423 | // M3 * constant 424 | impl Mul for M33 { 425 | type Output = M33; 426 | 427 | fn mul(self, rhs: T) -> Self::Output { 428 | let a_00 = self[(0, 0)] * rhs; 429 | let a_01 = self[(0, 1)] * rhs; 430 | let a_02 = self[(0, 2)] * rhs; 431 | let a_10 = self[(1, 0)] * rhs; 432 | let a_11 = self[(1, 1)] * rhs; 433 | let a_12 = self[(1, 2)] * rhs; 434 | let a_20 = self[(2, 0)] * rhs; 435 | let a_21 = self[(2, 1)] * rhs; 436 | let a_22 = self[(2, 2)] * rhs; 437 | 438 | M33::new([[a_00, a_01, a_02], [a_10, a_11, a_12], [a_20, a_21, a_22]]) 439 | } 440 | } 441 | 442 | // M33 / constant 443 | impl Div for M33 { 444 | type Output = Self; 445 | 446 | fn div(self, rhs: T) -> Self::Output { 447 | let a_00 = self[(0, 0)] / rhs; 448 | let a_01 = self[(0, 1)] / rhs; 449 | let a_02 = self[(0, 2)] / rhs; 450 | let a_10 = self[(1, 0)] / rhs; 451 | let a_11 = self[(1, 1)] / rhs; 452 | let a_12 = self[(1, 2)] / rhs; 453 | let a_20 = self[(2, 0)] / rhs; 454 | let a_21 = self[(2, 1)] / rhs; 455 | let a_22 = self[(2, 2)] / rhs; 456 | 457 | M33::new([[a_00, a_01, a_02], [a_10, a_11, a_12], [a_20, a_21, a_22]]) 458 | } 459 | } 460 | 461 | // f32 * M33 462 | impl Mul> for f32 { 463 | type Output = M33; 464 | 465 | fn mul(self, rhs: M33) -> Self::Output { 466 | let a_00 = self * rhs[(0, 0)]; 467 | let a_01 = self * rhs[(0, 1)]; 468 | let a_02 = self * rhs[(0, 2)]; 469 | let a_10 = self * rhs[(1, 0)]; 470 | let a_11 = self * rhs[(1, 1)]; 471 | let a_12 = self * rhs[(1, 2)]; 472 | let a_20 = self * rhs[(2, 0)]; 473 | let a_21 = self * rhs[(2, 1)]; 474 | let a_22 = self * rhs[(2, 2)]; 475 | 476 | M33::new([[a_00, a_01, a_02], [a_10, a_11, a_12], [a_20, a_21, a_22]]) 477 | } 478 | } 479 | 480 | // f64 * M33 481 | impl Mul> for f64 { 482 | type Output = M33; 483 | 484 | fn mul(self, rhs: M33) -> Self::Output { 485 | let a_00 = self * rhs[(0, 0)]; 486 | let a_01 = self * rhs[(0, 1)]; 487 | let a_02 = self * rhs[(0, 2)]; 488 | let a_10 = self * rhs[(1, 0)]; 489 | let a_11 = self * rhs[(1, 1)]; 490 | let a_12 = self * rhs[(1, 2)]; 491 | let a_20 = self * rhs[(2, 0)]; 492 | let a_21 = self * rhs[(2, 1)]; 493 | let a_22 = self * rhs[(2, 2)]; 494 | 495 | M33::new([[a_00, a_01, a_02], [a_10, a_11, a_12], [a_20, a_21, a_22]]) 496 | } 497 | } 498 | 499 | // M3 * M3 500 | impl Mul for M33 { 501 | type Output = Self; 502 | 503 | #[inline] 504 | fn mul(self, rhs: Self) -> Self { 505 | let m00 = 506 | self[(0, 0)] * rhs[(0, 0)] + self[(0, 1)] * rhs[(1, 0)] + self[(0, 2)] * rhs[(2, 0)]; 507 | let m01 = 508 | self[(0, 0)] * rhs[(0, 1)] + self[(0, 1)] * rhs[(1, 1)] + self[(0, 2)] * rhs[(2, 1)]; 509 | let m02 = 510 | self[(0, 0)] * rhs[(0, 2)] + self[(0, 1)] * rhs[(1, 2)] + self[(0, 2)] * rhs[(2, 2)]; 511 | 512 | let m10 = 513 | self[(1, 0)] * rhs[(0, 0)] + self[(1, 1)] * rhs[(1, 0)] + self[(1, 2)] * rhs[(2, 0)]; 514 | let m11 = 515 | self[(1, 0)] * rhs[(0, 1)] + self[(1, 1)] * rhs[(1, 1)] + self[(1, 2)] * rhs[(2, 1)]; 516 | let m12 = 517 | self[(1, 0)] * rhs[(0, 2)] + self[(1, 1)] * rhs[(1, 2)] + self[(1, 2)] * rhs[(2, 2)]; 518 | 519 | let m20 = 520 | self[(2, 0)] * rhs[(0, 0)] + self[(2, 1)] * rhs[(1, 0)] + self[(2, 2)] * rhs[(2, 0)]; 521 | let m21 = 522 | self[(2, 0)] * rhs[(0, 1)] + self[(2, 1)] * rhs[(1, 1)] + self[(2, 2)] * rhs[(2, 1)]; 523 | let m22 = 524 | self[(2, 0)] * rhs[(0, 2)] + self[(2, 1)] * rhs[(1, 2)] + self[(2, 2)] * rhs[(2, 2)]; 525 | 526 | M33::new([[m00, m01, m02], [m10, m11, m12], [m20, m21, m22]]) 527 | } 528 | } 529 | 530 | // -M33 531 | impl Neg for M33 { 532 | type Output = Self; 533 | 534 | fn neg(self) -> Self { 535 | let a_00 = -self[(0, 0)]; 536 | let a_01 = -self[(0, 1)]; 537 | let a_02 = -self[(0, 2)]; 538 | let a_10 = -self[(1, 0)]; 539 | let a_11 = -self[(1, 1)]; 540 | let a_12 = -self[(1, 2)]; 541 | let a_20 = -self[(2, 0)]; 542 | let a_21 = -self[(2, 1)]; 543 | let a_22 = -self[(2, 2)]; 544 | 545 | M33::new([[a_00, a_01, a_02], [a_10, a_11, a_12], [a_20, a_21, a_22]]) 546 | } 547 | } 548 | 549 | impl Zero for M33 { 550 | fn zero() -> M33 { 551 | M33::new([[T::zero(); 3]; 3]) 552 | } 553 | 554 | fn is_zero(&self) -> bool { 555 | *self == M33::zero() 556 | } 557 | } 558 | 559 | impl One for M33 { 560 | /// Create an identity matrix 561 | fn one() -> M33 { 562 | let one = T::one(); 563 | let zero = T::zero(); 564 | M33::new([[one, zero, zero], [zero, one, zero], [zero, zero, one]]) 565 | } 566 | } 567 | // 568 | impl Deref for M33 { 569 | type Target = [[T; 3]; 3]; 570 | #[inline] 571 | fn deref(&self) -> &Self::Target { 572 | &self.0 573 | } 574 | } 575 | 576 | impl DerefMut for M33 { 577 | #[inline] 578 | fn deref_mut(&mut self) -> &mut Self::Target { 579 | &mut self.0 580 | } 581 | } 582 | 583 | impl From<[[T; 3]; 3]> for M33 { 584 | fn from(data: [[T; 3]; 3]) -> M33 { 585 | M33(data) 586 | } 587 | } 588 | 589 | //------------------------------------------------------------------------- 590 | // index rows 591 | //------------------------------------------------------------------------- 592 | impl Index for M33 { 593 | type Output = [T; 3]; 594 | #[inline(always)] 595 | fn index(&self, index: usize) -> &[T; 3] { 596 | &self.0[index] 597 | } 598 | } 599 | 600 | impl IndexMut for M33 { 601 | #[inline(always)] 602 | fn index_mut(&mut self, index: usize) -> &mut [T; 3] { 603 | &mut self.0[index] 604 | } 605 | } 606 | 607 | //------------------------------------------------------------------------- 608 | // index elements 609 | //------------------------------------------------------------------------- 610 | impl Index<(usize, usize)> for M33 { 611 | type Output = T; 612 | #[inline(always)] 613 | fn index(&self, index: (usize, usize)) -> &T { 614 | &self.0[index.0][index.1] 615 | } 616 | } 617 | 618 | impl IndexMut<(usize, usize)> for M33 { 619 | #[inline(always)] 620 | fn index_mut(&mut self, index: (usize, usize)) -> &mut T { 621 | &mut self.0[index.0][index.1] 622 | } 623 | } 624 | 625 | //------------------------------------------------------------------------- 626 | // Display for M33 627 | //------------------------------------------------------------------------- 628 | impl fmt::Display for M33 { 629 | fn fmt(&self, dest: &mut fmt::Formatter) -> fmt::Result { 630 | println!(); 631 | writeln!( 632 | dest, 633 | "|{0:<7.2} {1:^7.2} {2:>7.2}|", 634 | self[(0, 0)], 635 | self[(0, 1)], 636 | self[(0, 2)] 637 | )?; 638 | writeln!( 639 | dest, 640 | "|{0:<7.2} {1:^7.2} {2:>7.2}|", 641 | self[(1, 0)], 642 | self[(1, 1)], 643 | self[(1, 2)] 644 | )?; 645 | writeln!( 646 | dest, 647 | "|{0:<7.2} {1:^7.2} {2:>7.2}|", 648 | self[(2, 0)], 649 | self[(2, 1)], 650 | self[(2, 2)] 651 | ) 652 | } 653 | } 654 | 655 | //------------------------------------------------------------------------- 656 | // macros 657 | //------------------------------------------------------------------------- 658 | #[macro_export] 659 | macro_rules! m33_new { 660 | ($($first_row:expr),*; 661 | $($second_row:expr),*; 662 | $($third_row:expr),* 663 | ) => { 664 | M33::new([[$($first_row),*], [$($second_row),*], [$($third_row),*]]) 665 | } 666 | } 667 | 668 | //------------------------------------------------------------------------- 669 | // tests 670 | //------------------------------------------------------------------------- 671 | 672 | #[cfg(test)] 673 | mod test_matrix3x3 { 674 | use crate::matrix3x3::M33; 675 | use crate::traits::LinearAlgebra; 676 | use crate::utils::compare_vecs; 677 | use crate::utils::nearly_equal; 678 | use crate::vector3::*; 679 | 680 | const EPS: f32 = 1e-8; 681 | 682 | #[test] 683 | fn create_matrix() { 684 | let matrix = M33::new([[0.0, 1.0, 2.0], [3.0, 4.0, 5.0], [6.0, 7.0, 8.0]]); 685 | assert_eq!(matrix[(0, 0)], 0.0); 686 | assert_eq!(matrix[(0, 1)], 1.0); 687 | assert_eq!(matrix[(0, 2)], 2.0); 688 | assert_eq!(matrix[(1, 0)], 3.0); 689 | assert_eq!(matrix[(1, 1)], 4.0); 690 | assert_eq!(matrix[(1, 2)], 5.0); 691 | assert_eq!(matrix[(2, 0)], 6.0); 692 | assert_eq!(matrix[(2, 1)], 7.0); 693 | assert_eq!(matrix[(2, 2)], 8.0); 694 | } 695 | 696 | #[test] 697 | fn trace_test() { 698 | let matrix = M33::new([[0.0, 1.0, 2.0], [3.0, 4.0, 5.0], [6.0, 7.0, 8.0]]); 699 | assert_eq!(matrix.trace(), 12.0); 700 | } 701 | 702 | #[test] 703 | fn add_matrix() { 704 | use super::test_matrix3x3::EPS; 705 | let m1 = M33::new([[0.0, 1.0, 2.0], [3.0, 4.0, 5.0], [6.0, 7.0, 8.0]]); 706 | 707 | let m2 = M33::new([[0.0, 1.0, 2.0], [3.0, 4.0, 5.0], [6.0, 7.0, 8.0]]); 708 | 709 | let expected = M33::new([[0.0, 2.0, 4.0], [6.0, 8.0, 10.0], [12.0, 14.0, 16.0]]); 710 | let result = m1 + m2; 711 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 712 | } 713 | 714 | #[test] 715 | fn sub_test() { 716 | use super::test_matrix3x3::EPS; 717 | let m1 = m33_new!(0.0, 1.0, 2.0; 718 | 3.0, 4.0, 5.0; 719 | 6.0, 7.0, 8.0); 720 | 721 | let m2 = m33_new!(0.0, 1.0, 2.0; 722 | 3.0, 4.0, 5.0; 723 | 6.0, 7.0, 8.0); 724 | 725 | let expected = m33_new!(0.0, 0.0, 0.0; 726 | 0.0, 0.0, 0.0; 727 | 0.0, 0.0, 0.0); 728 | 729 | let result = m1 - m2; 730 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 731 | } 732 | 733 | #[test] 734 | fn test_identity_creation() { 735 | use super::test_matrix3x3::EPS; 736 | let identity: M33 = M33::identity(); 737 | 738 | let expected = M33::new([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]); 739 | assert!(compare_vecs(&identity.as_vec(), &expected.as_vec(), EPS)); 740 | } 741 | 742 | #[test] 743 | fn test_zeros_creation() { 744 | use super::test_matrix3x3::EPS; 745 | let zero: M33 = M33::zeros(); 746 | 747 | let expected = M33::new([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]); 748 | assert!(compare_vecs(&zero.as_vec(), &expected.as_vec(), EPS)); 749 | } 750 | 751 | #[test] 752 | fn test_trace() { 753 | let m: M33 = M33::identity(); 754 | assert_eq!(m.trace(), 3.0); 755 | } 756 | 757 | #[test] 758 | fn test_norm2() { 759 | use super::test_matrix3x3::EPS; 760 | let m = M33::new([[0.0, 1.0, 2.0], [3.0, 4.0, 5.0], [6.0, 7.0, 8.0]]); 761 | assert!(nearly_equal(m.norm2(), 14.2828568570857, EPS)); 762 | } 763 | 764 | #[test] 765 | fn determinant_test() { 766 | let m = M33::new([[0.0, 1.0, 2.0], [3.0, 4.0, 5.0], [6.0, 7.0, 9.0]]); 767 | let expected = -3.0; 768 | let result = m.det(); 769 | 770 | assert!(nearly_equal(result, expected, EPS)); 771 | } 772 | 773 | #[test] 774 | fn inverse_test() { 775 | use super::test_matrix3x3::EPS; 776 | let m = M33::new([[1.0, 0.0, 3.0], [2.0, 1.0, 6.0], [1.0, 0.0, 9.0]]); 777 | // NOTE(elsuizo:2019-09-25): hay que buscar una que tenga una inversa mas facil jasjdfjas 778 | let expected = M33::new([ 779 | [1.5, 0.0, -0.5], 780 | [-2.0, 1.0, 0.0], 781 | [-0.16666666666666666, 0.0, 0.16666666666666666], 782 | ]); 783 | 784 | if let Some(result) = m.inverse() { 785 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 786 | } 787 | } 788 | 789 | #[test] 790 | fn inverse_fail() { 791 | let m = M33::new([[1.0, 0.0, 3.0], [1.0, 0.0, 3.0], [10.0, 0.0, 3.0]]); 792 | let result = m.inverse(); 793 | let expected = None; 794 | assert_eq!(result, expected); 795 | } 796 | 797 | #[test] 798 | fn test_mul_m33_v3() { 799 | let m = M33::new([[1.0, 1.0, 1.0], [3.0, 2.0, 1.0], [7.0, 3.0, 3.0]]); 800 | let v = V3::new([2.0, 7.0, 6.0]); 801 | let result = m * v; 802 | let expected = V3::new([15.0, 26.0, 53.0]); 803 | assert_eq!( 804 | &result[..], 805 | &expected[..], 806 | "\nExpected\n{:?}\nfound\n{:?}", 807 | &result[..], 808 | &expected[..] 809 | ); 810 | } 811 | 812 | #[test] 813 | fn get_columns_test() { 814 | let m = m33_new!(0.0, 1.0, 2.0; 815 | 3.0, 4.0, 5.0; 816 | 6.0, 7.0, 8.0); 817 | 818 | let result = m.get_cols(); 819 | 820 | let expected0 = V3::new([0.0, 3.0, 6.0]); 821 | let expected1 = V3::new([1.0, 4.0, 7.0]); 822 | let expected2 = V3::new([2.0, 5.0, 8.0]); 823 | 824 | let expected = V3::new([expected0, expected1, expected2]); 825 | assert_eq!( 826 | &result[..], 827 | &expected[..], 828 | "\nExpected\n{:?}\nfound\n{:?}", 829 | &result[..], 830 | &expected[..] 831 | ); 832 | } 833 | 834 | #[test] 835 | fn get_rows_test() { 836 | let m = m33_new!(0.0, 1.0, 2.0; 837 | 3.0, 4.0, 5.0; 838 | 6.0, 7.0, 8.0); 839 | 840 | let result = m.get_rows(); 841 | 842 | let expected0 = V3::new([0.0, 1.0, 2.0]); 843 | let expected1 = V3::new([3.0, 4.0, 5.0]); 844 | let expected2 = V3::new([6.0, 7.0, 8.0]); 845 | 846 | let expected = V3::new([expected0, expected1, expected2]); 847 | 848 | assert_eq!( 849 | &result[..], 850 | &expected[..], 851 | "\nExpected\n{:?}\nfound\n{:?}", 852 | &result[..], 853 | &expected[..] 854 | ); 855 | } 856 | 857 | #[test] 858 | fn new_from_vecs_test() { 859 | let expected = m33_new!(0.0, 1.0, 2.0; 860 | 3.0, 4.0, 5.0; 861 | 6.0, 7.0, 8.0); 862 | 863 | let cols = expected.get_cols(); 864 | 865 | let result = M33::new_from_vecs(cols); 866 | 867 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 868 | } 869 | 870 | #[test] 871 | fn qr_test() { 872 | let expected = m33_new!(0.0, 1.0, 2.0; 873 | 3.0, 4.0, 5.0; 874 | 6.0, 7.0, 8.0); 875 | if let Some((q, r)) = expected.qr() { 876 | let result = q * r; 877 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 878 | assert!(nearly_equal(q.det().abs(), 1.0, EPS)); 879 | } 880 | } 881 | 882 | #[test] 883 | fn get_diagonal() { 884 | let m = m33_new!(0.0, 1.0, 2.0; 885 | 3.0, 4.0, 5.0; 886 | 6.0, 7.0, 8.0); 887 | let result = m.get_diagonal(); 888 | let expected = V3::new([0.0, 4.0, 8.0]); 889 | assert_eq!( 890 | &result[..], 891 | &expected[..], 892 | "\nExpected\n{:?}\nfound\n{:?}", 893 | &result[..], 894 | &expected[..] 895 | ); 896 | } 897 | 898 | #[test] 899 | fn get_upper_triangular_test() { 900 | let m = m33_new!(0.0, 1.0, 2.0; 901 | 3.0, 4.0, 5.0; 902 | 6.0, 7.0, 8.0); 903 | let result = m.get_upper_triagular(); 904 | let expected = [1.0, 2.0, 5.0]; 905 | assert_eq!( 906 | &result[..], 907 | &expected[..], 908 | "\nExpected\n{:?}\nfound\n{:?}", 909 | &result[..], 910 | &expected[..] 911 | ); 912 | } 913 | 914 | #[test] 915 | fn get_lower_triangular_test() { 916 | let m = m33_new!(0.0, 1.0, 2.0; 917 | 3.0, 4.0, 5.0; 918 | 6.0, 7.0, 8.0); 919 | let result = m.get_lower_triangular(); 920 | let expected = [3.0, 6.0, 7.0]; 921 | assert_eq!( 922 | &result[..], 923 | &expected[..], 924 | "\nExpected\n{:?}\nfound\n{:?}", 925 | &result[..], 926 | &expected[..] 927 | ); 928 | } 929 | 930 | #[test] 931 | fn for_each_test() { 932 | let m = m33_new!(0.0, 1.0, 2.0; 933 | 3.0, 4.0, 5.0; 934 | 6.0, 7.0, 8.0); 935 | let result = m.for_each(|element| element + 1.0); 936 | let expected = m33_new!(1.0, 2.0, 3.0; 937 | 4.0, 5.0, 6.0; 938 | 7.0, 8.0, 9.0); 939 | assert!(compare_vecs(&result.as_vec(), &expected.as_vec(), EPS)); 940 | } 941 | } 942 | --------------------------------------------------------------------------------