dodgy_3d 0.5.1

An implementation of ORCA, a local collision avoidance algorithm for 3D.
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

use glam::Vec3;

use crate::{
  simulator::{AgentParameters, Simulator, SimulatorMargin},
  Agent,
};

macro_rules! assert_vec_near {
  ($left: expr, $right: expr, $eps: expr) => {{
    let left = $left;
    let right = $right;
    let eps = $eps;
    assert!(
      left.distance(right) < eps,
      "left: {}, right: {}, epsilon: {}",
      left,
      right,
      eps
    );
  }};
}

#[test]
fn two_agent_simulation() {
  let mut simulator = Simulator::new();

  simulator.add_agent(
    Agent {
      // Perturb one agent slightly so they can find a reasonable solution.
      position: Vec3::new(10.0, 0.0, 0.01),
      velocity: Vec3::ZERO,
      radius: 1.0,
      avoidance_responsibility: 1.0,
    },
    AgentParameters {
      goal_point: Vec3::new(-10.0, 0.0, 0.0),
      max_speed: 2.0,
      obstacle_margin: SimulatorMargin::AgentRadius,
      time_horizon: 2.0,
      obstacle_time_horizon: 1.0,
    },
  );

  simulator.add_agent(
    Agent {
      position: Vec3::new(-10.0, 0.0, 0.0),
      velocity: Vec3::ZERO,
      radius: 1.0,
      avoidance_responsibility: 1.0,
    },
    AgentParameters {
      goal_point: Vec3::new(10.0, 0.0, 0.0),
      max_speed: 2.0,
      obstacle_margin: SimulatorMargin::AgentRadius,
      time_horizon: 2.0,
      obstacle_time_horizon: 1.0,
    },
  );

  // Test accessors.
  assert_eq!(simulator.get_agent_count(), 2);

  assert_eq!(
    simulator.get_agent_parameters(0).goal_point,
    Vec3::new(-10.0, 0.0, 0.0)
  );
  assert_eq!(
    simulator.get_agent_parameters(1).goal_point,
    Vec3::new(10.0, 0.0, 0.0)
  );

  simulator.get_agent_mut(1).position = Vec3::new(-5.0, 0.0, 0.0);
  simulator.get_agent_parameters_mut(1).time_horizon = 3.0;

  for _ in 0..200 {
    simulator.step(0.1);
  }

  assert_vec_near!(
    simulator.get_agent(0).position,
    Vec3::new(-10.0, 0.0, 0.0),
    1e-4
  );
  assert_vec_near!(
    simulator.get_agent(1).position,
    Vec3::new(10.0, 0.0, 0.0),
    1e-4
  );

  simulator.remove_agent(0);

  // Agent 1 should now have "moved" into Agent 0.
  assert_eq!(simulator.get_agent_count(), 1);
  assert_vec_near!(
    simulator.get_agent(0).position,
    Vec3::new(10.0, 0.0, 0.0),
    1e-4
  );
}