Implement collision resolution

This commit is contained in:
Savanni D'Gerinel 2024-02-06 09:44:12 -05:00
parent 7364b81d10
commit d62454545f
1 changed files with 99 additions and 7 deletions

View File

@ -1,5 +1,8 @@
use crate::profile; use crate::profile;
use std::sync::{Arc, RwLock}; use std::{
ops::{Mul, Neg},
sync::{Arc, RwLock},
};
const GRAVITY: f64 = 1.; const GRAVITY: f64 = 1.;
@ -32,6 +35,26 @@ struct Vector {
dy: f64, 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)] #[derive(Clone, Copy, Debug, PartialEq)]
struct Grain { struct Grain {
radius: f64, radius: f64,
@ -40,6 +63,11 @@ struct Grain {
} }
impl 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) { 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.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()); 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) { pub fn next(&self, delta_t: std::time::Duration) {
self.move_objects(delta_t); self.move_objects(delta_t);
self.resolve_collisions();
} }
fn move_objects(&self, delta_t: std::time::Duration) { fn move_objects(&self, delta_t: std::time::Duration) {
@ -79,25 +108,44 @@ impl World {
grain.move_by(delta_t); grain.move_by(delta_t);
} }
} }
fn resolve_collisions(&self) {}
} }
pub trait Collision { pub trait Collision {
type Other; 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 { impl Collision for Grain {
type Other = 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 radii = (self.radius + other.radius) * (self.radius + other.radius);
let distance = self.location.distance_sq(&other.location); let distance = self.location.distance_sq(&other.location);
if distance < radii { 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 { } else {
None 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)] #[cfg(test)]
@ -123,10 +171,10 @@ mod test {
velocity: Vector { dx: 0., dy: 0. }, velocity: Vector { dx: 0., dy: 0. },
}; };
assert_matches!(grain_1.collision_depth(&grain_2), Some(v) => { assert_matches!(grain_1.overlap(&grain_2), Some((overlap, _)) => {
assert!(within(v, 19., 0.1)); assert!(within(overlap, 19., 0.1));
} ); } );
assert_matches!(grain_1.collision_depth(&grain_3), None); assert_matches!(grain_1.overlap(&grain_3), None);
} }
#[test] #[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);
}
} }