Испытание на столкновение выпуклого многоугольника с развернутым полигоном

В настоящее время работает над библиотекой предварительного решения коллизий. Моя цель — сделать его потенциально применимым в моих потенциальных проектах видеоигр, поэтому временные ограничения невелики, поскольку его следует использовать в игровом цикле: этот алгоритм в настоящее время занимает слишком много времени. Я знаю о расширении фаз, но считаю, что здесь тоже можно кое-что сделать. На данный момент меня не интересует подшаговый подход.

Общее объяснение алгоритма: прокрутите каждую линию многоугольника и проверьте, является ли она разделяющей осью, если да, проверьте, указывает ли относительная скорость относительно направленной наружу нормали линии, если она есть, пропустите. Если нет, найдите ближайшую вершину другого многоугольника, затем проверьте и ее, и вершину по часовой стрелке на предмет пересечения с линией (отрезок линии от вершины относительной скорости с отрезком линии между вершинами другого многоугольника).

Это алгоритм, который я придумал после того, как не нашел полезных ресурсов в Интернете (не то чтобы их там нет), поэтому я подозреваю, что могут потребоваться большие оптимизации, о которых я недостаточно знаком с вычислительной геометрией, чтобы знать о них. Или любые другие существенные оптимизации, если на то пошло.

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
   
   // snip: bounding test for 'bullet's (uses rv1), this is independent and does not significantly impact the performance of the function, or cause 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

      // 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
            clsst_dot = proj;
            clsst_index = p2vi;
         }
      }
      if clsst_dot < v_dot { continue; } // invalid seperating axis

      // inlined line-segment intersection tests
      // test from closest vertex AND the vertex clockwise therefrom to avoid polygons 'slipping' through each other in an edge case (when the closest vertex to the polygon's separating axes fall on either side of each other, such as with the setup shown below)

      let diff_verts = p1.verts[p1vi+1] - v;
      let dot = rv2.perp_dot(diff_verts);
      let dd = dot * dot;
      let vc0 = p2.verts[clsst_index] - dpos_v;
      let u_v0 = rv2.perp_dot(vc0) * dot;
      if u_v0 >= 0.0 && u_v0 <= dd { 
         let t = diff_verts.perp_dot(vc0) * dot;
         if t >= 0.0 && t <= dd && t < immenence * dd {
            immenence = t / dd;
            imminent_norm = n;
         }
      }
      let vc1 = p2.verts[clsst_index + 1] - dpos_v;
      let u_v1 = rv2.perp_dot(vc1) * dot;
      if u_v1 >= 0.0 && u_v1 <= dd { 
         let t = diff_verts.perp_dot(vc1) * dot;
         if t >= 0.0 && t <= dd && t < immenence * dd {
            immenence = t / dd;
            imminent_norm = n;
         }
      }
   }

   // note: repeat of the above code with 1 and 2 switched (without comments)
   //       not factored out to minimise any performance penalty
   //       one instance of code duplication is less of a concern for me
   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;
      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;
         }
      }
      if clsst_dot < v_dot { continue; } // invalid seperating axis

      let diff_verts = p2.verts[p2vi+1] - v;
      let dot = rv1.perp_dot(diff_verts);
      let dd = dot * dot;
      let vc0 = p1.verts[clsst_index] - dpos_v;
      let u_v0 = rv1.perp_dot(vc0) * dot;
      if u_v0 >= 0.0 && u_v0 <= dd { 
         let t = diff_verts.perp_dot(vc0) * dot;
         if t >= 0.0 && t <= dd && t < immenence * dd {
            immenence = t / dd;
            imminent_norm = -n; // flip to make it relative to 2
         }
      }
      let vc1 = p1.verts[clsst_index + 1] - dpos_v;
      let u_v1 = rv1.perp_dot(vc1) * dot;
      if u_v1 >= 0.0 && u_v1 <= dd { 
         let t = diff_verts.perp_dot(vc1) * 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
   }
}

Надеюсь, наименования переменных не слишком запутывают. Если да, то я могу изменить их на долгие значения, но пока добавил комментарии.

Этот алгоритм, проверенный criterion-rs занимает 118 нс между 6 и 4-сторонней формой на моей машине (R3200G), я был бы более чем доволен 50 нс, и я надеюсь на менее 80 нс. Я готов до некоторой степени изменить структуры данных, если это даст значительную отдачу, но я стараюсь минимизировать искусственные ограничения кода.

Это личный проект, поддержка API не является проблемой, и я не ищу совета по стилю кода, но не стесняйтесь давать его, если хотите.

Дополнительный код, который может быть актуален:

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

// this test fails if I don't test both closest and clockwise from closest for each separating axis
#[test]
   fn test_poly_sweep() {
      // the details here don't matter too much, Poly::new does all the work to make the input data compliant to the standard specified by the inline doc for Poly, body_sweep(...) ends up calling poly_poly_swept(...) in this case
      // the importance of this test is only to demonstrate why I am segment testing from two vertices per seperating axis instead of only the closest in code, rather than words
      // currently the algorithm works as expected for all tested scenarios
      let p1 = Poly::new(&[vec2(0.0, 0.0), vec2(1.0, 1.0), vec2(1.0, 0.0), vec2(0.0, 1.0)]);
      let b1 = Body { aabb: p1.aabb, shapes: vec![Shape::Poly(p1)], pos: vec2(0.0, 0.0), vel: vec2(2.0, 1.0), bullet: false };
      let p2 = Poly::new(&[vec2(0.0, 0.0), vec2(1.0, 1.0), vec2(1.0, 0.0), vec2(0.0, 1.0)]).translate(vec2(1.0, 0.0));
      let b2 = Body { aabb: p2.aabb, shapes: vec![Shape::Poly(p2)], pos: vec2(1.0, -0.5), vel: vec2(-2.0, 0.0), bullet: false };
      
      assert_eq!(body_sweep(&b1, &b2, 1.0).is_some(), true);
   }
```