| | | 1 | | //======================================================================= |
| | | 2 | | // ConvexHullCylinderContact.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using static FixedMathSharp.Geometry.CylinderContactAlgebra; |
| | | 10 | | |
| | | 11 | | namespace FixedMathSharp.Geometry; |
| | | 12 | | |
| | | 13 | | /// <summary>Whole-volume contact between a validated closed convex hull and a finite cylinder or stadium slab.</summary |
| | | 14 | | internal static class ConvexHullCylinderContact |
| | | 15 | | { |
| | | 16 | | /// <summary> |
| | | 17 | | /// Gets whole-volume contact when the cylinder center is inside or on the |
| | | 18 | | /// validated closed hull. False declines center admission, not exterior overlap. |
| | | 19 | | /// </summary> |
| | | 20 | | /// <remarks>A positive planar core describes a stadium slab; its height is twice the admitted half-thickness.</rema |
| | | 21 | | internal static bool TryGetContactWithContainedCenter(in ScaledRigidFrame3d source, ReadOnlySpan<Vector3d> points, |
| | | 22 | | ReadOnlySpan<int> triangles, Vector3d center, FixedQuaternion rotation, Fixed64 height, Fixed64 radius, |
| | | 23 | | out FixedContactAnchors contact, Vector2d coreDirection = default, Fixed64 coreLength = default) |
| | | 24 | | { |
| | 116 | 25 | | contact = default; |
| | 117 | 26 | | if (!WideOrientedBox.ContainsConvexHullPoint(source, points, triangles, center)) return false; |
| | 115 | 27 | | bool found = TryGetContact(source, points, triangles, center, rotation, height, radius, out contact, coreDirecti |
| | | 28 | | System.Diagnostics.Debug.Assert(found, "An admitted center must have whole-volume contact."); |
| | 115 | 29 | | return found; |
| | | 30 | | } |
| | | 31 | | |
| | | 32 | | internal static bool TryGetContact(in ScaledRigidFrame3d source, ReadOnlySpan<Vector3d> points, |
| | | 33 | | ReadOnlySpan<int> triangles, Vector3d center, FixedQuaternion rotation, Fixed64 height, Fixed64 radius, |
| | | 34 | | out FixedContactAnchors contact, Vector2d coreDirection = default, Fixed64 coreLength = default) |
| | | 35 | | { |
| | 241 | 36 | | contact = default; |
| | 241 | 37 | | var hull = new CylinderHullSupport(source, points, center, rotation, height, radius); |
| | 482 | 38 | | WideDirection3d coreOffset = default, coreAxis = default; |
| | 241 | 39 | | TriangleCircularGeometry coreChart = default; |
| | 241 | 40 | | if (coreLength != Fixed64.Zero) |
| | | 41 | | { |
| | 119 | 42 | | coreChart = GetGeometry(GetTriangle(points, triangles, 0), source, hull, triangles, 0) |
| | 119 | 43 | | .WithCore(coreDirection, coreLength, out coreOffset); |
| | 119 | 44 | | coreChart = coreChart.WithValueShift(hull.GetCoreValueShift(coreChart, coreOffset)); |
| | 119 | 45 | | coreAxis = new WideDirection3d(Signed576.ExtendValue(Signed192.Raw(coreDirection.X)), default, |
| | 119 | 46 | | Signed576.ExtendValue(Signed192.Raw(coreDirection.Y))); |
| | | 47 | | } |
| | 482 | 48 | | Span<ulong> bestValues = stackalloc ulong[ConvexContactCandidate.Slots * Words], values = stackalloc ulong[Conve |
| | 482 | 49 | | Span<int> bestSigns = stackalloc int[ConvexContactCandidate.Slots], signs = stackalloc int[ConvexContactCandidat |
| | 482 | 50 | | Span<ulong> bestDirection = stackalloc ulong[3 * Words], direction = stackalloc ulong[3 * Words]; |
| | 482 | 51 | | Span<int> bestDirectionSigns = stackalloc int[3], directionSigns = stackalloc int[3]; |
| | 964 | 52 | | int bestTriangle = -1, bestMask = 0, bestCoreSign = 0, gap = 0; |
| | 241 | 53 | | bool certified = false; |
| | 998 | 54 | | for (int pass = coreAxis.IsZero ? 1 : 0; pass <= 1 && !certified; pass++) |
| | | 55 | | { |
| | | 56 | | // A failed face seed must not change the complete fan's tie order. |
| | 266 | 57 | | bestTriangle = -1; |
| | 5044 | 58 | | for (int index = 0; index < triangles.Length; index += 3) |
| | | 59 | | { |
| | 2333 | 60 | | FixedTriangle triangle = GetTriangle(points, triangles, index); |
| | 2333 | 61 | | var geometry = GetGeometry(triangle, source, hull, triangles, index, coreChart); |
| | | 62 | | int candidateGap, mask, coreSign; |
| | 2333 | 63 | | bool found = pass == 0 |
| | 2333 | 64 | | ? TriangleCylinderAnalyticFeatures.TryGetSeed(geometry, coreOffset, coreAxis, values, signs, directi |
| | 2333 | 65 | | out candidateGap, out mask, out coreSign, hull, includeSeam: index == 0) |
| | 2333 | 66 | | : TriangleCylinderAnalyticFeatures.TryGetBest(geometry, coreOffset, coreAxis, values, signs, |
| | 2333 | 67 | | direction, directionSigns, out candidateGap, out mask, out coreSign, out _, hull); |
| | 2341 | 68 | | if (!found) return false; |
| | 2325 | 69 | | if (bestTriangle >= 0 && WideConvexPrismRelations.CompareConvexContactCandidates( |
| | 2325 | 70 | | new ConvexContactCandidate(values, signs, candidateGap), new ConvexContactCandidate(bestValues, |
| | 798 | 71 | | values.CopyTo(bestValues); signs.CopyTo(bestSigns); |
| | 798 | 72 | | direction.CopyTo(bestDirection); directionSigns.CopyTo(bestDirectionSigns); |
| | 1596 | 73 | | bestTriangle = index; bestMask = mask; bestCoreSign = coreSign; gap = candidateGap; |
| | 399 | 74 | | if (hull.HasBallMinimumCertificate(source, triangles, new ConvexContactCandidate(bestValues, bestSigns, |
| | 138 | 75 | | { certified = true; break; } |
| | | 76 | | } |
| | 258 | 77 | | if (pass == 0 && !certified) |
| | 71 | 78 | | certified = hull.HasExtrudedBallMinimumCertificate(source, triangles, coreChart, coreOffset, coreAxis, |
| | 71 | 79 | | new ConvexContactCandidate(bestValues, bestSigns, gap)); |
| | | 80 | | } |
| | 234 | 81 | | if (bestTriangle < 0) return false; |
| | 232 | 82 | | var bestGeometry = GetGeometry(GetTriangle(points, triangles, bestTriangle), source, hull, triangles, bestTriang |
| | 255 | 83 | | if (bestCoreSign != 0) bestGeometry = bestGeometry.AtCoreRegion(coreOffset, bestCoreSign); |
| | 232 | 84 | | var analytic = new ConvexContactCandidate(bestValues, bestSigns, gap); |
| | 232 | 85 | | bestGeometry.GetAnalyticDepth(analytic, bestDirectionSigns, bestMask, out Fixed64 depth, out bool clamped); |
| | | 86 | | |
| | 232 | 87 | | int valueWords = bestGeometry.ValueWords; |
| | 464 | 88 | | Span<ulong> rootValues = stackalloc ulong[5 * valueWords]; Span<sbyte> rootSigns = stackalloc sbyte[5]; |
| | 232 | 89 | | Span<ulong> rootCell = stackalloc ulong[(16 * (valueWords * 64 + 11) + 255) / 64]; |
| | 232 | 90 | | TriangleCylinderEdgeContacts.Selection rootSelection = default; |
| | 232 | 91 | | if (!certified && radius != Fixed64.Zero) |
| | 2960 | 92 | | for (int index = 0; index < triangles.Length; index += 3) |
| | | 93 | | { |
| | 1365 | 94 | | var geometry = GetGeometry(GetTriangle(points, triangles, index), source, hull, triangles, index, coreCh |
| | 1365 | 95 | | if (!TriangleCylinderEdgeContacts.TryKeepContacts(geometry, coreOffset, coreAxis, analytic, depth, clamp |
| | 1366 | 96 | | rootValues, rootSigns, rootCell, ref rootSelection, hull, index)) return false; |
| | | 97 | | } |
| | 231 | 98 | | Vector3d origin = source.IsRigid ? source.RigidOrigin : center; |
| | 231 | 99 | | FixedQuaternion anchorRotation = source.IsRigid ? source.Rotation : rotation; |
| | 231 | 100 | | if (rootSelection.HasValue) |
| | | 101 | | { |
| | 18 | 102 | | FixedTriangle triangle = GetTriangle(points, triangles, rootSelection.Triangle); |
| | 18 | 103 | | var geometry = GetGeometry(triangle, source, hull, triangles, rootSelection.Triangle, coreChart); |
| | 25 | 104 | | if (rootSelection.CoreSign != 0) geometry = geometry.AtCoreRegion(coreOffset, rootSelection.CoreSign); |
| | 18 | 105 | | TriangleCylinderEdgeContacts.GetMaterials(geometry, coreAxis, coreDirection, coreLength, triangle, rootSelec |
| | 18 | 106 | | out Vector3d normal, out Vector3d radial, out Vector3d point, out int integralMask); |
| | 20 | 107 | | if (rootSelection.IsZero) { depth = Fixed64.Zero; clamped = false; } |
| | | 108 | | else |
| | | 109 | | { |
| | 17 | 110 | | FiniteAxisValueRoot root = TriangleCylinderEdgeContacts.Restore(rootValues, rootSigns, rootCell, rootSel |
| | 17 | 111 | | ConvexContactValueRoot.GetRoundedDepth(geometry.RawScale, geometry.ValueShift, ref root, out depth, out |
| | | 112 | | } |
| | 18 | 113 | | FixedPointAnchor cylinderAnchor = TriangleCircularGeometry.GetSupport(center, rotation, Signed192.Raw(height |
| | 18 | 114 | | if (rootSelection.CoreSign != 0) |
| | | 115 | | { |
| | 7 | 116 | | Vector3d core = TriangleCapsuleSlabEndpointWitnesses.GetCore(coreDirection, coreLength, rootSelection.Co |
| | 7 | 117 | | cylinderAnchor = TriangleCylinderContact.AddCoreEndpoint(cylinderAnchor, core, coreDirection, coreLength |
| | | 118 | | } |
| | 18 | 119 | | contact = new FixedContactAnchors(new FixedPointAnchor(origin, anchorRotation, point), |
| | 18 | 120 | | cylinderAnchor, normal, depth, clamped); |
| | 18 | 121 | | return true; |
| | | 122 | | } |
| | 213 | 123 | | Vector3d analyticNormal = WideConvexPrismRelations.GetConvexContactCandidateNormal(analytic); |
| | 426 | 124 | | bestDirection.CopyTo(values); bestDirectionSigns.CopyTo(signs); |
| | 426 | 125 | | Slot(values, 1).Clear(); signs[1] = 0; |
| | 213 | 126 | | bool pole = bestDirectionSigns[0] == 0 && bestDirectionSigns[2] == 0; |
| | 213 | 127 | | Vector3d analyticRadial = pole ? default |
| | 213 | 128 | | : -WideConvexPrismRelations.GetConvexContactCandidateScaledNormal(new ConvexContactCandidate(values, signs, |
| | 213 | 129 | | Vector3d analyticCore = default; |
| | 213 | 130 | | int analyticIntegralMask = 5; |
| | 213 | 131 | | if (bestCoreSign != 0) |
| | | 132 | | { |
| | 16 | 133 | | analyticCore = TriangleCapsuleSlabEndpointWitnesses.GetCore(coreDirection, coreLength, bestCoreSign, out Fix |
| | 16 | 134 | | if (radius != Fixed64.Zero) |
| | 8 | 135 | | analyticRadial = TriangleCapsuleSlabEndpointWitnesses.AdjustAnalytic(bestDirection, bestDirectionSigns, |
| | 8 | 136 | | radius, analyticCore, term, analyticRadial, out analyticIntegralMask); |
| | | 137 | | } |
| | | 138 | | // A triangulated support face may carry the matching point in a different |
| | | 139 | | // triangle from the first candidate. Certify the finite witness before rounding. |
| | 588 | 140 | | for (int dimension = 3; dimension >= 1; dimension--) |
| | 4326 | 141 | | for (int index = 0; index < triangles.Length; index += 3) |
| | | 142 | | { |
| | 2082 | 143 | | FixedTriangle triangle = GetTriangle(points, triangles, index); |
| | 2082 | 144 | | var geometry = GetGeometry(triangle, source, hull, triangles, index, coreChart); |
| | 2082 | 145 | | var feature = bestCoreSign == 0 ? geometry : geometry.AtCoreRegion(coreOffset, bestCoreSign); |
| | 2082 | 146 | | int mask = geometry.GetSupportMask(bestDirection, bestDirectionSigns); |
| | 2082 | 147 | | int count = (mask & 1) + ((mask >> 1) & 1) + ((mask >> 2) & 1); |
| | 2082 | 148 | | if (count != dimension) continue; |
| | 454 | 149 | | int vertex = (mask & 1) != 0 ? 0 : (mask & 2) != 0 ? 1 : 2; |
| | 454 | 150 | | if (!hull.Supports(geometry, geometry.Vertex(vertex), bestDirection, bestDirectionSigns)) continue; |
| | | 151 | | FixedPointAnchor cylinderAnchor; |
| | | 152 | | Vector3d point; |
| | 244 | 153 | | int cap = bestDirectionSigns[1]; |
| | 244 | 154 | | if (!coreAxis.IsZero && bestCoreSign == 0) |
| | | 155 | | { |
| | 96 | 156 | | Vector3d radial = analyticRadial; |
| | 96 | 157 | | if (!TriangleCapsuleSlabWitnesses.TryGetPoints(geometry, triangle, mask, bestDirection, bestDirectio |
| | 96 | 158 | | coreOffset, coreDirection, coreLength, ref radial, out point, out Vector3d coreAndAxialPoint |
| | 96 | 159 | | out FixedPointAnchorTerm3d term)) continue; |
| | 96 | 160 | | cylinderAnchor = new FixedPointAnchor(center, rotation, coreAndAxialPoint, radial, term); |
| | | 161 | | } |
| | 148 | 162 | | else if (cap == 0) |
| | | 163 | | { |
| | 41 | 164 | | if (!TriangleCylinderWitnesses.TryGetSidePoint(feature, triangle, mask, bestDirection, bestDirection |
| | 41 | 165 | | out point, out Fixed64 axial, out int boundaryCap)) continue; |
| | 40 | 166 | | cylinderAnchor = boundaryCap != 0 |
| | 40 | 167 | | ? TriangleCircularGeometry.GetSupport(center, rotation, Signed192.Raw(height), analyticRadial, b |
| | 40 | 168 | | : new FixedPointAnchor(center, rotation, new Vector3d(Fixed64.Zero, axial, Fixed64.Zero), analyt |
| | | 169 | | } |
| | | 170 | | else |
| | | 171 | | { |
| | 107 | 172 | | Vector3d radial = analyticRadial; |
| | 107 | 173 | | if (pole) |
| | | 174 | | { |
| | 30 | 175 | | if (!TriangleCylinderWitnesses.TryGetCapPoint(feature, triangle, mask, out point, out radial)) c |
| | | 176 | | } |
| | 77 | 177 | | else if (!TriangleCylinderRimWitnesses.TryGetAnalyticPoint(feature, triangle, mask, |
| | 77 | 178 | | bestDirection, bestDirectionSigns, cap, geometry.Radius, out point)) continue; |
| | 76 | 179 | | cylinderAnchor = TriangleCircularGeometry.GetSupport(center, rotation, Signed192.Raw(height), radial |
| | | 180 | | } |
| | 212 | 181 | | if (bestCoreSign != 0) |
| | 16 | 182 | | cylinderAnchor = TriangleCylinderContact.AddCoreEndpoint(cylinderAnchor, analyticCore, coreDirection |
| | 16 | 183 | | bestCoreSign, analyticIntegralMask); |
| | 212 | 184 | | contact = new FixedContactAnchors(new FixedPointAnchor(origin, anchorRotation, point), cylinderAnchor, |
| | 212 | 185 | | analyticNormal, depth, clamped); |
| | 212 | 186 | | return true; |
| | | 187 | | } |
| | | 188 | | System.Diagnostics.Debug.Fail("A whole-hull minimum must have a finite matching witness."); |
| | 1 | 189 | | return false; |
| | | 190 | | } |
| | | 191 | | |
| | | 192 | | private static FixedTriangle GetTriangle(ReadOnlySpan<Vector3d> points, ReadOnlySpan<int> triangles, int index) => |
| | 6149 | 193 | | new(points[triangles[index]], points[triangles[index + 1]], points[triangles[index + 2]]); |
| | | 194 | | |
| | | 195 | | private static TriangleCircularGeometry GetGeometry(FixedTriangle triangle, in ScaledRigidFrame3d source, |
| | | 196 | | in CylinderHullSupport hull, ReadOnlySpan<int> triangles, int index, in TriangleCircularGeometry coreChart = def |
| | | 197 | | { |
| | 6149 | 198 | | var geometry = new TriangleCircularGeometry(triangle, source, hull, triangles[index], triangles[index + 1], tria |
| | 6149 | 199 | | return coreChart.RawScale.Sign == 0 ? geometry : geometry.WithCoreChart(coreChart); |
| | | 200 | | } |
| | | 201 | | |
| | | 202 | | } |