path_planning/rng/
leader_follower.rs

1/* Copyright (C) 2020 Dylan Staatz - All Rights Reserved. */
2
3use nalgebra::SVector;
4use rand::distributions::uniform::{SampleUniform, Uniform};
5use rand::distributions::Distribution;
6use rand::Rng;
7
8use crate::scalar::Scalar;
9
10/// A uniform distribution over type X in dimension N
11pub struct LeaderFollowerCoordinates<
12  X: SampleUniform,
13  const D: usize,
14  const N: usize,
15> {
16  mins: SVector<X, D>,
17  maxes: SVector<X, D>,
18  leader_samplers: Vec<Uniform<X>>,
19  follower_samplers: Vec<Uniform<X>>,
20  max_radius_squared: X,
21}
22
23impl<X: Scalar + SampleUniform, const D: usize, const N: usize>
24  LeaderFollowerCoordinates<X, D, N>
25{
26  pub fn new(mins: SVector<X, D>, maxes: SVector<X, D>, max_radius: X) -> Self {
27    assert_eq!(D * 2, N);
28
29    let mut leader_samplers = Vec::with_capacity(D);
30    for (a, b) in mins.iter().zip(maxes.iter()) {
31      assert!(a < b);
32      leader_samplers.push(Uniform::new(a, b));
33    }
34
35    let mut follower_samplers = Vec::with_capacity(D);
36    for _ in 0..D {
37      follower_samplers.push(Uniform::new(-max_radius, max_radius));
38    }
39
40    Self {
41      mins,
42      maxes,
43      leader_samplers,
44      follower_samplers,
45      max_radius_squared: max_radius * max_radius,
46    }
47  }
48}
49
50impl<X: Scalar + SampleUniform, const D: usize, const N: usize>
51  Distribution<SVector<X, N>> for LeaderFollowerCoordinates<X, D, N>
52{
53  fn sample<R: Rng + ?Sized>(&self, rng: &mut R) -> SVector<X, N> {
54    let leader =
55      SVector::<X, D>::from_fn(|i, _| self.leader_samplers[i].sample(rng));
56
57    loop {
58      // Sample a random point
59      let follower_offset =
60        SVector::<X, D>::from_fn(|i, _| self.follower_samplers[i].sample(rng));
61
62      // Check location is within bounds of space
63      let follower = &leader + &follower_offset;
64      if !self
65        .mins
66        .iter()
67        .zip(self.maxes.iter())
68        .zip(follower.iter())
69        .all(|((min, max), val)| min < val && val < max)
70      {
71        continue;
72      }
73
74      // Check that offset distance is less than max_radius
75      if !(follower_offset.norm_squared() < self.max_radius_squared) {
76        continue;
77      }
78
79      log::debug!("leader: {:?}, follower: {:?}", leader, follower);
80      return SVector::<X, N>::from_iterator(
81        leader.iter().chain(follower.iter()).cloned(),
82      );
83    }
84  }
85}
path_planning/rng/leader_follower.rs

path_planning/rng/
leader_follower.rs
