From 06340e41ab7d038b75f9f3880d790d63095f419d Mon Sep 17 00:00:00 2001 From: Roland Eischer <11982367+MinimalCpp@users.noreply.github.com> Date: Thu, 5 Nov 2026 17:43:10 +0100 Subject: [PATCH] fix: correct heightfield shape-cast normals on internal edges A shape-cast onto a 3D `HeightField` with `HeightFieldFlags::FIX_INTERNAL_EDGES` could stop on the internal edge between two coplanar triangles and return that edge's normal, tilted a few degrees off the face, as each triangle is cast on its own and GJK's time of impact can come out early. With the flag, the winning hit's normal is now projected into the normal cone of the surface around its witness point, from the heightfield's triangles within a band of 1e-3 of the hit triangle's longest edge. A real crease keeps its normal; where the band reaches the heightfield's border or a removed triangle, the normal is left as it is. `normal2` follows `normal1`; the time of impact, the witnesses and the cast without the flag are unchanged. Co-Authored-By: Claude Opus 5.5 --- CHANGELOG.md | 7 + .../heightfield_shape_cast_internal_edges.rs | 479 ++++++++++++++++++ .../shape_cast_heightfield_shape.rs | 213 +++++++- src/shape/heightfield3.rs | 41 +- src/shape/mod.rs | 2 +- 5 files changed, 713 insertions(+), 29 deletions(-) create mode 100644 crates/parry3d/tests/heightfield_shape_cast_internal_edges.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index ec4c721d..a178972b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,13 @@ `UnknownContactManifoldsWorkspace`, which the relevant algorithm can replace with a workspace of the correct type at its next execution. +### Fixed + +- Fix shape-casts onto a 3D `HeightField` with `HeightFieldFlags::FIX_INTERNAL_EDGES` returning the + normal of an internal edge between coplanar triangles, tilted a few degrees off the face, when + the cast stops on that edge. The hit's normals are now projected into the normal cone of the + surface where the hit lies, from the triangles there, so a real crease keeps its own. + ## 0.31.1 ### Modified diff --git a/crates/parry3d/tests/heightfield_shape_cast_internal_edges.rs b/crates/parry3d/tests/heightfield_shape_cast_internal_edges.rs new file mode 100644 index 00000000..ab37510b --- /dev/null +++ b/crates/parry3d/tests/heightfield_shape_cast_internal_edges.rs @@ -0,0 +1,479 @@ +//! A shape cast onto a flat face of a heightfield with `HeightFieldFlags::FIX_INTERNAL_EDGES` +//! returns the face's normal, not the normal of an internal edge between two of its coplanar +//! triangles. +//! +//! The cast tests each triangle on its own, and its time of impact can come out early by up to +//! about a millimetre, so the contact with the edge of a neighbouring triangle can come out first, +//! tilted off the face by a few degrees. + +use parry3d::math::{Pose, Real, Rotation, Vector}; +use parry3d::query::{cast_shapes, ShapeCastHit, ShapeCastOptions}; +use parry3d::shape::{ + Capsule, Cuboid, HeightField, HeightFieldCellStatus, HeightFieldFlags, Shape, +}; +use parry3d::utils::Array2; + +/// How far a normal may lie from the face's: the noise of a cast onto a face, well below the +/// normal of an internal edge, which lies a few degrees off. +const TOLERANCE: Real = 1.0e-3; + +/// A heightfield 40 m wide along `x` on 1 m columns and `length` long along `z` on 40 rows, centred +/// on the origin, with `height` at `x`. +fn heightfield( + height: impl Fn(Real) -> Real, + flags: HeightFieldFlags, + length: Real, +) -> HeightField { + let samples = 41; + // Rows run along `z` and columns along `x`, stored column by column. + let heights = (0..samples) + .flat_map(|j| (0..samples).map(move |_| -20.0 + j as Real)) + .map(height) + .collect(); + HeightField::with_flags( + Array2::new(samples, samples, heights), + Vector::new(40.0, 1.0, length), + flags, + ) +} + +/// The normal of a face of slope `degrees` rising along `+x`, and its gradient. +fn face(degrees: Real) -> (Vector, Real) { + let angle = degrees.to_radians(); + (Vector::new(-angle.sin(), angle.cos(), 0.0), angle.tan()) +} + +/// The upright capsule of a character: radius 0.4 m, height 1.8 m. +fn capsule() -> Capsule { + Capsule::new_y(0.5, 0.4) +} + +/// How far down the capsule is cast. +const MAX_TIME_OF_IMPACT: Real = 0.32; + +/// The start points of the casts: a grid of 40 × 100 points 1 cm apart starting at `x0`, near and +/// off the heightfield's grid lines and the diagonals of its cells, at `start_height(x)`. +fn starts(x0: Real, start_height: impl Fn(Real) -> Real) -> impl Iterator { + (0..40).flat_map(move |ix| { + let x = x0 + 0.0013 + ix as Real * 0.01; + let y = start_height(x); + (0..100).map(move |iz| Vector::new(x, y, -1.0 + 0.00071 + iz as Real * 0.01)) + }) +} + +/// The hits of the capsule cast straight down onto `surface` at `surface_pose` from the +/// [`starts`]. Each hit comes with its start point, and its normals in world space. +fn casts( + surface: &dyn Shape, + surface_pose: &Pose, + x0: Real, + start_height: impl Fn(Real) -> Real, +) -> Vec<(Vector, ShapeCastHit)> { + let options = ShapeCastOptions::with_max_time_of_impact(MAX_TIME_OF_IMPACT); + starts(x0, start_height) + .map(|origin| { + let hit = cast_shapes( + surface_pose, + Vector::ZERO, + surface, + &Pose::from_translation(origin), + -Vector::Y, + &capsule(), + options, + ) + .expect("the cast is supported") + .unwrap_or_else(|| panic!("the cast from {origin:?} hits the surface")); + (origin, hit.transform1_by(surface_pose)) + }) + .collect() +} + +/// The height at `x` of the capsule's centre above the face of slope `degrees` rising along `+x` +/// through the origin: 0.2 m above where its bottom touches the face. +fn above_face(degrees: Real) -> impl Fn(Real) -> Real { + let (normal, gradient) = face(degrees); + let capsule = capsule(); + let touching = capsule.radius / normal.y + capsule.half_height(); + move |x| x * gradient + touching + 0.2 +} + +/// The hits of the capsule cast down onto the face of slope `degrees` rising along `+x` through +/// the origin, `surface` at `surface_pose`. +fn casts_onto_face( + surface: &dyn Shape, + surface_pose: &Pose, + degrees: Real, +) -> Vec<(Vector, ShapeCastHit)> { + casts(surface, surface_pose, 1.6, above_face(degrees)) +} + +/// The hits whose normals lie more than [`TOLERANCE`] off `face`. +fn off_face(hits: &[(Vector, ShapeCastHit)], face: Vector) -> Vec<(Vector, Vector, Vector)> { + hits.iter() + .filter(|(_, hit)| { + (hit.normal1 - face).length() > TOLERANCE || (hit.normal2 + face).length() > TOLERANCE + }) + .map(|(origin, hit)| (*origin, hit.normal1, hit.normal2)) + .collect() +} + +/// The heights of a ridge: flat up to `x = 1`, a grid line, then falling along `+x` at `degrees`. +fn ridge(degrees: Real) -> impl Fn(Real) -> Real + Copy { + let (_, gradient) = face(degrees); + move |x| -(x - 1.0).max(0.0) * gradient +} + +/// The normal of the ridge of slope `degrees` at its point nearest `centre`, a point above it. +fn ridge_normal(degrees: Real, centre: Vector) -> Vector { + let angle = degrees.to_radians(); + let down_slope = Vector::new(angle.cos(), -angle.sin(), 0.0); + let on_flat = Vector::new(centre.x.min(1.0), 0.0, centre.z); + let on_ridge = Vector::new(1.0, 0.0, centre.z); + let on_slope = on_ridge + down_slope * (centre - on_ridge).dot(down_slope).max(0.0); + let nearest = if centre.distance(on_flat) <= centre.distance(on_slope) { + on_flat + } else { + on_slope + }; + (centre - nearest).normalize() +} + +#[test] +fn cast_onto_a_tilted_heightfield_face_returns_the_face_normal() { + for degrees in [45.16, 46.0, 48.0] { + let (normal, gradient) = face(degrees); + let hf = heightfield(|x| x * gradient, HeightFieldFlags::FIX_INTERNAL_EDGES, 40.0); + + let off = off_face(&casts_onto_face(&hf, &Pose::identity(), degrees), normal); + assert!( + off.is_empty(), + "{} casts onto the {degrees}° face return a normal off the face {normal:?}, e.g. {:?}", + off.len(), + &off[..off.len().min(3)], + ); + } +} + +#[test] +fn cast_onto_a_cuboid_in_the_face_plane_returns_the_face_normal() { + for degrees in [45.16, 46.0, 48.0] { + let (normal, _) = face(degrees); + // A slab whose top face lies in the face's plane through the origin. + let half_thickness = 0.5; + let slab = Cuboid::new(Vector::new(20.0, half_thickness, 20.0)); + let slab_pose = Pose::from_parts( + -normal * half_thickness, + Rotation::from_rotation_z(degrees.to_radians()), + ); + + let off = off_face(&casts_onto_face(&slab, &slab_pose, degrees), normal); + assert!( + off.is_empty(), + "{} casts onto the {degrees}° cuboid return a normal off the face {normal:?}, e.g. {:?}", + off.len(), + &off[..off.len().min(3)], + ); + } +} + +#[test] +fn the_flag_corrects_only_the_normals_of_a_cast() { + let degrees = 46.0; + let (normal, gradient) = face(degrees); + let fixed = heightfield(|x| x * gradient, HeightFieldFlags::FIX_INTERNAL_EDGES, 40.0); + let plain = heightfield(|x| x * gradient, HeightFieldFlags::empty(), 40.0); + let fixed_hits = casts_onto_face(&fixed, &Pose::identity(), degrees); + let plain_hits = casts_onto_face(&plain, &Pose::identity(), degrees); + + for ((origin, fixed), (_, plain)) in fixed_hits.iter().zip(&plain_hits) { + assert_eq!( + (fixed.time_of_impact, fixed.witness1, fixed.witness2), + (plain.time_of_impact, plain.witness1, plain.witness2), + "the cast from {origin:?} stops elsewhere with the flag", + ); + } + // Without the flag, the normals stay those of the triangles hit, internal edges included. + assert!( + !off_face(&plain_hits, normal).is_empty(), + "no cast stops on an internal edge, so the test no longer shows what the flag corrects", + ); +} + +#[test] +fn cast_onto_a_ridge_keeps_the_normal_of_its_edge() { + // Flat up to the ridge at `x = 1`, a grid line, then falling along `+x`. A capsule whose bottom + // sphere's centre lies past the ridge by less than `radius * sin(slope)` touches the ridge + // itself, with a normal between those of its two faces that the flag must keep. At 60°, one + // of the casts touches the ridge at a vertex of a slope triangle that meets the flat part + // only there. + for degrees in [46.0, 60.0] { + let (rising, _) = face(degrees); + let falling = Vector::new(-rising.x, rising.y, 0.0); + let fixed = heightfield(ridge(degrees), HeightFieldFlags::FIX_INTERNAL_EDGES, 40.0); + let plain = heightfield(ridge(degrees), HeightFieldFlags::empty(), 40.0); + let start_height = |_| capsule().radius + capsule().half_height() + 0.1; + let fixed_hits = casts(&fixed, &Pose::identity(), 0.8, start_height); + let plain_hits = casts(&plain, &Pose::identity(), 0.8, start_height); + + let mut on_ridge = 0; + for ((origin, fixed), (_, plain)) in fixed_hits.iter().zip(&plain_hits) { + assert!( + (fixed.normal1 - plain.normal1).length() <= TOLERANCE, + "the cast from {origin:?} bends the {degrees}° ridge's normal {:?} to {:?}", + plain.normal1, + fixed.normal1, + ); + if (plain.normal1 - Vector::Y).length() > TOLERANCE + && (plain.normal1 - falling).length() > TOLERANCE + { + on_ridge += 1; + } + } + assert!( + on_ridge > 0, + "no cast stops on the {degrees}° ridge, so the test no longer shows that its normal \ + is kept", + ); + } +} + +#[test] +fn cast_onto_a_ridge_of_long_cells_keeps_the_normal_of_its_edge() { + // Cells 1 km long along the ridge make slivers of the triangles beside it, whose edges run + // nearly parallel to it. The casts there are noisier, so each normal is held against the + // ridge's own rather than the normal without the flag: the flag may bring it closer, never + // move it away. + let degrees = 46.0; + let fixed = heightfield( + ridge(degrees), + HeightFieldFlags::FIX_INTERNAL_EDGES, + 40_000.0, + ); + let plain = heightfield(ridge(degrees), HeightFieldFlags::empty(), 40_000.0); + let start_height = |_| capsule().radius + capsule().half_height() + 0.1; + let fixed_hits = casts(&fixed, &Pose::identity(), 0.8, start_height); + let plain_hits = casts(&plain, &Pose::identity(), 0.8, start_height); + + // The angle between a hit's normal and the ridge's normal at the capsule's bottom sphere. + let error = |origin: Vector, hit: &ShapeCastHit| { + let centre = origin - Vector::Y * (hit.time_of_impact + capsule().half_height()); + hit.normal1.angle_between(ridge_normal(degrees, centre)) + }; + for ((origin, fixed), (_, plain)) in fixed_hits.iter().zip(&plain_hits) { + assert!( + error(*origin, fixed) <= error(*origin, plain) + TOLERANCE, + "the cast from {origin:?} moves the ridge's normal {:?} away to {:?}", + plain.normal1, + fixed.normal1, + ); + } +} + +#[test] +fn the_flag_corrects_the_normals_in_the_frame_of_each_shape() { + // The heightfield is turned about `y` and comes second in the cast, so each normal is in a + // frame of its own: the capsule's, the world's, and the heightfield's turned one. + let degrees = 46.0; + let (normal, gradient) = face(degrees); + let hf = heightfield(|x| x * gradient, HeightFieldFlags::FIX_INTERNAL_EDGES, 40.0); + let hf_pose = Pose::from_rotation(Rotation::from_rotation_y(0.5)); + let options = ShapeCastOptions::with_max_time_of_impact(MAX_TIME_OF_IMPACT); + + let mut off = Vec::new(); + for start in starts(1.6, above_face(degrees)) { + let origin = hf_pose * start; + let hit = cast_shapes( + &Pose::from_translation(origin), + -Vector::Y, + &capsule(), + &hf_pose, + Vector::ZERO, + &hf, + options, + ) + .expect("the cast is supported") + .unwrap_or_else(|| panic!("the cast from {origin:?} hits the heightfield")); + if (hit.normal1 + hf_pose.rotation * normal).length() > TOLERANCE + || (hit.normal2 - normal).length() > TOLERANCE + { + off.push((origin, hit.normal1, hit.normal2)); + } + } + assert!( + off.is_empty(), + "{} casts onto the turned face return a normal off the face, e.g. {:?}", + off.len(), + &off[..off.len().min(3)], + ); +} + +/// The face of slope `degrees` rising along `+x` with a hole: the right triangle of the cell +/// between `x = 2` and `x = 3`, `z = -1` and `z = 0` removed, which meets the grid line `x = 2` +/// only at `z = 0`. +fn face_with_hole(degrees: Real, flags: HeightFieldFlags) -> HeightField { + let (_, gradient) = face(degrees); + let mut hf = heightfield(|x| x * gradient, flags, 40.0); + hf.set_cell_status(19, 22, HeightFieldCellStatus::RIGHT_TRIANGLE_REMOVED); + hf +} + +/// How far the point at `x`, `z` of the face's plane lies outside the hole of [`face_with_hole`], +/// whose points satisfy `x + z >= 2` within its cell. +fn outside_hole(x: Real, z: Real) -> Real { + (2.0 - x - z) / Real::sqrt(2.0) +} + +#[test] +fn cast_beside_a_removed_triangle_returns_the_face_normal() { + // The casts touch the face 5 cm or more from the hole, some of them on the grid line + // `x = 2`, an internal edge between two intact triangles, one of them in the cell with the + // removed triangle. + let degrees = 46.0; + let (normal, _) = face(degrees); + let hf = face_with_hole(degrees, HeightFieldFlags::FIX_INTERNAL_EDGES); + // How far along `x` the capsule's bottom touches the face from its axis. + let reach = capsule().radius * -normal.x; + let options = ShapeCastOptions::with_max_time_of_impact(MAX_TIME_OF_IMPACT); + + let mut hits = Vec::new(); + for origin in starts(1.6, above_face(degrees)) { + if outside_hole(origin.x + reach, origin.z) < 0.05 { + continue; + } + let hit = cast_shapes( + &Pose::identity(), + Vector::ZERO, + &hf, + &Pose::from_translation(origin), + -Vector::Y, + &capsule(), + options, + ) + .expect("the cast is supported") + .unwrap_or_else(|| panic!("the cast from {origin:?} hits the face")); + hits.push((origin, hit)); + } + + let off = off_face(&hits, normal); + assert!( + off.is_empty(), + "{} casts beside the hole return a normal off the face {normal:?}, e.g. {:?}", + off.len(), + &off[..off.len().min(3)], + ); +} + +#[test] +fn cast_into_a_hole_keeps_the_normal_of_its_border() { + // The casts whose bottom would touch the face's plane inside the hole drop into it until they + // touch its border, with a normal off the face that the flag must keep. + let degrees = 46.0; + let (normal, _) = face(degrees); + let fixed = face_with_hole(degrees, HeightFieldFlags::FIX_INTERNAL_EDGES); + let plain = face_with_hole(degrees, HeightFieldFlags::empty()); + let reach = capsule().radius * -normal.x; + // Deep enough for the capsule to reach the border of the hole. + let options = ShapeCastOptions::with_max_time_of_impact(1.0); + + let mut on_border = 0; + for origin in starts(1.6, above_face(degrees)) { + if outside_hole(origin.x + reach, origin.z) > 0.0 { + continue; + } + let cast = |hf: &HeightField| { + cast_shapes( + &Pose::identity(), + Vector::ZERO, + hf, + &Pose::from_translation(origin), + -Vector::Y, + &capsule(), + options, + ) + .expect("the cast is supported") + .unwrap_or_else(|| panic!("the cast from {origin:?} hits the border of the hole")) + }; + let (fixed, plain) = (cast(&fixed), cast(&plain)); + assert!( + (fixed.normal1 - plain.normal1).length() <= TOLERANCE, + "the cast from {origin:?} bends the normal {:?} of the hole's border to {:?}", + plain.normal1, + fixed.normal1, + ); + if (plain.normal1 - normal).length() > TOLERANCE { + on_border += 1; + } + } + assert!( + on_border > 0, + "no cast stops on the border of the hole, so the test no longer shows that its normal is \ + kept", + ); +} + +#[test] +fn cast_into_a_hole_with_a_far_removed_corner_keeps_the_normal_of_its_border() { + // The free corner of the removed triangle, at `x = 3`, `z = 0`, lies 1e20 m up, in removed + // triangles only. Its distance to a point on the hole's border then rounds to NaN, which must + // still leave the border's normal as it is. + let degrees = 46.0; + let (normal, gradient) = face(degrees); + let spiked = |flags| { + let samples = 41; + let mut heights: Vec = (0..samples) + .flat_map(|j| (0..samples).map(move |_| (-20.0 + j as Real) * gradient)) + .collect(); + // Column 23 (`x = 3`), row 20 (`z = 0`). + heights[23 * samples + 20] = 1.0e20; + let mut hf = HeightField::with_flags( + Array2::new(samples, samples, heights), + Vector::new(40.0, 1.0, 40.0), + flags, + ); + hf.set_cell_status(19, 22, HeightFieldCellStatus::RIGHT_TRIANGLE_REMOVED); + for (i, j) in [(19, 23), (20, 22), (20, 23)] { + hf.set_cell_status(i, j, HeightFieldCellStatus::CELL_REMOVED); + } + hf + }; + let fixed = spiked(HeightFieldFlags::FIX_INTERNAL_EDGES); + let plain = spiked(HeightFieldFlags::empty()); + let reach = capsule().radius * -normal.x; + let options = ShapeCastOptions::with_max_time_of_impact(1.0); + + let mut on_border = 0; + for origin in starts(1.6, above_face(degrees)) { + if outside_hole(origin.x + reach, origin.z) > 0.0 { + continue; + } + let cast = |hf: &HeightField| { + cast_shapes( + &Pose::identity(), + Vector::ZERO, + hf, + &Pose::from_translation(origin), + -Vector::Y, + &capsule(), + options, + ) + .expect("the cast is supported") + }; + let (Some(fixed), Some(plain)) = (cast(&fixed), cast(&plain)) else { + continue; + }; + assert!( + (fixed.normal1 - plain.normal1).length() <= TOLERANCE, + "the cast from {origin:?} bends the normal {:?} of the hole's border to {:?}", + plain.normal1, + fixed.normal1, + ); + if (plain.normal1 - normal).length() > TOLERANCE { + on_border += 1; + } + } + assert!( + on_border > 0, + "no cast stops on the border of the hole, so the test no longer shows that its normal is \ + kept", + ); +} diff --git a/src/query/shape_cast/shape_cast_heightfield_shape.rs b/src/query/shape_cast/shape_cast_heightfield_shape.rs index 83182034..c940f557 100644 --- a/src/query/shape_cast/shape_cast_heightfield_shape.rs +++ b/src/query/shape_cast/shape_cast_heightfield_shape.rs @@ -4,7 +4,11 @@ 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}; +use crate::{ + bounding_volume::Aabb, + query::{PointQuery, RayCast}, + shape::{pseudo_normals::project_into_cone, HeightFieldCellStatus, HeightFieldFlags, Triangle}, +}; /// Time Of Impact between a moving shape and a heightfield. #[cfg(feature = "dim2")] @@ -144,14 +148,30 @@ pub fn cast_shapes_heightfield_shape( let clamped_curr_range_j = curr_range_j.start.clamp(0, ncells_j as isize) ..curr_range_j.end.clamp(0, ncells_j as isize); + let fix_internal_edges = heightfield1 + .flags() + .contains(HeightFieldFlags::FIX_INTERNAL_EDGES); let mut hit_triangles = |i, j| { if i >= 0 && j >= 0 { 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.cast_shapes(pos12, vel12, &tri, g2, options)? { + if let Some(mut 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) { + // Each triangle is cast on its own, so the hit may lie on an internal edge, + // with a normal the heightfield's surface doesn't have. + if fix_internal_edges { + if let Some(normal1) = project_into_surface_cone( + heightfield1, + &tri, + hit.witness1, + hit.normal1, + ) { + hit.normal1 = normal1; + hit.normal2 = pos12.rotation.inverse() * -normal1; + } + } best_hit = Some(hit); } } @@ -256,6 +276,195 @@ pub fn cast_shapes_heightfield_shape( Ok(best_hit) } +/// `normal`, the normal of a hit at `witness` on `triangle` of `heightfield`, projected into the +/// normal cone of the surface there, as the triangles of `heightfield` within a band around +/// `witness` approximate it: a cone around the face normal reaching the normal of the triangle in +/// the band that lies farthest from it. Inside a face, on an edge to a coplanar triangle, or at a +/// vertex of a flat region, that leaves only the face normal; on a real crease, the normals +/// between its faces. +/// +/// `None`, leaving `normal` as it is, where the band reaches the heightfield's border or a removed +/// triangle, as the cone opens towards the missing surface there, or if `normal` points to the +/// triangle's back. +#[cfg(feature = "dim3")] +fn project_into_surface_cone( + heightfield: &HeightField, + triangle: &Triangle, + witness: Vector, + mut normal: Vector, +) -> Option { + let face = triangle.normal()?; + // The witness lies on the surface up to the cast's rounding. A wider band only widens the cone. + let tolerance = 1.0e-3 + * triangle + .edges() + .iter() + .map(|edge| edge.length()) + .fold(0.0, Real::max); + let band = Aabb::new( + witness - Vector::splat(tolerance), + witness + Vector::splat(tolerance), + ); + let (cells_i, cells_j) = heightfield.unclamped_elements_range_in_local_aabb(&band); + + let mut farthest = face; + for i in cells_i { + for j in cells_j.clone() { + let (Ok(i), Ok(j)) = (usize::try_from(i), usize::try_from(j)) else { + return None; + }; + if i >= heightfield.nrows() || j >= heightfield.ncols() { + return None; + } + let status = heightfield.cell_status(i, j); + let (left, right) = heightfield.cell_triangles(i, j); + for (other, removed) in [ + (left, HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED), + (right, HeightFieldCellStatus::RIGHT_TRIANGLE_REMOVED), + ] { + if status.contains(removed) { + if reaches_cylinder(&other, witness, tolerance) { + return None; + } + continue; + } + if other.distance_to_local_point(witness, true) <= tolerance { + let other_normal = other.normal()?; + if other_normal.dot(face) < farthest.dot(face) { + farthest = other_normal; + } + } + } + } + } + + let pseudo_normal = (face + farthest).try_normalize()?; + project_into_cone(face, pseudo_normal, &mut normal).then_some(normal) +} + +/// Whether `triangle` reaches the cylinder around `point` of radius and half-height `tolerance`, +/// which holds the ball of radius `tolerance` around it. +/// +/// Only the triangle's part between the cylinder's heights is measured, seen from above, so heights +/// far from `point`, however extreme, cannot round it away. A non-finite height counts as reaching. +#[cfg(feature = "dim3")] +fn reaches_cylinder(triangle: &Triangle, point: Vector, tolerance: Real) -> bool { + let corners = [triangle.a, triangle.b, triangle.c]; + if corners.iter().any(|corner| !corner.y.is_finite()) { + return true; + } + let (low, high) = (point.y - tolerance, point.y + tolerance); + + // The triangle clipped to the slab between `low` and `high`: each edge adds its start, where + // within the slab, and where it crosses the slab's bounds, in order along the edge. + let mut part = [Vector::ZERO; 9]; + let mut len = 0; + for k in 0..3 { + let (a, b) = (corners[k], corners[(k + 1) % 3]); + if (low..=high).contains(&a.y) { + part[len] = a; + len += 1; + } + let bounds = if a.y <= b.y { [low, high] } else { [high, low] }; + for bound in bounds { + if (a.y < bound) != (b.y < bound) && a.y != bound && b.y != bound { + // Halved, the heights' differences cannot overflow; only `x` and `z` are read below. + let t = (bound * 0.5 - a.y * 0.5) / (b.y * 0.5 - a.y * 0.5); + part[len] = Vector::new(a.x + (b.x - a.x) * t, bound, a.z + (b.z - a.z) * t); + len += 1; + } + } + } + + // The clipped part seen from above, relative to `point`: within `tolerance` of its boundary, + // or with `point` inside it. + let plan = |v: Vector| Vector::new(v.x - point.x, 0.0, v.z - point.z); + let mut nearest = Real::MAX; + let mut side = 0.0; + let mut inside = true; + for k in 0..len { + let a = plan(part[k]); + let edge = plan(part[(k + 1) % len]) - a; + let length_squared = edge.length_squared(); + let t = if length_squared > 0.0 { + (-a.dot(edge) / length_squared).clamp(0.0, 1.0) + } else { + 0.0 + }; + nearest = nearest.min((a + edge * t).length()); + let cross = edge.z * a.x - edge.x * a.z; + if cross != 0.0 { + if side == 0.0 { + side = cross.signum(); + } else if cross.signum() != side { + inside = false; + } + } + } + len > 0 && (nearest <= tolerance || (inside && side != 0.0)) +} + +#[cfg(all(test, feature = "dim3"))] +mod test { + use super::reaches_cylinder; + use crate::math::{Real, Vector}; + use crate::shape::Triangle; + + /// The triangle of the cell between `x = 2` and `x = 3`, `z = -1` and `z = 0` where + /// `x + z >= 2`, on the plane of `height`. + fn removed(height: impl Fn(Real, Real) -> Real) -> Triangle { + let corner = |x: Real, z: Real| Vector::new(x, height(x, z), z); + Triangle::new(corner(3.0, -1.0), corner(3.0, 0.0), corner(2.0, 0.0)) + } + + #[test] + fn a_triangle_reaches_the_cylinder_on_its_border() { + let triangle = removed(|x, z| x + z); + let on_border = Vector::new(2.5, 2.0, -0.5); + assert!(reaches_cylinder(&triangle, on_border, 1.0e-3)); + let beside = Vector::new(2.45, 1.9, -0.55); + assert!(!reaches_cylinder(&triangle, beside, 1.0e-3)); + let inside = Vector::new(2.8, 2.6, -0.2); + assert!(reaches_cylinder(&triangle, inside, 1.0e-3)); + } + + #[test] + fn a_far_corner_does_not_round_the_border_away() { + let triangle = removed(|x, z| if x == 3.0 && z == 0.0 { 1.0e20 } else { x + z }); + let on_border = Vector::new(2.5, 2.0, -0.5); + assert!(reaches_cylinder(&triangle, on_border, 1.0e-3)); + let below = removed(|x, z| if x == 3.0 && z == 0.0 { -1.0e20 } else { x + z }); + assert!(reaches_cylinder(&below, on_border, 1.0e-3)); + for height in [Real::INFINITY, Real::NAN] { + let triangle = removed(|x, z| if x == 3.0 && z == 0.0 { height } else { x + z }); + assert!(reaches_cylinder(&triangle, on_border, 1.0e-3)); + } + } + + #[test] + fn heights_of_opposite_extremes_do_not_overflow_the_clipping() { + // The point lies on the edge from 3e38 below to 3e38 above, whose rise overflows. + let triangle = Triangle::new( + Vector::new(3.0, -3.0e38, -1.0), + Vector::new(3.0, 3.0e38, 0.0), + Vector::new(2.0, 3.0e38, 0.0), + ); + assert!(reaches_cylinder( + &triangle, + Vector::new(3.0, 0.0, -0.5), + 1.0e-3 + )); + } + + #[test] + fn a_steep_triangle_metres_away_does_not_reach_the_cylinder() { + // Seen from above, the triangle lies 3.5 cm from the point; in space, 5 m. + let triangle = removed(|x, z| 100.0 * (x + z)); + let point = Vector::new(2.0, 195.0, -0.05); + assert!(!reaches_cylinder(&triangle, point, 0.1)); + } +} + /// Time Of Impact between a moving shape and a heightfield. pub fn cast_shapes_shape_heightfield( dispatcher: &D, diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 8b19a5d4..c73955f5 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -349,6 +349,19 @@ impl HeightField { return (None, None); } + let (tri1, tri2) = self.cell_triangles(i, j); + ( + (!status.contains(HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED)).then_some(tri1), + (!status.contains(HeightFieldCellStatus::RIGHT_TRIANGLE_REMOVED)).then_some(tri2), + ) + } + + /// The two triangles of the cell (i, j) of this heightfield, whether removed or not. + /// + /// The cell must lie within the heightfield. + pub(crate) fn cell_triangles(&self, i: usize, j: usize) -> (Triangle, Triangle) { + let status = self.status[(i, j)]; + let cell_width = self.unit_cell_width(); let cell_height = self.unit_cell_height(); @@ -375,33 +388,9 @@ impl HeightField { p11 *= self.scale; if status.contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { - let tri1 = if status.contains(HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED) { - None - } else { - Some(Triangle::new(p00, p10, p11)) - }; - - let tri2 = if status.contains(HeightFieldCellStatus::RIGHT_TRIANGLE_REMOVED) { - None - } else { - Some(Triangle::new(p00, p11, p01)) - }; - - (tri1, tri2) + (Triangle::new(p00, p10, p11), Triangle::new(p00, p11, p01)) } else { - let tri1 = if status.contains(HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED) { - None - } else { - Some(Triangle::new(p00, p10, p01)) - }; - - let tri2 = if status.contains(HeightFieldCellStatus::RIGHT_TRIANGLE_REMOVED) { - None - } else { - Some(Triangle::new(p10, p11, p01)) - }; - - (tri1, tri2) + (Triangle::new(p00, p10, p01), Triangle::new(p10, p11, p01)) } } diff --git a/src/shape/mod.rs b/src/shape/mod.rs index 00ad6aec..5cfb1f1b 100644 --- a/src/shape/mod.rs +++ b/src/shape/mod.rs @@ -135,7 +135,7 @@ mod feature_id; #[cfg(feature = "dim2")] mod polygonal_feature2d; #[cfg(feature = "alloc")] -mod pseudo_normals; +pub(crate) mod pseudo_normals; mod segment_pseudo_normals; #[cfg(feature = "alloc")] mod shared_shape;