Copy in physics functions from a 2018 project

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"

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"

src/bin/racing-game.rs

@@ -0,0 +1,4 @@
+
+fn main () {
+	println! ("Racing game.");
+}

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;

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 <usize>,
+	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 <MB> (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 <MB> (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,
+	}
+}