335 lines
7.4 KiB
Rust
335 lines
7.4 KiB
Rust
|
use glam::{Vec2, Vec3};
|
||
|
use partial_min_max::{min, max};
|
||
|
|
||
|
pub struct PhysicsBody {
|
||
|
pos: Vec3,
|
||
|
vel: Vec3,
|
||
|
}
|
||
|
|
||
|
pub struct PhysicsResult {
|
||
|
body: PhysicsBody,
|
||
|
triangles_hit: Vec <usize>,
|
||
|
kill: bool,
|
||
|
}
|
||
|
|
||
|
pub struct Triangle {
|
||
|
verts: [Vec3; 3],
|
||
|
}
|
||
|
|
||
|
pub trait MeshBuffer {
|
||
|
fn num_triangles (&self) -> usize;
|
||
|
fn get_triangle (&self, i: usize) -> Triangle;
|
||
|
}
|
||
|
|
||
|
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)]
|
||
|
pub struct Collision {
|
||
|
t: f32,
|
||
|
p_impact: Vec3,
|
||
|
normal: Vec3,
|
||
|
i: usize,
|
||
|
}
|
||
|
|
||
|
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 ()
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
pub fn get_candidate <MB> (world: &MB, p0: Vec3, p1: Vec3)
|
||
|
-> Collision
|
||
|
where MB: MeshBuffer
|
||
|
{
|
||
|
let radius = 0.0625f32;
|
||
|
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 in 0..world.num_triangles () {
|
||
|
let tri = world.get_triangle (i);
|
||
|
|
||
|
let tri_min = tri.min () - radius3;
|
||
|
let tri_max = tri.max () + radius3;
|
||
|
|
||
|
let ray_min = min (p0, p1);
|
||
|
let ray_max = max (p0, 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;
|
||
|
}
|
||
|
|
||
|
// 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 t = distance_to_face0 / (distance_to_face0 - distance_to_face1);
|
||
|
|
||
|
// Because of previous early returns we know that 0.0 < t < 1.0
|
||
|
|
||
|
let p_impact = p0 * (1.0 - t) + p1 * (t);
|
||
|
|
||
|
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 - a) < 0.0 {
|
||
|
return false;
|
||
|
}
|
||
|
}
|
||
|
true
|
||
|
})();
|
||
|
|
||
|
if impact_inside_tri {
|
||
|
// Stop it
|
||
|
|
||
|
let c = Collision {
|
||
|
t,
|
||
|
p_impact,
|
||
|
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 t = (a_ray.x () - discriminant.sqrt ()) / (p1 - p0).length ();
|
||
|
|
||
|
if t < 0.0 || t > 1.0 {
|
||
|
// 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 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,
|
||
|
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
|
||
|
}
|
||
|
|
||
|
pub fn physics_step <MB> (input: &PhysicsBody, world: &MB)
|
||
|
-> PhysicsResult
|
||
|
where MB: MeshBuffer
|
||
|
{
|
||
|
let dt = 1.0 / 60.0;
|
||
|
let z = Vec3::from ((0.0, 0.0, 1.0));
|
||
|
let gravity = z * -16.0 * dt;
|
||
|
let margin = 0.00125;
|
||
|
|
||
|
let mut t_remaining = 1.0;
|
||
|
|
||
|
let mut old_pos = input.pos;
|
||
|
let mut new_vel = input.vel + gravity;
|
||
|
let mut new_pos = old_pos + new_vel * dt * t_remaining;
|
||
|
|
||
|
let mut triangles_hit = Vec::new ();
|
||
|
|
||
|
for i in 0..5 {
|
||
|
let candidate = get_candidate (world, old_pos, new_pos);
|
||
|
|
||
|
if candidate.t <= 1.0 {
|
||
|
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;
|
||
|
}
|
||
|
else {
|
||
|
t_remaining = 0.0;
|
||
|
old_pos = new_pos;
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
PhysicsResult {
|
||
|
body: PhysicsBody {
|
||
|
pos: old_pos,
|
||
|
vel: new_vel,
|
||
|
},
|
||
|
triangles_hit,
|
||
|
kill: false,
|
||
|
}
|
||
|
}
|