From 266bccb7b7b67c317af8bc93f712e8511648d20a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 5 May 2024 16:17:14 +0200 Subject: [PATCH] =?UTF-8?q?feat:=E2=80=AFshape-cast=20naming=20changes=20(?= =?UTF-8?q?#199)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat!: various renamin of shape-casting related struct/methods * chore: update changelog * feat: re-export ShapeCastOptions from the query module * feat: add a way to retrieve/modify the heightfield flags * fix compilation of tests * chore: a few internal renamings --- CHANGELOG.md | 32 ++++ .../examples/time_of_impact_query2d.rs | 23 ++- .../parry2d/tests/geometry/ball_ball_toi.rs | 6 +- crates/parry2d/tests/geometry/ray_cast.rs | 6 +- .../parry2d/tests/geometry/time_of_impact2.rs | 56 +++---- crates/parry3d/benches/query/ray.rs | 34 ++-- .../examples/time_of_impact_query3d.rs | 24 ++- .../parry3d/tests/geometry/ball_ball_toi.rs | 8 +- .../tests/geometry/ball_triangle_toi.rs | 7 +- .../parry3d/tests/geometry/cuboid_ray_cast.rs | 4 +- .../tests/geometry/still_objects_toi.rs | 9 +- .../parry3d/tests/geometry/time_of_impact3.rs | 23 ++- .../tests/geometry/trimesh_trimesh_toi.rs | 13 +- src/bounding_volume/simd_aabb.rs | 8 +- src/query/clip/clip_halfspace_polygon.rs | 8 +- src/query/default_query_dispatcher.rs | 109 ++++++------- src/query/gjk/gjk.rs | 43 +++-- src/query/mod.rs | 16 +- src/query/nonlinear_shape_cast/mod.rs | 20 +++ .../nonlinear_rigid_motion.rs | 0 .../nonlinear_shape_cast.rs} | 10 +- ...inear_shape_cast_composite_shape_shape.rs} | 46 +++--- ...inear_shape_cast_halfspace_support_map.rs} | 6 +- ...ear_shape_cast_support_map_support_map.rs} | 102 ++++++------ src/query/nonlinear_time_of_impact/mod.rs | 20 --- src/query/query_dispatcher.rs | 34 ++-- src/query/ray/ray.rs | 52 +++--- src/query/ray/ray_aabb.rs | 14 +- src/query/ray/ray_ball.rs | 8 +- src/query/ray/ray_bounding_sphere.rs | 16 +- src/query/ray/ray_composite_shape.rs | 98 ++++++----- src/query/ray/ray_cuboid.rs | 8 +- src/query/ray/ray_halfspace.rs | 8 +- src/query/ray/ray_heightfield.rs | 18 +-- src/query/ray/ray_round_shape.rs | 4 +- src/query/ray/ray_support_map.rs | 62 ++++--- src/query/ray/ray_triangle.rs | 21 +-- src/query/shape_cast/mod.rs | 26 +++ src/query/shape_cast/shape_cast.rs | 152 ++++++++++++++++++ .../shape_cast_ball_ball.rs} | 27 ++-- .../shape_cast_composite_shape_shape.rs} | 89 +++++----- .../shape_cast_halfspace_support_map.rs | 84 ++++++++++ .../shape_cast_heightfield_shape.rs} | 74 ++++----- .../shape_cast_support_map_support_map.rs | 69 ++++++++ src/query/time_of_impact/mod.rs | 28 ---- src/query/time_of_impact/time_of_impact.rs | 98 ----------- .../time_of_impact_halfspace_support_map.rs | 78 --------- .../time_of_impact_support_map_support_map.rs | 59 ------- .../visitors/ray_intersections_visitor.rs | 12 +- src/shape/heightfield3.rs | 12 +- src/shape/mod.rs | 2 + src/shape/round_shape.rs | 18 +++ .../triangle_triangle_intersection.rs | 6 +- 53 files changed, 978 insertions(+), 832 deletions(-) create mode 100644 src/query/nonlinear_shape_cast/mod.rs rename src/query/{nonlinear_time_of_impact => nonlinear_shape_cast}/nonlinear_rigid_motion.rs (100%) rename src/query/{nonlinear_time_of_impact/nonlinear_time_of_impact.rs => nonlinear_shape_cast/nonlinear_shape_cast.rs} (84%) rename src/query/{nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs => nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs} (81%) rename src/query/{nonlinear_time_of_impact/nonlinear_time_of_impact_halfspace_support_map.rs => nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs} (83%) rename src/query/{nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs => nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs} (85%) delete mode 100644 src/query/nonlinear_time_of_impact/mod.rs create mode 100644 src/query/shape_cast/mod.rs create mode 100644 src/query/shape_cast/shape_cast.rs rename src/query/{time_of_impact/time_of_impact_ball_ball.rs => shape_cast/shape_cast_ball_ball.rs} (66%) rename src/query/{time_of_impact/time_of_impact_composite_shape_shape.rs => shape_cast/shape_cast_composite_shape_shape.rs} (68%) create mode 100644 src/query/shape_cast/shape_cast_halfspace_support_map.rs rename src/query/{time_of_impact/time_of_impact_heightfield_shape.rs => shape_cast/shape_cast_heightfield_shape.rs} (79%) create mode 100644 src/query/shape_cast/shape_cast_support_map_support_map.rs delete mode 100644 src/query/time_of_impact/mod.rs delete mode 100644 src/query/time_of_impact/time_of_impact.rs delete mode 100644 src/query/time_of_impact/time_of_impact_halfspace_support_map.rs delete mode 100644 src/query/time_of_impact/time_of_impact_support_map_support_map.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index 0bd4c007..2169053f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,37 @@ # Change Log +## unreleased + +### Added + +- Add `ShapeCastOptions` that includes two new options for (linear) shape-casting. + `ShapeCastOptions::target_distance` which will return a hit as soon as the moving + shapes are closer than this distance; and `compute_impact_geometry_on_penetration` + which forces the calculation of proper witness points and normals even if the shapes + are initially intersecting (`time_of_impact == 0.0`). + +### Modified + +This version modifies many names related to shape-casting: + +- Renamed `TOI` to `ShapeCastHit`. +- Renamed `TOIStatus` to `ShapeCastStatus`. +- Rename `RayIntersection::toi` to `RayIntersection::time_of_impact`. +- More generally, all occurrences of the word `toi` have been replaced by `time_of_impact` + for better clarity. +- Rename `query::time_of_impact` to `query::cast_shapes`. More generally, all the + functions prefixed with `time_of_impact_` (e.g. `time_of_impact_ball_ball`) are + now prefixed with `cast_shapes_` (e.g. `cast_shapes_ball_ball`). +- Rename `QueryDispatcher::time_of_impact` to `QueryDispatcher::cast_shapes`. +- The (linear) shape-casting functions like `query::cast_shapes` (previously named + `query::time_of_impact) now take a `ShapeCastOptions` instead of the `max_toi` and + `stop_at_penetration` arguments. +- Rename `query::nonlinear_time_of_impact` to `query::cast_shapes_nonlinear`. +- Rename `QueryDispatcher::nonlinear_time_of_impact` to `QueryDispatcher::cast_sahpes_nonlinear`. +- Rename `NonlinearTOIMode` to `NonlinearShapeCastMode`, and `NonlinearTOIMode::DirectionalTOI` to + `NonlinearShapeCastMode::Directional`. +- Rename `TimeOfImpactStatus::Penetrating` to `ShapeCastStatus::PenetratingOrWithinTargetDist`. + ## v0.14.0 ### Modified diff --git a/crates/parry2d/examples/time_of_impact_query2d.rs b/crates/parry2d/examples/time_of_impact_query2d.rs index 15104ee9..d96e1d07 100644 --- a/crates/parry2d/examples/time_of_impact_query2d.rs +++ b/crates/parry2d/examples/time_of_impact_query2d.rs @@ -1,8 +1,8 @@ extern crate nalgebra as na; use na::{Isometry2, Vector2}; -use parry2d::math::Real; use parry2d::query; +use parry2d::query::ShapeCastOptions; use parry2d::shape::{Ball, Cuboid}; fn main() { @@ -20,42 +20,39 @@ fn main() { let ball_vel1 = Vector2::new(2.0, 2.0); let ball_vel2 = Vector2::new(-0.5, -0.5); - let toi_intersecting = query::time_of_impact( + let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, &ball_vel1, &ball, &cuboid_pos, &box_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - let toi_will_touch = query::time_of_impact( + let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, &ball_vel2, &ball, &cuboid_pos, &box_vel2, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - let toi_wont_touch = query::time_of_impact( + let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, &ball_vel1, &ball, &cuboid_pos, &box_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - assert_eq!(toi_intersecting.map(|toi| toi.toi), Some(0.0)); + assert_eq!(toi_intersecting.map(|hit| hit.time_of_impact), Some(0.0)); println!("Toi: {:?}", toi_will_touch); - assert!(toi_will_touch.is_some() && toi_will_touch.unwrap().toi > 0.0); - assert_eq!(toi_wont_touch.map(|toi| toi.toi), None); + assert!(toi_will_touch.is_some() && toi_will_touch.unwrap().time_of_impact > 0.0); + assert_eq!(toi_wont_touch.map(|hit| hit.time_of_impact), None); } diff --git a/crates/parry2d/tests/geometry/ball_ball_toi.rs b/crates/parry2d/tests/geometry/ball_ball_toi.rs index 8ddf5507..5e007b34 100644 --- a/crates/parry2d/tests/geometry/ball_ball_toi.rs +++ b/crates/parry2d/tests/geometry/ball_ball_toi.rs @@ -1,8 +1,8 @@ // Issue #35 use na::{self, Isometry2, Vector2}; -use parry2d::math::Real; use parry2d::query; +use parry2d::query::details::ShapeCastOptions; use parry2d::shape::Ball; #[test] @@ -13,7 +13,7 @@ fn test_ball_ball_toi() { let v1 = Vector2::new(0.0, 10.0); let v2 = Vector2::zeros(); - let cast = query::time_of_impact(&m1, &v1, &b, &m2, &v2, &b, Real::MAX, true).unwrap(); + let cast = query::cast_shapes(&m1, &v1, &b, &m2, &v2, &b, ShapeCastOptions::default()).unwrap(); - assert_eq!(cast.unwrap().toi, 0.9); + assert_eq!(cast.unwrap().time_of_impact, 0.9); } diff --git a/crates/parry2d/tests/geometry/ray_cast.rs b/crates/parry2d/tests/geometry/ray_cast.rs index a040e361..1856b478 100644 --- a/crates/parry2d/tests/geometry/ray_cast.rs +++ b/crates/parry2d/tests/geometry/ray_cast.rs @@ -102,7 +102,7 @@ fn raycast_starting_outside_of_triangle() { .cast_local_ray_and_get_normal(&ray, std::f32::MAX, true) .expect("No intersection"); - assert_ne!(intersect.toi, 0.0); + assert_ne!(intersect.time_of_impact, 0.0); } #[test] @@ -117,7 +117,7 @@ fn raycast_starting_inside_of_triangle() { .cast_local_ray_and_get_normal(&ray, std::f32::MAX, true) .expect("No intersection"); - assert_eq!(intersect.toi, 0.0); + assert_eq!(intersect.time_of_impact, 0.0); } #[test] @@ -132,7 +132,7 @@ fn raycast_starting_on_edge_of_triangle() { .cast_local_ray_and_get_normal(&ray, std::f32::MAX, true) .expect("No intersection"); - assert_eq!(intersect.toi, 0.0); + assert_eq!(intersect.time_of_impact, 0.0); } /// Ray Target diff --git a/crates/parry2d/tests/geometry/time_of_impact2.rs b/crates/parry2d/tests/geometry/time_of_impact2.rs index 95af766f..bcb47ec2 100644 --- a/crates/parry2d/tests/geometry/time_of_impact2.rs +++ b/crates/parry2d/tests/geometry/time_of_impact2.rs @@ -1,6 +1,7 @@ use na::{self, Isometry2, Point2, Vector2}; use parry2d::math::Real; use parry2d::query; +use parry2d::query::details::ShapeCastOptions; use parry2d::shape::{Ball, Cuboid, Polyline, Segment}; #[test] @@ -19,46 +20,43 @@ fn ball_cuboid_toi() { let ball_vel1 = Vector2::new(2.0, 2.0); let ball_vel2 = Vector2::new(-0.5, -0.5); - let toi_intersecting = query::time_of_impact( + let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, &ball_vel1, &ball, &cuboid_pos, &cuboid_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - let toi_will_touch = query::time_of_impact( + let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, &ball_vel2, &ball, &cuboid_pos, &cuboid_vel2, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - let toi_wont_touch = query::time_of_impact( + let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, &ball_vel2, &ball, &cuboid_pos, &cuboid_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - assert_eq!(toi_intersecting.map(|toi| toi.toi), Some(0.0)); + assert_eq!(toi_intersecting.map(|hit| hit.time_of_impact), Some(0.0)); assert!(relative_eq!( - toi_will_touch.unwrap().toi, + toi_will_touch.unwrap().time_of_impact, ((2.0 as Real).sqrt() - 1.0) / (ball_vel2 - cuboid_vel2).norm() )); - assert_eq!(toi_wont_touch.map(|toi| toi.toi), None); + assert_eq!(toi_wont_touch.map(|hit| hit.time_of_impact), None); } #[test] @@ -72,22 +70,21 @@ fn cuboid_cuboid_toi_issue_214() { let vel1 = Vector2::new(1.0, 0.0); let vel2 = Vector2::new(0.0, 0.0); - let toi = query::time_of_impact( + let hit = query::cast_shapes( &pos1, &vel1, &shape1, &pos2, &vel2, &shape2, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - assert!(toi.is_some()); + assert!(hit.is_some()); } #[test] -fn time_of_impact_should_return_toi_for_ball_and_rotated_polyline() { +fn cast_shapes_should_return_toi_for_ball_and_rotated_polyline() { let ball_isometry = Isometry2::identity(); let ball_velocity = Vector2::new(1.0, 0.0); let ball = Ball::new(0.5); @@ -104,23 +101,22 @@ fn time_of_impact_should_return_toi_for_ball_and_rotated_polyline() { Point2::new(1.0, 0.99999994) ); - let toi = query::time_of_impact( + let hit = query::cast_shapes( &ball_isometry, &ball_velocity, &ball, &polyline_isometry, &polyline_velocity, &polyline, - 1.0, - true, + ShapeCastOptions::with_max_time_of_impact(1.0), ) .unwrap(); - assert_eq!(toi.unwrap().toi, 0.5); + assert_eq!(hit.unwrap().time_of_impact, 0.5); } #[test] -fn time_of_impact_should_return_toi_for_ball_and_rotated_segment() { +fn cast_shapes_should_return_toi_for_ball_and_rotated_segment() { let ball_isometry = Isometry2::identity(); let ball_velocity = Vector2::new(1.0, 0.0); let ball = Ball::new(0.5); @@ -137,23 +133,22 @@ fn time_of_impact_should_return_toi_for_ball_and_rotated_segment() { Point2::new(1.0, 0.99999994) ); - let toi = query::time_of_impact( + let hit = query::cast_shapes( &ball_isometry, &ball_velocity, &ball, &segment_isometry, &segment_velocity, &segment, - 1.0, - true, + ShapeCastOptions::with_max_time_of_impact(1.0), ) .unwrap(); - assert_eq!(toi.unwrap().toi, 0.49999994); + assert_eq!(hit.unwrap().time_of_impact, 0.49999994); } #[test] -fn time_of_impact_should_return_toi_for_rotated_segment_and_ball() { +fn cast_shapes_should_return_toi_for_rotated_segment_and_ball() { let ball_isometry = Isometry2::identity(); let ball_velocity = Vector2::new(1.0, 0.0); let ball = Ball::new(0.5); @@ -170,17 +165,16 @@ fn time_of_impact_should_return_toi_for_rotated_segment_and_ball() { Point2::new(1.0, 0.99999994) ); - let toi = query::time_of_impact( + let hit = query::cast_shapes( &segment_isometry, &segment_velocity, &segment, &ball_isometry, &ball_velocity, &ball, - 1.0, - true, + ShapeCastOptions::with_max_time_of_impact(1.0), ) .unwrap(); - assert_eq!(toi.unwrap().toi, 0.5); + assert_eq!(hit.unwrap().time_of_impact, 0.5); } diff --git a/crates/parry3d/benches/query/ray.rs b/crates/parry3d/benches/query/ray.rs index 45402b1b..c1e9976a 100644 --- a/crates/parry3d/benches/query/ray.rs +++ b/crates/parry3d/benches/query/ray.rs @@ -13,14 +13,14 @@ use test::Bencher; #[macro_use] mod macros; -// FIXME: will the randomness of `solid` and `max_toi` affect too much the benchmark? +// FIXME: will the randomness of `solid` and `max_time_of_impact` affect too much the benchmark? bench_method!( bench_ray_against_ball, cast_ray, b: Ball, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -30,7 +30,7 @@ bench_method!( c: Cuboid, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -40,7 +40,7 @@ bench_method!( c: Capsule, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -50,7 +50,7 @@ bench_method!( c: Cone, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -60,7 +60,7 @@ bench_method!( c: Cylinder, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -70,7 +70,7 @@ bench_method!( a: Aabb, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -80,7 +80,7 @@ bench_method!( b: BoundingSphere, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -90,7 +90,7 @@ bench_method!( b: Ball, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -100,7 +100,7 @@ bench_method!( c: Cuboid, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -110,7 +110,7 @@ bench_method!( c: Capsule, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -120,7 +120,7 @@ bench_method!( c: Cone, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -130,7 +130,7 @@ bench_method!( c: Cylinder, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -140,7 +140,7 @@ bench_method!( c: Segment, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -150,7 +150,7 @@ bench_method!( c: Triangle, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -160,7 +160,7 @@ bench_method!( c: ConvexHull, pos: Isometry3, ray: Ray, - max_toi: f32, + max_time_of_impact: f32, solid: bool ); @@ -170,6 +170,6 @@ bench_method_gen!( m: TriMesh = generate_trimesh_around_origin, pos: Isometry3 = generate, ray: Ray = generate, - max_toi: f32 = generate, + max_time_of_impact: f32 = generate, solid: bool = generate ); diff --git a/crates/parry3d/examples/time_of_impact_query3d.rs b/crates/parry3d/examples/time_of_impact_query3d.rs index 2cdfa3d5..20028785 100644 --- a/crates/parry3d/examples/time_of_impact_query3d.rs +++ b/crates/parry3d/examples/time_of_impact_query3d.rs @@ -1,8 +1,7 @@ extern crate nalgebra as na; use na::{Isometry3, Vector3}; -use parry3d::math::Real; -use parry3d::query; +use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::{Ball, Cuboid}; fn main() { @@ -20,41 +19,38 @@ fn main() { let ball_vel1 = Vector3::new(2.0, 2.0, 2.0); let ball_vel2 = Vector3::new(-0.5, -0.5, -0.5); - let toi_intersecting = query::time_of_impact( + let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, &ball_vel1, &ball, &cuboid_pos, &cuboid_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - let toi_will_touch = query::time_of_impact( + let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, &ball_vel2, &ball, &cuboid_pos, &cuboid_vel2, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - let toi_wont_touch = query::time_of_impact( + let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, &ball_vel1, &ball, &cuboid_pos, &cuboid_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap(); - assert_eq!(toi_intersecting.map(|toi| toi.toi), Some(0.0)); - assert!(toi_will_touch.is_some() && toi_will_touch.unwrap().toi > 0.0); - assert_eq!(toi_wont_touch.map(|toi| toi.toi), None); + assert_eq!(toi_intersecting.map(|hit| hit.time_of_impact), Some(0.0)); + assert!(toi_will_touch.is_some() && toi_will_touch.unwrap().time_of_impact > 0.0); + assert_eq!(toi_wont_touch.map(|hit| hit.time_of_impact), None); } diff --git a/crates/parry3d/tests/geometry/ball_ball_toi.rs b/crates/parry3d/tests/geometry/ball_ball_toi.rs index 46bc35e1..55defdb0 100644 --- a/crates/parry3d/tests/geometry/ball_ball_toi.rs +++ b/crates/parry3d/tests/geometry/ball_ball_toi.rs @@ -1,8 +1,7 @@ // Issue #35 use na::{self, Isometry3, Vector3}; -use parry3d::math::Real; -use parry3d::query; +use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::Ball; #[test] @@ -13,7 +12,8 @@ fn test_ball_ball_toi() { let vel1 = Vector3::new(0.0, 10.0, 0.0); let vel2 = Vector3::zeros(); - let cast = query::time_of_impact(&m1, &vel1, &b, &m2, &vel2, &b, Real::MAX, true).unwrap(); + let cast = + query::cast_shapes(&m1, &vel1, &b, &m2, &vel2, &b, ShapeCastOptions::default()).unwrap(); - assert_eq!(cast.unwrap().toi, 0.9); + assert_eq!(cast.unwrap().time_of_impact, 0.9); } diff --git a/crates/parry3d/tests/geometry/ball_triangle_toi.rs b/crates/parry3d/tests/geometry/ball_triangle_toi.rs index 1e83bfbf..bf57906a 100644 --- a/crates/parry3d/tests/geometry/ball_triangle_toi.rs +++ b/crates/parry3d/tests/geometry/ball_triangle_toi.rs @@ -1,7 +1,7 @@ // Issue #123 use na::{self, Isometry3, Point3, Vector3}; -use parry3d::query; +use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::{Ball, Triangle}; #[test] @@ -18,8 +18,9 @@ fn ball_triangle_toi_infinite_loop_issue() { let vel1 = Vector3::new(0.0, 0.000000000000000000000000000000000000000006925, 0.0); let vel2 = Vector3::zeros(); - let cast = query::time_of_impact(&m1, &vel1, &b, &m2, &vel2, &t, std::f32::MAX, true).unwrap(); + let cast = + query::cast_shapes(&m1, &vel1, &b, &m2, &vel2, &t, ShapeCastOptions::default()).unwrap(); - println!("TOI: {:?}", cast); + println!("ShapeCastHit: {:?}", cast); assert!(cast.is_none()); // The provided velocity is too small. } diff --git a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs index fd9d0018..9c1cf7c2 100644 --- a/crates/parry3d/tests/geometry/cuboid_ray_cast.rs +++ b/crates/parry3d/tests/geometry/cuboid_ray_cast.rs @@ -37,7 +37,7 @@ where ray, name, rotation )); - let point = ray.origin + ray.dir * intersection.toi; + let point = ray.origin + ray.dir * intersection.time_of_impact; let point_nudged_in = point + intersection.normal * -0.001; let point_nudged_out = point + intersection.normal * 0.001; @@ -70,7 +70,7 @@ where shape .cast_ray_and_get_normal(&position, &new_ray, std::f32::MAX, true) .expect("recurring ray cast produced a different answer") - .toi + .time_of_impact ); } } diff --git a/crates/parry3d/tests/geometry/still_objects_toi.rs b/crates/parry3d/tests/geometry/still_objects_toi.rs index dff2d1c8..3c9bad02 100644 --- a/crates/parry3d/tests/geometry/still_objects_toi.rs +++ b/crates/parry3d/tests/geometry/still_objects_toi.rs @@ -1,5 +1,5 @@ use na::{self, Isometry3, Vector3}; -use parry3d::query::time_of_impact; +use parry3d::query::{cast_shapes, ShapeCastOptions}; use parry3d::shape::Cuboid; /** @@ -25,18 +25,17 @@ fn collide(v_y: f32) -> Option { let vel2 = Vector3::zeros(); let cuboid = Cuboid::new(Vector3::new(0.5, 0.5, 0.5)); - time_of_impact( + cast_shapes( &pos1, &vel1, &cuboid, &pos2, &vel2, &cuboid, - std::f32::MAX, - true, + ShapeCastOptions::default(), ) .unwrap() - .map(|toi| toi.toi) + .map(|hit| hit.time_of_impact) } #[test] diff --git a/crates/parry3d/tests/geometry/time_of_impact3.rs b/crates/parry3d/tests/geometry/time_of_impact3.rs index 8e014d97..71cbcf45 100644 --- a/crates/parry3d/tests/geometry/time_of_impact3.rs +++ b/crates/parry3d/tests/geometry/time_of_impact3.rs @@ -1,6 +1,6 @@ use na::{self, Isometry3, Vector3}; use parry3d::math::Real; -use parry3d::query; +use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::{Ball, Cuboid}; #[test] @@ -19,42 +19,39 @@ fn ball_cuboid_toi() { let ball_vel1 = Vector3::new(2.0, 2.0, 2.0); let ball_vel2 = Vector3::new(-0.5, -0.5, -0.5); - let toi_intersecting = query::time_of_impact( + let toi_intersecting = query::cast_shapes( &ball_pos_intersecting, &ball_vel1, &ball, &cuboid_pos, &cuboid_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap() - .map(|toi| toi.toi); - let toi_will_touch = query::time_of_impact( + .map(|hit| hit.time_of_impact); + let toi_will_touch = query::cast_shapes( &ball_pos_will_touch, &ball_vel2, &ball, &cuboid_pos, &cuboid_vel2, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap() - .map(|toi| toi.toi); - let toi_wont_touch = query::time_of_impact( + .map(|hit| hit.time_of_impact); + let toi_wont_touch = query::cast_shapes( &ball_pos_wont_touch, &ball_vel1, &ball, &cuboid_pos, &cuboid_vel1, &cuboid, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap() - .map(|toi| toi.toi); + .map(|hit| hit.time_of_impact); assert_eq!(toi_intersecting, Some(0.0)); assert!(relative_eq!( diff --git a/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs b/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs index 2c94c93d..22493edb 100644 --- a/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs +++ b/crates/parry3d/tests/geometry/trimesh_trimesh_toi.rs @@ -2,7 +2,7 @@ use na::{zero, Isometry3, Point3, Vector3}; use parry3d::math::Real; -use parry3d::query; +use parry3d::query::{self, ShapeCastOptions}; use parry3d::shape::TriMesh; fn build_pyramid() -> TriMesh { @@ -33,22 +33,21 @@ fn do_toi_test() -> Option { let vel_one = Vector3::new(SPEED, 0.0, 0.0); let vel_two = Vector3::new(0.0, 0.0, 0.0); - query::time_of_impact( + query::cast_shapes( &transform_one, &vel_one, &shape_one, &transform_two, &vel_two, &shape_two, - Real::MAX, - true, + ShapeCastOptions::default(), ) .unwrap() - .map(|toi| toi.toi) + .map(|hit| hit.time_of_impact) } #[test] fn trimesh_trimesh_toi() { - let toi = do_toi_test(); - assert_eq!(toi, Some(0.00998)); + let time_of_impact = do_toi_test(); + assert_eq!(time_of_impact, Some(0.00998)); } diff --git a/src/bounding_volume/simd_aabb.rs b/src/bounding_volume/simd_aabb.rs index 75bbfde7..d07d6299 100644 --- a/src/bounding_volume/simd_aabb.rs +++ b/src/bounding_volume/simd_aabb.rs @@ -202,14 +202,18 @@ impl SimdAabb { } /// Casts a ray on all the Aabbs represented by `self`. - pub fn cast_local_ray(&self, ray: &SimdRay, max_toi: SimdReal) -> (SimdBool, SimdReal) { + pub fn cast_local_ray( + &self, + ray: &SimdRay, + max_time_of_impact: SimdReal, + ) -> (SimdBool, SimdReal) { let zero = SimdReal::zero(); let one = SimdReal::one(); let infinity = SimdReal::splat(Real::MAX); let mut hit = SimdBool::splat(true); let mut tmin = SimdReal::zero(); - let mut tmax = max_toi; + let mut tmax = max_time_of_impact; // TODO: could this be optimized more considering we really just need a boolean answer? for i in 0usize..DIM { diff --git a/src/query/clip/clip_halfspace_polygon.rs b/src/query/clip/clip_halfspace_polygon.rs index d651ba97..5a9e93a3 100644 --- a/src/query/clip/clip_halfspace_polygon.rs +++ b/src/query/clip/clip_halfspace_polygon.rs @@ -38,9 +38,11 @@ pub fn clip_halfspace_polygon( let prev_pt = &polygon[prev_i]; let ray = Ray::new(*prev_pt, pt - prev_pt); - if let Some(toi) = query::details::ray_toi_with_halfspace(center, normal, &ray) { - if toi > 0.0 && toi < 1.0 { - result.push(ray.origin + ray.dir * toi) + if let Some(time_of_impact) = + query::details::ray_toi_with_halfspace(center, normal, &ray) + { + if time_of_impact > 0.0 && time_of_impact < 1.0 { + result.push(ray.origin + ray.dir * time_of_impact) } } diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 2b826eca..11af3acf 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -1,7 +1,8 @@ use crate::math::{Isometry, Point, Real, Vector}; +use crate::query::details::ShapeCastOptions; use crate::query::{ - self, details::NonlinearTOIMode, ClosestPoints, Contact, NonlinearRigidMotion, QueryDispatcher, - Unsupported, TOI, + self, details::NonlinearShapeCastMode, ClosestPoints, Contact, NonlinearRigidMotion, + QueryDispatcher, ShapeCastHit, Unsupported, }; #[cfg(feature = "std")] use crate::query::{ @@ -268,96 +269,88 @@ impl QueryDispatcher for DefaultQueryDispatcher { } } - fn time_of_impact( + fn cast_shapes( &self, pos12: &Isometry, local_vel12: &Vector, shape1: &dyn Shape, shape2: &dyn Shape, - max_toi: Real, - stop_at_penetration: bool, - ) -> Result, Unsupported> { + options: ShapeCastOptions, + ) -> Result, Unsupported> { if let (Some(b1), Some(b2)) = (shape1.as_ball(), shape2.as_ball()) { - Ok(query::details::time_of_impact_ball_ball( + Ok(query::details::cast_shapes_ball_ball( pos12, local_vel12, b1, b2, - max_toi, + options, )) } else if let (Some(p1), Some(s2)) = (shape1.as_shape::(), shape2.as_support_map()) { - Ok(query::details::time_of_impact_halfspace_support_map( + Ok(query::details::cast_shapes_halfspace_support_map( pos12, local_vel12, p1, s2, - max_toi, - stop_at_penetration, + options, )) } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::()) { - Ok(query::details::time_of_impact_support_map_halfspace( + Ok(query::details::cast_shapes_support_map_halfspace( pos12, local_vel12, s1, p2, - max_toi, - stop_at_penetration, + options, )) } else { #[cfg(feature = "std")] if let Some(heightfield1) = shape1.as_heightfield() { - return query::details::time_of_impact_heightfield_shape( + return query::details::cast_shapes_heightfield_shape( self, pos12, local_vel12, heightfield1, shape2, - max_toi, - stop_at_penetration, + options, ); } else if let Some(heightfield2) = shape1.as_heightfield() { - return query::details::time_of_impact_shape_heightfield( + return query::details::cast_shapes_shape_heightfield( self, pos12, local_vel12, shape1, heightfield2, - max_toi, - stop_at_penetration, + options, ); } else if let (Some(s1), Some(s2)) = (shape1.as_support_map(), shape2.as_support_map()) { - return Ok(query::details::time_of_impact_support_map_support_map( + return Ok(query::details::cast_shapes_support_map_support_map( pos12, local_vel12, s1, s2, - max_toi, - stop_at_penetration, + options, )); } else if let Some(c1) = shape1.as_composite_shape() { - return Ok(query::details::time_of_impact_composite_shape_shape( + return Ok(query::details::cast_shapes_composite_shape_shape( self, pos12, local_vel12, c1, shape2, - max_toi, - stop_at_penetration, + options, )); } else if let Some(c2) = shape2.as_composite_shape() { - return Ok(query::details::time_of_impact_shape_composite_shape( + return Ok(query::details::cast_shapes_shape_composite_shape( self, pos12, local_vel12, shape1, c2, - max_toi, - stop_at_penetration, + options, )); } @@ -365,7 +358,7 @@ impl QueryDispatcher for DefaultQueryDispatcher { } } - fn nonlinear_time_of_impact( + fn cast_shapes_nonlinear( &self, motion1: &NonlinearRigidMotion, shape1: &dyn Shape, @@ -374,53 +367,49 @@ impl QueryDispatcher for DefaultQueryDispatcher { start_time: Real, end_time: Real, stop_at_penetration: bool, - ) -> Result, Unsupported> { + ) -> Result, Unsupported> { if let (Some(sm1), Some(sm2)) = (shape1.as_support_map(), shape2.as_support_map()) { let mode = if stop_at_penetration { - NonlinearTOIMode::StopAtPenetration + NonlinearShapeCastMode::StopAtPenetration } else { - NonlinearTOIMode::directional_toi(shape1, shape2) + NonlinearShapeCastMode::directional_toi(shape1, shape2) }; Ok( - query::details::nonlinear_time_of_impact_support_map_support_map( + query::details::cast_shapes_nonlinear_support_map_support_map( self, motion1, sm1, shape1, motion2, sm2, shape2, start_time, end_time, mode, ), ) } else { #[cfg(feature = "std")] if let Some(c1) = shape1.as_composite_shape() { - return Ok( - query::details::nonlinear_time_of_impact_composite_shape_shape( - self, - motion1, - c1, - motion2, - shape2, - start_time, - end_time, - stop_at_penetration, - ), - ); + return Ok(query::details::cast_shapes_nonlinear_composite_shape_shape( + self, + motion1, + c1, + motion2, + shape2, + start_time, + end_time, + stop_at_penetration, + )); } else if let Some(c2) = shape2.as_composite_shape() { - return Ok( - query::details::nonlinear_time_of_impact_shape_composite_shape( - self, - motion1, - shape1, - motion2, - c2, - start_time, - end_time, - stop_at_penetration, - ), - ); + return Ok(query::details::cast_shapes_nonlinear_shape_composite_shape( + self, + motion1, + shape1, + motion2, + c2, + start_time, + end_time, + stop_at_penetration, + )); } /* } else if let (Some(p1), Some(s2)) = (shape1.as_shape::(), shape2.as_support_map()) { - // query::details::nonlinear_time_of_impact_halfspace_support_map(m1, vel1, p1, m2, vel2, s2) + // query::details::cast_shapes_nonlinear_halfspace_support_map(m1, vel1, p1, m2, vel2, s2) unimplemented!() } else if let (Some(s1), Some(p2)) = (shape1.as_support_map(), shape2.as_shape::()) { - // query::details::nonlinear_time_of_impact_support_map_halfspace(m1, vel1, s1, m2, vel2, p2) + // query::details::cast_shapes_nonlinear_support_map_halfspace(m1, vel1, s1, m2, vel2, p2) unimplemented!() */ Err(Unsupported) diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index 97f48055..7bbc0573 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -189,13 +189,20 @@ pub fn cast_local_ray( shape: &G, simplex: &mut VoronoiSimplex, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, ) -> Option<(Real, Vector)> where G: SupportMap, { let g2 = ConstantOrigin; - minkowski_ray_cast(&Isometry::identity(), shape, &g2, ray, max_toi, simplex) + minkowski_ray_cast( + &Isometry::identity(), + shape, + &g2, + ray, + max_time_of_impact, + simplex, + ) } /// Compute the normal and the distance that can travel `g1` along the direction @@ -214,17 +221,19 @@ where G2: SupportMap, { let ray = Ray::new(Point::origin(), *dir); - minkowski_ray_cast(pos12, g1, g2, &ray, Real::max_value(), simplex).map(|(toi, normal)| { - let witnesses = if !toi.is_zero() { - result(simplex, simplex.dimension() == DIM) - } else { - // If there is penetration, the witness points - // are undefined. - (Point::origin(), Point::origin()) - }; - - (toi, normal, witnesses.0, witnesses.1) - }) + minkowski_ray_cast(pos12, g1, g2, &ray, Real::max_value(), simplex).map( + |(time_of_impact, normal)| { + let witnesses = if !time_of_impact.is_zero() { + result(simplex, simplex.dimension() == DIM) + } else { + // If there is penetration, the witness points + // are undefined. + (Point::origin(), Point::origin()) + }; + + (time_of_impact, normal, witnesses.0, witnesses.1) + }, + ) } // Ray-cast on the Minkowski Difference `g1 - pos12 * g2`. @@ -233,7 +242,7 @@ fn minkowski_ray_cast( g1: &G1, g2: &G2, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, simplex: &mut VoronoiSimplex, ) -> Option<(Real, Vector)> where @@ -304,10 +313,10 @@ where ldir = *dir; ltoi += t; - // NOTE: we divide by ray_length instead of doing max_toi * ray_length - // because the multiplication may cause an overflow if max_toi is set + // NOTE: we divide by ray_length instead of doing max_time_of_impact * ray_length + // because the multiplication may cause an overflow if max_time_of_impact is set // to Real::max_value() by users that want to have an infinite ray. - if ltoi / ray_length > max_toi { + if ltoi / ray_length > max_time_of_impact { return None; } diff --git a/src/query/mod.rs b/src/query/mod.rs index 763d8cd0..ed6e1537 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -7,8 +7,8 @@ //! * [`distance()`] to compute the distance between two shapes. //! * [`contact()`] to compute one pair of contact points between two shapes, including penetrating contact. //! * [`intersection_test()`] to determine if two shapes are intersecting or not. -//! * [`time_of_impact()`] to determine when two shapes undergoing translational motions hit for the first time. -//! * [`nonlinear_time_of_impact()`] to determine when two shapes undergoing continuous rigid motions hit for the first time. +//! * [`cast_shapes()`] to determine when two shapes undergoing translational motions hit for the first time. +//! * [`cast_shapes_nonlinear()`] to determine when two shapes undergoing continuous rigid motions hit for the first time. //! //! Ray-casting and point-projection can be achieved by importing traits: //! @@ -35,14 +35,14 @@ pub use self::default_query_dispatcher::DefaultQueryDispatcher; pub use self::distance::distance; pub use self::error::Unsupported; pub use self::intersection_test::intersection_test; -pub use self::nonlinear_time_of_impact::{nonlinear_time_of_impact, NonlinearRigidMotion}; +pub use self::nonlinear_shape_cast::{cast_shapes_nonlinear, NonlinearRigidMotion}; pub use self::point::{PointProjection, PointQuery, PointQueryWithLocation}; #[cfg(feature = "std")] 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::split::{IntersectResult, SplitResult}; -pub use self::time_of_impact::{time_of_impact, TOIStatus, TOI}; mod clip; pub mod closest_points; @@ -56,13 +56,13 @@ pub mod epa; mod error; pub mod gjk; mod intersection_test; -mod nonlinear_time_of_impact; +mod nonlinear_shape_cast; pub mod point; mod query_dispatcher; mod ray; pub mod sat; +mod shape_cast; mod split; -mod time_of_impact; #[cfg(feature = "std")] pub mod visitors; @@ -75,8 +75,8 @@ pub mod details { pub use super::contact_manifolds::*; pub use super::distance::*; pub use super::intersection_test::*; - pub use super::nonlinear_time_of_impact::*; + pub use super::nonlinear_shape_cast::*; pub use super::point::*; pub use super::ray::*; - pub use super::time_of_impact::*; + pub use super::shape_cast::*; } diff --git a/src/query/nonlinear_shape_cast/mod.rs b/src/query/nonlinear_shape_cast/mod.rs new file mode 100644 index 00000000..a2882677 --- /dev/null +++ b/src/query/nonlinear_shape_cast/mod.rs @@ -0,0 +1,20 @@ +//! Implementation details of the `cast_shapes_nonlinear` function. + +#[cfg(feature = "std")] +pub use self::nonlinear_shape_cast_composite_shape_shape::{ + cast_shapes_nonlinear_composite_shape_shape, cast_shapes_nonlinear_shape_composite_shape, + NonlinearTOICompositeShapeShapeBestFirstVisitor, +}; +//pub use self::nonlinear_shape_cast_halfspace_support_map::{cast_shapes_nonlinear_halfspace_support_map, cast_shapes_nonlinear_support_map_halfspace}; +pub use self::nonlinear_rigid_motion::NonlinearRigidMotion; +pub use self::nonlinear_shape_cast::cast_shapes_nonlinear; +pub use self::nonlinear_shape_cast_support_map_support_map::{ + cast_shapes_nonlinear_support_map_support_map, NonlinearShapeCastMode, +}; + +#[cfg(feature = "std")] +mod nonlinear_shape_cast_composite_shape_shape; +//mod cast_shapes_nonlinear_halfspace_support_map; +mod nonlinear_rigid_motion; +mod nonlinear_shape_cast; +mod nonlinear_shape_cast_support_map_support_map; diff --git a/src/query/nonlinear_time_of_impact/nonlinear_rigid_motion.rs b/src/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs similarity index 100% rename from src/query/nonlinear_time_of_impact/nonlinear_rigid_motion.rs rename to src/query/nonlinear_shape_cast/nonlinear_rigid_motion.rs diff --git a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs similarity index 84% rename from src/query/nonlinear_time_of_impact/nonlinear_time_of_impact.rs rename to src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs index 10525589..67a48912 100644 --- a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast.rs @@ -1,6 +1,6 @@ use crate::math::Real; use crate::query::{ - DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, Unsupported, TOI, + DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, Unsupported, }; use crate::shape::Shape; @@ -15,12 +15,12 @@ use crate::shape::Shape; /// * `end_time` - The end time of the interval where the motion takes place. /// * `stop_at_penetration` - If the casted shape starts in a penetration state with any /// collider, two results are possible. If `stop_at_penetration` is `true` then, the -/// result will have a `toi` equal to `start_time`. If `stop_at_penetration` is `false` +/// result will have a `time_of_impact` equal to `start_time`. If `stop_at_penetration` is `false` /// then the nonlinear shape-casting will see if further motion wrt. the penetration normal /// would result in tunnelling. If it does not (i.e. we have a separating velocity along /// that normal) then the nonlinear shape-casting will attempt to find another impact, /// at a time `> start_time` that could result in tunnelling. -pub fn nonlinear_time_of_impact( +pub fn cast_shapes_nonlinear( motion1: &NonlinearRigidMotion, g1: &dyn Shape, motion2: &NonlinearRigidMotion, @@ -28,8 +28,8 @@ pub fn nonlinear_time_of_impact( start_time: Real, end_time: Real, stop_at_penetration: bool, -) -> Result, Unsupported> { - DefaultQueryDispatcher.nonlinear_time_of_impact( +) -> Result, Unsupported> { + DefaultQueryDispatcher.cast_shapes_nonlinear( motion1, g1, motion2, diff --git a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs similarity index 81% rename from src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs rename to src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs index 5a337d67..959d37de 100644 --- a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_composite_shape_shape.rs @@ -1,12 +1,14 @@ use crate::bounding_volume::{BoundingSphere, SimdAabb}; use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; -use crate::query::{self, details::NonlinearTOIMode, NonlinearRigidMotion, QueryDispatcher, TOI}; +use crate::query::{ + self, details::NonlinearShapeCastMode, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, +}; use crate::shape::{Ball, Shape, TypedSimdCompositeShape}; use simba::simd::SimdValue; /// Time Of Impact of a composite shape with any other shape, under a rigid motion (translation + rotation). -pub fn nonlinear_time_of_impact_composite_shape_shape( +pub fn cast_shapes_nonlinear_composite_shape_shape( dispatcher: &D, motion1: &NonlinearRigidMotion, g1: &G1, @@ -15,7 +17,7 @@ pub fn nonlinear_time_of_impact_composite_shape_shape( start_time: Real, end_time: Real, stop_at_penetration: bool, -) -> Option +) -> Option where D: QueryDispatcher, G1: TypedSimdCompositeShape, @@ -37,7 +39,7 @@ where } /// Time Of Impact of any shape with a composite shape, under a rigid motion (translation + rotation). -pub fn nonlinear_time_of_impact_shape_composite_shape( +pub fn cast_shapes_nonlinear_shape_composite_shape( dispatcher: &D, motion1: &NonlinearRigidMotion, g1: &dyn Shape, @@ -46,12 +48,12 @@ pub fn nonlinear_time_of_impact_shape_composite_shape( start_time: Real, end_time: Real, stop_at_penetration: bool, -) -> Option +) -> Option where D: QueryDispatcher, G2: TypedSimdCompositeShape, { - nonlinear_time_of_impact_composite_shape_shape( + cast_shapes_nonlinear_composite_shape_shape( dispatcher, motion2, g2, @@ -61,7 +63,7 @@ where end_time, stop_at_penetration, ) - .map(|toi| toi.swapped()) + .map(|hit| hit.swapped()) } /// A visitor used to determine the non-linear time of impact between a composite shape and another shape. @@ -115,7 +117,7 @@ where D: QueryDispatcher, G1: TypedSimdCompositeShape, { - type Result = (G1::PartId, TOI); + type Result = (G1::PartId, ShapeCastHit); #[inline] fn visit( @@ -139,7 +141,7 @@ where let ball_motion1 = self.motion1.prepend_translation(center1.coords); let ball_motion2 = self.motion2.prepend_translation(self.sphere2.center.coords); - if let Some(toi) = query::details::nonlinear_time_of_impact_support_map_support_map( + if let Some(hit) = query::details::cast_shapes_nonlinear_support_map_support_map( self.dispatcher, &ball_motion1, &ball1, @@ -149,15 +151,15 @@ where &ball2, self.start_time, self.end_time, - NonlinearTOIMode::StopAtPenetration, + NonlinearShapeCastMode::StopAtPenetration, ) { if let Some(data) = data { - if toi.toi < best && data[ii].is_some() { + if hit.time_of_impact < best && data[ii].is_some() { let part_id = *data[ii].unwrap(); self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { - let toi = if let Some(part_pos1) = part_pos1 { + let hit = if let Some(part_pos1) = part_pos1 { self.dispatcher - .nonlinear_time_of_impact( + .cast_shapes_nonlinear( &self.motion1.prepend(*part_pos1), g1, self.motion2, @@ -167,10 +169,10 @@ where self.stop_at_penetration, ) .unwrap_or(None) - .map(|toi| toi.transform1_by(part_pos1)) + .map(|hit| hit.transform1_by(part_pos1)) } else { self.dispatcher - .nonlinear_time_of_impact( + .cast_shapes_nonlinear( self.motion1, g1, self.motion2, @@ -182,18 +184,18 @@ where .unwrap_or(None) }; - // println!("Found toi: {:?}", toi); + // println!("Found time_of_impact: {:?}", time_of_impact); - if let Some(toi) = toi { - weights[ii] = toi.toi; - mask[ii] = toi.toi < best; - results[ii] = Some((part_id, toi)); + if let Some(hit) = hit { + weights[ii] = hit.time_of_impact; + mask[ii] = hit.time_of_impact < best; + results[ii] = Some((part_id, hit)); } }); } } else { - weights[ii] = toi.toi; - mask[ii] = toi.toi < best; + weights[ii] = hit.time_of_impact; + mask[ii] = hit.time_of_impact < best; } } } diff --git a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_halfspace_support_map.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs similarity index 83% rename from src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_halfspace_support_map.rs rename to src/query/nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs index 19f3fe5c..6bcf48dc 100644 --- a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_halfspace_support_map.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_halfspace_support_map.rs @@ -6,7 +6,7 @@ use crate::shape::HalfSpace; use crate::shape::SupportMap; /// Time Of Impact of a halfspace with a support-mapped shape under a rigid motion (translation + rotation). -pub fn nonlinear_time_of_impact_halfspace_support_map( +pub fn cast_shapes_nonlinear_halfspace_support_map( pos12: &Isometry, vel12: &Vector, halfspace: &HalfSpace, @@ -35,7 +35,7 @@ where } /// Time Of Impact of a halfspace with a support-mapped shape under a rigid motion (translation + rotation). -pub fn nonlinear_time_of_impact_support_map_halfspace( +pub fn cast_shapes_nonlinear_support_map_halfspace( pos12: &Isometry, vel12: &Vector, other: &G, @@ -44,5 +44,5 @@ pub fn nonlinear_time_of_impact_support_map_halfspace( where G: SupportMap, { - nonlinear_time_of_impact_halfspace_support_map(pos12, vel12, halfspace, other) + cast_shapes_nonlinear_halfspace_support_map(pos12, vel12, halfspace, other) } diff --git a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs similarity index 85% rename from src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs rename to src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs index d906dd1e..bb04772f 100644 --- a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs +++ b/src/query/nonlinear_shape_cast/nonlinear_shape_cast_support_map_support_map.rs @@ -3,34 +3,36 @@ use na::ComplexField; // for .abs() use na::{RealField, Unit}; use crate::math::{Point, Real, Vector}; -use crate::query::{self, ClosestPoints, NonlinearRigidMotion, QueryDispatcher, TOIStatus, TOI}; +use crate::query::{ + self, ClosestPoints, NonlinearRigidMotion, QueryDispatcher, ShapeCastHit, ShapeCastStatus, +}; use crate::shape::{Shape, SupportMap}; use crate::utils::WCross; use crate::query::gjk::ConstantPoint; use num::Bounded; -/// Enum specifying the behavior of TOI computation when there is a penetration at the starting time. +/// Enum specifying the behavior of shape-casting when there is a penetration at the starting time. #[derive(Copy, Clone, Debug)] -pub enum NonlinearTOIMode { - /// Stop TOI computation as soon as there is a penetration. +pub enum NonlinearShapeCastMode { + /// Stop shape-casting as soon as there is a penetration. StopAtPenetration, - /// When there is a penetration, don't stop the TOI search if the relative velocity + /// When there is a penetration, don't stop the shape-cast if the relative velocity /// at the penetration points is negative (i.e. if the points are separating). - DirectionalTOI { - /// The sum of the `Shape::ccd_thickness` of both shapes involved in the TOI computation. + Directional { + /// The sum of the `Shape::ccd_thickness` of both shapes involved in the shape-cast. sum_linear_thickness: Real, - /// The max of the `Shape::ccd_angular_thickness` of both shapes involved in the TOI computation. + /// The max of the `Shape::ccd_angular_thickness` of both shapes involved in the shape-cast. max_angular_thickness: Real, }, } -impl NonlinearTOIMode { - /// Initializes a directional TOI mode. +impl NonlinearShapeCastMode { + /// Initializes a directional `NonlinearShapeCastMode`. /// - /// With the "directional" TOI mode, the nonlinear TOI computation won't + /// With the "directional" shape-cast mode, the nonlinear shape-casting won't /// immediately stop if the shapes are already intersecting at `t = 0`. - /// Instead, it will search for the first time where a contact between + /// Instead, it will search for the first time when a contact between /// the shapes would result in a deeper penetration (with risk of tunnelling). /// This effectively checks the relative velocity of the shapes at their point /// of impact. @@ -44,7 +46,7 @@ impl NonlinearTOIMode { .ccd_angular_thickness() .max(shape2.ccd_angular_thickness()); - NonlinearTOIMode::DirectionalTOI { + NonlinearShapeCastMode::Directional { sum_linear_thickness, max_angular_thickness, } @@ -53,7 +55,7 @@ impl NonlinearTOIMode { /// Compute the time of first impact between two support-map shapes following /// a nonlinear (with translations and rotations) motion. -pub fn nonlinear_time_of_impact_support_map_support_map( +pub fn cast_shapes_nonlinear_support_map_support_map( dispatcher: &D, motion1: &NonlinearRigidMotion, sm1: &SM1, @@ -63,8 +65,8 @@ pub fn nonlinear_time_of_impact_support_map_support_map( g2: &dyn Shape, start_time: Real, end_time: Real, - mode: NonlinearTOIMode, -) -> Option + mode: NonlinearShapeCastMode, +) -> Option where D: ?Sized + QueryDispatcher, SM1: ?Sized + SupportMap, @@ -85,7 +87,7 @@ where compute_toi( dispatcher, motion2, sm2, g2, motion1, sm1, g1, start_time, end_time, mode, ) - .map(|toi| toi.swapped()) + .map(|hit| hit.swapped()) } } @@ -100,8 +102,8 @@ pub fn compute_toi( g2: &dyn Shape, start_time: Real, end_time: Real, - mode: NonlinearTOIMode, -) -> Option + mode: NonlinearShapeCastMode, +) -> Option where D: ?Sized + QueryDispatcher, SM1: ?Sized + SupportMap, @@ -110,18 +112,18 @@ where let mut prev_min_t = start_time; let abs_tol: Real = query::gjk::eps_tol(); - let mut result = TOI { - toi: start_time, + let mut result = ShapeCastHit { + time_of_impact: start_time, normal1: Vector::::x_axis(), normal2: Vector::::x_axis(), witness1: Point::::origin(), witness2: Point::::origin(), - status: TOIStatus::Penetrating, + status: ShapeCastStatus::PenetratingOrWithinTargetDist, }; loop { - let pos1 = motion1.position_at_time(result.toi); - let pos2 = motion2.position_at_time(result.toi); + let pos1 = motion1.position_at_time(result.time_of_impact); + let pos2 = motion2.position_at_time(result.time_of_impact); let pos12 = pos1.inv_mul(&pos2); // TODO: use the _with_params version of the closest points query. @@ -131,10 +133,10 @@ where { ClosestPoints::Intersecting => { // println!(">> Intersecting."); - if result.toi == start_time { - result.status = TOIStatus::Penetrating + if result.time_of_impact == start_time { + result.status = ShapeCastStatus::PenetratingOrWithinTargetDist } else { - result.status = TOIStatus::Failed; + result.status = ShapeCastStatus::Failed; } break; } @@ -151,9 +153,9 @@ where result.normal2 = pos12.inverse_transform_unit_vector(&-normal1); let curr_range = BisectionRange { - min_t: result.toi, + min_t: result.time_of_impact, max_t: end_time, - curr_t: result.toi, + curr_t: result.time_of_impact, }; let (new_range, niter) = @@ -162,7 +164,7 @@ where // "Bisection result: {:?}, normal1: {:?}, normal2: {:?}", // new_range, result.normal1, result.normal2 // ); - result.toi = new_range.curr_t; + result.time_of_impact = new_range.curr_t; if new_range.min_t - prev_min_t < abs_tol { if new_range.max_t == end_time { @@ -183,18 +185,18 @@ where } } - result.status = TOIStatus::Converged; + result.status = ShapeCastStatus::Converged; break; } prev_min_t = new_range.min_t; if niter == 0 { - result.status = TOIStatus::Converged; + result.status = ShapeCastStatus::Converged; break; } } else { - result.status = TOIStatus::Failed; + result.status = ShapeCastStatus::Failed; break; } } @@ -204,7 +206,7 @@ where log::error!( "Closest points not found despite setting the max distance to infinity." ); - result.status = TOIStatus::Failed; + result.status = ShapeCastStatus::Failed; break; } } @@ -212,17 +214,17 @@ where // In we started with a penetration, we need to compute a full contact manifold and // see if any of these contact points may result in tunnelling. If the is one, return - // that TOI instead. That way, object moving tangentially on a surface (always keeping + // that time of impact instead. That way, object moving tangentially on a surface (always keeping // a contact with it) won't report an useless impact. // - // Note that this must be done here instead of outside of the `nonlinear_time_of_impact` + // Note that this must be done here instead of outside of the `cast_shapes_nonlinear` // function so that this works properly with composite shapes. match mode { - NonlinearTOIMode::DirectionalTOI { + NonlinearShapeCastMode::Directional { sum_linear_thickness, max_angular_thickness, } => { - if (result.toi - start_time).abs() < 1.0e-5 { + if (result.time_of_impact - start_time).abs() < 1.0e-5 { handle_penetration_at_start_time( dispatcher, motion1, @@ -240,7 +242,7 @@ where Some(result) } } - NonlinearTOIMode::StopAtPenetration => Some(result), + NonlinearShapeCastMode::StopAtPenetration => Some(result), } } @@ -256,14 +258,14 @@ fn handle_penetration_at_start_time( end_time: Real, sum_linear_thickness: Real, max_angular_thickness: Real, -) -> Option +) -> Option where D: ?Sized + QueryDispatcher, SM1: ?Sized + SupportMap, SM2: ?Sized + SupportMap, { // Because we are doing non-linear CCD, we need an iterative methode here. - // First we need to check if the `toi = start_time` is legitimate, i.e., + // First we need to check if the `time_of_impact = start_time` is legitimate, i.e., // if tunnelling will happen if we don't clamp the motion. // // If the contact isn't "legitimate" (i.e. if we have a separating velocity), @@ -324,7 +326,7 @@ where // 1. Compute the relative velocity at that contact point. // 2. Check if this results in a potential tunnelling. - // 3. Use bisection to adjust the TOI to the time where a pair + // 3. Use bisection to adjust the shape-cast to the time where a pair // of contact points potentially causing tunneling hit for the first time. let r1 = contact.point1 - motion1.local_center; let r2 = contact.point2 - motion2.local_center; @@ -359,13 +361,13 @@ where if normal_vel * (end_time - next_time) > ccd_threshold { // dbg!("D1"); - let mut result = TOI { - toi: next_time, + let mut result = ShapeCastHit { + time_of_impact: next_time, witness1: contact.point1, witness2: contact.point2, normal1: contact.normal1, normal2: contact.normal2, - status: TOIStatus::Converged, + status: ShapeCastStatus::Converged, }; if contact.dist > 0.0 { @@ -388,9 +390,9 @@ where // TODO: the bisection isn't always enough here. We should check that we // still have a contact now. If not, we should run the loop from - // nonlinear_time_of_impact_support_map_support_map again from this + // cast_shapes_nonlinear_support_map_support_map again from this // point forward. - result.toi = new_range.curr_t; + result.time_of_impact = new_range.curr_t; } else { // dbg!("Bissecting points"); // This is an acceptable impact. Now determine when @@ -412,12 +414,12 @@ where // TODO: the bisection isn't always enough here. We should check that we // still have a contact now. If not, we should run the loop from - // nonlinear_time_of_impact_support_map_support_map again from this + // cast_shapes_nonlinear_support_map_support_map again from this // point forward. - result.toi = new_range.curr_t; + result.time_of_impact = new_range.curr_t; } - // println!("Fount new toi: {}", result.toi); + // println!("Fount new time_of_impact: {}", result.time_of_impact); return Some(result); } diff --git a/src/query/nonlinear_time_of_impact/mod.rs b/src/query/nonlinear_time_of_impact/mod.rs deleted file mode 100644 index 03e1937f..00000000 --- a/src/query/nonlinear_time_of_impact/mod.rs +++ /dev/null @@ -1,20 +0,0 @@ -//! Implementation details of the `nonlinear_time_of_impact` function. - -#[cfg(feature = "std")] -pub use self::nonlinear_time_of_impact_composite_shape_shape::{ - nonlinear_time_of_impact_composite_shape_shape, nonlinear_time_of_impact_shape_composite_shape, - NonlinearTOICompositeShapeShapeBestFirstVisitor, -}; -//pub use self::nonlinear_time_of_impact_halfspace_support_map::{nonlinear_time_of_impact_halfspace_support_map, nonlinear_time_of_impact_support_map_halfspace}; -pub use self::nonlinear_rigid_motion::NonlinearRigidMotion; -pub use self::nonlinear_time_of_impact::nonlinear_time_of_impact; -pub use self::nonlinear_time_of_impact_support_map_support_map::{ - nonlinear_time_of_impact_support_map_support_map, NonlinearTOIMode, -}; - -#[cfg(feature = "std")] -mod nonlinear_time_of_impact_composite_shape_shape; -//mod nonlinear_time_of_impact_halfspace_support_map; -mod nonlinear_rigid_motion; -mod nonlinear_time_of_impact; -mod nonlinear_time_of_impact_support_map_support_map; diff --git a/src/query/query_dispatcher.rs b/src/query/query_dispatcher.rs index 203e73f0..6bd55496 100644 --- a/src/query/query_dispatcher.rs +++ b/src/query/query_dispatcher.rs @@ -1,10 +1,11 @@ use crate::math::{Isometry, Real, Vector}; +use crate::query::details::ShapeCastOptions; #[cfg(feature = "std")] use crate::query::{ contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints}, ContactManifold, }; -use crate::query::{ClosestPoints, Contact, NonlinearRigidMotion, Unsupported, TOI}; +use crate::query::{ClosestPoints, Contact, NonlinearRigidMotion, ShapeCastHit, Unsupported}; use crate::shape::Shape; #[cfg(feature = "std")] @@ -97,19 +98,19 @@ pub trait QueryDispatcher: Send + Sync { /// - `pos12`: the position of the second shape relative to the first shape. /// - `local_vel12`: the relative velocity between the two shapes, expressed in the local-space /// of the first shape. In other world: `pos1.inverse() * (vel2 - vel1)`. - /// - `g1`: the first shape involved in the TOI computation. - /// - `g2`: the second shape involved in the TOI computation. - /// - `max_toi`: the maximum allowed TOI. This method returns `None` if the time-of-impact + /// - `g1`: the first shape involved in the shape-cast. + /// - `g2`: the second shape involved in the shape-cast. + /// - `target_dist`: a hit will be returned as soon as the two shapes get closer than `target_dist`. + /// - `max_time_of_impact`: the maximum allowed travel time. This method returns `None` if the time-of-impact /// detected is theater than this value. - fn time_of_impact( + fn cast_shapes( &self, pos12: &Isometry, local_vel12: &Vector, g1: &dyn Shape, g2: &dyn Shape, - max_toi: Real, - stop_at_penetration: bool, - ) -> Result, Unsupported>; + options: ShapeCastOptions, + ) -> Result, Unsupported>; /// Construct a `QueryDispatcher` that falls back on `other` for cases not handled by `self` fn chain(self, other: U) -> QueryDispatcherChain @@ -130,12 +131,12 @@ pub trait QueryDispatcher: Send + Sync { /// * `end_time` - The end time of the interval where the motion takes place. /// * `stop_at_penetration` - If the casted shape starts in a penetration state with any /// collider, two results are possible. If `stop_at_penetration` is `true` then, the - /// result will have a `toi` equal to `start_time`. If `stop_at_penetration` is `false` + /// result will have a `time_of_impact` equal to `start_time`. If `stop_at_penetration` is `false` /// then the nonlinear shape-casting will see if further motion wrt. the penetration normal /// would result in tunnelling. If it does not (i.e. we have a separating velocity along /// that normal) then the nonlinear shape-casting will attempt to find another impact, /// at a time `> start_time` that could result in tunnelling. - fn nonlinear_time_of_impact( + fn cast_shapes_nonlinear( &self, motion1: &NonlinearRigidMotion, g1: &dyn Shape, @@ -144,7 +145,7 @@ pub trait QueryDispatcher: Send + Sync { start_time: Real, end_time: Real, stop_at_penetration: bool, - ) -> Result, Unsupported>; + ) -> Result, Unsupported>; } /// The composition of two dispatchers @@ -187,16 +188,15 @@ where max_dist: Real, ) -> ClosestPoints); - chain_method!(time_of_impact( + chain_method!(cast_shapes( pos12: &Isometry, vel12: &Vector, g1: &dyn Shape, g2: &dyn Shape, - max_toi: Real, - stop_at_penetration: bool, - ) -> Option); + options: ShapeCastOptions, + ) -> Option); - chain_method!(nonlinear_time_of_impact( + chain_method!(cast_shapes_nonlinear( motion1: &NonlinearRigidMotion, g1: &dyn Shape, motion2: &NonlinearRigidMotion, @@ -204,7 +204,7 @@ where start_time: Real, end_time: Real, stop_at_penetration: bool, - ) -> Option); + ) -> Option); } #[cfg(feature = "std")] diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index e6a4adc2..d20bd478 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -68,18 +68,18 @@ impl Ray { )] pub struct RayIntersection { /// The time of impact of the ray with the object. The exact contact point can be computed - /// with: `ray.point_at(toi)` or equivalently `origin + dir * toi` where `origin` is the origin of the ray; - /// `dir` is its direction and `toi` is the value of this field. - pub toi: Real, + /// with: `ray.point_at(time_of_impact)` or equivalently `origin + dir * time_of_impact` where `origin` is the origin of the ray; + /// `dir` is its direction and `time_of_impact` is the value of this field. + pub time_of_impact: Real, /// The normal at the intersection point. /// - /// If the origin of the ray is inside of the shape and the shape is not solid, + /// If the origin of the ray is inside the shape and the shape is not solid, /// the normal will point towards the interior of the shape. /// Otherwise, the normal points outward. /// - /// If the `toi` is exactly zero, the normal might not be reliable. - // XXX: use a Unit instead. + /// If the `time_of_impact` is exactly zero, the normal might not be reliable. + // TODO: use a Unit instead. pub normal: Vector, /// Feature at the intersection point. @@ -90,9 +90,9 @@ impl RayIntersection { #[inline] /// Creates a new `RayIntersection`. #[cfg(feature = "dim3")] - pub fn new(toi: Real, normal: Vector, feature: FeatureId) -> RayIntersection { + pub fn new(time_of_impact: Real, normal: Vector, feature: FeatureId) -> RayIntersection { RayIntersection { - toi, + time_of_impact, normal, feature, } @@ -101,9 +101,9 @@ impl RayIntersection { #[inline] /// Creates a new `RayIntersection`. #[cfg(feature = "dim2")] - pub fn new(toi: Real, normal: Vector, feature: FeatureId) -> RayIntersection { + pub fn new(time_of_impact: Real, normal: Vector, feature: FeatureId) -> RayIntersection { RayIntersection { - toi, + time_of_impact, normal, feature, } @@ -112,7 +112,7 @@ impl RayIntersection { #[inline] pub fn transform_by(&self, transform: &Isometry) -> Self { RayIntersection { - toi: self.toi, + time_of_impact: self.time_of_impact, normal: transform * self.normal, feature: self.feature, } @@ -122,29 +122,35 @@ impl 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_toi: Real, solid: bool) -> Option { - self.cast_local_ray_and_get_normal(ray, max_toi, solid) - .map(|inter| inter.toi) + 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) + .map(|inter| inter.time_of_impact) } /// Computes the time of impact, and normal between this transformed shape and a ray. fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option; /// Tests whether a ray intersects this transformed shape. #[inline] - fn intersects_local_ray(&self, ray: &Ray, max_toi: Real) -> bool { - self.cast_local_ray(ray, max_toi, true).is_some() + 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() } /// Computes the time of impact between this transform shape and a ray. - fn cast_ray(&self, m: &Isometry, ray: &Ray, max_toi: Real, solid: bool) -> Option { + fn cast_ray( + &self, + m: &Isometry, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + ) -> Option { let ls_ray = ray.inverse_transform_by(m); - self.cast_local_ray(&ls_ray, max_toi, solid) + self.cast_local_ray(&ls_ray, max_time_of_impact, solid) } /// Computes the time of impact, and normal between this transformed shape and a ray. @@ -152,18 +158,18 @@ pub trait RayCast { &self, m: &Isometry, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { let ls_ray = ray.inverse_transform_by(m); - self.cast_local_ray_and_get_normal(&ls_ray, max_toi, solid) + self.cast_local_ray_and_get_normal(&ls_ray, max_time_of_impact, solid) .map(|inter| inter.transform_by(m)) } /// Tests whether a ray intersects this transformed shape. #[inline] - fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_toi: Real) -> bool { + fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_time_of_impact: Real) -> bool { let ls_ray = ray.inverse_transform_by(m); - self.intersects_local_ray(&ls_ray, max_toi) + self.intersects_local_ray(&ls_ray, max_time_of_impact) } } diff --git a/src/query/ray/ray_aabb.rs b/src/query/ray/ray_aabb.rs index 5b9bc91b..3cb4be40 100644 --- a/src/query/ray/ray_aabb.rs +++ b/src/query/ray/ray_aabb.rs @@ -9,9 +9,9 @@ use crate::shape::FeatureId; use num::Zero; impl RayCast for Aabb { - fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { let mut tmin: Real = 0.0; - let mut tmax: Real = max_toi; + let mut tmax: Real = max_time_of_impact; for i in 0usize..DIM { if ray.dir[i].is_zero() { @@ -52,10 +52,10 @@ impl RayCast for Aabb { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { - ray_aabb(self, ray, max_toi, solid).map(|(t, n, i)| { + ray_aabb(self, ray, max_time_of_impact, solid).map(|(t, n, i)| { let feature = if i < 0 { FeatureId::Face((-i) as u32 - 1 + 3) } else { @@ -70,7 +70,7 @@ impl RayCast for Aabb { fn ray_aabb( aabb: &Aabb, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option<(Real, Vector, isize)> { use crate::query::clip; @@ -78,12 +78,12 @@ fn ray_aabb( if near.0 < 0.0 { if solid { Some((0.0, na::zero(), far.2)) - } else if far.0 <= max_toi { + } else if far.0 <= max_time_of_impact { Some(far) } else { None } - } else if near.0 <= max_toi { + } else if near.0 <= max_time_of_impact { Some(near) } else { None diff --git a/src/query/ray/ray_ball.rs b/src/query/ray/ray_ball.rs index 335a4d46..1f8d0d0e 100644 --- a/src/query/ray/ray_ball.rs +++ b/src/query/ray/ray_ball.rs @@ -7,22 +7,22 @@ use num::Zero; impl RayCast for Ball { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { ray_toi_with_ball(&Point::origin(), self.radius, ray, solid) .1 - .filter(|toi| *toi <= max_toi) + .filter(|time_of_impact| *time_of_impact <= max_time_of_impact) } #[inline] fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { ray_toi_and_normal_with_ball(&Point::origin(), self.radius, ray, solid) .1 - .filter(|int| int.toi <= max_toi) + .filter(|int| int.time_of_impact <= max_time_of_impact) } } diff --git a/src/query/ray/ray_bounding_sphere.rs b/src/query/ray/ray_bounding_sphere.rs index d240a4d1..d4beec2d 100644 --- a/src/query/ray/ray_bounding_sphere.rs +++ b/src/query/ray/ray_bounding_sphere.rs @@ -5,25 +5,29 @@ use crate::shape::Ball; impl RayCast for BoundingSphere { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { let centered_ray = ray.translate_by(-self.center().coords); - Ball::new(self.radius()).cast_local_ray(¢ered_ray, max_toi, solid) + Ball::new(self.radius()).cast_local_ray(¢ered_ray, max_time_of_impact, solid) } #[inline] fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { let centered_ray = ray.translate_by(-self.center().coords); - Ball::new(self.radius()).cast_local_ray_and_get_normal(¢ered_ray, max_toi, solid) + Ball::new(self.radius()).cast_local_ray_and_get_normal( + ¢ered_ray, + max_time_of_impact, + solid, + ) } #[inline] - fn intersects_local_ray(&self, ray: &Ray, max_toi: Real) -> bool { + fn intersects_local_ray(&self, ray: &Ray, max_time_of_impact: Real) -> bool { let centered_ray = ray.translate_by(-self.center().coords); - Ball::new(self.radius()).intersects_local_ray(¢ered_ray, max_toi) + Ball::new(self.radius()).intersects_local_ray(¢ered_ray, max_time_of_impact) } } diff --git a/src/query/ray/ray_composite_shape.rs b/src/query/ray/ray_composite_shape.rs index 06188d0f..873dd32e 100644 --- a/src/query/ray/ray_composite_shape.rs +++ b/src/query/ray/ray_composite_shape.rs @@ -7,8 +7,9 @@ use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; impl RayCast for TriMesh { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { - let mut visitor = RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_toi, solid); + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + let mut visitor = + RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_time_of_impact, solid); self.qbvh() .traverse_best_first(&mut visitor) @@ -19,11 +20,15 @@ impl RayCast for TriMesh { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { - let mut visitor = - RayCompositeShapeToiAndNormalBestFirstVisitor::new(self, ray, max_toi, solid); + let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( + self, + ray, + max_time_of_impact, + solid, + ); self.qbvh() .traverse_best_first(&mut visitor) @@ -42,8 +47,9 @@ impl RayCast for TriMesh { impl RayCast for Polyline { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { - let mut visitor = RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_toi, solid); + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + let mut visitor = + RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_time_of_impact, solid); self.qbvh() .traverse_best_first(&mut visitor) @@ -54,11 +60,15 @@ impl RayCast for Polyline { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { - let mut visitor = - RayCompositeShapeToiAndNormalBestFirstVisitor::new(self, ray, max_toi, solid); + let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( + self, + ray, + max_time_of_impact, + solid, + ); self.qbvh() .traverse_best_first(&mut visitor) @@ -68,8 +78,9 @@ impl RayCast for Polyline { impl RayCast for Compound { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { - let mut visitor = RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_toi, solid); + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + let mut visitor = + RayCompositeShapeToiBestFirstVisitor::new(self, ray, max_time_of_impact, solid); self.qbvh() .traverse_best_first(&mut visitor) @@ -80,11 +91,15 @@ impl RayCast for Compound { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { - let mut visitor = - RayCompositeShapeToiAndNormalBestFirstVisitor::new(self, ray, max_toi, solid); + let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( + self, + ray, + max_time_of_impact, + solid, + ); self.qbvh() .traverse_best_first(&mut visitor) @@ -100,18 +115,18 @@ pub struct RayCompositeShapeToiBestFirstVisitor<'a, S> { shape: &'a S, ray: &'a Ray, simd_ray: SimdRay, - max_toi: Real, + max_time_of_impact: Real, solid: bool, } impl<'a, S> RayCompositeShapeToiBestFirstVisitor<'a, S> { /// Initialize a visitor for casting a ray on a composite shape. - pub fn new(shape: &'a S, ray: &'a Ray, max_toi: Real, solid: bool) -> Self { + pub fn new(shape: &'a S, ray: &'a Ray, max_time_of_impact: Real, solid: bool) -> Self { Self { shape, ray, simd_ray: SimdRay::splat(*ray), - max_toi, + max_time_of_impact, solid, } } @@ -131,14 +146,15 @@ where aabb: &SimdAabb, data: Option<[Option<&S::PartId>; SIMD_WIDTH]>, ) -> SimdBestFirstVisitStatus { - let (hit, toi) = aabb.cast_local_ray(&self.simd_ray, SimdReal::splat(self.max_toi)); + let (hit, time_of_impact) = + aabb.cast_local_ray(&self.simd_ray, SimdReal::splat(self.max_time_of_impact)); if let Some(data) = data { let mut weights = [0.0; SIMD_WIDTH]; let mut mask = [false; SIMD_WIDTH]; let mut results = [None; SIMD_WIDTH]; - let better_toi = toi.simd_lt(SimdReal::splat(best)); + let better_toi = time_of_impact.simd_lt(SimdReal::splat(best)); let bitmask = (hit & better_toi).bitmask(); for ii in 0..SIMD_WIDTH { @@ -146,15 +162,24 @@ where let part_id = *data[ii].unwrap(); self.shape .map_typed_part_at(part_id, |part_pos, part_shape, _| { - let toi = if let Some(part_pos) = part_pos { - part_shape.cast_ray(part_pos, self.ray, self.max_toi, self.solid) + let time_of_impact = if let Some(part_pos) = part_pos { + part_shape.cast_ray( + part_pos, + self.ray, + self.max_time_of_impact, + self.solid, + ) } else { - part_shape.cast_local_ray(self.ray, self.max_toi, self.solid) + part_shape.cast_local_ray( + self.ray, + self.max_time_of_impact, + self.solid, + ) }; - if let Some(toi) = toi { - results[ii] = Some((part_id, toi)); + if let Some(time_of_impact) = time_of_impact { + results[ii] = Some((part_id, time_of_impact)); mask[ii] = true; - weights[ii] = toi; + weights[ii] = time_of_impact; } }) } @@ -167,7 +192,7 @@ where } } else { SimdBestFirstVisitStatus::MaybeContinue { - weights: toi, + weights: time_of_impact, mask: hit, results: [None; SIMD_WIDTH], } @@ -180,18 +205,18 @@ pub struct RayCompositeShapeToiAndNormalBestFirstVisitor<'a, S> { shape: &'a S, ray: &'a Ray, simd_ray: SimdRay, - max_toi: Real, + max_time_of_impact: Real, solid: bool, } impl<'a, S> RayCompositeShapeToiAndNormalBestFirstVisitor<'a, S> { /// Initialize a visitor for casting a ray on a composite shape. - pub fn new(shape: &'a S, ray: &'a Ray, max_toi: Real, solid: bool) -> Self { + pub fn new(shape: &'a S, ray: &'a Ray, max_time_of_impact: Real, solid: bool) -> Self { Self { shape, ray, simd_ray: SimdRay::splat(*ray), - max_toi, + max_time_of_impact, solid, } } @@ -211,14 +236,15 @@ where aabb: &SimdAabb, data: Option<[Option<&S::PartId>; SIMD_WIDTH]>, ) -> SimdBestFirstVisitStatus { - let (hit, toi) = aabb.cast_local_ray(&self.simd_ray, SimdReal::splat(self.max_toi)); + let (hit, time_of_impact) = + aabb.cast_local_ray(&self.simd_ray, SimdReal::splat(self.max_time_of_impact)); if let Some(data) = data { let mut weights = [0.0; SIMD_WIDTH]; let mut mask = [false; SIMD_WIDTH]; let mut results = [None; SIMD_WIDTH]; - let better_toi = toi.simd_lt(SimdReal::splat(best)); + let better_toi = time_of_impact.simd_lt(SimdReal::splat(best)); let bitmask = (hit & better_toi).bitmask(); for ii in 0..SIMD_WIDTH { @@ -229,13 +255,13 @@ where part_shape.cast_ray_and_get_normal( part_pos, self.ray, - self.max_toi, + self.max_time_of_impact, self.solid, ) } else { part_shape.cast_local_ray_and_get_normal( self.ray, - self.max_toi, + self.max_time_of_impact, self.solid, ) }; @@ -243,7 +269,7 @@ where if let Some(result) = result { results[ii] = Some((*data[ii].unwrap(), result)); mask[ii] = true; - weights[ii] = result.toi; + weights[ii] = result.time_of_impact; } }); } @@ -256,7 +282,7 @@ where } } else { SimdBestFirstVisitStatus::MaybeContinue { - weights: toi, + weights: time_of_impact, mask: hit, results: [None; SIMD_WIDTH], } diff --git a/src/query/ray/ray_cuboid.rs b/src/query/ray/ray_cuboid.rs index d087a822..335da927 100644 --- a/src/query/ray/ray_cuboid.rs +++ b/src/query/ray/ray_cuboid.rs @@ -5,21 +5,21 @@ use crate::shape::Cuboid; impl RayCast for Cuboid { #[inline] - fn cast_local_ray(&self, ray: &Ray, max_toi: Real, solid: bool) -> Option { + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { let dl = Point::from(-self.half_extents); let ur = Point::from(self.half_extents); - Aabb::new(dl, ur).cast_local_ray(ray, max_toi, solid) + Aabb::new(dl, ur).cast_local_ray(ray, max_time_of_impact, solid) } #[inline] fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> 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_toi, solid) + Aabb::new(dl, ur).cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) } } diff --git a/src/query/ray/ray_halfspace.rs b/src/query/ray/ray_halfspace.rs index ae154037..5ec27df4 100644 --- a/src/query/ray/ray_halfspace.rs +++ b/src/query/ray/ray_halfspace.rs @@ -4,7 +4,7 @@ use crate::math::{Point, Real, Vector}; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::{FeatureId, HalfSpace}; -/// Computes the toi of an unbounded line with a halfspace described by its center and normal. +/// Computes the time_of_impact of an unbounded line with a halfspace described by its center and normal. #[inline] pub fn line_toi_with_halfspace( halfspace_center: &Point, @@ -22,7 +22,7 @@ pub fn line_toi_with_halfspace( } } -/// Computes the toi of a ray with a halfspace described by its center and normal. +/// Computes the time_of_impact of a ray with a halfspace described by its center and normal. #[inline] pub fn ray_toi_with_halfspace( center: &Point, @@ -43,7 +43,7 @@ impl RayCast for HalfSpace { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { let dpos = -ray.origin; @@ -57,7 +57,7 @@ impl RayCast for HalfSpace { let t = dot_normal_dpos / self.normal.dot(&ray.dir); - if t >= 0.0 && t <= max_toi { + if t >= 0.0 && t <= max_time_of_impact { let n = if dot_normal_dpos > 0.0 { -self.normal } else { diff --git a/src/query/ray/ray_heightfield.rs b/src/query/ray/ray_heightfield.rs index da313acb..869f2a51 100644 --- a/src/query/ray/ray_heightfield.rs +++ b/src/query/ray/ray_heightfield.rs @@ -12,17 +12,17 @@ impl RayCast for HeightField { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, _: bool, ) -> Option { let aabb = self.local_aabb(); let (min_t, mut max_t) = aabb.clip_ray_parameters(ray)?; - if min_t > max_toi { + if min_t > max_time_of_impact { return None; } - max_t = max_t.min(max_toi); + max_t = max_t.min(max_time_of_impact); let clip_ray_a = ray.point_at(min_t); @@ -100,7 +100,7 @@ impl RayCast for HeightField { &seg.scaled_direction(), ); - if t >= 0.0 && t <= 1.0 && s <= max_toi { + if t >= 0.0 && t <= 1.0 && s <= max_time_of_impact { let n = seg.normal().unwrap().into_inner(); let fid = if n.dot(&ray.dir) > 0.0 { // The ray hit the back face. @@ -124,14 +124,14 @@ impl RayCast for HeightField { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { use num_traits::Bounded; let aabb = self.local_aabb(); let (min_t, mut max_t) = aabb.clip_ray_parameters(ray)?; - max_t = max_t.min(max_toi); + max_t = max_t.min(max_time_of_impact); let clip_ray_a = ray.point_at(min_t); let mut cell = match self.cell_at_point(&clip_ray_a) { Some(cell) => cell, @@ -157,14 +157,14 @@ impl RayCast for HeightField { 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_toi, solid)); + .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_toi, solid)); + .and_then(|tri| tri.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid)); match (inter1, inter2) { (Some(mut inter1), Some(mut inter2)) => { - if inter1.toi < inter2.toi { + if inter1.time_of_impact < inter2.time_of_impact { inter1.feature = self.convert_triangle_feature_id(cell.0, cell.1, true, inter1.feature); return Some(inter1); diff --git a/src/query/ray/ray_round_shape.rs b/src/query/ray/ray_round_shape.rs index ab99f60b..b37d9cdd 100644 --- a/src/query/ray/ray_round_shape.rs +++ b/src/query/ray/ray_round_shape.rs @@ -7,14 +7,14 @@ impl RayCast for RoundShape { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { crate::query::details::local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, - max_toi, + max_time_of_impact, solid, ) } diff --git a/src/query/ray/ray_support_map.rs b/src/query/ray/ray_support_map.rs index 8db3e8bd..944ea4ff 100644 --- a/src/query/ray/ray_support_map.rs +++ b/src/query/ray/ray_support_map.rs @@ -22,7 +22,7 @@ pub fn local_ray_intersection_with_support_map_with_params( shape: &G, simplex: &mut VoronoiSimplex, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option where @@ -31,11 +31,11 @@ where 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_toi); + let inter = gjk::cast_local_ray(shape, simplex, ray, max_time_of_impact); if !solid { - inter.and_then(|(toi, normal)| { - if toi.is_zero() { + inter.and_then(|(time_of_impact, normal)| { + if time_of_impact.is_zero() { // the ray is inside of the shape. let ndir = ray.dir.normalize(); let supp = shape.local_support_point(&ndir); @@ -47,11 +47,11 @@ where simplex.reset(CSOPoint::single_point(supp - new_ray.origin.coords)); gjk::cast_local_ray(shape, simplex, &new_ray, shift + eps).and_then( - |(toi, outward_normal)| { - let toi = shift - toi; - if toi <= max_toi { + |(time_of_impact, outward_normal)| { + let time_of_impact = shift - time_of_impact; + if time_of_impact <= max_time_of_impact { Some(RayIntersection::new( - toi, + time_of_impact, -outward_normal, FeatureId::Unknown, )) @@ -61,11 +61,17 @@ where }, ) } else { - Some(RayIntersection::new(toi, normal, FeatureId::Unknown)) + Some(RayIntersection::new( + time_of_impact, + normal, + FeatureId::Unknown, + )) } }) } else { - inter.map(|(toi, normal)| RayIntersection::new(toi, normal, FeatureId::Unknown)) + inter.map(|(time_of_impact, normal)| { + RayIntersection::new(time_of_impact, normal, FeatureId::Unknown) + }) } } @@ -74,14 +80,14 @@ impl RayCast for Cylinder { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, - max_toi, + max_time_of_impact, solid, ) } @@ -92,14 +98,14 @@ impl RayCast for Cone { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, - max_toi, + max_time_of_impact, solid, ) } @@ -109,14 +115,14 @@ impl RayCast for Capsule { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, - max_toi, + max_time_of_impact, solid, ) } @@ -128,14 +134,14 @@ impl RayCast for ConvexPolyhedron { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, - max_toi, + max_time_of_impact, solid, ) } @@ -147,14 +153,14 @@ impl RayCast for ConvexPolygon { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), ray, - max_toi, + max_time_of_impact, solid, ) } @@ -165,7 +171,7 @@ impl RayCast for Segment { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { #[cfg(feature = "dim2")] @@ -195,11 +201,15 @@ impl RayCast for Segment { match (dist1 >= 0.0, dist2 >= 0.0) { (true, true) => { - let toi = dist1.min(dist2) / ray.dir.norm_squared(); - if toi > max_toi { + let time_of_impact = dist1.min(dist2) / ray.dir.norm_squared(); + if time_of_impact > max_time_of_impact { None } else if dist1 <= dist2 { - Some(RayIntersection::new(toi, normal, FeatureId::Vertex(0))) + Some(RayIntersection::new( + time_of_impact, + normal, + FeatureId::Vertex(0), + )) } else { Some(RayIntersection::new( dist2 / ray.dir.norm_squared(), @@ -221,7 +231,7 @@ impl RayCast for Segment { // The rays never intersect. None } - } else if s >= 0.0 && s <= max_toi && t >= 0.0 && t <= 1.0 { + } else if s >= 0.0 && s <= max_time_of_impact && t >= 0.0 && t <= 1.0 { let normal = self.normal().map(|n| *n).unwrap_or_else(Vector::zeros); if normal.dot(&ray.dir) > 0.0 { @@ -242,7 +252,7 @@ impl RayCast for Segment { self, &mut VoronoiSimplex::new(), ray, - max_toi, + max_time_of_impact, solid, ) } diff --git a/src/query/ray/ray_triangle.rs b/src/query/ray/ray_triangle.rs index 2de221aa..602bab05 100644 --- a/src/query/ray/ray_triangle.rs +++ b/src/query/ray/ray_triangle.rs @@ -15,7 +15,7 @@ impl RayCast for Triangle { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, solid: bool, ) -> Option { let edges = self.edges(); @@ -35,9 +35,10 @@ 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_toi, solid) { - if inter.toi < smallest_toi { - smallest_toi = inter.toi; + if let Some(inter) = edge.cast_local_ray_and_get_normal(ray, max_time_of_impact, solid) + { + if inter.time_of_impact < smallest_toi { + smallest_toi = inter.time_of_impact; best = Some(inter); } } @@ -51,12 +52,12 @@ impl RayCast for Triangle { fn cast_local_ray_and_get_normal( &self, ray: &Ray, - max_toi: Real, + max_time_of_impact: Real, _: bool, ) -> Option { let inter = local_ray_intersection_with_triangle(&self.a, &self.b, &self.c, ray)?.0; - if inter.toi <= max_toi { + if inter.time_of_impact <= max_time_of_impact { Some(inter) } else { None @@ -106,7 +107,7 @@ pub fn local_ray_intersection_with_triangle( let mut v; let mut w; - let toi; + let time_of_impact; let normal; if t < 0.0 { @@ -123,7 +124,7 @@ pub fn local_ray_intersection_with_triangle( } let invd = 1.0 / d; - toi = -t * invd; + time_of_impact = -t * invd; normal = -n.normalize(); v *= invd; w *= invd; @@ -141,14 +142,14 @@ pub fn local_ray_intersection_with_triangle( } let invd = 1.0 / d; - toi = t * invd; + time_of_impact = t * invd; normal = n.normalize(); v *= invd; w *= invd; } Some(( - RayIntersection::new(toi, normal, FeatureId::Face(fid)), + RayIntersection::new(time_of_impact, normal, FeatureId::Face(fid)), Vector3::new(-v - w + 1.0, v, w), )) } diff --git a/src/query/shape_cast/mod.rs b/src/query/shape_cast/mod.rs new file mode 100644 index 00000000..95a82520 --- /dev/null +++ b/src/query/shape_cast/mod.rs @@ -0,0 +1,26 @@ +//! Implementation details of the `cast_shapes` function. + +pub use self::shape_cast::{cast_shapes, 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, +}; +#[cfg(feature = "std")] +pub use self::{ + shape_cast_composite_shape_shape::{ + cast_shapes_composite_shape_shape, cast_shapes_shape_composite_shape, + TOICompositeShapeShapeBestFirstVisitor, + }, + shape_cast_heightfield_shape::{cast_shapes_heightfield_shape, cast_shapes_shape_heightfield}, + shape_cast_support_map_support_map::cast_shapes_support_map_support_map, +}; + +mod shape_cast; +mod shape_cast_ball_ball; +#[cfg(feature = "std")] +mod shape_cast_composite_shape_shape; +mod shape_cast_halfspace_support_map; +#[cfg(feature = "std")] +mod shape_cast_heightfield_shape; +#[cfg(feature = "std")] +mod shape_cast_support_map_support_map; diff --git a/src/query/shape_cast/shape_cast.rs b/src/query/shape_cast/shape_cast.rs new file mode 100644 index 00000000..1af87c10 --- /dev/null +++ b/src/query/shape_cast/shape_cast.rs @@ -0,0 +1,152 @@ +use na::Unit; + +use crate::math::{Isometry, Point, Real, Vector}; +use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; +use crate::shape::Shape; + +/// The status of the time-of-impact computation algorithm. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub enum ShapeCastStatus { + /// The shape-casting algorithm ran out of iterations before achieving convergence. + /// + /// The content of the `ShapeCastHit` will still be a conservative approximation of the actual result so + /// it is often fine to interpret this case as a success. + OutOfIterations, + /// The shape-casting algorithm converged successfully. + Converged, + /// Something went wrong during the shape-casting, likely due to numerical instabilities. + /// + /// The content of the `ShapeCastHit` will still be a conservative approximation of the actual result so + /// it is often fine to interpret this case as a success. + Failed, + /// The two shape already overlap, or are separated by a distance smaller than + /// [`ShapeCastOptions::target_dist`] at the time 0. + /// + /// The witness points and normals provided by the `ShapeCastHit` will have unreliable values unless + /// [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `true` when calling + /// the time-of-impact function. + PenetratingOrWithinTargetDist, +} + +/// The result of a shape casting.. +#[derive(Copy, Clone, Debug)] +pub struct ShapeCastHit { + /// The time at which the objects touch. + pub time_of_impact: Real, + /// The local-space closest point on the first shape at the time of impact. + /// + /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] + /// and [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false`. + pub witness1: Point, + /// The local-space closest point on the second shape at the time of impact. + /// + /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] + /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false` + /// when calling the time-of-impact function. + pub witness2: Point, + /// The local-space outward normal on the first shape at the time of impact. + /// + /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] + /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false` + /// when calling the time-of-impact function. + pub normal1: Unit>, + /// The local-space outward normal on the second shape at the time of impact. + /// + /// This value is unreliable if `status` is [`ShapeCastStatus::PenetratingOrWithinTargetDist`] + /// and both [`ShapeCastOptions::compute_impact_geometry_on_penetration`] was set to `false` + /// when calling the time-of-impact function. + pub normal2: Unit>, + /// The way the shape-casting algorithm terminated. + pub status: ShapeCastStatus, +} + +impl ShapeCastHit { + /// Swaps every data of this shape-casting result such that the role of both shapes are swapped. + /// + /// In practice, this makes it so that `self.witness1` and `self.normal1` are swapped with + /// `self.witness2` and `self.normal2`. + pub fn swapped(self) -> Self { + Self { + time_of_impact: self.time_of_impact, + witness1: self.witness2, + witness2: self.witness1, + normal1: self.normal2, + normal2: self.normal1, + status: self.status, + } + } + + /// Transform `self.witness1` and `self.normal1` by `pos`. + pub fn transform1_by(&self, pos: &Isometry) -> Self { + Self { + time_of_impact: self.time_of_impact, + witness1: pos * self.witness1, + witness2: self.witness2, + normal1: pos * self.normal1, + normal2: self.normal2, + status: self.status, + } + } +} + +/// Configuration for controlling the behavior of time-of-impact (i.e. shape-casting) calculations. +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct ShapeCastOptions { + /// The maximum time-of-impacts that can be computed. + /// + /// Any impact ocurring after this time will be ignored. + pub max_time_of_impact: Real, + /// The shapes will be considered as impacting as soon as their distance is smaller or + /// equal to this target distance. Must be positive or zero. + /// + /// If the shapes are separated by a distance smaller than `target_distance` at time 0, the + /// calculated witness points and normals are only reliable if + /// [`Self::compute_impact_geometry_on_penetration`] is set to `true`. + pub target_distance: Real, + /// If `false`, the time-of-impact algorithm will automatically discard any impact at time + /// 0 where the velocity is separating (i.e., the relative velocity is such that the distance + /// between the objects projected on the impact normal is increasing through time). + pub stop_at_penetration: bool, + /// If `true`, witness points and normals will be calculated even when the time-of-impact is 0. + pub compute_impact_geometry_on_penetration: bool, +} + +impl ShapeCastOptions { + // Constructor for the most common use-case. + /// Crates a [`ShapeCastOptions`] with the default values except for the maximum time of impact. + pub fn with_max_time_of_impact(max_time_of_impact: Real) -> Self { + Self { + max_time_of_impact, + ..Default::default() + } + } +} + +impl Default for ShapeCastOptions { + fn default() -> Self { + Self { + max_time_of_impact: Real::MAX, + target_distance: 0.0, + stop_at_penetration: true, + compute_impact_geometry_on_penetration: true, + } + } +} + +/// 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 `target_dist`, or penetrating. +pub fn cast_shapes( + pos1: &Isometry, + vel1: &Vector, + g1: &dyn Shape, + pos2: &Isometry, + vel2: &Vector, + g2: &dyn Shape, + options: ShapeCastOptions, +) -> Result, Unsupported> { + let pos12 = pos1.inv_mul(pos2); + let vel12 = pos1.inverse_transform_vector(&(vel2 - vel1)); + DefaultQueryDispatcher.cast_shapes(&pos12, &vel12, g1, g2, options) +} diff --git a/src/query/time_of_impact/time_of_impact_ball_ball.rs b/src/query/shape_cast/shape_cast_ball_ball.rs similarity index 66% rename from src/query/time_of_impact/time_of_impact_ball_ball.rs rename to src/query/shape_cast/shape_cast_ball_ball.rs index 6420ce2b..f41744e6 100644 --- a/src/query/time_of_impact/time_of_impact_ball_ball.rs +++ b/src/query/shape_cast/shape_cast_ball_ball.rs @@ -1,30 +1,33 @@ use na::Unit; use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::{self, Ray, TOIStatus, TOI}; +use crate::query::details::ShapeCastOptions; +use crate::query::{self, Ray, ShapeCastHit, ShapeCastStatus}; use crate::shape::Ball; use num::Zero; /// Time Of Impact of two balls under translational movement. #[inline] -pub fn time_of_impact_ball_ball( +pub fn cast_shapes_ball_ball( pos12: &Isometry, vel12: &Vector, b1: &Ball, b2: &Ball, - max_toi: Real, -) -> Option { - let rsum = b1.radius + b2.radius; + options: ShapeCastOptions, +) -> Option { + let rsum = b1.radius + b2.radius + options.target_distance; let radius = rsum; let center = Point::from(-pos12.translation.vector); let ray = Ray::new(Point::origin(), *vel12); - if let (inside, Some(toi)) = query::details::ray_toi_with_ball(¢er, radius, &ray, true) { - if toi > max_toi { + if let (inside, Some(time_of_impact)) = + query::details::ray_toi_with_ball(¢er, radius, &ray, true) + { + if time_of_impact > options.max_time_of_impact { return None; } - let dpt = ray.point_at(toi) - center; + let dpt = ray.point_at(time_of_impact) - center; let normal1; let normal2; let witness1; @@ -43,13 +46,13 @@ pub fn time_of_impact_ball_ball( } let status = if inside && center.coords.norm_squared() < rsum * rsum { - TOIStatus::Penetrating + ShapeCastStatus::PenetratingOrWithinTargetDist } else { - TOIStatus::Converged + ShapeCastStatus::Converged }; - Some(TOI { - toi, + Some(ShapeCastHit { + time_of_impact, normal1, normal2, witness1, diff --git a/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs b/src/query/shape_cast/shape_cast_composite_shape_shape.rs similarity index 68% rename from src/query/time_of_impact/time_of_impact_composite_shape_shape.rs rename to src/query/shape_cast/shape_cast_composite_shape_shape.rs index 420365a3..915a5e6d 100644 --- a/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs +++ b/src/query/shape_cast/shape_cast_composite_shape_shape.rs @@ -1,62 +1,53 @@ use crate::bounding_volume::SimdAabb; use crate::math::{Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; -use crate::query::{QueryDispatcher, Ray, SimdRay, TOI}; +use crate::query::shape_cast::ShapeCastOptions; +use crate::query::{QueryDispatcher, Ray, ShapeCastHit, SimdRay}; use crate::shape::{Shape, TypedSimdCompositeShape}; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; /// Time Of Impact of a composite shape with any other shape, under translational movement. -pub fn time_of_impact_composite_shape_shape( +pub fn cast_shapes_composite_shape_shape( dispatcher: &D, pos12: &Isometry, vel12: &Vector, g1: &G1, g2: &dyn Shape, - max_toi: Real, - stop_at_penetration: bool, -) -> Option + options: ShapeCastOptions, +) -> Option where D: QueryDispatcher, G1: TypedSimdCompositeShape, { - let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( - dispatcher, - pos12, - vel12, - g1, - g2, - max_toi, - stop_at_penetration, - ); + let mut visitor = + TOICompositeShapeShapeBestFirstVisitor::new(dispatcher, pos12, vel12, g1, g2, options); g1.typed_qbvh() .traverse_best_first(&mut visitor) .map(|res| res.1 .1) } /// Time Of Impact of any shape with a composite shape, under translational movement. -pub fn time_of_impact_shape_composite_shape( +pub fn cast_shapes_shape_composite_shape( dispatcher: &D, pos12: &Isometry, vel12: &Vector, g1: &dyn Shape, g2: &G2, - max_toi: Real, - stop_at_penetration: bool, -) -> Option + options: ShapeCastOptions, +) -> Option where D: QueryDispatcher, G2: TypedSimdCompositeShape, { - time_of_impact_composite_shape_shape( + cast_shapes_composite_shape_shape( dispatcher, &pos12.inverse(), &-pos12.inverse_transform_vector(vel12), g2, g1, - max_toi, - stop_at_penetration, + options, ) - .map(|toi| toi.swapped()) + .map(|time_of_impact| time_of_impact.swapped()) } /// A visitor used to find the time-of-impact between a composite shape and a shape. @@ -70,8 +61,7 @@ pub struct TOICompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?Sized + 'a vel12: &'a Vector, g1: &'a G1, g2: &'a dyn Shape, - max_toi: Real, - stop_at_penetration: bool, + options: ShapeCastOptions, } impl<'a, D: ?Sized, G1: ?Sized> TOICompositeShapeShapeBestFirstVisitor<'a, D, G1> @@ -86,8 +76,7 @@ where vel12: &'a Vector, g1: &'a G1, g2: &'a dyn Shape, - max_toi: Real, - stop_at_penetration: bool, + options: ShapeCastOptions, ) -> TOICompositeShapeShapeBestFirstVisitor<'a, D, G1> { let ls_aabb2 = g2.compute_aabb(pos12); let ray = Ray::new(Point::origin(), *vel12); @@ -95,14 +84,15 @@ where TOICompositeShapeShapeBestFirstVisitor { dispatcher, msum_shift: Vector::splat(-ls_aabb2.center().coords), - msum_margin: Vector::splat(ls_aabb2.half_extents()), + msum_margin: Vector::splat( + ls_aabb2.half_extents() + Vector::repeat(options.target_distance), + ), ray: SimdRay::splat(ray), pos12, vel12, g1, g2, - max_toi, - stop_at_penetration, + options, } } } @@ -113,7 +103,7 @@ where D: QueryDispatcher, G1: TypedSimdCompositeShape, { - type Result = (G1::PartId, TOI); + type Result = (G1::PartId, ShapeCastHit); #[inline] fn visit( @@ -128,11 +118,12 @@ where maxs: bv.maxs + self.msum_shift + self.msum_margin, }; - // Compute the TOI. - let (mask, toi) = msum.cast_local_ray(&self.ray, SimdReal::splat(self.max_toi)); + // Compute the time of impact. + let (mask, time_of_impact) = + msum.cast_local_ray(&self.ray, SimdReal::splat(self.options.max_time_of_impact)); if let Some(data) = data { - let better_toi = toi.simd_lt(SimdReal::splat(best)); + let better_toi = time_of_impact.simd_lt(SimdReal::splat(best)); let bitmask = (mask & better_toi).bitmask(); let mut weights = [0.0; SIMD_WIDTH]; let mut mask = [false; SIMD_WIDTH]; @@ -141,40 +132,32 @@ where for ii in 0..SIMD_WIDTH { if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { let part_id = *data[ii].unwrap(); - let mut toi = None; + let mut hit = None; self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { if let Some(part_pos1) = part_pos1 { - toi = self + hit = self .dispatcher - .time_of_impact( + .cast_shapes( &part_pos1.inv_mul(self.pos12), &part_pos1.inverse_transform_vector(self.vel12), g1, self.g2, - self.max_toi, - self.stop_at_penetration, + self.options, ) .unwrap_or(None) - .map(|toi| toi.transform1_by(part_pos1)); + .map(|hit| hit.transform1_by(part_pos1)); } else { - toi = self + hit = self .dispatcher - .time_of_impact( - self.pos12, - self.vel12, - g1, - self.g2, - self.max_toi, - self.stop_at_penetration, - ) + .cast_shapes(self.pos12, self.vel12, g1, self.g2, self.options) .unwrap_or(None); } }); - if let Some(toi) = toi { - results[ii] = Some((part_id, toi)); - mask[ii] = toi.toi < best; - weights[ii] = toi.toi; + if let Some(hit) = hit { + results[ii] = Some((part_id, hit)); + mask[ii] = hit.time_of_impact < best; + weights[ii] = hit.time_of_impact; } } } @@ -186,7 +169,7 @@ where } } else { SimdBestFirstVisitStatus::MaybeContinue { - weights: toi, + weights: time_of_impact, mask, results: [None; SIMD_WIDTH], } diff --git a/src/query/shape_cast/shape_cast_halfspace_support_map.rs b/src/query/shape_cast/shape_cast_halfspace_support_map.rs new file mode 100644 index 00000000..8d866cd2 --- /dev/null +++ b/src/query/shape_cast/shape_cast_halfspace_support_map.rs @@ -0,0 +1,84 @@ +use crate::math::{Isometry, Real, Vector}; +use crate::query::details::ShapeCastOptions; +use crate::query::{Ray, RayCast, ShapeCastHit, ShapeCastStatus}; +use crate::shape::{HalfSpace, RoundShapeRef, SupportMap}; + +/// Time Of Impact of a halfspace with a support-mapped shape under translational movement. +pub fn cast_shapes_halfspace_support_map( + pos12: &Isometry, + vel12: &Vector, + halfspace: &HalfSpace, + other: &G, + options: ShapeCastOptions, +) -> Option +where + G: SupportMap, +{ + // TODO: add method to get only the local support point. + // This would avoid the `inverse_transform_point` later. + if !options.stop_at_penetration && vel12.dot(&halfspace.normal) > 0.0 { + return None; + } + + let support_point = if options.target_distance > 0.0 { + let round_other = RoundShapeRef { + inner_shape: other, + border_radius: options.target_distance, + }; + round_other.support_point(pos12, &-halfspace.normal) + } else { + other.support_point(pos12, &-halfspace.normal) + }; + 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 time_of_impact > options.max_time_of_impact { + return None; + } + + let witness2 = support_point + *halfspace.normal * options.target_distance; + let mut witness1 = ray.point_at(time_of_impact); + // Project the witness point to the halfspace. + // Note that witness1 is already in the halfspace's local-space. + witness1 -= *halfspace.normal * witness1.coords.dot(&halfspace.normal); + + let status = if support_point.coords.dot(&halfspace.normal) < 0.0 { + ShapeCastStatus::PenetratingOrWithinTargetDist + } else { + ShapeCastStatus::Converged + }; + + Some(ShapeCastHit { + time_of_impact, + normal1: halfspace.normal, + normal2: pos12.inverse_transform_unit_vector(&-halfspace.normal), + witness1, + witness2: pos12.inverse_transform_point(&witness2), + status, + }) + } else { + None + } +} + +/// Time Of Impact of a halfspace with a support-mapped shape under translational movement. +pub fn cast_shapes_support_map_halfspace( + pos12: &Isometry, + vel12: &Vector, + other: &G, + halfspace: &HalfSpace, + options: ShapeCastOptions, +) -> Option +where + G: SupportMap, +{ + cast_shapes_halfspace_support_map( + &pos12.inverse(), + &-pos12.inverse_transform_vector(vel12), + halfspace, + other, + options, + ) + .map(|hit| hit.swapped()) +} diff --git a/src/query/time_of_impact/time_of_impact_heightfield_shape.rs b/src/query/shape_cast/shape_cast_heightfield_shape.rs similarity index 79% rename from src/query/time_of_impact/time_of_impact_heightfield_shape.rs rename to src/query/shape_cast/shape_cast_heightfield_shape.rs index c48eda35..d963722e 100644 --- a/src/query/time_of_impact/time_of_impact_heightfield_shape.rs +++ b/src/query/shape_cast/shape_cast_heightfield_shape.rs @@ -1,24 +1,25 @@ +use crate::bounding_volume::BoundingVolume; use crate::math::{Isometry, Real, Vector}; -use crate::query::{QueryDispatcher, Ray, Unsupported, TOI}; +use crate::query::details::ShapeCastOptions; +use crate::query::{QueryDispatcher, Ray, ShapeCastHit, Unsupported}; use crate::shape::{HeightField, Shape}; #[cfg(feature = "dim3")] use crate::{bounding_volume::Aabb, query::RayCast}; /// Time Of Impact between a moving shape and a heightfield. #[cfg(feature = "dim2")] -pub fn time_of_impact_heightfield_shape( +pub fn cast_shapes_heightfield_shape( dispatcher: &D, pos12: &Isometry, vel12: &Vector, heightfield1: &HeightField, g2: &dyn Shape, - max_toi: Real, - stop_at_penetration: bool, -) -> Result, Unsupported> + options: ShapeCastOptions, +) -> Result, Unsupported> where D: QueryDispatcher, { - let aabb2_1 = g2.compute_aabb(pos12); + let aabb2_1 = g2.compute_aabb(pos12).loosened(options.target_distance); let ray = Ray::new(aabb2_1.center(), *vel12); let mut curr_range = heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1); @@ -31,7 +32,7 @@ where curr_range.start -= 1; } - let mut best_hit = None::; + let mut best_hit = None::; /* * Test the segment under the ray. @@ -41,10 +42,8 @@ where for curr in clamped_curr_range { if let Some(seg) = heightfield1.segment_at(curr) { // TODO: pre-check using a ray-cast on the Aabbs first? - if let Some(hit) = - dispatcher.time_of_impact(pos12, vel12, &seg, g2, max_toi, stop_at_penetration)? - { - if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) { + if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &seg, g2, options)? { + if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) { best_hit = Some(hit); } } @@ -82,16 +81,14 @@ where curr_elt -= 1; } - if curr_param >= max_toi { + if curr_param >= options.max_time_of_impact { break; } if let Some(seg) = heightfield1.segment_at(curr_elt as usize) { // TODO: pre-check using a ray-cast on the Aabbs first? - if let Some(hit) = - dispatcher.time_of_impact(pos12, vel12, &seg, g2, max_toi, stop_at_penetration)? - { - if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) { + if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &seg, g2, options)? { + if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) { best_hit = Some(hit); } } @@ -103,29 +100,28 @@ where /// Time Of Impact between a moving shape and a heightfield. #[cfg(feature = "dim3")] -pub fn time_of_impact_heightfield_shape( +pub fn cast_shapes_heightfield_shape( dispatcher: &D, pos12: &Isometry, vel12: &Vector, heightfield1: &HeightField, g2: &dyn Shape, - max_toi: Real, - stop_at_penetration: bool, -) -> Result, Unsupported> + options: ShapeCastOptions, +) -> Result, Unsupported> where D: QueryDispatcher, { let aabb1 = heightfield1.local_aabb(); - let mut aabb2_1 = g2.compute_aabb(pos12); + let mut aabb2_1 = g2.compute_aabb(pos12).loosened(options.target_distance); let ray = Ray::new(aabb2_1.center(), *vel12); // 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(toi) = msum.cast_local_ray(&ray, max_toi, true) { + if let Some(time_of_impact) = msum.cast_local_ray(&ray, options.max_time_of_impact, true) { // Advance the aabb2 to the hit point. - aabb2_1.mins += ray.dir * toi; - aabb2_1.maxs += ray.dir * toi; + aabb2_1.mins += ray.dir * time_of_impact; + aabb2_1.maxs += ray.dir * time_of_impact; } else { return Ok(None); } @@ -133,7 +129,7 @@ where let (mut curr_range_i, mut curr_range_j) = heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1); let (ncells_i, ncells_j) = heightfield1.num_cells_ij(); - let mut best_hit = None::; + let mut best_hit = None::; /* * Enlarge the ranges by 1 to account for any movement within one cell. @@ -163,15 +159,9 @@ where let (tri_a, tri_b) = heightfield1.triangles_at(i as usize, j as usize); for tri in [tri_a, tri_b].into_iter().flatten() { // TODO: pre-check using a ray-cast on the Aabbs first? - if let Some(hit) = dispatcher.time_of_impact( - pos12, - vel12, - &tri, - g2, - max_toi, - stop_at_penetration, - )? { - if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) { + if let Some(hit) = dispatcher.cast_shapes(pos12, vel12, &tri, g2, options)? { + if hit.time_of_impact < best_hit.map(|h| h.time_of_impact).unwrap_or(Real::MAX) + { best_hit = Some(hit); } } @@ -219,7 +209,7 @@ where Real::MAX }; - if toi_x > max_toi && toi_z > max_toi { + if toi_x > options.max_time_of_impact && toi_z > options.max_time_of_impact { break; } @@ -277,26 +267,24 @@ where } /// Time Of Impact between a moving shape and a heightfield. -pub fn time_of_impact_shape_heightfield( +pub fn cast_shapes_shape_heightfield( dispatcher: &D, pos12: &Isometry, vel12: &Vector, g1: &dyn Shape, heightfield2: &HeightField, - max_toi: Real, - stop_at_penetration: bool, -) -> Result, Unsupported> + options: ShapeCastOptions, +) -> Result, Unsupported> where D: QueryDispatcher, { - Ok(time_of_impact_heightfield_shape( + Ok(cast_shapes_heightfield_shape( dispatcher, &pos12.inverse(), &-pos12.inverse_transform_vector(vel12), heightfield2, g1, - max_toi, - stop_at_penetration, + options, )? - .map(|toi| toi.swapped())) + .map(|hit| hit.swapped())) } 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 new file mode 100644 index 00000000..8654c46b --- /dev/null +++ b/src/query/shape_cast/shape_cast_support_map_support_map.rs @@ -0,0 +1,69 @@ +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::{ShapeCastHit, ShapeCastStatus}; +use crate::shape::{RoundShapeRef, SupportMap}; +use num::Zero; + +/// Time of impacts between two support-mapped shapes under translational movement. +pub fn cast_shapes_support_map_support_map( + pos12: &Isometry, + vel12: &Vector, + g1: &G1, + g2: &G2, + options: ShapeCastOptions, +) -> Option +where + G1: SupportMap, + G2: SupportMap, +{ + let gjk_result = if options.target_distance > 0.0 { + let round_g1 = RoundShapeRef { + inner_shape: g1, + border_radius: options.target_distance, + }; + gjk::directional_distance(pos12, &round_g1, g2, vel12, &mut VoronoiSimplex::new()) + } else { + gjk::directional_distance(pos12, g1, g2, vel12, &mut VoronoiSimplex::new()) + }; + + gjk_result.and_then(|(time_of_impact, normal1, witness1, witness2)| { + if time_of_impact > options.max_time_of_impact { + None + } 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 normal_vel = contact.normal1.dot(vel12); + + if !options.stop_at_penetration && normal_vel >= 0.0 { + None + } else { + Some(ShapeCastHit { + time_of_impact, + normal1: contact.normal1, + normal2: contact.normal2, + witness1: contact.point1, + witness2: contact.point2, + status: ShapeCastStatus::PenetratingOrWithinTargetDist, + }) + } + } else { + Some(ShapeCastHit { + time_of_impact, + normal1: Unit::new_unchecked(normal1), + normal2: Unit::new_unchecked(pos12.inverse_transform_vector(&-normal1)), + witness1: witness1 - normal1 * options.target_distance, + witness2: pos12.inverse_transform_point(&witness2), + status: if time_of_impact.is_zero() { + ShapeCastStatus::PenetratingOrWithinTargetDist + } else { + ShapeCastStatus::Converged + }, + }) + } + }) +} diff --git a/src/query/time_of_impact/mod.rs b/src/query/time_of_impact/mod.rs deleted file mode 100644 index 915c0c0d..00000000 --- a/src/query/time_of_impact/mod.rs +++ /dev/null @@ -1,28 +0,0 @@ -//! Implementation details of the `time_of_impact` function. - -pub use self::time_of_impact::{time_of_impact, TOIStatus, TOI}; -pub use self::time_of_impact_ball_ball::time_of_impact_ball_ball; -pub use self::time_of_impact_halfspace_support_map::{ - time_of_impact_halfspace_support_map, time_of_impact_support_map_halfspace, -}; -#[cfg(feature = "std")] -pub use self::{ - time_of_impact_composite_shape_shape::{ - time_of_impact_composite_shape_shape, time_of_impact_shape_composite_shape, - TOICompositeShapeShapeBestFirstVisitor, - }, - time_of_impact_heightfield_shape::{ - time_of_impact_heightfield_shape, time_of_impact_shape_heightfield, - }, - time_of_impact_support_map_support_map::time_of_impact_support_map_support_map, -}; - -mod time_of_impact; -mod time_of_impact_ball_ball; -#[cfg(feature = "std")] -mod time_of_impact_composite_shape_shape; -mod time_of_impact_halfspace_support_map; -#[cfg(feature = "std")] -mod time_of_impact_heightfield_shape; -#[cfg(feature = "std")] -mod time_of_impact_support_map_support_map; diff --git a/src/query/time_of_impact/time_of_impact.rs b/src/query/time_of_impact/time_of_impact.rs deleted file mode 100644 index a612c963..00000000 --- a/src/query/time_of_impact/time_of_impact.rs +++ /dev/null @@ -1,98 +0,0 @@ -use na::Unit; - -use crate::math::{Isometry, Point, Real, Vector}; -use crate::query::{DefaultQueryDispatcher, QueryDispatcher, Unsupported}; -use crate::shape::Shape; - -/// The status of the time-of-impact computation algorithm. -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -pub enum TOIStatus { - /// The TOI algorithm ran out of iterations before achieving convergence. - /// - /// The content of the `TOI` will still be a conservative approximation of the actual result so - /// it is often fine to interpret this case as a success. - OutOfIterations, - /// The TOI algorithm converged successfully. - Converged, - /// Something went wrong during the TOI computation, likely due to numerical instabilities. - /// - /// The content of the `TOI` will still be a conservative approximation of the actual result so - /// it is often fine to interpret this case as a success. - Failed, - /// The two shape already overlap at the time 0. - /// - /// The witness points and normals provided by the `TOI` will have undefined values. - Penetrating, -} - -/// The result of a time-of-impact (TOI) computation. -#[derive(Copy, Clone, Debug)] -pub struct TOI { - /// The time at which the objects touch. - pub toi: Real, - /// The local-space closest point on the first shape at the time of impact. - /// - /// Undefined if `status` is `Penetrating`. - pub witness1: Point, - /// The local-space closest point on the second shape at the time of impact. - /// - /// Undefined if `status` is `Penetrating`. - pub witness2: Point, - /// The local-space outward normal on the first shape at the time of impact. - /// - /// Undefined if `status` is `Penetrating`. - pub normal1: Unit>, - /// The local-space outward normal on the second shape at the time of impact. - /// - /// Undefined if `status` is `Penetrating`. - pub normal2: Unit>, - /// The way the time-of-impact computation algorithm terminated. - pub status: TOIStatus, -} - -impl TOI { - /// Swaps every data of this TOI result such that the role of both shapes are inverted. - /// - /// In practice, this makes it so that `self.witness1` and `self.normal1` become `self.witness2` and `self.normal2` and vice-versa. - pub fn swapped(self) -> Self { - Self { - toi: self.toi, - witness1: self.witness2, - witness2: self.witness1, - normal1: self.normal2, - normal2: self.normal1, - status: self.status, - } - } - - /// Transform `self.witness1` and `self.normal1` by `pos`. - pub fn transform1_by(&self, pos: &Isometry) -> Self { - Self { - toi: self.toi, - witness1: pos * self.witness1, - witness2: self.witness2, - normal1: pos * self.normal1, - normal2: self.normal2, - status: self.status, - } - } -} - -/// 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 penetrating. -pub fn time_of_impact( - pos1: &Isometry, - vel1: &Vector, - g1: &dyn Shape, - pos2: &Isometry, - vel2: &Vector, - g2: &dyn Shape, - max_toi: Real, - stop_at_penetration: bool, -) -> Result, Unsupported> { - let pos12 = pos1.inv_mul(pos2); - let vel12 = pos1.inverse_transform_vector(&(vel2 - vel1)); - DefaultQueryDispatcher.time_of_impact(&pos12, &vel12, g1, g2, max_toi, stop_at_penetration) -} diff --git a/src/query/time_of_impact/time_of_impact_halfspace_support_map.rs b/src/query/time_of_impact/time_of_impact_halfspace_support_map.rs deleted file mode 100644 index 2352d646..00000000 --- a/src/query/time_of_impact/time_of_impact_halfspace_support_map.rs +++ /dev/null @@ -1,78 +0,0 @@ -use crate::math::{Isometry, Real, Vector}; -use crate::query::{Ray, RayCast, TOIStatus, TOI}; -use crate::shape::{HalfSpace, SupportMap}; - -/// Time Of Impact of a halfspace with a support-mapped shape under translational movement. -pub fn time_of_impact_halfspace_support_map( - pos12: &Isometry, - vel12: &Vector, - halfspace: &HalfSpace, - other: &G, - max_toi: Real, - stop_at_penetration: bool, -) -> Option -where - G: SupportMap, -{ - // FIXME: add method to get only the local support point. - // This would avoid the `inverse_transform_point` later. - if !stop_at_penetration && vel12.dot(&halfspace.normal) > 0.0 { - return None; - } - - let support_point = other.support_point(pos12, &-halfspace.normal); - let closest_point = support_point; - let ray = Ray::new(closest_point, *vel12); - - if let Some(toi) = halfspace.cast_local_ray(&ray, max_toi, true) { - if toi > max_toi { - return None; - } - - let witness2 = support_point; - let mut witness1 = ray.point_at(toi); - - let status = if support_point.coords.dot(&halfspace.normal) < 0.0 { - TOIStatus::Penetrating - } else { - // Project the witness point to the halfspace. - // Note that witness2 is already in the halfspace's local-space. - witness1 = witness1 - *halfspace.normal * witness1.coords.dot(&halfspace.normal); - TOIStatus::Converged - }; - - Some(TOI { - toi, - normal1: halfspace.normal, - normal2: pos12.inverse_transform_unit_vector(&-halfspace.normal), - witness1, - witness2: pos12.inverse_transform_point(&witness2), - status, - }) - } else { - None - } -} - -/// Time Of Impact of a halfspace with a support-mapped shape under translational movement. -pub fn time_of_impact_support_map_halfspace( - pos12: &Isometry, - vel12: &Vector, - other: &G, - halfspace: &HalfSpace, - max_toi: Real, - stop_at_penetration: bool, -) -> Option -where - G: SupportMap, -{ - time_of_impact_halfspace_support_map( - &pos12.inverse(), - &-pos12.inverse_transform_vector(vel12), - halfspace, - other, - max_toi, - stop_at_penetration, - ) - .map(|toi| toi.swapped()) -} diff --git a/src/query/time_of_impact/time_of_impact_support_map_support_map.rs b/src/query/time_of_impact/time_of_impact_support_map_support_map.rs deleted file mode 100644 index 53a0b486..00000000 --- a/src/query/time_of_impact/time_of_impact_support_map_support_map.rs +++ /dev/null @@ -1,59 +0,0 @@ -use na::Unit; - -use crate::math::{Isometry, Real, Vector}; -use crate::query::details; -use crate::query::gjk::{self, VoronoiSimplex}; -use crate::query::{TOIStatus, TOI}; -use crate::shape::SupportMap; -use num::Zero; - -/// Time of impacts between two support-mapped shapes under translational movement. -pub fn time_of_impact_support_map_support_map( - pos12: &Isometry, - vel12: &Vector, - g1: &G1, - g2: &G2, - max_toi: Real, - stop_at_penetration: bool, -) -> Option -where - G1: SupportMap, - G2: SupportMap, -{ - gjk::directional_distance(pos12, g1, g2, vel12, &mut VoronoiSimplex::new()).and_then( - |(toi, normal1, witness1, witness2)| { - if toi > max_toi { - None - } else if !stop_at_penetration && toi < 1.0e-5 { - let contact = details::contact_support_map_support_map(pos12, g1, g2, Real::MAX)?; - let normal_vel = contact.normal1.dot(vel12); - - if normal_vel >= 0.0 { - None - } else { - Some(TOI { - toi, - normal1: contact.normal1, - normal2: contact.normal2, - witness1: contact.point1, - witness2: contact.point2, - status: TOIStatus::Penetrating, - }) - } - } else { - Some(TOI { - toi, - normal1: Unit::new_unchecked(normal1), - normal2: Unit::new_unchecked(pos12.inverse_transform_vector(&-normal1)), - witness1, - witness2: pos12.inverse_transform_point(&witness2), - status: if toi.is_zero() { - TOIStatus::Penetrating - } else { - TOIStatus::Converged - }, - }) - } - }, - ) -} diff --git a/src/query/visitors/ray_intersections_visitor.rs b/src/query/visitors/ray_intersections_visitor.rs index 642d963d..4218173d 100644 --- a/src/query/visitors/ray_intersections_visitor.rs +++ b/src/query/visitors/ray_intersections_visitor.rs @@ -8,7 +8,7 @@ use std::marker::PhantomData; /// Bounding Volume Tree visitor collecting intersections with a given ray. pub struct RayIntersectionsVisitor<'a, T, F> { simd_ray: SimdRay, - max_toi: SimdReal, + max_time_of_impact: SimdReal, callback: &'a mut F, _phantom: PhantomData, } @@ -19,10 +19,14 @@ where { /// Creates a new `RayIntersectionsVisitor`. #[inline] - pub fn new(ray: &Ray, max_toi: Real, callback: &'a mut F) -> RayIntersectionsVisitor<'a, T, F> { + pub fn new( + ray: &Ray, + max_time_of_impact: Real, + callback: &'a mut F, + ) -> RayIntersectionsVisitor<'a, T, F> { RayIntersectionsVisitor { simd_ray: SimdRay::splat(*ray), - max_toi: SimdReal::splat(max_toi), + max_time_of_impact: SimdReal::splat(max_time_of_impact), callback, _phantom: PhantomData, } @@ -35,7 +39,7 @@ where { #[inline] fn visit(&mut self, bv: &SimdAabb, b: Option<[Option<&T>; SIMD_WIDTH]>) -> SimdVisitStatus { - let mask = bv.cast_local_ray(&self.simd_ray, self.max_toi).0; + let mask = bv.cast_local_ray(&self.simd_ray, self.max_time_of_impact).0; if let Some(data) = b { let bitmask = mask.bitmask(); diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 356b90c4..1610873a 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -40,7 +40,7 @@ bitflags::bitflags! { )] #[repr(C)] #[derive(Default)] - /// The status of the cell of an heightfield. + /// Flags controlling the behavior of some operations involving heightfields. pub struct HeightFieldFlags: u8 { /// If set, a special treatment will be applied to contact manifold calculation to eliminate /// or fix contacts normals that could lead to incorrect bumps in physics simulation (especially @@ -527,6 +527,16 @@ impl HeightField { &mut self.status } + /// The heightfield’s flags controlling internal-edges handling. + pub fn flags(&self) -> HeightFieldFlags { + self.flags + } + + /// Sets the heightfield’s flags controlling internal-edges handling. + pub fn set_flags(&mut self, flags: HeightFieldFlags) { + self.flags = flags; + } + /// The heights of this heightfield. pub fn heights(&self) -> &DMatrix { &self.heights diff --git a/src/shape/mod.rs b/src/shape/mod.rs index 3e115dd7..740edbc3 100644 --- a/src/shape/mod.rs +++ b/src/shape/mod.rs @@ -70,6 +70,8 @@ pub type RoundConvexPolyhedron = RoundShape; #[cfg(feature = "std")] pub type RoundConvexPolygon = RoundShape; +pub(crate) use self::round_shape::RoundShapeRef; + mod ball; mod capsule; #[cfg(feature = "std")] diff --git a/src/shape/round_shape.rs b/src/shape/round_shape.rs index 318f61cd..f5bc1492 100644 --- a/src/shape/round_shape.rs +++ b/src/shape/round_shape.rs @@ -27,3 +27,21 @@ impl SupportMap for RoundShape { self.inner_shape.local_support_point_toward(dir) + **dir * self.border_radius } } + +/// A shape reference with rounded borders. +pub(crate) struct RoundShapeRef<'a, S: ?Sized> { + /// The shape being rounded. + pub inner_shape: &'a S, + /// The radius of the rounded border. + pub border_radius: Real, +} + +impl<'a, S: ?Sized + SupportMap> SupportMap for RoundShapeRef<'a, S> { + fn local_support_point(&self, dir: &Vector) -> Point { + self.local_support_point_toward(&Unit::new_normalize(*dir)) + } + + fn local_support_point_toward(&self, dir: &Unit>) -> Point { + self.inner_shape.local_support_point_toward(dir) + **dir * self.border_radius + } +} diff --git a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs index db309856..afe0a99c 100644 --- a/src/transformation/mesh_intersection/triangle_triangle_intersection.rs +++ b/src/transformation/mesh_intersection/triangle_triangle_intersection.rs @@ -260,9 +260,9 @@ fn segment_plane_intersection( let dir = segment.b - segment.a; let dir_norm = dir.norm(); - let toi = + let time_of_impact = query::details::line_toi_with_halfspace(plane_center, plane_normal, &segment.a, &dir)?; - let scaled_toi = toi * dir_norm; + let scaled_toi = time_of_impact * dir_norm; if scaled_toi < -EPS || scaled_toi > dir_norm + EPS { None @@ -271,6 +271,6 @@ fn segment_plane_intersection( } else if scaled_toi >= dir_norm - EPS { Some((segment.b, FeatureId::Vertex(vids.1))) } else { - Some((segment.a + dir * toi, FeatureId::Edge(eid))) + Some((segment.a + dir * time_of_impact, FeatureId::Edge(eid))) } }