path-planning 0.1.0

Path Planning Algorithms implemented in Rust.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114

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

use nalgebra::RealField;

/// The volume of a d-dimensional unit ball
///
/// Implemented using https://en.wikipedia.org/wiki/Volume_of_an_n-ball#Recurrence_relations
pub fn unit_d_ball_vol<X: RealField + Copy>(d: usize) -> X {
  let one = X::one();
  let two = one + one;
  let radius = one;

  let mut volumes = vec![one, two * radius];

  while volumes.len() <= d {
    let n = volumes.len();
    let n_ = X::from_subset(&(n as f64));
    let next = (two * X::pi() / n_) * radius.powi(2) * volumes[n - 2];
    volumes.push(next);
  }

  volumes[d]
}

/// The angle in the Euclidean plane, given in radians, between the positive x
/// axis and the ray to the point (x, y) != (0, 0).
///
/// Returns None when undefined (i.e. at (x, y) == (0, 0))
///
/// See https://en.wikipedia.org/wiki/Atan2
pub fn atan2<X: RealField + Copy>(y: X, x: X) -> Option<X> {
  match (x, y) {
    // x > 0
    (x, y) if x > X::zero() => Some(X::atan(y / x)),
    // x < 0 && y >= 0
    (x, y) if x < X::zero() && y >= X::zero() => Some(X::atan(y / x) + X::pi()),
    // x < 0 && y < 0
    (x, y) if x < X::zero() && y < X::zero() => Some(X::atan(y / x) - X::pi()),
    // x == 0 && y > 0
    (x, y) if x == X::zero() && y > X::zero() => Some(X::frac_pi_2()),
    // x == 0 && y < 0
    (x, y) if x == X::zero() && y < X::zero() => Some(-X::frac_pi_2()),
    // x == 0 && y == 0
    _ => None,
  }
}

#[cfg(test)]
mod tests {

  use std::f32::consts as f32c;
  use std::f64::consts as f64c;

  use super::*;

  fn rel_eq<X: RealField + Copy>(a: X, b: X) -> bool {
    let abs_difference = (a - b).abs();
    abs_difference < X::from_subset(&10.0).powi(-5)
  }

  #[test]
  fn test_unit_d_ball_vol_f32() {
    assert!(rel_eq(unit_d_ball_vol::<f32>(1), 2.));
    assert!(rel_eq(unit_d_ball_vol::<f32>(2), f32c::PI));
    assert!(rel_eq(unit_d_ball_vol::<f32>(3), f32c::PI * (4. / 3.)));
    assert!(rel_eq(
      unit_d_ball_vol::<f32>(4),
      f32c::PI.powi(2) * (1. / 2.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f32>(5),
      f32c::PI.powi(2) * (8. / 15.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f32>(6),
      f32c::PI.powi(3) * (1. / 6.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f32>(7),
      f32c::PI.powi(3) * (16. / 105.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f32>(8),
      f32c::PI.powi(4) * (1. / 24.)
    ));
  }

  #[test]
  fn test_unit_d_ball_vol_f64() {
    assert!(rel_eq(unit_d_ball_vol::<f64>(1), 2.));
    assert!(rel_eq(unit_d_ball_vol::<f64>(2), f64c::PI));
    assert!(rel_eq(unit_d_ball_vol::<f64>(3), f64c::PI * (4. / 3.)));
    assert!(rel_eq(
      unit_d_ball_vol::<f64>(4),
      f64c::PI.powi(2) * (1. / 2.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f64>(5),
      f64c::PI.powi(2) * (8. / 15.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f64>(6),
      f64c::PI.powi(3) * (1. / 6.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f64>(7),
      f64c::PI.powi(3) * (16. / 105.)
    ));
    assert!(rel_eq(
      unit_d_ball_vol::<f64>(8),
      f64c::PI.powi(4) * (1. / 24.)
    ));
  }
}