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
64
65
66
67
68
69
70
71
72
73
74

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

use std::marker::PhantomData;

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

use crate::trajectories::Trajectory;

/// A trajectory where that represents a straight line between two points in euclidean space
///
/// Stores just a cached value of the cost between two points that are stored elsewhere
#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize, NORM: Serialize",
  deserialize = "X: DeserializeOwned, NORM: DeserializeOwned"
))]
pub struct LinearTrajectory<X, NORM, const N: usize>
where
  X: SimdRealField,
  NORM: Norm<X>,
{
  norm: NORM,
  phantom_data: PhantomData<X>,
}

impl<X, NORM, const N: usize> LinearTrajectory<X, NORM, N>
where
  X: SimdRealField,
  NORM: Norm<X>,
{
  pub fn new(norm: NORM) -> Self {
    Self {
      norm,
      phantom_data: PhantomData,
    }
  }
}

impl<X, NORM, const N: usize> Trajectory<X, N> for LinearTrajectory<X, NORM, N>
where
  X: SimdRealField,
  NORM: Norm<X> + Clone,
{
  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>>,
  {
    self.norm.metric_distance(start, 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)
  }
}