1use 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#[derive(Copy, Clone, Debug, PartialEq)]
45pub struct V4<T>([T; 4]);
46
47impl<T> V4<T> {
48 pub const fn new(input: [T; 4]) -> Self {
50 Self(input)
51 }
52
53 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 pub fn zeros() -> V4<T> {
62 <V4<T> as Zero>::zero()
63 }
64
65 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
80impl<T: Num + Copy + core::cmp::PartialOrd> V4<T> {
82 pub fn norm_inf(&self) -> T {
83 norm_inf(&**self)
84 }
85}
86
87impl<T: Num + Copy + Signed + core::iter::Sum> V4<T> {
89 pub fn norm_l(&self) -> T {
90 norm_l(&**self)
91 }
92}
93
94impl<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 #[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 pub fn normalize(&mut self) -> Result<Self, VectorErrors> {
113 let n = self.norm2();
114 if n != T::zero() {
115 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
127impl<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
137impl<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
147impl<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
158impl 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
168impl<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
183impl<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
193impl<T: Num + Copy> SubAssign for V4<T> {
195 fn sub_assign(&mut self, other: Self) {
196 *self = *self - other
197 }
198}
199
200impl<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
210impl<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
251impl<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#[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}