refactor physics and try to fix a bunch of stuff

src/bin/platformer.rs

@@ -127,6 +127,11 @@ impl GameGraphics {
 
 #[tokio::main]
 async fn main () -> Result <()> {
+	tracing_subscriber::fmt::fmt ()
+	.with_env_filter (tracing_subscriber::EnvFilter::from_default_env())
+	.with_span_events (tracing_subscriber::fmt::format::FmtSpan::CLOSE)
+	.init ();
+	
 	let sdl_context = sdl2::init ().map_err (|e| anyhow! ("Can't init SDL: {}", e))?;
 	let video_subsystem = sdl_context.video ().map_err (|e| anyhow! ("Can't get SDL video subsystem: {}", e))?;
 	let window = video_subsystem.window ("3D platformer", 1280, 720)

src/physics.rs

@@ -51,12 +51,13 @@ impl Triangle {
 	}
 }
 
-#[derive (Clone, Debug, PartialEq)]
+#[derive (Clone, Copy, Debug, PartialEq)]
 pub struct Collision {
 	t: f32,
 	p_impact: Vec3,
 	normal: Vec3,
 	i: usize,
+	c_type: CollisionType,
 }
 
 impl Collision {
@@ -70,212 +71,29 @@ impl Collision {
 	}
 }
 
-pub fn get_candidate (world: &[Triangle], p0: Vec3, p1: Vec3, radius: f32)
+#[derive (Clone, Copy, Debug, PartialEq)]
--> Collision
+pub struct PrimCollision {
-{
+	t: f32,
-	let radius3 = Vec3::from ((
+	p_impact: Vec3,
-		radius,
+	normal: Vec3,
-		radius,
+}
-		radius
+
-	));
+impl PrimCollision {
-	let v = p1 - p0;
+	pub fn take_if_closer (&self, o: &Self) -> Self {
-	
+		if o.t < self.t && o.t >= 0.0 {
-	let mut candidate = Collision {
+			o.clone ()
-		t: 2.0,
-		p_impact: Default::default (),
-		normal: Default::default (),
-		i: 0,
-	};
-	
-	for (i, tri) in world.iter ().enumerate () {
-		let tri_min = tri.min () - radius3;
-		let tri_max = tri.max () + radius3;
-		
-		let ray_min = p0.min (p1);
-		let ray_max = p0.max (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;
 		}
-		
+		else {
-		// Collision for each triangle is roughly split into:
+			self.clone ()
-		// 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 denom = distance_to_face0 - distance_to_face1;
-				let t_times_denom = distance_to_face0;
-				
-				// Because of previous early returns we know that 0.0 < t < 1.0
-				
-				let p_impact_times_denom = p0 * (denom - t_times_denom) + p1 * (t_times_denom);
-				
-				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_times_denom - a * denom) < 0.0 {
-							return false;
-						}
-					}
-					true
-				})();
-				
-				if impact_inside_tri {
-					// Stop it
-					
-					let c = Collision {
-						t: t_times_denom / denom,
-						p_impact: p_impact_times_denom / denom,
-						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 denom = (p1 - p0).length ();
-			let t_times_denom = a_ray.x - discriminant.sqrt ();
-			let t = t_times_denom / denom;
-			
-			if t_times_denom < 0.0 || t_times_denom > denom {
-				// The cylinder is along the line,
-				// but outside the line segment
-				//triangles_hit.push_back (i);
-				continue;
-			}
-			
-			let p_impact_times_denom = p0 * (denom - t_times_denom) + p1 * (t_times_denom);
-			let p_impact = p_impact_times_denom / denom;
-			
-			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: t_times_denom / denom,
-					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
+
+#[derive (Clone, Copy, Debug, PartialEq)]
+enum CollisionType {
+	Face,
+	Edge,
+	Vert,
 }
 
 pub struct Params {
@@ -305,6 +123,7 @@ pub fn step (
 		let candidate = get_candidate (world, old_pos, new_pos, radius);
 		
 		if candidate.t <= 1.0 {
+			tracing::debug! ("Tri {}, type {:?}", candidate.i, candidate.c_type);
 			t_remaining *= 1.0 - candidate.t;
 			let speed_towards_normal = -Vec3::dot (new_vel, candidate.normal);
 			
@@ -339,6 +158,246 @@ pub fn step (
 	}
 }
 
+pub fn get_candidate (world: &[Triangle], p0: Vec3, p1: Vec3, radius: f32)
+-> Collision
+{
+	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,
+		c_type: CollisionType::Face,
+	};
+	
+	for (i, tri) in world.iter ().enumerate () {
+		let tri_min = tri.min () - radius3;
+		let tri_max = tri.max () + radius3;
+		
+		let ray_min = p0.min (p1);
+		let ray_max = p0.max (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
+			// tracing::trace! ("AABB reject");
+			continue;
+		}
+		
+		// Collision for each triangle is roughly split into:
+		// Face collisions
+		// Edge collisions
+		// Vertex collisions
+		
+		if let Some (c) = get_candidate_face (&tri, p0, p1, radius) {
+			candidate = candidate.take_if_closer (&Collision {
+				t: c.t,
+				p_impact: c.p_impact,
+				normal: c.normal,
+				i,
+				c_type: CollisionType::Face,
+			});
+		}
+		
+		// Edge collisions
+		
+		for j in 0..3 {
+			let a = tri.verts [j];
+			let b = tri.verts [(j + 1) % 3];
+			
+			if let Some (c) = get_candidate_edge (a, b, p0, p1, radius) {
+				candidate = candidate.take_if_closer (&Collision {
+					t: c.t,
+					p_impact: c.p_impact,
+					normal: c.normal,
+					i,
+					c_type: CollisionType::Edge,
+				});
+			}
+		}
+		
+		// Vertex collisions
+		
+		for j in 0..3 {
+			let a = tri.verts [j];
+			
+			if let Some (c) = get_candidate_vert (a, p0, p1, radius) {
+				candidate = candidate.take_if_closer (&Collision {
+					t: c.t,
+					p_impact: c.p_impact,
+					normal: c.normal,
+					i,
+					c_type: CollisionType::Vert,
+				});
+			}
+		}
+	}
+	
+	candidate
+}
+
+fn get_candidate_face (tri: &Triangle, p0: Vec3, p1: Vec3, radius: f32) 
+-> Option <PrimCollision> 
+{
+	let radius3 = Vec3::from ((
+		radius,
+		radius,
+		radius
+	));
+	let v = p1 - p0;
+	
+	let normal = Vec3::cross (tri.verts [2] - tri.verts [1], tri.verts [1] - tri.verts [0]).normalize ();
+	
+	let distance_to_face0 = Vec3::dot (normal, p0 - tri.verts [0]) - radius;
+	let distance_to_face1 = Vec3::dot (normal, p1 - tri.verts [0]) - radius;
+	
+	if distance_to_face0 < 0.0 || distance_to_face1 > 0.0 {
+		tracing::trace! ("passed_plane {} {}", distance_to_face0, distance_to_face1);
+		return None;
+	}
+	
+	let denom = distance_to_face0 - distance_to_face1;
+	let t_times_denom = distance_to_face0;
+	
+	// Because of previous early returns we know that 0.0 < t < 1.0
+	
+	let p_impact_times_denom = p0 * (denom - t_times_denom) + p1 * (t_times_denom);
+	
+	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_times_denom - a * denom) < 0.0 {
+			// tracing::trace! ("impact_inside_tri");
+			return None;
+		}
+	}
+	
+	Some (PrimCollision {
+		t: t_times_denom / denom,
+		p_impact: p_impact_times_denom / denom,
+		normal,
+	})
+}
+
+fn get_candidate_edge (a: Vec3, b: Vec3, p0: Vec3, p1: Vec3, radius: f32) 
+-> Option <PrimCollision> 
+{
+	let cylinder_axis = (b - a).normalize ();
+	let third_axis = Vec3::cross (cylinder_axis, p1 - p0).normalize ();
+	let perp_ray_axis = Vec3::cross (third_axis, cylinder_axis);
+	
+	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
+		// println! ("No possible collision");
+		return None;
+	}
+	
+	let denom = (p1 - p0).length ();
+	let t_times_denom = a_ray.x - discriminant.sqrt ();
+	let t = t_times_denom / denom;
+	
+	if t < 0.0 || t > 1.0 {
+		// The cylinder is along the line,
+		// but outside the line segment
+		//triangles_hit.push_back (i);
+		// tracing::trace! ("Cylinder is along line but outside line segment {}", t);
+		return None;
+	}
+	
+	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.
+		// tracing::trace! ("Finite cylinder reject");
+		return None;
+	}
+	
+	let edge_normal = (p_impact - (a + (impact_along_cylinder - a_along_cylinder) * cylinder_axis)).normalize ();
+	
+	//let into_triangle = Vec3::cross (cylinder_axis, normal);
+	let speed_towards_edge = -Vec3::dot (p1 - p0, edge_normal);
+	//let speed_into_triangle = Vec3::dot (p1 - p0, into_triangle);
+	
+	if ! (speed_towards_edge > 0.0 /*&& speed_into_triangle >= 0.0*/) {
+		// tracing::trace! ("speeds are wrong");
+		return None;
+	}
+	
+	Some (PrimCollision {
+		t,
+		p_impact,
+		normal: edge_normal,
+	})
+}
+
+fn get_candidate_vert (a: Vec3, p0: Vec3, p1: Vec3, radius: f32) 
+-> Option <PrimCollision> 
+{
+	let a_minus_p0 = a - p0;
+	
+	let ray_dir = (p1 - p0).normalize ();
+	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
+		return None;
+	}
+	
+	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
+		return None;
+	}
+	
+	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
+	
+	Some (PrimCollision {
+		t,
+		p_impact,
+		normal: vert_normal,
+	})
+}
+
 #[cfg (test)]
 mod test {
 	use super::*;
@@ -449,8 +508,8 @@ mod test {
 		] {
 			let a = step (&params, &world, radius, &body_before);
 			
-			assert! (a.body.pos.distance_squared (e.body.pos) <= 0.00125);
+			assert! (a.body.pos.distance_squared (e.body.pos) < 0.00125);
-			assert! (a.body.vel.distance_squared (e.body.vel) <= 0.00125);
+			assert! (a.body.vel.distance_squared (e.body.vel) < 0.00125);
 			assert_eq! (a.triangles_hit, e.triangles_hit);
 			assert_eq! (a.kill, e.kill);
 		}