Add shapecast example + loosen gjk epsilon

CHANGELOG.md

@@ -22,6 +22,8 @@
 - `point_cloud_bounding_sphere` and `point_cloud_bounding_sphere_with_center` now returns a `BoundingSphere`.
 - Removed `IntersectionCompositeShapeShapeBestFirstVisitor` (which had been deprecated for a while):
   use `IntersectionCompositeShapeShapeVisitor` instead.
+- Epsilon from `parry::query::gjk::eps_tol` is now `10e-2` (from `10e-5`).
+  - This fixes cases of incorrectly failing shapecasts.
 
 ## v0.17.5
 

Cargo.toml

@@ -1,5 +1,10 @@
 [workspace]
-members = ["crates/parry2d", "crates/parry3d", "crates/parry2d-f64", "crates/parry3d-f64"]
+members = [
+    "crates/parry2d",
+    "crates/parry3d",
+    "crates/parry2d-f64",
+    "crates/parry3d-f64",
+]
 resolver = "2"
 
 [workspace.lints]
@@ -17,4 +22,4 @@ parry3d-f64 = { path = "crates/parry3d-f64" }
 
 #simba = { path = "../simba" }
 #simba = { git = "https://github.com/dimforge/simba", rev = "45a5266eb36ed9d25907e9bf9130cd4ac846a748" }
-#nalgebra = { git = "https://github.com/dimforge/nalgebra", rev = "0cf79aef0e6155befc3279a3145f1940822b8377" }
+#nalgebra = { git = "https://github.com/dimforge/nalgebra", rev = "0cf79aef0e6155befc3279a3145f1940822b8377" }

crates/parry2d/Cargo.toml

@@ -30,7 +30,7 @@ std = [
     "arrayvec/std",
     "spade",
     "thiserror",
-    "ena"
+    "ena",
 ]
 dim2 = []
 f32 = []
@@ -142,6 +142,11 @@ name = "cuboid2d"
 path = "examples/cuboid2d.rs"
 doc-scrape-examples = true
 
+[[example]]
+name = "debug_shape_cast2d"
+path = "examples/debug_shape_cast2d.rs"
+doc-scrape-examples = true
+
 [[example]]
 name = "distance_query2d"
 path = "examples/distance_query2d.rs"

crates/parry2d/examples/debug_shape_cast2d.rs

@@ -0,0 +1,188 @@
+mod common_macroquad2d;
+
+use common_macroquad2d::draw_point;
+use macroquad::prelude::*;
+use nalgebra::{Isometry2, Point2};
+use parry2d::math::Isometry;
+use parry2d::query::{self, Ray, ShapeCastOptions};
+use parry2d::shape::{Ball, ConvexPolygon, Shape};
+
+const RENDER_SCALE: f32 = 1.0;
+const BALLCAST_WIDTH: f32 = 16.0;
+
+#[macroquad::main("raycasts_animated")]
+async fn main() {
+    for _i in 1.. {
+        clear_background(BLACK);
+
+        let screen_shift = Point2::new(screen_width() / 2.0, screen_height() / 2.0);
+
+        let to_cast_against = ConvexPolygon::from_convex_polyline(
+            [
+                [-24.0, 0.0].into(),
+                [0.0, 24.0].into(),
+                [24.0, 0.0].into(),
+                [0.0, -24.0].into(),
+            ]
+            .into(),
+        )
+        .expect("Failed to create ConvexPolygon from polyline");
+        let to_cast_against_pose = Isometry2::rotation(0f32);
+
+        let mouse_pos = mouse_position();
+        let mouse_position_world =
+            (Point2::<f32>::new(mouse_pos.0, mouse_pos.1) - screen_shift.coords) / RENDER_SCALE;
+        let target_pos: Point2<f32> = [-312.0, 152.0].into();
+
+        // Those 2 fail with `min_bound >= _eps_tol`, fixed with a tolerance * 100
+        shape_cast_debug(
+            screen_shift,
+            [99.0, -33.0].into(),
+            target_pos,
+            to_cast_against.clone(),
+        );
+        shape_cast_debug(
+            screen_shift,
+            [98.0, -31.0].into(),
+            target_pos,
+            to_cast_against.clone(),
+        );
+        // This fails with `niter == 100` (and `niter == 100_000`), fixed with a tolerance * 10_000
+        shape_cast_debug(
+            screen_shift,
+            [47.0, -32.0].into(),
+            target_pos,
+            to_cast_against.clone(),
+        );
+
+        // For debug purposes, raycast to mouse position.
+        // Rendered last to be on top of the other raycasts
+        shape_cast_debug(
+            screen_shift,
+            target_pos,
+            mouse_position_world,
+            to_cast_against.clone(),
+        );
+
+        /*
+         *
+         * Render the cuboid.
+         *
+         */
+        draw_polygon(
+            &to_cast_against.points(),
+            &to_cast_against_pose,
+            RENDER_SCALE,
+            screen_shift,
+            GREEN,
+        );
+
+        next_frame().await
+    }
+}
+
+fn shape_cast_debug(
+    screen_shift: Point2<f32>,
+    source_pos: Point2<f32>,
+    target_pos: Point2<f32>,
+    to_cast_against: ConvexPolygon,
+) {
+    /*
+     *
+     * Prepare a Raycast and compute its result against the shape.
+     *
+     */
+    let ray = Ray::new(source_pos, target_pos - source_pos);
+
+    let pos1: Point2<f32> = source_pos.into();
+    let vel1 = target_pos - source_pos;
+    let g1 = Ball::new(BALLCAST_WIDTH);
+    let pos2 = [0.0, 0.0];
+    let vel2 = [0.0, 0.0];
+    let g2 = to_cast_against.clone_dyn();
+
+    let toi = query::cast_shapes(
+        &pos1.into(),
+        &vel1,
+        &g1,
+        &pos2.into(),
+        &vel2.into(),
+        &*g2,
+        ShapeCastOptions::with_max_time_of_impact(1.0),
+    )
+    .unwrap();
+
+    /*
+     *
+     * Render the raycast's result.
+     *
+     */
+    drawcircle_at(pos1, BALLCAST_WIDTH, RENDER_SCALE, screen_shift, ORANGE);
+
+    if let Some(toi) = toi {
+        if toi.time_of_impact == 0f32 {
+            draw_point(ray.origin, RENDER_SCALE, screen_shift, YELLOW);
+        } else {
+            drawline_from_to(
+                ray.origin,
+                (ray.point_at(toi.time_of_impact).coords).into(),
+                RENDER_SCALE,
+                screen_shift,
+                GREEN,
+            );
+        }
+        drawcircle_at(
+            (ray.point_at(toi.time_of_impact).coords).into(),
+            BALLCAST_WIDTH,
+            RENDER_SCALE,
+            screen_shift,
+            GREEN,
+        );
+    } else {
+        drawline_from_to(
+            ray.origin,
+            ray.origin + ray.dir,
+            RENDER_SCALE,
+            screen_shift,
+            RED,
+        );
+    }
+}
+
+fn draw_polygon(
+    polygon: &[Point2<f32>],
+    pose: &Isometry<f32>,
+    scale: f32,
+    shift: Point2<f32>,
+    color: Color,
+) {
+    for i in 0..polygon.len() {
+        let a = pose * (scale * polygon[i]);
+        let b = pose * (scale * polygon[(i + 1) % polygon.len()]);
+        draw_line(
+            a.x + shift.x,
+            a.y + shift.y,
+            b.x + shift.x,
+            b.y + shift.y,
+            2.0,
+            color,
+        );
+    }
+}
+
+fn drawline_from_to(
+    from: Point2<f32>,
+    to: Point2<f32>,
+    scale: f32,
+    shift: Point2<f32>,
+    color: Color,
+) {
+    let from = from * scale + shift.coords;
+    let to = to * scale + shift.coords;
+    draw_line(from.x, from.y, to.x, to.y, 2.0, color);
+}
+
+fn drawcircle_at(center: Point2<f32>, radius: f32, scale: f32, shift: Point2<f32>, color: Color) {
+    let center = center * scale + shift.coords;
+    draw_circle_lines(center.x, center.y, radius, 1f32, color);
+}

crates/parry2d/tests/geometry/ray_cast.rs

@@ -170,8 +170,7 @@ fn convexpoly_raycast_fuzz() {
         let ray_origin = Point2::new(3., 1. + (i as Real * 0.0001));
         let ray_look_at = Point2::new(0., 2.);
         let collision = test_raycast(ray_origin, ray_look_at);
-        let eps = 1.0e-5;
-
+        let eps = parry2d::query::gjk::eps_tol();
         match collision {
             Some(distance) if distance >= 1.0 - eps && distance < (2.0 as Real).sqrt() => (),
             Some(distance) if distance >= 2.0 => panic!(

src/query/gjk/gjk.rs

@@ -35,7 +35,7 @@ pub enum GJKResult {
 /// The absolute tolerance used by the GJK algorithm.
 pub fn eps_tol() -> Real {
     let _eps = crate::math::DEFAULT_EPSILON;
-    _eps * 10.0
+    _eps * 10_000.0
 }
 
 /// Projects the origin on the boundary of the given shape.
@@ -391,3 +391,61 @@ fn result(simplex: &VoronoiSimplex, prev: bool) -> (Point<Real>, Point<Real>) {
         res
     }
 }
+
+#[cfg(test)]
+#[cfg(feature = "dim2")]
+mod test {
+    use na::Point2;
+
+    use crate::{
+        math::Real,
+        query::{self, ShapeCastOptions},
+        shape::{Ball, ConvexPolygon, Shape},
+    };
+
+    #[test]
+    fn gjk_issue_180() {
+        let to_cast_against = ConvexPolygon::from_convex_polyline(
+            [
+                [-24.0, 0.0].into(),
+                [0.0, 24.0].into(),
+                [24.0, 0.0].into(),
+                [0.0, -24.0].into(),
+            ]
+            .into(),
+        )
+        .unwrap();
+        let target_pos: Point2<Real> = [-312.0, 152.0].into();
+
+        check_converge(&to_cast_against, [47.0, -32.0].into(), target_pos);
+        check_converge(&to_cast_against, [99.0, -33.0].into(), target_pos);
+        check_converge(&to_cast_against, [98.0, -31.0].into(), target_pos);
+    }
+
+    fn check_converge(
+        to_cast_against: &ConvexPolygon,
+        source_pos: Point2<Real>,
+        target_pos: Point2<Real>,
+    ) {
+        let vel1 = target_pos - source_pos;
+        let g1 = Ball::new(16.0);
+        let pos2 = [0.0, 0.0];
+        let vel2 = [0.0, 0.0];
+        let g2 = to_cast_against.clone_dyn();
+
+        let toi = query::cast_shapes(
+            &source_pos.into(),
+            &vel1,
+            &g1,
+            &pos2.into(),
+            &vel2.into(),
+            &*g2,
+            ShapeCastOptions::with_max_time_of_impact(1.0),
+        )
+        .unwrap();
+        assert!(
+            toi.is_some(),
+            "casting against the convex polygon should converge."
+        );
+    }
+}