From d62454545fd3a69884629b0cc48936f60e0c681e Mon Sep 17 00:00:00 2001
From: Savanni D'Gerinel <savanni@luminescent-dreams.com>
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<f64> 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<f64>;
+    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<f64> {
+    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);
+    }
 }