pi_orca 0.5.1

A* Path Finding Algorithm
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146

use pi_orca::{rvos_imulator::RVOSimulator, vector2::Vector2};
// use rand::Rng;

// const temp: f32 = 200.;
// const num: usize = 100;
// const RAND_MAX: f32 = 32767.;
// const RVO_TWO_PI: f32 = 6.28318530717958647692;
fn main() {
    let mut sim = RVOSimulator::default();
    let mut goals = vec![];
    let mut agents = vec![];

    setup_scenario(&mut sim, &mut goals, &mut agents);
    let begin = std::time::Instant::now();
    let mut n = 0;
    loop {
        if n > 2000 {
            break;
        }
        update_visualization(&mut sim, &agents);

        set_preferred_velocities(&mut sim, &goals, &agents);
        sim.do_step();

        if reached_goal(&mut sim, &goals, &agents) {
            // break;
        }

        // std::thread::sleep(std::time::Duration::from_millis(16));
        n += 1;
    }
    println!("time = {:?}", begin.elapsed());
}

pub fn setup_scenario(sim: &mut RVOSimulator, goals: &mut Vec<Vector2>, agents: &mut Vec<f64>) {
    /* Specify the global time step of the simulation. */
    sim.set_time_step(0.25);

    /* Specify the default parameters for agents that are subsequently added. */
    sim.set_agent_defaults(15.0, 100, 5.0, 5.0, 2.0, 2.0, &Vector2::default());

    // let mut id = 0;
    /*
     * Add agents, specifying their start position, and store their goals on the
     * opposite side of the environment.
     */
    for i in 0..5 {
        for j in 0..5 {
            let pos = Vector2::new(55.0 + i as f32 * 10.0, 55.0 + j as f32 * 10.0);
            let id = sim.add_agent(&pos, 2.);
            agents.push(id);
            goals.push(-pos);

            let pos = Vector2::new(-55.0 - i as f32 * 10.0, 55.0 + j as f32 * 10.0);
            let id = sim.add_agent(&pos, 2.);
            agents.push(id);
            goals.push(-pos);

            let pos = Vector2::new(55.0 + i as f32 * 10.0, -55.0 - j as f32 * 10.0);
            let id = sim.add_agent(&pos, 2.);
            agents.push(id);
            goals.push(-pos);

            let pos = Vector2::new(-55.0 - i as f32 * 10.0, -55.0 - j as f32 * 10.0);
            let id = sim.add_agent(&pos, 2.);
            agents.push(id);
            goals.push(-pos);
        }
    }

    // println!("sim.remove_agent(id) r: {}", sim.remove_agent(id));
    /*
     * Add (polygonal) obstacles, specifying their vertices in counterclockwise
     * order.
     */

    let obstacle1 = vec![
        Vector2::new(-10.0, 40.0),
        Vector2::new(-40.0, 40.0),
        Vector2::new(-40.0, 10.0),
        Vector2::new(-10.0, 10.0),
    ];

    let obstacle2 = vec![
        Vector2::new(10.0, 40.0),
        Vector2::new(10.0, 10.0),
        Vector2::new(40.0, 10.0),
        Vector2::new(40.0, 40.0),
    ];

    let obstacle3 = vec![
        Vector2::new(10.0, -40.0),
        Vector2::new(40.0, -40.0),
        Vector2::new(40.0, -10.0),
        Vector2::new(10.0, -10.0),
    ];

    let obstacle4 = vec![
        Vector2::new(-10.0, -40.0),
        Vector2::new(-10.0, -10.0),
        Vector2::new(-40.0, -10.0),
        Vector2::new(-40.0, -40.0),
    ];

    sim.add_obstacle(obstacle1);
    sim.add_obstacle(obstacle2);
    sim.add_obstacle(obstacle3);
    sim.add_obstacle(obstacle4);
}

pub fn update_visualization(sim: &mut RVOSimulator, agents: &Vec<f64>) {
    println!("global_time : {}", sim.get_global_time());

    for id in agents {
        print!(" {:?}", sim.get_agent_position(*id));
    }
    println!("");
}

pub fn set_preferred_velocities(sim: &mut RVOSimulator, goals: &Vec<Vector2>, agents: &Vec<f64>) {
    /*
     * Set the preferred velocity to be a vector of unit magnitude (speed) in the
     * direction of the goal.
     */

    for (id, goal) in agents.iter().zip(goals.iter()) {
        let mut goal_vector = *goal - sim.get_agent_position(*id).unwrap();

        if Vector2::abs_sq(&goal_vector) > 1.0 {
            goal_vector = Vector2::normalize(&goal_vector);
        }
        // println!("goalVector : {:?}", goalVector);
        sim.set_agent_pref_velocity(*id, &goal_vector);
    }
}

pub fn reached_goal(sim: &mut RVOSimulator, goals: &Vec<Vector2>, agents: &Vec<f64>) -> bool {
    /* Check if all agents have reached their goals. */
    for (goal, id) in goals.iter().zip(agents) {
        if Vector2::abs_sq(&(sim.get_agent_position(*id).unwrap() - goal)) > 20.0 * 20.0 {
            return false;
        }
    }

    return true;
}