687 lines
15 KiB
Rust
687 lines
15 KiB
Rust
use glam::{Vec2, Vec3};
|
|
use partial_min_max::{min, max};
|
|
|
|
#[derive (Debug, Default, PartialEq)]
|
|
pub struct PhysicsBody {
|
|
pub pos: Vec3,
|
|
pub vel: Vec3,
|
|
}
|
|
|
|
#[derive (Debug, PartialEq)]
|
|
pub struct PhysicsResult {
|
|
pub body: PhysicsBody,
|
|
pub normals_hit: Vec <Vec3>,
|
|
pub kill: bool,
|
|
}
|
|
|
|
#[derive (Copy, Clone)]
|
|
pub struct Triangle {
|
|
pub verts: [Vec3; 3],
|
|
}
|
|
|
|
#[derive (Copy, Clone)]
|
|
pub struct Aabb {
|
|
pub min: Vec3,
|
|
pub max: Vec3,
|
|
}
|
|
|
|
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, Copy, Debug, PartialEq)]
|
|
pub struct Collision {
|
|
pub t: f32,
|
|
pub p_impact: Vec3,
|
|
normal: Vec3,
|
|
i: usize,
|
|
c_type: CollisionType,
|
|
}
|
|
|
|
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 ()
|
|
}
|
|
}
|
|
}
|
|
|
|
#[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 ()
|
|
}
|
|
else {
|
|
self.clone ()
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive (Clone, Copy, Debug, PartialEq)]
|
|
enum CollisionType {
|
|
Face,
|
|
Edge,
|
|
Vert,
|
|
}
|
|
|
|
pub struct Params {
|
|
pub dt: f32,
|
|
pub gravity: Vec3,
|
|
pub margin: f32,
|
|
}
|
|
|
|
pub fn step (
|
|
params: &Params,
|
|
tris: &[Triangle], aabbs: &[Aabb],
|
|
radius: f32, input: &PhysicsBody,
|
|
) -> PhysicsResult
|
|
{
|
|
let margin = params.margin;
|
|
let dt = params.dt;
|
|
|
|
let mut t_remaining = 1.0;
|
|
|
|
let mut old_pos = input.pos;
|
|
let mut new_vel = input.vel + params.gravity;
|
|
let mut new_pos = old_pos + new_vel * dt * t_remaining;
|
|
|
|
let mut normals_hit = Vec::new ();
|
|
|
|
// Do 5 iterations of the sub-step, trying to converge on a valid state
|
|
for _ in 0..5 {
|
|
let candidate = get_candidate (tris, aabbs, 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);
|
|
|
|
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;
|
|
|
|
normals_hit.push (candidate.normal);
|
|
}
|
|
else {
|
|
t_remaining = 0.0;
|
|
old_pos = new_pos;
|
|
break;
|
|
}
|
|
}
|
|
|
|
PhysicsResult {
|
|
body: PhysicsBody {
|
|
pos: old_pos,
|
|
vel: new_vel,
|
|
},
|
|
normals_hit,
|
|
kill: false,
|
|
}
|
|
}
|
|
|
|
pub fn get_candidate (
|
|
tris: &[Triangle],
|
|
aabbs: &[Aabb],
|
|
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,
|
|
};
|
|
|
|
let ray_min = p0.min (p1) - radius3;
|
|
let ray_max = p0.max (p1) + radius3;
|
|
|
|
for b in aabbs {
|
|
if
|
|
ray_max.x < b.min.x || ray_min.x > b.max.x ||
|
|
ray_max.y < b.min.y || ray_min.y > b.max.y ||
|
|
ray_max.z < b.min.z || ray_min.z > b.max.z
|
|
{
|
|
// AABB reject
|
|
// tracing::trace! ("AABB reject");
|
|
continue;
|
|
}
|
|
|
|
let verts = [
|
|
(b.min.x, b.min.y, b.min.z).into (),
|
|
(b.max.x, b.min.y, b.min.z).into (),
|
|
(b.max.x, b.max.y, b.min.z).into (),
|
|
(b.min.x, b.max.y, b.min.z).into (),
|
|
|
|
(b.min.x, b.min.y, b.max.z).into (),
|
|
(b.max.x, b.min.y, b.max.z).into (),
|
|
(b.max.x, b.max.y, b.max.z).into (),
|
|
(b.min.x, b.max.y, b.max.z).into (),
|
|
];
|
|
|
|
for (a, b, c, d) in [
|
|
(0, 1, 2, 3),
|
|
(4, 7, 6, 5),
|
|
|
|
(2, 1, 5, 6),
|
|
(3, 2, 6, 7),
|
|
|
|
(0, 3, 7, 4),
|
|
(1, 0, 4, 5),
|
|
] {
|
|
let a = verts [a];
|
|
let b = verts [b];
|
|
let c = verts [c];
|
|
let d = verts [d];
|
|
|
|
let normal = Vec3::cross (c - b, b - a).normalize ();
|
|
|
|
if let Some (c) = get_candidate_face (&[a, b, c, d], normal, p0, p1, radius) {
|
|
candidate = candidate.take_if_closer (&Collision {
|
|
t: c.t,
|
|
p_impact: c.p_impact,
|
|
normal: c.normal,
|
|
i: 0,
|
|
c_type: CollisionType::Face,
|
|
});
|
|
}
|
|
}
|
|
|
|
for (a, b) in [
|
|
(0, 1),
|
|
(1, 2),
|
|
(2, 3),
|
|
(3, 0),
|
|
|
|
(4, 5),
|
|
(5, 6),
|
|
(6, 7),
|
|
(7, 4),
|
|
|
|
(0, 4),
|
|
(1, 5),
|
|
(2, 6),
|
|
(3, 7),
|
|
] {
|
|
let a = verts [a];
|
|
let b = verts [b];
|
|
|
|
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: 0,
|
|
c_type: CollisionType::Edge,
|
|
});
|
|
}
|
|
}
|
|
|
|
for vert in &verts {
|
|
if let Some (c) = get_candidate_vert (*vert, p0, p1, radius) {
|
|
candidate = candidate.take_if_closer (&Collision {
|
|
t: c.t,
|
|
p_impact: c.p_impact,
|
|
normal: c.normal,
|
|
i: 0,
|
|
c_type: CollisionType::Vert,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
|
|
for (i, tri) in tris.iter ().enumerate () {
|
|
let tri_min = tri.min ();
|
|
let tri_max = tri.max ();
|
|
|
|
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
|
|
|
|
let normal = Vec3::cross (tri.verts [2] - tri.verts [1], tri.verts [1] - tri.verts [0]).normalize ();
|
|
|
|
if let Some (c) = get_candidate_face (&tri.verts, normal, 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
|
|
}
|
|
|
|
/// Collide a ray with a convex planar face, like a triangle or a rectangle.
|
|
|
|
fn get_candidate_face (verts: &[Vec3], normal: Vec3, p0: Vec3, p1: Vec3, radius: f32)
|
|
-> Option <PrimCollision>
|
|
{
|
|
let radius3 = Vec3::from ((
|
|
radius,
|
|
radius,
|
|
radius
|
|
));
|
|
let v = p1 - p0;
|
|
|
|
let distance_to_face0 = Vec3::dot (normal, p0 - verts [0]) - radius;
|
|
let distance_to_face1 = Vec3::dot (normal, p1 - 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..verts.len () {
|
|
let a = verts [j];
|
|
let b = verts [(j + 1) % verts.len ()];
|
|
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::*;
|
|
|
|
#[test]
|
|
fn test_physics () {
|
|
// Remember, Z is up
|
|
|
|
let world: Vec <_> = vec! [
|
|
(
|
|
(0.0, 0.0, 0.0),
|
|
(0.0, 2.0, 0.0),
|
|
(2.0, 0.0, 0.0),
|
|
),
|
|
].into_iter ()
|
|
.map (|(v0, v1, v2)| {
|
|
Triangle {
|
|
verts: [
|
|
v0.into (),
|
|
v1.into (),
|
|
v2.into (),
|
|
],
|
|
}
|
|
})
|
|
.collect ();
|
|
|
|
let params = Params {
|
|
dt: 1.0,
|
|
gravity: (0.0, 0.0, 0.0).into (),
|
|
margin: 0.00125,
|
|
};
|
|
|
|
let magic_0 = f32::sqrt (f32::powf (0.5 + params.margin, 2.0) / 2.0);
|
|
|
|
for ((radius, body_before), e) in [
|
|
// Ray striking triangle from above, stops at triangle
|
|
(
|
|
(
|
|
0.0,
|
|
PhysicsBody {
|
|
pos: (0.5, 0.5, 0.5).into (),
|
|
vel: (0.0, 0.0, -1.0).into (),
|
|
},
|
|
),
|
|
PhysicsResult {
|
|
body: PhysicsBody {
|
|
pos: (0.5, 0.5, params.margin).into (),
|
|
vel: (0.0, 0.0, 0.0).into (),
|
|
},
|
|
triangles_hit: vec! [0],
|
|
kill: false,
|
|
},
|
|
),
|
|
// Ball striking triangle from above, stops at ball radius
|
|
(
|
|
(
|
|
0.5,
|
|
PhysicsBody {
|
|
pos: (0.5, 0.5, 2.0).into (),
|
|
vel: (0.0, 0.0, -2.0).into (),
|
|
},
|
|
),
|
|
PhysicsResult {
|
|
body: PhysicsBody {
|
|
pos: (0.5, 0.5, params.margin + 0.5).into (),
|
|
vel: (0.0, 0.0, 0.0).into (),
|
|
},
|
|
triangles_hit: vec! [0],
|
|
kill: false,
|
|
},
|
|
),
|
|
// Ball striking triangle on edge
|
|
(
|
|
(
|
|
0.5,
|
|
PhysicsBody {
|
|
pos: (-2.0, 1.0, 0.0).into (),
|
|
vel: (2.0, 0.0, 0.0).into (),
|
|
},
|
|
),
|
|
PhysicsResult {
|
|
body: PhysicsBody {
|
|
pos: (-params.margin - 0.5, 1.0, 0.0).into (),
|
|
vel: (0.0, 0.0, 0.0).into (),
|
|
},
|
|
triangles_hit: vec! [0],
|
|
kill: false,
|
|
},
|
|
),
|
|
// Ball striking triangle on diagonal edge
|
|
(
|
|
(
|
|
0.5,
|
|
PhysicsBody {
|
|
pos: (2.0, 2.0, 0.0).into (),
|
|
vel: (-2.0, -2.0, 0.0).into (),
|
|
},
|
|
),
|
|
PhysicsResult {
|
|
body: PhysicsBody {
|
|
pos: (1.0 + magic_0, 1.0 + magic_0, 0.0).into (),
|
|
vel: (0.0, 0.0, 0.0).into (),
|
|
},
|
|
triangles_hit: vec! [0],
|
|
kill: false,
|
|
},
|
|
),
|
|
] {
|
|
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_eq! (a.triangles_hit, e.triangles_hit);
|
|
assert_eq! (a.kill, e.kill);
|
|
}
|
|
|
|
// With no bounce, a ball should settle on a flat triangle in one
|
|
// frame and reach a steady state
|
|
|
|
let params = Params {
|
|
dt: 1.0,
|
|
gravity: (0.0, 0.0, -1.0).into (),
|
|
margin: 0.00125,
|
|
};
|
|
|
|
let radius = 0.5;
|
|
let body_before = PhysicsBody {
|
|
pos: (0.5, 0.5, 2.0).into (),
|
|
vel: (0.0, 0.0, -4.0).into (),
|
|
};
|
|
|
|
assert_eq! (
|
|
get_candidate (&world, (0.5, 0.5, 2.0).into (), (0.5, 0.5, -3.0).into (), radius),
|
|
Collision {
|
|
t: 0.3,
|
|
p_impact: (0.5, 0.5, 0.5).into (),
|
|
normal: (0.0, 0.0, 1.0).into (),
|
|
i: 0,
|
|
},
|
|
);
|
|
|
|
let a = step (¶ms, &world, radius, &body_before);
|
|
|
|
let e = PhysicsResult {
|
|
body: PhysicsBody {
|
|
pos: (0.5, 0.5, 0.5 + params.margin).into (),
|
|
vel: (0.0, 0.0, 0.0).into (),
|
|
},
|
|
triangles_hit: vec! [0],
|
|
kill: false,
|
|
};
|
|
|
|
// Fixed point should be here
|
|
// If this test passes, at least a ball can rest on a single horizontal
|
|
// triangle. If it fails, that means the ball is not resting
|
|
// (maybe the velocity is non-zero even if the position is stable)
|
|
// or the ball is going to gain energy and bounce away, or it's going
|
|
// to slide through the triangle.
|
|
|
|
let body_before = PhysicsBody {
|
|
pos: (0.5, 0.5, 0.5 + params.margin).into (),
|
|
vel: (0.0, 0.0, 0.0).into (),
|
|
};
|
|
|
|
let a = step (¶ms, &world, radius, &body_before);
|
|
|
|
let e = PhysicsResult {
|
|
body: PhysicsBody {
|
|
pos: (0.5, 0.5, 0.5 + params.margin).into (),
|
|
vel: (0.0, 0.0, 0.0).into (),
|
|
},
|
|
triangles_hit: vec! [0],
|
|
kill: false,
|
|
};
|
|
|
|
assert_eq! (a, e);
|
|
}
|
|
}
|