diff --git a/CHANGELOG.md b/CHANGELOG.md index f26b7635..a27c073c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,11 @@ -## 0.22.0-beta.1 +## 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 diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 664398f2..a9d9367a 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -97,6 +97,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"] @@ -146,6 +147,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" @@ -161,6 +167,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 new file mode 100644 index 00000000..dadcacc5 --- /dev/null +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -0,0 +1,203 @@ +mod common_macroquad2d; + +use common_macroquad2d::draw_point; +use macroquad::prelude::*; +use nalgebra::{Isometry2, Point2}; +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.. { + clear_background(BLACK); + + let screen_shift = Point2::new(screen_width() / 2.0, screen_height() / 2.0); + + let to_cast_against = ConvexPolygon::from_convex_polyline( + [ + [-24.0, 0.0].into(), + [0.0, 24.0].into(), + [24.0, 0.0].into(), + [0.0, -24.0].into(), + ] + .into(), + ) + .expect("Failed to create ConvexPolygon from polyline"); + let to_cast_against_pose = Isometry2::rotation(0f32); + + let mouse_pos = mouse_position(); + let mouse_position_world = + (Point2::::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( + screen_shift, + [47.0, -32.0].into(), + target_pos, + to_cast_against.clone(), + epsilon_tolerance, + ); + + // For debug purposes, raycast to mouse position. + // Rendered last to be on top of the other raycasts + shape_cast_debug( + screen_shift, + target_pos, + mouse_position_world, + to_cast_against.clone(), + epsilon_tolerance, + ); + + /* + * + * Render the cuboid. + * + */ + draw_polygon( + &to_cast_against.points(), + &to_cast_against_pose, + RENDER_SCALE, + screen_shift, + GREEN, + ); + easy_draw_text(&format!("tolerance: {:.7}", epsilon_tolerance)); + + next_frame().await + } +} + +fn shape_cast_debug( + screen_shift: Point2, + source_pos: Point2, + target_pos: Point2, + to_cast_against: ConvexPolygon, + epsilon_tolerance: f32, +) { + /* + * + * 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_with_dispatcher( + &pos1.into(), + &vel1, + &g1, + &pos2.into(), + &vel2.into(), + &*g2, + ShapeCastOptions::with_max_time_of_impact(1.0), + DefaultQueryDispatcher { + gjk_options: query::gjk::GjkOptions { + epsilon_tolerance, + nb_max_iterations: 100, + }, + }, + ) + .unwrap(); + + /* + * + * Render the raycast's result. + * + */ + drawcircle_at(pos1, BALLCAST_WIDTH, RENDER_SCALE, screen_shift, ORANGE); + + if let Some(toi) = toi { + if toi.time_of_impact == 0f32 { + draw_point(ray.origin, RENDER_SCALE, screen_shift, YELLOW); + } else { + drawline_from_to( + ray.origin, + (ray.point_at(toi.time_of_impact).coords).into(), + RENDER_SCALE, + screen_shift, + GREEN, + ); + } + drawcircle_at( + (ray.point_at(toi.time_of_impact).coords).into(), + BALLCAST_WIDTH, + RENDER_SCALE, + screen_shift, + GREEN, + ); + } else { + drawline_from_to( + ray.origin, + ray.origin + ray.dir, + RENDER_SCALE, + screen_shift, + RED, + ); + } +} + +fn draw_polygon( + polygon: &[Point2], + 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); +} diff --git a/crates/parry2d/examples/point_query2d.rs b/crates/parry2d/examples/point_query2d.rs new file mode 100644 index 00000000..b0273d10 --- /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}; +use parry2d::query::gjk::GjkOptions; +use parry2d::query::point::QueryOptionsDispatcherMap; +use parry2d::query::QueryOptions; +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")] +async fn main() { + env_logger::init(); + 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 polygon_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0); + + // Create an initial dispatcher + let mut query_options_dispatcher = QueryOptionsDispatcherMap::default(); + + for i in 0.. { + clear_background(BLACK); + + let gjk_options = query_options_dispatcher + .get_option_mut::() + .unwrap(); + + // loops from default epsilon to an arbitrarily chosen slightly higher value. + 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) + } 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), 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.epsilon_tolerance)); + + 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 = 50; + let spacing = 0.4; + 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/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/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_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/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/epa2.rs b/crates/parry2d/tests/geometry/epa2.rs index 47727488..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()); } @@ -28,7 +31,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/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/geometry/ray_cast.rs b/crates/parry2d/tests/geometry/ray_cast.rs index 41fdbd97..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, + &(), ) }; @@ -170,8 +171,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/crates/parry2d/tests/query/point_composite_shape.rs b/crates/parry2d/tests/query/point_composite_shape.rs index dd113714..c41fd0ab 100644 --- a/crates/parry2d/tests/query/point_composite_shape.rs +++ b/crates/parry2d/tests/query/point_composite_shape.rs @@ -11,14 +11,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 +36,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 +55,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/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/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/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/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/crates/parry3d/tests/geometry/cuboid_ray_cast.rs b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs index 7f8f9e43..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 @@ -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(), @@ -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/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/bounding_volume/aabb.rs b/src/bounding_volume/aabb.rs index 2770f97c..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_queries.rs b/src/partitioning/bvh/bvh_queries.rs index 754cf0e3..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..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/closest_points/closest_points_ball_convex_polyhedron.rs b/src/query/closest_points/closest_points_ball_convex_polyhedron.rs index 49f59280..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,5 +1,5 @@ use crate::math::{Isometry, Real}; -use crate::query::ClosestPoints; +use crate::query::{ClosestPoints, QueryOptions}; use crate::shape::{Ball, Shape}; /// ClosestPoints between a ball and a convex polyhedron. @@ -12,8 +12,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 +38,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..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 0ac6449d..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/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/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_ball_convex_polyhedron.rs b/src/query/contact/contact_ball_convex_polyhedron.rs index 15786941..23a841fc 100644 --- a/src/query/contact/contact_ball_convex_polyhedron.rs +++ b/src/query/contact/contact_ball_convex_polyhedron.rs @@ -1,5 +1,5 @@ use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::Contact; +use crate::query::{Contact, QueryOptions}; use crate::shape::{Ball, Shape}; use na::{self, Unit}; @@ -14,8 +14,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 +30,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..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/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/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_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 3fcbb772..549aab61 100644 --- a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs @@ -1,6 +1,6 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; -use crate::query::{ContactManifold, Ray, TrackedContact}; +use crate::query::{ContactManifold, QueryOptions, Ray, TrackedContact}; use crate::shape::{Ball, PackedFeatureId, Shape}; use na::Unit; @@ -13,6 +13,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 +27,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 +39,7 @@ pub fn contact_manifold_convex_ball_shapes( prediction, manifold, false, + options, ); } } @@ -51,12 +54,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; @@ -101,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/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/contact_manifolds/contact_manifolds_voxels_ball.rs b/src/query/contact_manifolds/contact_manifolds_voxels_ball.rs index e9b1f91a..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 9839be20..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 9532b5df..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 fd603372..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,9 +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 904d9ab2..5fa4f876 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::GjkOptions; use crate::query::{ self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, Unsupported, @@ -15,8 +16,11 @@ use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; use alloc::vec::Vec; /// A dispatcher that exposes built-in queries -#[derive(Debug, Clone)] -pub struct DefaultQueryDispatcher; +#[derive(Debug, Clone, Default)] +pub struct DefaultQueryDispatcher { + /// Options for the GJK algorithm. + pub gjk_options: GjkOptions, +} impl QueryDispatcher for DefaultQueryDispatcher { fn intersection_test( @@ -42,11 +46,23 @@ 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 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() { Ok(query::details::intersection_test_point_query_ball( - pos12, shape1, b2, + pos12, + shape1, + b2, + // 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)) = (shape1.as_shape::(), shape2.as_support_map()) @@ -62,7 +78,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 = "alloc")] @@ -105,11 +124,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)) @@ -129,7 +154,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 = "alloc")] @@ -177,17 +205,29 @@ 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 = "alloc")] 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( @@ -219,11 +259,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::()) @@ -261,7 +309,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 = "alloc")] @@ -343,6 +395,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { s1, s2, options, + &self.gjk_options, )); } else if let Some(c1) = shape1.as_composite_shape() { return Ok(query::details::cast_shapes_composite_shape_shape( @@ -624,7 +677,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), @@ -667,7 +720,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.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/distance/distance_ball_convex_polyhedron.rs b/src/query/distance/distance_ball_convex_polyhedron.rs index 3b1f0f31..5817d8c4 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::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/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 60c8f693..e93373b3 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_epsilon_tolerance: Real) -> Option { + if neg_dist > gjk_epsilon_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 51f22a80..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) -> Option { - if neg_dist > gjk::eps_tol() { + 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,7 +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(_, _) => { @@ -276,22 +281,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) { @@ -325,8 +350,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; @@ -413,7 +448,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 9c9efe7f..bb373ad1 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -6,7 +6,7 @@ use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; 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}; @@ -32,10 +32,38 @@ 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 epsilon_tolerance: Real, + /// The maximum number of iterations of the GJK algorithm. + pub nb_max_iterations: u32, +} + +impl Default for GjkOptions { + fn default() -> Self { + Self { + epsilon_tolerance: eps_tol(), + nb_max_iterations: 100, + } + } +} + +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. pub fn eps_tol() -> Real { - let _eps = crate::math::DEFAULT_EPSILON; - _eps * 10.0 + crate::math::DEFAULT_EPSILON * 10.0 } /// Projects the origin on the boundary of the given shape. @@ -50,6 +78,7 @@ pub fn project_origin( m: &Isometry, g: &G, simplex: &mut VoronoiSimplex, + gjk_options: &GjkOptions, ) -> Option> { match closest_points( &m.inverse(), @@ -58,6 +87,7 @@ pub fn project_origin( Real::max_value(), true, simplex, + gjk_options, ) { GJKResult::Intersection => None, GJKResult::ClosestPoints(p, _, _) => Some(p), @@ -87,13 +117,13 @@ 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.epsilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); // TODO: reset the simplex if it is empty? @@ -149,7 +179,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 +206,7 @@ where } niter += 1; - if niter == 100 { + if niter == gjk_options.nb_max_iterations { return GJKResult::NoIntersection(Vector::x_axis()); } } @@ -188,6 +218,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,6 +228,7 @@ pub fn cast_local_ray( ray, max_time_of_impact, simplex, + gjk_options, ) } @@ -210,13 +242,14 @@ pub fn directional_distance( g2: &G2, dir: &Vector, simplex: &mut VoronoiSimplex, + 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).map( + 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) @@ -239,13 +272,14 @@ fn minkowski_ray_cast( ray: &Ray, max_time_of_impact: Real, simplex: &mut VoronoiSimplex, + gjk_options: &GjkOptions, ) -> 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_options.epsilon_tolerance; let _eps_rel: Real = ComplexField::sqrt(_eps_tol); let ray_length = ray.dir.norm(); @@ -351,7 +385,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 { @@ -363,7 +397,7 @@ where } niter += 1; - if niter == 100 { + if niter == gjk_options.nb_max_iterations { return None; } } @@ -391,3 +425,68 @@ 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, gjk::gjk::GjkOptions, DefaultQueryDispatcher, 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 dispatcher = DefaultQueryDispatcher { + gjk_options: GjkOptions { + epsilon_tolerance: crate::math::DEFAULT_EPSILON * 10000.0, + ..GjkOptions::default() + }, + }; + let toi = query::cast_shapes_with_dispatcher( + &source_pos.into(), + &vel1, + &g1, + &pos2.into(), + &vel2.into(), + &*g2, + ShapeCastOptions::with_max_time_of_impact(1.0), + dispatcher, + ) + .unwrap(); + assert!( + toi.is_some(), + "casting against the convex polygon should converge." + ); + } +} diff --git a/src/query/gjk/voronoi_simplex2.rs b/src/query/gjk/voronoi_simplex2.rs index e69d230a..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::{self, CSOPoint}; -use crate::query::{PointQuery, PointQueryWithLocation}; +use crate::query::gjk::CSOPoint; +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. @@ -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; } } @@ -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/gjk/voronoi_simplex3.rs b/src/query/gjk/voronoi_simplex3.rs index 6d79d352..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::{self, CSOPoint}; +use crate::query::gjk::CSOPoint; +use crate::query::query_options::QueryOptionsNotUsed; use crate::query::{PointQuery, PointQueryWithLocation}; use crate::shape::{ Segment, SegmentPointLocation, Tetrahedron, TetrahedronPointLocation, Triangle, @@ -54,14 +55,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 +70,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 +80,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; } } @@ -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.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/intersection_test/intersection_test_ball_point_query.rs b/src/query/intersection_test/intersection_test_ball_point_query.rs index 8dc68b7b..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/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/mod.rs b/src/query/mod.rs index 173039e9..ba734ddc 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -41,8 +41,11 @@ 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}; +pub use query_options::{DefaultQueryOptions, QueryOptions, QueryOptionsNotUsed}; #[cfg(all(feature = "dim3", feature = "alloc"))] pub use self::ray::RayCullingMode; @@ -62,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/nonlinear_shape_cast/nonlinear_shape_cast.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs index 6c9aaa9f..b6cd2b91 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/point/mod.rs b/src/query/point/mod.rs index ebacca00..8f739cd0 100644 --- a/src/query/point/mod.rs +++ b/src/query/point/mod.rs @@ -3,7 +3,12 @@ #[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::query_options_dispatcher::{ + QueryOptionsDispatcher, QueryOptionsDispatcherMap, + }, + point_support_map::local_point_projection_on_support_map, +}; mod point_aabb; mod point_ball; diff --git a/src/query/point/point_aabb.rs b/src/query/point/point_aabb.rs index 52a3ede3..41c157fb 100644 --- a/src/query/point/point_aabb.rs +++ b/src/query/point/point_aabb.rs @@ -1,7 +1,7 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real, Vector, DIM}; use crate::num::{Bounded, Zero}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::FeatureId; use na; @@ -59,7 +59,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 +75,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 +138,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 +152,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..2ea2bbfe 100644 --- a/src/query/point/point_ball.rs +++ b/src/query/point/point_ball.rs @@ -1,12 +1,17 @@ use na::{self, ComplexField}; use crate::math::{Point, Real}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; 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 +29,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 +54,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..bce93fff 100644 --- a/src/query/point/point_bounding_sphere.rs +++ b/src/query/point/point_bounding_sphere.rs @@ -1,13 +1,18 @@ use crate::bounding_volume::BoundingSphere; use crate::math::{Point, Real}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; 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 +22,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..d7a23fa9 100644 --- a/src/query/point/point_capsule.rs +++ b/src/query/point/point_capsule.rs @@ -1,14 +1,19 @@ use crate::approx::AbsDiffEq; use crate::math::{Point, Real, Vector}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; 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 +50,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 cbcb1457..5f467cf1 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -1,8 +1,15 @@ -#![allow(unused_parens)] // Needed by the macro. +pub mod query_options_dispatcher; + +use core::any::Any; use crate::math::{Point, Real}; use crate::partitioning::BvhNode; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::point::point_composite_shape::query_options_dispatcher::{ + QueryOptionsDispatcher, QueryOptionsDispatcherMap, +}; +use crate::query::{ + PointProjection, PointQuery, PointQueryWithLocation, QueryOptions, QueryOptionsNotUsed, +}; use crate::shape::{ CompositeShapeRef, FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, TypedCompositeShape, @@ -22,6 +29,7 @@ impl CompositeShapeRef<'_, S> { point: &Point, max_dist: Real, solid: bool, + options: &dyn QueryOptionsDispatcher, ) -> Option<( u32, ( @@ -36,13 +44,25 @@ impl CompositeShapeRef<'_, S> { .bvh() .find_best( max_dist, - |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true), + |node: &BvhNode, _best_so_far| { + node.aabb() + .distance_to_local_point(point, true, &QueryOptionsNotUsed) + }, |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_location(pose, point, solid) + shape.project_point_and_get_location( + pose, + point, + solid, + options.get_option_for_shape(&shape.type_id()), + ) } else { - shape.project_local_point_and_get_location(point, solid) + shape.project_local_point_and_get_location( + point, + solid, + options.get_option_for_shape(&shape.type_id()), + ) } })?; let cost = na::distance(&proj.0.point, point); @@ -57,19 +77,36 @@ impl CompositeShapeRef<'_, S> { /// Returns the projected point as well as the index of the sub-shape of `self` that was hit. /// If `solid` is `false` then the point will be projected to the closest boundary of `self` even /// if it is contained by one of its sub-shapes. - pub fn project_local_point(&self, point: &Point, solid: bool) -> (u32, PointProjection) { + pub fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptionsDispatcher, + ) -> (u32, PointProjection) { let (best_id, (_, proj)) = self .0 .bvh() .find_best( Real::MAX, - |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true), + |node: &BvhNode, _best_so_far| { + node.aabb() + .distance_to_local_point(point, true, &QueryOptionsNotUsed) + }, |primitive, _best_so_far| { let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| { if let Some(pose) = pose { - shape.project_point(pose, point, solid) + shape.project_point( + pose, + point, + solid, + options.get_option_for_shape(&shape.type_id()), + ) } else { - shape.project_local_point(point, solid) + shape.project_local_point( + point, + solid, + options.get_option_for_shape(&shape.type_id()), + ) } })?; let dist = na::distance(&proj.point, point); @@ -89,19 +126,30 @@ impl CompositeShapeRef<'_, S> { pub fn project_local_point_and_get_feature( &self, point: &Point, + options: &dyn QueryOptionsDispatcher, ) -> (u32, (PointProjection, FeatureId)) { let (best_id, (_, (proj, feature_id))) = self .0 .bvh() .find_best( Real::MAX, - |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true), + |node: &BvhNode, _best_so_far| { + node.aabb() + .distance_to_local_point(point, true, &QueryOptionsNotUsed) + }, |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); @@ -116,7 +164,11 @@ impl CompositeShapeRef<'_, S> { /// Returns the index of any sub-shape of `self` that contains the given point. #[inline] - pub fn contains_local_point(&self, point: &Point) -> Option { + pub fn contains_local_point( + &self, + point: &Point, + options: &dyn QueryOptionsDispatcher, + ) -> Option { self.0 .bvh() .leaves(|node: &BvhNode| node.aabb().contains_local_point(point)) @@ -124,9 +176,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) @@ -136,17 +195,25 @@ impl CompositeShapeRef<'_, S> { impl PointQuery for Polyline { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - CompositeShapeRef(self).project_local_point(point, solid).1 + fn project_local_point( + &self, + point: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> PointProjection { + CompositeShapeRef(self) + .project_local_point(point, solid, &QueryOptionsNotUsed) + .1 } #[inline] fn project_local_point_and_get_feature( &self, point: &Point, + _options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { - let (seg_id, (proj, feature)) = - CompositeShapeRef(self).project_local_point_and_get_feature(point); + 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) } @@ -154,52 +221,62 @@ 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 options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .contains_local_point(point) + .contains_local_point(point, options) .is_some() } } impl PointQuery for TriMesh { #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - CompositeShapeRef(self).project_local_point(point, solid).1 + fn project_local_point( + &self, + point: &Point, + solid: bool, + _options: &dyn QueryOptions, + ) -> PointProjection { + CompositeShapeRef(self) + .project_local_point(point, solid, &QueryOptionsNotUsed) + .1 } #[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, &QueryOptionsNotUsed); let feature_id = FeatureId::Face(id); return (proj, feature_id); } - let solid = cfg!(feature = "dim2"); - let (tri_id, proj) = CompositeShapeRef(self).project_local_point(point, solid); + 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) -> 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, &QueryOptionsNotUsed) .0 .is_inside; } CompositeShapeRef(self) - .contains_local_point(point) + .contains_local_point(point, &QueryOptionsNotUsed) .is_some() } @@ -209,26 +286,37 @@ 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 { - CompositeShapeRef(self).project_local_point(point, solid).1 + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); + CompositeShapeRef(self) + .project_local_point(point, solid, options) + .1 } #[inline] fn project_local_point_and_get_feature( &self, point: &Point, + options: &dyn QueryOptions, ) -> (PointProjection, FeatureId) { + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); ( CompositeShapeRef(self) - .project_local_point_and_get_feature(point) + .project_local_point_and_get_feature(point, options) .1 .0, FeatureId::Unknown, @@ -236,9 +324,10 @@ impl PointQuery for Compound { } #[inline] - fn contains_local_point(&self, point: &Point) -> bool { + fn contains_local_point(&self, point: &Point, options: &dyn QueryOptions) -> bool { + let options = QueryOptionsDispatcherMap::from_dyn_or_default(options); CompositeShapeRef(self) - .contains_local_point(point) + .contains_local_point(point, options) .is_some() } } @@ -251,9 +340,10 @@ impl PointQueryWithLocation for Polyline { &self, point: &Point, solid: bool, + _options: &dyn QueryOptions, ) -> (PointProjection, Self::Location) { let (seg_id, (proj, loc)) = CompositeShapeRef(self) - .project_local_point_and_get_location(point, Real::MAX, solid) + .project_local_point_and_get_location(point, Real::MAX, solid, &QueryOptionsNotUsed) .unwrap(); (proj, (seg_id, loc)) } @@ -268,8 +358,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() } @@ -279,10 +370,11 @@ impl PointQueryWithLocation for TriMesh { point: &Point, solid: bool, max_dist: Real, + _options: &dyn QueryOptions, ) -> Option<(PointProjection, Self::Location)> { #[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) + if let Some((part_id, (mut proj, location))) = CompositeShapeRef(self) + .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 new file mode 100644 index 00000000..f72353d0 --- /dev/null +++ b/src/query/point/point_composite_shape/query_options_dispatcher.rs @@ -0,0 +1,123 @@ +use alloc::boxed::Box; +use core::any::{Any, TypeId}; + +use crate::query::gjk::GjkOptions; +use crate::query::{DefaultQueryOptions, QueryOptions, QueryOptionsNotUsed}; +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 { + 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 + } +} + +/// 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::() + } + + /// 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 { + 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_cone.rs b/src/query/point/point_cone.rs index 8570daa0..217c9fda 100644 --- a/src/query/point/point_cone.rs +++ b/src/query/point/point_cone.rs @@ -1,11 +1,16 @@ use crate::math::{Point, Real, Vector}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; 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 +35,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 +70,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..cd846528 100644 --- a/src/query/point/point_cuboid.rs +++ b/src/query/point/point_cuboid.rs @@ -1,35 +1,46 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; 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..3bce04b4 100644 --- a/src/query/point/point_cylinder.rs +++ b/src/query/point/point_cylinder.rs @@ -1,11 +1,16 @@ use crate::math::{Point, Real}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; 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 +79,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..87c8f2e1 100644 --- a/src/query/point/point_halfspace.rs +++ b/src/query/point/point_halfspace.rs @@ -1,10 +1,15 @@ use crate::math::{Point, Real}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; 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 +24,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 +50,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..08eaba99 100644 --- a/src/query/point/point_heightfield.rs +++ b/src/query/point/point_heightfield.rs @@ -1,6 +1,6 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real, Vector}; -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. @@ -11,13 +11,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 +34,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 +48,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 +64,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 +89,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 793d70ae..defcbfbd 100644 --- a/src/query/point/point_query.rs +++ b/src/query/point/point_query.rs @@ -1,4 +1,5 @@ use crate::math::{Isometry, Point, Real}; +use crate::query::QueryOptions; use crate::shape::FeatureId; use na; @@ -50,8 +51,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 +68,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 +115,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 +149,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 +195,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 +204,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 +220,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 +237,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 ed01ea2b..42bc6276 100644 --- a/src/query/point/point_round_shape.rs +++ b/src/query/point/point_round_shape.rs @@ -1,32 +1,52 @@ +use log::warn; + use crate::math::{Point, Real}; use crate::query::gjk::VoronoiSimplex; -use crate::query::{PointProjection, PointQuery}; +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 // 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 = "alloc"))] return unimplemented!( "The projection of points on a round shape isn't supported without alloc yet." ); - #[cfg(feature = "alloc")] - return crate::query::details::local_point_projection_on_support_map( - self, - &mut VoronoiSimplex::new(), - point, - solid, - ); + #[cfg(feature = "alloc")] // TODO: can’t be used without alloc because of EPA + { + let options = if let Some(options) = options.as_any().downcast_ref() { + options + } else { + warn!("Incorrect option passed to project_local_point: using default options."); + &crate::query::gjk::GjkOptions::default() + }; + crate::query::details::local_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + options, + ) + } } #[inline] 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..22dadcc4 100644 --- a/src/query/point/point_segment.rs +++ b/src/query/point/point_segment.rs @@ -1,19 +1,26 @@ use crate::math::{Point, Real}; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; 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 +57,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 871dbef0..122759c6 100644 --- a/src/query/point/point_support_map.rs +++ b/src/query/point/point_support_map.rs @@ -3,8 +3,8 @@ use na::Unit; use crate::math::{Isometry, Point, Real, Vector}; #[cfg(feature = "alloc")] use crate::query::epa::EPA; -use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::gjk::{self, CSOPoint, ConstantOrigin, GjkOptions, VoronoiSimplex}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; #[cfg(feature = "dim2")] #[cfg(feature = "alloc")] use crate::shape::ConvexPolygon; @@ -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) @@ -54,16 +55,34 @@ where #[cfg(feature = "dim3")] 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) + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + 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_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + options, + ) } #[inline] 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 }; @@ -79,16 +98,34 @@ impl PointQuery for ConvexPolyhedron { #[cfg(feature = "dim2")] 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) + fn project_local_point( + &self, + point: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { + 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_point_projection_on_support_map( + self, + &mut VoronoiSimplex::new(), + point, + solid, + options, + ) } #[inline] 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..861910d1 100644 --- a/src/query/point/point_tetrahedron.rs +++ b/src/query/point/point_tetrahedron.rs @@ -1,19 +1,26 @@ use crate::math::{Point, Real, Vector}; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; 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 +40,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..666d85d7 100644 --- a/src/query/point/point_triangle.rs +++ b/src/query/point/point_triangle.rs @@ -1,5 +1,5 @@ use crate::math::{Point, Real, Vector, DIM}; -use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; +use crate::query::{PointProjection, PointQuery, PointQueryWithLocation, QueryOptions}; use crate::shape::{FeatureId, Triangle, TrianglePointLocation}; #[inline] @@ -19,19 +19,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 +66,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/point/point_voxels.rs b/src/query/point/point_voxels.rs index 3fe39418..22c8205b 100644 --- a/src/query/point/point_voxels.rs +++ b/src/query/point/point_voxels.rs @@ -1,10 +1,15 @@ use crate::math::{Point, Real}; -use crate::query::{PointProjection, PointQuery}; +use crate::query::{PointProjection, PointQuery, QueryOptions}; use crate::shape::{Cuboid, FeatureId, VoxelType, Voxels}; impl PointQuery for Voxels { #[inline] - fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { + fn project_local_point( + &self, + pt: &Point, + solid: bool, + options: &dyn QueryOptions, + ) -> PointProjection { // TODO: optimize this very naive implementation. let base_cuboid = Cuboid::new(self.voxel_size() / 2.0); let mut smallest_dist = Real::MAX; @@ -13,7 +18,7 @@ impl PointQuery for Voxels { for vox in self.voxels() { if vox.state.voxel_type() != VoxelType::Empty { let mut candidate = - base_cuboid.project_local_point(&(pt - vox.center.coords), solid); + base_cuboid.project_local_point(&(pt - vox.center.coords), solid, options); candidate.point += vox.center.coords; let candidate_dist = (candidate.point - pt).norm(); @@ -31,8 +36,12 @@ impl PointQuery for Voxels { 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/query_options.rs b/src/query/query_options.rs new file mode 100644 index 00000000..fa65af54 --- /dev/null +++ b/src/query/query_options.rs @@ -0,0 +1,56 @@ +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 { + /// 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; +} + +/// 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 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 { + 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 b37d9cdd..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::VoronoiSimplex; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::gjk::{GjkOptions, VoronoiSimplex}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{RoundShape, SupportMap}; impl RayCast for RoundShape { @@ -9,13 +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, + options, ) } } diff --git a/src/query/ray/ray_support_map.rs b/src/query/ray/ray_support_map.rs index 5311e63c..f12c6fca 100644 --- a/src/query/ray/ray_support_map.rs +++ b/src/query/ray/ray_support_map.rs @@ -3,8 +3,8 @@ use na; use crate::math::Real; #[cfg(feature = "dim2")] use crate::query; -use crate::query::gjk::{self, CSOPoint, VoronoiSimplex}; -use crate::query::{Ray, RayCast, RayIntersection}; +use crate::query::gjk::{self, CSOPoint, GjkOptions, VoronoiSimplex}; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; #[cfg(all(feature = "alloc", feature = "dim2"))] use crate::shape::ConvexPolygon; #[cfg(all(feature = "alloc", feature = "dim3"))] @@ -22,11 +22,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)| { @@ -41,7 +42,7 @@ pub fn local_ray_intersection_with_support_map_with_params 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, + options, ) } } @@ -95,13 +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, + options, ) } } @@ -112,13 +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, + options, ) } } @@ -131,13 +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, + options, ) } } @@ -150,13 +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, + options, ) } } @@ -168,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; @@ -179,7 +227,7 @@ impl RayCast for Segment { &ray.dir, &self.a, &seg_dir, - crate::math::DEFAULT_EPSILON, + options.epsilon_tolerance, ); if parallel { @@ -189,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); @@ -254,6 +302,7 @@ impl RayCast for Segment { ray, max_time_of_impact, solid, + 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..67dead6c 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::gjk::GjkOptions; +use crate::query::{QueryOptions, Ray, RayCast, RayIntersection}; use crate::shape::{CompositeShapeRef, FeatureId, TriMesh}; #[cfg(feature = "dim3")] @@ -7,9 +8,21 @@ 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 { + 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) + .cast_local_ray(ray, max_time_of_impact, solid, options) .map(|hit| hit.1) } @@ -19,9 +32,18 @@ impl RayCast for TriMesh { 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 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) + .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. @@ -41,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, @@ -142,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)) } @@ -157,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, @@ -164,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. @@ -180,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}; @@ -196,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/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/mod.rs b/src/query/shape_cast/mod.rs index 4dad80a5..a5d7b70a 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 78afc17e..188b452f 100644 --- a/src/query/shape_cast/shape_cast.rs +++ b/src/query/shape_cast/shape_cast.rs @@ -160,5 +160,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_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/query/shape_cast/shape_cast_support_map_support_map.rs b/src/query/shape_cast/shape_cast_support_map_support_map.rs index 7df76da1..d26f742b 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 @@ -3,7 +3,7 @@ use na::Unit; use crate::math::{Isometry, Real, Vector}; use crate::query::details; use crate::query::details::ShapeCastOptions; -use crate::query::gjk::{self, VoronoiSimplex}; +use crate::query::gjk::{self, GjkOptions, VoronoiSimplex}; use crate::query::{ShapeCastHit, ShapeCastStatus}; use crate::shape::{RoundShapeRef, SupportMap}; use num::Zero; @@ -15,6 +15,7 @@ pub fn cast_shapes_support_map_support_map( g1: &G1, g2: &G2, options: ShapeCastOptions, + gjk_options: &GjkOptions, ) -> 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_options, + ) } else { - gjk::directional_distance(pos12, g1, g2, vel12, &mut VoronoiSimplex::new()) + gjk::directional_distance( + pos12, + g1, + g2, + vel12, + &mut VoronoiSimplex::new(), + gjk_options, + ) }; gjk_result.and_then(|(time_of_impact, normal1, witness1, witness2)| { @@ -36,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 { diff --git a/src/query/split/split_trimesh.rs b/src/query/split/split_trimesh.rs index 3adeb603..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 95315f29..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/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); } } diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index 3d68c0bf..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; @@ -326,7 +326,10 @@ 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()), + &QueryOptionsNotUsed, + ) { to_visit.push(twin.face); } @@ -372,7 +375,12 @@ 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]] .iter() @@ -385,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()); } } @@ -669,7 +682,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, + &QueryOptionsNotUsed, + ) .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 65ec91ad..134e5efd 100644 --- a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs +++ b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs @@ -238,9 +238,10 @@ fn debug_check_intersections( let proj = |vect: Vector| Point2::new(vect.dot(&basis[0]), vect.dot(&basis[1])); let mut incorrect = false; + 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; @@ -248,7 +249,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; diff --git a/src/transformation/polygon_intersection.rs b/src/transformation/polygon_intersection.rs index 997f53b9..0fa305ab 100644 --- a/src/transformation/polygon_intersection.rs +++ b/src/transformation/polygon_intersection.rs @@ -738,18 +738,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)); }