From bfafb69f4f6452a56b0b388d2692f80d1b98d87c Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 17 Dec 2024 11:13:49 +0100 Subject: [PATCH 01/20] heavy wip ; I could reproduce a wrong shapecast :thinking: --- Cargo.toml | 12 +- crates/parry2d/Cargo.toml | 2 +- crates/parry2d/examples/debug_shape_cast.rs | 165 ++++++++++++++++++++ 3 files changed, 176 insertions(+), 3 deletions(-) create mode 100644 crates/parry2d/examples/debug_shape_cast.rs diff --git a/Cargo.toml b/Cargo.toml index 9e08c42f..036dfb4a 100644 --- a/Cargo.toml +++ b/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] @@ -9,6 +14,9 @@ rust.unexpected_cfgs = { level = "warn", check-cfg = [ 'cfg(feature, values("wavefront"))', ] } +[workspace.lints.clippy] +std_instead_of_core = "warn" + [patch.crates-io] parry2d = { path = "crates/parry2d" } parry3d = { path = "crates/parry3d" } @@ -17,4 +25,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" } \ No newline at end of file +#nalgebra = { git = "https://github.com/dimforge/nalgebra", rev = "0cf79aef0e6155befc3279a3145f1940822b8377" } diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 0b018190..5b5ea333 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -30,7 +30,7 @@ std = [ "arrayvec/std", "spade", "thiserror", - "ena" + "ena", ] dim2 = [] f32 = [] diff --git a/crates/parry2d/examples/debug_shape_cast.rs b/crates/parry2d/examples/debug_shape_cast.rs new file mode 100644 index 00000000..fbd6c9c6 --- /dev/null +++ b/crates/parry2d/examples/debug_shape_cast.rs @@ -0,0 +1,165 @@ +mod common_macroquad2d; + +use common_macroquad2d::draw_point; +use macroquad::prelude::*; +use nalgebra::{Isometry2, Point2, UnitComplex, Vector2}; +use parry2d::math::Isometry; +use parry2d::query::{self, Ray, RayCast, ShapeCastOptions}; +use parry2d::shape::{Ball, ConvexPolygon, Cuboid, Shape, SharedShape}; + +const RENDER_SCALE: f32 = 1.0; +const BALLCAST_WIDTH: f32 = 16.0; + +#[macroquad::main("raycasts_animated")] +async fn main() { + let animation_scale = 1.4; + let animation_rotation = 0.04; + + for i in 1.. { + clear_background(BLACK); + + let screen_shift = + Point2::new(screen_width() / 2.0, screen_height() / 2.0) + Vector2::new(-300.0, -200.0); + /* + * + * Compute the scaled cuboid. + * + */ + /*let cube = + Cuboid::new(Vector2::new(2.0, 2.0) * ((i as f32 / 50.0).sin().abs() * animation_scale)); + let cube_pose = Isometry2::rotation(0.008 * i as f32);*/ + + let to_cast_against = ConvexPolygon::from_convex_polyline( + [ + [600.0, 288.0].into(), + [576.0, 312.0].into(), + [552.0, 288.0].into(), + [576.0, 264.0].into(), + ] + .into(), + ) + .expect("Failed to create ConvexPolygon from polyline"); + let to_cast_against_pose = Isometry2::rotation(0f32); + let target_pos: Point2<_> = [631.4779, 250.20587].into(); + let mouse_pos = mouse_position(); + let mouse_position_world = + (Point2::::new(mouse_pos.0, mouse_pos.1) - screen_shift.coords) / RENDER_SCALE; + let target_pos = mouse_position_world; + let pf_source_pos: Point2 = [264.0, 440.0].into(); + /* + * + * Prepare a Raycast and compute its result against the shape. + * + */ + let ray = Ray::new(target_pos, pf_source_pos - target_pos); + + //let toi = to_cast_against.cast_ray(&to_cast_against_pose, &ray, std::f32::MAX, true); + + let pos1 = target_pos; + let vel1 = pf_source_pos - target_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); + dbg!("wath"); + } 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(), + 16.0, + RENDER_SCALE, + screen_shift, + GREEN, + ); + } else { + drawline_from_to( + ray.origin, + ray.origin + ray.dir, + RENDER_SCALE, + screen_shift, + RED, + ); + } + + /* + * + * Render the cuboid. + * + */ + draw_polygon( + &to_cast_against.points(), + &to_cast_against_pose, + RENDER_SCALE, + screen_shift, + GREEN, + ); + + next_frame().await + } +} + +fn draw_polygon( + polygon: &[Point2], + pose: &Isometry, + scale: f32, + shift: Point2, + 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, + to: Point2, + scale: f32, + shift: Point2, + 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, radius: f32, scale: f32, shift: Point2, color: Color) { + let center = center * scale + shift.coords; + draw_circle_lines(center.x, center.y, radius, 1f32, color); +} From a6f6503556ad3ea02cde8a6cf80db0f47afd4c00 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 17 Dec 2024 15:18:19 +0100 Subject: [PATCH 02/20] better example with all cases shown at once --- crates/parry2d/examples/debug_shape_cast.rs | 180 +++++++++++--------- 1 file changed, 100 insertions(+), 80 deletions(-) diff --git a/crates/parry2d/examples/debug_shape_cast.rs b/crates/parry2d/examples/debug_shape_cast.rs index fbd6c9c6..47cdf9fd 100644 --- a/crates/parry2d/examples/debug_shape_cast.rs +++ b/crates/parry2d/examples/debug_shape_cast.rs @@ -2,32 +2,21 @@ mod common_macroquad2d; use common_macroquad2d::draw_point; use macroquad::prelude::*; -use nalgebra::{Isometry2, Point2, UnitComplex, Vector2}; +use nalgebra::{Isometry2, Point2, Vector2}; use parry2d::math::Isometry; -use parry2d::query::{self, Ray, RayCast, ShapeCastOptions}; -use parry2d::shape::{Ball, ConvexPolygon, Cuboid, Shape, SharedShape}; +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() { - let animation_scale = 1.4; - let animation_rotation = 0.04; - - for i in 1.. { + for _i in 1.. { clear_background(BLACK); let screen_shift = - Point2::new(screen_width() / 2.0, screen_height() / 2.0) + Vector2::new(-300.0, -200.0); - /* - * - * Compute the scaled cuboid. - * - */ - /*let cube = - Cuboid::new(Vector2::new(2.0, 2.0) * ((i as f32 / 50.0).sin().abs() * animation_scale)); - let cube_pose = Isometry2::rotation(0.008 * i as f32);*/ + Point2::new(screen_width() / 2.0, screen_height() / 2.0) + Vector2::new(-500.0, -200.0); let to_cast_against = ConvexPolygon::from_convex_polyline( [ @@ -40,89 +29,120 @@ async fn main() { ) .expect("Failed to create ConvexPolygon from polyline"); let to_cast_against_pose = Isometry2::rotation(0f32); - let target_pos: Point2<_> = [631.4779, 250.20587].into(); + let mouse_pos = mouse_position(); let mouse_position_world = (Point2::::new(mouse_pos.0, mouse_pos.1) - screen_shift.coords) / RENDER_SCALE; - let target_pos = mouse_position_world; - let pf_source_pos: Point2 = [264.0, 440.0].into(); - /* - * - * Prepare a Raycast and compute its result against the shape. - * - */ - let ray = Ray::new(target_pos, pf_source_pos - target_pos); - - //let toi = to_cast_against.cast_ray(&to_cast_against_pose, &ray, std::f32::MAX, true); - - let pos1 = target_pos; - let vel1 = pf_source_pos - target_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(); + let target_pos: Point2 = [264.0, 440.0].into(); + shape_cast_debug( + screen_shift, + target_pos, + mouse_position_world, + to_cast_against.clone(), + ); + // Those 2 fail with `min_bound >= _eps_tol`, fixed with a tolerance * 100 + shape_cast_debug( + screen_shift, + [675.0, 255.0].into(), + target_pos, + to_cast_against.clone(), + ); + shape_cast_debug( + screen_shift, + [674.0, 257.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, + [623.0, 256.0].into(), + target_pos, + to_cast_against.clone(), + ); /* * - * Render the raycast's result. + * Render the cuboid. * */ - 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); - dbg!("wath"); - } 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(), - 16.0, - RENDER_SCALE, - screen_shift, - GREEN, - ); + 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, + source_pos: Point2, + target_pos: Point2, + 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 = 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.origin + ray.dir, + (ray.point_at(toi.time_of_impact).coords).into(), RENDER_SCALE, screen_shift, - RED, + GREEN, ); } - - /* - * - * Render the cuboid. - * - */ - draw_polygon( - &to_cast_against.points(), - &to_cast_against_pose, + drawcircle_at( + (ray.point_at(toi.time_of_impact).coords).into(), + BALLCAST_WIDTH, RENDER_SCALE, screen_shift, GREEN, ); - - next_frame().await + } else { + drawline_from_to( + ray.origin, + ray.origin + ray.dir, + RENDER_SCALE, + screen_shift, + RED, + ); } } From 9fa67fd4c220acbf608fde73052027d58ce8be77 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 17 Dec 2024 15:40:57 +0100 Subject: [PATCH 03/20] up epsilon for gjk --- crates/parry2d/tests/geometry/ray_cast.rs | 3 +-- src/query/gjk/gjk.rs | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/crates/parry2d/tests/geometry/ray_cast.rs b/crates/parry2d/tests/geometry/ray_cast.rs index de37fd60..8e3ab122 100644 --- a/crates/parry2d/tests/geometry/ray_cast.rs +++ b/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!( diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index ebee30ae..aa4a33cd 100644 --- a/src/query/gjk/gjk.rs +++ b/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. From 881557d4eb059002758a31ef9d72be798de02601 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 17 Dec 2024 15:45:37 +0100 Subject: [PATCH 04/20] add changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1ece976b..efa34f58 100644 --- a/CHANGELOG.md +++ b/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 From d8cd6bf14d3e0010f5efc7ef362d3ccfc951ba5a Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 18 Dec 2024 09:19:43 +0100 Subject: [PATCH 05/20] removed incorrect lint --- Cargo.toml | 3 --- 1 file changed, 3 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 036dfb4a..05320652 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,9 +14,6 @@ rust.unexpected_cfgs = { level = "warn", check-cfg = [ 'cfg(feature, values("wavefront"))', ] } -[workspace.lints.clippy] -std_instead_of_core = "warn" - [patch.crates-io] parry2d = { path = "crates/parry2d" } parry3d = { path = "crates/parry3d" } From 4e3077c4b7629e05a256822ffa55d8fe03b08856 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 18 Dec 2024 09:23:00 +0100 Subject: [PATCH 06/20] add example to cargo toml + rename with 2d postfix --- crates/parry2d/Cargo.toml | 5 +++++ .../examples/{debug_shape_cast.rs => debug_shape_cast2d.rs} | 0 2 files changed, 5 insertions(+) rename crates/parry2d/examples/{debug_shape_cast.rs => debug_shape_cast2d.rs} (100%) diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 5b5ea333..2d0cd521 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -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" diff --git a/crates/parry2d/examples/debug_shape_cast.rs b/crates/parry2d/examples/debug_shape_cast2d.rs similarity index 100% rename from crates/parry2d/examples/debug_shape_cast.rs rename to crates/parry2d/examples/debug_shape_cast2d.rs From e9cf11452155aded9f8c3353089ce69308c8fd2f Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 6 Jan 2025 11:59:34 +0100 Subject: [PATCH 07/20] center shape to raycast --- crates/parry2d/examples/debug_shape_cast2d.rs | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/crates/parry2d/examples/debug_shape_cast2d.rs b/crates/parry2d/examples/debug_shape_cast2d.rs index 47cdf9fd..b05d5386 100644 --- a/crates/parry2d/examples/debug_shape_cast2d.rs +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -2,7 +2,7 @@ mod common_macroquad2d; use common_macroquad2d::draw_point; use macroquad::prelude::*; -use nalgebra::{Isometry2, Point2, Vector2}; +use nalgebra::{Isometry2, Point2}; use parry2d::math::Isometry; use parry2d::query::{self, Ray, ShapeCastOptions}; use parry2d::shape::{Ball, ConvexPolygon, Shape}; @@ -15,15 +15,14 @@ async fn main() { for _i in 1.. { clear_background(BLACK); - let screen_shift = - Point2::new(screen_width() / 2.0, screen_height() / 2.0) + Vector2::new(-500.0, -200.0); + let screen_shift = Point2::new(screen_width() / 2.0, screen_height() / 2.0); let to_cast_against = ConvexPolygon::from_convex_polyline( [ - [600.0, 288.0].into(), - [576.0, 312.0].into(), - [552.0, 288.0].into(), - [576.0, 264.0].into(), + [-24.0, 0.0].into(), + [0.0, 24.0].into(), + [24.0, 0.0].into(), + [0.0, -24.0].into(), ] .into(), ) @@ -33,31 +32,35 @@ async fn main() { let mouse_pos = mouse_position(); let mouse_position_world = (Point2::::new(mouse_pos.0, mouse_pos.1) - screen_shift.coords) / RENDER_SCALE; - let target_pos: Point2 = [264.0, 440.0].into(); + let target_pos: Point2 = [-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, - mouse_position_world, to_cast_against.clone(), ); - // Those 2 fail with `min_bound >= _eps_tol`, fixed with a tolerance * 100 shape_cast_debug( screen_shift, - [675.0, 255.0].into(), + [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, - [674.0, 257.0].into(), + [47.0, -32.0].into(), target_pos, to_cast_against.clone(), ); - // This fails with `niter == 100` (and `niter == 100_000`), fixed with a tolerance * 10_000 + + // For debug purposes, raycast to mouse position. + // Rendered last to be on top of the other raycasts shape_cast_debug( screen_shift, - [623.0, 256.0].into(), target_pos, + mouse_position_world, to_cast_against.clone(), ); From d4e460eaf631b2bfcbfeaabc646e1076b1d3c8cb Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 20 Jan 2025 17:33:44 +0100 Subject: [PATCH 08/20] add unit test --- src/query/gjk/gjk.rs | 58 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index aa4a33cd..4b71dd03 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -391,3 +391,61 @@ fn result(simplex: &VoronoiSimplex, prev: bool) -> (Point, Point) { 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 = [-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, + target_pos: Point2, + ) { + 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." + ); + } +} From f0118bbac1eaf0849cdbe0d5cc432870217bf06f Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 25 Jun 2025 18:17:06 +0200 Subject: [PATCH 09/20] add ability to customize gjk at the dispatcher level (still needs to fix more path to eps_tol(), but serves as discussion starter --- crates/parry2d/examples/debug_shape_cast2d.rs | 8 +++- crates/parry2d/tests/geometry/epa2.rs | 2 +- .../closest_points_shape_shape.rs | 2 +- src/query/contact/contact_shape_shape.rs | 2 +- src/query/default_query_dispatcher.rs | 15 +++++++- src/query/distance/distance.rs | 2 +- src/query/gjk/gjk.rs | 38 ++++++++++++------- .../intersection_test/intersection_test.rs | 2 +- src/query/mod.rs | 4 +- .../nonlinear_shape_cast.rs | 2 +- src/query/shape_cast/mod.rs | 4 +- src/query/shape_cast/shape_cast.rs | 22 ++++++++++- .../shape_cast_support_map_support_map.rs | 19 +++++++++- 13 files changed, 94 insertions(+), 28 deletions(-) diff --git a/crates/parry2d/examples/debug_shape_cast2d.rs b/crates/parry2d/examples/debug_shape_cast2d.rs index b05d5386..d480c100 100644 --- a/crates/parry2d/examples/debug_shape_cast2d.rs +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -4,7 +4,8 @@ use common_macroquad2d::draw_point; use macroquad::prelude::*; use nalgebra::{Isometry2, Point2}; use parry2d::math::Isometry; -use parry2d::query::{self, Ray, ShapeCastOptions}; +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; @@ -101,7 +102,7 @@ fn shape_cast_debug( let vel2 = [0.0, 0.0]; let g2 = to_cast_against.clone_dyn(); - let toi = query::cast_shapes( + let toi = query::cast_shapes_with_dispatcher( &pos1.into(), &vel1, &g1, @@ -109,6 +110,9 @@ fn shape_cast_debug( &vel2.into(), &*g2, ShapeCastOptions::with_max_time_of_impact(1.0), + DefaultQueryDispatcher { + gjk_espilon_tolerance: eps_tol() * 1000f32, + }, ) .unwrap(); diff --git a/crates/parry2d/tests/geometry/epa2.rs b/crates/parry2d/tests/geometry/epa2.rs index 47727488..1481b95a 100644 --- a/crates/parry2d/tests/geometry/epa2.rs +++ b/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; diff --git a/src/query/closest_points/closest_points_shape_shape.rs b/src/query/closest_points/closest_points_shape_shape.rs index cb6d8db9..dcc82931 100644 --- a/src/query/closest_points/closest_points_shape_shape.rs +++ b/src/query/closest_points/closest_points_shape_shape.rs @@ -14,7 +14,7 @@ pub fn closest_points( max_dist: Real, ) -> Result { let pos12 = pos1.inv_mul(pos2); - DefaultQueryDispatcher + DefaultQueryDispatcher::default() .closest_points(&pos12, g1, g2, max_dist) .map(|res| res.transform_by(pos1, pos2)) } diff --git a/src/query/contact/contact_shape_shape.rs b/src/query/contact/contact_shape_shape.rs index f9fdb294..c11aa514 100644 --- a/src/query/contact/contact_shape_shape.rs +++ b/src/query/contact/contact_shape_shape.rs @@ -14,7 +14,7 @@ pub fn contact( prediction: Real, ) -> Result, 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); diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 11af3acf..689604a4 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -1,5 +1,6 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::details::ShapeCastOptions; +use crate::query::gjk; use crate::query::{ self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, Unsupported, @@ -14,7 +15,18 @@ use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; /// A dispatcher that exposes built-in queries #[derive(Debug, Clone)] -pub struct DefaultQueryDispatcher; +pub struct DefaultQueryDispatcher { + /// The absolute tolerance used by the GJK algorithm. + pub gjk_espilon_tolerance: Real, +} + +impl Default for DefaultQueryDispatcher { + fn default() -> Self { + Self { + gjk_espilon_tolerance: gjk::eps_tol(), + } + } +} impl QueryDispatcher for DefaultQueryDispatcher { fn intersection_test( @@ -333,6 +345,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { s1, s2, options, + self.gjk_espilon_tolerance, )); } else if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::cast_shapes_composite_shape_shape( diff --git a/src/query/distance/distance.rs b/src/query/distance/distance.rs index c23c5044..fe97a79a 100644 --- a/src/query/distance/distance.rs +++ b/src/query/distance/distance.rs @@ -13,5 +13,5 @@ pub fn distance( g2: &dyn Shape, ) -> Result { let pos12 = pos1.inv_mul(pos2); - DefaultQueryDispatcher.distance(&pos12, g1, g2) + DefaultQueryDispatcher::default().distance(&pos12, g1, g2) } diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 4b71dd03..544b1335 100644 --- a/src/query/gjk/gjk.rs +++ b/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_000.0 + _eps } /// Projects the origin on the boundary of the given shape. @@ -197,6 +197,7 @@ pub fn cast_local_ray( ray, max_time_of_impact, simplex, + eps_tol(), ) } @@ -210,25 +211,33 @@ pub fn directional_distance( g2: &G2, dir: &Vector, simplex: &mut VoronoiSimplex, + gjk_espilon_tolerance: Real, ) -> Option<(Real, Vector, Point, Point)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let ray = Ray::new(Point::origin(), *dir); - minkowski_ray_cast(pos12, g1, g2, &ray, Real::max_value(), simplex).map( - |(time_of_impact, normal)| { - let witnesses = if !time_of_impact.is_zero() { - result(simplex, simplex.dimension() == DIM) - } else { - // If there is penetration, the witness points - // are undefined. - (Point::origin(), Point::origin()) - }; - - (time_of_impact, normal, witnesses.0, witnesses.1) - }, + minkowski_ray_cast( + pos12, + g1, + g2, + &ray, + Real::max_value(), + simplex, + gjk_espilon_tolerance, ) + .map(|(time_of_impact, normal)| { + let witnesses = if !time_of_impact.is_zero() { + result(simplex, simplex.dimension() == DIM) + } else { + // If there is penetration, the witness points + // are undefined. + (Point::origin(), Point::origin()) + }; + + (time_of_impact, normal, witnesses.0, witnesses.1) + }) } // Ray-cast on the Minkowski Difference `g1 - pos12 * g2`. @@ -239,13 +248,14 @@ fn minkowski_ray_cast( ray: &Ray, max_time_of_impact: Real, simplex: &mut VoronoiSimplex, + gjk_espilon_tolerance: Real, ) -> Option<(Real, Vector)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let _eps = crate::math::DEFAULT_EPSILON; - let _eps_tol: Real = eps_tol(); + let _eps_tol: Real = gjk_espilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); let ray_length = ray.dir.norm(); diff --git a/src/query/intersection_test/intersection_test.rs b/src/query/intersection_test/intersection_test.rs index 8833cc30..ed87b9ce 100644 --- a/src/query/intersection_test/intersection_test.rs +++ b/src/query/intersection_test/intersection_test.rs @@ -10,5 +10,5 @@ pub fn intersection_test( g2: &dyn Shape, ) -> Result { let pos12 = pos1.inv_mul(pos2); - DefaultQueryDispatcher.intersection_test(&pos12, g1, g2) + DefaultQueryDispatcher::default().intersection_test(&pos12, g1, g2) } diff --git a/src/query/mod.rs b/src/query/mod.rs index ed6e1537..6118a291 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -41,7 +41,9 @@ pub use self::point::{PointProjection, PointQuery, PointQueryWithLocation}; pub use self::query_dispatcher::PersistentQueryDispatcher; pub use self::query_dispatcher::{QueryDispatcher, QueryDispatcherChain}; pub use self::ray::{Ray, RayCast, RayIntersection, SimdRay}; -pub use self::shape_cast::{cast_shapes, ShapeCastHit, ShapeCastOptions, ShapeCastStatus}; +pub use self::shape_cast::{ + cast_shapes, cast_shapes_with_dispatcher, ShapeCastHit, ShapeCastOptions, ShapeCastStatus, +}; pub use self::split::{IntersectResult, SplitResult}; mod clip; diff --git a/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs index 67a48912..c1c2fac6 100644 --- a/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs @@ -29,7 +29,7 @@ pub fn cast_shapes_nonlinear( end_time: Real, stop_at_penetration: bool, ) -> Result, Unsupported> { - DefaultQueryDispatcher.cast_shapes_nonlinear( + DefaultQueryDispatcher::default().cast_shapes_nonlinear( motion1, g1, motion2, diff --git a/src/query/shape_cast/mod.rs b/src/query/shape_cast/mod.rs index 95a82520..f9050c91 100644 --- a/src/query/shape_cast/mod.rs +++ b/src/query/shape_cast/mod.rs @@ -1,6 +1,8 @@ //! Implementation details of the `cast_shapes` function. -pub use self::shape_cast::{cast_shapes, ShapeCastHit, ShapeCastOptions, ShapeCastStatus}; +pub use self::shape_cast::{ + cast_shapes, cast_shapes_with_dispatcher, ShapeCastHit, ShapeCastOptions, ShapeCastStatus, +}; pub use self::shape_cast_ball_ball::cast_shapes_ball_ball; pub use self::shape_cast_halfspace_support_map::{ cast_shapes_halfspace_support_map, cast_shapes_support_map_halfspace, diff --git a/src/query/shape_cast/shape_cast.rs b/src/query/shape_cast/shape_cast.rs index eb9eaaf9..97ce9ccb 100644 --- a/src/query/shape_cast/shape_cast.rs +++ b/src/query/shape_cast/shape_cast.rs @@ -149,5 +149,25 @@ pub fn cast_shapes( ) -> Result, Unsupported> { let pos12 = pos1.inv_mul(pos2); let vel12 = pos1.inverse_transform_vector(&(vel2 - vel1)); - DefaultQueryDispatcher.cast_shapes(&pos12, &vel12, g1, g2, options) + DefaultQueryDispatcher::default().cast_shapes(&pos12, &vel12, g1, g2, options) +} + +/// Computes the smallest time when two shapes under translational movement are separated by a +/// distance smaller or equal to `distance`. +/// +/// Returns `0.0` if the objects are touching or closer than `options.target_distance`, +/// or penetrating. +pub fn cast_shapes_with_dispatcher( + pos1: &Isometry, + vel1: &Vector, + g1: &dyn Shape, + pos2: &Isometry, + vel2: &Vector, + g2: &dyn Shape, + options: ShapeCastOptions, + dispatcher: impl QueryDispatcher, +) -> Result, Unsupported> { + let pos12 = pos1.inv_mul(pos2); + let vel12 = pos1.inverse_transform_vector(&(vel2 - vel1)); + dispatcher.cast_shapes(&pos12, &vel12, g1, g2, options) } diff --git a/src/query/shape_cast/shape_cast_support_map_support_map.rs b/src/query/shape_cast/shape_cast_support_map_support_map.rs index 7df76da1..1dc20405 100644 --- a/src/query/shape_cast/shape_cast_support_map_support_map.rs +++ b/src/query/shape_cast/shape_cast_support_map_support_map.rs @@ -15,6 +15,7 @@ pub fn cast_shapes_support_map_support_map( g1: &G1, g2: &G2, options: ShapeCastOptions, + gjk_espilon_tolerance: Real, ) -> Option where G1: ?Sized + SupportMap, @@ -25,9 +26,23 @@ where inner_shape: g1, border_radius: options.target_distance, }; - gjk::directional_distance(pos12, &round_g1, g2, vel12, &mut VoronoiSimplex::new()) + gjk::directional_distance( + pos12, + &round_g1, + g2, + vel12, + &mut VoronoiSimplex::new(), + gjk_espilon_tolerance, + ) } else { - gjk::directional_distance(pos12, g1, g2, vel12, &mut VoronoiSimplex::new()) + gjk::directional_distance( + pos12, + g1, + g2, + vel12, + &mut VoronoiSimplex::new(), + gjk_espilon_tolerance, + ) }; gjk_result.and_then(|(time_of_impact, normal1, witness1, witness2)| { From 7067ee29d1ecc291de6276911338e3e05da8078b Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 2 Jul 2025 11:07:27 +0200 Subject: [PATCH 10/20] add dedicated gjk options struct and passed in most convenient places --- crates/parry2d/examples/debug_shape_cast2d.rs | 7 +- .../closest_points_support_map_support_map.rs | 7 +- .../contact_support_map_support_map.rs | 16 ++- .../contact_manifolds_pfm_pfm.rs | 5 + src/query/default_query_dispatcher.rs | 42 ++++---- .../distance_support_map_support_map.rs | 21 +++- src/query/epa/epa2.rs | 101 ++++++++++++++---- src/query/epa/epa3.rs | 53 +++++++-- src/query/gjk/gjk.rs | 73 ++++++++----- src/query/gjk/voronoi_simplex2.rs | 6 +- src/query/gjk/voronoi_simplex3.rs | 10 +- ...tersection_test_support_map_support_map.rs | 7 +- src/query/point/point_round_shape.rs | 4 + src/query/point/point_support_map.rs | 23 +++- src/query/ray/ray_round_shape.rs | 4 +- src/query/ray/ray_support_map.rs | 19 +++- .../shape_cast_support_map_support_map.rs | 11 +- 17 files changed, 300 insertions(+), 109 deletions(-) diff --git a/crates/parry2d/examples/debug_shape_cast2d.rs b/crates/parry2d/examples/debug_shape_cast2d.rs index d480c100..0aa769d2 100644 --- a/crates/parry2d/examples/debug_shape_cast2d.rs +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -3,7 +3,7 @@ mod common_macroquad2d; use common_macroquad2d::draw_point; use macroquad::prelude::*; use nalgebra::{Isometry2, Point2}; -use parry2d::math::Isometry; +use parry2d::math::{self, Isometry}; use parry2d::query::gjk::eps_tol; use parry2d::query::{self, DefaultQueryDispatcher, Ray, ShapeCastOptions}; use parry2d::shape::{Ball, ConvexPolygon, Shape}; @@ -111,7 +111,10 @@ fn shape_cast_debug( &*g2, ShapeCastOptions::with_max_time_of_impact(1.0), DefaultQueryDispatcher { - gjk_espilon_tolerance: eps_tol() * 1000f32, + gjk_options: query::gjk::GjkOptions { + espilon_tolerance: math::DEFAULT_EPSILON * 1000f32, + nb_max_iterations: 100, + }, }, ) .unwrap(); diff --git a/src/query/closest_points/closest_points_support_map_support_map.rs b/src/query/closest_points/closest_points_support_map_support_map.rs index 5930b03a..78fd926d 100644 --- a/src/query/closest_points/closest_points_support_map_support_map.rs +++ b/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: &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( prediction: Real, simplex: &mut VoronoiSimplex, init_dir: Option>, + 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) } diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 831ba246..3214d9dc 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/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: &G1, g2: &G2, prediction: Real, + gjk_options: &GjkOptions, ) -> Option 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( prediction: Real, simplex: &mut VoronoiSimplex, init_dir: Option>>, + 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; } diff --git a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs index bb13a838..87120728 100644 --- a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs @@ -1,5 +1,6 @@ use crate::math::{Isometry, Real}; use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; +use crate::query::gjk::GjkOptions; use crate::query::{ self, gjk::{GJKResult, VoronoiSimplex}, @@ -18,6 +19,7 @@ pub fn contact_manifold_pfm_pfm_shapes( normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, + gjk_options: &GjkOptions, ) where ManifoldData: Default, ContactData: Default + Copy, @@ -36,6 +38,7 @@ pub fn contact_manifold_pfm_pfm_shapes( normal_constraints2, prediction, manifold, + gjk_options, ); } } @@ -51,6 +54,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, + gjk_options: &GjkOptions, ) where S1: ?Sized + PolygonalFeatureMap, S2: ?Sized + PolygonalFeatureMap, @@ -73,6 +77,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( total_prediction, &mut VoronoiSimplex::new(), init_dir, + gjk_options, ); let old_manifold_points = manifold.points.clone(); diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 689604a4..d4da5a62 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -1,6 +1,6 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::details::ShapeCastOptions; -use crate::query::gjk; +use crate::query::gjk::GjkOptions; use crate::query::{ self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, Unsupported, @@ -14,18 +14,10 @@ use crate::query::{ use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; /// A dispatcher that exposes built-in queries -#[derive(Debug, Clone)] +#[derive(Debug, Clone, Default)] pub struct DefaultQueryDispatcher { - /// The absolute tolerance used by the GJK algorithm. - pub gjk_espilon_tolerance: Real, -} - -impl Default for DefaultQueryDispatcher { - fn default() -> Self { - Self { - gjk_espilon_tolerance: gjk::eps_tol(), - } - } + /// Options for the GJK algorithm. + pub gjk_options: GjkOptions, } impl QueryDispatcher for DefaultQueryDispatcher { @@ -72,7 +64,10 @@ impl QueryDispatcher for DefaultQueryDispatcher { )) } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { Ok(query::details::intersection_test_support_map_support_map( - pos12, s1, s2, + pos12, + s1, + s2, + &self.gjk_options, )) } else { #[cfg(feature = "std")] @@ -131,7 +126,10 @@ impl QueryDispatcher for DefaultQueryDispatcher { )) } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { Ok(query::details::distance_support_map_support_map( - pos12, s1, s2, + pos12, + s1, + s2, + &self.gjk_options, )) } else { #[cfg(feature = "std")] @@ -189,7 +187,11 @@ impl QueryDispatcher for DefaultQueryDispatcher { #[cfg(feature = "std")] if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { return Ok(query::details::contact_support_map_support_map( - pos12, s1, s2, prediction, + pos12, + s1, + s2, + prediction, + &self.gjk_options, )); } else if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::contact_composite_shape_shape( @@ -263,7 +265,11 @@ impl QueryDispatcher for DefaultQueryDispatcher { )) } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { Ok(query::details::closest_points_support_map_support_map( - pos12, s1, s2, max_dist, + pos12, + s1, + s2, + max_dist, + &self.gjk_options, )) } else { #[cfg(feature = "std")] @@ -345,7 +351,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { s1, s2, options, - self.gjk_espilon_tolerance, + &self.gjk_options, )); } else if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::cast_shapes_composite_shape_shape( @@ -613,7 +619,7 @@ where shape2.as_polygonal_feature_map(), ) { contact_manifold_pfm_pfm( - pos12, pfm1.0, pfm1.1, normal_constraints1, pfm2.0, pfm2.1, normal_constraints2, prediction, manifold, + pos12, pfm1.0, pfm1.1, normal_constraints1, pfm2.0, pfm2.1, normal_constraints2, prediction, manifold, &self.gjk_options ) } else { return Err(Unsupported); diff --git a/src/query/distance/distance_support_map_support_map.rs b/src/query/distance/distance_support_map_support_map.rs index c0096ec3..667b2ae4 100644 --- a/src/query/distance/distance_support_map_support_map.rs +++ b/src/query/distance/distance_support_map_support_map.rs @@ -1,17 +1,29 @@ 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::shape::SupportMap; use na::{self, Unit}; use num::Bounded; /// Distance between support-mapped shapes. -pub fn distance_support_map_support_map(pos12: &Isometry, g1: &G1, g2: &G2) -> Real +pub fn distance_support_map_support_map( + pos12: &Isometry, + g1: &G1, + g2: &G2, + gjk_options: &GjkOptions, +) -> Real where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { - distance_support_map_support_map_with_params(pos12, g1, g2, &mut VoronoiSimplex::new(), None) + distance_support_map_support_map_with_params( + pos12, + g1, + g2, + &mut VoronoiSimplex::new(), + None, + gjk_options, + ) } /// Distance between support-mapped shapes. @@ -23,6 +35,7 @@ pub fn distance_support_map_support_map_with_params( g2: &G2, simplex: &mut VoronoiSimplex, init_dir: Option>, + gjk_options: &GjkOptions, ) -> Real where G1: ?Sized + SupportMap, @@ -42,7 +55,7 @@ where )); } - match gjk::closest_points(pos12, g1, g2, Real::max_value(), true, simplex) { + match gjk::closest_points(pos12, g1, g2, Real::max_value(), true, simplex, gjk_options) { GJKResult::Intersection => 0.0, GJKResult::ClosestPoints(p1, p2, _) => na::distance(&p1, &p2), GJKResult::Proximity(_) => unreachable!(), diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 49961a80..69f6624f 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -7,7 +7,7 @@ use na::{self, Unit}; use num::Bounded; use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::gjk::{eps_tol, CSOPoint, ConstantOrigin, VoronoiSimplex}; use crate::shape::SupportMap; use crate::utils; @@ -18,8 +18,8 @@ struct FaceId { } impl FaceId { - fn new(id: usize, neg_dist: Real) -> Option { - if neg_dist > gjk::eps_tol() { + fn new(id: usize, neg_dist: Real, gjk_espilon_tolerance: Real) -> Option { + if neg_dist > gjk_espilon_tolerance { None } else { Some(FaceId { id, neg_dist }) @@ -59,10 +59,12 @@ struct Face { } impl Face { - pub fn new(vertices: &[CSOPoint], pts: [usize; 2]) -> (Self, bool) { - if let Some((proj, bcoords)) = - project_origin(&vertices[pts[0]].point, &vertices[pts[1]].point) - { + pub fn new(vertices: &[CSOPoint], pts: [usize; 2], epsilon_tolerance: Real) -> (Self, bool) { + if let Some((proj, bcoords)) = project_origin( + &vertices[pts[0]].point, + &vertices[pts[1]].point, + epsilon_tolerance, + ) { (Self::new_with_proj(vertices, proj, bcoords, pts), true) } else { ( @@ -226,9 +228,24 @@ impl EPA { let pts2 = [1, 2]; let pts3 = [2, 0]; - let (face1, proj_inside1) = Face::new(&self.vertices, pts1); - let (face2, proj_inside2) = Face::new(&self.vertices, pts2); - let (face3, proj_inside3) = Face::new(&self.vertices, pts3); + let (face1, proj_inside1) = Face::new( + &self.vertices, + pts1, + // TODO: allow custom options + eps_tol(), + ); + let (face2, proj_inside2) = Face::new( + &self.vertices, + pts2, + // TODO: allow custom options + eps_tol(), + ); + let (face3, proj_inside3) = Face::new( + &self.vertices, + pts3, + // TODO: allow custom options + eps_tol(), + ); self.faces.push(face1); self.faces.push(face2); @@ -236,17 +253,32 @@ impl EPA { if proj_inside1 { let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + self.heap.push(FaceId::new( + 0, + -dist1, + // TODO: allow custom options + eps_tol(), + )?); } if proj_inside2 { let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + self.heap.push(FaceId::new( + 1, + -dist2, + // TODO: allow custom options + eps_tol(), + )?); } if proj_inside3 { let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + self.heap.push(FaceId::new( + 2, + -dist3, + // TODO: allow custom options + eps_tol(), + )?); } if !(proj_inside1 || proj_inside2 || proj_inside3) { @@ -276,8 +308,18 @@ impl EPA { let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(0, dist1)?); - self.heap.push(FaceId::new(1, dist2)?); + self.heap.push(FaceId::new( + 0, + dist1, + // TODO: allow custom options + eps_tol(), + )?); + self.heap.push(FaceId::new( + 1, + dist2, + // TODO: allow custom options + eps_tol(), + )?); } let mut niter = 0; @@ -325,8 +367,18 @@ impl EPA { let pts2 = [support_point_id, face.pts[1]]; let new_faces = [ - Face::new(&self.vertices, pts1), - Face::new(&self.vertices, pts2), + Face::new( + &self.vertices, + pts1, + // TODO: allow custom options + eps_tol(), + ), + Face::new( + &self.vertices, + pts2, + // TODO: allow custom options + eps_tol(), + ), ]; for f in new_faces.iter() { @@ -340,7 +392,12 @@ impl EPA { } if !f.0.deleted { - self.heap.push(FaceId::new(self.faces.len(), -dist)?); + self.heap.push(FaceId::new( + self.faces.len(), + -dist, + // TODO: allow custom options + eps_tol(), + )?); } } @@ -361,7 +418,11 @@ impl EPA { } } -fn project_origin(a: &Point, b: &Point) -> Option<(Point, [Real; 2])> { +fn project_origin( + a: &Point, + b: &Point, + epsilon_tolerance: Real, +) -> Option<(Point, [Real; 2])> { let ab = *b - *a; let ap = -a.coords; let ab_ap = ab.dot(&ap); @@ -373,7 +434,7 @@ fn project_origin(a: &Point, b: &Point) -> Option<(Point, [Rea let position_on_segment; - let _eps: Real = gjk::eps_tol(); + let _eps: Real = epsilon_tolerance; if ab_ap < -_eps || ab_ap > sqnab + _eps { // Voronoï region of vertex 'a' or 'b'. diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 5ed482d4..667c6798 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -17,8 +17,8 @@ struct FaceId { } impl FaceId { - fn new(id: usize, neg_dist: Real) -> Option { - if neg_dist > gjk::eps_tol() { + fn new(id: usize, neg_dist: Real, gjk_espilon_tolerance: Real) -> Option { + if neg_dist > gjk_espilon_tolerance { None } else { Some(FaceId { id, neg_dist }) @@ -275,22 +275,42 @@ impl EPA { if proj_inside1 { let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + self.heap.push(FaceId::new( + 0, + -dist1, + // TODO: allow custom options + gjk::eps_tol(), + )?); } if proj_inside2 { let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + self.heap.push(FaceId::new( + 1, + -dist2, + // TODO: allow custom options + gjk::eps_tol(), + )?); } if proj_inside3 { let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + self.heap.push(FaceId::new( + 2, + -dist3, + // TODO: allow custom options + gjk::eps_tol(), + )?); } if proj_inside4 { let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords); - self.heap.push(FaceId::new(3, -dist4)?); + self.heap.push(FaceId::new( + 3, + -dist4, + // TODO: allow custom options + gjk::eps_tol(), + )?); } if !(proj_inside1 || proj_inside2 || proj_inside3 || proj_inside4) { @@ -324,8 +344,18 @@ impl EPA { self.faces.push(face1); self.faces.push(face2); - self.heap.push(FaceId::new(0, 0.0)?); - self.heap.push(FaceId::new(1, 0.0)?); + self.heap.push(FaceId::new( + 0, + 0.0, + // TODO: allow custom options + gjk::eps_tol(), + )?); + self.heap.push(FaceId::new( + 1, + 0.0, + // TODO: allow custom options + gjk::eps_tol(), + )?); } let mut niter = 0; @@ -412,7 +442,12 @@ impl EPA { return Some((points.0, points.1, face.normal)); } - self.heap.push(FaceId::new(new_face_id, -dist)?); + self.heap.push(FaceId::new( + new_face_id, + -dist, + // TODO: allow custom options + gjk::eps_tol(), + )?); } } } diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 544b1335..cd7ad1df 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -32,6 +32,26 @@ pub enum GJKResult { NoIntersection(Unit>), } +/// Options for the GJK algorithm. +#[derive(Debug, Clone)] +pub struct GjkOptions { + /// The absolute tolerance used by the GJK algorithm. + /// + /// Defaults to [math::DEFAULT_EPSILON][crate::math::DEFAULT_EPSILON] + pub espilon_tolerance: Real, + /// The maximum number of iterations of the GJK algorithm. + pub nb_max_iterations: u32, +} + +impl Default for GjkOptions { + fn default() -> Self { + Self { + espilon_tolerance: crate::math::DEFAULT_EPSILON, + nb_max_iterations: 100, + } + } +} + /// The absolute tolerance used by the GJK algorithm. pub fn eps_tol() -> Real { let _eps = crate::math::DEFAULT_EPSILON; @@ -50,6 +70,7 @@ pub fn project_origin( m: &Isometry, g: &G, simplex: &mut VoronoiSimplex, + gjk_options: &GjkOptions, ) -> Option> { match closest_points( &m.inverse(), @@ -58,6 +79,7 @@ pub fn project_origin( Real::max_value(), true, simplex, + gjk_options, ) { GJKResult::Intersection => None, GJKResult::ClosestPoints(p, _, _) => Some(p), @@ -87,13 +109,14 @@ pub fn closest_points( max_dist: Real, exact_dist: bool, simplex: &mut VoronoiSimplex, + gjk_options: &GjkOptions, ) -> GJKResult where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let _eps = crate::math::DEFAULT_EPSILON; - let _eps_tol: Real = eps_tol(); + let _eps_tol: Real = gjk_options.espilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); // TODO: reset the simplex if it is empty? @@ -149,7 +172,7 @@ where } } - if !simplex.add_point(cso_point) { + if !simplex.add_point(cso_point, _eps_tol) { if exact_dist { let (p1, p2) = result(simplex, false); return GJKResult::ClosestPoints(p1, p2, dir); @@ -176,7 +199,7 @@ where } niter += 1; - if niter == 100 { + if niter == gjk_options.nb_max_iterations { return GJKResult::NoIntersection(Vector::x_axis()); } } @@ -188,6 +211,7 @@ pub fn cast_local_ray( simplex: &mut VoronoiSimplex, ray: &Ray, max_time_of_impact: Real, + gjk_options: &GjkOptions, ) -> Option<(Real, Vector)> { let g2 = ConstantOrigin; minkowski_ray_cast( @@ -197,7 +221,7 @@ pub fn cast_local_ray( ray, max_time_of_impact, simplex, - eps_tol(), + gjk_options, ) } @@ -211,33 +235,26 @@ pub fn directional_distance( g2: &G2, dir: &Vector, simplex: &mut VoronoiSimplex, - gjk_espilon_tolerance: Real, + gjk_options: &GjkOptions, ) -> Option<(Real, Vector, Point, Point)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let ray = Ray::new(Point::origin(), *dir); - minkowski_ray_cast( - pos12, - g1, - g2, - &ray, - Real::max_value(), - simplex, - gjk_espilon_tolerance, - ) - .map(|(time_of_impact, normal)| { - let witnesses = if !time_of_impact.is_zero() { - result(simplex, simplex.dimension() == DIM) - } else { - // If there is penetration, the witness points - // are undefined. - (Point::origin(), Point::origin()) - }; + minkowski_ray_cast(pos12, g1, g2, &ray, Real::max_value(), simplex, gjk_options).map( + |(time_of_impact, normal)| { + let witnesses = if !time_of_impact.is_zero() { + result(simplex, simplex.dimension() == DIM) + } else { + // If there is penetration, the witness points + // are undefined. + (Point::origin(), Point::origin()) + }; - (time_of_impact, normal, witnesses.0, witnesses.1) - }) + (time_of_impact, normal, witnesses.0, witnesses.1) + }, + ) } // Ray-cast on the Minkowski Difference `g1 - pos12 * g2`. @@ -248,14 +265,14 @@ fn minkowski_ray_cast( ray: &Ray, max_time_of_impact: Real, simplex: &mut VoronoiSimplex, - gjk_espilon_tolerance: Real, + gjk_options: &GjkOptions, ) -> Option<(Real, Vector)> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { let _eps = crate::math::DEFAULT_EPSILON; - let _eps_tol: Real = gjk_espilon_tolerance; + let _eps_tol: Real = gjk_options.espilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); let ray_length = ray.dir.norm(); @@ -361,7 +378,7 @@ where } } - let _ = simplex.add_point(support_point.translate(&-curr_ray.origin.coords)); + let _ = simplex.add_point(support_point.translate(&-curr_ray.origin.coords), _eps_tol); proj = simplex.project_origin_and_reduce(); if simplex.dimension() == DIM { @@ -373,7 +390,7 @@ where } niter += 1; - if niter == 100 { + if niter == gjk_options.nb_max_iterations { return None; } } diff --git a/src/query/gjk/voronoi_simplex2.rs b/src/query/gjk/voronoi_simplex2.rs index e69d230a..db22e9b4 100644 --- a/src/query/gjk/voronoi_simplex2.rs +++ b/src/query/gjk/voronoi_simplex2.rs @@ -1,5 +1,5 @@ use crate::math::{Point, Real}; -use crate::query::gjk::{self, CSOPoint}; +use crate::query::gjk::CSOPoint; use crate::query::{PointQuery, PointQueryWithLocation}; use crate::shape::{Segment, SegmentPointLocation, Triangle, TrianglePointLocation}; @@ -48,13 +48,13 @@ impl VoronoiSimplex { } /// Add a point to this simplex. - pub fn add_point(&mut self, pt: CSOPoint) -> bool { + pub fn add_point(&mut self, pt: CSOPoint, gjk_epsilon_tolerance: Real) -> bool { self.prev_dim = self.dim; self.prev_proj = self.proj; self.prev_vertices = [0, 1, 2]; for i in 0..self.dim + 1 { - if (self.vertices[i].point - pt.point).norm_squared() < gjk::eps_tol() { + if (self.vertices[i].point - pt.point).norm_squared() < gjk_epsilon_tolerance { return false; } } diff --git a/src/query/gjk/voronoi_simplex3.rs b/src/query/gjk/voronoi_simplex3.rs index 8742e58d..782f8153 100644 --- a/src/query/gjk/voronoi_simplex3.rs +++ b/src/query/gjk/voronoi_simplex3.rs @@ -1,5 +1,5 @@ use crate::math::{Point, Real}; -use crate::query::gjk::{self, CSOPoint}; +use crate::query::gjk::CSOPoint; use crate::query::{PointQuery, PointQueryWithLocation}; use crate::shape::{ Segment, SegmentPointLocation, Tetrahedron, TetrahedronPointLocation, Triangle, @@ -54,14 +54,14 @@ impl VoronoiSimplex { } /// Add a point to this simplex. - pub fn add_point(&mut self, pt: CSOPoint) -> bool { + pub fn add_point(&mut self, pt: CSOPoint, gjk_epsilon_tolerance: Real) -> bool { self.prev_dim = self.dim; self.prev_proj = self.proj; self.prev_vertices = [0, 1, 2, 3]; match self.dim { 0 => { - if (self.vertices[0] - pt).norm_squared() < gjk::eps_tol() { + if (self.vertices[0] - pt).norm_squared() < gjk_epsilon_tolerance { return false; } } @@ -69,7 +69,7 @@ impl VoronoiSimplex { let ab = self.vertices[1] - self.vertices[0]; let ac = pt - self.vertices[0]; - if ab.cross(&ac).norm_squared() < gjk::eps_tol() { + if ab.cross(&ac).norm_squared() < gjk_epsilon_tolerance { return false; } } @@ -79,7 +79,7 @@ impl VoronoiSimplex { let ap = pt - self.vertices[0]; let n = ab.cross(&ac).normalize(); - if n.dot(&ap).abs() < gjk::eps_tol() { + if n.dot(&ap).abs() < gjk_epsilon_tolerance { return false; } } diff --git a/src/query/intersection_test/intersection_test_support_map_support_map.rs b/src/query/intersection_test/intersection_test_support_map_support_map.rs index db71997d..cb352a72 100644 --- a/src/query/intersection_test/intersection_test_support_map_support_map.rs +++ b/src/query/intersection_test/intersection_test_support_map_support_map.rs @@ -1,7 +1,7 @@ use na::{self, Unit}; 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::shape::SupportMap; /// Intersection test between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.) @@ -9,6 +9,7 @@ pub fn intersection_test_support_map_support_map( pos12: &Isometry, g1: &G1, g2: &G2, + gjk_options: &GjkOptions, ) -> bool where G1: ?Sized + SupportMap, @@ -20,6 +21,7 @@ where g2, &mut VoronoiSimplex::new(), None, + gjk_options, ) .0 } @@ -33,6 +35,7 @@ pub fn intersection_test_support_map_support_map_with_params( g2: &G2, simplex: &mut VoronoiSimplex, init_dir: Option>>, + gjk_options: &GjkOptions, ) -> (bool, Unit>) where G1: ?Sized + SupportMap, @@ -50,7 +53,7 @@ where simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir)); - match gjk::closest_points(pos12, g1, g2, 0.0, false, simplex) { + match gjk::closest_points(pos12, g1, g2, 0.0, false, simplex, gjk_options) { GJKResult::Intersection => (true, dir), GJKResult::Proximity(dir) => (false, dir), GJKResult::NoIntersection(dir) => (false, dir), diff --git a/src/query/point/point_round_shape.rs b/src/query/point/point_round_shape.rs index 64eabfce..b1c0d061 100644 --- a/src/query/point/point_round_shape.rs +++ b/src/query/point/point_round_shape.rs @@ -1,4 +1,6 @@ use crate::math::{Point, Real}; +#[cfg(feature = "std")] +use crate::query::gjk::GjkOptions; use crate::query::gjk::VoronoiSimplex; use crate::query::{PointProjection, PointQuery}; use crate::shape::{FeatureId, RoundShape, SupportMap}; @@ -19,6 +21,8 @@ impl PointQuery for RoundShape { &mut VoronoiSimplex::new(), point, solid, + // TODO: allow custom options + &GjkOptions::default(), ); } diff --git a/src/query/point/point_support_map.rs b/src/query/point/point_support_map.rs index 78e2a181..8761504d 100644 --- a/src/query/point/point_support_map.rs +++ b/src/query/point/point_support_map.rs @@ -3,7 +3,7 @@ use na::Unit; use crate::math::{Isometry, Point, Real, Vector}; #[cfg(feature = "std")] use crate::query::epa::EPA; -use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::gjk::{self, CSOPoint, ConstantOrigin, GjkOptions, VoronoiSimplex}; use crate::query::{PointProjection, PointQuery}; #[cfg(feature = "dim2")] #[cfg(feature = "std")] @@ -19,6 +19,7 @@ pub fn local_point_projection_on_support_map( simplex: &mut VoronoiSimplex, point: &Point, solid: bool, + gjk_options: &GjkOptions, ) -> PointProjection where G: SupportMap, @@ -31,7 +32,7 @@ where simplex.reset(support_point); - if let Some(proj) = gjk::project_origin(&m, shape, simplex) { + if let Some(proj) = gjk::project_origin(&m, shape, simplex, gjk_options) { PointProjection::new(false, proj) } else if solid { PointProjection::new(true, *point) @@ -55,7 +56,14 @@ where impl PointQuery for ConvexPolyhedron { #[inline] fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), point, solid) + local_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + // TODO: allow custom options + &GjkOptions::default(), + ) } #[inline] @@ -80,7 +88,14 @@ impl PointQuery for ConvexPolyhedron { impl PointQuery for ConvexPolygon { #[inline] fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), point, solid) + local_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + // TODO: allow custom options + &GjkOptions::default(), + ) } #[inline] diff --git a/src/query/ray/ray_round_shape.rs b/src/query/ray/ray_round_shape.rs index b37d9cdd..af4c1006 100644 --- a/src/query/ray/ray_round_shape.rs +++ b/src/query/ray/ray_round_shape.rs @@ -1,5 +1,5 @@ use crate::math::Real; -use crate::query::gjk::VoronoiSimplex; +use crate::query::gjk::{GjkOptions, VoronoiSimplex}; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::{RoundShape, SupportMap}; @@ -16,6 +16,8 @@ impl RayCast for RoundShape { ray, max_time_of_impact, solid, + // TODO: be able to pass custom option. + &GjkOptions::default(), ) } } diff --git a/src/query/ray/ray_support_map.rs b/src/query/ray/ray_support_map.rs index b1bd386b..751cad08 100644 --- a/src/query/ray/ray_support_map.rs +++ b/src/query/ray/ray_support_map.rs @@ -5,7 +5,7 @@ use na::ComplexField; // for .abs() use crate::math::Real; #[cfg(feature = "dim2")] use crate::query; -use crate::query::gjk::{self, CSOPoint, VoronoiSimplex}; +use crate::query::gjk::{self, CSOPoint, GjkOptions, VoronoiSimplex}; use crate::query::{Ray, RayCast, RayIntersection}; #[cfg(all(feature = "std", feature = "dim2"))] use crate::shape::ConvexPolygon; @@ -24,11 +24,12 @@ pub fn local_ray_intersection_with_support_map_with_params Option { let supp = shape.local_support_point(&-ray.dir); simplex.reset(CSOPoint::single_point(supp - ray.origin.coords)); - let inter = gjk::cast_local_ray(shape, simplex, ray, max_time_of_impact); + let inter = gjk::cast_local_ray(shape, simplex, ray, max_time_of_impact, gjk_options); if !solid { inter.and_then(|(time_of_impact, normal)| { @@ -43,7 +44,7 @@ pub fn local_ray_intersection_with_support_map_with_params( g1: &G1, g2: &G2, options: ShapeCastOptions, - gjk_espilon_tolerance: Real, + gjk_options: &GjkOptions, ) -> Option where G1: ?Sized + SupportMap, @@ -32,7 +32,7 @@ where g2, vel12, &mut VoronoiSimplex::new(), - gjk_espilon_tolerance, + gjk_options, ) } else { gjk::directional_distance( @@ -41,7 +41,7 @@ where g2, vel12, &mut VoronoiSimplex::new(), - gjk_espilon_tolerance, + gjk_options, ) }; @@ -51,7 +51,8 @@ where } else if (options.compute_impact_geometry_on_penetration || !options.stop_at_penetration) && time_of_impact < 1.0e-5 { - let contact = details::contact_support_map_support_map(pos12, g1, g2, Real::MAX)?; + let contact = + details::contact_support_map_support_map(pos12, g1, g2, Real::MAX, gjk_options)?; let normal_vel = contact.normal1.dot(vel12); if !options.stop_at_penetration && normal_vel >= 0.0 { From 67bd7498290fd78dfec4fe30514d71e9a33c957d Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 8 Jul 2025 15:31:43 +0200 Subject: [PATCH 11/20] wip: using point query options as dyn ; dispatching them depending on shapes knowing their underlying algorithm. --- crates/parry2d/examples/project_point2d.rs | 4 +- .../parry2d/examples/solid_point_query2d.rs | 9 +- crates/parry2d/tests/geometry/epa2.rs | 13 ++- .../parry2d/tests/geometry/epa_convergence.rs | 3 +- .../tests/query/point_composite_shape.rs | 20 ++-- crates/parry2d/tests/query/point_triangle.rs | 4 +- crates/parry3d/examples/project_point3d.rs | 6 +- .../parry3d/examples/solid_point_query3d.rs | 9 +- .../parry3d/tests/geometry/cuboid_ray_cast.rs | 4 +- .../tests/geometry/cylinder_cuboid_contact.rs | 6 ++ crates/parry3d/tests/geometry/epa3.rs | 15 ++- .../closest_points_ball_convex_polyhedron.rs | 11 ++- .../closest_points_cuboid_cuboid.rs | 4 +- .../closest_points_cuboid_triangle.rs | 4 +- .../contact/contact_ball_convex_polyhedron.rs | 8 +- src/query/contact/contact_cuboid_cuboid.rs | 4 +- .../contact_manifolds_convex_ball.rs | 7 +- src/query/default_query_dispatcher.rs | 36 +++++-- .../distance_ball_convex_polyhedron.rs | 7 +- src/query/epa/epa3.rs | 3 +- src/query/gjk/gjk.rs | 18 +++- src/query/gjk/voronoi_simplex2.rs | 8 +- src/query/gjk/voronoi_simplex3.rs | 12 +-- .../intersection_test_ball_point_query.rs | 2 +- src/query/point/point_aabb.rs | 18 +++- src/query/point/point_ball.rs | 23 ++++- src/query/point/point_bounding_sphere.rs | 29 ++++-- src/query/point/point_capsule.rs | 16 ++- src/query/point/point_composite_shape.rs | 59 ++++++++--- src/query/point/point_cone.rs | 16 ++- src/query/point/point_cuboid.rs | 24 +++-- src/query/point/point_cylinder.rs | 14 ++- src/query/point/point_halfspace.rs | 23 ++++- src/query/point/point_heightfield.rs | 22 ++++- src/query/point/point_query.rs | 97 +++++++++++++++---- src/query/point/point_round_shape.rs | 20 ++-- src/query/point/point_segment.rs | 15 ++- src/query/point/point_support_map.rs | 30 ++++-- src/query/point/point_tetrahedron.rs | 15 ++- src/query/point/point_triangle.rs | 17 +++- src/query/split/split_trimesh.rs | 3 +- .../composite_closest_point_visitor.rs | 4 +- .../composite_point_containment_test.rs | 6 +- src/shape/polyline.rs | 2 +- .../mesh_intersection/mesh_intersection.rs | 28 +++++- .../triangle_triangle_intersection.rs | 10 +- src/transformation/polygon_intersection.rs | 8 +- 47 files changed, 536 insertions(+), 180 deletions(-) diff --git a/crates/parry2d/examples/project_point2d.rs b/crates/parry2d/examples/project_point2d.rs index ba615a0f..05910de8 100644 --- a/crates/parry2d/examples/project_point2d.rs +++ b/crates/parry2d/examples/project_point2d.rs @@ -38,6 +38,7 @@ async fn main() { &Isometry::from_parts(translation, rot), &na_from_mquad(point_to_project), true, + &(), ); /* @@ -66,7 +67,8 @@ async fn main() { // fixed local point inside the shape let point_to_project = Vec2::ZERO; - let projected_point = trimesh.project_local_point(&na_from_mquad(point_to_project), true); + let projected_point = + trimesh.project_local_point(&na_from_mquad(point_to_project), true, &()); let color = if projected_point.is_inside { RED } else { diff --git a/crates/parry2d/examples/solid_point_query2d.rs b/crates/parry2d/examples/solid_point_query2d.rs index 7885d57a..4bb6bd91 100644 --- a/crates/parry2d/examples/solid_point_query2d.rs +++ b/crates/parry2d/examples/solid_point_query2d.rs @@ -9,25 +9,26 @@ fn main() { let pt_inside = Point2::origin(); let pt_outside = Point2::new(2.0, 2.0); + let options = &(); // Solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_inside, true), + cuboid.distance_to_point(&Isometry2::identity(), &pt_inside, true, options), 0.0 ); // Non-solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_inside, false), + cuboid.distance_to_point(&Isometry2::identity(), &pt_inside, false, options), -1.0 ); // The other point is outside of the cuboid so the `solid` flag has no effect. assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_outside, false), + cuboid.distance_to_point(&Isometry2::identity(), &pt_outside, false, options), 1.0 ); assert_eq!( - cuboid.distance_to_point(&Isometry2::identity(), &pt_outside, true), + cuboid.distance_to_point(&Isometry2::identity(), &pt_outside, true, options), 1.0 ); } diff --git a/crates/parry2d/tests/geometry/epa2.rs b/crates/parry2d/tests/geometry/epa2.rs index 1481b95a..859317c6 100644 --- a/crates/parry2d/tests/geometry/epa2.rs +++ b/crates/parry2d/tests/geometry/epa2.rs @@ -1,4 +1,5 @@ use na::{self, Isometry2, Vector2}; +use parry2d::query::gjk::GjkOptions; use parry2d::query::{self, ContactManifold, DefaultQueryDispatcher, PersistentQueryDispatcher}; use parry2d::shape::Cuboid; @@ -8,15 +9,17 @@ fn cuboid_cuboid_EPA() { let c = Cuboid::new(Vector2::new(2.0, 1.0)); let m1 = Isometry2::translation(3.5, 0.0); let m2 = Isometry2::identity(); - - let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) - .expect("Penetration not found."); + let options = GjkOptions::default(); + let res = + query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0, &options) + .expect("Penetration not found."); assert_eq!(res.dist, -0.5); assert_eq!(res.normal1, -Vector2::x_axis()); let m1 = Isometry2::translation(0.0, 0.2); - let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) - .expect("Penetration not found."); + let res = + query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0, &options) + .expect("Penetration not found."); assert_eq!(res.dist, -1.8); assert_eq!(res.normal1, -Vector2::y_axis()); } diff --git a/crates/parry2d/tests/geometry/epa_convergence.rs b/crates/parry2d/tests/geometry/epa_convergence.rs index e2206104..f19979eb 100644 --- a/crates/parry2d/tests/geometry/epa_convergence.rs +++ b/crates/parry2d/tests/geometry/epa_convergence.rs @@ -1,7 +1,7 @@ use na::Vector2; use parry2d::{ math::{Isometry, Point, Real}, - query, + query::{self, gjk::GjkOptions}, shape::{Capsule, ConvexPolygon, SharedShape}, }; @@ -23,6 +23,7 @@ fn capsule_convergence() { &shape1, &shape2, 10.0, + &GjkOptions::default(), ) .expect("Penetration not found."); let shared_shape1 = SharedShape::new(shape1); diff --git a/crates/parry2d/tests/query/point_composite_shape.rs b/crates/parry2d/tests/query/point_composite_shape.rs index dd113714..a4d857ae 100644 --- a/crates/parry2d/tests/query/point_composite_shape.rs +++ b/crates/parry2d/tests/query/point_composite_shape.rs @@ -1,5 +1,8 @@ use na::Point2; -use parry2d::{query::PointQuery, shape::TriMesh}; +use parry2d::{ + query::{gjk::GjkOptions, PointQuery}, + shape::TriMesh, +}; #[test] fn project_local_point_and_get_feature_gets_the_enclosing_triangle() { @@ -11,14 +14,15 @@ fn project_local_point_and_get_feature_gets_the_enclosing_triangle() { ]; let mesh = TriMesh::new(vertices, vec![[0, 1, 2], [3, 0, 2]]).unwrap(); + let options = &(); let query_pt = Point2::new(0.6, 0.6); // Inside the top-right triangle (index 1) - let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt); + let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt, options); let correct_tri_idx = 1; let correct_tri = mesh.triangle(correct_tri_idx); - let is_inside_correct = correct_tri.contains_local_point(&query_pt); + let is_inside_correct = correct_tri.contains_local_point(&query_pt, options); assert!(is_inside_correct); assert_eq!(proj.is_inside, is_inside_correct); @@ -35,16 +39,16 @@ fn project_local_point_and_get_feature_projects_correctly_from_outside() { ]; let mesh = TriMesh::new(vertices, vec![[0, 1, 2], [3, 0, 2]]).unwrap(); - + let options = &(); { let query_pt = Point2::new(-1.0, 0.0); // Left from the bottom-left triangle (index 0) - let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt); + let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt, options); let correct_tri_idx = 0; let correct_tri = mesh.triangle(correct_tri_idx); - let is_inside_correct = correct_tri.contains_local_point(&query_pt); + let is_inside_correct = correct_tri.contains_local_point(&query_pt, options); assert_eq!(is_inside_correct, false); assert_eq!(proj.is_inside, is_inside_correct); @@ -54,12 +58,12 @@ fn project_local_point_and_get_feature_projects_correctly_from_outside() { { let query_pt = Point2::new(0.5, 2.0); // Above the top-right triangle (index 1) - let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt); + let (proj, feat) = mesh.project_local_point_and_get_feature(&query_pt, options); let correct_tri_idx = 1; let correct_tri = mesh.triangle(correct_tri_idx); - let is_inside_correct = correct_tri.contains_local_point(&query_pt); + let is_inside_correct = correct_tri.contains_local_point(&query_pt, options); assert_eq!(is_inside_correct, false); assert_eq!(proj.is_inside, is_inside_correct); diff --git a/crates/parry2d/tests/query/point_triangle.rs b/crates/parry2d/tests/query/point_triangle.rs index 75fde225..a16f1d9d 100644 --- a/crates/parry2d/tests/query/point_triangle.rs +++ b/crates/parry2d/tests/query/point_triangle.rs @@ -12,8 +12,8 @@ fn project_local_point_point_on_ab() { let query_pt = Point::new(1.4, 1.0); - let proj1 = tri1.project_local_point(&query_pt, false); // Used to fail on 0.14 and earlier - let proj2 = tri2.project_local_point(&query_pt, false); + let proj1 = tri1.project_local_point(&query_pt, false, &()); // Used to fail on 0.14 and earlier + let proj2 = tri2.project_local_point(&query_pt, false, &()); assert_eq!(proj1.point, proj2.point); assert_eq!(proj1.point, query_pt); diff --git a/crates/parry3d/examples/project_point3d.rs b/crates/parry3d/examples/project_point3d.rs index c629030b..cfe573ed 100644 --- a/crates/parry3d/examples/project_point3d.rs +++ b/crates/parry3d/examples/project_point3d.rs @@ -24,7 +24,8 @@ async fn main() { let slow_elapsed_time = elapsed_time / 3.0; let point_to_project = lissajous_3d(slow_elapsed_time); - let projected_point = trimesh.project_local_point(&na_from_mquad(point_to_project), true); + let projected_point = + trimesh.project_local_point(&na_from_mquad(point_to_project), true, &()); let slow_elapsed_time = slow_elapsed_time * 0.7; // Setup 3D camera. @@ -65,7 +66,8 @@ async fn main() { // fixed point inside the shape let point_to_project = Vec3::ZERO; - let projected_point = trimesh.project_local_point(&na_from_mquad(point_to_project), true); + let projected_point = + trimesh.project_local_point(&na_from_mquad(point_to_project), true, &()); let color = if projected_point.is_inside { RED } else { diff --git a/crates/parry3d/examples/solid_point_query3d.rs b/crates/parry3d/examples/solid_point_query3d.rs index 4d0efed4..b84c08ee 100644 --- a/crates/parry3d/examples/solid_point_query3d.rs +++ b/crates/parry3d/examples/solid_point_query3d.rs @@ -8,26 +8,27 @@ fn main() { let cuboid = Cuboid::new(Vector3::new(1.0, 2.0, 2.0)); let pt_inside = Point3::origin(); let pt_outside = Point3::new(2.0, 2.0, 2.0); + let options = &(); // Solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_inside, true), + cuboid.distance_to_point(&Isometry3::identity(), &pt_inside, true, options), 0.0 ); // Non-solid projection. assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_inside, false), + cuboid.distance_to_point(&Isometry3::identity(), &pt_inside, false, options), -1.0 ); // The other point is outside of the cuboid so the `solid` flag has no effect. assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_outside, false), + cuboid.distance_to_point(&Isometry3::identity(), &pt_outside, false, options), 1.0 ); assert_eq!( - cuboid.distance_to_point(&Isometry3::identity(), &pt_outside, true), + cuboid.distance_to_point(&Isometry3::identity(), &pt_outside, true, options), 1.0 ); } diff --git a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs index 9c1cf7c2..082390ec 100644 --- a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs +++ b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs @@ -42,7 +42,7 @@ where let point_nudged_out = point + intersection.normal * 0.001; assert!( - shape.contains_point(&position, &point_nudged_in), + shape.contains_point(&position, &point_nudged_in, &()), "Shape {} rotated with {:#?} does not contain point nudged in {:#?}", name, rotation.axis(), @@ -50,7 +50,7 @@ where ); assert!( - !shape.contains_point(&position, &point_nudged_out), + !shape.contains_point(&position, &point_nudged_out, &()), "Shape {} rotated with {:#?} does contains point nudged out {:#?}", name, rotation.axis(), diff --git a/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs b/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs index e4817b20..9d7a7eba 100644 --- a/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs +++ b/crates/parry3d/tests/geometry/cylinder_cuboid_contact.rs @@ -1,5 +1,6 @@ use na::{self, Isometry3, Vector3}; use parry3d::query; +use parry3d::query::gjk::GjkOptions; use parry3d::shape::{Cuboid, Cylinder}; // Issue #157. @@ -9,16 +10,20 @@ fn cylinder_cuboid_contact() { let cyl_at = Isometry3::translation(10.97, 0.925, 61.02); let cuboid = Cuboid::new(Vector3::new(0.05, 0.75, 0.5)); let cuboid_at = Isometry3::translation(11.50, 0.75, 60.5); + let options = GjkOptions::default(); + let distance = query::details::distance_support_map_support_map( &cyl_at.inv_mul(&cuboid_at), &cyl, &cuboid, + &options, ); let intersecting = query::details::intersection_test_support_map_support_map( &cyl_at.inv_mul(&cuboid_at), &cyl, &cuboid, + &options, ); let contact = query::details::contact_support_map_support_map( @@ -26,6 +31,7 @@ fn cylinder_cuboid_contact() { &cyl, &cuboid, 10.0, + &options, ); assert!(distance == 0.0); diff --git a/crates/parry3d/tests/geometry/epa3.rs b/crates/parry3d/tests/geometry/epa3.rs index e77c38f3..a60829df 100644 --- a/crates/parry3d/tests/geometry/epa3.rs +++ b/crates/parry3d/tests/geometry/epa3.rs @@ -1,6 +1,6 @@ use na::{self, Isometry3, Point3, Vector3}; use parry3d::query; -use parry3d::query::gjk::VoronoiSimplex; +use parry3d::query::gjk::{GjkOptions, VoronoiSimplex}; use parry3d::shape::{Cuboid, Triangle}; #[test] @@ -9,15 +9,18 @@ fn cuboid_cuboid_EPA() { let c = Cuboid::new(Vector3::new(2.0, 1.0, 1.0)); let m1 = Isometry3::translation(3.5, 0.0, 0.0); let m2 = Isometry3::identity(); + let options = GjkOptions::default(); - let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) - .expect("Penetration not found."); + let res = + query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0, &options) + .expect("Penetration not found."); assert_eq!(res.dist, -0.5); assert_eq!(res.normal1, -Vector3::x_axis()); let m1 = Isometry3::translation(0.0, 0.2, 0.0); - let res = query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0) - .expect("Penetration not found."); + let res = + query::details::contact_support_map_support_map(&m1.inv_mul(&m2), &c, &c, 10.0, &options) + .expect("Penetration not found."); assert_eq!(res.dist, -1.8); assert_eq!(res.normal1, -Vector3::y_axis()); } @@ -38,6 +41,7 @@ fn triangle_vertex_touches_triangle_edge_epa() { Point3::new(-2.349647, 0.0, 11.037681), Point3::new(-2.349647, 1.0, 11.037681), ); + let options = GjkOptions::default(); let gjk_result = query::details::contact_support_map_support_map_with_params( &Isometry3::identity(), @@ -46,6 +50,7 @@ fn triangle_vertex_touches_triangle_edge_epa() { 0.00999999977, &mut VoronoiSimplex::new(), None, + &options, ); let query::gjk::GJKResult::ClosestPoints(a, _b, _normal) = &gjk_result else { diff --git a/src/query/closest_points/closest_points_ball_convex_polyhedron.rs b/src/query/closest_points/closest_points_ball_convex_polyhedron.rs index 49f59280..dcd70a0e 100644 --- a/src/query/closest_points/closest_points_ball_convex_polyhedron.rs +++ b/src/query/closest_points/closest_points_ball_convex_polyhedron.rs @@ -1,4 +1,5 @@ use crate::math::{Isometry, Real}; +use crate::query::point::point_query::QueryOptions; use crate::query::ClosestPoints; use crate::shape::{Ball, Shape}; @@ -12,8 +13,11 @@ pub fn closest_points_ball_convex_polyhedron( ball1: &Ball, shape2: &(impl Shape + ?Sized), prediction: Real, + options: &dyn QueryOptions, ) -> ClosestPoints { - match crate::query::details::contact_ball_convex_polyhedron(pos12, ball1, shape2, prediction) { + match crate::query::details::contact_ball_convex_polyhedron( + pos12, ball1, shape2, prediction, options, + ) { Some(contact) => { if contact.dist <= 0.0 { ClosestPoints::Intersecting @@ -35,8 +39,11 @@ pub fn closest_points_convex_polyhedron_ball( shape1: &(impl Shape + ?Sized), ball2: &Ball, prediction: Real, + options: &dyn QueryOptions, ) -> ClosestPoints { - match crate::query::details::contact_convex_polyhedron_ball(pos12, shape1, ball2, prediction) { + match crate::query::details::contact_convex_polyhedron_ball( + pos12, shape1, ball2, prediction, options, + ) { Some(contact) => { if contact.dist <= 0.0 { ClosestPoints::Intersecting diff --git a/src/query/closest_points/closest_points_cuboid_cuboid.rs b/src/query/closest_points/closest_points_cuboid_cuboid.rs index 81173b3d..2d26a603 100644 --- a/src/query/closest_points/closest_points_cuboid_cuboid.rs +++ b/src/query/closest_points/closest_points_cuboid_cuboid.rs @@ -42,7 +42,7 @@ pub fn closest_points_cuboid_cuboid( // from cuboid2 on the support-face of cuboid1. For simplicity, we just // project the support point from cuboid2 on cuboid1 itself (not just the face). let pt2_1 = cuboid2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, true); + let proj1 = cuboid1.project_local_point(&pt2_1, true, &()); if na::distance_squared(&proj1.point, &pt2_1) > margin * margin { return ClosestPoints::Disjoint; } else { @@ -58,7 +58,7 @@ pub fn closest_points_cuboid_cuboid( // from cuboid1 on the support-face of cuboid2. For simplicity, we just // project the support point from cuboid1 on cuboid2 itself (not just the face). let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = cuboid2.project_local_point(&pt1_2, true); + let proj2 = cuboid2.project_local_point(&pt1_2, true, &()); if na::distance_squared(&proj2.point, &pt1_2) > margin * margin { return ClosestPoints::Disjoint; diff --git a/src/query/closest_points/closest_points_cuboid_triangle.rs b/src/query/closest_points/closest_points_cuboid_triangle.rs index 0ac6449d..5f418b25 100644 --- a/src/query/closest_points/closest_points_cuboid_triangle.rs +++ b/src/query/closest_points/closest_points_cuboid_triangle.rs @@ -41,7 +41,7 @@ pub fn closest_points_cuboid_triangle( // from triangle2 on the support-face of triangle1. For simplicity, we just // project the support point from triangle2 on cuboid1 itself (not just the face). let pt2_1 = triangle2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, true); + let proj1 = cuboid1.project_local_point(&pt2_1, true, &()); if na::distance_squared(&proj1.point, &pt2_1) > margin * margin { return ClosestPoints::Disjoint; } else { @@ -55,7 +55,7 @@ pub fn closest_points_cuboid_triangle( // from cuboid1 on the support-face of triangle2. For simplicity, we just // project the support point from cuboid1 on triangle2 itself (not just the face). let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = triangle2.project_local_point(&pt1_2, true); + let proj2 = triangle2.project_local_point(&pt1_2, true, &()); if na::distance_squared(&proj2.point, &pt1_2) > margin * margin { return ClosestPoints::Disjoint; diff --git a/src/query/contact/contact_ball_convex_polyhedron.rs b/src/query/contact/contact_ball_convex_polyhedron.rs index 15786941..78695bf9 100644 --- a/src/query/contact/contact_ball_convex_polyhedron.rs +++ b/src/query/contact/contact_ball_convex_polyhedron.rs @@ -1,4 +1,5 @@ use crate::math::{Isometry, Point, Real, Vector}; +use crate::query::point::point_query::QueryOptions; use crate::query::Contact; use crate::shape::{Ball, Shape}; @@ -14,8 +15,10 @@ pub fn contact_ball_convex_polyhedron( ball1: &Ball, shape2: &(impl Shape + ?Sized), prediction: Real, + options: &dyn QueryOptions, ) -> Option { - contact_convex_polyhedron_ball(&pos12.inverse(), shape2, ball1, prediction).map(|c| c.flipped()) + contact_convex_polyhedron_ball(&pos12.inverse(), shape2, ball1, prediction, options) + .map(|c| c.flipped()) } /// Contact between a convex polyhedron and a ball. @@ -28,9 +31,10 @@ pub fn contact_convex_polyhedron_ball( shape1: &(impl Shape + ?Sized), ball2: &Ball, prediction: Real, + options: &dyn QueryOptions, ) -> Option { let center2_1 = Point::from(pos12.translation.vector); - let (proj, f1) = shape1.project_local_point_and_get_feature(¢er2_1); + let (proj, f1) = shape1.project_local_point_and_get_feature(¢er2_1, options); let dist; let normal1; diff --git a/src/query/contact/contact_cuboid_cuboid.rs b/src/query/contact/contact_cuboid_cuboid.rs index 00259f5d..91c49251 100644 --- a/src/query/contact/contact_cuboid_cuboid.rs +++ b/src/query/contact/contact_cuboid_cuboid.rs @@ -38,7 +38,7 @@ pub fn contact_cuboid_cuboid( // from cuboid2 on the support-face of cuboid1. For simplicity, we just // project the support point from cuboid2 on cuboid1 itself (not just the face). let pt2_1 = cuboid2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, false); + let proj1 = cuboid1.project_local_point(&pt2_1, false, &()); let separation = (pt2_1 - proj1.point).dot(&sep1.1); let normalized_dir = Unit::try_new_and_get(pt2_1 - proj1.point, Real::default_epsilon()); @@ -77,7 +77,7 @@ pub fn contact_cuboid_cuboid( // from cuboid1 on the support-face of cuboid2. For simplicity, we just // project the support point from cuboid1 on cuboid2 itself (not just the face). let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = cuboid2.project_local_point(&pt1_2, false); + let proj2 = cuboid2.project_local_point(&pt1_2, false, &()); let separation = (pt1_2 - proj2.point).dot(&sep2.1); let normalized_dir = Unit::try_new_and_get(pt1_2 - proj2.point, Real::default_epsilon()); diff --git a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs index 3fcbb772..8abdad20 100644 --- a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs @@ -1,5 +1,6 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; +use crate::query::point::point_query::QueryOptions; use crate::query::{ContactManifold, Ray, TrackedContact}; use crate::shape::{Ball, PackedFeatureId, Shape}; use na::Unit; @@ -13,6 +14,7 @@ pub fn contact_manifold_convex_ball_shapes( normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, + options: &dyn QueryOptions, ) where ContactData: Default + Copy, { @@ -26,6 +28,7 @@ pub fn contact_manifold_convex_ball_shapes( prediction, manifold, true, + options, ); } else if let Some(ball2) = shape2.as_ball() { contact_manifold_convex_ball( @@ -37,6 +40,7 @@ pub fn contact_manifold_convex_ball_shapes( prediction, manifold, false, + options, ); } } @@ -51,12 +55,13 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( prediction: Real, manifold: &mut ContactManifold, flipped: bool, + options: &dyn QueryOptions, ) where S1: ?Sized + Shape, ContactData: Default + Copy, { let local_p2_1 = Point::from(pos12.translation.vector); - let (proj, mut fid1) = shape1.project_local_point_and_get_feature(&local_p2_1); + let (proj, mut fid1) = shape1.project_local_point_and_get_feature(&local_p2_1, options); let mut local_p1 = proj.point; let dpos = local_p2_1 - local_p1; diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index d4da5a62..b17289b1 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -102,11 +102,17 @@ impl QueryDispatcher for DefaultQueryDispatcher { Ok(query::details::distance_ball_ball(b1, &p2, b2)) } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { Ok(query::details::distance_ball_convex_polyhedron( - pos12, b1, shape2, + pos12, + b1, + shape2, + &self.gjk_options, )) } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) { Ok(query::details::distance_convex_polyhedron_ball( - pos12, shape1, b2, + pos12, + shape1, + b2, + &self.gjk_options, )) } else if let (Some(c1), Some(c2)) = (shape1.as_cuboid(), shape2.as_cuboid()) { Ok(query::details::distance_cuboid_cuboid(pos12, c1, c2)) @@ -177,11 +183,19 @@ impl QueryDispatcher for DefaultQueryDispatcher { )) } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { Ok(query::details::contact_ball_convex_polyhedron( - pos12, b1, shape2, prediction, + pos12, + b1, + shape2, + prediction, + &self.gjk_options, )) } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) { Ok(query::details::contact_convex_polyhedron_ball( - pos12, shape1, b2, prediction, + pos12, + shape1, + b2, + prediction, + &self.gjk_options, )) } else { #[cfg(feature = "std")] @@ -223,11 +237,19 @@ impl QueryDispatcher for DefaultQueryDispatcher { )) } else if let (Some(b1), true) = (ball1, shape2.is_convex()) { Ok(query::details::closest_points_ball_convex_polyhedron( - pos12, b1, shape2, max_dist, + pos12, + b1, + shape2, + max_dist, + &self.gjk_options, )) } else if let (true, Some(b2)) = (shape1.is_convex(), ball2) { Ok(query::details::closest_points_convex_polyhedron_ball( - pos12, shape1, b2, max_dist, + pos12, + shape1, + b2, + max_dist, + &self.gjk_options, )) } else if let (Some(s1), Some(s2)) = (shape1.as_shape::(), shape2.as_shape::()) @@ -576,7 +598,7 @@ where contact_manifold_capsule_capsule_shapes(pos12, shape1, shape2, prediction, manifold) } (_, ShapeType::Ball) | (ShapeType::Ball, _) => { - contact_manifold_convex_ball_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold) + contact_manifold_convex_ball_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold, &self.gjk_options) } // (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => // contact_manifold_cuboid_capsule_shapes(pos12, shape1, shape2, prediction, manifold), diff --git a/src/query/distance/distance_ball_convex_polyhedron.rs b/src/query/distance/distance_ball_convex_polyhedron.rs index 3b1f0f31..c6141a4f 100644 --- a/src/query/distance/distance_ball_convex_polyhedron.rs +++ b/src/query/distance/distance_ball_convex_polyhedron.rs @@ -1,4 +1,5 @@ use crate::math::{Isometry, Point, Real}; +use crate::query::point::point_query::QueryOptions; use crate::shape::{Ball, Shape}; /// Distance between a ball and a convex polyhedron. @@ -10,8 +11,9 @@ pub fn distance_ball_convex_polyhedron( pos12: &Isometry, ball1: &Ball, shape2: &(impl Shape + ?Sized), + options: &dyn QueryOptions, ) -> Real { - distance_convex_polyhedron_ball(&pos12.inverse(), shape2, ball1) + distance_convex_polyhedron_ball(&pos12.inverse(), shape2, ball1, options) } /// Distance between a convex polyhedron and a ball. @@ -23,8 +25,9 @@ pub fn distance_convex_polyhedron_ball( pos12: &Isometry, shape1: &(impl Shape + ?Sized), ball2: &Ball, + options: &dyn QueryOptions, ) -> Real { let center2_1 = Point::from(pos12.translation.vector); - let proj = shape1.project_local_point(¢er2_1, true); + let proj = shape1.project_local_point(¢er2_1, true, options); (na::distance(&proj.point, ¢er2_1) - ball2.radius).max(0.0) } diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 667c6798..7a1eacd6 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -95,7 +95,8 @@ impl Face { vertices[pts[1]].point, vertices[pts[2]].point, ); - let (proj, loc) = tri.project_local_point_and_get_location(&Point::::origin(), true); + let (proj, loc) = + tri.project_local_point_and_get_location(&Point::::origin(), true, &()); match loc { TrianglePointLocation::OnVertex(_) | TrianglePointLocation::OnEdge(_, _) => { diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index cd7ad1df..deb67175 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -3,6 +3,7 @@ use na::{self, ComplexField, Unit}; use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::point::point_query::QueryOptions; use crate::shape::SupportMap; // use query::Proximity; use crate::math::{Isometry, Point, Real, Vector, DIM}; @@ -52,6 +53,12 @@ impl Default for GjkOptions { } } +impl QueryOptions for GjkOptions { + fn as_any(&self) -> &dyn core::any::Any { + self + } +} + /// The absolute tolerance used by the GJK algorithm. pub fn eps_tol() -> Real { let _eps = crate::math::DEFAULT_EPSILON; @@ -426,7 +433,7 @@ mod test { use crate::{ math::Real, - query::{self, ShapeCastOptions}, + query::{self, gjk::GjkOptions, DefaultQueryDispatcher, ShapeCastOptions}, shape::{Ball, ConvexPolygon, Shape}, }; @@ -460,7 +467,13 @@ mod test { let vel2 = [0.0, 0.0]; let g2 = to_cast_against.clone_dyn(); - let toi = query::cast_shapes( + let dispatcher = DefaultQueryDispatcher { + gjk_options: GjkOptions { + espilon_tolerance: crate::math::DEFAULT_EPSILON * 10000.0, + ..GjkOptions::default() + }, + }; + let toi = query::cast_shapes_with_dispatcher( &source_pos.into(), &vel1, &g1, @@ -468,6 +481,7 @@ mod test { &vel2.into(), &*g2, ShapeCastOptions::with_max_time_of_impact(1.0), + dispatcher, ) .unwrap(); assert!( diff --git a/src/query/gjk/voronoi_simplex2.rs b/src/query/gjk/voronoi_simplex2.rs index db22e9b4..2939f7ba 100644 --- a/src/query/gjk/voronoi_simplex2.rs +++ b/src/query/gjk/voronoi_simplex2.rs @@ -99,7 +99,7 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(&Point::::origin(), true, &()); match location { SegmentPointLocation::OnVertex(0) => { @@ -125,7 +125,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(&Point::::origin(), true, &()); match location { TrianglePointLocation::OnVertex(i) => { @@ -161,7 +161,7 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point(&Point::::origin(), true) + seg.project_local_point(&Point::::origin(), true, &()) .point } else { assert!(self.dim == 2); @@ -170,7 +170,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ); - tri.project_local_point(&Point::::origin(), true) + tri.project_local_point(&Point::::origin(), true, &()) .point } } diff --git a/src/query/gjk/voronoi_simplex3.rs b/src/query/gjk/voronoi_simplex3.rs index 782f8153..a4be41ad 100644 --- a/src/query/gjk/voronoi_simplex3.rs +++ b/src/query/gjk/voronoi_simplex3.rs @@ -126,7 +126,7 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(&Point::::origin(), true, &()); match location { SegmentPointLocation::OnVertex(0) => { @@ -152,7 +152,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(&Point::::origin(), true, &()); match location { TrianglePointLocation::OnVertex(i) => { @@ -192,7 +192,7 @@ impl VoronoiSimplex { self.vertices[2].point, self.vertices[3].point, ) - .project_local_point_and_get_location(&Point::::origin(), true); + .project_local_point_and_get_location(&Point::::origin(), true, &()); match location { TetrahedronPointLocation::OnVertex(i) => { @@ -284,7 +284,7 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point(&Point::::origin(), true) + seg.project_local_point(&Point::::origin(), true, &()) .point } else if self.dim == 2 { let tri = Triangle::new( @@ -292,7 +292,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ); - tri.project_local_point(&Point::::origin(), true) + tri.project_local_point(&Point::::origin(), true, &()) .point } else { let tetr = Tetrahedron::new( @@ -301,7 +301,7 @@ impl VoronoiSimplex { self.vertices[2].point, self.vertices[3].point, ); - tetr.project_local_point(&Point::::origin(), true) + tetr.project_local_point(&Point::::origin(), true, &()) .point } } diff --git a/src/query/intersection_test/intersection_test_ball_point_query.rs b/src/query/intersection_test/intersection_test_ball_point_query.rs index 8dc68b7b..936af232 100644 --- a/src/query/intersection_test/intersection_test_ball_point_query.rs +++ b/src/query/intersection_test/intersection_test_ball_point_query.rs @@ -18,6 +18,6 @@ pub fn intersection_test_point_query_ball( ball2: &Ball, ) -> bool { let local_p2_1 = Point::from(pos12.translation.vector); - let proj = point_query1.project_local_point(&local_p2_1, true); + let proj = point_query1.project_local_point(&local_p2_1, true, &()); proj.is_inside || (local_p2_1 - proj.point).norm_squared() <= ball2.radius * ball2.radius } diff --git a/src/query/point/point_aabb.rs b/src/query/point/point_aabb.rs index 52a3ede3..50f97f74 100644 --- a/src/query/point/point_aabb.rs +++ b/src/query/point/point_aabb.rs @@ -1,6 +1,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real, Vector, DIM}; use crate::num::{Bounded, Zero}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::FeatureId; use na; @@ -59,7 +60,12 @@ impl Aabb { impl PointQuery for Aabb { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> PointProjection { let (inside, ls_pt, _) = self.do_project_local_point(pt, solid); PointProjection::new(inside, ls_pt) } @@ -70,6 +76,7 @@ impl PointQuery for Aabb { fn project_local_point_and_get_feature( &self, pt: &Point, + _options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { let (inside, ls_pt, shift) = self.do_project_local_point(pt, false); let proj = PointProjection::new(inside, ls_pt); @@ -132,7 +139,12 @@ impl PointQuery for Aabb { } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { + fn distance_to_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> Real { let mins_pt = self.mins - pt; let pt_maxs = pt - self.maxs; let shift = mins_pt.sup(&pt_maxs).sup(&na::zero()); @@ -141,7 +153,7 @@ impl PointQuery for Aabb { shift.norm() } else { // TODO: optimize that. - -na::distance(pt, &self.project_local_point(pt, solid).point) + -na::distance(pt, &self.project_local_point(pt, solid, options).point) } } } diff --git a/src/query/point/point_ball.rs b/src/query/point/point_ball.rs index b3ba0b82..c8ae49e4 100644 --- a/src/query/point/point_ball.rs +++ b/src/query/point/point_ball.rs @@ -1,12 +1,18 @@ use na::{self, ComplexField}; use crate::math::{Point, Real}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Ball, FeatureId}; impl PointQuery for Ball { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> PointProjection { let distance_squared = pt.coords.norm_squared(); let inside = distance_squared <= self.radius * self.radius; @@ -24,12 +30,21 @@ impl PointQuery for Ball { fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - (self.project_local_point(pt, false), FeatureId::Face(0)) + ( + self.project_local_point(pt, false, options), + FeatureId::Face(0), + ) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { + fn distance_to_local_point( + &self, + pt: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> Real { let dist = pt.coords.norm() - self.radius; if solid && dist < 0.0 { @@ -40,7 +55,7 @@ impl PointQuery for Ball { } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { + fn contains_local_point(&self, pt: &Point, _options: &dyn QueryOptions) -> bool { pt.coords.norm_squared() <= self.radius * self.radius } } diff --git a/src/query/point/point_bounding_sphere.rs b/src/query/point/point_bounding_sphere.rs index 7dda5a70..89c31be8 100644 --- a/src/query/point/point_bounding_sphere.rs +++ b/src/query/point/point_bounding_sphere.rs @@ -1,13 +1,19 @@ use crate::bounding_volume::BoundingSphere; use crate::math::{Point, Real}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Ball, FeatureId}; impl PointQuery for BoundingSphere { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { let centered_pt = pt - self.center().coords; - let mut proj = Ball::new(self.radius()).project_local_point(¢ered_pt, solid); + let mut proj = Ball::new(self.radius()).project_local_point(¢ered_pt, solid, options); proj.point += self.center().coords; proj @@ -17,19 +23,28 @@ impl PointQuery for BoundingSphere { fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - (self.project_local_point(pt, false), FeatureId::Face(0)) + ( + self.project_local_point(pt, false, options), + FeatureId::Face(0), + ) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { + fn distance_to_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> Real { let centered_pt = pt - self.center().coords; - Ball::new(self.radius()).distance_to_local_point(¢ered_pt, solid) + Ball::new(self.radius()).distance_to_local_point(¢ered_pt, solid, options) } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { + fn contains_local_point(&self, pt: &Point, options: &dyn QueryOptions) -> bool { let centered_pt = pt - self.center().coords; - Ball::new(self.radius()).contains_local_point(¢ered_pt) + Ball::new(self.radius()).contains_local_point(¢ered_pt, options) } } diff --git a/src/query/point/point_capsule.rs b/src/query/point/point_capsule.rs index 94cccefc..38eb4c92 100644 --- a/src/query/point/point_capsule.rs +++ b/src/query/point/point_capsule.rs @@ -1,14 +1,20 @@ use crate::approx::AbsDiffEq; use crate::math::{Point, Real, Vector}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Capsule, FeatureId, Segment}; use na::{self, Unit}; impl PointQuery for Capsule { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { let seg = Segment::new(self.segment.a, self.segment.b); - let proj = seg.project_local_point(pt, solid); + let proj = seg.project_local_point(pt, solid, options); let dproj = *pt - proj.point; if let Some((dir, dist)) = Unit::try_new_and_get(dproj, Real::default_epsilon()) { @@ -45,7 +51,11 @@ impl PointQuery for Capsule { fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - (self.project_local_point(pt, false), FeatureId::Face(0)) + ( + self.project_local_point(pt, false, options), + FeatureId::Face(0), + ) } } diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index 979f942c..95c30d21 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -3,6 +3,7 @@ use crate::bounding_volume::SimdAabb; use crate::math::{Point, Real, SimdReal, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; +use crate::query::point::point_query::QueryOptions; use crate::query::visitors::CompositePointContainmentTest; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{ @@ -15,14 +16,21 @@ use crate::shape::{Compound, Polyline}; impl PointQuery for Polyline { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - self.project_local_point_and_get_location(point, solid).0 + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + self.project_local_point_and_get_location(point, solid, options) + .0 } #[inline] fn project_local_point_and_get_feature( &self, point: &Point, + _options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { let mut visitor = PointCompositeShapeProjWithFeatureBestFirstVisitor::new(self, point, false); @@ -35,7 +43,7 @@ impl PointQuery for Polyline { // TODO: implement distance_to_point too? #[inline] - fn contains_local_point(&self, point: &Point) -> bool { + fn contains_local_point(&self, point: &Point, _options: &dyn QueryOptions) -> bool { let mut visitor = CompositePointContainmentTest::new(self, point); let _ = self.qbvh().traverse_depth_first(&mut visitor); visitor.found @@ -44,19 +52,27 @@ impl PointQuery for Polyline { impl PointQuery for TriMesh { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - self.project_local_point_and_get_location(point, solid).0 + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + self.project_local_point_and_get_location(point, solid, options) + .0 } #[inline] fn project_local_point_and_get_feature( &self, point: &Point, + _options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { #[cfg(feature = "dim3")] if self.pseudo_normals().is_some() { // If we can, in 3D, take the pseudo-normals into account. - let (proj, (id, _feature)) = self.project_local_point_and_get_location(point, false); + let (proj, (id, _feature)) = + self.project_local_point_and_get_location(point, false, _options); let feature_id = FeatureId::Face(id); return (proj, feature_id); } @@ -73,12 +89,12 @@ impl PointQuery for TriMesh { // TODO: implement distance_to_point too? #[inline] - fn contains_local_point(&self, point: &Point) -> bool { + fn contains_local_point(&self, point: &Point, _options: &dyn QueryOptions) -> bool { #[cfg(feature = "dim3")] if self.pseudo_normals.is_some() { // If we can, in 3D, take the pseudo-normals into account. return self - .project_local_point_and_get_location(point, true) + .project_local_point_and_get_location(point, true, _options) .0 .is_inside; } @@ -94,15 +110,21 @@ impl PointQuery for TriMesh { pt: &Point, solid: bool, max_dist: Real, + _options: &dyn QueryOptions, ) -> Option { - self.project_local_point_and_get_location_with_max_dist(pt, solid, max_dist) + self.project_local_point_and_get_location_with_max_dist(pt, solid, max_dist, _options) .map(|proj| proj.0) } } impl PointQuery for Compound { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + point: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> PointProjection { let mut visitor = PointCompositeShapeProjBestFirstVisitor::new(self, point, solid); self.qbvh().traverse_best_first(&mut visitor).unwrap().1 .0 } @@ -111,12 +133,16 @@ impl PointQuery for Compound { fn project_local_point_and_get_feature( &self, point: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - (self.project_local_point(point, false), FeatureId::Unknown) + ( + self.project_local_point(point, false, options), + FeatureId::Unknown, + ) } #[inline] - fn contains_local_point(&self, point: &Point) -> bool { + fn contains_local_point(&self, point: &Point, _options: &dyn QueryOptions) -> bool { let mut visitor = CompositePointContainmentTest::new(self, point); let _ = self.qbvh().traverse_depth_first(&mut visitor); visitor.found @@ -131,6 +157,7 @@ impl PointQueryWithLocation for Polyline { &self, point: &Point, solid: bool, + _options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { let mut visitor = PointCompositeShapeProjWithLocationBestFirstVisitor::new(self, point, solid); @@ -147,8 +174,9 @@ impl PointQueryWithLocation for TriMesh { &self, point: &Point, solid: bool, + options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { - self.project_local_point_and_get_location_with_max_dist(point, solid, Real::MAX) + self.project_local_point_and_get_location_with_max_dist(point, solid, Real::MAX, options) .unwrap() } @@ -158,6 +186,7 @@ impl PointQueryWithLocation for TriMesh { point: &Point, solid: bool, max_dist: Real, + _options: &dyn QueryOptions, ) -> Option<(PointProjection, Self::Location)> { let mut visitor = PointCompositeShapeProjWithLocationBestFirstVisitor::new(self, point, solid); @@ -254,12 +283,12 @@ macro_rules! gen_visitor( part_shape.$project_point( part_pos, self.point - $(, self.$args)* + $(, self.$args)*, &() ) } else { part_shape.$project_local_point( self.point - $(, self.$args)* + $(, self.$args)*, &() ) }; diff --git a/src/query/point/point_cone.rs b/src/query/point/point_cone.rs index 8570daa0..8a2b8430 100644 --- a/src/query/point/point_cone.rs +++ b/src/query/point/point_cone.rs @@ -1,11 +1,17 @@ use crate::math::{Point, Real, Vector}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Cone, FeatureId, Segment}; use na; impl PointQuery for Cone { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { // Project on the basis. let mut dir_from_basis_center = pt.coords.xz(); let planar_dist_from_basis_center = dir_from_basis_center.normalize_mut(); @@ -30,7 +36,7 @@ impl PointQuery for Cone { let apex_point = Point::new(0.0, self.half_height, 0.0); let conic_side_segment = Segment::new(apex_point, projection_on_basis_circle); let conic_side_segment_dir = conic_side_segment.scaled_direction(); - let mut proj = conic_side_segment.project_local_point(pt, true); + let mut proj = conic_side_segment.project_local_point(pt, true, options); let apex_to_basis_center = Vector::new(0.0, -2.0 * self.half_height, 0.0); @@ -65,8 +71,12 @@ impl PointQuery for Cone { fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { // TODO: get the actual feature. - (self.project_local_point(pt, false), FeatureId::Unknown) + ( + self.project_local_point(pt, false, options), + FeatureId::Unknown, + ) } } diff --git a/src/query/point/point_cuboid.rs b/src/query/point/point_cuboid.rs index 3625dc90..637648f9 100644 --- a/src/query/point/point_cuboid.rs +++ b/src/query/point/point_cuboid.rs @@ -1,35 +1,47 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Cuboid, FeatureId}; impl PointQuery for Cuboid { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { let dl = Point::from(-self.half_extents); let ur = Point::from(self.half_extents); - Aabb::new(dl, ur).project_local_point(pt, solid) + Aabb::new(dl, ur).project_local_point(pt, solid, options) } #[inline] fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { let dl = Point::from(-self.half_extents); let ur = Point::from(self.half_extents); - Aabb::new(dl, ur).project_local_point_and_get_feature(pt) + Aabb::new(dl, ur).project_local_point_and_get_feature(pt, options) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { + fn distance_to_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> Real { let dl = Point::from(-self.half_extents); let ur = Point::from(self.half_extents); - Aabb::new(dl, ur).distance_to_local_point(pt, solid) + Aabb::new(dl, ur).distance_to_local_point(pt, solid, options) } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { + fn contains_local_point(&self, pt: &Point, _options: &dyn QueryOptions) -> bool { let dl = Point::from(-self.half_extents); let ur = Point::from(self.half_extents); Aabb::new(dl, ur).contains_local_point(pt) diff --git a/src/query/point/point_cylinder.rs b/src/query/point/point_cylinder.rs index d0e5b505..4ca9512b 100644 --- a/src/query/point/point_cylinder.rs +++ b/src/query/point/point_cylinder.rs @@ -1,11 +1,17 @@ use crate::math::{Point, Real}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{Cylinder, FeatureId}; use na; impl PointQuery for Cylinder { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> PointProjection { // Project on the basis. let mut dir_from_basis_center = pt.coords.xz(); let planar_dist_from_basis_center = dir_from_basis_center.normalize_mut(); @@ -74,8 +80,12 @@ impl PointQuery for Cylinder { fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { // TODO: get the actual feature. - (self.project_local_point(pt, false), FeatureId::Unknown) + ( + self.project_local_point(pt, false, options), + FeatureId::Unknown, + ) } } diff --git a/src/query/point/point_halfspace.rs b/src/query/point/point_halfspace.rs index a34b68b2..3800b5b9 100644 --- a/src/query/point/point_halfspace.rs +++ b/src/query/point/point_halfspace.rs @@ -1,10 +1,16 @@ use crate::math::{Point, Real}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{FeatureId, HalfSpace}; impl PointQuery for HalfSpace { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> PointProjection { let d = self.normal.dot(&pt.coords); let inside = d <= 0.0; @@ -19,12 +25,21 @@ impl PointQuery for HalfSpace { fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - (self.project_local_point(pt, false), FeatureId::Face(0)) + ( + self.project_local_point(pt, false, options), + FeatureId::Face(0), + ) } #[inline] - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { + fn distance_to_local_point( + &self, + pt: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> Real { let dist = self.normal.dot(&pt.coords); if dist < 0.0 && solid { @@ -36,7 +51,7 @@ impl PointQuery for HalfSpace { } #[inline] - fn contains_local_point(&self, pt: &Point) -> bool { + fn contains_local_point(&self, pt: &Point, _options: &dyn QueryOptions) -> bool { self.normal.dot(&pt.coords) <= 0.0 } } diff --git a/src/query/point/point_heightfield.rs b/src/query/point/point_heightfield.rs index ec0abccd..06041843 100644 --- a/src/query/point/point_heightfield.rs +++ b/src/query/point/point_heightfield.rs @@ -1,5 +1,6 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real, Vector}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{FeatureId, HeightField, TrianglePointLocation}; #[cfg(not(feature = "std"))] @@ -11,13 +12,14 @@ impl PointQuery for HeightField { pt: &Point, solid: bool, max_dist: Real, + options: &dyn QueryOptions, ) -> Option { let aabb = Aabb::new(pt - Vector::repeat(max_dist), pt + Vector::repeat(max_dist)); let mut sq_smallest_dist = Real::MAX; let mut best_proj = None; self.map_elements_in_local_aabb(&aabb, &mut |_, triangle| { - let proj = triangle.project_local_point(pt, solid); + let proj = triangle.project_local_point(pt, solid, options); let sq_dist = na::distance_squared(pt, &proj.point); if sq_dist < sq_smallest_dist { @@ -33,7 +35,12 @@ impl PointQuery for HeightField { } #[inline] - fn project_local_point(&self, point: &Point, _: bool) -> PointProjection { + fn project_local_point( + &self, + point: &Point, + _: bool, + options: &dyn QueryOptions, + ) -> PointProjection { let mut smallest_dist = Real::MAX; let mut best_proj = PointProjection::new(false, *point); @@ -42,7 +49,7 @@ impl PointQuery for HeightField { #[cfg(feature = "dim3")] let iter = self.triangles(); for elt in iter { - let proj = elt.project_local_point(point, false); + let proj = elt.project_local_point(point, false, options); let dist = na::distance_squared(point, &proj.point); if dist < smallest_dist { @@ -58,15 +65,19 @@ impl PointQuery for HeightField { fn project_local_point_and_get_feature( &self, point: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { // TODO: compute the feature properly. - (self.project_local_point(point, false), FeatureId::Unknown) + ( + self.project_local_point(point, false, options), + FeatureId::Unknown, + ) } // TODO: implement distance_to_point too? #[inline] - fn contains_local_point(&self, _point: &Point) -> bool { + fn contains_local_point(&self, _point: &Point, _options: &dyn QueryOptions) -> bool { false } } @@ -79,6 +90,7 @@ impl PointQueryWithLocation for HeightField { &self, _point: &Point, _: bool, + _options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { unimplemented!() } diff --git a/src/query/point/point_query.rs b/src/query/point/point_query.rs index 67428170..8c74ff19 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -1,3 +1,5 @@ +use core::any::Any; + use crate::math::{Isometry, Point, Real}; use crate::shape::FeatureId; use na; @@ -40,6 +42,16 @@ impl PointProjection { } } +pub trait QueryOptions { + fn as_any(&self) -> &dyn Any; +} + +impl QueryOptions for () { + fn as_any(&self) -> &dyn Any { + self + } +} + /// Trait of objects that can be tested for point inclusion and projection. pub trait PointQuery { /// Projects a point on `self`, unless the projection lies further than the given max distance. @@ -50,8 +62,9 @@ pub trait PointQuery { pt: &Point, solid: bool, max_dist: Real, + options: &dyn QueryOptions, ) -> Option { - let proj = self.project_local_point(pt, solid); + let proj = self.project_local_point(pt, solid, options); if na::distance(&proj.point, pt) > max_dist { None } else { @@ -66,24 +79,43 @@ pub trait PointQuery { pt: &Point, solid: bool, max_dist: Real, + options: &dyn QueryOptions, ) -> Option { - self.project_local_point_with_max_dist(&m.inverse_transform_point(pt), solid, max_dist) - .map(|proj| proj.transform_by(m)) + self.project_local_point_with_max_dist( + &m.inverse_transform_point(pt), + solid, + max_dist, + options, + ) + .map(|proj| proj.transform_by(m)) } /// Projects a point on `self`. /// /// The point is assumed to be expressed in the local-space of `self`. - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection; + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection; /// Projects a point on the boundary of `self` and returns the id of the /// feature the point was projected on. - fn project_local_point_and_get_feature(&self, pt: &Point) - -> (PointProjection, FeatureId); + fn project_local_point_and_get_feature( + &self, + pt: &Point, + options: &dyn QueryOptions, + ) -> (PointProjection, FeatureId); /// Computes the minimal distance between a point and `self`. - fn distance_to_local_point(&self, pt: &Point, solid: bool) -> Real { - let proj = self.project_local_point(pt, solid); + fn distance_to_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> Real { + let proj = self.project_local_point(pt, solid, options); let dist = na::distance(pt, &proj.point); if solid || !proj.is_inside { @@ -94,20 +126,32 @@ pub trait PointQuery { } /// Tests if the given point is inside of `self`. - fn contains_local_point(&self, pt: &Point) -> bool { - self.project_local_point(pt, true).is_inside + fn contains_local_point(&self, pt: &Point, options: &dyn QueryOptions) -> bool { + self.project_local_point(pt, true, options).is_inside } /// Projects a point on `self` transformed by `m`. - fn project_point(&self, m: &Isometry, pt: &Point, solid: bool) -> PointProjection { - self.project_local_point(&m.inverse_transform_point(pt), solid) + fn project_point( + &self, + m: &Isometry, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + self.project_local_point(&m.inverse_transform_point(pt), solid, options) .transform_by(m) } /// Computes the minimal distance between a point and `self` transformed by `m`. #[inline] - fn distance_to_point(&self, m: &Isometry, pt: &Point, solid: bool) -> Real { - self.distance_to_local_point(&m.inverse_transform_point(pt), solid) + fn distance_to_point( + &self, + m: &Isometry, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> Real { + self.distance_to_local_point(&m.inverse_transform_point(pt), solid, options) } /// Projects a point on the boundary of `self` transformed by `m` and returns the id of the @@ -116,15 +160,21 @@ pub trait PointQuery { &self, m: &Isometry, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let res = self.project_local_point_and_get_feature(&m.inverse_transform_point(pt)); + let res = self.project_local_point_and_get_feature(&m.inverse_transform_point(pt), options); (res.0.transform_by(m), res.1) } /// Tests if the given point is inside of `self` transformed by `m`. #[inline] - fn contains_point(&self, m: &Isometry, pt: &Point) -> bool { - self.contains_local_point(&m.inverse_transform_point(pt)) + fn contains_point( + &self, + m: &Isometry, + pt: &Point, + options: &dyn QueryOptions, + ) -> bool { + self.contains_local_point(&m.inverse_transform_point(pt), options) } } @@ -156,6 +206,7 @@ pub trait PointQueryWithLocation { &self, pt: &Point, solid: bool, + options: &dyn QueryOptions, ) -> (PointProjection, Self::Location); /// Projects a point on `self` transformed by `m`. @@ -164,8 +215,13 @@ pub trait PointQueryWithLocation { m: &Isometry, pt: &Point, solid: bool, + options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { - let res = self.project_local_point_and_get_location(&m.inverse_transform_point(pt), solid); + let res = self.project_local_point_and_get_location( + &m.inverse_transform_point(pt), + solid, + options, + ); (res.0.transform_by(m), res.1) } @@ -175,8 +231,9 @@ pub trait PointQueryWithLocation { pt: &Point, solid: bool, max_dist: Real, + options: &dyn QueryOptions, ) -> Option<(PointProjection, Self::Location)> { - let (proj, location) = self.project_local_point_and_get_location(pt, solid); + let (proj, location) = self.project_local_point_and_get_location(pt, solid, options); if na::distance(&proj.point, pt) > max_dist { None } else { @@ -191,11 +248,13 @@ pub trait PointQueryWithLocation { pt: &Point, solid: bool, max_dist: Real, + options: &dyn QueryOptions, ) -> Option<(PointProjection, Self::Location)> { self.project_local_point_and_get_location_with_max_dist( &m.inverse_transform_point(pt), solid, max_dist, + options, ) .map(|res| (res.0.transform_by(m), res.1)) } diff --git a/src/query/point/point_round_shape.rs b/src/query/point/point_round_shape.rs index b1c0d061..fe91881e 100644 --- a/src/query/point/point_round_shape.rs +++ b/src/query/point/point_round_shape.rs @@ -1,7 +1,6 @@ use crate::math::{Point, Real}; -#[cfg(feature = "std")] -use crate::query::gjk::GjkOptions; use crate::query::gjk::VoronoiSimplex; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{FeatureId, RoundShape, SupportMap}; @@ -9,11 +8,17 @@ use crate::shape::{FeatureId, RoundShape, SupportMap}; // call this and adjust the projected point accordingly. impl PointQuery for RoundShape { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { #[cfg(not(feature = "std"))] // TODO: can’t be used without std because of EPA return unimplemented!( "The projection of points on a round shapes isn’t supported on no-std platforms yet." ); + let options = options.as_any().downcast_ref().unwrap(); #[cfg(feature = "std")] // TODO: can’t be used without std because of EPA return crate::query::details::local_point_projection_on_support_map( @@ -21,8 +26,7 @@ impl PointQuery for RoundShape { &mut VoronoiSimplex::new(), point, solid, - // TODO: allow custom options - &GjkOptions::default(), + options, ); } @@ -30,7 +34,11 @@ impl PointQuery for RoundShape { fn project_local_point_and_get_feature( &self, point: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - (self.project_local_point(point, false), FeatureId::Unknown) + ( + self.project_local_point(point, false, options), + FeatureId::Unknown, + ) } } diff --git a/src/query/point/point_segment.rs b/src/query/point/point_segment.rs index 03e00052..b70b7645 100644 --- a/src/query/point/point_segment.rs +++ b/src/query/point/point_segment.rs @@ -1,19 +1,27 @@ use crate::math::{Point, Real}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{FeatureId, Segment, SegmentPointLocation}; impl PointQuery for Segment { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - self.project_local_point_and_get_location(pt, solid).0 + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + self.project_local_point_and_get_location(pt, solid, options) + .0 } #[inline] fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let (proj, loc) = self.project_local_point_and_get_location(pt, false); + let (proj, loc) = self.project_local_point_and_get_location(pt, false, options); let feature = match loc { SegmentPointLocation::OnVertex(i) => FeatureId::Vertex(i), SegmentPointLocation::OnEdge(..) => { @@ -50,6 +58,7 @@ impl PointQueryWithLocation for Segment { &self, pt: &Point, _: bool, + _options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { let ab = self.b - self.a; let ap = pt - self.a; diff --git a/src/query/point/point_support_map.rs b/src/query/point/point_support_map.rs index 8761504d..3acd74db 100644 --- a/src/query/point/point_support_map.rs +++ b/src/query/point/point_support_map.rs @@ -4,6 +4,10 @@ use crate::math::{Isometry, Point, Real, Vector}; #[cfg(feature = "std")] use crate::query::epa::EPA; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, GjkOptions, VoronoiSimplex}; +#[cfg(feature = "dim2")] +use crate::query::point::point_query::QueryOptions; +#[cfg(feature = "dim3")] +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; #[cfg(feature = "dim2")] #[cfg(feature = "std")] @@ -55,14 +59,18 @@ where #[cfg(feature = "dim3")] impl PointQuery for ConvexPolyhedron { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), point, solid, - // TODO: allow custom options - &GjkOptions::default(), + options.as_any().downcast_ref().unwrap(), ) } @@ -70,8 +78,9 @@ impl PointQuery for ConvexPolyhedron { fn project_local_point_and_get_feature( &self, point: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let proj = self.project_local_point(point, false); + let proj = self.project_local_point(point, false, options); let dpt = *point - proj.point; let local_dir = if proj.is_inside { -dpt } else { dpt }; @@ -87,14 +96,18 @@ impl PointQuery for ConvexPolyhedron { #[cfg(feature = "dim2")] impl PointQuery for ConvexPolygon { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), point, solid, - // TODO: allow custom options - &GjkOptions::default(), + options.as_any().downcast_ref().unwrap(), ) } @@ -102,8 +115,9 @@ impl PointQuery for ConvexPolygon { fn project_local_point_and_get_feature( &self, point: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let proj = self.project_local_point(point, false); + let proj = self.project_local_point(point, false, options); let dpt = *point - proj.point; let local_dir = if proj.is_inside { -dpt } else { dpt }; diff --git a/src/query/point/point_tetrahedron.rs b/src/query/point/point_tetrahedron.rs index 2ace3e08..36730f15 100644 --- a/src/query/point/point_tetrahedron.rs +++ b/src/query/point/point_tetrahedron.rs @@ -1,19 +1,27 @@ use crate::math::{Point, Real, Vector}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{FeatureId, Tetrahedron, TetrahedronPointLocation}; impl PointQuery for Tetrahedron { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - self.project_local_point_and_get_location(pt, solid).0 + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + self.project_local_point_and_get_location(pt, solid, options) + .0 } #[inline] fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let (proj, loc) = self.project_local_point_and_get_location(pt, false); + let (proj, loc) = self.project_local_point_and_get_location(pt, false, options); let feature = match loc { TetrahedronPointLocation::OnVertex(i) => FeatureId::Vertex(i), TetrahedronPointLocation::OnEdge(i, _) => FeatureId::Edge(i), @@ -33,6 +41,7 @@ impl PointQueryWithLocation for Tetrahedron { &self, pt: &Point, solid: bool, + _options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { let ab = self.b - self.a; let ac = self.c - self.a; diff --git a/src/query/point/point_triangle.rs b/src/query/point/point_triangle.rs index 576a13db..604ab547 100644 --- a/src/query/point/point_triangle.rs +++ b/src/query/point/point_triangle.rs @@ -1,4 +1,5 @@ use crate::math::{Point, Real, Vector, DIM}; +use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{FeatureId, Triangle, TrianglePointLocation}; @@ -19,19 +20,26 @@ fn compute_result(pt: &Point, proj: Point) -> PointProjection { impl PointQuery for Triangle { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { - self.project_local_point_and_get_location(pt, solid).0 + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + self.project_local_point_and_get_location(pt, solid, options) + .0 } #[inline] fn project_local_point_and_get_feature( &self, pt: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { let (proj, loc) = if DIM == 2 { - self.project_local_point_and_get_location(pt, false) + self.project_local_point_and_get_location(pt, false, options) } else { - self.project_local_point_and_get_location(pt, true) + self.project_local_point_and_get_location(pt, true, options) }; let feature = match loc { @@ -59,6 +67,7 @@ impl PointQueryWithLocation for Triangle { &self, pt: &Point, solid: bool, + _options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { // To understand the ideas, consider reading the slides below // https://box2d.org/files/ErinCatto_GJK_GDC2010.pdf diff --git a/src/query/split/split_trimesh.rs b/src/query/split/split_trimesh.rs index fa0e96df..bb5b5ed3 100644 --- a/src/query/split/split_trimesh.rs +++ b/src/query/split/split_trimesh.rs @@ -322,7 +322,8 @@ impl TriMesh { vertices_lhs[idx1[2] as usize], ); - if self.contains_local_point(&tri.center()) { + // FIXME: Thierry: pass an option? + if self.contains_local_point(&tri.center(), &()) { indices_lhs.push(idx1); idx2.swap(1, 2); // Flip orientation for the second half of the split. diff --git a/src/query/visitors/composite_closest_point_visitor.rs b/src/query/visitors/composite_closest_point_visitor.rs index d523e892..fda723bf 100644 --- a/src/query/visitors/composite_closest_point_visitor.rs +++ b/src/query/visitors/composite_closest_point_visitor.rs @@ -52,9 +52,9 @@ impl SimdBestFirstVisitor self.shape .map_part_at(*data[ii].unwrap(), &mut |part_pos, obj, _| { let proj = if let Some(part_pos) = part_pos { - obj.project_point(part_pos, self.point, self.solid) + obj.project_point(part_pos, self.point, self.solid, &()) } else { - obj.project_local_point(self.point, self.solid) + obj.project_local_point(self.point, self.solid, &()) }; weights[ii] = na::distance(self.point, &proj.point); diff --git a/src/query/visitors/composite_point_containment_test.rs b/src/query/visitors/composite_point_containment_test.rs index 84818107..4a571680 100644 --- a/src/query/visitors/composite_point_containment_test.rs +++ b/src/query/visitors/composite_point_containment_test.rs @@ -47,7 +47,11 @@ impl SimdVisitor if (bitmask & (1 << ii)) != 0 { let Some(data) = data else { continue }; self.shape.map_typed_part_at(*data, |part_pos, obj, _| { - if obj.contains_local_point(&part_pos.inverse_transform_point(self.point)) { + if obj.contains_local_point( + &part_pos.inverse_transform_point(self.point), + // FIXME: Thierry: safer type checking + &(), + ) { self.found = true; } }); diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index 46662e12..96b8461a 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -226,7 +226,7 @@ impl Polyline { point: Point, #[cfg(feature = "dim3")] axis: u8, ) -> (PointProjection, (u32, SegmentPointLocation)) { - let mut proj = self.project_local_point_and_get_location(&point, false); + let mut proj = self.project_local_point_and_get_location(&point, false, &()); let segment1 = self.segment((proj.1).0); #[cfg(feature = "dim2")] diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index b83f539b..b00c1181 100644 --- a/src/transformation/mesh_intersection/mesh_intersection.rs +++ b/src/transformation/mesh_intersection/mesh_intersection.rs @@ -322,7 +322,11 @@ fn extract_connected_components( let tri1 = mesh1.triangle(twin.face); if flip2 - ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&tri1.center())) + ^ mesh2.contains_local_point( + &pos12.inverse_transform_point(&tri1.center()), + // FIXME: Thierry: safer type checking + &(), + ) { to_visit.push(twin.face); } @@ -368,7 +372,13 @@ fn extract_connected_components( let repr_pt = mesh1.triangle(repr_face).center(); let indices = mesh1.indices(); - if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt)) { + if flip2 + ^ mesh2.contains_local_point( + &pos12.inverse_transform_point(&repr_pt), + // FIXME: Thierry: safer type checking + &(), + ) + { new_indices1.extend( cc.grouped_faces[range[0]..range[1]] .iter() @@ -381,7 +391,13 @@ fn extract_connected_components( // Deal with the case where there is no intersection between the meshes. let repr_pt = mesh1.triangle(0).center(); - if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt)) { + if flip2 + ^ mesh2.contains_local_point( + &pos12.inverse_transform_point(&repr_pt), + // FIXME: Thierry: safer type checking + &(), + ) + { new_indices1.extend_from_slice(mesh1.indices()); } } @@ -665,7 +681,11 @@ fn merge_triangle_sets( let epsilon = metadata.global_insertion_epsilon; let projection = mesh2 - .project_local_point_and_get_location(&pos2.inverse_transform_point(¢er), true) + .project_local_point_and_get_location( + &pos2.inverse_transform_point(¢er), + true, + &(), + ) .0; if flip2 ^ (projection.is_inside_eps(¢er, epsilon)) { diff --git a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs index 7e3d6d01..f8142b30 100644 --- a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs +++ b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs @@ -214,7 +214,7 @@ fn segment_plane_intersection( } } -/// Prints debug information if the calulated intersection of two triangles is detected to be +/// Prints debug information if the calculated intersection of two triangles is detected to be /// invalid. /// /// If the intersection is valid, this prints nothing. If it isn’t valid, this will print a few @@ -231,9 +231,11 @@ fn debug_check_intersections( ) { let proj = |vect: Vector| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); let mut incorrect = false; + // FIXME: Thierry: safer type checking. (associated type?) + let options = (); for pt in intersections { if !tri1 - .project_local_point(&pt.p1, false) + .project_local_point(&pt.p1, false, &options) .is_inside_eps(&pt.p1, 1.0e-5) { incorrect = true; @@ -241,7 +243,7 @@ fn debug_check_intersections( } if !tri2 - .project_local_point(&pt.p1, false) + .project_local_point(&pt.p1, false, &options) .is_inside_eps(&pt.p1, 1.0e-5) { incorrect = true; @@ -273,7 +275,7 @@ fn debug_check_intersections( } println!(")"); - println!("~~~~~~~ (copy/paste the folliwing input in the `intersect_triangle_common_vertex` test)"); + println!("~~~~~~~ (copy/paste the following input in the `intersect_triangle_common_vertex` test)"); println!("(Triangle::new("); for pt1 in poly1 { println!(" Point2::new({},{}),", pt1.x, pt1.y); diff --git a/src/transformation/polygon_intersection.rs b/src/transformation/polygon_intersection.rs index a770ae4b..7444f903 100644 --- a/src/transformation/polygon_intersection.rs +++ b/src/transformation/polygon_intersection.rs @@ -735,18 +735,18 @@ mod test { println!( "tri1 is in tri2: {}", tri1.vertices().iter().all(|pt| tri2 - .project_local_point(pt, false) + .project_local_point(pt, false, &()) .is_inside_eps(pt, 1.0e-5)) ); println!( "tri2 is in tri1: {}", tri2.vertices().iter().all(|pt| tri1 - .project_local_point(pt, false) + .project_local_point(pt, false, &()) .is_inside_eps(pt, 1.0e-5)) ); for pt in &inter { - let proj1 = tri1.project_local_point(&pt, false); - let proj2 = tri2.project_local_point(&pt, false); + let proj1 = tri1.project_local_point(&pt, false, &()); + let proj2 = tri2.project_local_point(&pt, false, &()); assert!(proj1.is_inside_eps(&pt, 1.0e-5)); assert!(proj2.is_inside_eps(&pt, 1.0e-5)); } From ca45b438c21cc582672ef9950ffbc37820a31794 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 9 Jul 2025 10:38:30 +0200 Subject: [PATCH 12/20] add an example to test out API for compound shape options It's not straightforward to do, that will likely need a solution similar to a dispatcher --- crates/parry2d/Cargo.toml | 6 ++ crates/parry2d/examples/debug_shape_cast2d.rs | 1 - crates/parry2d/examples/point_query2d.rs | 98 +++++++++++++++++++ .../tests/query/point_composite_shape.rs | 5 +- src/query/point/point_composite_shape.rs | 12 ++- src/query/point/point_round_shape.rs | 31 ++++-- src/query/point/point_support_map.rs | 29 +++++- src/query/split/split_trimesh.rs | 1 - .../composite_point_containment_test.rs | 2 +- .../mesh_intersection/mesh_intersection.rs | 16 +-- .../triangle_triangle_intersection.rs | 1 - 11 files changed, 165 insertions(+), 37 deletions(-) create mode 100644 crates/parry2d/examples/point_query2d.rs diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 2d0cd521..4983f552 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -93,6 +93,7 @@ oorandom = "11" ptree = "0.4.0" rand = { version = "0.8" } macroquad = "0.4.12" +env_logger = "0.11" [package.metadata.docs.rs] rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"] @@ -162,6 +163,11 @@ name = "point_in_poly2d" path = "examples/point_in_poly2d.rs" doc-scrape-examples = true +[[example]] +name = "point_query2d" +path = "examples/point_query2d.rs" +doc-scrape-examples = true + [[example]] name = "polygons_intersection2d" path = "examples/polygons_intersection2d.rs" diff --git a/crates/parry2d/examples/debug_shape_cast2d.rs b/crates/parry2d/examples/debug_shape_cast2d.rs index 0aa769d2..b4d84f39 100644 --- a/crates/parry2d/examples/debug_shape_cast2d.rs +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -4,7 +4,6 @@ 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}; diff --git a/crates/parry2d/examples/point_query2d.rs b/crates/parry2d/examples/point_query2d.rs new file mode 100644 index 00000000..fbf2d9e0 --- /dev/null +++ b/crates/parry2d/examples/point_query2d.rs @@ -0,0 +1,98 @@ +//! Builds on top of `point_in_poly2d` example but uses [PointQuery] methods and different shapes. + +mod common_macroquad2d; + +use core::f32; + +use common_macroquad2d::draw_point; +use macroquad::prelude::*; +use nalgebra::{Isometry, Point2, Rotation, Translation, UnitComplex}; +use parry2d::query::gjk::GjkOptions; +use parry2d::shape::Compound; +use parry2d::shape::{ConvexPolygon, Shape, SharedShape}; + +const RENDER_SCALE: f32 = 30.0; + +#[macroquad::main("points_in_poly2d")] +async fn main() { + env_logger::init(); + let mut simple_convex = SharedShape::new(simple_convex()); + let mut simple_compound = Box::new(Compound::new(vec![( + Isometry::default(), + simple_convex.clone(), + )])); + let test_points = grid_points(); + + let animation_rotation = UnitComplex::new(0.02); + let polygon_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0); + + for i in 0.. { + let shape_to_query = if (i) % 2 == 0 { + simple_convex.0.as_ref() + } else { + (simple_compound.as_ref() as &dyn Shape) + }; + + clear_background(BLACK); + + // TODO: Display the shape + /* polygon + .iter_mut() + .for_each(|pt| *pt = animation_rotation * *pt); + + draw_polygon(&polygon, RENDER_SCALE, polygon_render_pos, BLUE); + */ + /* + * Compute polygon intersections. + */ + for point in &test_points { + let pos12 = Isometry::default().inv_mul(&Isometry::from_parts( + Translation::from(point.coords), + Rotation::identity(), + )); + if shape_to_query.contains_local_point( + &(pos12 * point), + // FIXME: Thierry (pr 298): options should contain everything necessary to be dispatched. + &GjkOptions { + espilon_tolerance: f32::EPSILON * 10000.0, + nb_max_iterations: 100, + }, + ) { + draw_point(*point, RENDER_SCALE, polygon_render_pos, RED); + } else { + draw_point(*point, RENDER_SCALE, polygon_render_pos, GREEN); + } + } + + next_frame().await + } +} + +fn simple_convex() -> ConvexPolygon { + 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(); + to_cast_against +} + +fn grid_points() -> Vec> { + let count = 80 * 5; + let spacing = 0.6 / 5.0; + let mut pts = vec![]; + for i in 0..count { + for j in 0..count { + pts.push(Point2::new( + (i as f32 - count as f32 / 2.0) * spacing, + (j as f32 - count as f32 / 2.0) * spacing, + )); + } + } + pts +} diff --git a/crates/parry2d/tests/query/point_composite_shape.rs b/crates/parry2d/tests/query/point_composite_shape.rs index a4d857ae..c41fd0ab 100644 --- a/crates/parry2d/tests/query/point_composite_shape.rs +++ b/crates/parry2d/tests/query/point_composite_shape.rs @@ -1,8 +1,5 @@ use na::Point2; -use parry2d::{ - query::{gjk::GjkOptions, PointQuery}, - shape::TriMesh, -}; +use parry2d::{query::PointQuery, shape::TriMesh}; #[test] fn project_local_point_and_get_feature_gets_the_enclosing_triangle() { diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index 95c30d21..b4267bd7 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -228,6 +228,8 @@ impl PointQueryWithLocation for TriMesh { } } +use crate::query::gjk::GjkOptions; + /* * Visitors */ @@ -283,12 +285,18 @@ macro_rules! gen_visitor( part_shape.$project_point( part_pos, self.point - $(, self.$args)*, &() + $(, self.$args)*, &GjkOptions { + espilon_tolerance: f32::EPSILON * 10000.0, + nb_max_iterations: 100, + } ) } else { part_shape.$project_local_point( self.point - $(, self.$args)*, &() + $(, self.$args)*, &GjkOptions { + espilon_tolerance: f32::EPSILON * 10000.0, + nb_max_iterations: 100, + } ) }; diff --git a/src/query/point/point_round_shape.rs b/src/query/point/point_round_shape.rs index fe91881e..4b79b9a6 100644 --- a/src/query/point/point_round_shape.rs +++ b/src/query/point/point_round_shape.rs @@ -1,5 +1,7 @@ +use log::warn; + use crate::math::{Point, Real}; -use crate::query::gjk::VoronoiSimplex; +use crate::query::gjk::{GjkOptions, VoronoiSimplex}; use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery}; use crate::shape::{FeatureId, RoundShape, SupportMap}; @@ -18,16 +20,27 @@ impl PointQuery for RoundShape { return unimplemented!( "The projection of points on a round shapes isn’t supported on no-std platforms yet." ); - let options = options.as_any().downcast_ref().unwrap(); #[cfg(feature = "std")] // TODO: can’t be used without std because of EPA - return crate::query::details::local_point_projection_on_support_map( - self, - &mut VoronoiSimplex::new(), - point, - solid, - options, - ); + { + let Some(options) = options.as_any().downcast_ref() else { + warn!("Incorrect option passed to project_local_point: using default options."); + return crate::query::details::local_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + &GjkOptions::default(), + ); + }; + return crate::query::details::local_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + options, + ); + } } #[inline] diff --git a/src/query/point/point_support_map.rs b/src/query/point/point_support_map.rs index 3acd74db..36fd594b 100644 --- a/src/query/point/point_support_map.rs +++ b/src/query/point/point_support_map.rs @@ -65,12 +65,22 @@ impl PointQuery for ConvexPolyhedron { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { + let Some(options) = options.as_any().downcast_ref() else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + return local_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + &GjkOptions::default(), + ); + }; local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), point, solid, - options.as_any().downcast_ref().unwrap(), + options, ) } @@ -102,13 +112,24 @@ impl PointQuery for ConvexPolygon { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { - local_point_projection_on_support_map( + let Some(options) = options.as_any().downcast_ref() else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + return local_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + &GjkOptions::default(), + ); + }; + + return local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), point, solid, - options.as_any().downcast_ref().unwrap(), - ) + &options, + ); } #[inline] diff --git a/src/query/split/split_trimesh.rs b/src/query/split/split_trimesh.rs index bb5b5ed3..9a0d99fa 100644 --- a/src/query/split/split_trimesh.rs +++ b/src/query/split/split_trimesh.rs @@ -322,7 +322,6 @@ impl TriMesh { vertices_lhs[idx1[2] as usize], ); - // FIXME: Thierry: pass an option? if self.contains_local_point(&tri.center(), &()) { indices_lhs.push(idx1); diff --git a/src/query/visitors/composite_point_containment_test.rs b/src/query/visitors/composite_point_containment_test.rs index 4a571680..9d199c7c 100644 --- a/src/query/visitors/composite_point_containment_test.rs +++ b/src/query/visitors/composite_point_containment_test.rs @@ -49,7 +49,7 @@ impl SimdVisitor self.shape.map_typed_part_at(*data, |part_pos, obj, _| { if obj.contains_local_point( &part_pos.inverse_transform_point(self.point), - // FIXME: Thierry: safer type checking + // FIXME: Thierry: passed option should be depending on shape... &(), ) { self.found = true; diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index b00c1181..d66056b3 100644 --- a/src/transformation/mesh_intersection/mesh_intersection.rs +++ b/src/transformation/mesh_intersection/mesh_intersection.rs @@ -324,7 +324,6 @@ fn extract_connected_components( if flip2 ^ mesh2.contains_local_point( &pos12.inverse_transform_point(&tri1.center()), - // FIXME: Thierry: safer type checking &(), ) { @@ -372,12 +371,7 @@ fn extract_connected_components( let repr_pt = mesh1.triangle(repr_face).center(); let indices = mesh1.indices(); - if flip2 - ^ mesh2.contains_local_point( - &pos12.inverse_transform_point(&repr_pt), - // FIXME: Thierry: safer type checking - &(), - ) + if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt), &()) { new_indices1.extend( cc.grouped_faces[range[0]..range[1]] @@ -391,13 +385,7 @@ fn extract_connected_components( // Deal with the case where there is no intersection between the meshes. let repr_pt = mesh1.triangle(0).center(); - if flip2 - ^ mesh2.contains_local_point( - &pos12.inverse_transform_point(&repr_pt), - // FIXME: Thierry: safer type checking - &(), - ) - { + if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt), &()) { new_indices1.extend_from_slice(mesh1.indices()); } } diff --git a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs index f8142b30..a98a83b0 100644 --- a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs +++ b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs @@ -231,7 +231,6 @@ fn debug_check_intersections( ) { let proj = |vect: Vector| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); let mut incorrect = false; - // FIXME: Thierry: safer type checking. (associated type?) let options = (); for pt in intersections { if !tri1 From cd33ad8f6d510d439c38453f5eb7c9e60cb86197 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 16 Jul 2025 16:11:37 +0200 Subject: [PATCH 13/20] QueryOptionsDispatcherMap makes it possible to forward options to a compound shape. --- crates/parry2d/examples/point_query2d.rs | 57 ++++---- src/query/gjk/gjk.rs | 3 + src/query/point/mod.rs | 5 +- src/query/point/point_composite_shape.rs | 161 ++++++++++++++++++++--- src/query/point/point_query.rs | 4 + 5 files changed, 184 insertions(+), 46 deletions(-) diff --git a/crates/parry2d/examples/point_query2d.rs b/crates/parry2d/examples/point_query2d.rs index fbf2d9e0..a2be1195 100644 --- a/crates/parry2d/examples/point_query2d.rs +++ b/crates/parry2d/examples/point_query2d.rs @@ -1,16 +1,19 @@ //! Builds on top of `point_in_poly2d` example but uses [PointQuery] methods and different shapes. mod common_macroquad2d; - use core::f32; use common_macroquad2d::draw_point; use macroquad::prelude::*; use nalgebra::{Isometry, Point2, Rotation, Translation, UnitComplex}; +use parry2d::query::details::point_query::QueryOptions; use parry2d::query::gjk::GjkOptions; +use parry2d::query::point::QueryOptionsDispatcherMap; use parry2d::shape::Compound; use parry2d::shape::{ConvexPolygon, Shape, SharedShape}; +use crate::common_macroquad2d::easy_draw_text; + const RENDER_SCALE: f32 = 30.0; #[macroquad::main("points_in_poly2d")] @@ -26,43 +29,41 @@ async fn main() { let animation_rotation = UnitComplex::new(0.02); let polygon_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0); - for i in 0.. { - let shape_to_query = if (i) % 2 == 0 { - simple_convex.0.as_ref() - } else { - (simple_compound.as_ref() as &dyn Shape) - }; + // Create an initial dispatcher + let mut query_options_dispatcher = QueryOptionsDispatcherMap::default(); + for i in 0.. { clear_background(BLACK); - // TODO: Display the shape - /* polygon - .iter_mut() - .for_each(|pt| *pt = animation_rotation * *pt); + let gjk_options = query_options_dispatcher + .get_option_mut::() + .unwrap(); - draw_polygon(&polygon, RENDER_SCALE, polygon_render_pos, BLUE); - */ - /* - * Compute polygon intersections. - */ + // loops from default epsilon to an arbitrarily chosen slightly higher value. + gjk_options.espilon_tolerance = + f32::EPSILON + (((i as f32 / 10f32).sin() + 1f32) / 2f32) * 0.000002f32; + let (shape_to_query, options) = if (i) % 2 == 0 { + (simple_convex.0.as_ref(), &*gjk_options as &dyn QueryOptions) + } else { + ( + simple_compound.as_ref() as &dyn Shape, + &query_options_dispatcher as &dyn QueryOptions, + ) + }; for point in &test_points { let pos12 = Isometry::default().inv_mul(&Isometry::from_parts( Translation::from(point.coords), Rotation::identity(), )); - if shape_to_query.contains_local_point( - &(pos12 * point), - // FIXME: Thierry (pr 298): options should contain everything necessary to be dispatched. - &GjkOptions { - espilon_tolerance: f32::EPSILON * 10000.0, - nb_max_iterations: 100, - }, - ) { - draw_point(*point, RENDER_SCALE, polygon_render_pos, RED); - } else { + if shape_to_query.contains_local_point(&(pos12 * point), options) { draw_point(*point, RENDER_SCALE, polygon_render_pos, GREEN); + } else { + draw_point(*point, RENDER_SCALE, polygon_render_pos, DARKGRAY); } } + let gjk_options = query_options_dispatcher.get_option::().unwrap(); + + easy_draw_text(&format!("tolerance: {:.7}", gjk_options.espilon_tolerance)); next_frame().await } @@ -83,8 +84,8 @@ fn simple_convex() -> ConvexPolygon { } fn grid_points() -> Vec> { - let count = 80 * 5; - let spacing = 0.6 / 5.0; + let count = 50; + let spacing = 0.4; let mut pts = vec![]; for i in 0..count { for j in 0..count { diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 5bb3d07a..5451ba98 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -57,6 +57,9 @@ impl QueryOptions for GjkOptions { fn as_any(&self) -> &dyn core::any::Any { self } + fn as_any_mut(&mut self) -> &mut dyn core::any::Any { + self + } } /// The absolute tolerance used by the GJK algorithm. diff --git a/src/query/point/mod.rs b/src/query/point/mod.rs index ebacca00..802fad2e 100644 --- a/src/query/point/mod.rs +++ b/src/query/point/mod.rs @@ -3,7 +3,10 @@ #[doc(inline)] pub use self::point_query::{PointProjection, PointQuery, PointQueryWithLocation}; #[cfg(feature = "alloc")] -pub use self::point_support_map::local_point_projection_on_support_map; +pub use self::{ + point_composite_shape::{QueryOptionsDispatcher, QueryOptionsDispatcherMap}, + point_support_map::local_point_projection_on_support_map, +}; mod point_aabb; mod point_ball; diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index a41405ac..b4d748f8 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -1,15 +1,122 @@ +use alloc::boxed::Box; +use core::any::{Any, TypeId}; +use downcast_rs::Downcast; + use crate::math::{Point, Real}; use crate::partitioning::BvhNode; +use crate::query::gjk::GjkOptions; use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{ CompositeShapeRef, FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, TypedCompositeShape, }; +use hashbrown::HashMap; use na; use crate::shape::{Compound, Polyline}; +/// Retrieves a stored option which should be used for given shape. +pub trait QueryOptionsDispatcher { + /// Retrieves a stored option which should be used for given shape. + fn get_option_for_shape(&self, shape_type_id: &TypeId) -> &dyn QueryOptions; +} + +impl QueryOptionsDispatcher for () { + fn get_option_for_shape(&self, _shape_type_id: &TypeId) -> &dyn QueryOptions { + &() + } +} + +/// Implements [QueryOptionsDispatcher] with a generic dispatch of options. +/// This supports Compound shapes, user-defined shapes and user-defined algorithms. +pub struct QueryOptionsDispatcherMap { + /// Contains options to apply for shapes query algorithms. + /// + /// - Because algorithms can be recursive, Rc is used. + /// - Because algorithms can contain user-defined shapes, TypeId is used. + /// - Because algorithms can contain user-defined algorithms (and options), Box is used. + pub options_for_shape: HashMap fn(&'a Self) -> &'a dyn QueryOptions>>, + + /// Store options inside here. + /// It's used to capture variables, as [Self::options_for_shape] should be cloneable. + pub options: HashMap>, +} + +impl Default for QueryOptionsDispatcherMap { + fn default() -> QueryOptionsDispatcherMap { + let self_ref: Box fn(&'a Self) -> &'a dyn QueryOptions> = + Box::new(|s: &QueryOptionsDispatcherMap| -> &dyn QueryOptions { s }); + let gjk_options: Box fn(&'a Self) -> &'a dyn QueryOptions> = + Box::new(|s: &QueryOptionsDispatcherMap| -> &dyn QueryOptions { + s.options[&TypeId::of::()].as_ref() + }); + let query_options_dispatcher = QueryOptionsDispatcherMap { + options_for_shape: [ + (TypeId::of::(), self_ref), + #[cfg(feature = "dim2")] + ( + TypeId::of::(), + gjk_options.clone(), + ), + #[cfg(feature = "dim3")] + (TypeId::of::(), gjk_options), + ] + .into(), + options: [( + TypeId::of::(), + Box::new(GjkOptions::default()) as Box, + )] + .into(), + }; + query_options_dispatcher + } +} + +impl<'a> QueryOptions for QueryOptionsDispatcherMap { + fn as_any(&self) -> &dyn Any { + self + } + fn as_any_mut(&mut self) -> &mut dyn Any { + self + } +} + +impl QueryOptionsDispatcherMap { + pub fn get_option(&self) -> Option<&T> { + let option = self.options.get(&TypeId::of::()); + let Some(option) = option else { + return None; + }; + option.as_ref().as_any().downcast_ref::() + } + pub fn get_option_mut(&mut self) -> Option<&mut T> { + let option = self.options.get_mut(&TypeId::of::())?; + option.as_mut().as_any_mut().downcast_mut::() + } +} + +impl QueryOptionsDispatcher for QueryOptionsDispatcherMap { + fn get_option_for_shape(&self, shape_type_id: &TypeId) -> &dyn QueryOptions { + let option = self.options_for_shape.get(shape_type_id); + if let Some(option) = option { + return (*option)(self); + } else { + return &(); + } + } +} + +fn to_dispatcher_map_or_default(options: &dyn QueryOptions) -> &dyn QueryOptionsDispatcher { + let options = options + .as_any() + .downcast_ref::() + .map_or(&() as &dyn QueryOptionsDispatcher, |m| { + m as &dyn QueryOptionsDispatcher + }); + options +} + impl CompositeShapeRef<'_, S> { /// Project a point on this composite shape. /// @@ -21,7 +128,7 @@ impl CompositeShapeRef<'_, S> { point: &Point, max_dist: Real, solid: bool, - options: &dyn QueryOptions, + options: &dyn QueryOptionsDispatcher, ) -> Option<( u32, ( @@ -46,15 +153,13 @@ impl CompositeShapeRef<'_, S> { pose, point, solid, - // TODO: Get the correct option for the shape - &(), + options.get_option_for_shape(&shape.type_id()), ) } else { shape.project_local_point_and_get_location( point, solid, - // TODO: Get the correct option for the shape - &(), + options.get_option_for_shape(&shape.type_id()), ) } })?; @@ -74,7 +179,7 @@ impl CompositeShapeRef<'_, S> { &self, point: &Point, solid: bool, - options: &dyn QueryOptions, + options: &dyn QueryOptionsDispatcher, ) -> (u32, PointProjection) { let (best_id, (_, proj)) = self .0 @@ -91,15 +196,13 @@ impl CompositeShapeRef<'_, S> { pose, point, solid, - // TODO: Get the correct option for the shape - &(), + options.get_option_for_shape(&shape.type_id()), ) } else { shape.project_local_point( point, solid, - // TODO: Get the correct option for the shape - &(), + options.get_option_for_shape(&shape.type_id()), ) } })?; @@ -120,7 +223,7 @@ impl CompositeShapeRef<'_, S> { pub fn project_local_point_and_get_feature( &self, point: &Point, - options: &dyn QueryOptions, + options: &dyn QueryOptionsDispatcher, ) -> (u32, (PointProjection, FeatureId)) { let (best_id, (_, (proj, feature_id))) = self .0 @@ -133,9 +236,16 @@ impl CompositeShapeRef<'_, S> { |primitive, _best_so_far| { let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| { if let Some(pose) = pose { - shape.project_point_and_get_feature(pose, point, &()) + shape.project_point_and_get_feature( + pose, + point, + options.get_option_for_shape(&shape.type_id()), + ) } else { - shape.project_local_point_and_get_feature(point, &()) + shape.project_local_point_and_get_feature( + point, + options.get_option_for_shape(&shape.type_id()), + ) } })?; let cost = na::distance(&proj.0.point, point); @@ -153,7 +263,7 @@ impl CompositeShapeRef<'_, S> { pub fn contains_local_point( &self, point: &Point, - options: &dyn QueryOptions, + options: &dyn QueryOptionsDispatcher, ) -> Option { self.0 .bvh() @@ -162,9 +272,16 @@ impl CompositeShapeRef<'_, S> { self.0 .map_typed_part_at(*leaf_id, |pose, shape, _| { if let Some(pose) = pose { - shape.contains_point(pose, point, &()) + shape.contains_point( + pose, + point, + options.get_option_for_shape(&shape.type_id()), + ) } else { - shape.contains_local_point(point, &()) + shape.contains_local_point( + point, + options.get_option_for_shape(&shape.type_id()), + ) } }) .unwrap_or(false) @@ -180,6 +297,7 @@ impl PointQuery for Polyline { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { + let options = to_dispatcher_map_or_default(options); CompositeShapeRef(self) .project_local_point(point, solid, options) .1 @@ -191,6 +309,7 @@ impl PointQuery for Polyline { point: &Point, options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { + let options = to_dispatcher_map_or_default(options); let (seg_id, (proj, feature)) = CompositeShapeRef(self).project_local_point_and_get_feature(point, options); let polyline_feature = self.segment_feature_to_polyline_feature(seg_id, feature); @@ -201,6 +320,7 @@ impl PointQuery for Polyline { #[inline] fn contains_local_point(&self, point: &Point, options: &dyn QueryOptions) -> bool { + let options = to_dispatcher_map_or_default(options); CompositeShapeRef(self) .contains_local_point(point, options) .is_some() @@ -215,6 +335,7 @@ impl PointQuery for TriMesh { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { + let options = to_dispatcher_map_or_default(options); CompositeShapeRef(self) .project_local_point(point, solid, options) .1 @@ -234,7 +355,7 @@ impl PointQuery for TriMesh { let feature_id = FeatureId::Face(id); return (proj, feature_id); } - + let options = to_dispatcher_map_or_default(options); let solid = cfg!(feature = "dim2"); let (tri_id, proj) = CompositeShapeRef(self).project_local_point(point, solid, options); (proj, FeatureId::Face(tri_id)) @@ -253,6 +374,7 @@ impl PointQuery for TriMesh { .is_inside; } + let options = to_dispatcher_map_or_default(options); CompositeShapeRef(self) .contains_local_point(point, options) .is_some() @@ -279,6 +401,7 @@ impl PointQuery for Compound { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { + let options = to_dispatcher_map_or_default(options); CompositeShapeRef(self) .project_local_point(point, solid, options) .1 @@ -290,6 +413,7 @@ impl PointQuery for Compound { point: &Point, options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { + let options = to_dispatcher_map_or_default(options); ( CompositeShapeRef(self) .project_local_point_and_get_feature(point, options) @@ -301,6 +425,7 @@ impl PointQuery for Compound { #[inline] fn contains_local_point(&self, point: &Point, options: &dyn QueryOptions) -> bool { + let options = to_dispatcher_map_or_default(options); CompositeShapeRef(self) .contains_local_point(point, options) .is_some() @@ -317,6 +442,7 @@ impl PointQueryWithLocation for Polyline { solid: bool, options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { + let options = to_dispatcher_map_or_default(options); let (seg_id, (proj, loc)) = CompositeShapeRef(self) .project_local_point_and_get_location(point, Real::MAX, solid, options) .unwrap(); @@ -347,6 +473,7 @@ impl PointQueryWithLocation for TriMesh { max_dist: Real, options: &dyn QueryOptions, ) -> Option<(PointProjection, Self::Location)> { + let options = to_dispatcher_map_or_default(options); #[allow(unused_mut)] // mut is needed in 3D. if let Some((part_id, (mut proj, location))) = CompositeShapeRef(self) .project_local_point_and_get_location(point, max_dist, solid, options) diff --git a/src/query/point/point_query.rs b/src/query/point/point_query.rs index c5bbb77a..b406293c 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -44,12 +44,16 @@ impl PointProjection { pub trait QueryOptions { fn as_any(&self) -> &dyn Any; + fn as_any_mut(&mut self) -> &mut dyn Any; } impl QueryOptions for () { fn as_any(&self) -> &dyn Any { self } + fn as_any_mut(&mut self) -> &mut dyn Any { + self + } } /// Trait of objects that can be tested for point inclusion and projection. From b2de11a4a73e7ae06c243c763fd20c6b37c7ec4a Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 16 Jul 2025 16:38:27 +0200 Subject: [PATCH 14/20] code cleanup --- crates/parry2d/examples/point_query2d.rs | 7 +- src/query/gjk/gjk.rs | 3 +- src/query/point/mod.rs | 4 +- src/query/point/point_composite_shape.rs | 102 ++---------------- .../query_options_dispatcher.rs | 96 +++++++++++++++++ src/query/point/point_round_shape.rs | 16 ++- src/query/point/point_support_map.rs | 31 ++---- 7 files changed, 126 insertions(+), 133 deletions(-) create mode 100644 src/query/point/point_composite_shape/query_options_dispatcher.rs diff --git a/crates/parry2d/examples/point_query2d.rs b/crates/parry2d/examples/point_query2d.rs index a2be1195..e026e8ce 100644 --- a/crates/parry2d/examples/point_query2d.rs +++ b/crates/parry2d/examples/point_query2d.rs @@ -5,7 +5,7 @@ use core::f32; use common_macroquad2d::draw_point; use macroquad::prelude::*; -use nalgebra::{Isometry, Point2, Rotation, Translation, UnitComplex}; +use nalgebra::{Isometry, Point2, Rotation, Translation}; use parry2d::query::details::point_query::QueryOptions; use parry2d::query::gjk::GjkOptions; use parry2d::query::point::QueryOptionsDispatcherMap; @@ -19,14 +19,13 @@ const RENDER_SCALE: f32 = 30.0; #[macroquad::main("points_in_poly2d")] async fn main() { env_logger::init(); - let mut simple_convex = SharedShape::new(simple_convex()); - let mut simple_compound = Box::new(Compound::new(vec![( + let simple_convex = SharedShape::new(simple_convex()); + let simple_compound = Box::new(Compound::new(vec![( Isometry::default(), simple_convex.clone(), )])); let test_points = grid_points(); - let animation_rotation = UnitComplex::new(0.02); let polygon_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0); // Create an initial dispatcher diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 5451ba98..579fcd08 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -64,8 +64,7 @@ impl QueryOptions for GjkOptions { /// The absolute tolerance used by the GJK algorithm. pub fn eps_tol() -> Real { - let _eps = crate::math::DEFAULT_EPSILON; - _eps + crate::math::DEFAULT_EPSILON } /// Projects the origin on the boundary of the given shape. diff --git a/src/query/point/mod.rs b/src/query/point/mod.rs index 802fad2e..8f739cd0 100644 --- a/src/query/point/mod.rs +++ b/src/query/point/mod.rs @@ -4,7 +4,9 @@ pub use self::point_query::{PointProjection, PointQuery, PointQueryWithLocation}; #[cfg(feature = "alloc")] pub use self::{ - point_composite_shape::{QueryOptionsDispatcher, QueryOptionsDispatcherMap}, + point_composite_shape::query_options_dispatcher::{ + QueryOptionsDispatcher, QueryOptionsDispatcherMap, + }, point_support_map::local_point_projection_on_support_map, }; diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index b4d748f8..e8dc353b 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -1,112 +1,22 @@ -use alloc::boxed::Box; -use core::any::{Any, TypeId}; -use downcast_rs::Downcast; +pub mod query_options_dispatcher; + +use core::any::Any; use crate::math::{Point, Real}; use crate::partitioning::BvhNode; -use crate::query::gjk::GjkOptions; +use crate::query::point::point_composite_shape::query_options_dispatcher::{ + QueryOptionsDispatcher, QueryOptionsDispatcherMap, +}; use crate::query::point::point_query::QueryOptions; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{ CompositeShapeRef, FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, TypedCompositeShape, }; -use hashbrown::HashMap; use na; use crate::shape::{Compound, Polyline}; -/// Retrieves a stored option which should be used for given shape. -pub trait QueryOptionsDispatcher { - /// Retrieves a stored option which should be used for given shape. - fn get_option_for_shape(&self, shape_type_id: &TypeId) -> &dyn QueryOptions; -} - -impl QueryOptionsDispatcher for () { - fn get_option_for_shape(&self, _shape_type_id: &TypeId) -> &dyn QueryOptions { - &() - } -} - -/// Implements [QueryOptionsDispatcher] with a generic dispatch of options. -/// This supports Compound shapes, user-defined shapes and user-defined algorithms. -pub struct QueryOptionsDispatcherMap { - /// Contains options to apply for shapes query algorithms. - /// - /// - Because algorithms can be recursive, Rc is used. - /// - Because algorithms can contain user-defined shapes, TypeId is used. - /// - Because algorithms can contain user-defined algorithms (and options), Box is used. - pub options_for_shape: HashMap fn(&'a Self) -> &'a dyn QueryOptions>>, - - /// Store options inside here. - /// It's used to capture variables, as [Self::options_for_shape] should be cloneable. - pub options: HashMap>, -} - -impl Default for QueryOptionsDispatcherMap { - fn default() -> QueryOptionsDispatcherMap { - let self_ref: Box fn(&'a Self) -> &'a dyn QueryOptions> = - Box::new(|s: &QueryOptionsDispatcherMap| -> &dyn QueryOptions { s }); - let gjk_options: Box fn(&'a Self) -> &'a dyn QueryOptions> = - Box::new(|s: &QueryOptionsDispatcherMap| -> &dyn QueryOptions { - s.options[&TypeId::of::()].as_ref() - }); - let query_options_dispatcher = QueryOptionsDispatcherMap { - options_for_shape: [ - (TypeId::of::(), self_ref), - #[cfg(feature = "dim2")] - ( - TypeId::of::(), - gjk_options.clone(), - ), - #[cfg(feature = "dim3")] - (TypeId::of::(), gjk_options), - ] - .into(), - options: [( - TypeId::of::(), - Box::new(GjkOptions::default()) as Box, - )] - .into(), - }; - query_options_dispatcher - } -} - -impl<'a> QueryOptions for QueryOptionsDispatcherMap { - fn as_any(&self) -> &dyn Any { - self - } - fn as_any_mut(&mut self) -> &mut dyn Any { - self - } -} - -impl QueryOptionsDispatcherMap { - pub fn get_option(&self) -> Option<&T> { - let option = self.options.get(&TypeId::of::()); - let Some(option) = option else { - return None; - }; - option.as_ref().as_any().downcast_ref::() - } - pub fn get_option_mut(&mut self) -> Option<&mut T> { - let option = self.options.get_mut(&TypeId::of::())?; - option.as_mut().as_any_mut().downcast_mut::() - } -} - -impl QueryOptionsDispatcher for QueryOptionsDispatcherMap { - fn get_option_for_shape(&self, shape_type_id: &TypeId) -> &dyn QueryOptions { - let option = self.options_for_shape.get(shape_type_id); - if let Some(option) = option { - return (*option)(self); - } else { - return &(); - } - } -} - fn to_dispatcher_map_or_default(options: &dyn QueryOptions) -> &dyn QueryOptionsDispatcher { let options = options .as_any() diff --git a/src/query/point/point_composite_shape/query_options_dispatcher.rs b/src/query/point/point_composite_shape/query_options_dispatcher.rs new file mode 100644 index 00000000..8968ac08 --- /dev/null +++ b/src/query/point/point_composite_shape/query_options_dispatcher.rs @@ -0,0 +1,96 @@ +use alloc::boxed::Box; +use core::any::{Any, TypeId}; + +use crate::query::gjk::GjkOptions; +use crate::query::point::point_query::QueryOptions; +use crate::shape::Compound; +use hashbrown::HashMap; + +/// Retrieves a stored option which should be used for given shape. +pub trait QueryOptionsDispatcher { + /// Retrieves a stored option which should be used for given shape. + fn get_option_for_shape(&self, shape_type_id: &TypeId) -> &dyn QueryOptions; +} + +impl QueryOptionsDispatcher for () { + fn get_option_for_shape(&self, _shape_type_id: &TypeId) -> &dyn QueryOptions { + &() + } +} + +/// Implements [QueryOptionsDispatcher] with a generic dispatch of options. +/// This supports Compound shapes, user-defined shapes and user-defined algorithms. +pub struct QueryOptionsDispatcherMap { + /// Contains options to apply for shapes query algorithms. + /// + /// - Because algorithms can be recursive, Rc is used. + /// - Because algorithms can contain user-defined shapes, TypeId is used. + /// - Because algorithms can contain user-defined algorithms (and options), `Box` is used. + pub options_for_shape: HashMap &dyn QueryOptions>>, + + /// Store options inside here. + /// It's used to capture variables, as [Self::options_for_shape] should be cloneable. + pub options: HashMap>, +} + +impl Default for QueryOptionsDispatcherMap { + fn default() -> QueryOptionsDispatcherMap { + let self_ref: Box &dyn QueryOptions> = + Box::new(|s: &QueryOptionsDispatcherMap| -> &dyn QueryOptions { s }); + let gjk_options: Box &dyn QueryOptions> = + Box::new(|s: &QueryOptionsDispatcherMap| -> &dyn QueryOptions { + s.options[&TypeId::of::()].as_ref() + }); + QueryOptionsDispatcherMap { + options_for_shape: [ + (TypeId::of::(), self_ref), + #[cfg(feature = "dim2")] + ( + TypeId::of::(), + gjk_options.clone(), + ), + #[cfg(feature = "dim3")] + (TypeId::of::(), gjk_options), + ] + .into(), + options: [( + TypeId::of::(), + Box::new(GjkOptions::default()) as Box, + )] + .into(), + } + } +} + +impl QueryOptions for QueryOptionsDispatcherMap { + fn as_any(&self) -> &dyn Any { + self + } + fn as_any_mut(&mut self) -> &mut dyn Any { + self + } +} + +impl QueryOptionsDispatcherMap { + /// Retrieves the stored [QueryOptions] of type T. + pub fn get_option(&self) -> Option<&T> { + let option = self.options.get(&TypeId::of::())?; + option.as_ref().as_any().downcast_ref::() + } + /// Retrieves mutably the stored [QueryOptions] of type T. + pub fn get_option_mut(&mut self) -> Option<&mut T> { + let option = self.options.get_mut(&TypeId::of::())?; + option.as_mut().as_any_mut().downcast_mut::() + } +} + +impl QueryOptionsDispatcher for QueryOptionsDispatcherMap { + fn get_option_for_shape(&self, shape_type_id: &TypeId) -> &dyn QueryOptions { + let option = self.options_for_shape.get(shape_type_id); + if let Some(option) = option { + (*option)(self) + } else { + &() + } + } +} diff --git a/src/query/point/point_round_shape.rs b/src/query/point/point_round_shape.rs index 80bfeaef..7df29735 100644 --- a/src/query/point/point_round_shape.rs +++ b/src/query/point/point_round_shape.rs @@ -23,23 +23,19 @@ impl PointQuery for RoundShape { #[cfg(feature = "alloc")] // TODO: can’t be used without alloc because of EPA { - let Some(options) = options.as_any().downcast_ref() else { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { warn!("Incorrect option passed to project_local_point: using default options."); - return crate::query::details::local_point_projection_on_support_map( - self, - &mut VoronoiSimplex::new(), - point, - solid, - &GjkOptions::default(), - ); + &GjkOptions::default() }; - return crate::query::details::local_point_projection_on_support_map( + crate::query::details::local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), point, solid, options, - ); + ) } } diff --git a/src/query/point/point_support_map.rs b/src/query/point/point_support_map.rs index c0433a8d..a3cadad7 100644 --- a/src/query/point/point_support_map.rs +++ b/src/query/point/point_support_map.rs @@ -65,15 +65,11 @@ impl PointQuery for ConvexPolyhedron { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { - let Some(options) = options.as_any().downcast_ref() else { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { log::warn!("Incorrect option passed to project_local_point: using default options."); - return local_point_projection_on_support_map( - self, - &mut VoronoiSimplex::new(), - point, - solid, - &GjkOptions::default(), - ); + &GjkOptions::default() }; local_point_projection_on_support_map( self, @@ -112,24 +108,19 @@ impl PointQuery for ConvexPolygon { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { - let Some(options) = options.as_any().downcast_ref() else { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { log::warn!("Incorrect option passed to project_local_point: using default options."); - return local_point_projection_on_support_map( - self, - &mut VoronoiSimplex::new(), - point, - solid, - &GjkOptions::default(), - ); + &GjkOptions::default() }; - - return local_point_projection_on_support_map( + local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), point, solid, - &options, - ); + options, + ) } #[inline] From df4e56953484010ff5a754c38c66ebdbbe0bcc76 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 17 Jul 2025 09:34:32 +0200 Subject: [PATCH 15/20] more docs + fix default gjk epsilon --- src/query/gjk/gjk.rs | 2 +- src/query/point/mod.rs | 2 +- src/query/point/point_query.rs | 8 ++++++++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 579fcd08..c8c6d44a 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -64,7 +64,7 @@ impl QueryOptions for GjkOptions { /// The absolute tolerance used by the GJK algorithm. pub fn eps_tol() -> Real { - crate::math::DEFAULT_EPSILON + crate::math::DEFAULT_EPSILON * 10.0 } /// Projects the origin on the boundary of the given shape. diff --git a/src/query/point/mod.rs b/src/query/point/mod.rs index 8f739cd0..b46766e4 100644 --- a/src/query/point/mod.rs +++ b/src/query/point/mod.rs @@ -1,7 +1,7 @@ //! Point inclusion and projection. #[doc(inline)] -pub use self::point_query::{PointProjection, PointQuery, PointQueryWithLocation}; +pub use self::point_query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; #[cfg(feature = "alloc")] pub use self::{ point_composite_shape::query_options_dispatcher::{ diff --git a/src/query/point/point_query.rs b/src/query/point/point_query.rs index b406293c..72539d17 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -42,6 +42,14 @@ impl PointProjection { } } +/// Options to pass to [QueryOptions]. The type to be passed depends on each shape implementation. +/// If you're not sure, use `&()` which will evaluate to default options for included shapes in +/// [QueryOptions] implementations. +/// +/// # See Also +/// - [GjkOptions][crate::query::gjk::GjkOptions] +/// - [QueryOptionsDispatcher][crate::query::point::QueryOptionsDispatcher] +/// - [QueryOptionsDispatcherMap][crate::query::point::QueryOptionsDispatcherMap] pub trait QueryOptions { fn as_any(&self) -> &dyn Any; fn as_any_mut(&mut self) -> &mut dyn Any; From 4c0c3ae2e04a89ab5b69ebaebf35024426ab75fd Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 17 Jul 2025 09:48:06 +0200 Subject: [PATCH 16/20] fix bench + fix default epsilon used in QueryOptions default --- crates/parry3d/benches/query/algorithm.rs | 14 +++++++------- src/query/gjk/gjk.rs | 3 +-- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/crates/parry3d/benches/query/algorithm.rs b/crates/parry3d/benches/query/algorithm.rs index 521fd1b5..6da620e7 100644 --- a/crates/parry3d/benches/query/algorithm.rs +++ b/crates/parry3d/benches/query/algorithm.rs @@ -1,5 +1,5 @@ use na::Point3; -use parry3d::query::gjk::{CSOPoint, VoronoiSimplex}; +use parry3d::query::gjk::{eps_tol, CSOPoint, VoronoiSimplex}; use test::Bencher; #[bench] @@ -14,9 +14,9 @@ fn bench_johnson_simplex(bh: &mut Bencher) { spl.reset(a); - spl.add_point(b); - spl.add_point(c); - spl.add_point(d); + spl.add_point(b, eps_tol()); + spl.add_point(c, eps_tol()); + spl.add_point(d, eps_tol()); test::black_box(spl.project_origin_and_reduce()); }) @@ -34,9 +34,9 @@ fn bench_johnson_simplex_tls(bh: &mut Bencher) { spl.reset(a); - spl.add_point(b); - spl.add_point(c); - spl.add_point(d); + spl.add_point(b, eps_tol()); + spl.add_point(c, eps_tol()); + spl.add_point(d, eps_tol()); test::black_box(spl.project_origin_and_reduce()); }) diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index c8c6d44a..d41c9bf6 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -47,7 +47,7 @@ pub struct GjkOptions { impl Default for GjkOptions { fn default() -> Self { Self { - espilon_tolerance: crate::math::DEFAULT_EPSILON, + espilon_tolerance: eps_tol(), nb_max_iterations: 100, } } @@ -124,7 +124,6 @@ where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { - let _eps = crate::math::DEFAULT_EPSILON; let _eps_tol: Real = gjk_options.espilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); From 04b00e63270c90adf4b1eab17f1fae0833527ac5 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 17 Jul 2025 10:47:28 +0200 Subject: [PATCH 17/20] update changelog --- CHANGELOG.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 804c1e41..3d256e33 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,10 @@ +### Unreleased + +#### Added + +- Added `QueryOptions`, a trait to allow customizing `PointQuery` algorithms, + to help with fixing false negatives in queries. ([#298](https://github.com/dimforge/parry/pull/298)) + # 0.22.0-beta.1 ### Fixed @@ -133,8 +140,6 @@ - `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 From 6769e551a34cf5c01ac107783eb0a9730e43fc59 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 17 Jul 2025 14:53:51 +0200 Subject: [PATCH 18/20] more consistent headings --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3d256e33..fadb4e5f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,6 @@ -### Unreleased +## Unreleased -#### Added +### Added - Added `QueryOptions`, a trait to allow customizing `PointQuery` algorithms, to help with fixing false negatives in queries. ([#298](https://github.com/dimforge/parry/pull/298)) From f0e82d76bb60b1898d05a888ee9d45ab855dda3a Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 24 Jul 2025 11:05:48 +0200 Subject: [PATCH 19/20] wip ray support --- crates/parry2d/examples/debug_shape_cast2d.rs | 2 +- crates/parry2d/examples/point_query2d.rs | 4 +- .../parry3d/tests/geometry/cuboid_ray_cast.rs | 6 +- src/bounding_volume/aabb.rs | 2 +- src/partitioning/bvh/bvh_queries.rs | 7 +- src/partitioning/bvh/bvh_tree.rs | 2 +- .../closest_points_ball_convex_polyhedron.rs | 3 +- .../closest_points_cuboid_cuboid.rs | 6 +- .../closest_points_cuboid_triangle.rs | 6 +- .../contact/contact_ball_convex_polyhedron.rs | 3 +- ...nifolds_composite_shape_composite_shape.rs | 7 +- ...contact_manifolds_composite_shape_shape.rs | 7 +- .../contact_manifolds_convex_ball.rs | 7 +- src/query/default_query_dispatcher.rs | 12 +++- .../distance_ball_convex_polyhedron.rs | 2 +- src/query/epa/epa2.rs | 4 +- src/query/epa/epa3.rs | 12 ++-- src/query/gjk/gjk.rs | 15 ++-- src/query/gjk/voronoi_simplex3.rs | 25 +++++-- .../intersection_test_ball_point_query.rs | 8 ++- src/query/mod.rs | 2 + src/query/point/mod.rs | 2 +- src/query/point/point_aabb.rs | 3 +- src/query/point/point_ball.rs | 3 +- src/query/point/point_bounding_sphere.rs | 3 +- src/query/point/point_capsule.rs | 3 +- src/query/point/point_composite_shape.rs | 46 ++++++------- .../query_options_dispatcher.rs | 14 +++- src/query/point/point_cone.rs | 3 +- src/query/point/point_cuboid.rs | 3 +- src/query/point/point_cylinder.rs | 3 +- src/query/point/point_halfspace.rs | 3 +- src/query/point/point_heightfield.rs | 3 +- src/query/point/point_query.rs | 25 +------ src/query/point/point_round_shape.rs | 7 +- src/query/point/point_segment.rs | 3 +- src/query/point/point_support_map.rs | 6 +- src/query/point/point_tetrahedron.rs | 3 +- src/query/point/point_triangle.rs | 3 +- src/query/point/point_voxels.rs | 3 +- src/query/query_options.rs | 41 +++++++++++ src/query/ray/ray.rs | 38 +++++++--- src/query/ray/ray_aabb.rs | 11 ++- src/query/ray/ray_ball.rs | 11 ++- src/query/ray/ray_bounding_sphere.rs | 23 +++++-- src/query/ray/ray_composite_shape.rs | 69 ++++++++++++++++--- src/query/ray/ray_cuboid.rs | 15 ++-- src/query/ray/ray_halfspace.rs | 3 +- src/query/ray/ray_heightfield.rs | 16 +++-- src/query/ray/ray_round_shape.rs | 12 +++- src/query/ray/ray_support_map.rs | 66 ++++++++++++++---- src/query/ray/ray_triangle.rs | 11 ++- src/query/ray/ray_trimesh.rs | 22 ++++-- src/query/ray/ray_voxels.rs | 5 +- .../shape_cast_composite_shape_shape.rs | 4 +- .../shape_cast_halfspace_support_map.rs | 6 +- .../shape_cast_heightfield_shape.rs | 6 +- src/shape/segment.rs | 6 +- 58 files changed, 428 insertions(+), 218 deletions(-) create mode 100644 src/query/query_options.rs diff --git a/crates/parry2d/examples/debug_shape_cast2d.rs b/crates/parry2d/examples/debug_shape_cast2d.rs index b4d84f39..c7ad6279 100644 --- a/crates/parry2d/examples/debug_shape_cast2d.rs +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -111,7 +111,7 @@ fn shape_cast_debug( ShapeCastOptions::with_max_time_of_impact(1.0), DefaultQueryDispatcher { gjk_options: query::gjk::GjkOptions { - espilon_tolerance: math::DEFAULT_EPSILON * 1000f32, + epsilon_tolerance: math::DEFAULT_EPSILON * 1000f32, nb_max_iterations: 100, }, }, diff --git a/crates/parry2d/examples/point_query2d.rs b/crates/parry2d/examples/point_query2d.rs index e026e8ce..2bfd2d58 100644 --- a/crates/parry2d/examples/point_query2d.rs +++ b/crates/parry2d/examples/point_query2d.rs @@ -39,7 +39,7 @@ async fn main() { .unwrap(); // loops from default epsilon to an arbitrarily chosen slightly higher value. - gjk_options.espilon_tolerance = + gjk_options.epsilon_tolerance = f32::EPSILON + (((i as f32 / 10f32).sin() + 1f32) / 2f32) * 0.000002f32; let (shape_to_query, options) = if (i) % 2 == 0 { (simple_convex.0.as_ref(), &*gjk_options as &dyn QueryOptions) @@ -62,7 +62,7 @@ async fn main() { } let gjk_options = query_options_dispatcher.get_option::().unwrap(); - easy_draw_text(&format!("tolerance: {:.7}", gjk_options.espilon_tolerance)); + easy_draw_text(&format!("tolerance: {:.7}", gjk_options.epsilon_tolerance)); next_frame().await } diff --git a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs index 1b103482..63e65733 100644 --- a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs +++ b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs @@ -31,7 +31,7 @@ where let position = Isometry3::from_parts(Translation3::identity(), rotation); let intersection = shape - .cast_ray_and_get_normal(&position, &ray, f32::MAX, true) + .cast_ray_and_get_normal(&position, &ray, f32::MAX, true, &()) .expect(&format!( "Ray {:?} did not hit Shape {} rotated with {:?}", ray, name, rotation @@ -61,14 +61,14 @@ where assert!( shape - .cast_ray_and_get_normal(&position, &new_ray, f32::MAX, true) + .cast_ray_and_get_normal(&position, &new_ray, f32::MAX, true, &()) .is_none(), "Ray {:#?} from outside Shape {} rotated with {:#?} did hit at t={}", ray, name, rotation, shape - .cast_ray_and_get_normal(&position, &new_ray, f32::MAX, true) + .cast_ray_and_get_normal(&position, &new_ray, f32::MAX, true, &()) .expect("recurring ray cast produced a different answer") .time_of_impact ); diff --git a/src/bounding_volume/aabb.rs b/src/bounding_volume/aabb.rs index 2770f97c..0037f537 100644 --- a/src/bounding_volume/aabb.rs +++ b/src/bounding_volume/aabb.rs @@ -301,7 +301,7 @@ impl Aabb { }; let ray = Ray::new(Point::origin(), vel12); - msum.intersects_local_ray(&ray, 1.0) + msum.intersects_local_ray(&ray, 1.0, &()) } /// Computes the intersection of this `Aabb` and another one. diff --git a/src/partitioning/bvh/bvh_queries.rs b/src/partitioning/bvh/bvh_queries.rs index 697c716e..b6b11631 100644 --- a/src/partitioning/bvh/bvh_queries.rs +++ b/src/partitioning/bvh/bvh_queries.rs @@ -2,7 +2,7 @@ use super::{Bvh, BvhNode}; use crate::bounding_volume::{Aabb, BoundingVolume}; use crate::math::Point; use crate::math::Real; -use crate::query::PointProjection; +use crate::query::{PointProjection, QueryOptionsNotUsed}; use crate::query::{PointQuery, Ray}; #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] @@ -51,7 +51,10 @@ impl Bvh { ) -> Option<(u32, (Real, PointProjection))> { self.find_best( max_distance, - |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true, &()), + |node: &BvhNode, _| { + node.aabb() + .distance_to_local_point(point, true, &QueryOptionsNotUsed) + }, |primitive, _| { let proj = primitive_check(primitive, max_distance)?; Some((na::distance(&proj.point, point), proj)) diff --git a/src/partitioning/bvh/bvh_tree.rs b/src/partitioning/bvh/bvh_tree.rs index ead3b7c4..3fe4062f 100644 --- a/src/partitioning/bvh/bvh_tree.rs +++ b/src/partitioning/bvh/bvh_tree.rs @@ -355,7 +355,7 @@ impl BvhNode { /// Returns `Real::MAX` if there is no hit. pub fn cast_ray(&self, ray: &Ray, max_toi: Real) -> Real { self.aabb() - .cast_local_ray(ray, max_toi, true) + .cast_local_ray(ray, max_toi, true, &()) .unwrap_or(Real::MAX) } diff --git a/src/query/closest_points/closest_points_ball_convex_polyhedron.rs b/src/query/closest_points/closest_points_ball_convex_polyhedron.rs index dcd70a0e..84b5dcf5 100644 --- a/src/query/closest_points/closest_points_ball_convex_polyhedron.rs +++ b/src/query/closest_points/closest_points_ball_convex_polyhedron.rs @@ -1,6 +1,5 @@ use crate::math::{Isometry, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::ClosestPoints; +use crate::query::{ClosestPoints, QueryOptions}; use crate::shape::{Ball, Shape}; /// ClosestPoints between a ball and a convex polyhedron. diff --git a/src/query/closest_points/closest_points_cuboid_cuboid.rs b/src/query/closest_points/closest_points_cuboid_cuboid.rs index 2d26a603..1f7a21fc 100644 --- a/src/query/closest_points/closest_points_cuboid_cuboid.rs +++ b/src/query/closest_points/closest_points_cuboid_cuboid.rs @@ -1,5 +1,5 @@ use crate::math::{Isometry, Real}; -use crate::query::{sat, ClosestPoints, PointQuery}; +use crate::query::{sat, ClosestPoints, PointQuery, QueryOptionsNotUsed}; use crate::shape::{Cuboid, SupportMap}; /// Closest points between two cuboids. @@ -42,7 +42,7 @@ pub fn closest_points_cuboid_cuboid( // from cuboid2 on the support-face of cuboid1. For simplicity, we just // project the support point from cuboid2 on cuboid1 itself (not just the face). let pt2_1 = cuboid2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, true, &()); + let proj1 = cuboid1.project_local_point(&pt2_1, true, &QueryOptionsNotUsed); if na::distance_squared(&proj1.point, &pt2_1) > margin * margin { return ClosestPoints::Disjoint; } else { @@ -58,7 +58,7 @@ pub fn closest_points_cuboid_cuboid( // from cuboid1 on the support-face of cuboid2. For simplicity, we just // project the support point from cuboid1 on cuboid2 itself (not just the face). let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = cuboid2.project_local_point(&pt1_2, true, &()); + let proj2 = cuboid2.project_local_point(&pt1_2, true, &QueryOptionsNotUsed); if na::distance_squared(&proj2.point, &pt1_2) > margin * margin { return ClosestPoints::Disjoint; diff --git a/src/query/closest_points/closest_points_cuboid_triangle.rs b/src/query/closest_points/closest_points_cuboid_triangle.rs index 5f418b25..ca0f2869 100644 --- a/src/query/closest_points/closest_points_cuboid_triangle.rs +++ b/src/query/closest_points/closest_points_cuboid_triangle.rs @@ -1,5 +1,5 @@ use crate::math::{Isometry, Real}; -use crate::query::{sat, ClosestPoints, PointQuery}; +use crate::query::{sat, ClosestPoints, PointQuery, QueryOptionsNotUsed}; use crate::shape::{Cuboid, SupportMap, Triangle}; /// Closest points between a cuboid and a triangle. @@ -41,7 +41,7 @@ pub fn closest_points_cuboid_triangle( // from triangle2 on the support-face of triangle1. For simplicity, we just // project the support point from triangle2 on cuboid1 itself (not just the face). let pt2_1 = triangle2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, true, &()); + let proj1 = cuboid1.project_local_point(&pt2_1, true, &QueryOptionsNotUsed); if na::distance_squared(&proj1.point, &pt2_1) > margin * margin { return ClosestPoints::Disjoint; } else { @@ -55,7 +55,7 @@ pub fn closest_points_cuboid_triangle( // from cuboid1 on the support-face of triangle2. For simplicity, we just // project the support point from cuboid1 on triangle2 itself (not just the face). let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = triangle2.project_local_point(&pt1_2, true, &()); + let proj2 = triangle2.project_local_point(&pt1_2, true, &QueryOptionsNotUsed); if na::distance_squared(&proj2.point, &pt1_2) > margin * margin { return ClosestPoints::Disjoint; diff --git a/src/query/contact/contact_ball_convex_polyhedron.rs b/src/query/contact/contact_ball_convex_polyhedron.rs index 78695bf9..23a841fc 100644 --- a/src/query/contact/contact_ball_convex_polyhedron.rs +++ b/src/query/contact/contact_ball_convex_polyhedron.rs @@ -1,6 +1,5 @@ use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::point::point_query::QueryOptions; -use crate::query::Contact; +use crate::query::{Contact, QueryOptions}; use crate::shape::{Ball, Shape}; use na::{self, Unit}; diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs index 45e0e17d..5837fa89 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs @@ -6,9 +6,10 @@ use crate::math::{Isometry, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; -use crate::query::contact_manifolds::ContactManifoldsWorkspace; -use crate::query::query_dispatcher::PersistentQueryDispatcher; -use crate::query::ContactManifold; +use crate::query::{ + contact_manifolds::ContactManifoldsWorkspace, query_dispatcher::PersistentQueryDispatcher, + ContactManifold, +}; use crate::shape::CompositeShape; use crate::utils::hashmap::{Entry, HashMap}; use crate::utils::IsometryOpt; diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs index 9256501f..eab17e25 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs @@ -5,9 +5,10 @@ use crate::math::{Isometry, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; -use crate::query::contact_manifolds::ContactManifoldsWorkspace; -use crate::query::query_dispatcher::PersistentQueryDispatcher; -use crate::query::ContactManifold; +use crate::query::{ + contact_manifolds::ContactManifoldsWorkspace, query_dispatcher::PersistentQueryDispatcher, + ContactManifold, +}; use crate::shape::{CompositeShape, Shape}; use crate::utils::hashmap::{Entry, HashMap}; use crate::utils::IsometryOpt; diff --git a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs index 8abdad20..549aab61 100644 --- a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs @@ -1,7 +1,6 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{ContactManifold, Ray, TrackedContact}; +use crate::query::{ContactManifold, QueryOptions, Ray, TrackedContact}; use crate::shape::{Ball, PackedFeatureId, Shape}; use na::Unit; @@ -106,7 +105,9 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( }, ); - if let Some(hit) = shape1.cast_local_ray_and_get_normal(&ray1, Real::MAX, false) { + if let Some(hit) = + shape1.cast_local_ray_and_get_normal(&ray1, Real::MAX, false, options) + { local_p1 = ray1.point_at(hit.time_of_impact); dist = if proj.is_inside { -hit.time_of_impact diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index cea0571d..8d72faa5 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -46,11 +46,19 @@ impl QueryDispatcher for DefaultQueryDispatcher { )) } else if let Some(b1) = shape1.as_ball() { Ok(query::details::intersection_test_ball_point_query( - pos12, b1, shape2, + pos12, + b1, + shape2, + // FIXME: This may need a query option dispatcher, because a user custom shape against a ball may need a different option. + &self.gjk_options, )) } else if let Some(b2) = shape2.as_ball() { Ok(query::details::intersection_test_point_query_ball( - pos12, shape1, b2, + pos12, + shape1, + b2, + // FIXME: This may need a query option dispatcher, because a user custom shape against a ball may need a different option. + &self.gjk_options, )) } else if let (Some(p1), Some(s2)) = (shape1.as_shape::(), shape2.as_support_map()) diff --git a/src/query/distance/distance_ball_convex_polyhedron.rs b/src/query/distance/distance_ball_convex_polyhedron.rs index c6141a4f..5817d8c4 100644 --- a/src/query/distance/distance_ball_convex_polyhedron.rs +++ b/src/query/distance/distance_ball_convex_polyhedron.rs @@ -1,5 +1,5 @@ use crate::math::{Isometry, Point, Real}; -use crate::query::point::point_query::QueryOptions; +use crate::query::QueryOptions; use crate::shape::{Ball, Shape}; /// Distance between a ball and a convex polyhedron. diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 0287ee6c..e93373b3 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -18,8 +18,8 @@ struct FaceId { } impl FaceId { - fn new(id: usize, neg_dist: Real, gjk_espilon_tolerance: Real) -> Option { - if neg_dist > gjk_espilon_tolerance { + fn new(id: usize, neg_dist: Real, gjk_epsilon_tolerance: Real) -> Option { + if neg_dist > gjk_epsilon_tolerance { None } else { Some(FaceId { id, neg_dist }) diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index c87e4f53..b66a8c09 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -2,6 +2,7 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::query_options::QueryOptionsNotUsed; use crate::query::PointQueryWithLocation; use crate::shape::{SupportMap, Triangle, TrianglePointLocation}; use crate::utils; @@ -18,8 +19,8 @@ struct FaceId { } impl FaceId { - fn new(id: usize, neg_dist: Real, gjk_espilon_tolerance: Real) -> Option { - if neg_dist > gjk_espilon_tolerance { + fn new(id: usize, neg_dist: Real, gjk_epsilon_tolerance: Real) -> Option { + if neg_dist > gjk_epsilon_tolerance { None } else { Some(FaceId { id, neg_dist }) @@ -96,8 +97,11 @@ impl Face { vertices[pts[1]].point, vertices[pts[2]].point, ); - let (proj, loc) = - tri.project_local_point_and_get_location(&Point::::origin(), true, &()); + let (proj, loc) = tri.project_local_point_and_get_location( + &Point::::origin(), + true, + &QueryOptionsNotUsed, + ); match loc { TrianglePointLocation::OnVertex(_) | TrianglePointLocation::OnEdge(_, _) => { diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index d41c9bf6..bb373ad1 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -3,11 +3,10 @@ use na::{self, ComplexField, Unit}; use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; -use crate::query::point::point_query::QueryOptions; use crate::shape::SupportMap; // use query::Proximity; use crate::math::{Isometry, Point, Real, Vector, DIM}; -use crate::query::{self, Ray}; +use crate::query::{self, QueryOptions, Ray}; use num::{Bounded, Zero}; @@ -39,7 +38,7 @@ pub struct GjkOptions { /// The absolute tolerance used by the GJK algorithm. /// /// Defaults to [math::DEFAULT_EPSILON][crate::math::DEFAULT_EPSILON] - pub espilon_tolerance: Real, + pub epsilon_tolerance: Real, /// The maximum number of iterations of the GJK algorithm. pub nb_max_iterations: u32, } @@ -47,7 +46,7 @@ pub struct GjkOptions { impl Default for GjkOptions { fn default() -> Self { Self { - espilon_tolerance: eps_tol(), + epsilon_tolerance: eps_tol(), nb_max_iterations: 100, } } @@ -124,7 +123,7 @@ where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, { - let _eps_tol: Real = gjk_options.espilon_tolerance; + let _eps_tol: Real = gjk_options.epsilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); // TODO: reset the simplex if it is empty? @@ -280,7 +279,7 @@ where G2: ?Sized + SupportMap, { let _eps = crate::math::DEFAULT_EPSILON; - let _eps_tol: Real = gjk_options.espilon_tolerance; + let _eps_tol: Real = gjk_options.epsilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); let ray_length = ray.dir.norm(); @@ -434,7 +433,7 @@ mod test { use crate::{ math::Real, - query::{self, gjk::GjkOptions, DefaultQueryDispatcher, ShapeCastOptions}, + query::{self, gjk::gjk::GjkOptions, DefaultQueryDispatcher, ShapeCastOptions}, shape::{Ball, ConvexPolygon, Shape}, }; @@ -470,7 +469,7 @@ mod test { let dispatcher = DefaultQueryDispatcher { gjk_options: GjkOptions { - espilon_tolerance: crate::math::DEFAULT_EPSILON * 10000.0, + epsilon_tolerance: crate::math::DEFAULT_EPSILON * 10000.0, ..GjkOptions::default() }, }; diff --git a/src/query/gjk/voronoi_simplex3.rs b/src/query/gjk/voronoi_simplex3.rs index 78b7c86b..40b54dad 100644 --- a/src/query/gjk/voronoi_simplex3.rs +++ b/src/query/gjk/voronoi_simplex3.rs @@ -1,5 +1,6 @@ use crate::math::{Point, Real}; use crate::query::gjk::CSOPoint; +use crate::query::query_options::QueryOptionsNotUsed; use crate::query::{PointQuery, PointQueryWithLocation}; use crate::shape::{ Segment, SegmentPointLocation, Tetrahedron, TetrahedronPointLocation, Triangle, @@ -126,7 +127,11 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) - .project_local_point_and_get_location(&Point::::origin(), true, &()); + .project_local_point_and_get_location( + &Point::::origin(), + true, + &QueryOptionsNotUsed, + ); match location { SegmentPointLocation::OnVertex(0) => { @@ -152,7 +157,11 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ) - .project_local_point_and_get_location(&Point::::origin(), true, &()); + .project_local_point_and_get_location( + &Point::::origin(), + true, + &QueryOptionsNotUsed, + ); match location { TrianglePointLocation::OnVertex(i) => { @@ -192,7 +201,11 @@ impl VoronoiSimplex { self.vertices[2].point, self.vertices[3].point, ) - .project_local_point_and_get_location(&Point::::origin(), true, &()); + .project_local_point_and_get_location( + &Point::::origin(), + true, + &QueryOptionsNotUsed, + ); match location { TetrahedronPointLocation::OnVertex(i) => { @@ -284,7 +297,7 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point(&Point::::origin(), true, &()) + seg.project_local_point(&Point::::origin(), true, &QueryOptionsNotUsed) .point } else if self.dim == 2 { let tri = Triangle::new( @@ -292,7 +305,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ); - tri.project_local_point(&Point::::origin(), true, &()) + tri.project_local_point(&Point::::origin(), true, &QueryOptionsNotUsed) .point } else { let tetr = Tetrahedron::new( @@ -301,7 +314,7 @@ impl VoronoiSimplex { self.vertices[2].point, self.vertices[3].point, ); - tetr.project_local_point(&Point::::origin(), true, &()) + tetr.project_local_point(&Point::::origin(), true, &QueryOptionsNotUsed) .point } } diff --git a/src/query/intersection_test/intersection_test_ball_point_query.rs b/src/query/intersection_test/intersection_test_ball_point_query.rs index 936af232..f5a03f7d 100644 --- a/src/query/intersection_test/intersection_test_ball_point_query.rs +++ b/src/query/intersection_test/intersection_test_ball_point_query.rs @@ -1,5 +1,5 @@ use crate::math::{Isometry, Point, Real}; -use crate::query::PointQuery; +use crate::query::{PointQuery, QueryOptions}; use crate::shape::Ball; /// Intersection test between a ball and a shape implementing the `PointQuery` trait. @@ -7,8 +7,9 @@ pub fn intersection_test_ball_point_query( pos12: &Isometry, ball1: &Ball, point_query2: &P, + options: &dyn QueryOptions, ) -> bool { - intersection_test_point_query_ball(&pos12.inverse(), point_query2, ball1) + intersection_test_point_query_ball(&pos12.inverse(), point_query2, ball1, options) } /// Intersection test between a shape implementing the `PointQuery` trait and a ball. @@ -16,8 +17,9 @@ pub fn intersection_test_point_query_ball( pos12: &Isometry, point_query1: &P, ball2: &Ball, + options: &dyn QueryOptions, ) -> bool { let local_p2_1 = Point::from(pos12.translation.vector); - let proj = point_query1.project_local_point(&local_p2_1, true, &()); + let proj = point_query1.project_local_point(&local_p2_1, true, options); proj.is_inside || (local_p2_1 - proj.point).norm_squared() <= ball2.radius * ball2.radius } diff --git a/src/query/mod.rs b/src/query/mod.rs index 10c6dd7c..ba734ddc 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -45,6 +45,7 @@ pub use self::shape_cast::{ cast_shapes, cast_shapes_with_dispatcher, ShapeCastHit, ShapeCastOptions, ShapeCastStatus, }; pub use self::split::{IntersectResult, SplitResult}; +pub use query_options::{DefaultQueryOptions, QueryOptions, QueryOptionsNotUsed}; #[cfg(all(feature = "dim3", feature = "alloc"))] pub use self::ray::RayCullingMode; @@ -64,6 +65,7 @@ mod intersection_test; mod nonlinear_shape_cast; pub mod point; mod query_dispatcher; +mod query_options; mod ray; pub mod sat; mod shape_cast; diff --git a/src/query/point/mod.rs b/src/query/point/mod.rs index b46766e4..8f739cd0 100644 --- a/src/query/point/mod.rs +++ b/src/query/point/mod.rs @@ -1,7 +1,7 @@ //! Point inclusion and projection. #[doc(inline)] -pub use self::point_query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; +pub use self::point_query::{PointProjection, PointQuery, PointQueryWithLocation}; #[cfg(feature = "alloc")] pub use self::{ point_composite_shape::query_options_dispatcher::{ diff --git a/src/query/point/point_aabb.rs b/src/query/point/point_aabb.rs index 50f97f74..41c157fb 100644 --- a/src/query/point/point_aabb.rs +++ b/src/query/point/point_aabb.rs @@ -1,8 +1,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real, Vector, DIM}; use crate::num::{Bounded, Zero}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::FeatureId; use na; diff --git a/src/query/point/point_ball.rs b/src/query/point/point_ball.rs index c8ae49e4..2ea2bbfe 100644 --- a/src/query/point/point_ball.rs +++ b/src/query/point/point_ball.rs @@ -1,8 +1,7 @@ use na::{self, ComplexField}; use crate::math::{Point, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Ball, FeatureId}; impl PointQuery for Ball { diff --git a/src/query/point/point_bounding_sphere.rs b/src/query/point/point_bounding_sphere.rs index 89c31be8..bce93fff 100644 --- a/src/query/point/point_bounding_sphere.rs +++ b/src/query/point/point_bounding_sphere.rs @@ -1,7 +1,6 @@ use crate::bounding_volume::BoundingSphere; use crate::math::{Point, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Ball, FeatureId}; impl PointQuery for BoundingSphere { diff --git a/src/query/point/point_capsule.rs b/src/query/point/point_capsule.rs index 38eb4c92..d7a23fa9 100644 --- a/src/query/point/point_capsule.rs +++ b/src/query/point/point_capsule.rs @@ -1,7 +1,6 @@ use crate::approx::AbsDiffEq; use crate::math::{Point, Real, Vector}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Capsule, FeatureId, Segment}; use na::{self, Unit}; diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index e8dc353b..e2198eb3 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -7,8 +7,9 @@ use crate::partitioning::BvhNode; use crate::query::point::point_composite_shape::query_options_dispatcher::{ QueryOptionsDispatcher, QueryOptionsDispatcherMap, }; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{ + PointProjection, PointQuery, PointQueryWithLocation, QueryOptions, QueryOptionsNotUsed, +}; use crate::shape::{ CompositeShapeRef, FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, TypedCompositeShape, @@ -17,16 +18,6 @@ use na; use crate::shape::{Compound, Polyline}; -fn to_dispatcher_map_or_default(options: &dyn QueryOptions) -> &dyn QueryOptionsDispatcher { - let options = options - .as_any() - .downcast_ref::() - .map_or(&() as &dyn QueryOptionsDispatcher, |m| { - m as &dyn QueryOptionsDispatcher - }); - options -} - impl CompositeShapeRef<'_, S> { /// Project a point on this composite shape. /// @@ -54,7 +45,8 @@ impl CompositeShapeRef<'_, S> { .find_best( max_dist, |node: &BvhNode, _best_so_far| { - node.aabb().distance_to_local_point(point, true, &()) + node.aabb() + .distance_to_local_point(point, true, &QueryOptionsNotUsed) }, |primitive, _best_so_far| { let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| { @@ -97,7 +89,8 @@ impl CompositeShapeRef<'_, S> { .find_best( Real::MAX, |node: &BvhNode, _best_so_far| { - node.aabb().distance_to_local_point(point, true, &()) + node.aabb() + .distance_to_local_point(point, true, &QueryOptionsNotUsed) }, |primitive, _best_so_far| { let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| { @@ -141,7 +134,8 @@ impl CompositeShapeRef<'_, S> { .find_best( Real::MAX, |node: &BvhNode, _best_so_far| { - node.aabb().distance_to_local_point(point, true, &()) + node.aabb() + .distance_to_local_point(point, true, &QueryOptionsNotUsed) }, |primitive, _best_so_far| { let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| { @@ -207,7 +201,7 @@ impl PointQuery for Polyline { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) .project_local_point(point, solid, options) .1 @@ -219,7 +213,7 @@ impl PointQuery for Polyline { point: &Point, options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); let (seg_id, (proj, feature)) = CompositeShapeRef(self).project_local_point_and_get_feature(point, options); let polyline_feature = self.segment_feature_to_polyline_feature(seg_id, feature); @@ -230,7 +224,7 @@ impl PointQuery for Polyline { #[inline] fn contains_local_point(&self, point: &Point, options: &dyn QueryOptions) -> bool { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) .contains_local_point(point, options) .is_some() @@ -245,7 +239,7 @@ impl PointQuery for TriMesh { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) .project_local_point(point, solid, options) .1 @@ -265,7 +259,7 @@ impl PointQuery for TriMesh { let feature_id = FeatureId::Face(id); return (proj, feature_id); } - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); let solid = cfg!(feature = "dim2"); let (tri_id, proj) = CompositeShapeRef(self).project_local_point(point, solid, options); (proj, FeatureId::Face(tri_id)) @@ -284,7 +278,7 @@ impl PointQuery for TriMesh { .is_inside; } - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) .contains_local_point(point, options) .is_some() @@ -311,7 +305,7 @@ impl PointQuery for Compound { solid: bool, options: &dyn QueryOptions, ) -> PointProjection { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) .project_local_point(point, solid, options) .1 @@ -323,7 +317,7 @@ impl PointQuery for Compound { point: &Point, options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); ( CompositeShapeRef(self) .project_local_point_and_get_feature(point, options) @@ -335,7 +329,7 @@ impl PointQuery for Compound { #[inline] fn contains_local_point(&self, point: &Point, options: &dyn QueryOptions) -> bool { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) .contains_local_point(point, options) .is_some() @@ -352,7 +346,7 @@ impl PointQueryWithLocation for Polyline { solid: bool, options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); let (seg_id, (proj, loc)) = CompositeShapeRef(self) .project_local_point_and_get_location(point, Real::MAX, solid, options) .unwrap(); @@ -383,7 +377,7 @@ impl PointQueryWithLocation for TriMesh { max_dist: Real, options: &dyn QueryOptions, ) -> Option<(PointProjection, Self::Location)> { - let options = to_dispatcher_map_or_default(options); + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); #[allow(unused_mut)] // mut is needed in 3D. if let Some((part_id, (mut proj, location))) = CompositeShapeRef(self) .project_local_point_and_get_location(point, max_dist, solid, options) diff --git a/src/query/point/point_composite_shape/query_options_dispatcher.rs b/src/query/point/point_composite_shape/query_options_dispatcher.rs index 8968ac08..59f9732d 100644 --- a/src/query/point/point_composite_shape/query_options_dispatcher.rs +++ b/src/query/point/point_composite_shape/query_options_dispatcher.rs @@ -2,7 +2,7 @@ use alloc::boxed::Box; use core::any::{Any, TypeId}; use crate::query::gjk::GjkOptions; -use crate::query::point::point_query::QueryOptions; +use crate::query::QueryOptions; use crate::shape::Compound; use hashbrown::HashMap; @@ -82,6 +82,18 @@ impl QueryOptionsDispatcherMap { let option = self.options.get_mut(&TypeId::of::())?; option.as_mut().as_any_mut().downcast_mut::() } + + /// Downcasts a [QueryOptions] into a [QueryOptionsDispatcher], if it's one, otherwise returns `()`, + /// which will result in default options being used. + pub fn from_dyn_or_default(options: &dyn QueryOptions) -> &dyn QueryOptionsDispatcher { + let options = options + .as_any() + .downcast_ref::() + .map_or(&() as &dyn QueryOptionsDispatcher, |m| { + m as &dyn QueryOptionsDispatcher + }); + options + } } impl QueryOptionsDispatcher for QueryOptionsDispatcherMap { diff --git a/src/query/point/point_cone.rs b/src/query/point/point_cone.rs index 8a2b8430..217c9fda 100644 --- a/src/query/point/point_cone.rs +++ b/src/query/point/point_cone.rs @@ -1,6 +1,5 @@ use crate::math::{Point, Real, Vector}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Cone, FeatureId, Segment}; use na; diff --git a/src/query/point/point_cuboid.rs b/src/query/point/point_cuboid.rs index 637648f9..cd846528 100644 --- a/src/query/point/point_cuboid.rs +++ b/src/query/point/point_cuboid.rs @@ -1,7 +1,6 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Cuboid, FeatureId}; impl PointQuery for Cuboid { diff --git a/src/query/point/point_cylinder.rs b/src/query/point/point_cylinder.rs index 4ca9512b..3bce04b4 100644 --- a/src/query/point/point_cylinder.rs +++ b/src/query/point/point_cylinder.rs @@ -1,6 +1,5 @@ use crate::math::{Point, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Cylinder, FeatureId}; use na; diff --git a/src/query/point/point_halfspace.rs b/src/query/point/point_halfspace.rs index 3800b5b9..87c8f2e1 100644 --- a/src/query/point/point_halfspace.rs +++ b/src/query/point/point_halfspace.rs @@ -1,6 +1,5 @@ use crate::math::{Point, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{FeatureId, HalfSpace}; impl PointQuery for HalfSpace { diff --git a/src/query/point/point_heightfield.rs b/src/query/point/point_heightfield.rs index 06041843..08eaba99 100644 --- a/src/query/point/point_heightfield.rs +++ b/src/query/point/point_heightfield.rs @@ -1,7 +1,6 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real, Vector}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; use crate::shape::{FeatureId, HeightField, TrianglePointLocation}; #[cfg(not(feature = "std"))] use na::ComplexField; // For sqrt. diff --git a/src/query/point/point_query.rs b/src/query/point/point_query.rs index 72539d17..defcbfbd 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -1,6 +1,5 @@ -use core::any::Any; - use crate::math::{Isometry, Point, Real}; +use crate::query::QueryOptions; use crate::shape::FeatureId; use na; @@ -42,28 +41,6 @@ impl PointProjection { } } -/// Options to pass to [QueryOptions]. The type to be passed depends on each shape implementation. -/// If you're not sure, use `&()` which will evaluate to default options for included shapes in -/// [QueryOptions] implementations. -/// -/// # See Also -/// - [GjkOptions][crate::query::gjk::GjkOptions] -/// - [QueryOptionsDispatcher][crate::query::point::QueryOptionsDispatcher] -/// - [QueryOptionsDispatcherMap][crate::query::point::QueryOptionsDispatcherMap] -pub trait QueryOptions { - fn as_any(&self) -> &dyn Any; - fn as_any_mut(&mut self) -> &mut dyn Any; -} - -impl QueryOptions for () { - fn as_any(&self) -> &dyn Any { - self - } - fn as_any_mut(&mut self) -> &mut dyn Any { - self - } -} - /// Trait of objects that can be tested for point inclusion and projection. pub trait PointQuery { /// Projects a point on `self`, unless the projection lies further than the given max distance. diff --git a/src/query/point/point_round_shape.rs b/src/query/point/point_round_shape.rs index 7df29735..42bc6276 100644 --- a/src/query/point/point_round_shape.rs +++ b/src/query/point/point_round_shape.rs @@ -1,9 +1,8 @@ use log::warn; use crate::math::{Point, Real}; -use crate::query::gjk::{GjkOptions, VoronoiSimplex}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::gjk::VoronoiSimplex; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{FeatureId, RoundShape, SupportMap}; // TODO: if PointQuery had a `project_point_with_normal` method, we could just @@ -27,7 +26,7 @@ impl PointQuery for RoundShape { options } else { warn!("Incorrect option passed to project_local_point: using default options."); - &GjkOptions::default() + &crate::query::gjk::GjkOptions::default() }; crate::query::details::local_point_projection_on_support_map( self, diff --git a/src/query/point/point_segment.rs b/src/query/point/point_segment.rs index b70b7645..22dadcc4 100644 --- a/src/query/point/point_segment.rs +++ b/src/query/point/point_segment.rs @@ -1,6 +1,5 @@ use crate::math::{Point, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; use crate::shape::{FeatureId, Segment, SegmentPointLocation}; impl PointQuery for Segment { diff --git a/src/query/point/point_support_map.rs b/src/query/point/point_support_map.rs index a3cadad7..122759c6 100644 --- a/src/query/point/point_support_map.rs +++ b/src/query/point/point_support_map.rs @@ -4,11 +4,7 @@ use crate::math::{Isometry, Point, Real, Vector}; #[cfg(feature = "alloc")] use crate::query::epa::EPA; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, GjkOptions, VoronoiSimplex}; -#[cfg(feature = "dim2")] -use crate::query::point::point_query::QueryOptions; -#[cfg(feature = "dim3")] -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; #[cfg(feature = "dim2")] #[cfg(feature = "alloc")] use crate::shape::ConvexPolygon; diff --git a/src/query/point/point_tetrahedron.rs b/src/query/point/point_tetrahedron.rs index 36730f15..861910d1 100644 --- a/src/query/point/point_tetrahedron.rs +++ b/src/query/point/point_tetrahedron.rs @@ -1,6 +1,5 @@ use crate::math::{Point, Real, Vector}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; use crate::shape::{FeatureId, Tetrahedron, TetrahedronPointLocation}; impl PointQuery for Tetrahedron { diff --git a/src/query/point/point_triangle.rs b/src/query/point/point_triangle.rs index 604ab547..666d85d7 100644 --- a/src/query/point/point_triangle.rs +++ b/src/query/point/point_triangle.rs @@ -1,6 +1,5 @@ use crate::math::{Point, Real, Vector, DIM}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; use crate::shape::{FeatureId, Triangle, TrianglePointLocation}; #[inline] diff --git a/src/query/point/point_voxels.rs b/src/query/point/point_voxels.rs index 2985440f..22c8205b 100644 --- a/src/query/point/point_voxels.rs +++ b/src/query/point/point_voxels.rs @@ -1,6 +1,5 @@ use crate::math::{Point, Real}; -use crate::query::point::point_query::QueryOptions; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Cuboid, FeatureId, VoxelType, Voxels}; impl PointQuery for Voxels { diff --git a/src/query/query_options.rs b/src/query/query_options.rs new file mode 100644 index 00000000..1353155b --- /dev/null +++ b/src/query/query_options.rs @@ -0,0 +1,41 @@ +use core::any::Any; + +/// Options to pass to [QueryOptions]. The type to be passed depends on each shape implementation. +/// If you're not sure, use `&()` which will evaluate to default options for included shapes in +/// [QueryOptions] implementations. +/// +/// # See Also +/// - [GjkOptions][crate::query::gjk::GjkOptions] +/// - [QueryOptionsDispatcher][crate::query::point::QueryOptionsDispatcher] +/// - [QueryOptionsDispatcherMap][crate::query::point::QueryOptionsDispatcherMap] +pub trait QueryOptions { + fn as_any(&self) -> &dyn Any; + fn as_any_mut(&mut self) -> &mut dyn Any; +} + +/// Type alias used to communicate that an option is not used downstream. +/// +/// Pass this to a method awaiting `&dyn QueryOptions` that we know doesn't use any algorithm options, +/// so the flow is easier to understand. +/// +/// Its presence should be challenged any time a new algorithm is modified or a new option is introduced. +pub use DefaultQueryOptions as QueryOptionsNotUsed; + +pub struct DefaultQueryOptions; + +impl QueryOptions for DefaultQueryOptions { + fn as_any(&self) -> &dyn Any { + self + } + fn as_any_mut(&mut self) -> &mut dyn Any { + self + } +} +impl QueryOptions for () { + fn as_any(&self) -> &dyn Any { + self + } + fn as_any_mut(&mut self) -> &mut dyn Any { + self + } +} diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index 1f1bd1a8..5a683980 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -1,6 +1,7 @@ //! Traits and structure needed to cast rays. use crate::math::{Isometry, Point, Real, Vector}; +use crate::query::QueryOptions; use crate::shape::FeatureId; #[cfg(feature = "alloc")] @@ -132,8 +133,14 @@ impl BvhLeafCost for RayIntersection { /// Traits of objects which can be transformed and tested for intersection with a ray. pub trait RayCast { /// Computes the time of impact between this transform shape and a ray. - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { - self.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + options: &dyn QueryOptions, + ) -> Option { + self.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) .map(|inter| inter.time_of_impact) } @@ -143,12 +150,19 @@ pub trait RayCast { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option; /// Tests whether a ray intersects this transformed shape. #[inline] - fn intersects_local_ray(&self, ray: &Ray, max_time_of_impact: Real) -> bool { - self.cast_local_ray(ray, max_time_of_impact, true).is_some() + fn intersects_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + options: &dyn QueryOptions, + ) -> bool { + self.cast_local_ray(ray, max_time_of_impact, true, options) + .is_some() } /// Computes the time of impact between this transform shape and a ray. @@ -158,9 +172,10 @@ pub trait RayCast { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { let ls_ray = ray.inverse_transform_by(m); - self.cast_local_ray(&ls_ray, max_time_of_impact, solid) + self.cast_local_ray(&ls_ray, max_time_of_impact, solid, options) } /// Computes the time of impact, and normal between this transformed shape and a ray. @@ -170,16 +185,23 @@ pub trait RayCast { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { let ls_ray = ray.inverse_transform_by(m); - self.cast_local_ray_and_get_normal(&ls_ray, max_time_of_impact, solid) + self.cast_local_ray_and_get_normal(&ls_ray, max_time_of_impact, solid, options) .map(|inter| inter.transform_by(m)) } /// Tests whether a ray intersects this transformed shape. #[inline] - fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_time_of_impact: Real) -> bool { + fn intersects_ray( + &self, + m: &Isometry, + ray: &Ray, + max_time_of_impact: Real, + options: &dyn QueryOptions, + ) -> bool { let ls_ray = ray.inverse_transform_by(m); - self.intersects_local_ray(&ls_ray, max_time_of_impact) + self.intersects_local_ray(&ls_ray, max_time_of_impact, options) } } diff --git a/src/query/ray/ray_aabb.rs b/src/query/ray/ray_aabb.rs index 66cbe5b7..e9489b59 100644 --- a/src/query/ray/ray_aabb.rs +++ b/src/query/ray/ray_aabb.rs @@ -4,12 +4,18 @@ use na; use crate::bounding_volume::Aabb; use crate::math::{Real, Vector, DIM}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::FeatureId; use num::Zero; impl RayCast for Aabb { - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + _options: &dyn QueryOptions, + ) -> Option { let mut tmin: Real = 0.0; let mut tmax: Real = max_time_of_impact; @@ -54,6 +60,7 @@ impl RayCast for Aabb { ray: &Ray, max_time_of_impact: Real, solid: bool, + _options: &dyn QueryOptions, ) -> Option { ray_aabb(self, ray, max_time_of_impact, solid).map(|(t, n, i)| { let feature = if i < 0 { diff --git a/src/query/ray/ray_ball.rs b/src/query/ray/ray_ball.rs index 1f8d0d0e..e9af7641 100644 --- a/src/query/ray/ray_ball.rs +++ b/src/query/ray/ray_ball.rs @@ -1,13 +1,19 @@ use na::{self, ComplexField}; use crate::math::{Point, Real}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{Ball, FeatureId}; use num::Zero; impl RayCast for Ball { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + _options: &dyn QueryOptions, + ) -> Option { ray_toi_with_ball(&Point::origin(), self.radius, ray, solid) .1 .filter(|time_of_impact| *time_of_impact <= max_time_of_impact) @@ -19,6 +25,7 @@ impl RayCast for Ball { ray: &Ray, max_time_of_impact: Real, solid: bool, + _options: &dyn QueryOptions, ) -> Option { ray_toi_and_normal_with_ball(&Point::origin(), self.radius, ray, solid) .1 diff --git a/src/query/ray/ray_bounding_sphere.rs b/src/query/ray/ray_bounding_sphere.rs index d4beec2d..c81ce494 100644 --- a/src/query/ray/ray_bounding_sphere.rs +++ b/src/query/ray/ray_bounding_sphere.rs @@ -1,13 +1,19 @@ use crate::bounding_volume::BoundingSphere; use crate::math::Real; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::Ball; impl RayCast for BoundingSphere { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + options: &dyn QueryOptions, + ) -> Option { let centered_ray = ray.translate_by(-self.center().coords); - Ball::new(self.radius()).cast_local_ray(¢ered_ray, max_time_of_impact, solid) + Ball::new(self.radius()).cast_local_ray(¢ered_ray, max_time_of_impact, solid, options) } #[inline] @@ -16,18 +22,25 @@ impl RayCast for BoundingSphere { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { let centered_ray = ray.translate_by(-self.center().coords); Ball::new(self.radius()).cast_local_ray_and_get_normal( ¢ered_ray, max_time_of_impact, solid, + options, ) } #[inline] - fn intersects_local_ray(&self, ray: &Ray, max_time_of_impact: Real) -> bool { + fn intersects_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + options: &dyn QueryOptions, + ) -> bool { let centered_ray = ray.translate_by(-self.center().coords); - Ball::new(self.radius()).intersects_local_ray(¢ered_ray, max_time_of_impact) + Ball::new(self.radius()).intersects_local_ray(¢ered_ray, max_time_of_impact, options) } } diff --git a/src/query/ray/ray_composite_shape.rs b/src/query/ray/ray_composite_shape.rs index 82018592..bdbb585b 100644 --- a/src/query/ray/ray_composite_shape.rs +++ b/src/query/ray/ray_composite_shape.rs @@ -1,6 +1,9 @@ +use core::any::Any; + use crate::math::Real; use crate::partitioning::BvhNode; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::point::{QueryOptionsDispatcher, QueryOptionsDispatcherMap}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{CompositeShapeRef, Compound, Polyline, TypedCompositeShape}; impl CompositeShapeRef<'_, S> { @@ -22,6 +25,7 @@ impl CompositeShapeRef<'_, S> { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptionsDispatcher, ) -> Option<(u32, Real)> { let hit = self .0 @@ -29,9 +33,20 @@ impl CompositeShapeRef<'_, S> { .cast_ray(ray, max_time_of_impact, |primitive, best_so_far| { self.0.map_typed_part_at(primitive, |pose, part, _| { if let Some(pose) = pose { - part.cast_ray(pose, ray, best_so_far, solid) + part.cast_ray( + pose, + ray, + best_so_far, + solid, + options.get_option_for_shape(&part.type_id()), + ) } else { - part.cast_local_ray(ray, best_so_far, solid) + part.cast_local_ray( + ray, + best_so_far, + solid, + options.get_option_for_shape(&part.type_id()), + ) } })? })?; @@ -45,6 +60,7 @@ impl CompositeShapeRef<'_, S> { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptionsDispatcher, ) -> Option<(u32, RayIntersection)> { self.0.bvh().find_best( max_time_of_impact, @@ -52,9 +68,20 @@ impl CompositeShapeRef<'_, S> { |primitive, best_so_far| { self.0.map_typed_part_at(primitive, |pose, part, _| { if let Some(pose) = pose { - part.cast_ray_and_get_normal(pose, ray, best_so_far, solid) + part.cast_ray_and_get_normal( + pose, + ray, + best_so_far, + solid, + options.get_option_for_shape(&part.type_id()), + ) } else { - part.cast_local_ray_and_get_normal(ray, best_so_far, solid) + part.cast_local_ray_and_get_normal( + ray, + best_so_far, + solid, + options.get_option_for_shape(&part.type_id()), + ) } })? }, @@ -64,9 +91,17 @@ impl CompositeShapeRef<'_, S> { impl RayCast for Polyline { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + options: &dyn QueryOptions, + ) -> Option { + // FIXME: Polyline is made of Segments, which casts only require GjkOptions. So ideally we shouldn't need a full dispatcher. + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .cast_local_ray(ray, max_time_of_impact, solid) + .cast_local_ray(ray, max_time_of_impact, solid, options) .map(|hit| hit.1) } @@ -76,18 +111,28 @@ impl RayCast for Polyline { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + // FIXME: Polyline is made of Segments, which casts only require GjkOptions. So ideally we shouldn't need a full dispatcher. + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) .map(|hit| hit.1) } } impl RayCast for Compound { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + options: &dyn QueryOptions, + ) -> Option { + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .cast_local_ray(ray, max_time_of_impact, solid) + .cast_local_ray(ray, max_time_of_impact, solid, options) .map(|hit| hit.1) } @@ -97,9 +142,11 @@ impl RayCast for Compound { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) .map(|hit| hit.1) } } diff --git a/src/query/ray/ray_cuboid.rs b/src/query/ray/ray_cuboid.rs index 335da927..8ad6eb4e 100644 --- a/src/query/ray/ray_cuboid.rs +++ b/src/query/ray/ray_cuboid.rs @@ -1,14 +1,20 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::Cuboid; impl RayCast for Cuboid { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + options: &dyn QueryOptions, + ) -> Option { let dl = Point::from(-self.half_extents); let ur = Point::from(self.half_extents); - Aabb::new(dl, ur).cast_local_ray(ray, max_time_of_impact, solid) + Aabb::new(dl, ur).cast_local_ray(ray, max_time_of_impact, solid, options) } #[inline] @@ -17,9 +23,10 @@ impl RayCast for Cuboid { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { let dl = Point::from(-self.half_extents); let ur = Point::from(self.half_extents); - Aabb::new(dl, ur).cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + Aabb::new(dl, ur).cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) } } diff --git a/src/query/ray/ray_halfspace.rs b/src/query/ray/ray_halfspace.rs index 5ec27df4..c167345c 100644 --- a/src/query/ray/ray_halfspace.rs +++ b/src/query/ray/ray_halfspace.rs @@ -1,7 +1,7 @@ use na; use crate::math::{Point, Real, Vector}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{FeatureId, HalfSpace}; /// Computes the time_of_impact of an unbounded line with a halfspace described by its center and normal. @@ -45,6 +45,7 @@ impl RayCast for HalfSpace { ray: &Ray, max_time_of_impact: Real, solid: bool, + _options: &dyn QueryOptions, ) -> Option { let dpos = -ray.origin; diff --git a/src/query/ray/ray_heightfield.rs b/src/query/ray/ray_heightfield.rs index 869f2a51..fdfdef25 100644 --- a/src/query/ray/ray_heightfield.rs +++ b/src/query/ray/ray_heightfield.rs @@ -1,7 +1,7 @@ use crate::math::Real; #[cfg(feature = "dim2")] use crate::query; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; #[cfg(feature = "dim2")] use crate::shape::FeatureId; use crate::shape::HeightField; @@ -14,6 +14,7 @@ impl RayCast for HeightField { ray: &Ray, max_time_of_impact: Real, _: bool, + _options: &dyn QueryOptions, ) -> Option { let aabb = self.local_aabb(); let (min_t, mut max_t) = aabb.clip_ray_parameters(ray)?; @@ -126,6 +127,7 @@ impl RayCast for HeightField { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { use num_traits::Bounded; @@ -155,12 +157,12 @@ impl RayCast for HeightField { loop { let tris = self.triangles_at(cell.0, cell.1); - let inter1 = tris - .0 - .and_then(|tri| tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid)); - let inter2 = tris - .1 - .and_then(|tri| tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid)); + let inter1 = tris.0.and_then(|tri| { + tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) + }); + let inter2 = tris.1.and_then(|tri| { + tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) + }); match (inter1, inter2) { (Some(mut inter1), Some(mut inter2)) => { diff --git a/src/query/ray/ray_round_shape.rs b/src/query/ray/ray_round_shape.rs index af4c1006..50bb744f 100644 --- a/src/query/ray/ray_round_shape.rs +++ b/src/query/ray/ray_round_shape.rs @@ -1,6 +1,6 @@ use crate::math::Real; use crate::query::gjk::{GjkOptions, VoronoiSimplex}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{RoundShape, SupportMap}; impl RayCast for RoundShape { @@ -9,15 +9,21 @@ impl RayCast for RoundShape { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + &GjkOptions::default() + }; crate::query::details::local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, max_time_of_impact, solid, - // TODO: be able to pass custom option. - &GjkOptions::default(), + options, ) } } diff --git a/src/query/ray/ray_support_map.rs b/src/query/ray/ray_support_map.rs index 8f62d510..f12c6fca 100644 --- a/src/query/ray/ray_support_map.rs +++ b/src/query/ray/ray_support_map.rs @@ -4,7 +4,7 @@ use crate::math::Real; #[cfg(feature = "dim2")] use crate::query; use crate::query::gjk::{self, CSOPoint, GjkOptions, VoronoiSimplex}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; #[cfg(all(feature = "alloc", feature = "dim2"))] use crate::shape::ConvexPolygon; #[cfg(all(feature = "alloc", feature = "dim3"))] @@ -78,15 +78,21 @@ impl RayCast for Cylinder { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + &GjkOptions::default() + }; local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, max_time_of_impact, solid, - // TODO: be able to pass custom option. - &GjkOptions::default(), + options, ) } } @@ -98,15 +104,21 @@ impl RayCast for Cone { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + &GjkOptions::default() + }; local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, max_time_of_impact, solid, - // TODO: be able to pass custom option. - &GjkOptions::default(), + options, ) } } @@ -117,15 +129,21 @@ impl RayCast for Capsule { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + &GjkOptions::default() + }; local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, max_time_of_impact, solid, - // TODO: be able to pass custom option. - &GjkOptions::default(), + options, ) } } @@ -138,15 +156,21 @@ impl RayCast for ConvexPolyhedron { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + &GjkOptions::default() + }; local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, max_time_of_impact, solid, - // TODO: be able to pass custom option. - &GjkOptions::default(), + options, ) } } @@ -159,15 +183,21 @@ impl RayCast for ConvexPolygon { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + &GjkOptions::default() + }; local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, max_time_of_impact, solid, - // TODO: be able to pass custom option. - &GjkOptions::default(), + options, ) } } @@ -179,7 +209,14 @@ impl RayCast for Segment { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to project_local_point: using default options."); + &GjkOptions::default() + }; #[cfg(feature = "dim2")] { use crate::math::Vector; @@ -190,7 +227,7 @@ impl RayCast for Segment { &ray.dir, &self.a, &seg_dir, - crate::math::DEFAULT_EPSILON, + options.epsilon_tolerance, ); if parallel { @@ -200,7 +237,7 @@ impl RayCast for Segment { let dpos = self.a - ray.origin; let normal = self.normal().map(|n| *n).unwrap_or_else(Vector::zeros); - if dpos.dot(&normal).abs() < crate::math::DEFAULT_EPSILON { + if dpos.dot(&normal).abs() < options.epsilon_tolerance { // The rays and the segment are collinear. let dist1 = dpos.dot(&ray.dir); let dist2 = dist1 + seg_dir.dot(&ray.dir); @@ -265,8 +302,7 @@ impl RayCast for Segment { ray, max_time_of_impact, solid, - // TODO: be able to pass custom option. - &GjkOptions::default(), + options, ) } } diff --git a/src/query/ray/ray_triangle.rs b/src/query/ray/ray_triangle.rs index 5c30b049..e3128bfe 100644 --- a/src/query/ray/ray_triangle.rs +++ b/src/query/ray/ray_triangle.rs @@ -1,7 +1,7 @@ use crate::math::Real; #[cfg(feature = "dim2")] use crate::math::Vector; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{FeatureId, Triangle}; #[cfg(feature = "dim3")] use {crate::math::Point, na::Vector3}; @@ -14,6 +14,7 @@ impl RayCast for Triangle { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { let edges = self.edges(); @@ -32,7 +33,8 @@ impl RayCast for Triangle { let mut smallest_toi = Real::MAX; for edge in &edges { - if let Some(inter) = edge.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + if let Some(inter) = + edge.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) { if inter.time_of_impact < smallest_toi { smallest_toi = inter.time_of_impact; @@ -51,8 +53,10 @@ impl RayCast for Triangle { ray: &Ray, max_time_of_impact: Real, _: bool, + options: &dyn QueryOptions, ) -> Option { - let inter = local_ray_intersection_with_triangle(&self.a, &self.b, &self.c, ray)?.0; + let inter = + local_ray_intersection_with_triangle(&self.a, &self.b, &self.c, ray, options)?.0; if inter.time_of_impact <= max_time_of_impact { Some(inter) @@ -72,6 +76,7 @@ pub fn local_ray_intersection_with_triangle( b: &Point, c: &Point, ray: &Ray, + _options: &dyn QueryOptions, ) -> Option<(RayIntersection, Vector3)> { let ab = *b - *a; let ac = *c - *a; diff --git a/src/query/ray/ray_trimesh.rs b/src/query/ray/ray_trimesh.rs index ee6f9685..a1f15df6 100644 --- a/src/query/ray/ray_trimesh.rs +++ b/src/query/ray/ray_trimesh.rs @@ -1,5 +1,6 @@ use crate::math::Real; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::point::QueryOptionsDispatcherMap; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{CompositeShapeRef, FeatureId, TriMesh}; #[cfg(feature = "dim3")] @@ -7,9 +8,17 @@ pub use ray_cast_with_culling::RayCullingMode; impl RayCast for TriMesh { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + fn cast_local_ray( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + options: &dyn QueryOptions, + ) -> Option { + // FIXME: Polyline is made of Triangles, which casts only require GjkOptions. So ideally we shouldn't need a full dispatcher. + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .cast_local_ray(ray, max_time_of_impact, solid) + .cast_local_ray(ray, max_time_of_impact, solid, options) .map(|hit| hit.1) } @@ -19,9 +28,12 @@ impl RayCast for TriMesh { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { + // FIXME: Polyline is made of Triangles, which casts only require GjkOptions. So ideally we shouldn't need a full dispatcher. + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) .map(|(best, mut res)| { // We hit a backface. // NOTE: we need this for `TriMesh::is_backface` to work properly. @@ -164,7 +176,7 @@ mod ray_cast_with_culling { ray, }; CompositeShapeRef(&mesh_with_culling) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, false) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, false, &()) .map(|(best, mut res)| { // We hit a backface. // NOTE: we need this for `TriMesh::is_backface` to work properly. diff --git a/src/query/ray/ray_voxels.rs b/src/query/ray/ray_voxels.rs index aa6f9612..8ee6de13 100644 --- a/src/query/ray/ray_voxels.rs +++ b/src/query/ray/ray_voxels.rs @@ -1,5 +1,5 @@ use crate::math::{Real, Vector}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{FeatureId, Voxels}; impl RayCast for Voxels { @@ -9,6 +9,7 @@ impl RayCast for Voxels { ray: &Ray, max_time_of_impact: Real, solid: bool, + options: &dyn QueryOptions, ) -> Option { use num_traits::Bounded; @@ -38,7 +39,7 @@ impl RayCast for Voxels { // We hit a voxel! // TODO: if `solid` is false, and we started hitting from the first iteration, // then we should continue the ray propagation until we reach empty space again. - let hit = aabb.cast_local_ray_and_get_normal(ray, max_t, solid); + let hit = aabb.cast_local_ray_and_get_normal(ray, max_t, solid, options); if let Some(mut hit) = hit { // TODO: have the feature id be based on the voxel type? diff --git a/src/query/shape_cast/shape_cast_composite_shape_shape.rs b/src/query/shape_cast/shape_cast_composite_shape_shape.rs index d7c7aada..399b90c7 100644 --- a/src/query/shape_cast/shape_cast_composite_shape_shape.rs +++ b/src/query/shape_cast/shape_cast_composite_shape_shape.rs @@ -2,7 +2,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Isometry, Point, Real, Vector}; use crate::partitioning::BvhNode; use crate::query::shape_cast::ShapeCastOptions; -use crate::query::{QueryDispatcher, Ray, RayCast, ShapeCastHit}; +use crate::query::{QueryDispatcher, QueryOptionsNotUsed, Ray, RayCast, ShapeCastHit}; use crate::shape::{CompositeShapeRef, Shape, TypedCompositeShape}; use simba::simd::SimdValue; @@ -36,7 +36,7 @@ impl CompositeShapeRef<'_, S> { }; // Compute the time of impact. - msum.cast_local_ray(&ray, best_so_far, true) + msum.cast_local_ray(&ray, best_so_far, true, &QueryOptionsNotUsed) .unwrap_or(Real::MAX) }, |part_id, _| { diff --git a/src/query/shape_cast/shape_cast_halfspace_support_map.rs b/src/query/shape_cast/shape_cast_halfspace_support_map.rs index bbc4b0ce..95ccf8d7 100644 --- a/src/query/shape_cast/shape_cast_halfspace_support_map.rs +++ b/src/query/shape_cast/shape_cast_halfspace_support_map.rs @@ -1,6 +1,6 @@ use crate::math::{Isometry, Real, Vector}; use crate::query::details::ShapeCastOptions; -use crate::query::{Ray, RayCast, ShapeCastHit, ShapeCastStatus}; +use crate::query::{QueryOptionsNotUsed, Ray, RayCast, ShapeCastHit, ShapeCastStatus}; use crate::shape::{HalfSpace, RoundShapeRef, SupportMap}; /// Time Of Impact of a halfspace with a support-mapped shape under translational movement. @@ -29,7 +29,9 @@ pub fn cast_shapes_halfspace_support_map( let closest_point = support_point; let ray = Ray::new(closest_point, *vel12); - if let Some(time_of_impact) = halfspace.cast_local_ray(&ray, options.max_time_of_impact, true) { + if let Some(time_of_impact) = + halfspace.cast_local_ray(&ray, options.max_time_of_impact, true, &QueryOptionsNotUsed) + { if time_of_impact > options.max_time_of_impact { return None; } diff --git a/src/query/shape_cast/shape_cast_heightfield_shape.rs b/src/query/shape_cast/shape_cast_heightfield_shape.rs index 0f329f4e..f36dbb79 100644 --- a/src/query/shape_cast/shape_cast_heightfield_shape.rs +++ b/src/query/shape_cast/shape_cast_heightfield_shape.rs @@ -105,6 +105,8 @@ pub fn cast_shapes_heightfield_shape( g2: &dyn Shape, options: ShapeCastOptions, ) -> Result, Unsupported> { + use crate::query::QueryOptionsNotUsed; + let aabb1 = heightfield1.local_aabb(); let mut aabb2_1 = g2.compute_aabb(pos12).loosened(options.target_distance); let ray = Ray::new(aabb2_1.center(), *vel12); @@ -112,7 +114,9 @@ pub fn cast_shapes_heightfield_shape( // Find the first hit between the aabbs. let hext2_1 = aabb2_1.half_extents(); let msum = Aabb::new(aabb1.mins - hext2_1, aabb1.maxs + hext2_1); - if let Some(time_of_impact) = msum.cast_local_ray(&ray, options.max_time_of_impact, true) { + if let Some(time_of_impact) = + msum.cast_local_ray(&ray, options.max_time_of_impact, true, &QueryOptionsNotUsed) + { // Advance the aabb2 to the hit point. aabb2_1.mins += ray.dir * time_of_impact; aabb2_1.maxs += ray.dir * time_of_impact; diff --git a/src/shape/segment.rs b/src/shape/segment.rs index 58a8502d..692b24a8 100644 --- a/src/shape/segment.rs +++ b/src/shape/segment.rs @@ -384,7 +384,7 @@ mod test { ), }; - let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX); + let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX, &()); assert_eq!(hit, false); } #[test] @@ -411,7 +411,7 @@ mod test { ), }; - let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX); + let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX, &()); assert_eq!(hit, true); } #[test] @@ -438,7 +438,7 @@ mod test { ), }; - let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX); + let hit = segment.intersects_ray(&Isometry::identity(), &ray, Real::MAX, &()); assert_eq!(hit, false); } } From 80a1b25202ccc540fa7e286b323346b0df539a99 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 24 Jul 2025 16:46:43 +0200 Subject: [PATCH 20/20] Made more clear when query options are needed or not, adapted example to change epsilon each frame --- crates/parry2d/examples/debug_shape_cast2d.rs | 15 ++++- crates/parry2d/examples/point_query2d.rs | 2 +- crates/parry2d/examples/raycasts_animated.rs | 2 +- crates/parry2d/examples/solid_ray_cast2d.rs | 8 +-- crates/parry2d/tests/geometry/ray_cast.rs | 27 ++++----- crates/parry3d/examples/getting_started.rs | 2 +- crates/parry3d/examples/solid_ray_cast3d.rs | 8 +-- src/bounding_volume/aabb.rs | 4 +- src/partitioning/bvh/bvh_tree.rs | 4 +- src/query/contact/contact_cuboid_cuboid.rs | 6 +- .../contact_manifolds_voxels_ball.rs | 6 +- ...ontact_manifolds_voxels_composite_shape.rs | 8 ++- .../contact_manifolds_voxels_shape.rs | 8 ++- .../contact_manifolds_voxels_voxels.rs | 13 +++-- src/query/default_query_dispatcher.rs | 8 ++- src/query/gjk/voronoi_simplex2.rs | 18 ++++-- src/query/point/point_composite_shape.rs | 42 ++++++-------- .../query_options_dispatcher.rs | 19 ++++++- src/query/query_options.rs | 19 ++++++- src/query/ray/ray_trimesh.rs | 57 +++++++++++++++---- src/query/split/split_trimesh.rs | 4 +- src/shape/polyline.rs | 5 +- .../mesh_intersection/mesh_intersection.rs | 19 +++++-- 23 files changed, 201 insertions(+), 103 deletions(-) diff --git a/crates/parry2d/examples/debug_shape_cast2d.rs b/crates/parry2d/examples/debug_shape_cast2d.rs index c7ad6279..dadcacc5 100644 --- a/crates/parry2d/examples/debug_shape_cast2d.rs +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -7,12 +7,14 @@ use parry2d::math::{self, Isometry}; use parry2d::query::{self, DefaultQueryDispatcher, Ray, ShapeCastOptions}; use parry2d::shape::{Ball, ConvexPolygon, Shape}; +use crate::common_macroquad2d::easy_draw_text; + const RENDER_SCALE: f32 = 1.0; const BALLCAST_WIDTH: f32 = 16.0; #[macroquad::main("raycasts_animated")] async fn main() { - for _i in 1.. { + for i in 1.. { clear_background(BLACK); let screen_shift = Point2::new(screen_width() / 2.0, screen_height() / 2.0); @@ -33,19 +35,22 @@ async fn main() { let mouse_position_world = (Point2::::new(mouse_pos.0, mouse_pos.1) - screen_shift.coords) / RENDER_SCALE; let target_pos: Point2 = [-312.0, 152.0].into(); - + let epsilon_tolerance = + f32::EPSILON + (((i as f32 / 10f32).sin() + 1f32) / 2f32) * 0.002f32; // 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(), + epsilon_tolerance, ); shape_cast_debug( screen_shift, [98.0, -31.0].into(), target_pos, to_cast_against.clone(), + epsilon_tolerance, ); // This fails with `niter == 100` (and `niter == 100_000`), fixed with a tolerance * 10_000 shape_cast_debug( @@ -53,6 +58,7 @@ async fn main() { [47.0, -32.0].into(), target_pos, to_cast_against.clone(), + epsilon_tolerance, ); // For debug purposes, raycast to mouse position. @@ -62,6 +68,7 @@ async fn main() { target_pos, mouse_position_world, to_cast_against.clone(), + epsilon_tolerance, ); /* @@ -76,6 +83,7 @@ async fn main() { screen_shift, GREEN, ); + easy_draw_text(&format!("tolerance: {:.7}", epsilon_tolerance)); next_frame().await } @@ -86,6 +94,7 @@ fn shape_cast_debug( source_pos: Point2, target_pos: Point2, to_cast_against: ConvexPolygon, + epsilon_tolerance: f32, ) { /* * @@ -111,7 +120,7 @@ fn shape_cast_debug( ShapeCastOptions::with_max_time_of_impact(1.0), DefaultQueryDispatcher { gjk_options: query::gjk::GjkOptions { - epsilon_tolerance: math::DEFAULT_EPSILON * 1000f32, + epsilon_tolerance, nb_max_iterations: 100, }, }, diff --git a/crates/parry2d/examples/point_query2d.rs b/crates/parry2d/examples/point_query2d.rs index 2bfd2d58..b0273d10 100644 --- a/crates/parry2d/examples/point_query2d.rs +++ b/crates/parry2d/examples/point_query2d.rs @@ -6,9 +6,9 @@ use core::f32; use common_macroquad2d::draw_point; use macroquad::prelude::*; use nalgebra::{Isometry, Point2, Rotation, Translation}; -use parry2d::query::details::point_query::QueryOptions; use parry2d::query::gjk::GjkOptions; use parry2d::query::point::QueryOptionsDispatcherMap; +use parry2d::query::QueryOptions; use parry2d::shape::Compound; use parry2d::shape::{ConvexPolygon, Shape, SharedShape}; diff --git a/crates/parry2d/examples/raycasts_animated.rs b/crates/parry2d/examples/raycasts_animated.rs index 3aa85fe8..254c630d 100644 --- a/crates/parry2d/examples/raycasts_animated.rs +++ b/crates/parry2d/examples/raycasts_animated.rs @@ -35,7 +35,7 @@ async fn main() { Point2::new(2.0, 2.0), UnitComplex::new(animation_rotation * i as f32) * -Vector2::x(), ); - let toi = cube.cast_ray(&cube_pose, &ray, f32::MAX, true); + let toi = cube.cast_ray(&cube_pose, &ray, f32::MAX, true, &()); /* * diff --git a/crates/parry2d/examples/solid_ray_cast2d.rs b/crates/parry2d/examples/solid_ray_cast2d.rs index 78223df9..93c82868 100644 --- a/crates/parry2d/examples/solid_ray_cast2d.rs +++ b/crates/parry2d/examples/solid_ray_cast2d.rs @@ -12,7 +12,7 @@ fn main() { // Solid cast. assert_eq!( cuboid - .cast_ray(&Isometry2::identity(), &ray_inside, f32::MAX, true) + .cast_ray(&Isometry2::identity(), &ray_inside, f32::MAX, true, &()) .unwrap(), 0.0 ); @@ -20,16 +20,16 @@ fn main() { // Non-solid cast. assert_eq!( cuboid - .cast_ray(&Isometry2::identity(), &ray_inside, f32::MAX, false) + .cast_ray(&Isometry2::identity(), &ray_inside, f32::MAX, false, &()) .unwrap(), 2.0 ); // The other ray does not intersect this shape. assert!(cuboid - .cast_ray(&Isometry2::identity(), &ray_miss, f32::MAX, false) + .cast_ray(&Isometry2::identity(), &ray_miss, f32::MAX, false, &()) .is_none()); assert!(cuboid - .cast_ray(&Isometry2::identity(), &ray_miss, f32::MAX, true) + .cast_ray(&Isometry2::identity(), &ray_miss, f32::MAX, true, &()) .is_none()); } diff --git a/crates/parry2d/tests/geometry/ray_cast.rs b/crates/parry2d/tests/geometry/ray_cast.rs index 160cad69..8ffc36dc 100644 --- a/crates/parry2d/tests/geometry/ray_cast.rs +++ b/crates/parry2d/tests/geometry/ray_cast.rs @@ -9,7 +9,7 @@ fn issue_178_parallel_raycast() { let ray = Ray::new(Point2::new(0.0, 0.0), Vector2::new(0.0, 1.0)); let seg = Segment::new(Point2::new(2.0, 1.0), Point2::new(2.0, 0.0)); - let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); + let cast = seg.cast_ray(&m1, &ray, f32::MAX, true, &()); assert!(cast.is_none()); } @@ -19,7 +19,7 @@ fn parallel_raycast() { let ray = Ray::new(Point2::new(0.0, 0.0), Vector2::new(0.0, 1.0)); let seg = Segment::new(Point2::new(2.0, 1.0), Point2::new(2.0, -1.0)); - let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); + let cast = seg.cast_ray(&m1, &ray, f32::MAX, true, &()); assert!(cast.is_none()); } @@ -29,7 +29,7 @@ fn collinear_raycast_starting_on_segment() { let ray = Ray::new(Point2::new(0.0, 0.0), Vector2::new(0.0, 1.0)); let seg = Segment::new(Point2::new(0.0, 1.0), Point2::new(0.0, -1.0)); - let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); + let cast = seg.cast_ray(&m1, &ray, f32::MAX, true, &()); assert_eq!(cast, Some(0.0)); } @@ -39,7 +39,7 @@ fn collinear_raycast_starting_below_segment() { let ray = Ray::new(Point2::new(0.0, -2.0), Vector2::new(0.0, 1.0)); let seg = Segment::new(Point2::new(0.0, 1.0), Point2::new(0.0, -1.0)); - let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); + let cast = seg.cast_ray(&m1, &ray, f32::MAX, true, &()); assert_eq!(cast, Some(1.0)); } @@ -49,7 +49,7 @@ fn collinear_raycast_starting_above_segment() { let ray = Ray::new(Point2::new(0.0, 2.0), Vector2::new(0.0, 1.0)); let seg = Segment::new(Point2::new(0.0, 1.0), Point2::new(0.0, -1.0)); - let cast = seg.cast_ray(&m1, &ray, f32::MAX, true); + let cast = seg.cast_ray(&m1, &ray, f32::MAX, true, &()); assert_eq!(cast, None); } @@ -57,14 +57,14 @@ fn collinear_raycast_starting_above_segment() { fn perpendicular_raycast_starting_behind_segment() { let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); let ray = Ray::new(Point2::new(-1.0, 0.0), Vector2::new(1.0, 0.0)); - assert!(segment.intersects_local_ray(&ray, f32::MAX)); + assert!(segment.intersects_local_ray(&ray, f32::MAX, &())); } #[test] fn perpendicular_raycast_starting_in_front_of_segment() { let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); let ray = Ray::new(Point2::new(1.0, 0.0), Vector2::new(1.0, 0.0)); - assert!(!segment.intersects_local_ray(&ray, f32::MAX)); + assert!(!segment.intersects_local_ray(&ray, f32::MAX, &())); } #[test] @@ -72,7 +72,7 @@ fn perpendicular_raycast_starting_on_segment() { let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); let ray = Ray::new(Point2::new(0.0, 3.0), Vector2::new(1.0, 0.0)); - let cast = segment.cast_local_ray(&ray, f32::MAX, true); + let cast = segment.cast_local_ray(&ray, f32::MAX, true, &()); assert_eq!(cast, Some(0.0)); } @@ -80,14 +80,14 @@ fn perpendicular_raycast_starting_on_segment() { fn perpendicular_raycast_starting_above_segment() { let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); let ray = Ray::new(Point2::new(0.0, 11.0), Vector2::new(1.0, 0.0)); - assert!(!segment.intersects_local_ray(&ray, f32::MAX)); + assert!(!segment.intersects_local_ray(&ray, f32::MAX, &())); } #[test] fn perpendicular_raycast_starting_below_segment() { let segment = Segment::new(Point2::new(0.0f32, -10.0), Point2::new(0.0, 10.0)); let ray = Ray::new(Point2::new(0.0, -11.0), Vector2::new(1.0, 0.0)); - assert!(!segment.intersects_local_ray(&ray, f32::MAX)); + assert!(!segment.intersects_local_ray(&ray, f32::MAX, &())); } #[test] @@ -99,7 +99,7 @@ fn raycast_starting_outside_of_triangle() { ); let ray = Ray::new(Point2::new(-10.0, 0.0), Vector2::new(1.0, 0.0)); let intersect = triangle - .cast_local_ray_and_get_normal(&ray, f32::MAX, true) + .cast_local_ray_and_get_normal(&ray, f32::MAX, true, &()) .expect("No intersection"); assert_ne!(intersect.time_of_impact, 0.0); @@ -114,7 +114,7 @@ fn raycast_starting_inside_of_triangle() { ); let ray = Ray::new(Point2::new(2.0, 0.0), Vector2::new(1.0, 0.0)); let intersect = triangle - .cast_local_ray_and_get_normal(&ray, f32::MAX, true) + .cast_local_ray_and_get_normal(&ray, f32::MAX, true, &()) .expect("No intersection"); assert_eq!(intersect.time_of_impact, 0.0); @@ -129,7 +129,7 @@ fn raycast_starting_on_edge_of_triangle() { ); let ray = Ray::new(Point2::new(0.0, 0.0), Vector2::new(1.0, 0.0)); let intersect = triangle - .cast_local_ray_and_get_normal(&ray, f32::MAX, true) + .cast_local_ray_and_get_normal(&ray, f32::MAX, true, &()) .expect("No intersection"); assert_eq!(intersect.time_of_impact, 0.0); @@ -163,6 +163,7 @@ fn convexpoly_raycast_fuzz() { &Ray::new(ray_origin, ray_angle.normalize()), Real::MAX, true, + &(), ) }; diff --git a/crates/parry3d/examples/getting_started.rs b/crates/parry3d/examples/getting_started.rs index 0776f4c2..76660c82 100644 --- a/crates/parry3d/examples/getting_started.rs +++ b/crates/parry3d/examples/getting_started.rs @@ -8,5 +8,5 @@ fn main() { let cube = Cuboid::new(Vector3::new(1.0f32, 1.0, 1.0)); let ray = Ray::new(Point3::new(0.0f32, 0.0, -1.0), Vector3::z()); - assert!(cube.intersects_ray(&Isometry3::identity(), &ray, f32::MAX)); + assert!(cube.intersects_ray(&Isometry3::identity(), &ray, f32::MAX, &())); } diff --git a/crates/parry3d/examples/solid_ray_cast3d.rs b/crates/parry3d/examples/solid_ray_cast3d.rs index 0dc9afab..9866ff17 100644 --- a/crates/parry3d/examples/solid_ray_cast3d.rs +++ b/crates/parry3d/examples/solid_ray_cast3d.rs @@ -12,7 +12,7 @@ fn main() { // Solid cast. assert_eq!( cuboid - .cast_ray(&Isometry3::identity(), &ray_inside, f32::MAX, true) + .cast_ray(&Isometry3::identity(), &ray_inside, f32::MAX, true, &()) .unwrap(), 0.0 ); @@ -20,16 +20,16 @@ fn main() { // Non-solid cast. assert_eq!( cuboid - .cast_ray(&Isometry3::identity(), &ray_inside, f32::MAX, false) + .cast_ray(&Isometry3::identity(), &ray_inside, f32::MAX, false, &()) .unwrap(), 2.0 ); // The other ray does not intersect this shape. assert!(cuboid - .cast_ray(&Isometry3::identity(), &ray_miss, f32::MAX, false) + .cast_ray(&Isometry3::identity(), &ray_miss, f32::MAX, false, &()) .is_none()); assert!(cuboid - .cast_ray(&Isometry3::identity(), &ray_miss, f32::MAX, true) + .cast_ray(&Isometry3::identity(), &ray_miss, f32::MAX, true, &()) .is_none()); } diff --git a/src/bounding_volume/aabb.rs b/src/bounding_volume/aabb.rs index 0037f537..cec677c0 100644 --- a/src/bounding_volume/aabb.rs +++ b/src/bounding_volume/aabb.rs @@ -12,7 +12,7 @@ use num::Bounded; use na::ComplexField; // for .sin_cos() -use crate::query::{Ray, RayCast}; +use crate::query::{QueryOptionsNotUsed, Ray, RayCast}; #[cfg(feature = "rkyv")] use rkyv::{bytecheck, CheckBytes}; @@ -301,7 +301,7 @@ impl Aabb { }; let ray = Ray::new(Point::origin(), vel12); - msum.intersects_local_ray(&ray, 1.0, &()) + msum.intersects_local_ray(&ray, 1.0, &QueryOptionsNotUsed) } /// Computes the intersection of this `Aabb` and another one. diff --git a/src/partitioning/bvh/bvh_tree.rs b/src/partitioning/bvh/bvh_tree.rs index 3fe4062f..bd3fcec4 100644 --- a/src/partitioning/bvh/bvh_tree.rs +++ b/src/partitioning/bvh/bvh_tree.rs @@ -1,7 +1,7 @@ use super::BvhOptimizationHeapEntry; use crate::bounding_volume::{Aabb, BoundingVolume}; use crate::math::{Point, Real, Vector}; -use crate::query::{Ray, RayCast}; +use crate::query::{QueryOptionsNotUsed, Ray, RayCast}; use crate::utils::VecMap; use alloc::collections::{BinaryHeap, VecDeque}; use alloc::vec::Vec; @@ -355,7 +355,7 @@ impl BvhNode { /// Returns `Real::MAX` if there is no hit. pub fn cast_ray(&self, ray: &Ray, max_toi: Real) -> Real { self.aabb() - .cast_local_ray(ray, max_toi, true, &()) + .cast_local_ray(ray, max_toi, true, &QueryOptionsNotUsed) .unwrap_or(Real::MAX) } diff --git a/src/query/contact/contact_cuboid_cuboid.rs b/src/query/contact/contact_cuboid_cuboid.rs index 91c49251..1a9380a0 100644 --- a/src/query/contact/contact_cuboid_cuboid.rs +++ b/src/query/contact/contact_cuboid_cuboid.rs @@ -1,5 +1,5 @@ use crate::math::{Isometry, Real}; -use crate::query::{sat, Contact, PointQuery}; +use crate::query::{sat, Contact, PointQuery, QueryOptionsNotUsed}; use crate::shape::{Cuboid, SupportMap}; use approx::AbsDiffEq; use na::Unit; @@ -38,7 +38,7 @@ pub fn contact_cuboid_cuboid( // from cuboid2 on the support-face of cuboid1. For simplicity, we just // project the support point from cuboid2 on cuboid1 itself (not just the face). let pt2_1 = cuboid2.support_point(pos12, &-sep1.1); - let proj1 = cuboid1.project_local_point(&pt2_1, false, &()); + let proj1 = cuboid1.project_local_point(&pt2_1, false, &QueryOptionsNotUsed); let separation = (pt2_1 - proj1.point).dot(&sep1.1); let normalized_dir = Unit::try_new_and_get(pt2_1 - proj1.point, Real::default_epsilon()); @@ -77,7 +77,7 @@ pub fn contact_cuboid_cuboid( // from cuboid1 on the support-face of cuboid2. For simplicity, we just // project the support point from cuboid1 on cuboid2 itself (not just the face). let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1); - let proj2 = cuboid2.project_local_point(&pt1_2, false, &()); + let proj2 = cuboid2.project_local_point(&pt1_2, false, &QueryOptionsNotUsed); let separation = (pt1_2 - proj2.point).dot(&sep2.1); let normalized_dir = Unit::try_new_and_get(pt1_2 - proj2.point, Real::default_epsilon()); diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs b/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs index 3d95cc1b..f61ef672 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs @@ -1,6 +1,6 @@ use crate::bounding_volume::BoundingVolume; use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::{ContactManifold, PointQuery, TrackedContact}; +use crate::query::{ContactManifold, PointQuery, QueryOptionsNotUsed, TrackedContact}; use crate::shape::{ Ball, Cuboid, OctantPattern, PackedFeatureId, Shape, VoxelState, VoxelType, Voxels, }; @@ -163,7 +163,7 @@ pub fn project_point_on_pseudo_cube( // collision. let cuboid = Cuboid::new(Vector::repeat(1.0)); let unit_dpos_pt = Point::from(unit_dpos); - let proj = cuboid.project_local_point(&unit_dpos_pt, false, &()); + let proj = cuboid.project_local_point(&unit_dpos_pt, false, &QueryOptionsNotUsed); let mut normal = unit_dpos_pt - proj.point; let dist = normal.try_normalize_mut(1.0e-8)?; Some((normal, dist)) @@ -172,7 +172,7 @@ pub fn project_point_on_pseudo_cube( OctantPattern::EDGE_X | OctantPattern::EDGE_Y | OctantPattern::EDGE_Z => { let cuboid = Cuboid::new(Vector::repeat(1.0)); let unit_dpos_pt = Point::from(unit_dpos); - let proj = cuboid.project_local_point(&unit_dpos_pt, false, &()); + let proj = cuboid.project_local_point(&unit_dpos_pt, false, &QueryOptionsNotUsed); let mut normal = unit_dpos_pt - proj.point; let dist = normal.try_normalize_mut(1.0e-8)?; Some((normal, dist)) diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs index 6ad9b3f3..f72696b0 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs @@ -5,7 +5,7 @@ use crate::query::details::{ }; use crate::query::{ ContactManifold, ContactManifoldsWorkspace, PersistentQueryDispatcher, PointQuery, - TypedWorkspaceData, WorkspaceData, + QueryOptionsNotUsed, TypedWorkspaceData, WorkspaceData, }; use crate::shape::{CompositeShape, Cuboid, Shape, SupportMap, VoxelType, Voxels}; use crate::utils::hashmap::Entry; @@ -259,8 +259,10 @@ pub fn contact_manifolds_voxels_composite_shape( manifold.subshape_pos1.transform_point(&pt.local_p1) - vox1.center.coords }; - sub_detector.selected_contacts |= - (test_voxel.contains_local_point(&pt_in_voxel_space, &()) as u32) << i; + sub_detector.selected_contacts |= (test_voxel + .contains_local_point(&pt_in_voxel_space, &QueryOptionsNotUsed) + as u32) + << i; } }); }; diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs index d17c358c..cf649682 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs @@ -2,7 +2,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Isometry, Point, Real, Translation, Vector, DIM}; use crate::query::{ ContactManifold, ContactManifoldsWorkspace, PersistentQueryDispatcher, PointQuery, - TypedWorkspaceData, WorkspaceData, + QueryOptionsNotUsed, TypedWorkspaceData, WorkspaceData, }; use crate::shape::{AxisMask, Cuboid, Shape, SupportMap, VoxelData, VoxelType, Voxels}; use crate::utils::hashmap::{Entry, HashMap}; @@ -288,8 +288,10 @@ pub fn contact_manifolds_voxels_shape( } else { manifold.subshape_pos1.transform_point(&pt.local_p1) - vox1.center.coords }; - sub_detector.selected_contacts |= - (test_voxel.contains_local_point(&pt_in_voxel_space, &()) as u32) << i; + sub_detector.selected_contacts |= (test_voxel + .contains_local_point(&pt_in_voxel_space, &QueryOptionsNotUsed) + as u32) + << i; } } } diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs b/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs index 0bd09b96..ef7eddc3 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs @@ -4,7 +4,7 @@ use crate::query::contact_manifolds::{CanonicalVoxelShape, VoxelsShapeContactMan use crate::query::details::VoxelsShapeSubDetector; use crate::query::{ ContactManifold, ContactManifoldsWorkspace, PersistentQueryDispatcher, PointQuery, - TypedWorkspaceData, WorkspaceData, + QueryOptionsNotUsed, TypedWorkspaceData, WorkspaceData, }; use crate::shape::{Cuboid, Shape, SupportMap, VoxelData, VoxelType, Voxels}; use crate::utils::hashmap::Entry; @@ -226,10 +226,13 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( manifold.subshape_pos1.transform_point(&pt.local_p1) - vox1.center.coords; let pt_in_voxel_space2 = manifold.subshape_pos2.transform_point(&pt.local_p2) - vox2.center.coords; - sub_detector.selected_contacts |= - ((test_voxel1.contains_local_point(&pt_in_voxel_space1, &()) as u32) << i) - & ((test_voxel2.contains_local_point(&pt_in_voxel_space2, &()) as u32) - << i); + sub_detector.selected_contacts |= ((test_voxel1 + .contains_local_point(&pt_in_voxel_space1, &QueryOptionsNotUsed) + as u32) + << i) + & ((test_voxel2.contains_local_point(&pt_in_voxel_space2, &QueryOptionsNotUsed) + as u32) + << i); } }; diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 8d72faa5..5fa4f876 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -49,7 +49,9 @@ impl QueryDispatcher for DefaultQueryDispatcher { pos12, b1, shape2, - // FIXME: This may need a query option dispatcher, because a user custom shape against a ball may need a different option. + // FIXME: This needs a query option dispatcher, because a user custom shape against a ball may need a different option. + // Currently, some paths lead to an unused option. + // (Gjk is used for intersections with convex shapes and cylinder) &self.gjk_options, )) } else if let Some(b2) = shape2.as_ball() { @@ -57,7 +59,9 @@ impl QueryDispatcher for DefaultQueryDispatcher { pos12, shape1, b2, - // FIXME: This may need a query option dispatcher, because a user custom shape against a ball may need a different option. + // FIXME: This needs a query option dispatcher, because a user custom shape against a ball may need a different option. + // Currently, some paths lead to an unused option. + // (Gjk is used for intersections with convex shapes and cylinder) &self.gjk_options, )) } else if let (Some(p1), Some(s2)) = diff --git a/src/query/gjk/voronoi_simplex2.rs b/src/query/gjk/voronoi_simplex2.rs index 2939f7ba..b8d87dc7 100644 --- a/src/query/gjk/voronoi_simplex2.rs +++ b/src/query/gjk/voronoi_simplex2.rs @@ -1,6 +1,6 @@ use crate::math::{Point, Real}; use crate::query::gjk::CSOPoint; -use crate::query::{PointQuery, PointQueryWithLocation}; +use crate::query::{PointQuery, PointQueryWithLocation, QueryOptionsNotUsed}; use crate::shape::{Segment, SegmentPointLocation, Triangle, TrianglePointLocation}; /// A simplex of dimension up to 2 using Voronoï regions for computing point projections. @@ -99,7 +99,11 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) - .project_local_point_and_get_location(&Point::::origin(), true, &()); + .project_local_point_and_get_location( + &Point::::origin(), + true, + &QueryOptionsNotUsed, + ); match location { SegmentPointLocation::OnVertex(0) => { @@ -125,7 +129,11 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ) - .project_local_point_and_get_location(&Point::::origin(), true, &()); + .project_local_point_and_get_location( + &Point::::origin(), + true, + &QueryOptionsNotUsed, + ); match location { TrianglePointLocation::OnVertex(i) => { @@ -161,7 +169,7 @@ impl VoronoiSimplex { self.vertices[0].point } else if self.dim == 1 { let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point(&Point::::origin(), true, &()) + seg.project_local_point(&Point::::origin(), true, &QueryOptionsNotUsed) .point } else { assert!(self.dim == 2); @@ -170,7 +178,7 @@ impl VoronoiSimplex { self.vertices[1].point, self.vertices[2].point, ); - tri.project_local_point(&Point::::origin(), true, &()) + tri.project_local_point(&Point::::origin(), true, &QueryOptionsNotUsed) .point } } diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index e2198eb3..5f467cf1 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -199,11 +199,10 @@ impl PointQuery for Polyline { &self, point: &Point, solid: bool, - options: &dyn QueryOptions, + _options: &dyn QueryOptions, ) -> PointProjection { - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .project_local_point(point, solid, options) + .project_local_point(point, solid, &QueryOptionsNotUsed) .1 } @@ -211,11 +210,10 @@ impl PointQuery for Polyline { fn project_local_point_and_get_feature( &self, point: &Point, - options: &dyn QueryOptions, + _options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); - let (seg_id, (proj, feature)) = - CompositeShapeRef(self).project_local_point_and_get_feature(point, options); + let (seg_id, (proj, feature)) = CompositeShapeRef(self) + .project_local_point_and_get_feature(point, &QueryOptionsNotUsed); let polyline_feature = self.segment_feature_to_polyline_feature(seg_id, feature); (proj, polyline_feature) } @@ -237,11 +235,10 @@ impl PointQuery for TriMesh { &self, point: &Point, solid: bool, - options: &dyn QueryOptions, + _options: &dyn QueryOptions, ) -> PointProjection { - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .project_local_point(point, solid, options) + .project_local_point(point, solid, &QueryOptionsNotUsed) .1 } @@ -249,38 +246,37 @@ impl PointQuery for TriMesh { fn project_local_point_and_get_feature( &self, point: &Point, - options: &dyn QueryOptions, + _options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { #[cfg(feature = "dim3")] if self.pseudo_normals().is_some() { // If we can, in 3D, take the pseudo-normals into account. let (proj, (id, _feature)) = - self.project_local_point_and_get_location(point, false, options); + self.project_local_point_and_get_location(point, false, &QueryOptionsNotUsed); let feature_id = FeatureId::Face(id); return (proj, feature_id); } - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); let solid = cfg!(feature = "dim2"); - let (tri_id, proj) = CompositeShapeRef(self).project_local_point(point, solid, options); + let (tri_id, proj) = + CompositeShapeRef(self).project_local_point(point, solid, &QueryOptionsNotUsed); (proj, FeatureId::Face(tri_id)) } // TODO: implement distance_to_point too? #[inline] - fn contains_local_point(&self, point: &Point, options: &dyn QueryOptions) -> bool { + fn contains_local_point(&self, point: &Point, _options: &dyn QueryOptions) -> bool { #[cfg(feature = "dim3")] if self.pseudo_normals.is_some() { // If we can, in 3D, take the pseudo-normals into account. return self - .project_local_point_and_get_location(point, true, options) + .project_local_point_and_get_location(point, true, &QueryOptionsNotUsed) .0 .is_inside; } - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .contains_local_point(point, options) + .contains_local_point(point, &QueryOptionsNotUsed) .is_some() } @@ -344,11 +340,10 @@ impl PointQueryWithLocation for Polyline { &self, point: &Point, solid: bool, - options: &dyn QueryOptions, + _options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); let (seg_id, (proj, loc)) = CompositeShapeRef(self) - .project_local_point_and_get_location(point, Real::MAX, solid, options) + .project_local_point_and_get_location(point, Real::MAX, solid, &QueryOptionsNotUsed) .unwrap(); (proj, (seg_id, loc)) } @@ -375,12 +370,11 @@ impl PointQueryWithLocation for TriMesh { point: &Point, solid: bool, max_dist: Real, - options: &dyn QueryOptions, + _options: &dyn QueryOptions, ) -> Option<(PointProjection, Self::Location)> { - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); #[allow(unused_mut)] // mut is needed in 3D. if let Some((part_id, (mut proj, location))) = CompositeShapeRef(self) - .project_local_point_and_get_location(point, max_dist, solid, options) + .project_local_point_and_get_location(point, max_dist, solid, &QueryOptionsNotUsed) { #[cfg(feature = "dim3")] if let Some(pseudo_normals) = self.pseudo_normals_if_oriented() { diff --git a/src/query/point/point_composite_shape/query_options_dispatcher.rs b/src/query/point/point_composite_shape/query_options_dispatcher.rs index 59f9732d..f72353d0 100644 --- a/src/query/point/point_composite_shape/query_options_dispatcher.rs +++ b/src/query/point/point_composite_shape/query_options_dispatcher.rs @@ -2,7 +2,7 @@ use alloc::boxed::Box; use core::any::{Any, TypeId}; use crate::query::gjk::GjkOptions; -use crate::query::QueryOptions; +use crate::query::{DefaultQueryOptions, QueryOptions, QueryOptionsNotUsed}; use crate::shape::Compound; use hashbrown::HashMap; @@ -14,7 +14,22 @@ pub trait QueryOptionsDispatcher { impl QueryOptionsDispatcher for () { fn get_option_for_shape(&self, _shape_type_id: &TypeId) -> &dyn QueryOptions { - &() + self + } +} +impl QueryOptionsDispatcher for DefaultQueryOptions { + fn get_option_for_shape(&self, _shape_type_id: &TypeId) -> &dyn QueryOptions { + self + } +} +impl QueryOptionsDispatcher for QueryOptionsNotUsed { + fn get_option_for_shape(&self, _shape_type_id: &TypeId) -> &dyn QueryOptions { + self + } +} +impl QueryOptionsDispatcher for GjkOptions { + fn get_option_for_shape(&self, _shape_type_id: &TypeId) -> &dyn QueryOptions { + self } } diff --git a/src/query/query_options.rs b/src/query/query_options.rs index 1353155b..fa65af54 100644 --- a/src/query/query_options.rs +++ b/src/query/query_options.rs @@ -9,18 +9,32 @@ use core::any::Any; /// - [QueryOptionsDispatcher][crate::query::point::QueryOptionsDispatcher] /// - [QueryOptionsDispatcherMap][crate::query::point::QueryOptionsDispatcherMap] pub trait QueryOptions { + /// Downcast to [Any] to be compatible with [PointQuery][crate::query::PointQuery] or [RayCast][crate::query::RayCast] query options. fn as_any(&self) -> &dyn Any; + /// Downcast to [Any] mutably. fn as_any_mut(&mut self) -> &mut dyn Any; } -/// Type alias used to communicate that an option is not used downstream. +/// Unit type used to communicate that an option is not used downstream. /// /// Pass this to a method awaiting `&dyn QueryOptions` that we know doesn't use any algorithm options, /// so the flow is easier to understand. /// /// Its presence should be challenged any time a new algorithm is modified or a new option is introduced. -pub use DefaultQueryOptions as QueryOptionsNotUsed; +pub struct QueryOptionsNotUsed; +impl QueryOptions for QueryOptionsNotUsed { + fn as_any(&self) -> &dyn Any { + self + } + fn as_any_mut(&mut self) -> &mut dyn Any { + self + } +} +/// Unit type to help with passing default options, it can be challenging to know which [QueryOptions] +/// implementation should be passed to a function so this is an easy default. +/// +/// Note that you can also pass `&()` in such cases. pub struct DefaultQueryOptions; impl QueryOptions for DefaultQueryOptions { @@ -31,6 +45,7 @@ impl QueryOptions for DefaultQueryOptions { self } } + impl QueryOptions for () { fn as_any(&self) -> &dyn Any { self diff --git a/src/query/ray/ray_trimesh.rs b/src/query/ray/ray_trimesh.rs index a1f15df6..67dead6c 100644 --- a/src/query/ray/ray_trimesh.rs +++ b/src/query/ray/ray_trimesh.rs @@ -1,5 +1,5 @@ use crate::math::Real; -use crate::query::point::QueryOptionsDispatcherMap; +use crate::query::gjk::GjkOptions; use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{CompositeShapeRef, FeatureId, TriMesh}; @@ -15,8 +15,12 @@ impl RayCast for TriMesh { solid: bool, options: &dyn QueryOptions, ) -> Option { - // FIXME: Polyline is made of Triangles, which casts only require GjkOptions. So ideally we shouldn't need a full dispatcher. - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!("Incorrect option passed to cast_local_ray: using default options."); + &GjkOptions::default() + }; CompositeShapeRef(self) .cast_local_ray(ray, max_time_of_impact, solid, options) .map(|hit| hit.1) @@ -30,8 +34,14 @@ impl RayCast for TriMesh { solid: bool, options: &dyn QueryOptions, ) -> Option { - // FIXME: Polyline is made of Triangles, which casts only require GjkOptions. So ideally we shouldn't need a full dispatcher. - let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + log::warn!( + "Incorrect option passed to cast_local_ray_and_get_normal: using default options." + ); + &GjkOptions::default() + }; CompositeShapeRef(self) .cast_local_ray_and_get_normal(ray, max_time_of_impact, solid, options) .map(|(best, mut res)| { @@ -53,6 +63,7 @@ mod ray_cast_with_culling { use crate::math::{Isometry, Real, Vector}; use crate::partitioning::Bvh; use crate::query::details::NormalConstraints; + use crate::query::gjk::GjkOptions; use crate::query::{Ray, RayIntersection}; use crate::shape::{ CompositeShape, CompositeShapeRef, FeatureId, Shape, TriMesh, Triangle, TypedCompositeShape, @@ -154,9 +165,10 @@ mod ray_cast_with_culling { ray: &Ray, max_time_of_impact: Real, culling: RayCullingMode, + gjk_options: &GjkOptions, ) -> Option { let ls_ray = ray.inverse_transform_by(m); - self.cast_local_ray_with_culling(&ls_ray, max_time_of_impact, culling) + self.cast_local_ray_with_culling(&ls_ray, max_time_of_impact, culling, gjk_options) .map(|inter| inter.transform_by(m)) } @@ -169,6 +181,7 @@ mod ray_cast_with_culling { ray: &Ray, max_time_of_impact: Real, culling: RayCullingMode, + gjk_options: &GjkOptions, ) -> Option { let mesh_with_culling = TriMeshWithCulling { trimesh: self, @@ -176,7 +189,7 @@ mod ray_cast_with_culling { ray, }; CompositeShapeRef(&mesh_with_culling) - .cast_local_ray_and_get_normal(ray, max_time_of_impact, false, &()) + .cast_local_ray_and_get_normal(ray, max_time_of_impact, false, gjk_options) .map(|(best, mut res)| { // We hit a backface. // NOTE: we need this for `TriMesh::is_backface` to work properly. @@ -192,6 +205,7 @@ mod ray_cast_with_culling { #[cfg(test)] mod test { + use super::GjkOptions; use crate::query::{Ray, RayCullingMode}; use crate::shape::TriMesh; use nalgebra::{Point3, Vector3}; @@ -208,18 +222,39 @@ mod ray_cast_with_culling { let ray_down = Ray::new(Point3::new(0.0, 0.0, 1.0), Vector3::new(0.0, 0.0, -1.0)); let mesh = TriMesh::new(vertices, indices).unwrap(); + let query_options = GjkOptions::default(); assert!(mesh - .cast_local_ray_with_culling(&ray_up, 1000.0, RayCullingMode::IgnoreFrontfaces) + .cast_local_ray_with_culling( + &ray_up, + 1000.0, + RayCullingMode::IgnoreFrontfaces, + &query_options + ) .is_some()); assert!(mesh - .cast_local_ray_with_culling(&ray_down, 1000.0, RayCullingMode::IgnoreFrontfaces) + .cast_local_ray_with_culling( + &ray_down, + 1000.0, + RayCullingMode::IgnoreFrontfaces, + &query_options, + ) .is_none()); assert!(mesh - .cast_local_ray_with_culling(&ray_up, 1000.0, RayCullingMode::IgnoreBackfaces) + .cast_local_ray_with_culling( + &ray_up, + 1000.0, + RayCullingMode::IgnoreBackfaces, + &query_options, + ) .is_none()); assert!(mesh - .cast_local_ray_with_culling(&ray_down, 1000.0, RayCullingMode::IgnoreBackfaces) + .cast_local_ray_with_culling( + &ray_down, + 1000.0, + RayCullingMode::IgnoreBackfaces, + &query_options + ) .is_some()); } } diff --git a/src/query/split/split_trimesh.rs b/src/query/split/split_trimesh.rs index 273a4815..f392d64a 100644 --- a/src/query/split/split_trimesh.rs +++ b/src/query/split/split_trimesh.rs @@ -1,6 +1,6 @@ use crate::bounding_volume::Aabb; use crate::math::{Isometry, Point, Real, UnitVector, Vector}; -use crate::query::{IntersectResult, PointQuery, SplitResult}; +use crate::query::{IntersectResult, PointQuery, QueryOptionsNotUsed, SplitResult}; use crate::shape::{Cuboid, FeatureId, Polyline, Segment, Shape, TriMesh, TriMeshFlags, Triangle}; use crate::transformation::{intersect_meshes, MeshIntersectionError}; use crate::utils::{hashmap::HashMap, SortedPair, WBasis}; @@ -322,7 +322,7 @@ impl TriMesh { vertices_lhs[idx1[2] as usize], ); - if self.contains_local_point(&tri.center(), &()) { + if self.contains_local_point(&tri.center(), &QueryOptionsNotUsed) { indices_lhs.push(idx1); idx2.swap(1, 2); // Flip orientation for the second half of the split. diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index e18ea0b0..1a9458e8 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -1,7 +1,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Isometry, Point, Real, Vector}; use crate::partitioning::{Bvh, BvhBuildStrategy}; -use crate::query::{PointProjection, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQueryWithLocation, QueryOptionsNotUsed}; use crate::shape::composite_shape::CompositeShape; use crate::shape::{FeatureId, Segment, SegmentPointLocation, Shape, TypedCompositeShape}; #[cfg(feature = "alloc")] @@ -230,7 +230,8 @@ impl Polyline { point: Point, #[cfg(feature = "dim3")] axis: u8, ) -> (PointProjection, (u32, SegmentPointLocation)) { - let mut proj = self.project_local_point_and_get_location(&point, false, &()); + let mut proj = + self.project_local_point_and_get_location(&point, false, &QueryOptionsNotUsed); let segment1 = self.segment((proj.1).0); #[cfg(feature = "dim2")] diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index 62b3d5e5..c2f6d193 100644 --- a/src/transformation/mesh_intersection/mesh_intersection.rs +++ b/src/transformation/mesh_intersection/mesh_intersection.rs @@ -3,7 +3,7 @@ use crate::bounding_volume::BoundingVolume; use crate::math::{Isometry, Real}; use crate::partitioning::BvhNode; use crate::query::point::point_query::PointQueryWithLocation; -use crate::query::PointQuery; +use crate::query::{PointQuery, QueryOptionsNotUsed}; use crate::shape::{TriMesh, Triangle}; use crate::utils; use crate::utils::hashmap::Entry; @@ -328,7 +328,7 @@ fn extract_connected_components( if flip2 ^ mesh2.contains_local_point( &pos12.inverse_transform_point(&tri1.center()), - &(), + &QueryOptionsNotUsed, ) { to_visit.push(twin.face); @@ -375,7 +375,11 @@ fn extract_connected_components( let repr_pt = mesh1.triangle(repr_face).center(); let indices = mesh1.indices(); - if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt), &()) + if flip2 + ^ mesh2.contains_local_point( + &pos12.inverse_transform_point(&repr_pt), + &QueryOptionsNotUsed, + ) { new_indices1.extend( cc.grouped_faces[range[0]..range[1]] @@ -389,7 +393,12 @@ fn extract_connected_components( // Deal with the case where there is no intersection between the meshes. let repr_pt = mesh1.triangle(0).center(); - if flip2 ^ mesh2.contains_local_point(&pos12.inverse_transform_point(&repr_pt), &()) { + if flip2 + ^ mesh2.contains_local_point( + &pos12.inverse_transform_point(&repr_pt), + &QueryOptionsNotUsed, + ) + { new_indices1.extend_from_slice(mesh1.indices()); } } @@ -676,7 +685,7 @@ fn merge_triangle_sets( .project_local_point_and_get_location( &pos2.inverse_transform_point(¢er), true, - &(), + &QueryOptionsNotUsed, ) .0;