path-planning 0.1.0

Path Planning Algorithms implemented in Rust.
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

/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */

use std::marker::PhantomData;

use nalgebra::storage::Storage;
use nalgebra::{Const, SVector, SimdRealField, Vector};
use serde::{de::DeserializeOwned, Deserialize, Serialize};

use super::Trajectory;

/// A trajectory where that represents a straight line between two points in euclidean space
///
/// Has zero size
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize",
  deserialize = "X: DeserializeOwned"
))]
pub struct EuclideanTrajectory<X, const N: usize> {
  phantom_data: PhantomData<X>,
}

impl<X, const N: usize> EuclideanTrajectory<X, N> {
  pub fn new() -> Self {
    Self {
      phantom_data: PhantomData,
    }
  }
}

impl<X, const N: usize> Trajectory<X, N> for EuclideanTrajectory<X, N>
where
  X: SimdRealField,
{
  fn cost(&self) -> Option<X> {
    None
  }

  fn calc_cost<S1, S2>(
    &self,
    start: &Vector<X, Const<N>, S1>,
    end: &Vector<X, Const<N>, S2>,
  ) -> X
  where
    S1: Storage<X, Const<N>>,
    S2: Storage<X, Const<N>>,
  {
    start.metric_distance(end)
  }

  fn interpolate<S1, S2>(
    &self,
    start: &Vector<X, Const<N>, S1>,
    end: &Vector<X, Const<N>, S2>,
    t: X,
  ) -> SVector<X, N>
  where
    S1: Storage<X, Const<N>>,
    S2: Storage<X, Const<N>>,
  {
    start.lerp(end, t)
  }
}