| | | 1 | | //======================================================================= |
| | | 2 | | // MeshSurfaceBoundaryFan.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Colliders; |
| | | 12 | | using SwiftCollections; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.CollisionHandling; |
| | | 15 | | |
| | | 16 | | /// <summary>Collects exact incident sectors through certified positive-length surface joins.</summary> |
| | | 17 | | internal sealed class MeshSurfaceBoundaryFan |
| | | 18 | | { |
| | 966 | 19 | | private readonly MeshSurfaceBoundaryDomain _boundary = new(); |
| | 966 | 20 | | private readonly SwiftList<(int Member, int Surface)> _owners = new(); |
| | 966 | 21 | | private readonly SwiftHashSet<(int Member, int Surface)> _visited = new(); |
| | 966 | 22 | | private readonly SwiftList<WideAxis3> _rays = new(); |
| | | 23 | | private bool _trusted; |
| | | 24 | | |
| | 11 | 25 | | internal int RayCount => _rays.Count; |
| | 977 | 26 | | internal ReadOnlySpan<WideAxis3> Rays => _rays.AsReadOnlySpan(); |
| | 1201 | 27 | | internal ReadOnlySpan<(int Member, int Surface)> Owners => _owners.AsReadOnlySpan(); |
| | | 28 | | internal bool HasExposedBoundary { get; private set; } |
| | | 29 | | |
| | | 30 | | internal bool BuildEndpoint(ReadOnlySpan<PhysicsMesh> members, MeshSurfaceBoundaryDomain source, int span, bool uppe |
| | | 31 | | ReadOnlySpan<MeshSurfaceBoundaryDomain> prepared = default) |
| | | 32 | | { |
| | 122 | 33 | | var edge = source.Spans[span]; |
| | 122 | 34 | | Signed832 n = upper ? edge.Interval.WideUpperNumerator : edge.Interval.WideLowerNumerator; |
| | 122 | 35 | | Signed832 d = upper ? edge.Interval.WideUpperDenominator : edge.Interval.WideLowerDenominator; |
| | 250 | 36 | | if (!edge.Interval.IsClipped) { n = upper ? Signed832.ExtendValue(Signed192.Signed(1)) : default; d = Signed832. |
| | 122 | 37 | | return Build(members, source.SourceMember, source.SourceSurface, |
| | 122 | 38 | | new FixedSegment(edge.Start, edge.End), n, d, prepared); |
| | | 39 | | } |
| | | 40 | | |
| | | 41 | | internal bool BuildAtPoint(ReadOnlySpan<PhysicsMesh> members, int sourceMember, int sourceSurface, Vector3d point, |
| | | 42 | | ReadOnlySpan<MeshSurfaceBoundaryDomain> prepared = default) => |
| | 917 | 43 | | Build(members, sourceMember, sourceSurface, new FixedSegment(point, point), default, Signed832.ExtendValue(Signe |
| | | 44 | | |
| | | 45 | | internal bool BuildAtAnchor(ReadOnlySpan<PhysicsMesh> members, int sourceMember, int sourceSurface, |
| | | 46 | | in FixedPointAnchor point, ReadOnlySpan<MeshSurfaceBoundaryDomain> prepared) => |
| | 43 | 47 | | Build(members, sourceMember, sourceSurface, default, default, Signed832.ExtendValue(Signed192.Signed(1)), |
| | 43 | 48 | | prepared, RationalPoint3d.FromAnchor(point)); |
| | | 49 | | |
| | | 50 | | // The caller supplies an admitted point. Traverse only positive-length |
| | | 51 | | // certified joins incident to it; a point-only neighbor adds no sector. |
| | | 52 | | private bool Build(ReadOnlySpan<PhysicsMesh> members, int sourceMember, int sourceSurface, |
| | | 53 | | FixedSegment source, Signed832 n, Signed832 d, ReadOnlySpan<MeshSurfaceBoundaryDomain> prepared = default, |
| | | 54 | | RationalPoint3d? point = null) |
| | | 55 | | { |
| | 5410 | 56 | | _trusted = false; HasExposedBoundary = false; _owners.FastClear(); _visited.Clear(); _rays.FastClear(); |
| | 1082 | 57 | | var first = (sourceMember, members[sourceMember].GetManifoldSurfaceOwner(sourceSurface)); |
| | 2164 | 58 | | _owners.Add(first); _visited.Add(first); |
| | 1082 | 59 | | PhysicsMesh original = members[sourceMember]; |
| | 1082 | 60 | | Span<WideAxis3> fan = stackalloc WideAxis3[4]; |
| | 5946 | 61 | | for (int index = 0; index < _owners.Count; index++) |
| | | 62 | | { |
| | 3822 | 63 | | var key = _owners[index]; PhysicsMesh mesh = members[key.Member]; |
| | 1911 | 64 | | MeshSurfaceBoundaryDomain boundary = _boundary; |
| | 2056 | 65 | | if (prepared.IsEmpty) boundary.Build(members, key.Member, key.Surface); |
| | | 66 | | else |
| | | 67 | | { |
| | 1766 | 68 | | MeshSurfaceBoundaryDomain? found = null; |
| | 6963 | 69 | | foreach (var candidate in prepared) |
| | 6118 | 70 | | if (candidate.SourceMember == key.Member && candidate.SourceSurface == key.Surface) { found = candid |
| | 1776 | 71 | | if (found == null) { HasExposedBoundary = true; continue; } |
| | 1761 | 72 | | boundary = found; |
| | | 73 | | } |
| | 22366 | 74 | | foreach (var edge in boundary.Spans) |
| | | 75 | | { |
| | 9286 | 76 | | int location = point.HasValue ? WideTriangleRelations.LocatePointOnFrameEdge(point.Value, |
| | 9286 | 77 | | new FixedSegment(edge.Start, edge.End), mesh.GeometryFrame, edge.Interval) |
| | 9286 | 78 | | : WideTriangleRelations.LocateEdgePoint(source, original.GeometryFrame, n, d, |
| | 9286 | 79 | | new FixedSegment(edge.Start, edge.End), mesh.GeometryFrame, edge.Interval); |
| | 9286 | 80 | | if (location < 0) continue; |
| | 3533 | 81 | | if (edge.Kind == SpanKind.Ambiguous) { _rays.FastClear(); return false; } |
| | 5244 | 82 | | if (edge.Kind == SpanKind.Exposed) HasExposedBoundary = true; |
| | 3479 | 83 | | var neighbor = (edge.OtherMember, edge.OtherSurface); |
| | 4308 | 84 | | if (edge.Kind == SpanKind.Joined && _visited.Add(neighbor)) _owners.Add(neighbor); |
| | | 85 | | } |
| | 1888 | 86 | | int count = GetFan(mesh, key.Surface, source, original, n, d, fan, point, |
| | 1888 | 87 | | key.Member == sourceMember && key.Surface == first.Item2 ? sourceSurface : key.Surface); |
| | 1892 | 88 | | if (count == 0) { _rays.FastClear(); return false; } |
| | | 89 | | // Each positive affine denominator is irrelevant to a ray's halfspace sign. |
| | 12624 | 90 | | for (int ray = 0; ray < count; ray++) |
| | 4426 | 91 | | _rays.Add(mesh.GeometryFrame.GetWorldDirectionNumerators(fan[ray])); |
| | | 92 | | } |
| | 1062 | 93 | | _trusted = true; |
| | 1062 | 94 | | return true; |
| | | 95 | | } |
| | | 96 | | |
| | | 97 | | private static int GetFan(PhysicsMesh mesh, int owner, FixedSegment source, PhysicsMesh original, |
| | | 98 | | Signed832 n, Signed832 d, Span<WideAxis3> fan, RationalPoint3d? point, int admittedTriangle) |
| | | 99 | | { |
| | 1888 | 100 | | ReadOnlySpan<int> edges = mesh.GetCanonicalSurfaceBoundaryVertexPairs(owner); |
| | 1888 | 101 | | ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices; |
| | 9842 | 102 | | for (int edge = 0; edge < edges.Length; edge += 2) |
| | | 103 | | { |
| | 9654 | 104 | | int first = edges[edge], second = edges[edge + 1]; |
| | 4827 | 105 | | int location = point.HasValue ? WideTriangleRelations.LocatePointOnFrameEdge(point.Value, |
| | 4827 | 106 | | new FixedSegment(vertices[first], vertices[second]), mesh.GeometryFrame) |
| | 4827 | 107 | | : WideTriangleRelations.LocateEdgePoint(source, original.GeometryFrame, n, d, |
| | 4827 | 108 | | new FixedSegment(vertices[first], vertices[second]), mesh.GeometryFrame); |
| | 4827 | 109 | | if (location < 0) continue; |
| | 2802 | 110 | | if (location == 2) (first, second) = (second, first); |
| | 1794 | 111 | | return MeshSurfaceBoundaryTopology.GetIncidentFan(mesh, owner, first, second, location == 1, fan); |
| | | 112 | | } |
| | 94 | 113 | | if (point.HasValue) |
| | | 114 | | { |
| | | 115 | | // An authored diagonal inside a certified coplanar owner is filled |
| | | 116 | | // surface, not a rim. Final anchor rounding can put the point on |
| | | 117 | | // either incident triangle; both still require exact finite proof. |
| | 8 | 118 | | bool admitted = PointOnTriangle(mesh, admittedTriangle, point.Value); |
| | | 119 | | // Prepared coplanar neighbors share this committed manifold owner. |
| | 32 | 120 | | foreach (int neighbor in mesh.GetCoplanarTriangleNeighbors(admittedTriangle)) |
| | 8 | 121 | | if (!admitted) |
| | 5 | 122 | | admitted = PointOnTriangle(mesh, neighbor, point.Value); |
| | 10 | 123 | | if (!admitted) return 0; |
| | | 124 | | } |
| | 92 | 125 | | mesh.GetGeometryTriangleVertices(owner, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 184 | 126 | | fan[0] = MeshSurfaceBoundaryTopology.Ray(a, b); fan[1] = -fan[0]; |
| | 184 | 127 | | fan[2] = MeshSurfaceBoundaryTopology.Ray(a, c); fan[3] = -fan[2]; |
| | 92 | 128 | | return 4; |
| | | 129 | | } |
| | | 130 | | |
| | | 131 | | private static bool PointOnTriangle(PhysicsMesh mesh, int triangle, in RationalPoint3d point) |
| | | 132 | | { |
| | 183 | 133 | | mesh.GetGeometryTriangleVertices(triangle, out var a, out var b, out var c); |
| | 183 | 134 | | FixedTriangle face = new(a, b, c); |
| | 183 | 135 | | var frame = mesh.GeometryFrame; |
| | 183 | 136 | | WideDirection3d normal = WideTriangleRelations.GetScaledTriangleLocalNormal(face, frame).Transform(frame.Geometr |
| | 183 | 137 | | return WideDirection3d.Dot(normal, WideTriangleRelations.GetScaledPointDifference(point, |
| | 183 | 138 | | frame.GetPoint(a), frame.RotationDenominator)).IsZero && WideTriangleRelations.ContainsProjection(face, fram |
| | | 139 | | } |
| | | 140 | | |
| | | 141 | | internal static bool SupportsTriangle(PhysicsMesh mesh, int triangle, in RationalPoint3d point, |
| | | 142 | | in WideDirection3d direction) |
| | | 143 | | { |
| | | 144 | | // A rounded closest witness can land on a seam without remaining the |
| | | 145 | | // actual closest point. Finite incidence and the triangle normal cone |
| | | 146 | | // certify that the supplied point-to-core direction still owns it. |
| | 171 | 147 | | if (!PointOnTriangle(mesh, triangle, point)) return false; |
| | 169 | 148 | | mesh.GetGeometryTriangleVertices(triangle, out var a, out var b, out var c); |
| | 169 | 149 | | Span<Vector3d> vertices = stackalloc Vector3d[3] { a, b, c }; |
| | 1344 | 150 | | foreach (Vector3d vertex in vertices) |
| | 505 | 151 | | if (WideDirection3d.Dot(direction, WideTriangleRelations.GetScaledPointDifference( |
| | 505 | 152 | | mesh.GeometryFrame.GetPoint(vertex), point, |
| | 509 | 153 | | WideArithmetic.TruncateSigned320By64(point.Denominator))).Sign > 0) return false; |
| | 165 | 154 | | return true; |
| | | 155 | | } |
| | | 156 | | /// <summary>Tests an exact world direction against every incident inward ray.</summary> |
| | | 157 | | internal bool Supports(in WideDirection3d direction) |
| | | 158 | | { |
| | 73 | 159 | | if (!_trusted) return false; |
| | 671 | 160 | | foreach (WideAxis3 ray in _rays.AsReadOnlySpan()) |
| | 338 | 161 | | if (WideDirection3d.Dot(direction, ray).Sign > 0) return false; |
| | 20 | 162 | | return true; |
| | | 163 | | } |
| | | 164 | | |
| | 40 | 165 | | internal bool Supports(Vector3d direction) => Supports(new WideDirection3d( |
| | 40 | 166 | | Signed320.ExtendValue(Signed192.Raw(direction.X)), Signed320.ExtendValue(Signed192.Raw(direction.Y)), |
| | 40 | 167 | | Signed320.ExtendValue(Signed192.Raw(direction.Z)))); |
| | | 168 | | } |