author | alfadur |
Wed, 28 Aug 2019 00:10:41 +0300 | |
changeset 15369 | 0f2fd8d12734 |
parent 15274 | 42b710b0f883 |
child 15756 | 1d652bca512e |
permissions | -rw-r--r-- |
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 |
||
15274 | 74 |
pub fn remove(&mut self, gear_id: GearId) {} |
75 |
||
15261 | 76 |
pub fn update_position( |
77 |
&mut self, |
|
78 |
gear_id: GearId, |
|
79 |
old_position: &FPPoint, |
|
80 |
new_position: &FPPoint, |
|
81 |
) { |
|
82 |
let old_bin_index = self.bin_index(old_position); |
|
83 |
let new_bin_index = self.bin_index(new_position); |
|
84 |
||
85 |
let old_bin = self.lookup_bin(old_position); |
|
15262 | 86 |
if let Some(index) = old_bin.dynamic_refs.iter().position(|id| *id == gear_id) { |
87 |
if old_bin_index == new_bin_index { |
|
88 |
old_bin.dynamic_entries[index].center = *new_position |
|
89 |
} else { |
|
90 |
let bounds = old_bin.dynamic_entries.swap_remove(index); |
|
91 |
let new_bin = self.get_bin(new_bin_index); |
|
92 |
||
93 |
new_bin.dynamic_refs.push(gear_id); |
|
94 |
new_bin.dynamic_entries.push(CircleBounds { |
|
95 |
center: *new_position, |
|
96 |
..bounds |
|
97 |
}); |
|
98 |
} |
|
99 |
} else if let Some(index) = old_bin.static_refs.iter().position(|id| *id == gear_id) { |
|
15261 | 100 |
let bounds = old_bin.static_entries.swap_remove(index); |
101 |
old_bin.static_refs.swap_remove(index); |
|
102 |
||
103 |
let new_bin = if old_bin_index == new_bin_index { |
|
104 |
old_bin |
|
105 |
} else { |
|
106 |
self.get_bin(new_bin_index) |
|
107 |
}; |
|
108 |
||
109 |
new_bin.dynamic_refs.push(gear_id); |
|
110 |
new_bin.dynamic_entries.push(CircleBounds { |
|
111 |
center: *new_position, |
|
112 |
..bounds |
|
113 |
}); |
|
114 |
} |
|
15120 | 115 |
} |
116 |
||
117 |
pub fn check_collisions(&self, collisions: &mut DetectedCollisions) { |
|
118 |
for bin in &self.bins { |
|
15263 | 119 |
for (index, bounds) in bin.dynamic_entries.iter().enumerate() { |
120 |
for (other_index, other) in bin.dynamic_entries.iter().enumerate().skip(index + 1) { |
|
15120 | 121 |
if bounds.intersects(other) && bounds != other { |
15263 | 122 |
collisions.push( |
123 |
bin.dynamic_refs[index], |
|
124 |
Some(bin.dynamic_refs[other_index]), |
|
15273
bfd185ad03e7
use better contact point for inter-gear collision
alfadur
parents:
15263
diff
changeset
|
125 |
&((bounds.center + other.center) / 2), |
15263 | 126 |
) |
15120 | 127 |
} |
128 |
} |
|
129 |
||
15263 | 130 |
for (other_index, other) in bin.static_entries.iter().enumerate() { |
15120 | 131 |
if bounds.intersects(other) { |
15263 | 132 |
collisions.push( |
133 |
bin.dynamic_refs[index], |
|
134 |
Some(bin.static_refs[other_index]), |
|
15273
bfd185ad03e7
use better contact point for inter-gear collision
alfadur
parents:
15263
diff
changeset
|
135 |
&((bounds.center + other.center) / 2), |
15263 | 136 |
) |
15120 | 137 |
} |
138 |
} |
|
139 |
} |
|
140 |
} |
|
141 |
} |
|
142 |
} |