motion-planning 0.1.0-alpha.5

Motion planning primitives.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122

use core::ops::{Add, Mul, Neg};
use core::{convert::From, fmt};

#[cfg(test)]
use super::assert_f64_roughly_eq;

#[allow(clippy::module_name_repetitions)]
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Vec3d<V>(pub V, pub V, pub V);

impl<V> fmt::Display for Vec3d<V>
where
	V: fmt::Display,
{
	fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
		write!(f, "({},{},{})", self.0, self.1, self.2)
	}
}

impl<V> Neg for Vec3d<V>
where
	V: From<V> + Neg<Output = V>,
{
	type Output = Self;

	fn neg(self) -> Self::Output {
		Vec3d(-self.0, -self.1, -self.2)
	}
}

impl<T, V> Mul<T> for Vec3d<V>
where
	T: Copy + From<V> + Mul<T, Output = T>,
{
	type Output = Vec3d<T>;

	fn mul(self, scalar: T) -> Self::Output {
		Vec3d(
			scalar * self.0.into(),
			scalar * self.1.into(),
			scalar * self.2.into(),
		)
	}
}

impl<T: Add<T>> Add<Vec3d<T>> for Vec3d<T>
where
	T: From<<T as Add>::Output>,
{
	type Output = Self;

	fn add(self, other: Self) -> Self::Output {
		Vec3d(
			(self.0 + other.0).into(),
			(self.1 + other.1).into(),
			(self.2 + other.2).into(),
		)
	}
}

impl<V> Vec3d<V>
where
	V: Add<V, Output = V> + Copy + Mul<V, Output = V>,
{
	pub fn dot(&self, other: &Vec3d<V>) -> V {
		(self.0 * other.0) + (self.1 * other.1) + (self.2 * other.2)
	}
}

#[test]
fn vec_addition() {
	let a: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);
	let b: Vec3d<f32> = Vec3d(5.0, 4.0, 3.0);

	assert_eq!(a + b, Vec3d(6., 6., 6.));
}

#[test]
fn vec_display() {
	let v: Vec3d<f32> = Vec3d(0., 1.25, 4.);

	assert_eq!(format!("{}", v), "(0,1.25,4)");
}

#[test]
fn vec_negation() {
	let vec: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);

	assert_eq!(-vec, Vec3d(-1.0, -2.0, -3.0));
}

#[test]
fn vec_scalar_multiplication_f64() {
	let a: Vec3d<f64> = Vec3d(1.0, 2.0, 3.0);
	let b: f64 = 2.0;

	assert_eq!(a * b, Vec3d(2.0f64, 4.0f64, 6.0f64));
}

#[test]
fn vec_scalar_multiplication_f32() {
	let a: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);
	let b: f32 = 2.0;

	assert_eq!(a * b, Vec3d(2.0f32, 4.0f32, 6.0f32));
}

#[test]
fn vec_scalar_multiplication_vf32_sf64() {
	let a: Vec3d<f32> = Vec3d(1.0, 2.0, 3.0);
	let b: f64 = 2.0;

	assert_eq!(a * b, Vec3d(2.0f64, 4.0f64, 6.0f64));
}

#[test]
fn vec_dot() {
	let a: Vec3d<f64> = Vec3d(1., 2., 3.);
	let b: Vec3d<f64> = Vec3d(5., 4., 3.);

	assert_f64_roughly_eq!(a.dot(&b), 22.0_f64);
}