QueryOptions parameters (GjkOptions)

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,195 @@
+mod common_macroquad2d;
+
+use common_macroquad2d::draw_point;
+use macroquad::prelude::*;
+use nalgebra::{Isometry2, Point2};
+use parry2d::math::{self, Isometry};
+use parry2d::query::gjk::eps_tol;
+use parry2d::query::{self, DefaultQueryDispatcher, 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_with_dispatcher(
+        &pos1.into(),
+        &vel1,
+        &g1,
+        &pos2.into(),
+        &vel2.into(),
+        &*g2,
+        ShapeCastOptions::with_max_time_of_impact(1.0),
+        DefaultQueryDispatcher {
+            gjk_options: query::gjk::GjkOptions {
+                espilon_tolerance: math::DEFAULT_EPSILON * 1000f32,
+                nb_max_iterations: 100,
+            },
+        },
+    )
+    .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/epa2.rs

@@ -28,7 +28,7 @@ fn cuboids_large_size_ratio_issue_181() {
 
     let pos_b = Isometry2::new(Vector2::new(5.0, 0.0), 1.5);
 
-    let dispatcher = DefaultQueryDispatcher;
+    let dispatcher = DefaultQueryDispatcher::default();
     let mut p = Vector2::new(0.0, 0.0);
     let mut angle = 0.0;
 

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/closest_points/closest_points_shape_shape.rs

@@ -14,7 +14,7 @@ pub fn closest_points(
     max_dist: Real,
 ) -> Result<ClosestPoints, Unsupported> {
     let pos12 = pos1.inv_mul(pos2);
-    DefaultQueryDispatcher
+    DefaultQueryDispatcher::default()
         .closest_points(&pos12, g1, g2, max_dist)
         .map(|res| res.transform_by(pos1, pos2))
 }

src/query/closest_points/closest_points_support_map_support_map.rs

@@ -1,5 +1,5 @@
 use crate::math::{Isometry, Real, Vector};
-use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex};
+use crate::query::gjk::{self, CSOPoint, GJKResult, GjkOptions, VoronoiSimplex};
 use crate::query::ClosestPoints;
 use crate::shape::SupportMap;
 
@@ -11,6 +11,7 @@ pub fn closest_points_support_map_support_map<G1, G2>(
     g1: &G1,
     g2: &G2,
     prediction: Real,
+    options: &GjkOptions,
 ) -> ClosestPoints
 where
     G1: ?Sized + SupportMap,
@@ -23,6 +24,7 @@ where
         prediction,
         &mut VoronoiSimplex::new(),
         None,
+        options,
     ) {
         GJKResult::ClosestPoints(pt1, pt2, _) => {
             ClosestPoints::WithinMargin(pt1, pos12.inverse_transform_point(&pt2))
@@ -43,6 +45,7 @@ pub fn closest_points_support_map_support_map_with_params<G1, G2>(
     prediction: Real,
     simplex: &mut VoronoiSimplex,
     init_dir: Option<Vector<Real>>,
+    options: &GjkOptions,
 ) -> GJKResult
 where
     G1: ?Sized + SupportMap,
@@ -65,5 +68,5 @@ where
         ));
     }
 
-    gjk::closest_points(pos12, g1, g2, prediction, true, simplex)
+    gjk::closest_points(pos12, g1, g2, prediction, true, simplex, options)
 }

src/query/contact/contact_shape_shape.rs

@@ -14,7 +14,7 @@ pub fn contact(
     prediction: Real,
 ) -> Result<Option<Contact>, Unsupported> {
     let pos12 = pos1.inv_mul(pos2);
-    let mut result = DefaultQueryDispatcher.contact(&pos12, g1, g2, prediction);
+    let mut result = DefaultQueryDispatcher::default().contact(&pos12, g1, g2, prediction);
 
     if let Ok(Some(contact)) = &mut result {
         contact.transform_by_mut(pos1, pos2);

src/query/contact/contact_support_map_support_map.rs

@@ -1,6 +1,6 @@
 use crate::math::{Isometry, Real, Vector};
 use crate::query::epa::EPA;
-use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex};
+use crate::query::gjk::{self, CSOPoint, GJKResult, GjkOptions, VoronoiSimplex};
 use crate::query::Contact;
 use crate::shape::SupportMap;
 
@@ -12,13 +12,22 @@ pub fn contact_support_map_support_map<G1, G2>(
     g1: &G1,
     g2: &G2,
     prediction: Real,
+    gjk_options: &GjkOptions,
 ) -> Option<Contact>
 where
     G1: ?Sized + SupportMap,
     G2: ?Sized + SupportMap,
 {
     let simplex = &mut VoronoiSimplex::new();
-    match contact_support_map_support_map_with_params(pos12, g1, g2, prediction, simplex, None) {
+    match contact_support_map_support_map_with_params(
+        pos12,
+        g1,
+        g2,
+        prediction,
+        simplex,
+        None,
+        gjk_options,
+    ) {
         GJKResult::ClosestPoints(point1, point2_1, normal1) => {
             let dist = (point2_1 - point1).dot(&normal1);
             let point2 = pos12.inverse_transform_point(&point2_1);
@@ -44,6 +53,7 @@ pub fn contact_support_map_support_map_with_params<G1, G2>(
     prediction: Real,
     simplex: &mut VoronoiSimplex,
     init_dir: Option<Unit<Vector<Real>>>,
+    gjk_options: &GjkOptions,
 ) -> GJKResult
 where
     G1: ?Sized + SupportMap,
@@ -61,7 +71,7 @@ where
 
     simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir));
 
-    let cpts = gjk::closest_points(pos12, g1, g2, prediction, true, simplex);
+    let cpts = gjk::closest_points(pos12, g1, g2, prediction, true, simplex, gjk_options);
     if cpts != GJKResult::Intersection {
         return cpts;
     }