5
\$\begingroup\$

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>>,
}
\$\endgroup\$
2
  • 2
    \$\begingroup\$ Welcome to Code Review. There's some content missing. You seem to be using a 2d library that contains Vec2 and Aabb, neither are part of the Rust standard library. Also, Poly::new is not in your currently posted code. Please add at least your Cargo.toml dependency section as well as your use 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\$
    – Zeta
    Apr 22, 2021 at 19:38
  • 1
    \$\begingroup\$ @Zeta I have included some omitted code, and clarified some other bits. The implementation of Poly::new is not relevant, the documentation for Poly describes the resulting arrangement of data. (I'm trying not to bloat this post with too much code, especially if it's not necessary to review). The library I'm using is cgmath, I have included that in a use statement. Thank you for your suggestions, I hope these amendments are good enough. \$\endgroup\$
    – sfbea
    Apr 22, 2021 at 20:13

1 Answer 1

1
\$\begingroup\$

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
   }
}
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.