diff --git a/CHANGELOG.md b/CHANGELOG.md index 91b81068..ee1c2315 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,7 +4,13 @@ - Fix `enhanced-determinism` not producing cross-platform deterministic results for parry3d/parry3d-f64. The feature now enables `glamx/scalar-math` to disable architecture-specific SIMD (NEON on arm64, SSE2 on x86_64) which caused floating-point non-associativity in Vec3/Vec4/Quat dot products. - +- Oriented (one-sided) 2D polylines ([#254](https://github.com/dimforge/parry/issues/254)). Build a `Polyline` with +`PolylineFlags::ORIENTED` via `Polyline::with_flags`, or toggle it with `Polyline::set_flags`, to clamp contact normals +to the outward side. This removes the spurious sideways push a body gets at a convex corner of a thin double-sided +polyline. A single 2D flag covers what `TriMesh` splits across `TriMeshFlags::ORIENTED` (assume an outward orientation +and compute pseudo-normals) and `TriMeshFlags::FIX_INTERNAL_EDGES` (use them to clamp contact normals). Also adds +`Polyline::flags`, `Polyline::segment_normal_constraints`, and the `PolylineFlags` / `SegmentPseudoNormals` types. +([#425](https://github.com/dimforge/parry/pull/425)) ## 0.28.0 ### Breaking changes diff --git a/crates/parry2d/examples/oriented_polyline2d.rs b/crates/parry2d/examples/oriented_polyline2d.rs new file mode 100644 index 00000000..b8d43aa8 --- /dev/null +++ b/crates/parry2d/examples/oriented_polyline2d.rs @@ -0,0 +1,462 @@ +//! Visualises the polyline orientation bug and its fix, side by side. +//! +//! A [`Polyline`] is double-sided by default: a contact normal can point to whichever side of a +//! segment the other shape happens to be nearest, so a body that crosses the line is pushed deeper +//! through it instead of back out. An *oriented* polyline (built with +//! [`PolylineFlags::ORIENTED`]) clamps every contact normal into the outward cone, so a +//! one-sided boundary pushes a body back to the play side no matter which side it is on. +//! +//! The same boundaries (a convex peak with solid below, and a box with solid inside whose 90-degree +//! corners are where ghost collisions are worst) and the same circle path are drawn twice: the upper +//! panel uses double-sided polylines (the bug), the lower panel uses oriented ones (the fix). The +//! circle's motion has three phases: a free lissajous roam, a guided trace that hugs each segment and +//! presses into every corner, then a guided trace that only traces the outside (never entering). +//! Each frame computes the contact manifold +//! (the same path the physics engine uses; this is where pseudo-normals apply, not the simpler +//! `query::contact`) and draws the actual contact normal and where the solver would push the circle. +//! +//! The test circle turns magenta when its center is *inside* a boundary, with a line to the nearest +//! surface of each. Only the oriented (lower) polylines have an interior, so only they light up. +//! +//! ```bash +//! cargo run -p parry2d --example oriented_polyline2d +//! ``` + +mod utils2d; + +use core::f32::consts::{PI, TAU}; + +use kiss3d::prelude::*; +use parry2d::math::{Pose, Rotation}; +use parry2d::query::{ + ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, PersistentQueryDispatcher, + PointQuery, +}; +use parry2d::shape::{Ball, Polyline, PolylineFlags, Shape}; +use utils2d::{draw_circle, draw_line_2d, lissajous_2d}; + +/// The test circle's fill when its center is inside an oriented boundary. +const INSIDE_COLOR: Color = Color::new(1.0, 0.30, 0.80, 1.0); + +/// One stop on the guided trace: where to be, how long to take getting there, and how long to dwell. +struct Waypoint { + pos: Vec2, + travel: f32, + dwell: f32, +} + +#[kiss3d::main] +async fn main() { + print_legend(); + + let mut window = + Window::new("oriented_polyline2d -- double-sided (top) vs oriented (bottom)").await; + let mut camera = PanZoomCamera2d::new(Vec2::new(0.0, 2.0), 2.0); + let mut scene = SceneNode2d::empty(); + let font = Font::default(); + + // Convex peak, wound right-to-left so outward (right-hand) normals point up: solid below, play above. + let peak = Polyline::new( + vec![ + Vec2::new(96.0, -36.0), + Vec2::new(54.0, 40.0), + Vec2::new(12.0, -36.0), + ], + Some(vec![[0, 1], [1, 2]]), + ); + + // Box, wound counter-clockwise so the solid is inside; its square corners are the issue's case. + let boxed = Polyline::new( + vec![ + Vec2::new(-12.0, -36.0), + Vec2::new(-12.0, 36.0), + Vec2::new(-96.0, 36.0), + Vec2::new(-96.0, -36.0), + ], + Some(vec![[0, 1], [1, 2], [2, 3], [3, 0]]), + ); + + // The two panels share geometry; only the lower one is told its outward side. + let plain = [peak, boxed]; + let mut oriented = plain.clone(); + for boundary in &mut oriented { + boundary.set_flags(PolylineFlags::ORIENTED); + } + + let ball = Ball::new(16.0); + + // Three phases: free roam, a corner-pressing trace, then an outside-only trace. + let trace_in = build_trace(&plain, ball.radius, -0.5 * ball.radius); + let trace_out = build_trace(&plain, ball.radius, ball.radius); + let trace_in_duration: f32 = trace_in + .iter() + .map(|waypoint| waypoint.travel + waypoint.dwell) + .sum(); + let trace_out_duration: f32 = trace_out + .iter() + .map(|waypoint| waypoint.travel + waypoint.dwell) + .sum(); + let lissajous_duration = 8.0; + let cycle = lissajous_duration + trace_in_duration + trace_out_duration; + + let dispatcher = DefaultQueryDispatcher; + let panel = Vec2::new(0.0, 81.0); // upper panel sits at +panel, lower at -panel + + let start_time = web_time::Instant::now(); + + while window.render_2d(&mut scene, &mut camera).await { + let elapsed = start_time.elapsed().as_secs_f32(); + + // The same path feeds both panels. + let phase = if cycle > 0.0 { elapsed % cycle } else { 0.0 }; + let local_pos = if phase < lissajous_duration { + lissajous_2d(phase * 0.35) * 120.0 + } else if phase < lissajous_duration + trace_in_duration { + trace_position(&trace_in, phase - lissajous_duration) + } else { + trace_position(&trace_out, phase - lissajous_duration - trace_in_duration) + }; + + draw_panel(&mut window, &dispatcher, &plain, &ball, local_pos, panel); + draw_panel( + &mut window, + &dispatcher, + &oriented, + &ball, + local_pos, + -panel, + ); + + window.draw_text( + "upper: double-sided polyline (the bug)", + Vec2::new(8.0, 6.0), + 34.0, + &font, + WHITE, + ); + window.draw_text( + "lower: oriented polyline (fixed)", + Vec2::new(8.0, 42.0), + 34.0, + &font, + WHITE, + ); + window.draw_text( + "magenta line + circle: center is inside that boundary (oriented panel only)", + Vec2::new(8.0, 78.0), + 34.0, + &font, + INSIDE_COLOR, + ); + } +} + +/// Draws one panel (boundaries, circle, contact resolution), translated by `offset`. +fn draw_panel( + window: &mut Window, + dispatcher: &DefaultQueryDispatcher, + boundaries: &[Polyline], + ball: &Ball, + local_pos: Vec2, + offset: Vec2, +) { + let radius = ball.radius; + + for boundary in boundaries { + draw_boundary(window, boundary, offset); + draw_cones(window, boundary, offset); + } + + // Draw a line to the nearest surface of each boundary the center is inside (a point can be inside + // several), then tint the circle. Only oriented polylines report an interior. + let mut inside_any = false; + for boundary in boundaries { + let projection = boundary.project_local_point(local_pos, false); + if projection.is_inside { + inside_any = true; + draw_line_2d( + window, + local_pos + offset, + projection.point + offset, + INSIDE_COLOR, + ); + } + } + let circle_color = if inside_any { INSIDE_COLOR } else { YELLOW }; + draw_circle(window, local_pos + offset, radius, circle_color); + + for boundary in boundaries { + draw_resolution( + window, + dispatcher, + boundary, + ball, + local_pos, + offset, + |w, p| { + draw_circle(w, p, radius, LIME); + }, + ); + } +} + +/// Whether the polyline is a closed loop (last segment's end meets the first's start). +fn is_closed(polyline: &Polyline) -> bool { + let num = polyline.num_segments(); + num > 0 && (polyline.segment((num - 1) as u32).b - polyline.segment(0).a).length() < 1.0e-3 +} + +/// Builds a trace hugging each segment; at each corner it moves `corner_offset` along the outward +/// bisector (negative dips inside, positive stays outside) and dwells. +fn build_trace(boundaries: &[Polyline], radius: f32, corner_offset: f32) -> Vec { + const CORNER_DWELL: f32 = 0.8; + + let mut waypoints: Vec = Vec::new(); + + for boundary in boundaries { + let num = boundary.num_segments(); + if num == 0 { + continue; + } + + // Closed loops (the box) wrap last->first; open chains (the peak) don't, so their ends aren't corners. + let closed = is_closed(boundary); + + let normals: Vec = (0..num as u32) + .map(|i| { + let segment = boundary.segment(i); + let direction = (segment.b - segment.a).normalize_or_zero(); + Vec2::new(direction.y, -direction.x) + }) + .collect(); + + for i in 0..num { + let segment = boundary.segment(i as u32); + let normal = normals[i]; + + push_waypoint(&mut waypoints, segment.a + normal * radius, false, 0.0); + push_waypoint(&mut waypoints, segment.b + normal * radius, false, 0.0); + + let next = if i + 1 < num { + Some(i + 1) + } else if closed { + Some(0) + } else { + None + }; + + if let Some(j) = next { + let bisector = (normal + normals[j]).normalize_or_zero(); + push_waypoint( + &mut waypoints, + segment.b + bisector * corner_offset, + true, + CORNER_DWELL, + ); + } + } + } + + waypoints +} + +/// Appends a waypoint, timing the move from the previous: a steady glide, or a slow crawl if `slow`. +fn push_waypoint(waypoints: &mut Vec, pos: Vec2, slow: bool, dwell: f32) { + const GLIDE_SPEED: f32 = 60.0; // pixels per second + const APPROACH_TRAVEL: f32 = 1.4; // seconds to cross into a corner + + let travel = match waypoints.last() { + None => 0.0, + Some(_) if slow => APPROACH_TRAVEL, + Some(previous) => ((pos - previous.pos).length() / GLIDE_SPEED).max(0.05), + }; + + waypoints.push(Waypoint { pos, travel, dwell }); +} + +/// Position along the guided trace at local time `t` (seconds since the trace phase started). +fn trace_position(waypoints: &[Waypoint], mut t: f32) -> Vec2 { + let Some(first) = waypoints.first() else { + return Vec2::ZERO; + }; + + let mut current = first.pos; + for waypoint in waypoints { + if t < waypoint.travel { + let fraction = if waypoint.travel > 0.0 { + t / waypoint.travel + } else { + 1.0 + }; + return current.lerp(waypoint.pos, fraction); + } + t -= waypoint.travel; + + if t < waypoint.dwell { + return waypoint.pos; + } + t -= waypoint.dwell; + + current = waypoint.pos; + } + + current +} + +/// Draws a boundary (white) with a gray outward-reference arrow per segment, translated by `offset`. +fn draw_boundary(window: &mut Window, polyline: &Polyline, offset: Vec2) { + for i in 0..polyline.num_segments() as u32 { + let segment = polyline.segment(i); + let a = segment.a + offset; + let b = segment.b + offset; + draw_line_2d(window, a, b, WHITE); + + let direction = (segment.b - segment.a).normalize_or_zero(); + // Right-hand normal: the outward/play side for this winding. + let outward = Vec2::new(direction.y, -direction.x); + draw_arrow(window, (a + b) * 0.5, outward, 22.0, GRAY); + } +} + +/// Draws the valid-normal cone at each corner (the wedge between adjacent face normals) in the +/// oriented panel; a double-sided polyline returns `None` here, so the upper panel gets no cone. +fn draw_cones(window: &mut Window, polyline: &Polyline, offset: Vec2) { + const CONE_COLOR: Color = Color::new(0.30, 0.60, 1.0, 1.0); + + let num = polyline.num_segments(); + if num == 0 { + return; + } + + let closed = is_closed(polyline); + + for i in 0..num { + let next = if i + 1 < num { + i + 1 + } else if closed { + 0 + } else { + continue; + }; + + // The vertex pseudo-normal bisects the two adjacent faces, so the clamp's cone is that wedge. + let (Some(here), Some(beyond)) = ( + polyline.segment_normal_constraints(i as u32), + polyline.segment_normal_constraints(next as u32), + ) else { + return; + }; + + let vertex = polyline.segment(i as u32).b + offset; + draw_cone(window, vertex, here.face, beyond.face, CONE_COLOR); + } +} + +/// Draws a fan filling the angular wedge from `dir_a` to `dir_b` (the shorter way) at `vertex`. +fn draw_cone(window: &mut Window, vertex: Vec2, dir_a: Vec2, dir_b: Vec2, color: Color) { + const LENGTH: f32 = 30.0; + const RAYS: usize = 6; + + let angle_a = dir_a.y.atan2(dir_a.x); + let mut delta = dir_b.y.atan2(dir_b.x) - angle_a; + while delta > PI { + delta -= TAU; + } + while delta < -PI { + delta += TAU; + } + + let mut previous: Option = None; + for k in 0..=RAYS { + let angle = angle_a + delta * (k as f32 / RAYS as f32); + let tip = vertex + Vec2::new(angle.cos(), angle.sin()) * LENGTH; + draw_line_2d(window, vertex, tip, color); + if let Some(prev) = previous { + draw_line_2d(window, prev, tip, color); + } + previous = Some(tip); + } +} + +/// Draws the deepest contact's normal (red) and, via `draw_ghost`, the resolved position. The manifold +/// is computed in the boundary's local frame; only the drawing is translated by `offset`. +fn draw_resolution( + window: &mut Window, + dispatcher: &DefaultQueryDispatcher, + polyline: &Polyline, + shape: &dyn Shape, + local_pos: Vec2, + offset: Vec2, + draw_ghost: impl Fn(&mut Window, Vec2), +) { + const PREDICTION: f32 = 8.0; + + let pos12 = Pose::from_parts(local_pos, Rotation::identity()); + let mut manifolds: Vec> = Vec::new(); + let mut workspace: Option = None; + // Polyline vs ball is always supported, so the unsupported-pair error cannot happen here. + let _ = dispatcher.contact_manifolds( + &pos12, + polyline, + shape, + PREDICTION, + &mut manifolds, + &mut workspace, + ); + + let mut deepest: Option<(Vec2, Vec2, f32)> = None; + for manifold in &manifolds { + let normal = manifold.local_n1; + for contact in &manifold.points { + if deepest.is_none_or(|(_, _, best)| contact.dist < best) { + deepest = Some((contact.local_p1, normal, contact.dist)); + } + } + } + + if let Some((point, normal, distance)) = deepest { + draw_arrow(window, point + offset, normal, 45.0, RED); + + let penetration = (-distance).max(0.0); + if penetration > 0.0 { + draw_ghost(window, local_pos + normal * penetration + offset); + } + } +} + +/// Draws an arrow from `base` along `direction`, scaled to `length`. +fn draw_arrow(window: &mut Window, base: Vec2, direction: Vec2, length: f32, color: Color) { + let Some(direction) = direction.try_normalize() else { + return; + }; + + let tip = base + direction * length; + draw_line_2d(window, base, tip, color); + + let back = -direction * (length * 0.28); + let side = Vec2::new(-direction.y, direction.x) * (length * 0.16); + draw_line_2d(window, tip, tip + back + side, color); + draw_line_2d(window, tip, tip + back - side, color); +} + +fn print_legend() { + println!("oriented_polyline2d orientation demo (side-by-side)"); + println!(" UPPER panel: double-sided polyline -- the bug"); + println!(" LOWER panel: oriented polyline -- the fix"); + println!("both panels run the same circle path; scroll to zoom, drag to pan."); + println!("legend:"); + println!(" white boundary polylines: a peak (solid below) and a box (solid inside)"); + println!(" gray reference outward (play-side) normal per segment"); + println!(" blue valid-normal cone the oriented polyline allows at each corner (lower only)"); + println!(" yellow test circle (center outside every boundary)"); + println!(" magenta circle + line to each oriented boundary whose interior holds the center (lower only)"); + println!(" red actual contact normal -- the direction the solver pushes the circle"); + println!(" green where the circle is pushed out to"); + println!("phases: free roam, then a corner-pressing trace, then an outside-only trace."); + println!( + "watch the corners: the upper panel has no cone, so red/green flip to the wrong side; in" + ); + println!( + "the lower panel the red normal stays inside the blue cone, pushing the circle back out." + ); +} diff --git a/crates/parry2d/tests/oriented_polyline.rs b/crates/parry2d/tests/oriented_polyline.rs new file mode 100644 index 00000000..c07b4a63 --- /dev/null +++ b/crates/parry2d/tests/oriented_polyline.rs @@ -0,0 +1,58 @@ +//! End-to-end checks that an oriented polyline's [`PolylineFlags`] reach the contact-manifold path +//! (the same path the physics engine uses), not just the standalone pseudo-normal math. + +use parry2d::math::{Pose, Real, Rotation, Vector}; +use parry2d::query::{ + ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, PersistentQueryDispatcher, +}; +use parry2d::shape::{Ball, Polyline, PolylineFlags}; + +/// Deepest contact normal (on the polyline, pointing toward the ball) of `ball` at `ball_pos`, or +/// `None` if the manifold is empty. +fn deepest_normal(polyline: &Polyline, ball: &Ball, ball_pos: Vector) -> Option { + let pos12 = Pose::from_parts(ball_pos, Rotation::identity()); + let mut manifolds: Vec> = Vec::new(); + let mut workspace: Option = None; + DefaultQueryDispatcher + .contact_manifolds(&pos12, polyline, ball, 0.0, &mut manifolds, &mut workspace) + .unwrap(); + + let mut deepest: Option<(Vector, Real)> = None; + for manifold in &manifolds { + for contact in &manifold.points { + if deepest.map_or(true, |(_, best)| contact.dist < best) { + deepest = Some((manifold.local_n1, contact.dist)); + } + } + } + deepest.map(|(normal, _)| normal) +} + +#[test] +fn oriented_polyline_keeps_front_contact_outward() { + // Ball in front (play side): orientation keeps the contact, normal outward (+Y). + let vertices = vec![Vector::new(1.0, 0.0), Vector::new(-1.0, 0.0)]; + let indices = Some(vec![[0, 1]]); + let ball = Ball::new(0.7); + let ball_pos = Vector::new(0.0, 0.5); + + let oriented = Polyline::with_flags(vertices, indices, PolylineFlags::ORIENTED); + let normal = deepest_normal(&oriented, &ball, ball_pos).expect("front contact kept"); + + assert!(normal.dot(Vector::Y) > 0.0); +} + +#[test] +fn oriented_polyline_is_one_sided() { + // Solid below (outward up); a ball behind it collides when double-sided but is ignored once oriented. + let vertices = vec![Vector::new(1.0, 0.0), Vector::new(-1.0, 0.0)]; + let indices = Some(vec![[0, 1]]); + let ball = Ball::new(1.0); + let ball_pos = Vector::new(0.0, -0.5); + + let plain = Polyline::new(vertices.clone(), indices.clone()); + let oriented = Polyline::with_flags(vertices, indices, PolylineFlags::ORIENTED); + + assert!(deepest_normal(&plain, &ball, ball_pos).is_some()); + assert!(deepest_normal(&oriented, &ball, ball_pos).is_none()); +} diff --git a/run_visual_examples.sh b/run_visual_examples.sh index c1d3de27..478c4f73 100755 --- a/run_visual_examples.sh +++ b/run_visual_examples.sh @@ -25,6 +25,7 @@ EXAMPLES_2D=( aabb2d bounding_sphere2d convex_hull2d + oriented_polyline2d project_point2d point_in_poly2d polygons_intersection2d diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index 6cc70f8e..0bd1dfca 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -134,17 +134,26 @@ impl CompositeShapeRef<'_, S> { impl PointQuery for Polyline { #[inline] fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection { - CompositeShapeRef(self) - .project_local_point(point, Real::MAX, solid) - .unwrap_or_else(|| unreachable!()) - .1 + self.project_local_point_and_get_location(point, solid).0 } #[inline] + #[allow(unused_mut)] // Because we need mut in 2D but not in 3D. fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) { - let (seg_id, (proj, feature)) = CompositeShapeRef(self) + let (seg_id, (mut proj, feature)) = CompositeShapeRef(self) .project_local_point_and_get_feature(point, Real::MAX) .unwrap_or_else(|| unreachable!()); + + // A point behind the outward pseudo-normal is inside. + #[cfg(feature = "dim2")] + if let Some(constraints) = self.segment_normal_constraints(seg_id) { + let pseudo_normal = match feature { + FeatureId::Vertex(i) => constraints.edges[i as usize], + _ => constraints.face, + }; + proj.is_inside = (point - proj.point).dot(pseudo_normal) <= 0.0; + } + let polyline_feature = self.segment_feature_to_polyline_feature(seg_id, feature); (proj, polyline_feature) } @@ -153,6 +162,15 @@ impl PointQuery for Polyline { #[inline] fn contains_local_point(&self, point: Vector) -> bool { + // An oriented polyline has a solid interior; reuse the projection's inside test. + #[cfg(feature = "dim2")] + if self.flags().contains(crate::shape::PolylineFlags::ORIENTED) { + return self + .project_local_point_and_get_location(point, true) + .0 + .is_inside; + } + CompositeShapeRef(self) .contains_local_point(point) .is_some() @@ -253,10 +271,39 @@ impl PointQueryWithLocation for Polyline { point: Vector, solid: bool, ) -> (PointProjection, Self::Location) { - let (seg_id, (proj, loc)) = CompositeShapeRef(self) - .project_local_point_and_get_location(point, Real::MAX, solid) - .unwrap(); - (proj, (seg_id, loc)) + self.project_local_point_and_get_location_with_max_dist(point, solid, Real::MAX) + .unwrap() + } + + /// Projects a point on `self`, with a maximum projection distance. + fn project_local_point_and_get_location_with_max_dist( + &self, + point: Vector, + solid: bool, + max_dist: Real, + ) -> Option<(PointProjection, Self::Location)> { + #[allow(unused_mut)] // Because we need mut in 2D but not in 3D. + if let Some((seg_id, (mut proj, loc))) = + CompositeShapeRef(self).project_local_point_and_get_location(point, max_dist, solid) + { + // A point behind the outward pseudo-normal is inside. + #[cfg(feature = "dim2")] + if let Some(constraints) = self.segment_normal_constraints(seg_id) { + let pseudo_normal = match loc { + SegmentPointLocation::OnVertex(i) => constraints.edges[i as usize], + SegmentPointLocation::OnEdge(_) => constraints.face, + }; + proj.is_inside = (point - proj.point).dot(pseudo_normal) <= 0.0; + + if proj.is_inside && solid { + proj.point = point; + } + } + + Some((proj, (seg_id, loc))) + } else { + None + } } } diff --git a/src/shape/mod.rs b/src/shape/mod.rs index 156a3458..dbb1ba28 100644 --- a/src/shape/mod.rs +++ b/src/shape/mod.rs @@ -25,6 +25,10 @@ pub use self::{ voxels::{AxisMask, OctantPattern, VoxelData, VoxelState, VoxelType, Voxels, VoxelsChunkRef}, }; +// `PolylineFlags` is a 2D-only feature. +#[cfg(all(feature = "dim2", feature = "alloc"))] +pub use self::polyline::PolylineFlags; + #[cfg(feature = "dim2")] #[cfg(feature = "alloc")] pub use self::convex_polygon::ConvexPolygon; @@ -46,6 +50,7 @@ pub use self::cylinder::Cylinder; pub use self::heightfield3::*; #[cfg(feature = "dim3")] pub use self::polygonal_feature3d::PolygonalFeature; +pub use self::segment_pseudo_normals::SegmentPseudoNormals; #[cfg(feature = "dim3")] pub use self::tetrahedron::{Tetrahedron, TetrahedronPointLocation}; pub use self::triangle_pseudo_normals::TrianglePseudoNormals; @@ -121,6 +126,9 @@ mod feature_id; #[cfg(feature = "dim2")] mod polygonal_feature2d; #[cfg(feature = "alloc")] +mod pseudo_normals; +mod segment_pseudo_normals; +#[cfg(feature = "alloc")] mod shared_shape; mod triangle_pseudo_normals; #[cfg(feature = "alloc")] diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index 21dcb015..37760f8e 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -3,12 +3,39 @@ use crate::math::{Pose, Vector}; use crate::partitioning::{Bvh, BvhBuildStrategy}; use crate::query::{PointProjection, PointQueryWithLocation}; use crate::shape::composite_shape::CompositeShape; -use crate::shape::{FeatureId, Segment, SegmentPointLocation, Shape, TypedCompositeShape}; +use crate::shape::{ + FeatureId, Segment, SegmentPointLocation, SegmentPseudoNormals, Shape, TypedCompositeShape, +}; #[cfg(feature = "alloc")] use alloc::vec::Vec; use crate::query::details::NormalConstraints; +#[cfg(feature = "dim2")] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize) +)] +#[repr(C)] +#[derive(Clone, Copy, Debug, Default, Eq, Hash, Ord, PartialEq, PartialOrd)] +/// Controls how a [`Polyline`] is loaded. +pub struct PolylineFlags(u8); + +#[cfg(feature = "dim2")] +bitflags::bitflags! { + impl PolylineFlags: u8 { + /// If set, the polyline is treated as one-sided: a pseudo-normal is computed at every + /// vertex and contact normals are clamped to the outward side, the *right* of each + /// segment's direction. The solid must be wound counter-clockwise, so the outward side is + /// on the right. This removes the spurious sideways push a body gets at a convex corner of + /// a double-sided polyline. This one flag covers what `TriMesh` splits across + /// `TriMeshFlags::ORIENTED` (compute pseudo-normals) and `TriMeshFlags::FIX_INTERNAL_EDGES` + /// (use them to clamp contacts). + const ORIENTED = 1; + } +} + #[derive(Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( @@ -98,6 +125,12 @@ pub struct Polyline { bvh: Bvh, vertices: Vec, indices: Vec<[u32; 2]>, + /// Per-vertex outward pseudo-normals, present when [`PolylineFlags::ORIENTED`] is set; contact + /// normals are then clamped to one side so the polyline acts as a one-sided surface. + #[cfg(feature = "dim2")] + pseudo_normals: Option>, + #[cfg(feature = "dim2")] + flags: PolylineFlags, } impl Polyline { @@ -165,8 +198,9 @@ impl Polyline { /// # } /// ``` pub fn new(vertices: Vec, indices: Option>) -> Self { + // Index from 1 so empty input produces no segments instead of underflowing on `len - 1`. let indices = - indices.unwrap_or_else(|| (0..vertices.len() as u32 - 1).map(|i| [i, i + 1]).collect()); + indices.unwrap_or_else(|| (1..vertices.len() as u32).map(|i| [i - 1, i]).collect()); let leaves = indices.iter().enumerate().map(|(i, idx)| { let aabb = Segment::new(vertices[idx[0] as usize], vertices[idx[1] as usize]).local_aabb(); @@ -181,9 +215,135 @@ impl Polyline { bvh, vertices, indices, + #[cfg(feature = "dim2")] + pseudo_normals: None, + #[cfg(feature = "dim2")] + flags: PolylineFlags::empty(), + } + } + + /// Creates a new polyline with the given [`PolylineFlags`] controlling its optional associated + /// data, e.g. orientation via [`PolylineFlags::ORIENTED`]. + /// + /// # Example + /// + /// ``` + /// # #[cfg(all(feature = "dim2", feature = "f32"))] { + /// use parry2d::shape::{Polyline, PolylineFlags}; + /// use parry2d::math::Vector; + /// + /// // A unit square wound counter-clockwise, so the solid is inside and outward points away. + /// let vertices = vec![ + /// Vector::new(-1.0, -1.0), + /// Vector::new(1.0, -1.0), + /// Vector::new(1.0, 1.0), + /// Vector::new(-1.0, 1.0), + /// ]; + /// let indices = vec![[0, 1], [1, 2], [2, 3], [3, 0]]; + /// let polyline = Polyline::with_flags(vertices, Some(indices), PolylineFlags::ORIENTED); + /// + /// // The bottom edge's outward normal points down, away from the interior. + /// let bottom = polyline.segment_normal_constraints(0).unwrap(); + /// assert!(bottom.face.abs_diff_eq(Vector::new(0.0, -1.0), 1.0e-5)); + /// # } + /// ``` + #[cfg(feature = "dim2")] + pub fn with_flags( + vertices: Vec, + indices: Option>, + flags: PolylineFlags, + ) -> Self { + let mut result = Self::new(vertices, indices); + result.set_flags(flags); + result + } + + /// Sets the [`PolylineFlags`], computing or discarding the polyline's optional associated data. + #[cfg(feature = "dim2")] + pub fn set_flags(&mut self, flags: PolylineFlags) { + self.flags = flags; + + if flags.contains(PolylineFlags::ORIENTED) { + self.compute_pseudo_normals(); + } else { + self.pseudo_normals = None; } } + /// The [`PolylineFlags`] controlling this polyline's optional associated data. + #[cfg(feature = "dim2")] + pub fn flags(&self) -> PolylineFlags { + self.flags + } + + /// Computes the outward pseudo-normal at every vertex (the normalized sum of its incident + /// segments' outward normals) for the one-sided behavior of [`PolylineFlags::ORIENTED`]. + #[cfg(feature = "dim2")] + fn compute_pseudo_normals(&mut self) { + let mut vertex_normals = Vec::new(); + vertex_normals.resize(self.vertices.len(), Vector::ZERO); + + // A 2D vertex has at most two incident segments, so the normalized sum is their exact + // bisector -- no angle weighting (unlike the 3D `TrianglePseudoNormals`). + for idx in &self.indices { + let a = idx[0] as usize; + let b = idx[1] as usize; + let normal = crate::utils::ccw_face_normal([self.vertices[a], self.vertices[b]]) + .unwrap_or(Vector::ZERO); + vertex_normals[a] += normal; + vertex_normals[b] += normal; + } + + for normal in &mut vertex_normals { + *normal = normal.normalize_or_zero(); + } + + self.pseudo_normals = Some(vertex_normals); + } + + /// Returns the [`SegmentPseudoNormals`] for the segment with index `i`, or `None` unless this + /// polyline was built with [`PolylineFlags::ORIENTED`]. + /// + /// # Example + /// + /// ``` + /// # #[cfg(all(feature = "dim2", feature = "f32"))] { + /// use parry2d::shape::{Polyline, PolylineFlags}; + /// use parry2d::math::Vector; + /// + /// let vertices = vec![Vector::new(0.0, 0.0), Vector::new(2.0, 0.0), Vector::new(2.0, 2.0)]; + /// let mut polyline = Polyline::new(vertices, Some(vec![[0, 1], [1, 2]])); + /// + /// // A double-sided polyline has no constraints... + /// assert!(polyline.segment_normal_constraints(0).is_none()); + /// + /// // ...until it is oriented. + /// polyline.set_flags(PolylineFlags::ORIENTED); + /// assert!(polyline.segment_normal_constraints(0).is_some()); + /// # } + /// ``` + #[cfg(feature = "dim2")] + pub fn segment_normal_constraints(&self, i: u32) -> Option { + let pseudo_normals = self.pseudo_normals.as_ref()?; + let idx = self.indices[i as usize]; + let a = idx[0] as usize; + let b = idx[1] as usize; + let face = crate::utils::ccw_face_normal([self.vertices[a], self.vertices[b]])?; + Some(SegmentPseudoNormals { + face, + edges: [pseudo_normals[a], pseudo_normals[b]], + }) + } + + /// Pseudo-normals are a 2D-only feature; in 3D there are no segment normal constraints. 3D stub + /// so the composite-shape impls compile; mirrors `TriMesh::triangle_normal_constraints`'s dim2 + /// stub. + #[cfg(feature = "dim3")] + #[doc(hidden)] + pub fn segment_normal_constraints(&self, _i: u32) -> Option { + None + } + /// Computes the axis-aligned bounding box of this polyline in world space. /// /// The AABB is the smallest box aligned with the world axes that fully contains @@ -574,10 +734,27 @@ impl Polyline { self.vertices.iter_mut().for_each(|pt| *pt *= scale); let mut bvh = self.bvh.clone(); bvh.scale(scale); - Self { - bvh, - vertices: self.vertices, - indices: self.indices, + + #[cfg(feature = "dim2")] + { + let mut result = Self { + bvh, + vertices: self.vertices, + indices: self.indices, + pseudo_normals: None, + flags: PolylineFlags::empty(), + }; + // Recompute the pseudo-normals from the scaled geometry. + result.set_flags(self.flags); + result + } + #[cfg(feature = "dim3")] + { + Self { + bvh, + vertices: self.vertices, + indices: self.indices, + } } } @@ -639,6 +816,12 @@ impl Polyline { let leaves = self.segments().map(|seg| seg.local_aabb()).enumerate(); let bvh = Bvh::from_iter(BvhBuildStrategy::Binned, leaves); self.bvh = bvh; + + // Reversing flips the winding, so recompute the outward side. + #[cfg(feature = "dim2")] + if self.flags.contains(PolylineFlags::ORIENTED) { + self.compute_pseudo_normals(); + } } /// Extracts the connected components of this polyline, consuming `self`. @@ -782,8 +965,13 @@ impl CompositeShape for Polyline { i: u32, f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ) { - let tri = self.segment(i); - f(None, &tri, None) + let seg = self.segment(i); + let normals = self.segment_normal_constraints(i); + f( + None, + &seg, + normals.as_ref().map(|n| n as &dyn NormalConstraints), + ) } fn bvh(&self) -> &Bvh { @@ -793,7 +981,7 @@ impl CompositeShape for Polyline { impl TypedCompositeShape for Polyline { type PartShape = Segment; - type PartNormalConstraints = (); + type PartNormalConstraints = SegmentPseudoNormals; #[inline(always)] fn map_typed_part_at( @@ -802,7 +990,8 @@ impl TypedCompositeShape for Polyline { mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option { let seg = self.segment(i); - Some(f(None, &seg, None)) + let normals = self.segment_normal_constraints(i); + Some(f(None, &seg, normals.as_ref())) } #[inline(always)] @@ -812,6 +1001,130 @@ impl TypedCompositeShape for Polyline { mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option { let seg = self.segment(i); - Some(f(None, &seg, None)) + let normals = self.segment_normal_constraints(i); + Some(f( + None, + &seg, + normals.as_ref().map(|n| n as &dyn NormalConstraints), + )) + } +} + +#[cfg(test)] +#[cfg(all(feature = "dim2", feature = "alloc"))] +mod pseudo_normal_tests { + use crate::math::Vector; + use crate::shape::{Polyline, PolylineFlags}; + + fn ccw_square() -> Polyline { + // CCW unit square: solid inside, outward away from the center. + let vertices = vec![ + Vector::new(-1.0, -1.0), + Vector::new(1.0, -1.0), + Vector::new(1.0, 1.0), + Vector::new(-1.0, 1.0), + ]; + Polyline::new(vertices, Some(vec![[0, 1], [1, 2], [2, 3], [3, 0]])) + } + + #[test] + fn not_oriented_by_default() { + assert!(ccw_square().segment_normal_constraints(0).is_none()); + } + + #[test] + fn face_and_edge_normals_are_unit_and_outward() { + let mut polyline = ccw_square(); + polyline.set_flags(PolylineFlags::ORIENTED); + + for i in 0..polyline.num_segments() as u32 { + let segment = polyline.segment(i); + let constraints = polyline.segment_normal_constraints(i).unwrap(); + + assert!((constraints.face.length() - 1.0).abs() < 1.0e-5); + for edge in constraints.edges { + assert!((edge.length() - 1.0).abs() < 1.0e-5); + } + + // The square is centered on the origin, so a midpoint doubles as its outward direction. + let midpoint = (segment.a + segment.b) * 0.5; + assert!(constraints.face.dot(midpoint) > 0.0); + } + } + + #[test] + fn corner_pseudo_normal_bisects_its_two_faces() { + let mut polyline = ccw_square(); + polyline.set_flags(PolylineFlags::ORIENTED); + + // Vertex 1's pseudo-normal bisects the bottom edge (-Y) and right edge (+X): (1, -1) normalized. + let bottom = polyline.segment_normal_constraints(0).unwrap(); + assert!(bottom.edges[1].abs_diff_eq(Vector::new(1.0, -1.0).normalize(), 1.0e-5)); + } + + #[test] + fn degenerate_input_yields_no_segments() { + // Auto-generated indices must not underflow `len - 1` on 0- or 1-vertex input. + assert_eq!(Polyline::new(vec![], None).num_segments(), 0); + assert_eq!(Polyline::new(vec![Vector::ZERO], None).num_segments(), 0); + } + + #[test] + fn oriented_point_query_treats_interior_as_inside() { + use crate::query::{PointQuery, PointQueryWithLocation}; + + let mut polyline = ccw_square(); + polyline.set_flags(PolylineFlags::ORIENTED); + let inside = Vector::new(0.25, -0.5); + + // Solid: an inside point is its own projection. + let solid = polyline.project_local_point(inside, true); + assert!(solid.is_inside); + assert!(solid.point.abs_diff_eq(inside, 1.0e-5)); + + // Non-solid: still inside, but projected to the boundary. + let hollow = polyline.project_local_point(inside, false); + assert!(hollow.is_inside); + assert!(!hollow.point.abs_diff_eq(inside, 1.0e-5)); + + assert!(polyline.contains_local_point(inside)); + assert!( + polyline + .project_local_point_and_get_location(inside, false) + .0 + .is_inside + ); + assert!( + polyline + .project_local_point_and_get_feature(inside) + .0 + .is_inside + ); + } + + #[test] + fn oriented_point_query_leaves_exterior_outside() { + use crate::query::PointQuery; + + let mut polyline = ccw_square(); + polyline.set_flags(PolylineFlags::ORIENTED); + let outside = Vector::new(2.0, 0.0); + + let proj = polyline.project_local_point(outside, true); + assert!(!proj.is_inside); + assert!(proj.point.abs_diff_eq(Vector::new(1.0, 0.0), 1.0e-5)); + assert!(!polyline.contains_local_point(outside)); + } + + #[test] + fn unoriented_polyline_has_no_interior() { + use crate::query::PointQuery; + + // Unoriented: a hollow wireframe with no interior. + let polyline = ccw_square(); + let center = Vector::ZERO; + + assert!(!polyline.project_local_point(center, true).is_inside); + assert!(!polyline.contains_local_point(center)); } } diff --git a/src/shape/pseudo_normals.rs b/src/shape/pseudo_normals.rs new file mode 100644 index 00000000..de27ca16 --- /dev/null +++ b/src/shape/pseudo_normals.rs @@ -0,0 +1,57 @@ +use crate::math::Vector; + +/// Projects `dir` into the outward cone formed by a feature's `face` normal and its `closest_edge` +/// pseudo-normal, shared by [`TrianglePseudoNormals`](crate::shape::TrianglePseudoNormals) and +/// [`SegmentPseudoNormals`](crate::shape::SegmentPseudoNormals). +/// +/// Returns whether `dir` lies in the front half-space; a contact whose normal points into the back +/// is meant to be discarded by the caller. +pub(crate) fn project_into_cone(face: Vector, closest_edge: Vector, dir: &mut Vector) -> bool { + let dot_face = dir.dot(face); + + // Apply the projection. Note that this isn’t 100% accurate since this approximates the + // vertex normal cone using the closest edge’s normal cone instead of the + // true polygonal cone on S² (but taking into account this polygonal cone exactly + // would be quite computationally expensive). + + if closest_edge == face { + // The normal cone is degenerate, there is only one possible direction. + *dir = face; + return dot_face >= 0.0; + } + + // TODO: take into account the two closest edges instead for better continuity + // of vertex normals? + let dot_edge_face = face.dot(closest_edge); + let dot_dir_face = face.dot(*dir); + let dot_corrected_dir_face = 2.0 * dot_edge_face * dot_edge_face - 1.0; // cos(2 * angle(closest_edge, face)) + + if dot_dir_face >= dot_corrected_dir_face { + // The direction is in the pseudo-normal cone. No correction to apply. + return true; + } + + // We need to correct. + let edge_on_normal = face * dot_edge_face; + let edge_orthogonal_to_normal = closest_edge - edge_on_normal; + + let dir_on_normal = face * dot_dir_face; + let dir_orthogonal_to_normal = *dir - dir_on_normal; + let Some(unit_dir_orthogonal_to_normal) = dir_orthogonal_to_normal.try_normalize() else { + return dot_face >= 0.0; + }; + + // NOTE: the normalization might be redundant as the result vector is guaranteed to be + // unit sized. Though some rounding errors could throw it off. + let adjusted_pseudo_normal = + edge_on_normal + unit_dir_orthogonal_to_normal * edge_orthogonal_to_normal.length(); + let (adjusted_pseudo_normal, length) = adjusted_pseudo_normal.normalize_and_length(); + if length <= 1.0e-6 { + return dot_face >= 0.0; + }; + + // The reflection of the face normal wrt. the adjusted pseudo-normal gives us the + // second end of the pseudo-normal cone the direction is projected on. + *dir = adjusted_pseudo_normal * (2.0 * face.dot(adjusted_pseudo_normal)) - face; + dot_face >= 0.0 +} diff --git a/src/shape/segment_pseudo_normals.rs b/src/shape/segment_pseudo_normals.rs new file mode 100644 index 00000000..773fa183 --- /dev/null +++ b/src/shape/segment_pseudo_normals.rs @@ -0,0 +1,95 @@ +use crate::math::Vector; + +#[cfg(feature = "alloc")] +use crate::query::details::NormalConstraints; + +/// The pseudo-normals of a polyline segment, approximating the outward normal cones of its +/// features. +/// +/// This is the 2D segment analog of [`TrianglePseudoNormals`](crate::shape::TrianglePseudoNormals): +/// `face` is the segment's outward normal and `edges` are the outward pseudo-normals at its two +/// endpoints. An oriented polyline uses them to clamp contact normals to one side, so it acts as a +/// one-sided surface. +#[derive(Clone, Debug)] +pub struct SegmentPseudoNormals { + /// The segment's outward normal. + pub face: Vector, + /// The outward pseudo-normals at the segment's two endpoints. + pub edges: [Vector; 2], +} + +#[cfg(feature = "alloc")] +impl NormalConstraints for SegmentPseudoNormals { + /// Projects `dir` so it lies within the outward cone defined by `self`. + fn project_local_normal_mut(&self, dir: &mut Vector) -> bool { + // Find the closest pseudo-normal. + let closest_edge = if dir.dot(self.edges[0]) >= dir.dot(self.edges[1]) { + self.edges[0] + } else { + self.edges[1] + }; + crate::shape::pseudo_normals::project_into_cone(self.face, closest_edge, dir) + } +} + +#[cfg(test)] +#[cfg(all(feature = "dim2", feature = "alloc"))] +mod test { + use super::{NormalConstraints, SegmentPseudoNormals}; + use crate::math::Vector; + + fn bisector(v1: Vector, v2: Vector) -> Vector { + (v1 + v2).normalize() + } + + #[test] + fn degenerate_cone_collapses_to_face() { + let pn = SegmentPseudoNormals { + face: Vector::Y, + edges: [Vector::Y, Vector::Y], + }; + + assert_eq!( + pn.project_local_normal(Vector::new(1.0, 1.0)), + Some(Vector::Y) + ); + assert!(pn.project_local_normal(-Vector::Y).is_none()); + } + + #[test] + fn clamps_into_the_outward_cone() { + // A cone centered on +Y, bounded at +-45 degrees (endpoint pseudo-normals at +-22.5 degrees). + let ends = [ + bisector(Vector::Y, Vector::X), + bisector(Vector::Y, -Vector::X), + ]; + let edges = [bisector(Vector::Y, ends[0]), bisector(Vector::Y, ends[1])]; + let pn = SegmentPseudoNormals { + face: Vector::Y, + edges, + }; + + // Inside the cone: returned unchanged. + assert_eq!(pn.project_local_normal(Vector::Y), Some(Vector::Y)); + for edge in edges { + assert_eq!(pn.project_local_normal(edge), Some(edge)); + } + let inside = Vector::new(0.2, 1.0).normalize(); + assert!(pn + .project_local_normal(inside) + .unwrap() + .abs_diff_eq(inside, 1.0e-5)); + + // Outside the cone but still outward: pulled onto the boundary, not left crossing the surface. + let sideways = Vector::new(1.0, 0.2).normalize(); + let clamped = pn.project_local_normal(sideways).unwrap(); + assert!(clamped.dot(Vector::Y) > sideways.dot(Vector::Y)); + assert!(clamped.abs_diff_eq(ends[0], 1.0e-5)); + + // Into the solid (-face half-space): rejected. + assert!(pn.project_local_normal(-Vector::Y).is_none()); + assert!(pn + .project_local_normal(Vector::new(0.3, -1.0).normalize()) + .is_none()); + } +} diff --git a/src/shape/triangle_pseudo_normals.rs b/src/shape/triangle_pseudo_normals.rs index 7f90f298..09bd9569 100644 --- a/src/shape/triangle_pseudo_normals.rs +++ b/src/shape/triangle_pseudo_normals.rs @@ -32,62 +32,14 @@ impl NormalConstraints for TrianglePseudoNormals { /// Projects the given direction to it is contained in the polygonal /// cone defined `self`. fn project_local_normal_mut(&self, dir: &mut Vector) -> bool { - let dot_face = dir.dot(self.face); - // Find the closest pseudo-normal. let dots = Vector3::new( dir.dot(self.edges[0]), dir.dot(self.edges[1]), dir.dot(self.edges[2]), ); - let closest_dot = dots.max_position(); - let closest_edge = self.edges[closest_dot]; - - // Apply the projection. Note that this isn’t 100% accurate since this approximates the - // vertex normal cone using the closest edge’s normal cone instead of the - // true polygonal cone on S² (but taking into account this polygonal cone exactly - // would be quite computationally expensive). - - if closest_edge == self.face { - // The normal cone is degenerate, there is only one possible direction. - *dir = self.face; - return dot_face >= 0.0; - } - - // TODO: take into account the two closest edges instead for better continuity - // of vertex normals? - let dot_edge_face = self.face.dot(closest_edge); - let dot_dir_face = self.face.dot(*dir); - let dot_corrected_dir_face = 2.0 * dot_edge_face * dot_edge_face - 1.0; // cos(2 * angle(closest_edge, face)) - - if dot_dir_face >= dot_corrected_dir_face { - // The direction is in the pseudo-normal cone. No correction to apply. - return true; - } - - // We need to correct. - let edge_on_normal = self.face * dot_edge_face; - let edge_orthogonal_to_normal = closest_edge - edge_on_normal; - - let dir_on_normal = self.face * dot_dir_face; - let dir_orthogonal_to_normal = *dir - dir_on_normal; - let Some(unit_dir_orthogonal_to_normal) = dir_orthogonal_to_normal.try_normalize() else { - return dot_face >= 0.0; - }; - - // NOTE: the normalization might be redundant as the result vector is guaranteed to be - // unit sized. Though some rounding errors could throw it off. - let adjusted_pseudo_normal = - edge_on_normal + unit_dir_orthogonal_to_normal * edge_orthogonal_to_normal.length(); - let (adjusted_pseudo_normal, length) = adjusted_pseudo_normal.normalize_and_length(); - if length <= 1.0e-6 { - return dot_face >= 0.0; - }; - - // The reflection of the face normal wrt. the adjusted pseudo-normal gives us the - // second end of the pseudo-normal cone the direction is projected on. - *dir = adjusted_pseudo_normal * (2.0 * self.face.dot(adjusted_pseudo_normal)) - self.face; - dot_face >= 0.0 + let closest_edge = self.edges[dots.max_position()]; + crate::shape::pseudo_normals::project_into_cone(self.face, closest_edge, dir) } }