path_planning/util/
bounds.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3use nalgebra::constraint::{SameNumberOfRows, ShapeConstraint};
4use nalgebra::storage::Storage;
5use nalgebra::{Const, Dim, SVector, Scalar, SimdBool, SimdRealField, Vector};
6use serde::{de::DeserializeOwned, Deserialize, Serialize};
7
8#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
9#[serde(bound(
10  serialize = "X: Scalar + Serialize",
11  deserialize = "X: Scalar + DeserializeOwned"
12))]
13pub struct Bounds<X, const N: usize> {
14  pub mins: SVector<X, N>,
15  pub maxs: SVector<X, N>,
16}
17
18impl<X: Scalar + PartialEq, const N: usize> PartialEq for Bounds<X, N> {
19  fn eq(&self, other: &Self) -> bool {
20    self.mins == other.mins && self.maxs == other.maxs
21  }
22}
23
24impl<X, const N: usize> Bounds<X, N> {
25  pub fn new(mins: SVector<X, N>, maxs: SVector<X, N>) -> Self {
26    Self { mins, maxs }
27  }
28}
29
30impl<X: SimdRealField + Copy, const N: usize> Bounds<X, N> {
31  pub fn splat(bounds: Bounds<X::Element, N>) -> Self
32  where
33    X::Element: Scalar,
34  {
35    Self {
36      mins: bounds.mins.map(|el| X::splat(el)),
37      maxs: bounds.maxs.map(|el| X::splat(el)),
38    }
39  }
40
41  pub fn is_valid(&self) -> bool {
42    self
43      .mins
44      .iter()
45      .zip(self.maxs.iter())
46      .map(|(&min, &max)| min.simd_lt(max))
47      .reduce(|acc, next| acc & next)
48      .unwrap()
49      .all()
50  }
51
52  pub fn volume(&self) -> X {
53    let temp = self.maxs - self.mins;
54    temp.iter().fold(X::one(), |a, &b| a * b)
55  }
56
57  pub fn within<R, S>(&self, point: &Vector<X, R, S>) -> X::SimdBool
58  where
59    R: Dim,
60    S: Storage<X, R>,
61    ShapeConstraint: SameNumberOfRows<R, Const<N>>,
62  {
63    self
64      .mins
65      .iter()
66      .zip(self.maxs.iter())
67      .zip(point.iter())
68      .map(|((&min, &max), &p)| min.simd_lt(p) & p.simd_lt(max))
69      .reduce(|acc, next| acc & next)
70      .unwrap()
71  }
72}
path_planning/util/bounds.rs

path_planning/util/
bounds.rs
