Currently working on a pre-solve collision library. My goal is to have it be potentially usable in potential video game projects of mine, so time constraint is small as this should be used in a game loop: this algorithm currently takes too long. I am aware of broadphasing, but I believe there is gains to be made here too. I'm not interested in a substepping approach for the time being.
A general explanation of the algorithm: Loop through each side of the polygon, check if relative velocity points against the line's outward-facing normal. If so, line segment-line segment test from each point of the opposite polygon along velocity against the side of the polygon. Return the shortest collision distance if any.
This is an algorithm I made up after finding no helpful resources online (not that they aren't there), so I suspect that there may be large optimisations to be made that I'm not familiar enough with computational geometry to be aware of. Or any other significant optimisations, for that matter.
use cgmath::{ElementWise, InnerSpace, Vector2}; // latest version of the cgmath crate on crates.io
fn poly_poly_sweep(b1: &Body, p1: &Poly, b2: &Body, p2: &Poly, t: f64) -> Option<(f64, Vector2<f64>)> {
let dpos = b2.pos - b1.pos;
let rv1 = (b1.vel - b2.vel).mul_element_wise(t);
// snip (ignore, ~5ns perf penalty, no side effects)
let mut immenence = f64::MAX; // time to collision
let mut imminent_norm = cgmath::vec2(0.0, 0.0); // norm of collision from 2
let rv2 = -rv1;
for p1vi in 0..p1.norms.len() {
let n = p1.norms[p1vi];
// check if normal faces a similar direction to rv2
if n.dot(rv2) >= 0.0 { continue; }
let v = p1.verts[p1vi];
let dpos_v = v - dpos;
let v_dot = n.dot(v - dpos) as f64; // seperating axis dot
let mut clsst_dot = f64::MAX; // closest vert dot
let mut clsst_index = usize::MAX; // closest vert index
let mut multi_clsst_len = 0;
// SAT, store closest vert
for p2vi in 0..p2.norms.len() {
let proj = n.dot(p2.verts[p2vi]) as f64;
if proj < clsst_dot { // closer vert found
// will store the most anticlockwise if proj is equal
clsst_dot = proj;
clsst_index = p2vi;
multi_clsst_len = 0;
} else if proj == clsst_dot { // not rounding-error safe?
multi_clsst_len += 1;
}
}
if clsst_dot < v_dot { continue; } // invalid seperating axis
// inlined line-segment intersection tests
let diff_verts = p1.verts[p1vi+1] - v;
let dot = rv2.perp_dot(diff_verts);
let dd = dot * dot;
for p2vi in clsst_index..(clsst_index + multi_clsst_len + 1) {
let vc = p2.verts[p2vi] - dpos_v;
let desc = rv2.perp_dot(vc) * dot;
if desc >= 0.0 && desc <= dd {
let t = diff_verts.perp_dot(vc) * dot;
if t >= 0.0 && t <= dd && t < immenence * dd {
immenence = t / dd;
imminent_norm = n;
}
}
}
}
for p2vi in 0..p2.norms.len() {
let n = p2.norms[p2vi];
if n.dot(rv1) >= 0.0 { continue; }
let v = p2.verts[p2vi];
let dpos_v = v - dpos;
let v_dot = n.dot(dpos_v) as f64;
let mut clsst_dot = f64::MAX;
let mut clsst_index = usize::MAX;
let mut multi_clsst_len = 0;
for p1vi in 0..p1.norms.len() {
let proj = n.dot(p1.verts[p1vi]) as f64;
if proj < clsst_dot {
clsst_dot = proj;
clsst_index = p1vi;
multi_clsst_len = 0;
} else if proj == clsst_dot {
multi_clsst_len += 1;
}
}
if clsst_dot < v_dot { continue; }
let diff_verts = p2.verts[p2vi+1] - v;
let dot = rv1.perp_dot(diff_verts);
let dd = dot * dot;
for p1vi in clsst_index..(clsst_index + multi_clsst_len + 1) {
let vc = p1.verts[p1vi] - dpos_v;
let desc = rv1.perp_dot(vc) * dot;
if desc >= 0.0 && desc <= dd {
let t = diff_verts.perp_dot(vc) * dot;
if t >= 0.0 && t <= dd && t < immenence * dd {
immenence = t / dd;
imminent_norm = -n;
}
}
}
}
if immenence < 1.0 {
Some((immenence, imminent_norm))
} else {
None
}
}
This algorithm, as tested by criterion-rs
takes 96-100ns between a 6 and 4 sided polygon on my machine (R3200G), I'd be more than happy with 50ns, and am hoping for under 60-70ns. I am willing to change data structures to a certain extent if it will give significant returns, but am looking to minimize artificial restrictions on the code.
This is a personal project, API maintenance is not an issue, nor am I looking for code-style advice, but feel free to give such if you wish.
Extra code that may be relevant:
#[derive(Debug, Clone)]
pub struct Body {
/// Posistion
pub pos: Vector2<f64>,
/// Compositing shapes
pub shapes: Vec<Shape>,
/// Bounding box
pub aabb: Aabb,
/// Velocity
pub vel: Vector2<f64>,
/// Whether the object is *relatively* fast moving.
pub bullet: bool,
}
#[derive(Debug, Clone, Copy)]
pub struct Aabb {
pub min: Vector2<f64>,
pub max: Vector2<f64>,
}
/// A 2D convex polygon, vertices arranged clockwise - tailed with a duplicate of the first, with unit-length normals - without duplication.
#[derive(Debug, Clone)]
pub struct Poly {
pub aabb: Aabb,
/// First vertex's duplicate tails. `verts.len() - 1 == norms.len()`
pub verts: Vec<Vector2<f64>>,
/// Length equals actual vertex count. `verts.len() - 1 == norms.len()`
pub norms: Vec<Vector2<f64>>,
}
Vec2
andAabb
, neither are part of the Rust standard library. Also,Poly::new
is not in your currently posted code. Please add at least yourCargo.toml
dependency section as well as youruse
statements. Also keep in mind that you shouldn't strip parts of the code otherwise it may be hard for reviewers to reason about your code and review any part of it. Thanks. \$\endgroup\$