1 use std::{ |
1 mod common; |
2 ops::RangeInclusive |
2 mod physics; |
|
3 mod grid; |
|
4 mod collision; |
|
5 |
|
6 use fpnum::FPNum; |
|
7 use land2d::Land2D; |
|
8 |
|
9 use crate::{ |
|
10 common::GearId, |
|
11 physics::{ |
|
12 PhysicsProcessor, |
|
13 PhysicsData |
|
14 }, |
|
15 collision::{ |
|
16 CollisionProcessor, |
|
17 CollisionData, |
|
18 ContactData |
|
19 } |
3 }; |
20 }; |
4 |
21 |
5 use fpnum::*; |
|
6 use integral_geometry::{ |
|
7 Point, Size, GridIndex |
|
8 }; |
|
9 use land2d::Land2D; |
|
10 |
|
11 type Index = u16; |
|
12 |
|
13 #[derive(PartialEq, Eq, Clone, Copy, Debug)] |
|
14 struct PhysicsData { |
|
15 position: FPPoint, |
|
16 velocity: FPPoint, |
|
17 } |
|
18 |
|
19 #[derive(PartialEq, Eq, Clone, Copy, Debug)] |
|
20 struct CollisionData { |
|
21 bounds: CircleBounds |
|
22 } |
|
23 |
|
24 #[derive(PartialEq, Eq, Clone, Copy, Debug)] |
|
25 struct ContactData { |
|
26 elasticity: FPNum, |
|
27 friction: FPNum |
|
28 } |
|
29 |
|
30 pub struct PhysicsCollection { |
|
31 positions: Vec<FPPoint>, |
|
32 velocities: Vec<FPPoint> |
|
33 } |
|
34 |
|
35 impl PhysicsCollection { |
|
36 fn push(&mut self, data: PhysicsData) { |
|
37 self.positions.push(data.position); |
|
38 self.velocities.push(data.velocity); |
|
39 } |
|
40 |
|
41 fn iter_mut_pos(&mut self) -> impl Iterator<Item = (&mut FPPoint, &FPPoint)> { |
|
42 self.positions.iter_mut().zip(self.velocities.iter()) |
|
43 } |
|
44 } |
|
45 |
|
46 pub struct JoinedData { |
22 pub struct JoinedData { |
|
23 gear_id: GearId, |
47 physics: PhysicsData, |
24 physics: PhysicsData, |
48 collision: CollisionData, |
25 collision: CollisionData, |
49 contact: ContactData |
26 contact: ContactData |
50 } |
27 } |
51 |
28 |
52 pub struct World { |
29 pub struct World { |
53 enabled_physics: PhysicsCollection, |
30 physics: PhysicsProcessor, |
54 disabled_physics: Vec<PhysicsData>, |
31 collision: CollisionProcessor, |
55 |
|
56 enabled_collision: Vec<CollisionData>, |
|
57 disabled_collision: Vec<CollisionData>, |
|
58 grid: Grid, |
|
59 |
|
60 physics_cleanup: Vec<PhysicsData>, |
|
61 collision_output: Vec<(Index, Index)>, |
|
62 land_collision_output: Vec<Index> |
|
63 } |
|
64 |
|
65 #[derive(PartialEq, Eq, Clone, Copy, Debug)] |
|
66 struct CircleBounds { |
|
67 center: FPPoint, |
|
68 radius: FPNum |
|
69 } |
|
70 |
|
71 impl CircleBounds { |
|
72 pub fn intersects(&self, other: &CircleBounds) -> bool { |
|
73 (other.center - self.center).is_in_range(self.radius + other.radius) |
|
74 } |
|
75 |
|
76 pub fn rows(&self) -> impl Iterator<Item = (usize, RangeInclusive<usize>)> { |
|
77 let radius = self.radius.abs_round() as usize; |
|
78 let center = Point::from_fppoint(&self.center); |
|
79 (center.y as usize - radius..=center.y as usize + radius) |
|
80 .map(move |row| (row, center.x as usize - radius..=center.x as usize + radius)) |
|
81 } |
|
82 } |
|
83 |
|
84 fn fppoint_round(point: &FPPoint) -> Point { |
|
85 Point::new(point.x().round() as i32, point.y().round() as i32) |
|
86 } |
|
87 |
|
88 struct GridBin { |
|
89 refs: Vec<Index>, |
|
90 static_entries: Vec<CircleBounds>, |
|
91 dynamic_entries: Vec<CircleBounds> |
|
92 } |
|
93 |
|
94 impl GridBin { |
|
95 fn new() -> Self { |
|
96 Self { |
|
97 refs: vec![], |
|
98 static_entries: vec![], |
|
99 dynamic_entries: vec![] |
|
100 } |
|
101 } |
|
102 } |
|
103 |
|
104 const GRID_BIN_SIZE: usize = 256; |
|
105 |
|
106 struct Grid { |
|
107 bins: Vec<GridBin>, |
|
108 space_size: Size, |
|
109 bins_count: Size, |
|
110 index: GridIndex |
|
111 } |
|
112 |
|
113 impl Grid { |
|
114 fn new(size: Size) -> Self { |
|
115 assert!(size.is_power_of_two()); |
|
116 let bins_count = |
|
117 Size::new(size.width / GRID_BIN_SIZE, |
|
118 size.height / GRID_BIN_SIZE); |
|
119 |
|
120 Self { |
|
121 bins: (0..bins_count.area()).map(|_| GridBin::new()).collect(), |
|
122 space_size: size, |
|
123 bins_count, |
|
124 index: Size::square(GRID_BIN_SIZE).to_grid_index() |
|
125 } |
|
126 } |
|
127 |
|
128 fn bin_index(&self, position: &FPPoint) -> Point { |
|
129 self.index.map(fppoint_round(position)) |
|
130 } |
|
131 |
|
132 fn lookup_bin(&mut self, position: &FPPoint) -> &mut GridBin { |
|
133 let index = self.bin_index(position); |
|
134 &mut self.bins[index.x as usize * self.bins_count.width + index.y as usize] |
|
135 } |
|
136 |
|
137 fn insert_static(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) { |
|
138 self.lookup_bin(position).static_entries.push(*bounds) |
|
139 } |
|
140 |
|
141 fn insert_dynamic(&mut self, index: Index, position: &FPPoint, bounds: &CircleBounds) { |
|
142 self.lookup_bin(position).dynamic_entries.push(*bounds) |
|
143 } |
|
144 |
|
145 fn check_collisions(&self, collisions: &mut Vec<(Index, Index)>) { |
|
146 for bin in &self.bins { |
|
147 for bounds in &bin.dynamic_entries { |
|
148 for other in &bin.dynamic_entries { |
|
149 if bounds.intersects(other) && bounds != other { |
|
150 collisions.push((0, 0)) |
|
151 } |
|
152 } |
|
153 |
|
154 for other in &bin.static_entries { |
|
155 if bounds.intersects(other) { |
|
156 collisions.push((0, 0)) |
|
157 } |
|
158 } |
|
159 } |
|
160 } |
|
161 } |
|
162 } |
32 } |
163 |
33 |
164 impl World { |
34 impl World { |
165 pub fn step(&mut self, time_step: FPNum, land: &Land2D<u32>) { |
35 pub fn step(&mut self, time_step: FPNum, land: &Land2D<u32>) { |
166 for (pos, vel) in self.enabled_physics.iter_mut_pos() { |
36 let updates = self.physics.process(time_step); |
167 *pos += *vel |
37 self.collision.process(land, &updates); |
168 } |
|
169 |
|
170 self.grid.check_collisions(&mut self.collision_output); |
|
171 } |
|
172 |
|
173 fn check_land_collisions(&mut self, land: &Land2D<u32>) { |
|
174 for collision in &self.enabled_collision { |
|
175 if collision.bounds.rows().any(|(y, r)| (&land[y][r]).iter().any(|v| *v != 0)) { |
|
176 self.land_collision_output.push(0) |
|
177 } |
|
178 } |
|
179 } |
38 } |
180 |
39 |
181 pub fn add_gear(&mut self, data: JoinedData) { |
40 pub fn add_gear(&mut self, data: JoinedData) { |
182 if data.physics.velocity == FPPoint::zero() { |
41 self.physics.push(data.gear_id, data.physics); |
183 self.disabled_physics.push(data.physics); |
42 self.collision.push(data.gear_id, data.physics, data.collision); |
184 self.grid.insert_static(0, &data.physics.position, &data.collision.bounds); |
|
185 } else { |
|
186 self.enabled_physics.push(data.physics); |
|
187 self.grid.insert_dynamic(0, &data.physics.position, &data.collision.bounds); |
|
188 } |
|
189 } |
43 } |
190 } |
44 } |
191 |
45 |
192 #[cfg(test)] |
46 #[cfg(test)] |
193 mod tests { |
47 mod tests { |