path_planning/trajectories/
linear.rs

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

path_planning/trajectories/
linear.rs
