diff --git a/src/bin/platformer.rs b/src/bin/platformer.rs index 8cf36d9..aa19973 100644 --- a/src/bin/platformer.rs +++ b/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) diff --git a/src/physics.rs b/src/physics.rs index 706c91b..fd313cf 100644 --- a/src/physics.rs +++ b/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) --> 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, - }; - - 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; +#[derive (Clone, Copy, Debug, PartialEq)] +pub struct PrimCollision { + t: f32, + p_impact: Vec3, + normal: Vec3, +} + +impl PrimCollision { + pub fn take_if_closer (&self, o: &Self) -> Self { + if o.t < self.t && o.t >= 0.0 { + o.clone () } - - // 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 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); + else { + self.clone () } } - - 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 +{ + 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 +{ + 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 +{ + 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 (¶ms, &world, radius, &body_before); - 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.pos.distance_squared (e.body.pos) < 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); }