From d62454545fd3a69884629b0cc48936f60e0c681e Mon Sep 17 00:00:00 2001 From: Savanni D'Gerinel Date: Tue, 6 Feb 2024 09:44:12 -0500 Subject: [PATCH] Implement collision resolution --- falling-sand/src/physics.rs | 106 +++++++++++++++++++++++++++++++++--- 1 file changed, 99 insertions(+), 7 deletions(-) diff --git a/falling-sand/src/physics.rs b/falling-sand/src/physics.rs index 6153a95..a95fdde 100644 --- a/falling-sand/src/physics.rs +++ b/falling-sand/src/physics.rs @@ -1,5 +1,8 @@ use crate::profile; -use std::sync::{Arc, RwLock}; +use std::{ + ops::{Mul, Neg}, + sync::{Arc, RwLock}, +}; const GRAVITY: f64 = 1.; @@ -32,6 +35,26 @@ struct Vector { dy: f64, } +impl Neg for Vector { + type Output = Vector; + fn neg(self) -> Self { + Self { + dx: -self.dx, + dy: -self.dy, + } + } +} + +impl Mul for Vector { + type Output = Vector; + fn mul(self, rhs: f64) -> Self { + Self { + dx: self.dx * rhs, + dy: self.dy * rhs, + } + } +} + #[derive(Clone, Copy, Debug, PartialEq)] struct Grain { radius: f64, @@ -40,6 +63,11 @@ struct Grain { } impl Grain { + fn translate(&mut self, vec: Vector) { + self.location.x = self.location.x + vec.dx; + self.location.y = self.location.y + vec.dy; + } + fn move_by(&mut self, delta_t: std::time::Duration) { self.location.x = self.location.x + self.velocity.dx * (delta_t.as_secs_f64()); self.location.y = self.location.y + self.velocity.dy * (delta_t.as_secs_f64()); @@ -72,6 +100,7 @@ impl World { pub fn next(&self, delta_t: std::time::Duration) { self.move_objects(delta_t); + self.resolve_collisions(); } fn move_objects(&self, delta_t: std::time::Duration) { @@ -79,25 +108,44 @@ impl World { grain.move_by(delta_t); } } + + fn resolve_collisions(&self) {} } pub trait Collision { type Other; - fn collision_depth(&self, other: &Self::Other) -> Option; + fn overlap(&self, other: &Self::Other) -> Option<(f64, Vector)>; + fn resolve_collision(&mut self, other: &mut Self::Other); } impl Collision for Grain { type Other = Grain; - fn collision_depth(&self, other: &Self::Other) -> Option { + fn overlap(&self, other: &Self::Other) -> Option<(f64, Vector)> { let radii = (self.radius + other.radius) * (self.radius + other.radius); let distance = self.location.distance_sq(&other.location); if distance < radii { - Some(radii.sqrt() - distance.sqrt()) + let dx = other.location.x - self.location.x; + let dy = other.location.y - self.location.y; + Some((radii.sqrt() - distance.sqrt(), Vector { dx, dy })) } else { None } } + + fn resolve_collision(&mut self, other: &mut Self::Other) { + match self.overlap(other) { + Some((overlap, v)) => { + let max_overlap = self.radius + other.radius; + let resolution = overlap / max_overlap; + let v = v * resolution; + println!("{:?} {} {} {}", v, overlap, max_overlap, resolution); + self.translate(-v); + other.translate(v); + } + None => {} + } + } } #[cfg(test)] @@ -123,10 +171,10 @@ mod test { velocity: Vector { dx: 0., dy: 0. }, }; - assert_matches!(grain_1.collision_depth(&grain_2), Some(v) => { - assert!(within(v, 19., 0.1)); + assert_matches!(grain_1.overlap(&grain_2), Some((overlap, _)) => { + assert!(within(overlap, 19., 0.1)); } ); - assert_matches!(grain_1.collision_depth(&grain_3), None); + assert_matches!(grain_1.overlap(&grain_3), None); } #[test] @@ -188,4 +236,48 @@ mod test { ); } } + + #[test] + fn it_resolves_a_collision_when_no_collision_is_present() { + let mut grain_1 = Grain { + radius: 10., + location: Point { x: 0., y: 0. }, + velocity: Vector { dx: 0., dy: 0. }, + }; + let mut grain_2 = Grain { + radius: 4., + location: Point { x: 15., y: 0. }, + velocity: Vector { dx: 0., dy: 0. }, + }; + + let grain_1_b = grain_1.clone(); + let grain_2_b = grain_2.clone(); + + grain_1.resolve_collision(&mut grain_2); + assert!(grain_1.location.eq(&grain_1_b.location, 0.001)); + assert!(grain_2.location.eq(&grain_2_b.location, 0.001)); + } + + #[test] + fn it_resolves_a_collision() { + let mut grain_1 = Grain { + radius: 4., + location: Point { x: 0., y: 0. }, + velocity: Vector { dx: 0., dy: 0. }, + }; + let mut grain_2 = Grain { + radius: 4., + location: Point { x: 4., y: 0. }, + velocity: Vector { dx: 0., dy: 0. }, + }; + + grain_1.resolve_collision(&mut grain_2); + assert!( + grain_1.location.eq(&Point { x: -2., y: 0. }, 0.001), + "{:?}", + grain_1 + ); + assert!(grain_2.location.eq(&Point { x: 6., y: 0. }, 0.001)); + assert_eq!(grain_1.overlap(&grain_2), None); + } }