motion_planning/
vec.rs

1use core::ops::{Add, Mul, Neg};
2use core::{convert::From, fmt};
3
4#[cfg(test)]
5use super::assert_f64_roughly_eq;
6
7#[allow(clippy::module_name_repetitions)]
8#[derive(Clone, Copy, Debug, PartialEq)]
9pub struct Vec3d<V>(pub V, pub V, pub V);
10
11impl<V> fmt::Display for Vec3d<V>
12where
13	V: fmt::Display,
14{
15	fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
16		write!(f, "({},{},{})", self.0, self.1, self.2)
17	}
18}
19
20impl<V> Neg for Vec3d<V>
21where
22	V: From<V> + Neg<Output = V>,
23{
24	type Output = Self;
25
26	fn neg(self) -> Self::Output {
27		Vec3d(-self.0, -self.1, -self.2)
28	}
29}
30
31impl<T, V> Mul<T> for Vec3d<V>
32where
33	T: Copy + From<V> + Mul<T, Output = T>,
34{
35	type Output = Vec3d<T>;
36
37	fn mul(self, scalar: T) -> Self::Output {
38		Vec3d(
39			scalar * self.0.into(),
40			scalar * self.1.into(),
41			scalar * self.2.into(),
42		)
43	}
44}
45
46impl<T: Add<T>> Add<Vec3d<T>> for Vec3d<T>
47where
48	T: From<<T as Add>::Output>,
49{
50	type Output = Self;
51
52	fn add(self, other: Self) -> Self::Output {
53		Vec3d(
54			(self.0 + other.0).into(),
55			(self.1 + other.1).into(),
56			(self.2 + other.2).into(),
57		)
58	}
59}
60
61impl<V> Vec3d<V>
62where
63	V: Add<V, Output = V> + Copy + Mul<V, Output = V>,
64{
65	pub fn dot(&self, other: &Vec3d<V>) -> V {
66		(self.0 * other.0) + (self.1 * other.1) + (self.2 * other.2)
67	}
68}
69
70#[test]
71fn vec_addition() {
72	let a: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);
73	let b: Vec3d<f32> = Vec3d(5.0, 4.0, 3.0);
74
75	assert_eq!(a + b, Vec3d(6., 6., 6.));
76}
77
78#[test]
79fn vec_display() {
80	let v: Vec3d<f32> = Vec3d(0., 1.25, 4.);
81
82	assert_eq!(format!("{}", v), "(0,1.25,4)");
83}
84
85#[test]
86fn vec_negation() {
87	let vec: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);
88
89	assert_eq!(-vec, Vec3d(-1.0, -2.0, -3.0));
90}
91
92#[test]
93fn vec_scalar_multiplication_f64() {
94	let a: Vec3d<f64> = Vec3d(1.0, 2.0, 3.0);
95	let b: f64 = 2.0;
96
97	assert_eq!(a * b, Vec3d(2.0f64, 4.0f64, 6.0f64));
98}
99
100#[test]
101fn vec_scalar_multiplication_f32() {
102	let a: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);
103	let b: f32 = 2.0;
104
105	assert_eq!(a * b, Vec3d(2.0f32, 4.0f32, 6.0f32));
106}
107
108#[test]
109fn vec_scalar_multiplication_vf32_sf64() {
110	let a: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);
111	let b: f64 = 2.0;
112
113	assert_eq!(a * b, Vec3d(2.0f64, 4.0f64, 6.0f64));
114}
115
116#[test]
117fn vec_dot() {
118	let a: Vec3d<f64> = Vec3d(1., 2., 3.);
119	let b: Vec3d<f64> = Vec3d(5., 4., 3.);
120
121	assert_f64_roughly_eq!(a.dot(&b), 22.0_f64);
122}
motion_planning/vec.rs

motion_planning/
vec.rs
