diff --git a/CHANGELOG.md b/CHANGELOG.md index 1ece976b..efa34f58 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,8 @@ - `point_cloud_bounding_sphere` and `point_cloud_bounding_sphere_with_center` now returns a `BoundingSphere`. - Removed `IntersectionCompositeShapeShapeBestFirstVisitor` (which had been deprecated for a while): use `IntersectionCompositeShapeShapeVisitor` instead. +- Epsilon from `parry::query::gjk::eps_tol` is now `10e-2` (from `10e-5`). + - This fixes cases of incorrectly failing shapecasts. ## v0.17.5 diff --git a/Cargo.toml b/Cargo.toml index 9e08c42f..05320652 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,5 +1,10 @@ [workspace] -members = ["crates/parry2d", "crates/parry3d", "crates/parry2d-f64", "crates/parry3d-f64"] +members = [ + "crates/parry2d", + "crates/parry3d", + "crates/parry2d-f64", + "crates/parry3d-f64", +] resolver = "2" [workspace.lints] @@ -17,4 +22,4 @@ parry3d-f64 = { path = "crates/parry3d-f64" } #simba = { path = "../simba" } #simba = { git = "https://github.com/dimforge/simba", rev = "45a5266eb36ed9d25907e9bf9130cd4ac846a748" } -#nalgebra = { git = "https://github.com/dimforge/nalgebra", rev = "0cf79aef0e6155befc3279a3145f1940822b8377" } \ No newline at end of file +#nalgebra = { git = "https://github.com/dimforge/nalgebra", rev = "0cf79aef0e6155befc3279a3145f1940822b8377" } diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 0b018190..2d0cd521 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -30,7 +30,7 @@ std = [ "arrayvec/std", "spade", "thiserror", - "ena" + "ena", ] dim2 = [] f32 = [] @@ -142,6 +142,11 @@ name = "cuboid2d" path = "examples/cuboid2d.rs" doc-scrape-examples = true +[[example]] +name = "debug_shape_cast2d" +path = "examples/debug_shape_cast2d.rs" +doc-scrape-examples = true + [[example]] name = "distance_query2d" path = "examples/distance_query2d.rs" diff --git a/crates/parry2d/examples/debug_shape_cast2d.rs b/crates/parry2d/examples/debug_shape_cast2d.rs new file mode 100644 index 00000000..b05d5386 --- /dev/null +++ b/crates/parry2d/examples/debug_shape_cast2d.rs @@ -0,0 +1,188 @@ +mod common_macroquad2d; + +use common_macroquad2d::draw_point; +use macroquad::prelude::*; +use nalgebra::{Isometry2, Point2}; +use parry2d::math::Isometry; +use parry2d::query::{self, Ray, ShapeCastOptions}; +use parry2d::shape::{Ball, ConvexPolygon, Shape}; + +const RENDER_SCALE: f32 = 1.0; +const BALLCAST_WIDTH: f32 = 16.0; + +#[macroquad::main("raycasts_animated")] +async fn main() { + for _i in 1.. { + clear_background(BLACK); + + let screen_shift = Point2::new(screen_width() / 2.0, screen_height() / 2.0); + + let to_cast_against = ConvexPolygon::from_convex_polyline( + [ + [-24.0, 0.0].into(), + [0.0, 24.0].into(), + [24.0, 0.0].into(), + [0.0, -24.0].into(), + ] + .into(), + ) + .expect("Failed to create ConvexPolygon from polyline"); + let to_cast_against_pose = Isometry2::rotation(0f32); + + let mouse_pos = mouse_position(); + let mouse_position_world = + (Point2::::new(mouse_pos.0, mouse_pos.1) - screen_shift.coords) / RENDER_SCALE; + let target_pos: Point2 = [-312.0, 152.0].into(); + + // Those 2 fail with `min_bound >= _eps_tol`, fixed with a tolerance * 100 + shape_cast_debug( + screen_shift, + [99.0, -33.0].into(), + target_pos, + to_cast_against.clone(), + ); + shape_cast_debug( + screen_shift, + [98.0, -31.0].into(), + target_pos, + to_cast_against.clone(), + ); + // This fails with `niter == 100` (and `niter == 100_000`), fixed with a tolerance * 10_000 + shape_cast_debug( + screen_shift, + [47.0, -32.0].into(), + target_pos, + to_cast_against.clone(), + ); + + // For debug purposes, raycast to mouse position. + // Rendered last to be on top of the other raycasts + shape_cast_debug( + screen_shift, + target_pos, + mouse_position_world, + to_cast_against.clone(), + ); + + /* + * + * Render the cuboid. + * + */ + draw_polygon( + &to_cast_against.points(), + &to_cast_against_pose, + RENDER_SCALE, + screen_shift, + GREEN, + ); + + next_frame().await + } +} + +fn shape_cast_debug( + screen_shift: Point2, + source_pos: Point2, + target_pos: Point2, + to_cast_against: ConvexPolygon, +) { + /* + * + * Prepare a Raycast and compute its result against the shape. + * + */ + let ray = Ray::new(source_pos, target_pos - source_pos); + + let pos1: Point2 = source_pos.into(); + let vel1 = target_pos - source_pos; + let g1 = Ball::new(BALLCAST_WIDTH); + let pos2 = [0.0, 0.0]; + let vel2 = [0.0, 0.0]; + let g2 = to_cast_against.clone_dyn(); + + let toi = query::cast_shapes( + &pos1.into(), + &vel1, + &g1, + &pos2.into(), + &vel2.into(), + &*g2, + ShapeCastOptions::with_max_time_of_impact(1.0), + ) + .unwrap(); + + /* + * + * Render the raycast's result. + * + */ + drawcircle_at(pos1, BALLCAST_WIDTH, RENDER_SCALE, screen_shift, ORANGE); + + if let Some(toi) = toi { + if toi.time_of_impact == 0f32 { + draw_point(ray.origin, RENDER_SCALE, screen_shift, YELLOW); + } else { + drawline_from_to( + ray.origin, + (ray.point_at(toi.time_of_impact).coords).into(), + RENDER_SCALE, + screen_shift, + GREEN, + ); + } + drawcircle_at( + (ray.point_at(toi.time_of_impact).coords).into(), + BALLCAST_WIDTH, + RENDER_SCALE, + screen_shift, + GREEN, + ); + } else { + drawline_from_to( + ray.origin, + ray.origin + ray.dir, + RENDER_SCALE, + screen_shift, + RED, + ); + } +} + +fn draw_polygon( + polygon: &[Point2], + pose: &Isometry, + scale: f32, + shift: Point2, + color: Color, +) { + for i in 0..polygon.len() { + let a = pose * (scale * polygon[i]); + let b = pose * (scale * polygon[(i + 1) % polygon.len()]); + draw_line( + a.x + shift.x, + a.y + shift.y, + b.x + shift.x, + b.y + shift.y, + 2.0, + color, + ); + } +} + +fn drawline_from_to( + from: Point2, + to: Point2, + scale: f32, + shift: Point2, + color: Color, +) { + let from = from * scale + shift.coords; + let to = to * scale + shift.coords; + draw_line(from.x, from.y, to.x, to.y, 2.0, color); +} + +fn drawcircle_at(center: Point2, radius: f32, scale: f32, shift: Point2, color: Color) { + let center = center * scale + shift.coords; + draw_circle_lines(center.x, center.y, radius, 1f32, color); +} diff --git a/crates/parry2d/tests/geometry/ray_cast.rs b/crates/parry2d/tests/geometry/ray_cast.rs index de37fd60..8e3ab122 100644 --- a/crates/parry2d/tests/geometry/ray_cast.rs +++ b/crates/parry2d/tests/geometry/ray_cast.rs @@ -170,8 +170,7 @@ fn convexpoly_raycast_fuzz() { let ray_origin = Point2::new(3., 1. + (i as Real * 0.0001)); let ray_look_at = Point2::new(0., 2.); let collision = test_raycast(ray_origin, ray_look_at); - let eps = 1.0e-5; - + let eps = parry2d::query::gjk::eps_tol(); match collision { Some(distance) if distance >= 1.0 - eps && distance < (2.0 as Real).sqrt() => (), Some(distance) if distance >= 2.0 => panic!( diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index ebee30ae..4b71dd03 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -35,7 +35,7 @@ pub enum GJKResult { /// The absolute tolerance used by the GJK algorithm. pub fn eps_tol() -> Real { let _eps = crate::math::DEFAULT_EPSILON; - _eps * 10.0 + _eps * 10_000.0 } /// Projects the origin on the boundary of the given shape. @@ -391,3 +391,61 @@ fn result(simplex: &VoronoiSimplex, prev: bool) -> (Point, Point) { res } } + +#[cfg(test)] +#[cfg(feature = "dim2")] +mod test { + use na::Point2; + + use crate::{ + math::Real, + query::{self, ShapeCastOptions}, + shape::{Ball, ConvexPolygon, Shape}, + }; + + #[test] + fn gjk_issue_180() { + let to_cast_against = ConvexPolygon::from_convex_polyline( + [ + [-24.0, 0.0].into(), + [0.0, 24.0].into(), + [24.0, 0.0].into(), + [0.0, -24.0].into(), + ] + .into(), + ) + .unwrap(); + let target_pos: Point2 = [-312.0, 152.0].into(); + + check_converge(&to_cast_against, [47.0, -32.0].into(), target_pos); + check_converge(&to_cast_against, [99.0, -33.0].into(), target_pos); + check_converge(&to_cast_against, [98.0, -31.0].into(), target_pos); + } + + fn check_converge( + to_cast_against: &ConvexPolygon, + source_pos: Point2, + target_pos: Point2, + ) { + let vel1 = target_pos - source_pos; + let g1 = Ball::new(16.0); + let pos2 = [0.0, 0.0]; + let vel2 = [0.0, 0.0]; + let g2 = to_cast_against.clone_dyn(); + + let toi = query::cast_shapes( + &source_pos.into(), + &vel1, + &g1, + &pos2.into(), + &vel2.into(), + &*g2, + ShapeCastOptions::with_max_time_of_impact(1.0), + ) + .unwrap(); + assert!( + toi.is_some(), + "casting against the convex polygon should converge." + ); + } +}