static_math/
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> <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};
36use core::ops::BitAnd;
37use crate::slices_methods::{norm_inf, norm_l};
38use crate::errors::VectorErrors;
39use crate::matrix4x4::M44;
40
41//-------------------------------------------------------------------------
42//                        code
43//-------------------------------------------------------------------------
44#[derive(Copy, Clone, Debug, PartialEq)]
45pub struct V4<T>([T; 4]);
46
47impl<T> V4<T> {
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
59impl<T: Num + Copy> V4<T> {
60    /// create a V4 with all elements zeros
61    pub fn zeros() -> V4<T> {
62        <V4<T> 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
73impl BitAnd for V4<bool> {
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
81impl<T: Num + Copy + core::cmp::PartialOrd> V4<T> {
82    pub fn norm_inf(&self) -> T {
83        norm_inf(&**self)
84    }
85}
86
87// norm l
88impl<T: Num + Copy + Signed + core::iter::Sum> V4<T> {
89    pub fn norm_l(&self) -> T {
90        norm_l(&**self)
91    }
92}
93
94// -V4
95impl<T: Num + Copy + Signed> Neg for V4<T> {
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
104impl<T: Float> V4<T> {
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<Self, VectorErrors> {
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)
128impl<T: Num + Copy> Mul for V4<T> {
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
138impl<T: Num + Copy> Mul<T> for V4<T> {
139    type Output = V4<T>;
140
141    #[inline(always)]
142    fn mul(self, rhs: T) -> V4<T> {
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
149impl<T: Num + Copy> Div<T> for V4<T> {
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<f32>
159impl Mul<V4<f32>> for f32 {
160    type Output = V4<f32>;
161
162    #[inline]
163    fn mul(self, rhs: V4<f32>) -> V4<f32> {
164        V4::new_from(self * rhs[0], self * rhs[1], self * rhs[2], self * rhs[3])
165    }
166}
167
168// V4 * M44
169impl<T: Num + Copy> Mul<M44<T>> for V4<T> {
170    type Output = V4<T>;
171
172    #[inline]
173    fn mul(self, rhs: M44<T>) -> V4<T> {
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
184impl<T: Num + Copy> Sub for V4<T> {
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
194impl<T: Num + Copy> SubAssign for V4<T> {
195    fn sub_assign(&mut self, other: Self) {
196        *self = *self - other
197    }
198}
199
200// V4 + V4
201impl<T: Num + Copy> Add for V4<T> {
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
211impl<T: Num + Copy> AddAssign for V4<T> {
212    #[inline]
213    fn add_assign(&mut self, other: Self) {
214        *self = *self + other
215    }
216}
217
218impl<T: Num + Copy> Zero for V4<T> {
219    #[inline]
220    fn zero() -> V4<T> {
221        V4::new([T::zero(); 4])
222    }
223
224    fn is_zero(&self) -> bool {
225        *self == V4::zero()
226    }
227}
228
229impl<T> Deref for V4<T> {
230    type Target = [T; 4];
231    #[inline]
232    fn deref(&self) -> &Self::Target {
233        &self.0
234    }
235}
236
237impl<T> DerefMut for V4<T> {
238    #[inline]
239    fn deref_mut(&mut self) -> &mut Self::Target {
240        &mut self.0
241    }
242}
243
244impl<T> From<[T; 4]> for V4<T> {
245    fn from(data: [T; 4]) -> V4<T> {
246        V4(data)
247    }
248}
249
250
251//-------------------------------------------------------------------------
252//                        Display impl
253//-------------------------------------------------------------------------
254impl<T: Num + fmt::Display> fmt::Display for V4<T> {
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//                        tests
262//-------------------------------------------------------------------------
263#[cfg(test)]
264mod vector4_test {
265    use crate::matrix4x4::M44;
266    use crate::vector4::V4;
267
268    #[test]
269    fn vector4_creation_test() {
270        let v = V4::new([1, 1, 1, 1]);
271        assert_eq!(v[0], 1);
272        assert_eq!(v[1], 1);
273        assert_eq!(v[2], 1);
274        assert_eq!(v[3], 1);
275    }
276
277    #[test]
278    fn vector4_zeros_test() {
279        let result: V4<f32> = V4::zeros();
280        let expected = V4::new([0.0, 0.0, 0.0, 0.0]);
281        assert_eq!(
282            &result[..],
283            &expected[..],
284            "\nExpected\n{:?}\nfound\n{:?}",
285            &result[..],
286            &expected[..]
287        );
288    }
289
290    #[test]
291    fn vector4_add_test() {
292        let v1 = V4::new([1.0, 2.0, 3.0, 4.0]);
293        let v2 = V4::new([5.0, 6.0, 7.0, 8.0]);
294        let result = v1 + v2;
295        let expected = V4::new([6.0, 8.0, 10.0, 12.0]);
296        assert_eq!(
297            &result[..],
298            &expected[..],
299            "\nExpected\n{:?}\nfound\n{:?}",
300            &result[..],
301            &expected[..]
302        );
303    }
304
305    #[test]
306    fn sub_test() {
307        let v1 = V4::new([1.0, 2.0, 3.0, 4.0]);
308        let v2 = V4::new([5.0, 6.0, 7.0, 8.0]);
309        let result = v1 - v2;
310        let expected = V4::new([-4.0, -4.0, -4.0, -4.0]);
311        assert_eq!(
312            &result[..],
313            &expected[..],
314            "\nExpected\n{:?}\nfound\n{:?}",
315            &result[..],
316            &expected[..]
317        );
318    }
319
320    #[test]
321    fn vector4_product_test() {
322        let v1 = V4::new([1.0, 2.0, 3.0, 4.0]);
323        let v2 = V4::new([5.0, 6.0, 7.0, 8.0]);
324        let result = v1 * v2;
325        let expected = 70.0;
326        assert_eq!(result, expected);
327    }
328
329    #[test]
330    fn vector4_norm_test() {
331        let v1 = V4::new([1.0, 2.0, 3.0, 4.0]);
332        let result = v1.norm2();
333        let expected = 5.477225575051661;
334        assert_eq!(result, expected);
335    }
336
337    #[test]
338    fn mul_const_rhs() {
339        let v = V4::new([1.0, 2.0, 3.0, 4.0]);
340        let result = 2.0 * v;
341        let expected = V4::new([2.0, 4.0, 6.0, 8.0]);
342        assert_eq!(
343            &result[..],
344            &expected[..],
345            "\nExpected\n{:?}\nfound\n{:?}",
346            &result[..],
347            &expected[..]
348        );
349    }
350
351    #[test]
352    fn mul_const() {
353        let v = V4::new([1.0, 2.0, 3.0, 4.0]);
354        let result = v * 2.0;
355        let expected = V4::new([2.0, 4.0, 6.0, 8.0]);
356        assert_eq!(
357            &result[..],
358            &expected[..],
359            "\nExpected\n{:?}\nfound\n{:?}",
360            &result[..],
361            &expected[..]
362        );
363    }
364
365    #[test]
366    fn vector4_mul_matrix4x4_test() {
367        let m = M44::new([
368            [1.0, 2.0, 3.0, 4.0],
369            [5.0, 6.0, 7.0, 8.0],
370            [9.0, 10.0, 11.0, 12.0],
371            [13.0, 14.0, 15.0, 16.0],
372        ]);
373
374        let v1 = V4::new([1.0, 2.0, 3.0, 4.0]);
375        let result = v1 * m;
376        let expected = V4::new([90.0, 100.0, 110.0, 120.0]);
377        assert_eq!(
378            &result[..],
379            &expected[..],
380            "\nExpected\n{:?}\nfound\n{:?}",
381            &result[..],
382            &expected[..]
383        );
384    }
385
386    #[test]
387    fn normalize_test() {
388        let result = V4::new([1.0, 1.0, 1.0, 1.0]).normalize().unwrap();
389        let expected = V4::new([0.5, 0.5, 0.5, 0.5]);
390        assert_eq!(
391            &result[..],
392            &expected[..],
393            "\nExpected\n{:?}\nfound\n{:?}",
394            &result[..],
395            &expected[..]
396        );
397    }
398
399    #[test]
400    fn sub_assigment_test() {
401        let mut result = V4::new([1.0, 2.0, 3.0, 4.0]);
402        let v = V4::new([5.0, 6.0, 7.0, 8.0]);
403        let expected = V4::new([-4.0, -4.0, -4.0, -4.0]);
404        result -= v;
405        assert_eq!(
406            &result[..],
407            &expected[..],
408            "\nExpected\n{:?}\nfound\n{:?}",
409            &result[..],
410            &expected[..]
411        );
412    }
413
414    #[test]
415    fn add_assigment_test() {
416        let mut result = V4::new_from(1.0, 2.0, 3.0, 4.0);
417        let v = V4::new_from(5.0, 6.0, 7.0, 8.0);
418        let expected = V4::new_from(6.0, 8.0, 10.0, 12.0);
419        result += v;
420        assert_eq!(
421            &result[..],
422            &expected[..],
423            "\nExpected\n{:?}\nfound\n{:?}",
424            &result[..],
425            &expected[..]
426        );
427    }
428
429    #[test]
430    fn norm_inf_test() {
431        let v = V4::new_from(10, 100, -9, 0);
432        let result = v.norm_inf();
433        let expected = 100;
434        assert_eq!(result, expected);
435    }
436
437    #[test]
438    fn norm_l_test() {
439        let v = V4::new_from(1, -1, 1, -1);
440        let result = v.norm_l();
441        let expected = 4;
442        assert_eq!(result, expected);
443    }
444
445    #[test]
446    fn product_lhs_f32_test() {
447        let v = V4::new_from(1.0f32, 2.0, 3.0, 4.0);
448        let result = 2.0f32 * v;
449        let expected = V4::new_from(2.0, 4.0, 6.0, 8.0);
450        assert_eq!(result, expected);
451    }
452}