1use glam::{Vec2, Vec3};
9
10macro_rules! aabb {
11 ($name:ident, $vector:ident, $description:literal) => {
12 #[doc = $description]
13 #[derive(Clone, Copy, Debug, PartialEq)]
14 pub struct $name {
15 pub min: $vector,
17 pub max: $vector,
19 }
20
21 impl $name {
22 pub fn from_center(center: $vector, size: $vector) -> Self {
25 assert!(center.is_finite() && size.is_finite() && size.min_element() >= 0.0);
26 Self {
27 min: center - size * 0.5,
28 max: center + size * 0.5,
29 }
30 }
31
32 pub fn size(&self) -> $vector {
34 self.max - self.min
35 }
36
37 pub fn center(&self) -> $vector {
39 (self.min + self.max) * 0.5
40 }
41
42 pub fn contains(&self, point: $vector) -> bool {
44 point.cmpge(self.min).all() && point.cmple(self.max).all()
45 }
46
47 pub fn intersects(&self, other: &Self) -> bool {
49 self.min.cmplt(other.max).all() && self.max.cmpgt(other.min).all()
50 }
51 }
52 };
53}
54
55aabb!(
56 Aabb2,
57 Vec2,
58 "Axis-aligned 2D box, also used for UI rectangles."
59);
60aabb!(Aabb3, Vec3, "Axis-aligned 3D box.");
61
62#[derive(Clone, Copy, Debug)]
64pub struct Body2D {
65 pub position: Vec2,
67 pub half_size: Vec2,
69 pub velocity: Vec2,
71 pub grounded: bool,
73}
74
75impl Body2D {
76 pub fn new(position: Vec2, size: Vec2) -> Self {
78 let bounds = Aabb2::from_center(position, size);
79 Self {
80 position,
81 half_size: bounds.size() * 0.5,
82 velocity: Vec2::ZERO,
83 grounded: false,
84 }
85 }
86
87 pub fn bounds(&self) -> Aabb2 {
89 Aabb2 {
90 min: self.position - self.half_size,
91 max: self.position + self.half_size,
92 }
93 }
94
95 pub fn move_and_slide(&mut self, dt: f32, solids: &[Aabb2]) {
98 assert!(dt.is_finite() && dt >= 0.0);
99 let mut position = self.position.to_array();
100 let half = self.half_size.to_array();
101 crate::physics::depenetrate_box(
102 &mut position,
103 half,
104 solids.iter().map(|s| (s.min.to_array(), s.max.to_array())),
105 );
106 let mut velocity = self.velocity.to_array();
107 self.grounded = false;
108 for axis in [0, 1] {
109 let step = velocity[axis] * dt;
110 let (resolved, hit) = sweep_axis(
111 position,
112 half,
113 axis,
114 step,
115 solids.iter().map(|s| (s.min.to_array(), s.max.to_array())),
116 );
117 position[axis] = resolved;
118 if hit {
119 velocity[axis] = 0.0;
120 self.grounded |= axis == 1 && step > 0.0;
121 }
122 }
123 self.position = Vec2::from_array(position);
124 self.velocity = Vec2::from_array(velocity);
125 }
126}
127
128#[derive(Clone, Copy, Debug)]
130pub struct Body3D {
131 pub position: Vec3,
133 pub half_size: Vec3,
135 pub velocity: Vec3,
137 pub grounded: bool,
139}
140
141impl Body3D {
142 pub fn new(position: Vec3, size: Vec3) -> Self {
144 let bounds = Aabb3::from_center(position, size);
145 Self {
146 position,
147 half_size: bounds.size() * 0.5,
148 velocity: Vec3::ZERO,
149 grounded: false,
150 }
151 }
152
153 pub fn bounds(&self) -> Aabb3 {
155 Aabb3 {
156 min: self.position - self.half_size,
157 max: self.position + self.half_size,
158 }
159 }
160
161 pub fn move_and_slide(&mut self, dt: f32, solids: &[Aabb3]) {
164 assert!(dt.is_finite() && dt >= 0.0);
165 let mut position = self.position.to_array();
166 let half = self.half_size.to_array();
167 crate::physics::depenetrate_box(
168 &mut position,
169 half,
170 solids.iter().map(|s| (s.min.to_array(), s.max.to_array())),
171 );
172 let mut velocity = self.velocity.to_array();
173 self.grounded = false;
174 for axis in [0, 2, 1] {
175 let step = velocity[axis] * dt;
176 let (resolved, hit) = sweep_axis(
177 position,
178 half,
179 axis,
180 step,
181 solids.iter().map(|s| (s.min.to_array(), s.max.to_array())),
182 );
183 position[axis] = resolved;
184 if hit {
185 velocity[axis] = 0.0;
186 self.grounded |= axis == 1 && step < 0.0;
187 }
188 }
189 self.position = Vec3::from_array(position);
190 self.velocity = Vec3::from_array(velocity);
191 }
192}
193
194fn sweep_axis<const N: usize>(
195 position: [f32; N],
196 half: [f32; N],
197 axis: usize,
198 step: f32,
199 solids: impl Iterator<Item = ([f32; N], [f32; N])>,
200) -> (f32, bool) {
201 if step == 0.0 {
202 return (position[axis], false);
203 }
204 let mut allowed = step;
205 let mut boundary = 0.0;
206 let mut hit = false;
207 for (min, max) in solids {
208 let overlaps_other_axes = (0..N)
209 .filter(|&i| i != axis)
210 .all(|i| position[i] + half[i] > min[i] && position[i] - half[i] < max[i]);
211 if !overlaps_other_axes {
212 continue;
213 }
214 let low = position[axis] - half[axis];
215 let high = position[axis] + half[axis];
216 if step > 0.0 && high <= min[axis] && high + allowed >= min[axis] {
217 allowed = (min[axis] - high).max(0.0);
218 boundary = min[axis];
219 hit = true;
220 } else if step < 0.0 && low >= max[axis] && low + allowed <= max[axis] {
221 allowed = (max[axis] - low).min(0.0);
222 boundary = max[axis];
223 hit = true;
224 }
225 }
226 if !hit {
227 return (position[axis] + step, false);
228 }
229 let resolved = if step > 0.0 {
232 let center = boundary - half[axis];
233 if center + half[axis] > boundary {
234 center.next_down()
235 } else {
236 center
237 }
238 } else {
239 let center = boundary + half[axis];
240 if center - half[axis] < boundary {
241 center.next_up()
242 } else {
243 center
244 }
245 };
246 (resolved, true)
247}
248
249#[cfg(test)]
250mod tests {
251 use super::*;
252
253 #[test]
254 fn touching_boxes_are_not_overlapping() {
255 let a = Aabb2::from_center(Vec2::ZERO, Vec2::splat(2.0));
256 let b = Aabb2::from_center(Vec2::new(2.0, 0.0), Vec2::splat(2.0));
257 assert!(!a.intersects(&b));
258 assert!(a.contains(Vec2::ONE));
259 }
260
261 #[test]
262 fn fast_2d_fall_cannot_tunnel_through_floor() {
263 let floor = Aabb2::from_center(Vec2::new(0.0, 10.0), Vec2::new(100.0, 1.0));
264 let mut body = Body2D::new(Vec2::ZERO, Vec2::splat(2.0));
265 body.velocity = Vec2::new(0.0, 10_000.0);
266 body.move_and_slide(1.0, &[floor]);
267 assert_eq!(body.position.y, 8.5);
268 assert_eq!(body.velocity.y, 0.0);
269 assert!(body.grounded);
270 }
271
272 #[test]
273 fn wall_contact_preserves_tangential_motion() {
274 let wall = Aabb2::from_center(Vec2::new(10.0, 0.0), Vec2::new(1.0, 100.0));
275 let mut body = Body2D::new(Vec2::ZERO, Vec2::splat(2.0));
276 body.velocity = Vec2::new(100.0, 2.0);
277 body.move_and_slide(1.0, &[wall]);
278 assert_eq!(body.position, Vec2::new(8.5, 2.0));
279 assert_eq!(body.velocity, Vec2::new(0.0, 2.0));
280 }
281
282 #[test]
283 fn three_dimensions_use_y_up_and_stop_at_nearest_solid() {
284 let low = Aabb3::from_center(Vec3::new(0.0, -10.0, 0.0), Vec3::new(100.0, 1.0, 100.0));
285 let high = Aabb3::from_center(Vec3::ZERO, Vec3::new(100.0, 1.0, 100.0));
286 let mut body = Body3D::new(Vec3::new(0.0, 10.0, 0.0), Vec3::splat(2.0));
287 body.velocity.y = -10_000.0;
288 body.move_and_slide(1.0, &[low, high]);
289 assert_eq!(body.position.y, 1.5);
290 assert!(body.grounded);
291 assert_eq!(body.velocity.y, 0.0);
292 }
293
294 #[test]
295 fn upward_collision_does_not_ground_player() {
296 let ceiling = Aabb3::from_center(Vec3::new(0.0, 5.0, 0.0), Vec3::splat(2.0));
297 let mut body = Body3D::new(Vec3::ZERO, Vec3::splat(2.0));
298 body.velocity.y = 100.0;
299 body.move_and_slide(0.1, &[ceiling]);
300 assert_eq!(body.position.y, 3.0);
301 assert!(!body.grounded);
302 }
303
304 #[test]
305 fn fractional_contacts_remain_outside_solids_in_2d() {
306 for coordinate in [-8.8, -0.7, 0.7, 1.6, 3.4, 8.8] {
307 for axis in 0..2 {
308 let mut center = Vec2::ZERO;
309 center[axis] = coordinate;
310 let solid = Aabb2::from_center(center, Vec2::splat(0.7));
311 for direction in [-1.0, 1.0] {
312 let mut body = Body2D::new(Vec2::ZERO, Vec2::new(0.8, 1.8));
313 body.position[axis] = if direction < 0.0 {
314 solid.max[axis] + body.half_size[axis] + 0.137
315 } else {
316 solid.min[axis] - body.half_size[axis] - 0.137
317 };
318 let mut contacts = 0;
319 for _ in 0..120 {
320 body.velocity[axis] = direction * 3.25;
321 body.move_and_slide(1.0 / 120.0, &[solid]);
322 assert!(!body.bounds().intersects(&solid), "{body:?}, {solid:?}");
323 if body.velocity[axis] == 0.0 {
324 contacts += 1;
325 assert_eq!(body.grounded, axis == 1 && direction > 0.0);
326 }
327 }
328 assert!(contacts > 100, "lost contact: {body:?}, {solid:?}");
329 }
330 }
331 }
332 }
333
334 #[test]
335 fn fractional_contacts_remain_outside_solids_in_3d() {
336 for coordinate in [-8.8, -0.7, 0.7, 1.6, 3.4, 8.8] {
337 for axis in 0..3 {
338 let mut center = Vec3::ZERO;
339 center[axis] = coordinate;
340 let solid = Aabb3::from_center(center, Vec3::splat(0.7));
341 for direction in [-1.0, 1.0] {
342 let mut body = Body3D::new(Vec3::ZERO, Vec3::new(0.8, 1.8, 0.8));
343 body.position[axis] = if direction < 0.0 {
344 solid.max[axis] + body.half_size[axis] + 0.137
345 } else {
346 solid.min[axis] - body.half_size[axis] - 0.137
347 };
348 let mut contacts = 0;
349 for _ in 0..120 {
350 body.velocity[axis] = direction * 3.25;
351 body.move_and_slide(1.0 / 120.0, &[solid]);
352 assert!(!body.bounds().intersects(&solid), "{body:?}, {solid:?}");
353 if body.velocity[axis] == 0.0 {
354 contacts += 1;
355 assert_eq!(body.grounded, axis == 1 && direction < 0.0);
356 }
357 }
358 assert!(contacts > 100, "lost contact: {body:?}, {solid:?}");
359 }
360 }
361 }
362 }
363}
364
365pub use crate::physics::{Penetration2D, Penetration3D};
367
368#[derive(Clone, Copy, Debug, PartialEq)]
370pub struct Circle {
371 pub center: Vec2,
373 pub radius: f32,
375}
376impl Circle {
377 pub fn new(center: Vec2, radius: f32) -> Self {
379 assert!(center.is_finite());
380 crate::physics::Shape2D::round(radius);
381 Self { center, radius }
382 }
383 pub fn overlap(&self, other: &Self) -> Option<Penetration2D> {
385 crate::physics::Shape2D::round(self.radius).overlap(
386 self.center,
387 crate::physics::Shape2D::round(other.radius),
388 other.center,
389 )
390 }
391 pub fn overlap_box(&self, other: &Aabb2) -> Option<Penetration2D> {
394 crate::physics::Shape2D::round(self.radius).overlap(
395 self.center,
396 other.overlap_shape()?,
397 other.center(),
398 )
399 }
400}
401#[derive(Clone, Copy, Debug, PartialEq)]
403pub struct Sphere {
404 pub center: Vec3,
406 pub radius: f32,
408}
409impl Sphere {
410 pub fn new(center: Vec3, radius: f32) -> Self {
412 assert!(center.is_finite());
413 crate::physics::Shape3D::round(radius);
414 Self { center, radius }
415 }
416 pub fn overlap(&self, other: &Self) -> Option<Penetration3D> {
418 crate::physics::Shape3D::round(self.radius).overlap(
419 self.center,
420 crate::physics::Shape3D::round(other.radius),
421 other.center,
422 )
423 }
424 pub fn overlap_box(&self, other: &Aabb3) -> Option<Penetration3D> {
427 crate::physics::Shape3D::round(self.radius).overlap(
428 self.center,
429 other.overlap_shape()?,
430 other.center(),
431 )
432 }
433}
434impl Aabb2 {
435 fn overlap_shape(&self) -> Option<crate::physics::Shape2D> {
436 assert!(
437 self.min.is_finite() && self.max.is_finite() && self.min.cmple(self.max).all(),
438 "invalid overlap bounds"
439 );
440 let size = self.size();
441 (size.min_element() > 0.0).then(|| crate::physics::Shape2D::box_shape(size))
442 }
443 pub fn overlap(&self, other: &Self) -> Option<Penetration2D> {
446 self.overlap_shape()?
447 .overlap(self.center(), other.overlap_shape()?, other.center())
448 }
449 pub fn overlap_circle(&self, other: &Circle) -> Option<Penetration2D> {
452 other.overlap_box(self).map(|c| Penetration2D {
453 normal: -c.normal,
454 depth: c.depth,
455 })
456 }
457}
458impl Aabb3 {
459 fn overlap_shape(&self) -> Option<crate::physics::Shape3D> {
460 assert!(
461 self.min.is_finite() && self.max.is_finite() && self.min.cmple(self.max).all(),
462 "invalid overlap bounds"
463 );
464 let size = self.size();
465 (size.min_element() > 0.0).then(|| crate::physics::Shape3D::box_shape(size))
466 }
467 pub fn overlap(&self, other: &Self) -> Option<Penetration3D> {
470 self.overlap_shape()?
471 .overlap(self.center(), other.overlap_shape()?, other.center())
472 }
473 pub fn overlap_sphere(&self, other: &Sphere) -> Option<Penetration3D> {
476 other.overlap_box(self).map(|c| Penetration3D {
477 normal: -c.normal,
478 depth: c.depth,
479 })
480 }
481}