diff --git a/src/physics.rs b/src/physics.rs index da97c33..886abc9 100644 --- a/src/physics.rs +++ b/src/physics.rs @@ -191,21 +191,24 @@ pub fn get_candidate (world: &[Triangle], p0: Vec3, p1: Vec3, radius: f32) continue; } - let t = (a_ray.x - discriminant.sqrt ()) / (p1 - p0).length (); + let denom = (p1 - p0).length (); + let t_times_denom = a_ray.x - discriminant.sqrt (); - if t < 0.0 || t > 1.0 { + 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 = p0 * (1.0 - t) + p1 * (t); + 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 impact_along_cylinder_times_denom = Vec3::dot (cylinder_axis, p_impact_times_denom); + let impact_along_cylinder = impact_along_cylinder_times_denom / denom; let a_along_cylinder = Vec3::dot (cylinder_axis, a); - if impact_along_cylinder < a_along_cylinder || impact_along_cylinder > Vec3::dot (cylinder_axis, b) + if impact_along_cylinder_times_denom < a_along_cylinder * denom || impact_along_cylinder_times_denom > Vec3::dot (cylinder_axis, b) * denom { // The infinite cylinder is on the line segment, // but the finite cylinder is not. @@ -220,7 +223,7 @@ pub fn get_candidate (world: &[Triangle], p0: Vec3, p1: Vec3, radius: f32) if speed_towards_edge > 0.0 && speed_into_triangle >= 0.0 { let c = Collision { - t, + t: t_times_denom / denom, p_impact, normal: edge_normal, i, @@ -446,8 +449,8 @@ mod test { ] { let a = physics_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); }