refactor physics and try to fix a bunch of stuff
_
parent
8e7101dccd
commit
3aeced7c2f
|
|
@ -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)
|
||||
|
|
|
|||
471
src/physics.rs
471
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 <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 (¶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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue