Implement collision resolution
This commit is contained in:
parent
7364b81d10
commit
d62454545f
|
@ -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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue