static_math/
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> <Martin Noblia>
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//-------------------------------------------------------------------------
31use core::fmt;
32use num::{Float, Zero, Num, Signed};
33use core::ops::{Deref, DerefMut};
34
35use core::ops::{Add, Sub, Div, Mul, SubAssign, AddAssign, Neg};
36
37use crate::slices_methods::{norm_inf, norm_l};
38use crate::errors::VectorErrors;
39use crate::matrix6x6::M66;
40
41//-------------------------------------------------------------------------
42//                        code
43//-------------------------------------------------------------------------
44#[derive(Copy, Clone, Debug, PartialEq)]
45pub struct V6<T>([T; 6]);
46
47impl<T> V6<T> {
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
59impl<T: Num + Copy> V6<T> {
60    /// create a V6 with all elements zeros
61    pub fn zeros() -> Self {
62        <V6<T> 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
72impl<T: Num + Copy + core::cmp::PartialOrd> V6<T> {
73    pub fn norm_inf(&self) -> T {
74        norm_inf(&**self)
75    }
76}
77
78impl<T: Num + Copy + Signed + core::iter::Sum> V6<T> {
79    pub fn norm_l(&self) -> T {
80        norm_l(&**self)
81    }
82}
83
84impl<T: Num + Copy + Signed> Neg for V6<T> {
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
93impl<T: Float> V6<T> {
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<Self, VectorErrors> {
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
118impl<T: Num + Copy> Mul<T> for V6<T> {
119    type Output = V6<T>;
120
121    #[inline]
122    fn mul(self, rhs: T) -> V6<T> {
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
129impl<T: Num + Copy> Div<T> for V6<T> {
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<f32>
140impl Mul<V6<f32>> for f32 {
141    type Output = V6<f32>;
142
143    #[inline]
144    fn mul(self, rhs: V6<f32>) -> V6<f32> {
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)
151impl<T: Num + Copy> Mul for V6<T> {
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
162impl<T: Num + Copy> Mul<M66<T>> for V6<T> {
163    type Output = V6<T>;
164
165    fn mul(self, rhs: M66<T>) -> V6<T> {
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
222impl<T: Num + Copy> Sub for V6<T> {
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
233impl<T: Num + Copy> SubAssign for V6<T> {
234    #[inline]
235    fn sub_assign(&mut self, other: Self) {
236        *self = *self - other
237    }
238}
239
240// V6 + V6
241impl<T: Num + Copy> Add for V6<T> {
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
252impl<T: Num + Copy> AddAssign for V6<T> {
253    #[inline]
254    fn add_assign(&mut self, other: Self) {
255        *self = *self + other
256    }
257}
258
259// impl Zero trait
260impl<T: Num + Copy> Zero for V6<T> {
261    fn zero() -> V6<T> {
262        V6::new([T::zero(); 6])
263    }
264
265    fn is_zero(&self) -> bool {
266        *self == V6::zero()
267    }
268}
269
270impl<T> Deref for V6<T> {
271    type Target = [T; 6];
272    #[inline]
273    fn deref(&self) -> &Self::Target {
274        &self.0
275    }
276}
277
278impl<T> DerefMut for V6<T> {
279    #[inline]
280    fn deref_mut(&mut self) -> &mut Self::Target {
281        &mut self.0
282    }
283}
284
285impl<T> From<[T; 6]> for V6<T> {
286    fn from(data: [T; 6]) -> V6<T> {
287        V6(data)
288    }
289}
290
291//-------------------------------------------------------------------------
292//                        Display impl
293//-------------------------------------------------------------------------
294impl<T: Num + fmt::Display> fmt::Display for V6<T> {
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//                        tests
303//-------------------------------------------------------------------------
304#[cfg(test)]
305mod vector6_tests {
306
307    use crate::matrix6x6::M66;
308    use crate::vector6::V6;
309
310    #[test]
311    fn vector6_creation_test() {
312        let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
313        assert_eq!(v[0], 1.0);
314        assert_eq!(v[1], 2.0);
315        assert_eq!(v[2], 3.0);
316        assert_eq!(v[3], 4.0);
317        assert_eq!(v[4], 5.0);
318        assert_eq!(v[5], 6.0);
319    }
320
321    #[test]
322    fn vector6_add_test() {
323        let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
324        let result = v + v;
325        let expected = V6::new([2.0, 4.0, 6.0, 8.0, 10.0, 12.0]);
326        assert_eq!(
327            &result[..],
328            &expected[..],
329            "\nExpected\n{:?}\nfound\n{:?}",
330            &result[..],
331            &expected[..]
332        );
333    }
334
335    #[test]
336    fn sub_test() {
337        let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
338        let result = v - v;
339        let expected = V6::new([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
340        assert_eq!(
341            &result[..],
342            &expected[..],
343            "\nExpected\n{:?}\nfound\n{:?}",
344            &result[..],
345            &expected[..]
346        );
347    }
348
349    #[test]
350    fn mul_const_rhs() {
351        let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
352        let result = 2.0 * v;
353        let expected = V6::new([2.0, 4.0, 6.0, 8.0, 10.0, 12.0]);
354        assert_eq!(
355            &result[..],
356            &expected[..],
357            "\nExpected\n{:?}\nfound\n{:?}",
358            &result[..],
359            &expected[..]
360        );
361    }
362
363    #[test]
364    fn mul_const() {
365        let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
366        let result = v * 2.0;
367        let expected = V6::new([2.0, 4.0, 6.0, 8.0, 10.0, 12.0]);
368        assert_eq!(
369            &result[..],
370            &expected[..],
371            "\nExpected\n{:?}\nfound\n{:?}",
372            &result[..],
373            &expected[..]
374        );
375    }
376
377    #[test]
378    fn product_test() {
379        let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
380        let result = v * v;
381        let expected = 91.0;
382        assert_eq!(result, expected);
383    }
384    #[test]
385    fn product_matrix6x6_test() {
386        let v = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
387
388        let m = M66::new([
389            [0.0, 1.0, 2.0, 3.0, 4.0, 5.0],
390            [6.0, 7.0, 8.0, 9.0, 10.0, 11.0],
391            [12.0, 13.0, 14.0, 15.0, 16.0, 17.0],
392            [18.0, 19.0, 20.0, 21.0, 22.0, 23.0],
393            [24.0, 25.0, 26.0, 27.0, 28.0, 29.0],
394            [30.0, 31.0, 32.0, 33.0, 34.0, 35.0],
395        ]);
396        let result = v * m;
397        let expected = V6::new([420.0, 441.0, 462.0, 483.0, 504.0, 525.0]);
398        assert_eq!(
399            &result[..],
400            &expected[..],
401            "\nExpected\n{:?}\nfound\n{:?}",
402            &result[..],
403            &expected[..]
404        );
405    }
406
407    #[test]
408    fn normalize_test() {
409        let result = V6::new([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]).normalize().unwrap();
410        let expected = V6::new([0.4082482904638631, 0.4082482904638631, 0.4082482904638631, 0.4082482904638631, 0.4082482904638631, 0.4082482904638631]);
411        assert_eq!(
412            &result[..],
413            &expected[..],
414            "\nExpected\n{:?}\nfound\n{:?}",
415            &result[..],
416            &expected[..]
417        );
418    }
419
420    #[test]
421    fn sub_assigment_test() {
422        let mut result = V6::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
423        let v = V6::new([0.0, 1.0, 2.0, 3.0, 4.0, 5.0]);
424        let expected = V6::new([1.0, 1.0, 1.0, 1.0, 1.0, 1.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 add_assigment_test() {
437        let mut result = V6::new_from(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
438        let v = V6::new_from(0.0, 1.0, 2.0, 3.0, 4.0, 5.0);
439        let expected = V6::new_from(1.0, 3.0, 5.0, 7.0, 9.0, 11.0);
440        result += v;
441        assert_eq!(
442            &result[..],
443            &expected[..],
444            "\nExpected\n{:?}\nfound\n{:?}",
445            &result[..],
446            &expected[..]
447        );
448    }
449
450    #[test]
451    fn norm_inf_test() {
452        let v = V6::new_from(1, 10, -10, 10, 100, 3);
453        let result = v.norm_inf();
454        let expected = 100;
455        assert_eq!(result, expected);
456    }
457
458    #[test]
459    fn norm_l_test() {
460        let v = V6::new_from(1, -1, 1, -1, 1, -1);
461        let result = v.norm_l();
462        let expected = 6;
463        assert_eq!(result, expected);
464    }
465}
static_math/vector6.rs

static_math/
vector6.rs
