path-planning 0.1.0

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

mod euclidean;
mod linear;

pub use euclidean::*;
pub use linear::*;

////////////////////////////////////////////////////////////////////////////////

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

/// Genericly stored trajectory, implemented by [`FullTraj`] and [`FullTrajRef`]
///
/// Generic parameter S details what kind of nalgebra storage is being used
pub trait FullTrajectory<X, T, S1, S2, const N: usize>
where
  T: Trajectory<X, N>,
  S1: Storage<X, Const<N>>,
  S2: Storage<X, Const<N>>,
{
  /// The starting coordinate of the trajectory
  fn start(&self) -> &Vector<X, Const<N>, S1>;

  /// The ending coordinate of the trajectory
  fn end(&self) -> &Vector<X, Const<N>, S2>;

  /// The extra [`Trajectory`]
  fn trajectory(&self) -> &T;

  /// The cost to move along the trajectory
  fn cost(&self) -> X;

  /// Interpolate a coordinate between the start and end points
  ///
  /// Start point is assumed to be at t = 0.0 and end point at t = 1.0
  fn interpolate(&self, t: X) -> SVector<X, N>;
}

/// A trajectory with array storage
pub type FullTrajOwned<X, T, const N: usize> =
  FullTraj<X, T, ArrayStorage<X, N, 1>, ArrayStorage<X, N, 1>, N>;

/// An owned trajectory
#[derive(Debug, Clone, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Serialize, T: Serialize, S1: Serialize, S2: Serialize",
  deserialize = "X: DeserializeOwned, T: DeserializeOwned, S1: DeserializeOwned, S2: DeserializeOwned"
))]
pub struct FullTraj<X, T, S1, S2, const N: usize>
where
  X: Scalar,
  T: Trajectory<X, N>,
  S1: Storage<X, Const<N>>,
  S2: Storage<X, Const<N>>,
{
  start: Vector<X, Const<N>, S1>,
  end: Vector<X, Const<N>, S2>,
  trajectory_data: T,
}

impl<X, T, S1, S2, const N: usize> FullTraj<X, T, S1, S2, N>
where
  X: Scalar,
  T: Trajectory<X, N>,
  S1: Storage<X, Const<N>>,
  S2: Storage<X, Const<N>>,
{
  pub fn new(
    start: Vector<X, Const<N>, S1>,
    end: Vector<X, Const<N>, S2>,
    trajectory_data: T,
  ) -> Self {
    Self {
      start,
      end,
      trajectory_data,
    }
  }

  pub fn get_ref<'a>(&'a self) -> FullTrajRef<'a, X, T, S1, S2, N> {
    FullTrajRef::new(&self.start, &self.end, &self.trajectory_data)
  }

  pub fn to_trajectory(self) -> T {
    self.trajectory_data
  }
}

impl<X, T, S1, S2, const N: usize> FullTrajectory<X, T, S1, S2, N>
  for FullTraj<X, T, S1, S2, N>
where
  X: Scalar,
  T: Trajectory<X, N>,
  S1: Storage<X, Const<N>>,
  S2: Storage<X, Const<N>>,
{
  fn start(&self) -> &Vector<X, Const<N>, S1> {
    &self.start
  }

  fn end(&self) -> &Vector<X, Const<N>, S2> {
    &self.end
  }

  fn trajectory(&self) -> &T {
    &self.trajectory_data
  }

  fn cost(&self) -> X {
    match self.trajectory_data.cost() {
      Some(cost) => cost,
      None => self.trajectory_data.calc_cost(&self.start, &self.end),
    }
  }

  fn interpolate(&self, t: X) -> SVector<X, N> {
    self.trajectory_data.interpolate(&self.start, &self.end, t)
  }
}

/// A reference trajectory to an trajectory with array storage
pub type FullTrajRefOwned<'a, X, T, const N: usize> =
  FullTrajRef<'a, X, T, ArrayStorage<X, N, 1>, ArrayStorage<X, N, 1>, N>;

/// A
#[derive(Debug, Clone)]
pub struct FullTrajRef<'a, X, T, S1, S2, const N: usize>
where
  T: Trajectory<X, N>,
  S1: Storage<X, Const<N>>,
  S2: Storage<X, Const<N>>,
{
  start: &'a Vector<X, Const<N>, S1>,
  end: &'a Vector<X, Const<N>, S2>,
  trajectory_data: &'a T,
}

impl<'a, X, T, S1, S2, const N: usize> FullTrajRef<'a, X, T, S1, S2, N>
where
  X: Scalar,
  T: Trajectory<X, N>,
  S1: Storage<X, Const<N>>,
  S2: Storage<X, Const<N>>,
{
  pub fn new(
    start: &'a Vector<X, Const<N>, S1>,
    end: &'a Vector<X, Const<N>, S2>,
    trajectory_data: &'a T,
  ) -> Self {
    Self {
      start,
      end,
      trajectory_data,
    }
  }
}

impl<'a, X, T, S1, S2, const N: usize> FullTrajRef<'a, X, T, S1, S2, N>
where
  X: Scalar,
  T: Trajectory<X, N> + Clone,
  S1: Storage<X, Const<N>> + Clone,
  S2: Storage<X, Const<N>> + Clone,
{
  pub fn get_owned(&self) -> FullTraj<X, T, S1, S2, N> {
    FullTraj::new(
      self.start.clone(),
      self.end.clone(),
      self.trajectory_data.clone(),
    )
  }
}

impl<'a, X, T, S1, S2, const N: usize> FullTrajectory<X, T, S1, S2, N>
  for FullTrajRef<'a, X, T, S1, S2, N>
where
  T: Trajectory<X, N>,
  S1: Storage<X, Const<N>>,
  S2: Storage<X, Const<N>>,
{
  fn start(&self) -> &Vector<X, Const<N>, S1> {
    self.start
  }

  fn end(&self) -> &Vector<X, Const<N>, S2> {
    self.end
  }

  fn trajectory(&self) -> &T {
    self.trajectory_data
  }

  fn cost(&self) -> X {
    match self.trajectory_data.cost() {
      Some(cost) => cost,
      None => self.trajectory_data.calc_cost(self.start, self.end),
    }
  }

  fn interpolate(&self, t: X) -> SVector<X, N> {
    self.trajectory_data.interpolate(self.start, self.end, t)
  }
}

/// The data saved on the edge in the graph need to support the cost and interpolate functions
pub trait Trajectory<X, const N: usize>: Clone {
  /// The cached cost to move along the trajectory
  ///
  /// Doesn't required endpoints
  fn cost(&self) -> Option<X>;

  /// Calculate the cost from start to end across this trajectory
  ///
  /// Start and end points must the same as the ones used when the original trajectory was created
  /// to get the sensible output. Implementations are free to panic if start and end points differ.
  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>>;

  /// Interpolate a coordinate between the start and end points
  ///
  /// Start point is assumed to be at t = 0.0 and end point at t = 1.0
  ///
  /// Start and end points must the same as the ones used when the original trajectory was created
  /// to get the sensible output. Implementations are free to panic if start and end points differ.
  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>>;
}