path-planning 0.1.0

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

use nalgebra::{SVector, Scalar};
use rand::distributions::uniform::{SampleUniform, Uniform};
use rand::distributions::Distribution;
use rand::Rng;

use crate::util::bounds::Bounds;

/// A uniform distribution over type X in dimension N
pub struct LinearCoordinates<X: SampleUniform, const N: usize>(Vec<Uniform<X>>);

impl<X: SampleUniform, const N: usize> LinearCoordinates<X, N> {
  pub fn new(mins: SVector<X, N>, maxs: SVector<X, N>) -> Self {
    let mut dists = Vec::with_capacity(N);

    for (a, b) in mins.iter().zip(maxs.iter()) {
      dists.push(Uniform::new(a, b));
    }
    Self(dists)
  }
}

impl<X: SampleUniform, const N: usize> From<Bounds<X, N>>
  for LinearCoordinates<X, N>
{
  fn from(bounds: Bounds<X, N>) -> Self {
    Self::new(bounds.mins, bounds.maxs)
  }
}

impl<X: Scalar + SampleUniform, const N: usize> Distribution<SVector<X, N>>
  for LinearCoordinates<X, N>
{
  fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> SVector<X, N> {
    SVector::<X, N>::from_fn(|i, _| self.0[i].sample(rng))
  }
}

#[cfg(test)]
mod tests {

  use nalgebra::vector;
  use rand::SeedableRng;
  use rand_pcg::Pcg32;

  use super::*;

  const SEED: u64 = 0xe580e2e93fd6b040;

  #[test]
  fn test_sample_f32() {
    let mins: [f32; 4] = [0.0, 0.0, 0.0, 0.0];
    let maxs: [f32; 4] = [1.0, 1.0, 2.0, 2.0];
    let dist = LinearCoordinates::new(mins.into(), maxs.into());
    let mut rng = Pcg32::seed_from_u64(SEED);
    assert_eq!(
      dist.sample(&mut rng),
      vector![0.9118717, 0.16728437, 1.772038, 0.30819774]
    );
  }

  #[test]
  fn test_sample_f64() {
    let mins: [f64; 3] = [0.0, 0.0, -1.0];
    let maxs: [f64; 3] = [1.0, 2.0, 0.0];
    let dist = LinearCoordinates::new(mins.into(), maxs.into());
    let mut rng = Pcg32::seed_from_u64(SEED);
    assert_eq!(
      dist.sample(&mut rng),
      vector![0.16728437317346012, 0.3081977641610516, -0.6118928084775344]
    );
  }
}