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;