Convex polygon-polygon swept collision test

Question

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>>,
}

Answer 1

I have found a significant optimisation, in taking out all of the extra code I had added to the basic concept of the algorithm.

I'm not entirely satisfied with this gain, as it still takes ~80ns for the 6 side-4 side test described. I have also identified a bug in the original code and modified it accordingly, though it's performance was relatively unchanged in the 6-4 test case.

The new code:

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
   
   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];
      if n.dot(rv2) >= 0.0 { continue; }
      let v = -p1.verts[p1vi];

      let diff_verts = p1.verts[p1vi+1] + v;
      let dot = rv2.perp_dot(diff_verts);
      let dd = dot * dot;
      for p2vi in 0..p2.norms.len() {
         let p2v = p2.verts[p2vi] + dpos + v;
         let desc = rv2.perp_dot(p2v) * dot;
         if desc >= 0.0 && desc <= dd { 
            let t = diff_verts.perp_dot(p2v) * 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 diff_verts = p2.verts[p2vi+1] + v;
      let dot = rv1.perp_dot(diff_verts);
      let dd = dot * dot;
      for p1vi in 0..p1.norms.len() {
         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
   }
}