5 |
5 |
6 use fpnum::FPPoint; |
6 use fpnum::FPPoint; |
7 use integral_geometry::{GridIndex, Point, Size}; |
7 use integral_geometry::{GridIndex, Point, Size}; |
8 |
8 |
9 struct GridBin { |
9 struct GridBin { |
10 static_refs: Vec<GearId>, |
10 refs: Vec<GearId>, |
11 static_entries: Vec<CircleBounds>, |
11 entries: Vec<CircleBounds>, |
12 |
|
13 dynamic_refs: Vec<GearId>, |
|
14 dynamic_entries: Vec<CircleBounds>, |
|
15 } |
12 } |
16 |
13 |
17 impl GridBin { |
14 impl GridBin { |
18 fn new() -> Self { |
15 fn new() -> Self { |
19 Self { |
16 Self { |
20 static_refs: vec![], |
17 refs: vec![], |
21 static_entries: vec![], |
18 entries: vec![], |
22 dynamic_refs: vec![], |
19 } |
23 dynamic_entries: vec![], |
20 } |
|
21 |
|
22 fn add(&mut self, gear_id: GearId, bounds: &CircleBounds) { |
|
23 self.refs.push(gear_id); |
|
24 self.entries.push(*bounds); |
|
25 } |
|
26 |
|
27 fn remove(&mut self, gear_id: GearId) -> bool { |
|
28 if let Some(pos) = self.refs.iter().position(|id| *id == gear_id) { |
|
29 self.refs.swap_remove(pos); |
|
30 self.entries.swap_remove(pos); |
|
31 true |
|
32 } else { |
|
33 false |
24 } |
34 } |
25 } |
35 } |
26 } |
36 } |
27 |
37 |
28 const GRID_BIN_SIZE: usize = 128; |
38 const GRID_BIN_SIZE: usize = 128; |
62 |
72 |
63 fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin { |
73 fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin { |
64 self.get_bin(self.bin_index(position)) |
74 self.get_bin(self.bin_index(position)) |
65 } |
75 } |
66 |
76 |
67 pub fn insert_static(&mut self, gear_id: GearId, bounds: &CircleBounds) { |
77 pub fn insert(&mut self, gear_id: GearId, bounds: &CircleBounds) { |
68 let bin = self.lookup_bin(&bounds.center); |
78 self.lookup_bin(&bounds.center).add(gear_id, bounds); |
69 bin.static_refs.push(gear_id); |
|
70 bin.static_entries.push(*bounds) |
|
71 } |
79 } |
72 |
80 |
73 pub fn insert_dynamic(&mut self, gear_id: GearId, bounds: &CircleBounds) { |
81 fn remove_all(&mut self, gear_id: GearId) { |
74 let bin = self.lookup_bin(&bounds.center); |
82 for bin in &mut self.bins { |
75 bin.dynamic_refs.push(gear_id); |
83 if bin.remove(gear_id) { |
76 bin.dynamic_entries.push(*bounds); |
84 break; |
77 } |
|
78 |
|
79 pub fn remove(&mut self, gear_id: GearId) {} |
|
80 |
|
81 pub fn update_position( |
|
82 &mut self, |
|
83 gear_id: GearId, |
|
84 old_position: &FPPoint, |
|
85 new_position: &FPPoint, |
|
86 ) { |
|
87 let old_bin_index = self.bin_index(old_position); |
|
88 let new_bin_index = self.bin_index(new_position); |
|
89 |
|
90 if let Some(old_bin) = self.try_get_bin(old_bin_index) { |
|
91 let bounds = if let Some(index) = |
|
92 old_bin.dynamic_refs.iter().position(|id| *id == gear_id) |
|
93 { |
|
94 if old_bin_index == new_bin_index { |
|
95 old_bin.dynamic_entries[index].center = *new_position; |
|
96 None |
|
97 } else { |
|
98 Some(old_bin.dynamic_entries.swap_remove(index)) |
|
99 } |
|
100 } else if let Some(index) = old_bin.static_refs.iter().position(|id| *id == gear_id) { |
|
101 old_bin.static_refs.swap_remove(index); |
|
102 Some(old_bin.static_entries.swap_remove(index)) |
|
103 } else { |
|
104 None |
|
105 }; |
|
106 |
|
107 if let Some(bounds) = bounds { |
|
108 let new_bin = if old_bin_index == new_bin_index { |
|
109 Some(old_bin) |
|
110 } else { |
|
111 self.try_get_bin(new_bin_index) |
|
112 }; |
|
113 |
|
114 if let Some(new_bin) = new_bin { |
|
115 new_bin.dynamic_refs.push(gear_id); |
|
116 new_bin.dynamic_entries.push(CircleBounds { |
|
117 center: *new_position, |
|
118 ..bounds |
|
119 }); |
|
120 } |
|
121 } |
85 } |
122 } |
86 } |
123 } |
87 } |
124 |
88 |
|
89 pub fn remove(&mut self, gear_id: GearId, bounds: Option<&CircleBounds>) { |
|
90 if let Some(bounds) = bounds { |
|
91 if !self.lookup_bin(&bounds.center).remove(gear_id) { |
|
92 self.remove_all(gear_id); |
|
93 } |
|
94 } else { |
|
95 self.remove_all(gear_id); |
|
96 } |
|
97 |
|
98 } |
|
99 |
125 pub fn check_collisions(&self, collisions: &mut DetectedCollisions) { |
100 pub fn check_collisions(&self, collisions: &mut DetectedCollisions) { |
126 for bin in &self.bins { |
101 for bin in &self.bins { |
127 for (index, bounds) in bin.dynamic_entries.iter().enumerate() { |
102 for (index, bounds) in bin.entries.iter().enumerate() { |
128 for (other_index, other) in bin.dynamic_entries.iter().enumerate().skip(index + 1) { |
|
129 if bounds.intersects(other) && bounds != other { |
|
130 collisions.push( |
|
131 bin.dynamic_refs[index], |
|
132 Some(bin.dynamic_refs[other_index]), |
|
133 &((bounds.center + other.center) / 2), |
|
134 ) |
|
135 } |
|
136 } |
|
137 |
103 |
138 for (other_index, other) in bin.static_entries.iter().enumerate() { |
|
139 if bounds.intersects(other) { |
|
140 collisions.push( |
|
141 bin.dynamic_refs[index], |
|
142 Some(bin.static_refs[other_index]), |
|
143 &((bounds.center + other.center) / 2), |
|
144 ) |
|
145 } |
|
146 } |
|
147 } |
104 } |
148 } |
105 } |
149 } |
106 } |
150 } |
107 } |