|
15120
|
1 |
use crate::{
|
|
|
2 |
collision::{fppoint_round, CircleBounds, DetectedCollisions},
|
|
|
3 |
common::GearId,
|
|
|
4 |
};
|
|
|
5 |
|
|
|
6 |
use fpnum::FPPoint;
|
|
|
7 |
use integral_geometry::{GridIndex, Point, Size};
|
|
|
8 |
|
|
|
9 |
struct GridBin {
|
|
15261
|
10 |
static_refs: Vec<GearId>,
|
|
15120
|
11 |
static_entries: Vec<CircleBounds>,
|
|
15261
|
12 |
|
|
|
13 |
dynamic_refs: Vec<GearId>,
|
|
15120
|
14 |
dynamic_entries: Vec<CircleBounds>,
|
|
|
15 |
}
|
|
|
16 |
|
|
|
17 |
impl GridBin {
|
|
|
18 |
fn new() -> Self {
|
|
|
19 |
Self {
|
|
15261
|
20 |
static_refs: vec![],
|
|
15120
|
21 |
static_entries: vec![],
|
|
15261
|
22 |
dynamic_refs: vec![],
|
|
15120
|
23 |
dynamic_entries: vec![],
|
|
|
24 |
}
|
|
|
25 |
}
|
|
|
26 |
}
|
|
|
27 |
|
|
|
28 |
const GRID_BIN_SIZE: usize = 128;
|
|
|
29 |
|
|
|
30 |
pub struct Grid {
|
|
|
31 |
bins: Vec<GridBin>,
|
|
|
32 |
space_size: Size,
|
|
|
33 |
bins_count: Size,
|
|
|
34 |
index: GridIndex,
|
|
|
35 |
}
|
|
|
36 |
|
|
|
37 |
impl Grid {
|
|
|
38 |
pub fn new(size: Size) -> Self {
|
|
|
39 |
assert!(size.is_power_of_two());
|
|
|
40 |
let bins_count = Size::new(size.width / GRID_BIN_SIZE, size.height / GRID_BIN_SIZE);
|
|
|
41 |
|
|
|
42 |
Self {
|
|
|
43 |
bins: (0..bins_count.area()).map(|_| GridBin::new()).collect(),
|
|
|
44 |
space_size: size,
|
|
|
45 |
bins_count,
|
|
|
46 |
index: Size::square(GRID_BIN_SIZE).to_grid_index(),
|
|
|
47 |
}
|
|
|
48 |
}
|
|
|
49 |
|
|
|
50 |
fn bin_index(&self, position: &FPPoint) -> Point {
|
|
|
51 |
self.index.map(fppoint_round(position))
|
|
|
52 |
}
|
|
|
53 |
|
|
15261
|
54 |
fn get_bin(&mut self, index: Point) -> &mut GridBin {
|
|
|
55 |
&mut self.bins[index.x as usize * self.bins_count.width + index.y as usize]
|
|
|
56 |
}
|
|
|
57 |
|
|
15120
|
58 |
fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin {
|
|
15261
|
59 |
self.get_bin(self.bin_index(position))
|
|
15120
|
60 |
}
|
|
|
61 |
|
|
|
62 |
pub fn insert_static(&mut self, gear_id: GearId, bounds: &CircleBounds) {
|
|
15261
|
63 |
let bin = self.lookup_bin(&bounds.center);
|
|
|
64 |
bin.static_refs.push(gear_id);
|
|
|
65 |
bin.static_entries.push(*bounds)
|
|
15120
|
66 |
}
|
|
|
67 |
|
|
|
68 |
pub fn insert_dynamic(&mut self, gear_id: GearId, bounds: &CircleBounds) {
|
|
15261
|
69 |
let bin = self.lookup_bin(&bounds.center);
|
|
|
70 |
bin.dynamic_refs.push(gear_id);
|
|
|
71 |
bin.dynamic_entries.push(*bounds);
|
|
|
72 |
}
|
|
|
73 |
|
|
|
74 |
pub fn update_position(
|
|
|
75 |
&mut self,
|
|
|
76 |
gear_id: GearId,
|
|
|
77 |
old_position: &FPPoint,
|
|
|
78 |
new_position: &FPPoint,
|
|
|
79 |
) {
|
|
|
80 |
let old_bin_index = self.bin_index(old_position);
|
|
|
81 |
let new_bin_index = self.bin_index(new_position);
|
|
|
82 |
|
|
|
83 |
let old_bin = self.lookup_bin(old_position);
|
|
|
84 |
if let Some(index) = old_bin.static_refs.iter().position(|id| *id == gear_id) {
|
|
|
85 |
let bounds = old_bin.static_entries.swap_remove(index);
|
|
|
86 |
old_bin.static_refs.swap_remove(index);
|
|
|
87 |
|
|
|
88 |
let new_bin = if old_bin_index == new_bin_index {
|
|
|
89 |
old_bin
|
|
|
90 |
} else {
|
|
|
91 |
self.get_bin(new_bin_index)
|
|
|
92 |
};
|
|
|
93 |
|
|
|
94 |
new_bin.dynamic_refs.push(gear_id);
|
|
|
95 |
new_bin.dynamic_entries.push(CircleBounds {
|
|
|
96 |
center: *new_position,
|
|
|
97 |
..bounds
|
|
|
98 |
});
|
|
|
99 |
} else if let Some(index) = old_bin.dynamic_refs.iter().position(|id| *id == gear_id) {
|
|
|
100 |
if old_bin_index == new_bin_index {
|
|
|
101 |
old_bin.dynamic_entries[index].center = *new_position
|
|
|
102 |
} else {
|
|
|
103 |
let bounds = old_bin.dynamic_entries.swap_remove(index);
|
|
|
104 |
let new_bin = self.get_bin(new_bin_index);
|
|
|
105 |
|
|
|
106 |
new_bin.dynamic_refs.push(gear_id);
|
|
|
107 |
new_bin.dynamic_entries.push(CircleBounds {
|
|
|
108 |
center: *new_position,
|
|
|
109 |
..bounds
|
|
|
110 |
});
|
|
|
111 |
}
|
|
|
112 |
}
|
|
15120
|
113 |
}
|
|
|
114 |
|
|
|
115 |
pub fn check_collisions(&self, collisions: &mut DetectedCollisions) {
|
|
|
116 |
for bin in &self.bins {
|
|
|
117 |
for bounds in &bin.dynamic_entries {
|
|
|
118 |
for other in &bin.dynamic_entries {
|
|
|
119 |
if bounds.intersects(other) && bounds != other {
|
|
|
120 |
collisions.push(0, 0, &bounds.center)
|
|
|
121 |
}
|
|
|
122 |
}
|
|
|
123 |
|
|
|
124 |
for other in &bin.static_entries {
|
|
|
125 |
if bounds.intersects(other) {
|
|
|
126 |
collisions.push(0, 0, &bounds.center)
|
|
|
127 |
}
|
|
|
128 |
}
|
|
|
129 |
}
|
|
|
130 |
}
|
|
|
131 |
}
|
|
|
132 |
}
|