From 198254aa79bb9d45bc46e8ea8984a613774a7c49 Mon Sep 17 00:00:00 2001 From: _ <> Date: Thu, 26 Mar 2020 01:27:26 +0000 Subject: [PATCH] Copy in physics functions from a 2018 project --- Cargo.lock | 7 + Cargo.toml | 1 + src/bin/racing-game.rs | 4 + src/lib.rs | 1 + src/physics.rs | 334 +++++++++++++++++++++++++++++++++++++++++ 5 files changed, 347 insertions(+) create mode 100644 src/bin/racing-game.rs create mode 100644 src/physics.rs diff --git a/Cargo.lock b/Cargo.lock index 09899ba..2bb3664 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -212,11 +212,17 @@ dependencies = [ "iota 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)", "maplit 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)", "nom 5.1.0 (registry+https://github.com/rust-lang/crates.io-index)", + "partial-min-max 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)", "png 0.15.3 (registry+https://github.com/rust-lang/crates.io-index)", "rand 0.6.5 (registry+https://github.com/rust-lang/crates.io-index)", "sdl2 0.32.2 (registry+https://github.com/rust-lang/crates.io-index)", ] +[[package]] +name = "partial-min-max" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" + [[package]] name = "png" version = "0.15.3" @@ -444,6 +450,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" "checksum num-integer 0.1.41 (registry+https://github.com/rust-lang/crates.io-index)" = "b85e541ef8255f6cf42bbfe4ef361305c6c135d10919ecc26126c4e5ae94bc09" "checksum num-iter 0.1.39 (registry+https://github.com/rust-lang/crates.io-index)" = "76bd5272412d173d6bf9afdf98db8612bbabc9a7a830b7bfc9c188911716132e" "checksum num-traits 0.2.10 (registry+https://github.com/rust-lang/crates.io-index)" = "d4c81ffc11c212fa327657cb19dd85eb7419e163b5b076bede2bdb5c974c07e4" +"checksum partial-min-max 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "6448add382c60bbbc64f9dab41309a12ec530c05191601042f911356ac09758c" "checksum png 0.15.3 (registry+https://github.com/rust-lang/crates.io-index)" = "ef859a23054bbfee7811284275ae522f0434a3c8e7f4b74bd4a35ae7e1c4a283" "checksum rand 0.6.5 (registry+https://github.com/rust-lang/crates.io-index)" = "6d71dacdc3c88c1fde3885a3be3fbab9f35724e6ce99467f7d9c5026132184ca" "checksum rand_chacha 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)" = "556d3a1ca6600bfcbab7c7c91ccb085ac7fbbcd70e008a98742e7847f4f7bcef" diff --git a/Cargo.toml b/Cargo.toml index 5893d82..5bf2cf4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,6 +19,7 @@ maplit = "1.0.2" # TODO: Drop nom depend. It's way overkill for iqm. nom = "5.1.0" +partial-min-max = "0.4.0" png = "0.15.3" rand = "0.6.5" sdl2 = "0.32.2" diff --git a/src/bin/racing-game.rs b/src/bin/racing-game.rs new file mode 100644 index 0000000..8b8062e --- /dev/null +++ b/src/bin/racing-game.rs @@ -0,0 +1,4 @@ + +fn main () { + println! ("Racing game."); +} diff --git a/src/lib.rs b/src/lib.rs index f4608be..8cdd714 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -9,6 +9,7 @@ pub mod glezz; pub mod gl_state; pub mod gpu_buffers; pub mod iqm; +pub mod physics; pub mod renderable_model; pub mod shader; pub mod shader_closure; diff --git a/src/physics.rs b/src/physics.rs new file mode 100644 index 0000000..7fae11c --- /dev/null +++ b/src/physics.rs @@ -0,0 +1,334 @@ +use glam::{Vec2, Vec3}; +use partial_min_max::{min, max}; + +pub struct PhysicsBody { + pos: Vec3, + vel: Vec3, +} + +pub struct PhysicsResult { + body: PhysicsBody, + triangles_hit: Vec , + kill: bool, +} + +pub struct Triangle { + verts: [Vec3; 3], +} + +pub trait MeshBuffer { + fn num_triangles (&self) -> usize; + fn get_triangle (&self, i: usize) -> Triangle; +} + +fn vec_min (a: &Vec3, b: &Vec3) -> Vec3 { + Vec3::from (( + min (a.x (), b.x ()), + min (a.y (), b.y ()), + min (a.z (), b.z ()) + )) +} + +fn vec_max (a: &Vec3, b: &Vec3) -> Vec3 { + Vec3::from (( + max (a.x (), b.x ()), + max (a.y (), b.y ()), + max (a.z (), b.z ()) + )) +} + +impl Triangle { + pub fn min (&self) -> Vec3 { + self.verts [1..].iter ().fold ( + self.verts [0], + |pre, v| vec_min (&pre, v) + ) + } + + pub fn max (&self) -> Vec3 { + self.verts [1..].iter ().fold ( + self.verts [0], + |pre, v| vec_max (&pre, v) + ) + } +} + +#[derive (Clone)] +pub struct Collision { + t: f32, + p_impact: Vec3, + normal: Vec3, + i: usize, +} + +impl Collision { + pub fn take_if_closer (&self, o: &Self) -> Self { + if o.t < self.t && o.t >= 0.0 { + o.clone () + } + else { + self.clone () + } + } +} + +pub fn get_candidate (world: &MB, p0: Vec3, p1: Vec3) +-> Collision +where MB: MeshBuffer +{ + let radius = 0.0625f32; + let radius3 = Vec3::from (( + radius, + radius, + radius + )); + let v = p1 - p0; + + let mut candidate = Collision { + t: 2.0, + p_impact: Default::default (), + normal: Default::default (), + i: 0, + }; + + for i in 0..world.num_triangles () { + let tri = world.get_triangle (i); + + let tri_min = tri.min () - radius3; + let tri_max = tri.max () + radius3; + + let ray_min = min (p0, p1); + let ray_max = max (p0, p1); + + if + ray_max.x () < tri_min.x () || ray_min.x () > tri_max.x () || + ray_max.y () < tri_min.y () || ray_min.y () > tri_max.y () || + ray_max.z () < tri_min.z () || ray_min.z () > tri_max.z () + { + // AABB reject + continue; + } + + // Collision for each triangle is roughly split into: + // Face collisions + // Edge collisions + // Vertex collisions + + let normal = Vec3::cross (tri.verts [2] - tri.verts [1], tri.verts [1] - tri.verts [0]).normalize (); + + let speed_towards_face = -Vec3::dot (v, normal); + + // Face collisions + + if speed_towards_face >= 0.0 { + let distance_to_face0 = Vec3::dot (normal, p0 - tri.verts [0]) - radius; + let distance_to_face1 = Vec3::dot (normal, p1 - tri.verts [0]) - radius; + + let passed_plane = distance_to_face0 > 0.0 && distance_to_face1 < 0.0; + + if passed_plane { + let t = distance_to_face0 / (distance_to_face0 - distance_to_face1); + + // Because of previous early returns we know that 0.0 < t < 1.0 + + let p_impact = p0 * (1.0 - t) + p1 * (t); + + let impact_inside_tri = (|| { + for j in 0..3 { + let a = tri.verts [j]; + let b = tri.verts [(j + 1) % 3]; + let tangent = Vec3::cross (b - a, normal); + + if Vec3::dot (tangent, p_impact - a) < 0.0 { + return false; + } + } + true + })(); + + if impact_inside_tri { + // Stop it + + let c = Collision { + t, + p_impact, + normal, + i, + }; + + candidate = candidate.take_if_closer (&c); + + // goto triangle_collided + } + } + } + + // Edge collisions + + let ray_dir = (p1 - p0).normalize (); + + for j in 0..3 { + let a = tri.verts [j]; + let b = tri.verts [(j + 1) % 3]; + + let cylinder_axis = (b - a).normalize (); + let third_axis = Vec3::cross (cylinder_axis, ray_dir).normalize (); + let perp_ray_axis = Vec3::cross (third_axis, cylinder_axis); + + let into_triangle = Vec3::cross (cylinder_axis, normal); + + let a_minus_p0 = a - p0; + + let a_ray = Vec2::from (( + Vec3::dot (a_minus_p0, perp_ray_axis), + Vec3::dot (a_minus_p0, third_axis) + )); + + // a_ray is now in a space where X is the ray's t, + // Z is the cylinder axis (and irrelevant for now), + // and Y is the cross of those. + + // I forgot the maths word for this + let discriminant = radius * radius - a_ray.y () * a_ray.y (); + + if discriminant < 0.0 { + // No possible collision + continue; + } + + let t = (a_ray.x () - discriminant.sqrt ()) / (p1 - p0).length (); + + if t < 0.0 || t > 1.0 { + // The cylinder is along the line, + // but outside the line segment + //triangles_hit.push_back (i); + continue; + } + + let p_impact = p0 * (1.0 - t) + p1 * (t); + + let impact_along_cylinder = Vec3::dot (cylinder_axis, p_impact); + let a_along_cylinder = Vec3::dot (cylinder_axis, a); + + if impact_along_cylinder < a_along_cylinder || impact_along_cylinder > Vec3::dot (cylinder_axis, b) + { + // The infinite cylinder is on the line segment, + // but the finite cylinder is not. + //cout << "Finite cylinder reject" << endl; + continue; + } + + let edge_normal = (p_impact - (a + (impact_along_cylinder - a_along_cylinder) * cylinder_axis)).normalize (); + + let speed_towards_edge = -Vec3::dot (v, edge_normal); + let speed_into_triangle = Vec3::dot (v, into_triangle); + + if speed_towards_edge > 0.0 && speed_into_triangle >= 0.0 { + let c = Collision { + t, + p_impact, + normal: edge_normal, + i, + }; + + candidate = candidate.take_if_closer (&c); + } + } + + // Vertex collisions + + for j in 0..3 { + let a = tri.verts [j]; + let a_minus_p0 = a - p0; + + let a_ray_x = Vec3::dot (a_minus_p0, ray_dir); + let nearest_on_ray = p0 + a_ray_x * ray_dir; + let a_ray_y = (a - nearest_on_ray).length (); + + let discriminant = radius * radius - a_ray_y * a_ray_y; + + if discriminant < 0.0 { + // The sphere is not on the line + continue; + } + + let t = (a_ray_x - discriminant.sqrt ()) / (p1 - p0).length (); + + if t < 0.0 || t > 1.0 { + // The sphere is along the line, + // but outside the line segment + continue; + } + + let p_impact = p0 * (1.0 - t) + p1 * (t); + let vert_normal = (p_impact - a).normalize (); + + // I skip the speed check here cause I'm pretty sure + // the previous work makes it redundant + + let c = Collision { + t, + p_impact, + normal: vert_normal, + i, + }; + + candidate = candidate.take_if_closer (&c); + } + } + + candidate +} + +pub fn physics_step (input: &PhysicsBody, world: &MB) +-> PhysicsResult +where MB: MeshBuffer +{ + let dt = 1.0 / 60.0; + let z = Vec3::from ((0.0, 0.0, 1.0)); + let gravity = z * -16.0 * dt; + let margin = 0.00125; + + let mut t_remaining = 1.0; + + let mut old_pos = input.pos; + let mut new_vel = input.vel + gravity; + let mut new_pos = old_pos + new_vel * dt * t_remaining; + + let mut triangles_hit = Vec::new (); + + for i in 0..5 { + let candidate = get_candidate (world, old_pos, new_pos); + + if candidate.t <= 1.0 { + t_remaining *= 1.0 - candidate.t; + let speed_towards_normal = -Vec3::dot (new_vel, candidate.normal); + + let push_out_pos = candidate.p_impact + candidate.normal * margin; + + // Rewind the object to when it hit the margin + let speed = new_vel.length (); + let dir = new_vel / speed; + old_pos = candidate.p_impact + dir * margin / Vec3::dot (candidate.normal, dir); + + // Push the object out of the triangle along the normal + new_vel += candidate.normal * speed_towards_normal; + // But also compensate for the slide distance it lost + new_pos = push_out_pos + new_vel * dt * t_remaining; + } + else { + t_remaining = 0.0; + old_pos = new_pos; + break; + } + } + + PhysicsResult { + body: PhysicsBody { + pos: old_pos, + vel: new_vel, + }, + triangles_hit, + kill: false, + } +}