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 integral_geometry::Size; |
|
8 use land2d::Land2D; |
|
9 |
|
10 use crate::{ |
|
11 common::{ |
|
12 GearId, |
|
13 GearData, |
|
14 GearDataAggregator, |
|
15 GearDataProcessor |
|
16 }, |
|
17 physics::{ |
|
18 PhysicsProcessor, |
|
19 PhysicsData |
|
20 }, |
|
21 collision::{ |
|
22 CollisionProcessor, |
|
23 CollisionData, |
|
24 ContactData |
|
25 } |
3 }; |
26 }; |
4 |
27 |
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 { |
28 pub struct JoinedData { |
|
29 gear_id: GearId, |
47 physics: PhysicsData, |
30 physics: PhysicsData, |
48 collision: CollisionData, |
31 collision: CollisionData, |
49 contact: ContactData |
32 contact: ContactData |
50 } |
33 } |
51 |
34 |
52 pub struct World { |
35 pub struct World { |
53 enabled_physics: PhysicsCollection, |
36 physics: PhysicsProcessor, |
54 disabled_physics: Vec<PhysicsData>, |
37 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 } |
38 } |
64 |
39 |
65 #[derive(PartialEq, Eq, Clone, Copy, Debug)] |
40 macro_rules! processor_map { |
66 struct CircleBounds { |
41 ( $data_type: ident => $field: ident ) => { |
67 center: FPPoint, |
42 impl GearDataAggregator<$data_type> for World { |
68 radius: FPNum |
43 fn find_processor(&mut self) -> &mut GearDataProcessor<$data_type> { |
69 } |
44 &mut self.$field |
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 } |
45 } |
160 } |
46 } |
161 } |
47 } |
162 } |
48 } |
163 |
49 |
|
50 processor_map!(PhysicsData => physics); |
|
51 processor_map!(CollisionData => collision); |
|
52 |
164 impl World { |
53 impl World { |
165 pub fn step(&mut self, time_step: FPNum, land: &Land2D<u32>) { |
54 pub fn new(world_size: Size) -> Self { |
166 for (pos, vel) in self.enabled_physics.iter_mut_pos() { |
55 Self { |
167 *pos += *vel |
56 physics: PhysicsProcessor::new(), |
168 } |
57 collision: CollisionProcessor::new(world_size) |
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 } |
58 } |
179 } |
59 } |
180 |
60 |
181 pub fn add_gear(&mut self, data: JoinedData) { |
61 pub fn step(&mut self, time_step: FPNum, land: &Land2D<u32>) { |
182 if data.physics.velocity == FPPoint::zero() { |
62 let updates = self.physics.process(time_step); |
183 self.disabled_physics.push(data.physics); |
63 self.collision.process(land, &updates); |
184 self.grid.insert_static(0, &data.physics.position, &data.collision.bounds); |
64 } |
185 } else { |
65 |
186 self.enabled_physics.push(data.physics); |
66 pub fn add_gear_data<T>(&mut self, gear_id: GearId, data: T) |
187 self.grid.insert_dynamic(0, &data.physics.position, &data.collision.bounds); |
67 where T: GearData, |
188 } |
68 Self: GearDataAggregator<T> |
|
69 { |
|
70 self.find_processor().add(gear_id, data); |
189 } |
71 } |
190 } |
72 } |
191 |
73 |
192 #[cfg(test)] |
74 #[cfg(test)] |
193 mod tests { |
75 mod tests { |
|
76 use crate::{ |
|
77 World, |
|
78 physics::PhysicsData, |
|
79 collision::{CollisionData, CircleBounds} |
|
80 }; |
|
81 use fpnum::{FPNum, FPPoint, fp}; |
|
82 use integral_geometry::Size; |
|
83 use land2d::Land2D; |
194 |
84 |
|
85 #[test] |
|
86 fn data_flow() { |
|
87 let world_size = Size::new(2048, 2048); |
|
88 |
|
89 let mut world = World::new(world_size); |
|
90 let gear_id = 46631; |
|
91 |
|
92 world.add_gear_data(gear_id, PhysicsData { |
|
93 position: FPPoint::zero(), |
|
94 velocity: FPPoint::unit_y() |
|
95 }); |
|
96 |
|
97 world.add_gear_data(gear_id, CollisionData { |
|
98 bounds: CircleBounds { |
|
99 center: FPPoint::zero(), |
|
100 radius: fp!(10) |
|
101 } |
|
102 }); |
|
103 |
|
104 let land = Land2D::new(Size::new(world_size.width - 2, world_size.height - 2), 0); |
|
105 |
|
106 world.step(fp!(1), &land); |
|
107 } |
195 } |
108 } |