path-planning 0.1.0

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

use nalgebra::constraint::{SameNumberOfRows, ShapeConstraint};
use nalgebra::storage::Storage;
use nalgebra::{Const, Dim, SVector, Scalar, SimdBool, SimdRealField, Vector};
use serde::{de::DeserializeOwned, Deserialize, Serialize};

#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
#[serde(bound(
  serialize = "X: Scalar + Serialize",
  deserialize = "X: Scalar + DeserializeOwned"
))]
pub struct Bounds<X, const N: usize> {
  pub mins: SVector<X, N>,
  pub maxs: SVector<X, N>,
}

impl<X: Scalar + PartialEq, const N: usize> PartialEq for Bounds<X, N> {
  fn eq(&self, other: &Self) -> bool {
    self.mins == other.mins && self.maxs == other.maxs
  }
}

impl<X, const N: usize> Bounds<X, N> {
  pub fn new(mins: SVector<X, N>, maxs: SVector<X, N>) -> Self {
    Self { mins, maxs }
  }
}

impl<X: SimdRealField + Copy, const N: usize> Bounds<X, N> {
  pub fn splat(bounds: Bounds<X::Element, N>) -> Self
  where
    X::Element: Scalar,
  {
    Self {
      mins: bounds.mins.map(|el| X::splat(el)),
      maxs: bounds.maxs.map(|el| X::splat(el)),
    }
  }

  pub fn is_valid(&self) -> bool {
    self
      .mins
      .iter()
      .zip(self.maxs.iter())
      .map(|(&min, &max)| min.simd_lt(max))
      .reduce(|acc, next| acc & next)
      .unwrap()
      .all()
  }

  pub fn volume(&self) -> X {
    let temp = self.maxs - self.mins;
    temp.iter().fold(X::one(), |a, &b| a * b)
  }

  pub fn within<R, S>(&self, point: &Vector<X, R, S>) -> X::SimdBool
  where
    R: Dim,
    S: Storage<X, R>,
    ShapeConstraint: SameNumberOfRows<R, Const<N>>,
  {
    self
      .mins
      .iter()
      .zip(self.maxs.iter())
      .zip(point.iter())
      .map(|((&min, &max), &p)| min.simd_lt(p) & p.simd_lt(max))
      .reduce(|acc, next| acc & next)
      .unwrap()
  }
}