1use glam::{Vec2, Vec3};
8
9macro_rules! aabb {
10 ($name:ident, $vector:ident, $description:literal) => {
11 #[doc = $description]
12 #[derive(Clone, Copy, Debug, PartialEq)]
13 pub struct $name {
14 pub min: $vector,
16 pub max: $vector,
18 }
19
20 impl $name {
21 pub fn from_center(center: $vector, size: $vector) -> Self {
24 assert!(center.is_finite() && size.is_finite() && size.min_element() >= 0.0);
25 Self {
26 min: center - size * 0.5,
27 max: center + size * 0.5,
28 }
29 }
30
31 pub fn size(&self) -> $vector {
33 self.max - self.min
34 }
35
36 pub fn center(&self) -> $vector {
38 (self.min + self.max) * 0.5
39 }
40
41 pub fn contains(&self, point: $vector) -> bool {
43 point.cmpge(self.min).all() && point.cmple(self.max).all()
44 }
45
46 pub fn intersects(&self, other: &Self) -> bool {
48 self.min.cmplt(other.max).all() && self.max.cmpgt(other.min).all()
49 }
50 }
51 };
52}
53
54aabb!(
55 Aabb2,
56 Vec2,
57 "Axis-aligned 2D box, also used for UI rectangles."
58);
59aabb!(Aabb3, Vec3, "Axis-aligned 3D box.");
60
61#[derive(Clone, Copy, Debug)]
63pub struct Body2D {
64 pub position: Vec2,
66 pub half_size: Vec2,
68 pub velocity: Vec2,
70 pub grounded: bool,
72}
73
74impl Body2D {
75 pub fn new(position: Vec2, size: Vec2) -> Self {
77 let bounds = Aabb2::from_center(position, size);
78 Self {
79 position,
80 half_size: bounds.size() * 0.5,
81 velocity: Vec2::ZERO,
82 grounded: false,
83 }
84 }
85
86 pub fn bounds(&self) -> Aabb2 {
88 Aabb2 {
89 min: self.position - self.half_size,
90 max: self.position + self.half_size,
91 }
92 }
93
94 pub fn move_and_slide(&mut self, dt: f32, solids: &[Aabb2]) {
97 assert!(dt.is_finite() && dt >= 0.0);
98 let mut position = self.position.to_array();
99 let half = self.half_size.to_array();
100 let mut velocity = self.velocity.to_array();
101 self.grounded = false;
102 for axis in [0, 1] {
103 let step = velocity[axis] * dt;
104 let (resolved, hit) = sweep_axis(
105 position,
106 half,
107 axis,
108 step,
109 solids.iter().map(|s| (s.min.to_array(), s.max.to_array())),
110 );
111 position[axis] = resolved;
112 if hit {
113 velocity[axis] = 0.0;
114 self.grounded |= axis == 1 && step > 0.0;
115 }
116 }
117 self.position = Vec2::from_array(position);
118 self.velocity = Vec2::from_array(velocity);
119 }
120}
121
122#[derive(Clone, Copy, Debug)]
124pub struct Body3D {
125 pub position: Vec3,
127 pub half_size: Vec3,
129 pub velocity: Vec3,
131 pub grounded: bool,
133}
134
135impl Body3D {
136 pub fn new(position: Vec3, size: Vec3) -> Self {
138 let bounds = Aabb3::from_center(position, size);
139 Self {
140 position,
141 half_size: bounds.size() * 0.5,
142 velocity: Vec3::ZERO,
143 grounded: false,
144 }
145 }
146
147 pub fn bounds(&self) -> Aabb3 {
149 Aabb3 {
150 min: self.position - self.half_size,
151 max: self.position + self.half_size,
152 }
153 }
154
155 pub fn move_and_slide(&mut self, dt: f32, solids: &[Aabb3]) {
158 assert!(dt.is_finite() && dt >= 0.0);
159 let mut position = self.position.to_array();
160 let half = self.half_size.to_array();
161 let mut velocity = self.velocity.to_array();
162 self.grounded = false;
163 for axis in [0, 2, 1] {
164 let step = velocity[axis] * dt;
165 let (resolved, hit) = sweep_axis(
166 position,
167 half,
168 axis,
169 step,
170 solids.iter().map(|s| (s.min.to_array(), s.max.to_array())),
171 );
172 position[axis] = resolved;
173 if hit {
174 velocity[axis] = 0.0;
175 self.grounded |= axis == 1 && step < 0.0;
176 }
177 }
178 self.position = Vec3::from_array(position);
179 self.velocity = Vec3::from_array(velocity);
180 }
181}
182
183fn sweep_axis<const N: usize>(
184 position: [f32; N],
185 half: [f32; N],
186 axis: usize,
187 step: f32,
188 solids: impl Iterator<Item = ([f32; N], [f32; N])>,
189) -> (f32, bool) {
190 if step == 0.0 {
191 return (position[axis], false);
192 }
193 let mut allowed = step;
194 let mut boundary = 0.0;
195 let mut hit = false;
196 for (min, max) in solids {
197 let overlaps_other_axes = (0..N)
198 .filter(|&i| i != axis)
199 .all(|i| position[i] + half[i] > min[i] && position[i] - half[i] < max[i]);
200 if !overlaps_other_axes {
201 continue;
202 }
203 let low = position[axis] - half[axis];
204 let high = position[axis] + half[axis];
205 if step > 0.0 && high <= min[axis] && high + allowed >= min[axis] {
206 allowed = (min[axis] - high).max(0.0);
207 boundary = min[axis];
208 hit = true;
209 } else if step < 0.0 && low >= max[axis] && low + allowed <= max[axis] {
210 allowed = (max[axis] - low).min(0.0);
211 boundary = max[axis];
212 hit = true;
213 }
214 }
215 if !hit {
216 return (position[axis] + step, false);
217 }
218 let resolved = if step > 0.0 {
221 let center = boundary - half[axis];
222 if center + half[axis] > boundary {
223 center.next_down()
224 } else {
225 center
226 }
227 } else {
228 let center = boundary + half[axis];
229 if center - half[axis] < boundary {
230 center.next_up()
231 } else {
232 center
233 }
234 };
235 (resolved, true)
236}
237
238#[cfg(test)]
239mod tests {
240 use super::*;
241
242 #[test]
243 fn touching_boxes_are_not_overlapping() {
244 let a = Aabb2::from_center(Vec2::ZERO, Vec2::splat(2.0));
245 let b = Aabb2::from_center(Vec2::new(2.0, 0.0), Vec2::splat(2.0));
246 assert!(!a.intersects(&b));
247 assert!(a.contains(Vec2::ONE));
248 }
249
250 #[test]
251 fn fast_2d_fall_cannot_tunnel_through_floor() {
252 let floor = Aabb2::from_center(Vec2::new(0.0, 10.0), Vec2::new(100.0, 1.0));
253 let mut body = Body2D::new(Vec2::ZERO, Vec2::splat(2.0));
254 body.velocity = Vec2::new(0.0, 10_000.0);
255 body.move_and_slide(1.0, &[floor]);
256 assert_eq!(body.position.y, 8.5);
257 assert_eq!(body.velocity.y, 0.0);
258 assert!(body.grounded);
259 }
260
261 #[test]
262 fn wall_contact_preserves_tangential_motion() {
263 let wall = Aabb2::from_center(Vec2::new(10.0, 0.0), Vec2::new(1.0, 100.0));
264 let mut body = Body2D::new(Vec2::ZERO, Vec2::splat(2.0));
265 body.velocity = Vec2::new(100.0, 2.0);
266 body.move_and_slide(1.0, &[wall]);
267 assert_eq!(body.position, Vec2::new(8.5, 2.0));
268 assert_eq!(body.velocity, Vec2::new(0.0, 2.0));
269 }
270
271 #[test]
272 fn three_dimensions_use_y_up_and_stop_at_nearest_solid() {
273 let low = Aabb3::from_center(Vec3::new(0.0, -10.0, 0.0), Vec3::new(100.0, 1.0, 100.0));
274 let high = Aabb3::from_center(Vec3::ZERO, Vec3::new(100.0, 1.0, 100.0));
275 let mut body = Body3D::new(Vec3::new(0.0, 10.0, 0.0), Vec3::splat(2.0));
276 body.velocity.y = -10_000.0;
277 body.move_and_slide(1.0, &[low, high]);
278 assert_eq!(body.position.y, 1.5);
279 assert!(body.grounded);
280 assert_eq!(body.velocity.y, 0.0);
281 }
282
283 #[test]
284 fn upward_collision_does_not_ground_player() {
285 let ceiling = Aabb3::from_center(Vec3::new(0.0, 5.0, 0.0), Vec3::splat(2.0));
286 let mut body = Body3D::new(Vec3::ZERO, Vec3::splat(2.0));
287 body.velocity.y = 100.0;
288 body.move_and_slide(0.1, &[ceiling]);
289 assert_eq!(body.position.y, 3.0);
290 assert!(!body.grounded);
291 }
292
293 #[test]
294 fn fractional_contacts_remain_outside_solids_in_2d() {
295 for coordinate in [-8.8, -0.7, 0.7, 1.6, 3.4, 8.8] {
296 for axis in 0..2 {
297 let mut center = Vec2::ZERO;
298 center[axis] = coordinate;
299 let solid = Aabb2::from_center(center, Vec2::splat(0.7));
300 for direction in [-1.0, 1.0] {
301 let mut body = Body2D::new(Vec2::ZERO, Vec2::new(0.8, 1.8));
302 body.position[axis] = if direction < 0.0 {
303 solid.max[axis] + body.half_size[axis] + 0.137
304 } else {
305 solid.min[axis] - body.half_size[axis] - 0.137
306 };
307 let mut contacts = 0;
308 for _ in 0..120 {
309 body.velocity[axis] = direction * 3.25;
310 body.move_and_slide(1.0 / 120.0, &[solid]);
311 assert!(!body.bounds().intersects(&solid), "{body:?}, {solid:?}");
312 if body.velocity[axis] == 0.0 {
313 contacts += 1;
314 assert_eq!(body.grounded, axis == 1 && direction > 0.0);
315 }
316 }
317 assert!(contacts > 100, "lost contact: {body:?}, {solid:?}");
318 }
319 }
320 }
321 }
322
323 #[test]
324 fn fractional_contacts_remain_outside_solids_in_3d() {
325 for coordinate in [-8.8, -0.7, 0.7, 1.6, 3.4, 8.8] {
326 for axis in 0..3 {
327 let mut center = Vec3::ZERO;
328 center[axis] = coordinate;
329 let solid = Aabb3::from_center(center, Vec3::splat(0.7));
330 for direction in [-1.0, 1.0] {
331 let mut body = Body3D::new(Vec3::ZERO, Vec3::new(0.8, 1.8, 0.8));
332 body.position[axis] = if direction < 0.0 {
333 solid.max[axis] + body.half_size[axis] + 0.137
334 } else {
335 solid.min[axis] - body.half_size[axis] - 0.137
336 };
337 let mut contacts = 0;
338 for _ in 0..120 {
339 body.velocity[axis] = direction * 3.25;
340 body.move_and_slide(1.0 / 120.0, &[solid]);
341 assert!(!body.bounds().intersects(&solid), "{body:?}, {solid:?}");
342 if body.velocity[axis] == 0.0 {
343 contacts += 1;
344 assert_eq!(body.grounded, axis == 1 && direction < 0.0);
345 }
346 }
347 assert!(contacts > 100, "lost contact: {body:?}, {solid:?}");
348 }
349 }
350 }
351 }
352}