diff --git a/.github/workflows/parry-ci-build.yml b/.github/workflows/parry-ci-build.yml index cb7f5a99..7e0cc94a 100644 --- a/.github/workflows/parry-ci-build.yml +++ b/.github/workflows/parry-ci-build.yml @@ -42,7 +42,7 @@ jobs: - name: Check serialization run: cargo check --features bytemuck-serialize,serde-serialize,rkyv-serialize; - name: Run tests - run: cargo test + run: cargo test --features wavefront build-wasm: runs-on: ubuntu-latest env: diff --git a/CHANGELOG.md b/CHANGELOG.md index df312927..cbb3d64b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ - `TriMesh::intersection_with_aabb` - `SharedShape::trimesh` - `SharedShape::trimesh_with_flags` +- `point_cloud_bounding_sphere_with_center` now returns a `BoundingSphere`. ## v0.17.1 diff --git a/crates/parry2d/examples/aabb2d.rs b/crates/parry2d/examples/aabb2d.rs index 29af24ce..9223f6e5 100644 --- a/crates/parry2d/examples/aabb2d.rs +++ b/crates/parry2d/examples/aabb2d.rs @@ -1,36 +1,101 @@ +mod common_macroquad2d; + extern crate nalgebra as na; +use common_macroquad2d::{draw_polyline, lissajous_2d, mquad_from_na, na_from_mquad}; +use macroquad::prelude::*; use na::Isometry2; -use parry2d::bounding_volume::BoundingVolume; +use parry2d::bounding_volume::{Aabb, BoundingVolume}; use parry2d::shape::Ball; -fn main() { - /* - * Initialize the shapes. - */ - let ball1 = Ball::new(0.5); - let ball2 = Ball::new(1.0); - - let ball1_pos = Isometry2::translation(0.0, 1.0); - let ball2_pos = Isometry2::identity(); - - /* - * Compute their axis-aligned bounding boxes. - */ - let aabb_ball1 = ball1.aabb(&ball1_pos); - let aabb_ball2 = ball2.aabb(&ball2_pos); - - // Merge the two boxes. - let bounding_aabb = aabb_ball1.merged(&aabb_ball2); - - // Enlarge the ball2 aabb. - let loose_aabb_ball2 = aabb_ball2.loosened(1.0); - - // Intersection and inclusion tests. - assert!(aabb_ball1.intersects(&aabb_ball2)); - assert!(bounding_aabb.contains(&aabb_ball1)); - assert!(bounding_aabb.contains(&aabb_ball2)); - assert!(!aabb_ball2.contains(&bounding_aabb)); - assert!(!aabb_ball1.contains(&bounding_aabb)); - assert!(loose_aabb_ball2.contains(&aabb_ball2)); +const RENDER_SCALE: f32 = 30.0; + +#[macroquad::main("parry2d::utils::point_in_poly2d")] +async fn main() { + let render_pos = Vec2::new(300.0, 300.0); + + loop { + let elapsed_time = get_time() as f32 * 0.7; + clear_background(BLACK); + + /* + * Initialize the shapes. + */ + let ball1 = Ball::new(0.5); + let ball2 = Ball::new(1.0); + + let ball1_pos = na_from_mquad(lissajous_2d(elapsed_time)) * 5f32; + let ball2_pos = Isometry2::identity(); + + /* + * Compute their axis-aligned bounding boxes. + */ + let aabb_ball1 = ball1.aabb(&ball1_pos.into()); + let aabb_ball2 = ball2.aabb(&ball2_pos); + + // Merge the two boxes. + let bounding_aabb = aabb_ball1.merged(&aabb_ball2); + + // Enlarge the ball2 aabb. + let loose_aabb_ball2 = aabb_ball2.loosened(2f32); + + // Intersection test + let color = if aabb_ball1.intersects(&aabb_ball2) { + RED + } else { + GREEN + }; + + assert!(bounding_aabb.contains(&aabb_ball1)); + assert!(bounding_aabb.contains(&aabb_ball2)); + assert!(loose_aabb_ball2.contains(&aabb_ball2)); + + let ball1_translation = mquad_from_na(ball1_pos.coords.into()) * RENDER_SCALE + render_pos; + draw_circle( + ball1_translation.x, + ball1_translation.y, + ball1.radius * RENDER_SCALE, + color, + ); + let ball2_translation = + mquad_from_na(ball2_pos.translation.vector.into()) * RENDER_SCALE + render_pos; + draw_circle( + ball2_translation.x, + ball2_translation.y, + ball2.radius * RENDER_SCALE, + color, + ); + + draw_aabb(aabb_ball1, render_pos, color); + draw_aabb(aabb_ball2, render_pos, color); + draw_aabb(bounding_aabb, render_pos, YELLOW); + + // Inclusion test + let color_included: Color = if loose_aabb_ball2.contains(&aabb_ball1) { + BLUE + } else { + MAGENTA + }; + draw_aabb(loose_aabb_ball2, render_pos, color_included); + next_frame().await + } +} + +fn draw_aabb(aabb: Aabb, offset: Vec2, color: Color) { + let mins = mquad_from_na(aabb.mins) * RENDER_SCALE + offset; + let maxs = mquad_from_na(aabb.maxs) * RENDER_SCALE + offset; + + let line = vec![ + Vec2::new(mins.x, mins.y), + Vec2::new(mins.x, maxs.y), + Vec2::new(maxs.x, maxs.y), + Vec2::new(maxs.x, mins.y), + Vec2::new(mins.x, mins.y), + ]; + let drawable_line = line + .iter() + .zip(line.iter().cycle().skip(1).take(line.len())) + .map(|item| (item.0.clone(), item.1.clone())) + .collect(); + draw_polyline(drawable_line, color); } diff --git a/crates/parry2d/examples/bounding_sphere2d.rs b/crates/parry2d/examples/bounding_sphere2d.rs index a5551dad..b5a495f1 100644 --- a/crates/parry2d/examples/bounding_sphere2d.rs +++ b/crates/parry2d/examples/bounding_sphere2d.rs @@ -1,36 +1,130 @@ +mod common_macroquad2d; + extern crate nalgebra as na; +use common_macroquad2d::{draw_polyline, lissajous_2d, mquad_from_na, na_from_mquad}; +use macroquad::prelude::*; use na::{Isometry2, Vector2}; -use parry2d::bounding_volume::BoundingVolume; +use parry2d::bounding_volume::{Aabb, BoundingVolume}; use parry2d::shape::Cuboid; -fn main() { - /* - * Initialize the shapes. - */ - let cube1 = Cuboid::new(Vector2::repeat(0.5)); - let cube2 = Cuboid::new(Vector2::new(1.0, 0.5)); - - let cube1_pos = Isometry2::translation(0.0, 1.0); - let cube2_pos = Isometry2::identity(); - - /* - * Compute their bounding spheres. - */ - let bounding_sphere_cube1 = cube1.bounding_sphere(&cube1_pos); - let bounding_sphere_cube2 = cube2.bounding_sphere(&cube2_pos); - - // Merge the two spheres. - let bounding_bounding_sphere = bounding_sphere_cube1.merged(&bounding_sphere_cube2); - - // Enlarge the cube2 bounding sphere. - let loose_bounding_sphere_cube2 = bounding_sphere_cube2.loosened(1.0); - - // Intersection and inclusion tests. - assert!(bounding_sphere_cube1.intersects(&bounding_sphere_cube2)); - assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube1)); - assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube2)); - assert!(!bounding_sphere_cube2.contains(&bounding_bounding_sphere)); - assert!(!bounding_sphere_cube1.contains(&bounding_bounding_sphere)); - assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube2)); +const RENDER_SCALE: f32 = 30.0; + +#[macroquad::main("parry2d::utils::point_in_poly2d")] +async fn main() { + let render_pos = Vec2::new(300.0, 300.0); + + loop { + let elapsed_time = get_time() as f32 * 0.7; + clear_background(BLACK); + + /* + * Initialize the shapes. + */ + let cube1: Cuboid = Cuboid::new(Vector2::repeat(0.5)); + let cube2 = Cuboid::new(Vector2::new(1., 0.5)); + + let cube1_pos = na_from_mquad(lissajous_2d(elapsed_time)) * 5f32; + let cube1_pos = Isometry2::from(cube1_pos); + let cube2_pos = Isometry2::identity(); + + /* + * Compute their bounding spheres. + */ + let bounding_sphere_cube1 = cube1.bounding_sphere(&cube1_pos); + let bounding_sphere_cube2 = cube2.bounding_sphere(&cube2_pos); + + // Merge the two spheres. + let bounding_bounding_sphere = bounding_sphere_cube1.merged(&bounding_sphere_cube2); + + // Enlarge the cube2 bounding sphere. + let loose_bounding_sphere_cube2 = bounding_sphere_cube2.loosened(3.0); + + // Intersection test + let color = if bounding_sphere_cube1.intersects(&bounding_sphere_cube2) { + RED + } else { + GREEN + }; + + // Due to float imprecisions, it's dangerous to assume that both shapes will be + // contained in the merged. + // You can leverage `BoundingVolume::loosened` with an epsilon for expected results. + // + // These might fail: + // assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube1)); + // assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube2)); + + assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube1)); + assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube2)); + + let cube1_translation = + mquad_from_na(cube1_pos.translation.vector.into()) * RENDER_SCALE + render_pos; + draw_cuboid(cube1, cube1_translation, color); + + let cube2_translation = + mquad_from_na(cube2_pos.translation.vector.into()) * RENDER_SCALE + render_pos; + draw_cuboid(cube2, cube2_translation, color); + draw_circle_lines( + bounding_sphere_cube1.center.x * RENDER_SCALE + render_pos.x, + bounding_sphere_cube1.center.y * RENDER_SCALE + render_pos.y, + bounding_sphere_cube1.radius * RENDER_SCALE, + 2f32, + color, + ); + draw_circle_lines( + bounding_sphere_cube2.center.x * RENDER_SCALE + render_pos.x, + bounding_sphere_cube2.center.y * RENDER_SCALE + render_pos.y, + bounding_sphere_cube2.radius * RENDER_SCALE, + 2f32, + color, + ); + draw_circle_lines( + bounding_bounding_sphere.center.x * RENDER_SCALE + render_pos.x, + bounding_bounding_sphere.center.y * RENDER_SCALE + render_pos.y, + bounding_bounding_sphere.radius * RENDER_SCALE, + 2f32, + YELLOW, + ); + + // Inclusion test + let color_included: Color = if loose_bounding_sphere_cube2.contains(&bounding_sphere_cube1) + { + BLUE + } else { + MAGENTA + }; + draw_circle_lines( + loose_bounding_sphere_cube2.center.x * RENDER_SCALE + render_pos.x, + loose_bounding_sphere_cube2.center.y * RENDER_SCALE + render_pos.y, + loose_bounding_sphere_cube2.radius * RENDER_SCALE, + 2f32, + color_included, + ); + next_frame().await + } +} + +fn draw_cuboid(cuboid: Cuboid, pos: Vec2, color: Color) { + let aabb = cuboid.local_aabb(); + draw_aabb(aabb, pos, color) +} + +fn draw_aabb(aabb: Aabb, offset: Vec2, color: Color) { + let mins = mquad_from_na(aabb.mins) * RENDER_SCALE + offset; + let maxs = mquad_from_na(aabb.maxs) * RENDER_SCALE + offset; + + let line = vec![ + Vec2::new(mins.x, mins.y), + Vec2::new(mins.x, maxs.y), + Vec2::new(maxs.x, maxs.y), + Vec2::new(maxs.x, mins.y), + Vec2::new(mins.x, mins.y), + ]; + let drawable_line = line + .iter() + .zip(line.iter().cycle().skip(1).take(line.len())) + .map(|item| (item.0.clone(), item.1.clone())) + .collect(); + draw_polyline(drawable_line, color); } diff --git a/crates/parry2d/examples/common_macroquad2d.rs b/crates/parry2d/examples/common_macroquad2d.rs index da74dd59..6d37fae5 100644 --- a/crates/parry2d/examples/common_macroquad2d.rs +++ b/crates/parry2d/examples/common_macroquad2d.rs @@ -10,6 +10,10 @@ use nalgebra::Point2; use parry2d::math::Real; use parry2d::shape::TriMesh; +/// As this file is used as a module from other examples, +/// rustc warns about dead code: +/// - `main()` is needed for this file to be included in examples +/// - For other functions, they may be "dead code" for an example, but not for others. #[allow(dead_code)] fn main() { println!( @@ -18,45 +22,62 @@ fn main() { ); } +/// Converts a [`nalgebra::Point2`] to a [`Vec2`], which is used by [`macroquad`] #[allow(dead_code)] pub fn mquad_from_na(a: Point2) -> Vec2 { Vec2::new(a.x, a.y) } +/// Converts a [`Vec2`] to a [`nalgebra::Point2`], which is used by [`parry3d`] #[allow(dead_code)] pub fn na_from_mquad(a: Vec2) -> Point2 { Point2::new(a.x, a.y) } +/// Uses [`macroquad`] to display the line passed as parameter. #[allow(dead_code)] -pub fn draw_polyline(polygon: Vec<(Vec2, Vec2)>, color: Color) { - for i in 0..polygon.len() { - let a = polygon[i].0; - let b = polygon[i].1; +pub fn draw_polyline(polyline: Vec<(Vec2, Vec2)>, color: Color) { + for line in polyline { + let a = line.0; + let b = line.1; draw_line_2d(a, b, color); } } +/// Draws a text in the top left corner of the screen. +/// +/// This uses a hardcoded position, size, color. #[allow(dead_code)] pub fn easy_draw_text(text: &str) { macroquad::text::draw_text(text, 10.0, 48.0 + 18.0, 30.0, WHITE); } +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates for time `t`. +/// +/// This uses hardcoded parameters to have an arbitrary pleasing trajectory. #[allow(dead_code)] pub fn lissajous_2d(t: f32) -> Vec2 { // Some hardcoded parameters to have a pleasing lissajous trajectory. - let (a, b, delta_x, delta_y) = (3.0, 2.0, FRAC_PI_2, FRAC_PI_4); + lissajous_2d_with_params(t, 3.0, 2.0, FRAC_PI_2, FRAC_PI_4) +} + +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates. +#[allow(dead_code)] +pub fn lissajous_2d_with_params(t: f32, a: f32, b: f32, delta_x: f32, delta_y: f32) -> Vec2 { + // Some hardcoded parameters to have a pleasing lissajous trajectory. let x = (a * t + delta_x).sin(); let y = (b * t + delta_y).sin(); Vec2::new(x, y) * 0.75f32 } +/// Uses [`macroquad`] to display the line passed as parameter. #[allow(dead_code)] pub fn draw_line_2d(a: Vec2, b: Vec2, color: Color) { draw_line(a.x, a.y, b.x, b.y, 2f32, color); } +/// Uses [`macroquad`] to display the line passed as parameter. #[allow(dead_code)] pub fn draw_trimesh2(trimesh: &TriMesh, offset: Vec2) { let vertices = trimesh.vertices(); @@ -71,6 +92,7 @@ pub fn draw_trimesh2(trimesh: &TriMesh, offset: Vec2) { } } +/// Uses [`macroquad`] to display a wireframe of the polygon. #[allow(dead_code)] pub fn draw_polygon(polygon: &[Point2], scale: f32, shift: Point2, color: Color) { for i in 0..polygon.len() { @@ -87,6 +109,7 @@ pub fn draw_polygon(polygon: &[Point2], scale: f32, shift: Point2, col } } +/// Uses [`macroquad`] to display the a cross, representing a point. #[allow(dead_code)] pub fn draw_point(point: Point2, scale: f32, shift: Point2, color: Color) { let edge_len = 0.15; diff --git a/crates/parry2d/examples/convex_hull2d.rs b/crates/parry2d/examples/convex_hull2d.rs index 9d7e1b5e..33b3c3ff 100644 --- a/crates/parry2d/examples/convex_hull2d.rs +++ b/crates/parry2d/examples/convex_hull2d.rs @@ -1,21 +1,44 @@ -extern crate nalgebra as na; +mod common_macroquad2d; -use na::Point2; +use std::f32::consts::{FRAC_PI_2, FRAC_PI_4}; + +use common_macroquad2d::{draw_point, draw_polygon, lissajous_2d_with_params, na_from_mquad}; +use macroquad::prelude::*; +use nalgebra::Point2; use parry2d::transformation; -fn main() { - let points = vec![ - Point2::new(0.77705324, 0.05374551), - Point2::new(0.35096353, 0.9873069), - Point2::new(0.09537989, 0.44411153), - Point2::new(0.108208835, 0.72445065), - Point2::new(0.7661844, 0.86163324), - Point2::new(0.5185994, 0.66594696), - Point2::new(0.768981, 0.23657233), - Point2::new(0.058607936, 0.09037298), - Point2::new(0.8818559, 0.3804205), - Point2::new(0.9571466, 0.17664945), - ]; - - let _ = transformation::convex_hull(&points[..]); +const RENDER_SCALE: f32 = 30.0; + +#[macroquad::main("parry2d::utils::point_in_poly2d")] +async fn main() { + let count = 9; + let mut pts = vec![Point2::default(); count]; + + let render_pos = Point2::new(300.0, 300.0); + + loop { + let elapsed_time = get_time() as f32; + let elapsed_time_slow = elapsed_time * 0.2; + clear_background(BLACK); + + for (i, pt) in pts.iter_mut().enumerate() { + *pt = na_from_mquad(lissajous_2d_with_params( + (i * i) as f32 + elapsed_time_slow, + 2.0 + i as f32 / 3.0, + (i as f32 / count as f32) + elapsed_time_slow.cos() * 0.1, + (elapsed_time_slow as f32 + i as f32).cos() * 0.1 + FRAC_PI_2, + FRAC_PI_4, + )) * 5f32; + draw_point(*pt, RENDER_SCALE, render_pos, RED); + } + + /* + * + * Compute the convex hull. + * + */ + let convex_hull = transformation::convex_hull(&pts); + draw_polygon(&convex_hull, RENDER_SCALE, render_pos, WHITE); + next_frame().await + } } diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index f9dc4269..4ad6c3a3 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -44,6 +44,7 @@ simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] enhanced-determinism = ["simba/libm_force", "indexmap"] parallel = ["rayon"] +# Adds `TriMesh:to_obj_file` function. wavefront = ["obj"] alloc = [] improved_fixed_point_support = [] @@ -88,3 +89,7 @@ thiserror = { version = "1", optional = true } oorandom = "11" ptree = "0.4.0" rand = { version = "0.8" } + +[package.metadata.docs.rs] +rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"] +features = ["wavefront"] diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index 052704ac..9ad8d9c0 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -45,6 +45,7 @@ simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] enhanced-determinism = ["simba/libm_force", "indexmap"] parallel = ["rayon"] +# Adds `TriMesh:to_obj_file` function. wavefront = ["obj"] alloc = [] improved_fixed_point_support = [] @@ -95,6 +96,7 @@ rand_isaac = "0.3" [package.metadata.docs.rs] rustdoc-args = ["-Zunstable-options", "--generate-link-to-definition"] cargo-args = ["-Zunstable-options", "-Zrustdoc-scrape-examples"] +features = ["wavefront"] # The following listing is to allow for examples to be scraped, # see https://doc.rust-lang.org/rustdoc/scraped-examples.html#scraped-examples for details. diff --git a/crates/parry3d/examples/aabb3d.rs b/crates/parry3d/examples/aabb3d.rs index 171111a8..87101e6b 100644 --- a/crates/parry3d/examples/aabb3d.rs +++ b/crates/parry3d/examples/aabb3d.rs @@ -1,36 +1,84 @@ +mod common_macroquad3d; + extern crate nalgebra as na; +use common_macroquad3d::{lissajous_3d, mquad_from_na, na_from_mquad}; +use macroquad::prelude::*; use na::Isometry3; -use parry3d::bounding_volume::BoundingVolume; +use parry3d::bounding_volume::{Aabb, BoundingVolume}; use parry3d::shape::Ball; -fn main() { - /* - * Initialize the shapes. - */ - let ball1 = Ball::new(0.5); - let ball2 = Ball::new(1.0); - - let ball1_pos = Isometry3::translation(0.0, 1.0, 0.0); - let ball2_pos = Isometry3::identity(); // Identity matrix. - - /* - * Compute their axis-aligned bounding boxes. - */ - let aabb_ball1 = ball1.aabb(&ball1_pos); - let aabb_ball2 = ball2.aabb(&ball2_pos); - - // Merge the two boxes. - let bounding_aabb = aabb_ball1.merged(&aabb_ball2); - - // Enlarge the ball2 aabb. - let loose_aabb_ball2 = aabb_ball2.loosened(1.0); - - // Intersection and inclusion tests. - assert!(aabb_ball1.intersects(&aabb_ball2)); - assert!(bounding_aabb.contains(&aabb_ball1)); - assert!(bounding_aabb.contains(&aabb_ball2)); - assert!(!aabb_ball2.contains(&bounding_aabb)); - assert!(!aabb_ball1.contains(&bounding_aabb)); - assert!(loose_aabb_ball2.contains(&aabb_ball2)); +#[macroquad::main("parry2d::utils::point_in_poly2d")] +async fn main() { + let camera_pos = Vec3::new(8f32, 8f32, 12f32); + + loop { + let elapsed_time = get_time() as f32 * 0.7; + clear_background(BLACK); + // Initialize 3D camera. + set_camera(&Camera3D { + position: camera_pos, + up: Vec3::new(0f32, 1f32, 0f32), + target: Vec3::new(0.5f32, 0f32, 0.5f32), + ..Default::default() + }); + + /* + * Initialize the shapes. + */ + let ball1 = Ball::new(0.5); + let ball2 = Ball::new(1.0); + + let ball1_pos = na_from_mquad(lissajous_3d(elapsed_time)) * 4f32; + let ball2_pos = Isometry3::identity(); + + /* + * Compute their axis-aligned bounding boxes. + */ + let aabb_ball1 = ball1.aabb(&ball1_pos.into()); + let aabb_ball2 = ball2.aabb(&ball2_pos); + + // Merge the two boxes. + let bounding_aabb = aabb_ball1.merged(&aabb_ball2); + + // Enlarge the ball2 aabb. + let loose_aabb_ball2 = aabb_ball2.loosened(2.25f32); + + // Intersection and inclusion tests. + let color = if aabb_ball1.intersects(&aabb_ball2) { + RED + } else { + GREEN + }; + + assert!(bounding_aabb.contains(&aabb_ball1)); + assert!(bounding_aabb.contains(&aabb_ball2)); + assert!(loose_aabb_ball2.contains(&aabb_ball2)); + + let ball1_translation = mquad_from_na(ball1_pos.coords.into()); + draw_sphere(ball1_translation, ball1.radius, None, color); + let ball2_translation = mquad_from_na(ball2_pos.translation.vector.into()); + draw_sphere(ball2_translation, ball2.radius, None, color); + + draw_aabb(aabb_ball1, color); + draw_aabb(aabb_ball2, color); + draw_aabb(bounding_aabb, YELLOW); + + let color_included: Color = if loose_aabb_ball2.contains(&aabb_ball1) { + BLUE + } else { + MAGENTA + }; + draw_aabb(loose_aabb_ball2, color_included); + next_frame().await + } +} + +fn draw_aabb(aabb: Aabb, color: Color) { + let size = aabb.maxs - aabb.mins; + draw_cube_wires( + mquad_from_na(aabb.maxs - size / 2f32), + mquad_from_na(size.into()), + color, + ); } diff --git a/crates/parry3d/examples/bounding_sphere3d.rs b/crates/parry3d/examples/bounding_sphere3d.rs index 0cbf01f9..889085ea 100644 --- a/crates/parry3d/examples/bounding_sphere3d.rs +++ b/crates/parry3d/examples/bounding_sphere3d.rs @@ -1,36 +1,113 @@ +mod common_macroquad3d; + extern crate nalgebra as na; +use std::ops::Rem; + +use common_macroquad3d::{lissajous_3d, mquad_from_na, na_from_mquad}; +use macroquad::prelude::*; use na::{Isometry3, Vector3}; use parry3d::bounding_volume::BoundingVolume; use parry3d::shape::Cuboid; -fn main() { - /* - * Initialize the shapes. - */ - let cube1 = Cuboid::new(Vector3::repeat(0.5)); - let cube2 = Cuboid::new(Vector3::new(0.5, 1.0, 0.5)); - - let cube1_pos = Isometry3::translation(0.0, 0.0, 1.0); // 1.0 along the `z` axis. - let cube2_pos = Isometry3::identity(); // Identity matrix. - - /* - * Compute their bounding spheres. - */ - let bounding_sphere_cube1 = cube1.bounding_sphere(&cube1_pos); - let bounding_sphere_cube2 = cube2.bounding_sphere(&cube2_pos); - - // Merge the two spheres. - let bounding_bounding_sphere = bounding_sphere_cube1.merged(&bounding_sphere_cube2); - - // Enlarge the cube2 bounding sphere. - let loose_bounding_sphere_cube2 = bounding_sphere_cube2.loosened(1.0); - - // Intersection and inclusion tests. - assert!(bounding_sphere_cube1.intersects(&bounding_sphere_cube2)); - assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube1)); - assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube2)); - assert!(!bounding_sphere_cube2.contains(&bounding_bounding_sphere)); - assert!(!bounding_sphere_cube1.contains(&bounding_bounding_sphere)); - assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube2)); +#[macroquad::main("parry2d::utils::point_in_poly2d")] +async fn main() { + let camera_pos = Vec3::new(8f32, 8f32, 12f32); + + loop { + let elapsed_time = get_time() as f32 * 0.7; + clear_background(BLACK); + // Initialize 3D camera. + set_camera(&Camera3D { + position: camera_pos, + up: Vec3::new(0f32, 1f32, 0f32), + target: Vec3::new(0.5f32, 0f32, 0.5f32), + ..Default::default() + }); + + /* + * Initialize the shapes. + */ + let cube1 = Cuboid::new(Vector3::repeat(0.5)); + let cube2 = Cuboid::new(Vector3::new(0.5, 1.0, 0.5)); + + let cube1_pos = na_from_mquad(lissajous_3d(elapsed_time)) * 4f32; + let cube1_pos = Isometry3::from(cube1_pos); + let cube2_pos = Isometry3::identity(); // Identity matrix. + + /* + * Compute their bounding spheres. + */ + let bounding_sphere_cube1 = cube1.bounding_sphere(&cube1_pos); + let bounding_sphere_cube2 = cube2.bounding_sphere(&cube2_pos); + + // Merge the two spheres. + let bounding_bounding_sphere = bounding_sphere_cube1.merged(&bounding_sphere_cube2); + + // Enlarge the cube2 bounding sphere. + let loose_bounding_sphere_cube2 = bounding_sphere_cube2.loosened(3.0); + + // Intersection and inclusion tests. + let mut color = if bounding_sphere_cube1.intersects(&bounding_sphere_cube2) { + RED + } else { + GREEN + }; + color.a = 1f32 * (elapsed_time.rem(1f32) - 0.5).abs() * 2f32; + + // Due to float imprecisions, it's dangerous to assume that both shapes will be + // contained in the merged. + // You can leverage `BoundingVolume::loosened` with an epsilon for expected results. + // + // These might fail: + //assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube1)); + //assert!(bounding_bounding_sphere.contains(&bounding_sphere_cube2)); + assert!(loose_bounding_sphere_cube2.contains(&bounding_sphere_cube2)); + + let cube1_translation = mquad_from_na(cube1_pos.translation.vector.into()); + draw_cube_wires( + cube1_translation, + mquad_from_na(cube1.half_extents.into()) * 2f32, + WHITE, + ); + let cube2_translation = mquad_from_na(cube2_pos.translation.vector.into()); + draw_cube_wires( + cube2_translation, + mquad_from_na(cube2.half_extents.into()) * 2f32, + WHITE, + ); + + draw_sphere_wires( + mquad_from_na(bounding_sphere_cube1.center), + bounding_sphere_cube1.radius, + None, + color, + ); + draw_sphere_wires( + mquad_from_na(bounding_sphere_cube2.center), + bounding_sphere_cube2.radius, + None, + color, + ); + draw_sphere_wires( + mquad_from_na(bounding_bounding_sphere.center), + bounding_bounding_sphere.radius, + None, + YELLOW, + ); + + let color_included: Color = if loose_bounding_sphere_cube2.contains(&bounding_sphere_cube1) + { + BLUE + } else { + MAGENTA + }; + draw_sphere_wires( + mquad_from_na(loose_bounding_sphere_cube2.center), + loose_bounding_sphere_cube2.radius, + None, + color_included, + ); + next_frame().await + } } diff --git a/crates/parry3d/examples/common_macroquad3d.rs b/crates/parry3d/examples/common_macroquad3d.rs index 45aeb50e..093a8710 100644 --- a/crates/parry3d/examples/common_macroquad3d.rs +++ b/crates/parry3d/examples/common_macroquad3d.rs @@ -1,4 +1,3 @@ -#[allow(unused, dead_code)] use std::f32::consts::{FRAC_PI_2, FRAC_PI_4, FRAC_PI_6}; use macroquad::{ @@ -18,41 +17,66 @@ fn main() { ); } +/// Converts a [`nalgebra::Point3`] to a [`Vec3`], which is used by [`macroquad`] #[allow(dead_code)] pub fn mquad_from_na(a: Point3) -> Vec3 { Vec3::new(a.x, a.y, a.z) } +/// Converts a [`Vec3`] to a [`nalgebra::Point3`], which is used by [`parry3d`] #[allow(dead_code)] pub fn na_from_mquad(a: Vec3) -> Point3 { Point3::new(a.x, a.y, a.z) } +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates for time `t`. +/// +/// This uses hardcoded parameters to have an arbitrary pleasing trajectory. #[allow(dead_code)] pub fn lissajous_3d(t: f32) -> Vec3 { // Some hardcoded parameters to have a pleasing lissajous trajectory. - let (a, b, c, delta_x, delta_y, delta_z) = (3.0, 2.0, 1.0, FRAC_PI_2, FRAC_PI_4, FRAC_PI_6); + lissajous_3d_with_params(t, 3.0, 2.0, 1.0, FRAC_PI_2, FRAC_PI_4, FRAC_PI_6) +} +/// Returns [lissajous curve](https://en.wikipedia.org/wiki/Lissajous_curve) coordinates. +#[allow(dead_code)] +pub fn lissajous_3d_with_params( + t: f32, + a: f32, + b: f32, + c: f32, + delta_x: f32, + delta_y: f32, + delta_z: f32, +) -> Vec3 { let x = (a * t + delta_x).sin(); let y = (b * t + delta_y).sin(); let z = (c * t + delta_z).sin(); Vec3::new(x, y, z) * 0.75f32 } +/// Uses [`macroquad`] to display the line passed as parameter. #[allow(dead_code)] -pub fn draw_polyline(polygon: Vec<(Vec3, Vec3)>, color: Color) { - for i in 0..polygon.len() { - let a = polygon[i].0; - let b = polygon[i].1; +pub fn draw_polyline(polyline: Vec<(Vec3, Vec3)>, color: Color) { + for line in polyline { + let a = line.0; + let b = line.1; draw_line_3d(a, b, color); } } +/// Draws a text in the top left corner of the screen. +/// +/// This uses a hardcoded position, size, color. #[allow(dead_code)] pub fn easy_draw_text(text: &str) { macroquad::text::draw_text(text, 10.0, 48.0 + 18.0, 30.0, WHITE); } +/// Create a usable mesh for [`macroquad`]. +/// +/// This duplicates the trimesh vertices, computes their normals, +/// and bakes light into its vertices colors using [`mquad_compute_normals_and_bake_light`]. #[allow(dead_code)] pub fn mquad_mesh_from_points( trimesh: &(Vec>, Vec<[u32; 3]>), @@ -94,6 +118,7 @@ pub fn mquad_mesh_from_points( mesh } +/// Bakes light into vertices, using an hardcoded light strength. #[allow(dead_code)] pub fn mquad_compute_normals_and_bake_light( points: &Vec, diff --git a/crates/parry3d/examples/convex_hull3d.rs b/crates/parry3d/examples/convex_hull3d.rs index c75fb956..32d2fcc5 100644 --- a/crates/parry3d/examples/convex_hull3d.rs +++ b/crates/parry3d/examples/convex_hull3d.rs @@ -1,21 +1,52 @@ -extern crate nalgebra as na; +mod common_macroquad3d; -use na::Point3; +use std::f32::consts::{FRAC_PI_2, FRAC_PI_4, FRAC_PI_6}; + +use common_macroquad3d::{ + lissajous_3d_with_params, mquad_from_na, mquad_mesh_from_points, na_from_mquad, +}; +use macroquad::prelude::*; +use nalgebra::Point3; use parry3d::transformation; -fn main() { - let points = vec![ - Point3::new(0.77705324, 0.05374551, 0.9822232), - Point3::new(0.35096353, 0.9873069, 0.28922123), - Point3::new(0.09537989, 0.44411153, 0.05486667), - Point3::new(0.108208835, 0.72445065, 0.6669141), - Point3::new(0.7661844, 0.86163324, 0.80507314), - Point3::new(0.5185994, 0.66594696, 0.072779536), - Point3::new(0.768981, 0.23657233, 0.44346774), - Point3::new(0.058607936, 0.09037298, 0.017009139), - Point3::new(0.8818559, 0.3804205, 0.25173646), - Point3::new(0.9571466, 0.17664945, 0.6029223), - ]; +#[macroquad::main("parry2d::utils::point_in_poly2d")] +async fn main() { + let count = 9; + let mut pts = vec![Point3::default(); count]; + + let camera_pos = Vec3::new(8.0, 8.0, 8.0); + loop { + let elapsed_time = get_time() as f32; + let elapsed_time_slow = elapsed_time * 0.2; + clear_background(BLACK); - let _ = transformation::convex_hull(&points[..]); + for (i, pt) in pts.iter_mut().enumerate() { + *pt = na_from_mquad(lissajous_3d_with_params( + (i * i) as f32 + elapsed_time_slow, + 2.0 + i as f32 / 3.0, + 1f32 + (i as f32).sin() * 0.2, + (i as f32 / count as f32) + elapsed_time_slow.cos() * 0.1, + (elapsed_time_slow as f32 + i as f32).cos() * 0.1 + FRAC_PI_2, + FRAC_PI_4, + FRAC_PI_6, + )) * 5f32; + draw_sphere(mquad_from_na(*pt), 0.1f32, None, RED); + } + // Initialize 3D camera. + set_camera(&Camera3D { + position: camera_pos, + up: Vec3::new(0f32, 1f32, 0f32), + target: Vec3::new(0.5f32, 0f32, 0.5f32), + ..Default::default() + }); + /* + * + * Compute the convex hull. + * + */ + let convex_hull = transformation::convex_hull(&pts); + let mesh = mquad_mesh_from_points(&convex_hull, Vec3::new(5.0, 10.0, 3.0), DARKGRAY); + draw_mesh(&mesh); + next_frame().await + } } diff --git a/src/lib.rs b/src/lib.rs index d4c3cb6e..ab47d350 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,7 +11,7 @@ the rust programming language. #![deny(unused_parens)] #![deny(non_upper_case_globals)] #![deny(unused_results)] -#![deny(missing_docs)] +#![warn(missing_docs)] #![warn(unused_imports)] #![allow(missing_copy_implementations)] #![allow(clippy::too_many_arguments)] // Maybe revisit this one later. @@ -20,7 +20,7 @@ the rust programming language. #![allow(clippy::type_complexity)] // Complains about closures that are fairly simple. #![doc(html_root_url = "http://docs.rs/parry/0.1.1")] #![cfg_attr(not(feature = "std"), no_std)] -#![deny(unused_qualifications)] // TODO: deny that everytime +#![deny(unused_qualifications)] #[cfg(all( feature = "simd-is-enabled", diff --git a/src/query/contact/contact_support_map_support_map.rs b/src/query/contact/contact_support_map_support_map.rs index 831ba246..240aa421 100644 --- a/src/query/contact/contact_support_map_support_map.rs +++ b/src/query/contact/contact_support_map_support_map.rs @@ -19,12 +19,16 @@ where { let simplex = &mut VoronoiSimplex::new(); match contact_support_map_support_map_with_params(pos12, g1, g2, prediction, simplex, None) { - GJKResult::ClosestPoints(point1, point2_1, normal1) => { + GJKResult::ClosestPoints(point1, point2_1, Ok(normal1)) => { let dist = (point2_1 - point1).dot(&normal1); let point2 = pos12.inverse_transform_point(&point2_1); let normal2 = pos12.inverse_transform_unit_vector(&-normal1); Some(Contact::new(point1, point2, normal1, normal2, dist)) } + GJKResult::ClosestPoints(_, _, Err(_)) => { + // TODO: propagate the error. + None + } GJKResult::NoIntersection(_) => None, GJKResult::Intersection => unreachable!(), GJKResult::Proximity(_) => unreachable!(), @@ -68,6 +72,7 @@ where // The point is inside of the CSO: use the fallback algorithm let mut epa = EPA::new(); + if let Some((p1, p2, n)) = epa.closest_points(pos12, g1, g2, simplex) { return GJKResult::ClosestPoints(p1, p2, n); } diff --git a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs index bb13a838..20058300 100644 --- a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs @@ -79,7 +79,7 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( manifold.clear(); match contact { - GJKResult::ClosestPoints(p1, p2_1, dir) => { + GJKResult::ClosestPoints(p1, p2_1, Ok(dir)) => { let mut local_n1 = dir; let mut local_n2 = pos12.inverse_transform_unit_vector(&-dir); let dist = (p2_1 - p1).dot(&local_n1); diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 02357f08..2ceb89c3 100644 --- a/src/query/epa/epa2.rs +++ b/src/query/epa/epa2.rs @@ -8,6 +8,7 @@ use num::Bounded; use crate::math::{Isometry, Point, Real, Vector}; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::FaceDegenerate; use crate::shape::SupportMap; use crate::utils; @@ -52,7 +53,7 @@ impl Ord for FaceId { #[derive(Clone, Debug)] struct Face { pts: [usize; 2], - normal: Unit>, + normal: Result>, FaceDegenerate>, proj: Point, bcoords: [Real; 2], deleted: bool, @@ -83,10 +84,10 @@ impl Face { if let Some(n) = utils::ccw_face_normal([&vertices[pts[0]].point, &vertices[pts[1]].point]) { - normal = n; + normal = Ok(n); deleted = false; } else { - normal = Unit::new_unchecked(na::zero()); + normal = Err(FaceDegenerate); deleted = true; } @@ -158,7 +159,11 @@ impl EPA { g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<( + Point, + Point, + Result>, FaceDegenerate>, + )> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -213,7 +218,7 @@ impl EPA { } } - return Some((Point::origin(), Point::origin(), n)); + return Some((Point::origin(), Point::origin(), Ok(n))); } else if simplex.dimension() == 2 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; @@ -235,18 +240,24 @@ impl EPA { self.faces.push(face3); if proj_is_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + if let Ok(normal) = self.faces[0].normal { + let dist1 = normal.dot(&self.vertices[0].point.coords); + self.heap.push(FaceId::new(0, -dist1)?); + } } if proj_is_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + if let Ok(normal) = self.faces[1].normal { + let dist2 = normal.dot(&self.vertices[1].point.coords); + self.heap.push(FaceId::new(1, -dist2)?); + } } if proj_is_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + if let Ok(normal) = self.faces[2].normal { + let dist3 = normal.dot(&self.vertices[2].point.coords); + self.heap.push(FaceId::new(2, -dist3)?); + } } } else { let pts1 = [0, 1]; @@ -265,8 +276,17 @@ impl EPA { pts2, )); - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); + let dist1 = self.faces[0] + .normal + .as_ref() + .map(|normal| normal.dot(&self.vertices[0].point.coords)) + .unwrap_or(0.0); + + let dist2 = self.faces[1] + .normal + .as_ref() + .map(|normal| normal.dot(&self.vertices[1].point.coords)) + .unwrap_or(0.0); self.heap.push(FaceId::new(0, dist1)?); self.heap.push(FaceId::new(1, dist2)?); @@ -287,12 +307,15 @@ impl EPA { if face.deleted { continue; } + let Ok(face_normal) = face.normal else { + continue; + }; - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face_normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); + let candidate_max_dist = cso_point.point.coords.dot(&face_normal); if candidate_max_dist < max_dist { best_face_id = face_id; @@ -308,7 +331,7 @@ impl EPA { { let best_face = &self.faces[best_face_id.id]; let cpts = best_face.closest_points(&self.vertices); - return Some((cpts.0, cpts.1, best_face.normal)); + return Some((cpts.0, cpts.1, best_face.normal.clone())); } old_dist = curr_dist; @@ -323,12 +346,17 @@ impl EPA { for f in new_faces.iter() { if f.1 { - let dist = f.0.normal.dot(&f.0.proj.coords); + let new_face_normal = + f.0.normal + .clone() + .unwrap_or(Unit::new_unchecked(Vector::zeros())); + + let dist = new_face_normal.dot(&f.0.proj.coords); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. let cpts = f.0.closest_points(&self.vertices); - return Some((cpts.0, cpts.1, f.0.normal)); + return Some((cpts.0, cpts.1, Ok(new_face_normal))); } if !f.0.deleted { @@ -349,7 +377,7 @@ impl EPA { let best_face = &self.faces[best_face_id.id]; let cpts = best_face.closest_points(&self.vertices); - Some((cpts.0, cpts.1, best_face.normal)) + Some((cpts.0, cpts.1, best_face.normal.clone())) } } diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 9f8bdc90..c195a1c2 100644 --- a/src/query/epa/epa3.rs +++ b/src/query/epa/epa3.rs @@ -2,7 +2,7 @@ use crate::math::{Isometry, Point, Real, Vector}; use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex}; -use crate::query::PointQueryWithLocation; +use crate::query::{FaceDegenerate, PointQueryWithLocation}; use crate::shape::{SupportMap, Triangle, TrianglePointLocation}; use crate::utils; use na::{self, Unit}; @@ -52,7 +52,7 @@ impl Ord for FaceId { struct Face { pts: [usize; 3], adj: [usize; 3], - normal: Unit>, + normal: Result>, FaceDegenerate>, bcoords: [Real; 3], deleted: bool, } @@ -71,13 +71,9 @@ impl Face { &vertices[pts[1]].point, &vertices[pts[2]].point, ]) { - normal = n; + normal = Ok(n); } else { - // This is a bit of a hack for degenerate faces. - // TODO: It will work OK with our current code, though - // we should do this in another way to avoid any risk - // of misusing the face normal in the future. - normal = Unit::new_unchecked(na::zero()); + normal = Err(FaceDegenerate); } Face { @@ -149,8 +145,13 @@ impl Face { // have a zero normal, causing the dot product to be zero. // So return true for these case will let us skip the triangle // during silhouette computation. - (*pt - *p0).dot(&self.normal) >= -gjk::eps_tol() - || Triangle::new(*p1, *p2, *pt).is_affinely_dependent() + match &self.normal { + Ok(normal) => { + (*pt - *p0).dot(normal) >= -gjk::eps_tol() + || Triangle::new(*p1, *p2, *pt).is_affinely_dependent() + } + Err(_) => true, + } } } @@ -215,7 +216,11 @@ impl EPA { g1: &G1, g2: &G2, simplex: &VoronoiSimplex, - ) -> Option<(Point, Point, Unit>)> + ) -> Option<( + Point, + Point, + Result>, FaceDegenerate>, + )> where G1: ?Sized + SupportMap, G2: ?Sized + SupportMap, @@ -235,7 +240,7 @@ impl EPA { if simplex.dimension() == 0 { let mut n: Vector = na::zero(); n[1] = 1.0; - return Some((Point::origin(), Point::origin(), Unit::new_unchecked(n))); + return Some((Point::origin(), Point::origin(), Ok(Unit::new_unchecked(n)))); } else if simplex.dimension() == 3 { let dp1 = self.vertices[1] - self.vertices[0]; let dp2 = self.vertices[2] - self.vertices[0]; @@ -266,23 +271,31 @@ impl EPA { self.faces.push(face4); if proj_inside1 { - let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords); - self.heap.push(FaceId::new(0, -dist1)?); + if let Ok(normal) = self.faces[0].normal { + let dist1 = normal.dot(&self.vertices[0].point.coords); + self.heap.push(FaceId::new(0, -dist1)?); + } } if proj_inside2 { - let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords); - self.heap.push(FaceId::new(1, -dist2)?); + if let Ok(normal) = self.faces[1].normal { + let dist2 = normal.dot(&self.vertices[1].point.coords); + self.heap.push(FaceId::new(1, -dist2)?); + } } if proj_inside3 { - let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords); - self.heap.push(FaceId::new(2, -dist3)?); + if let Ok(normal) = self.faces[2].normal { + let dist3 = normal.dot(&self.vertices[2].point.coords); + self.heap.push(FaceId::new(2, -dist3)?); + } } if proj_inside4 { - let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords); - self.heap.push(FaceId::new(3, -dist4)?); + if let Ok(normal) = self.faces[3].normal { + let dist4 = normal.dot(&self.vertices[3].point.coords); + self.heap.push(FaceId::new(3, -dist4)?); + } } } else { if simplex.dimension() == 1 { @@ -323,17 +336,17 @@ impl EPA { while let Some(face_id) = self.heap.pop() { // Create new faces. let face = self.faces[face_id.id].clone(); - if face.deleted { continue; } - - let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal); + let Ok(face_normal) = &face.normal else { + continue; + }; + let cso_point = CSOPoint::from_shapes(pos12, g1, g2, face_normal); + let candidate_max_dist = cso_point.point.coords.dot(face_normal); let support_point_id = self.vertices.len(); self.vertices.push(cso_point); - let candidate_max_dist = cso_point.point.coords.dot(&face.normal); - if candidate_max_dist < max_dist { best_face_id = face_id; max_dist = candidate_max_dist; @@ -347,8 +360,9 @@ impl EPA { ((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist) { let best_face = &self.faces[best_face_id.id]; + let best_face_normal = face.normal.unwrap_or(Unit::new_unchecked(Vector::zeros())); let points = best_face.closest_points(&self.vertices); - return Some((points.0, points.1, best_face.normal)); + return Some((points.0, points.1, Ok(best_face_normal))); } old_dist = curr_dist; @@ -388,12 +402,16 @@ impl EPA { if new_face.1 { let pt = self.vertices[self.faces[new_face_id].pts[0]].point.coords; - let dist = self.faces[new_face_id].normal.dot(&pt); + let new_face_normal = self.faces[new_face_id] + .normal + .clone() + .unwrap_or(Unit::new_unchecked(Vector::zeros())); + let dist = new_face_normal.dot(&pt); if dist < curr_dist { // TODO: if we reach this point, there were issues due to // numerical errors. let points = face.closest_points(&self.vertices); - return Some((points.0, points.1, face.normal)); + return Some((points.0, points.1, Ok(new_face_normal))); } self.heap.push(FaceId::new(new_face_id, -dist)?); @@ -423,7 +441,7 @@ impl EPA { let best_face = &self.faces[best_face_id.id]; let points = best_face.closest_points(&self.vertices); - Some((points.0, points.1, best_face.normal)) + Some((points.0, points.1, best_face.normal.clone())) } fn compute_silhouette(&mut self, point: usize, id: usize, opp_pt_id: usize) { diff --git a/src/query/gjk/gjk.rs b/src/query/gjk/gjk.rs index ebee30ae..88d4d89a 100644 --- a/src/query/gjk/gjk.rs +++ b/src/query/gjk/gjk.rs @@ -3,6 +3,7 @@ use na::{self, ComplexField, Unit}; use crate::query::gjk::{CSOPoint, ConstantOrigin, VoronoiSimplex}; +use crate::query::FaceDegenerate; use crate::shape::SupportMap; // use query::Proximity; use crate::math::{Isometry, Point, Real, Vector, DIM}; @@ -19,7 +20,11 @@ pub enum GJKResult { /// /// Both points and vector are expressed in the local-space of the first geometry involved /// in the GJK execution. - ClosestPoints(Point, Point, Unit>), + ClosestPoints( + Point, + Point, + Result>, FaceDegenerate>, + ), /// Result of the GJK algorithm when the origin is too close to the polytope but not inside of it. /// /// The returned vector is expressed in the local-space of the first geometry involved in the @@ -125,7 +130,7 @@ where if max_bound >= old_max_bound { if exact_dist { let (p1, p2) = result(simplex, true); - return GJKResult::ClosestPoints(p1, p2, old_dir); // upper bounds inconsistencies + return GJKResult::ClosestPoints(p1, p2, Ok(old_dir)); // upper bounds inconsistencies } else { return GJKResult::Proximity(old_dir); } @@ -143,7 +148,7 @@ where } else if max_bound - min_bound <= _eps_rel * max_bound { if exact_dist { let (p1, p2) = result(simplex, false); - return GJKResult::ClosestPoints(p1, p2, dir); // the distance found has a good enough precision + return GJKResult::ClosestPoints(p1, p2, Ok(dir)); // the distance found has a good enough precision } else { return GJKResult::Proximity(dir); } @@ -152,7 +157,7 @@ where if !simplex.add_point(cso_point) { if exact_dist { let (p1, p2) = result(simplex, false); - return GJKResult::ClosestPoints(p1, p2, dir); + return GJKResult::ClosestPoints(p1, p2, Ok(dir)); } else { return GJKResult::Proximity(dir); } @@ -165,7 +170,7 @@ where if min_bound >= _eps_tol { if exact_dist { let (p1, p2) = result(simplex, true); - return GJKResult::ClosestPoints(p1, p2, old_dir); + return GJKResult::ClosestPoints(p1, p2, Ok(old_dir)); } else { // NOTE: previous implementation used old_proj here. return GJKResult::Proximity(old_dir); diff --git a/src/query/gjk/voronoi_simplex2.rs b/src/query/gjk/voronoi_simplex2.rs index 451eef92..e69d230a 100644 --- a/src/query/gjk/voronoi_simplex2.rs +++ b/src/query/gjk/voronoi_simplex2.rs @@ -98,11 +98,8 @@ impl VoronoiSimplex { self.proj[0] = 1.0; self.vertices[0].point } else if self.dim == 1 { - // TODO: NLL - let (proj, location) = { - let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point_and_get_location(&Point::::origin(), true) - }; + let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) + .project_local_point_and_get_location(&Point::::origin(), true); match location { SegmentPointLocation::OnVertex(0) => { @@ -123,15 +120,12 @@ impl VoronoiSimplex { proj.point } else { assert!(self.dim == 2); - // TODO: NLL - let (proj, location) = { - let tri = Triangle::new( - self.vertices[0].point, - self.vertices[1].point, - self.vertices[2].point, - ); - tri.project_local_point_and_get_location(&Point::::origin(), true) - }; + let (proj, location) = Triangle::new( + self.vertices[0].point, + self.vertices[1].point, + self.vertices[2].point, + ) + .project_local_point_and_get_location(&Point::::origin(), true); match location { TrianglePointLocation::OnVertex(i) => { diff --git a/src/query/gjk/voronoi_simplex3.rs b/src/query/gjk/voronoi_simplex3.rs index 72c141b7..8742e58d 100644 --- a/src/query/gjk/voronoi_simplex3.rs +++ b/src/query/gjk/voronoi_simplex3.rs @@ -125,11 +125,8 @@ impl VoronoiSimplex { self.proj[0] = 1.0; self.vertices[0].point } else if self.dim == 1 { - // TODO: NLL - let (proj, location) = { - let seg = Segment::new(self.vertices[0].point, self.vertices[1].point); - seg.project_local_point_and_get_location(&Point::::origin(), true) - }; + let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point) + .project_local_point_and_get_location(&Point::::origin(), true); match location { SegmentPointLocation::OnVertex(0) => { @@ -150,15 +147,12 @@ impl VoronoiSimplex { proj.point } else if self.dim == 2 { - // TODO: NLL - let (proj, location) = { - let tri = Triangle::new( - self.vertices[0].point, - self.vertices[1].point, - self.vertices[2].point, - ); - tri.project_local_point_and_get_location(&Point::::origin(), true) - }; + let (proj, location) = Triangle::new( + self.vertices[0].point, + self.vertices[1].point, + self.vertices[2].point, + ) + .project_local_point_and_get_location(&Point::::origin(), true); match location { TrianglePointLocation::OnVertex(i) => { @@ -192,16 +186,13 @@ impl VoronoiSimplex { proj.point } else { assert!(self.dim == 3); - // TODO: NLL - let (proj, location) = { - let tetr = Tetrahedron::new( - self.vertices[0].point, - self.vertices[1].point, - self.vertices[2].point, - self.vertices[3].point, - ); - tetr.project_local_point_and_get_location(&Point::::origin(), true) - }; + let (proj, location) = Tetrahedron::new( + self.vertices[0].point, + self.vertices[1].point, + self.vertices[2].point, + self.vertices[3].point, + ) + .project_local_point_and_get_location(&Point::::origin(), true); match location { TetrahedronPointLocation::OnVertex(i) => { diff --git a/src/query/mod.rs b/src/query/mod.rs index ed6e1537..aa6cfaa4 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -80,3 +80,7 @@ pub mod details { pub use super::ray::*; pub use super::shape_cast::*; } + +/// Represents a degenerate Face normal. +#[derive(Clone, Debug, PartialEq, Eq)] +pub struct FaceDegenerate; diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index 4c9489c6..d20bd478 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -79,7 +79,7 @@ pub struct RayIntersection { /// Otherwise, the normal points outward. /// /// If the `time_of_impact` is exactly zero, the normal might not be reliable. - // TODO: use a Unit instead. // TODO: Thierry: should we use Unit for [`Ray::dir`] too ? + // TODO: use a Unit instead. pub normal: Vector, /// Feature at the intersection point. diff --git a/src/transformation/mesh_intersection/mesh_intersection.rs b/src/transformation/mesh_intersection/mesh_intersection.rs index c1f8170d..06b5cc48 100644 --- a/src/transformation/mesh_intersection/mesh_intersection.rs +++ b/src/transformation/mesh_intersection/mesh_intersection.rs @@ -5,8 +5,6 @@ use crate::query::{visitors::BoundingVolumeIntersectionsSimultaneousVisitor, Poi use crate::shape::{TriMesh, Triangle}; use crate::utils; use na::{Point3, Vector3}; -#[cfg(feature = "wavefront")] -use obj::{Group, IndexTuple, ObjData, Object, SimplePolygon}; use rstar::RTree; use spade::{ConstrainedDelaunayTriangulation, InsertionError, Triangulation as _}; use std::collections::BTreeMap; @@ -513,7 +511,7 @@ fn insert_into_set( fn smallest_angle(points: &[Point3]) -> Real { let n = points.len(); - let mut worst_cos = -2.0; + let mut worst_cos: Real = -2.0; for i in 0..points.len() { let d1 = (points[i] - points[(i + 1) % n]).normalize(); let d2 = (points[(i + 2) % n] - points[(i + 1) % n]).normalize(); @@ -669,9 +667,9 @@ fn merge_triangle_sets( #[cfg(test)] mod tests { use super::*; - use crate::shape::TriMeshFlags; - use crate::transformation::wavefront::*; + use crate::shape::{Ball, Cuboid, TriMeshFlags}; use obj::Obj; + use obj::ObjData; #[test] fn test_same_mesh_intersection() { @@ -685,7 +683,7 @@ mod tests { let mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as f64, v[1] as f64, v[2] as f64)) + .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -693,9 +691,10 @@ mod tests { .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32]) .collect::>(), TriMeshFlags::all(), - ); + ) + .unwrap(); - let res = intersect_meshes( + let _ = intersect_meshes( &Isometry::identity(), &mesh, false, @@ -706,7 +705,34 @@ mod tests { .unwrap() .unwrap(); - mesh.to_obj_file(&PathBuf::from("same_test.obj")); + let _ = mesh.to_obj_file(&PathBuf::from("same_test.obj")); + } + + #[test] + fn test_non_origin_pos1_pos2_intersection() { + let ball = Ball::new(2f32 as Real).to_trimesh(10, 10); + let cuboid = Cuboid::new(Vector3::new(2.0, 1.0, 1.0)).to_trimesh(); + let mut sphere_mesh = TriMesh::new(ball.0, ball.1).unwrap(); + sphere_mesh.set_flags(TriMeshFlags::all()).unwrap(); + let mut cuboid_mesh = TriMesh::new(cuboid.0, cuboid.1).unwrap(); + cuboid_mesh.set_flags(TriMeshFlags::all()).unwrap(); + + let res = intersect_meshes( + &Isometry::translation(1.0, 0.0, 0.0), + &cuboid_mesh, + false, + &Isometry::translation(2.0, 0.0, 0.0), + &sphere_mesh, + false, + ) + .unwrap() + .unwrap(); + + let _ = res.to_obj_file(&PathBuf::from("test_non_origin_pos1_pos2_intersection.obj")); + + let bounding_sphere = res.local_bounding_sphere(); + assert!(bounding_sphere.center == Point3::new(1.5, 0.0, 0.0)); + assert_relative_eq!(2.0615528, bounding_sphere.radius, epsilon = 1.0e-5); } #[test] @@ -721,7 +747,7 @@ mod tests { let offset_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as f64, v[1] as f64, v[2] as f64)) + .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -729,7 +755,8 @@ mod tests { .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32]) .collect::>(), TriMeshFlags::all(), - ); + ) + .unwrap(); let Obj { data: ObjData { @@ -741,7 +768,7 @@ mod tests { let center_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as f64, v[1] as f64, v[2] as f64)) + .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -749,7 +776,8 @@ mod tests { .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32]) .collect::>(), TriMeshFlags::all(), - ); + ) + .unwrap(); let res = intersect_meshes( &Isometry::identity(), @@ -762,7 +790,7 @@ mod tests { .unwrap() .unwrap(); - res.to_obj_file(&PathBuf::from("offset_test.obj")); + let _ = res.to_obj_file(&PathBuf::from("offset_test.obj")); } #[test] @@ -777,7 +805,7 @@ mod tests { let stair_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as f64, v[1] as f64, v[2] as f64)) + .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -785,7 +813,8 @@ mod tests { .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32]) .collect::>(), TriMeshFlags::all(), - ); + ) + .unwrap(); let Obj { data: ObjData { @@ -797,7 +826,7 @@ mod tests { let bar_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as f64, v[1] as f64, v[2] as f64)) + .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -805,7 +834,8 @@ mod tests { .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32]) .collect::>(), TriMeshFlags::all(), - ); + ) + .unwrap(); let res = intersect_meshes( &Isometry::identity(), @@ -818,7 +848,7 @@ mod tests { .unwrap() .unwrap(); - res.to_obj_file(&PathBuf::from("stair_test.obj")); + let _ = res.to_obj_file(&PathBuf::from("stair_test.obj")); } #[test] @@ -833,7 +863,7 @@ mod tests { let bunny_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as f64, v[1] as f64, v[2] as f64)) + .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -841,7 +871,8 @@ mod tests { .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32]) .collect::>(), TriMeshFlags::all(), - ); + ) + .unwrap(); let Obj { data: ObjData { @@ -853,7 +884,7 @@ mod tests { let cylinder_mesh = TriMesh::with_flags( position .iter() - .map(|v| Point3::new(v[0] as f64, v[1] as f64, v[2] as f64)) + .map(|v| Point3::new(v[0] as Real, v[1] as Real, v[2] as Real)) .collect::>(), objects[0].groups[0] .polys @@ -861,7 +892,8 @@ mod tests { .map(|p| [p.0[0].0 as u32, p.0[1].0 as u32, p.0[2].0 as u32]) .collect::>(), TriMeshFlags::all(), - ); + ) + .unwrap(); let res = intersect_meshes( &Isometry::identity(), @@ -874,6 +906,6 @@ mod tests { .unwrap() .unwrap(); - res.to_obj_file(&PathBuf::from("complex_test.obj")); + let _ = res.to_obj_file(&PathBuf::from("complex_test.obj")); } } diff --git a/src/transformation/mod.rs b/src/transformation/mod.rs index 653c0c6d..ca974549 100644 --- a/src/transformation/mod.rs +++ b/src/transformation/mod.rs @@ -40,4 +40,4 @@ mod to_trimesh; pub mod utils; #[cfg(feature = "wavefront")] -pub mod wavefront; +mod wavefront; diff --git a/src/transformation/wavefront.rs b/src/transformation/wavefront.rs index 829d6749..257e81fb 100644 --- a/src/transformation/wavefront.rs +++ b/src/transformation/wavefront.rs @@ -3,20 +3,24 @@ use obj::{Group, IndexTuple, ObjData, ObjError, Object, SimplePolygon}; use std::path::PathBuf; impl TriMesh { + /// Outputs a Wavefront (`.obj`) file at the given path. + /// + /// This function is enabled by the `wavefront` feature flag. pub fn to_obj_file(&self, path: &PathBuf) -> Result<(), ObjError> { let mut file = std::fs::File::create(path).unwrap(); ObjData { + #[expect(clippy::unnecessary_cast)] position: self .vertices() - .into_iter() + .iter() .map(|v| [v.x as f32, v.y as f32, v.z as f32]) .collect(), objects: vec![Object { groups: vec![Group { polys: self .indices() - .into_iter() + .iter() .map(|tri| { SimplePolygon(vec![ IndexTuple(tri[0] as usize, None, None),