path_planning/trajectories/
euclidean.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3use std::marker::PhantomData;
4
5use nalgebra::storage::Storage;
6use nalgebra::{Const, SVector, SimdRealField, Vector};
7use serde::{de::DeserializeOwned, Deserialize, Serialize};
8
9use super::Trajectory;
10
11/// A trajectory where that represents a straight line between two points in euclidean space
12///
13/// Has zero size
14#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
15#[serde(bound(
16  serialize = "X: Serialize",
17  deserialize = "X: DeserializeOwned"
18))]
19pub struct EuclideanTrajectory<X, const N: usize> {
20  phantom_data: PhantomData<X>,
21}
22
23impl<X, const N: usize> EuclideanTrajectory<X, N> {
24  pub fn new() -> Self {
25    Self {
26      phantom_data: PhantomData,
27    }
28  }
29}
30
31impl<X, const N: usize> Trajectory<X, N> for EuclideanTrajectory<X, N>
32where
33  X: SimdRealField,
34{
35  fn cost(&self) -> Option<X> {
36    None
37  }
38
39  fn calc_cost<S1, S2>(
40    &self,
41    start: &Vector<X, Const<N>, S1>,
42    end: &Vector<X, Const<N>, S2>,
43  ) -> X
44  where
45    S1: Storage<X, Const<N>>,
46    S2: Storage<X, Const<N>>,
47  {
48    start.metric_distance(end)
49  }
50
51  fn interpolate<S1, S2>(
52    &self,
53    start: &Vector<X, Const<N>, S1>,
54    end: &Vector<X, Const<N>, S2>,
55    t: X,
56  ) -> SVector<X, N>
57  where
58    S1: Storage<X, Const<N>>,
59    S2: Storage<X, Const<N>>,
60  {
61    start.lerp(end, t)
62  }
63}
path_planning/trajectories/euclidean.rs

path_planning/trajectories/
euclidean.rs
