| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleConeContact.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>Complete finite-cone/triangle support-fan contact ownership.</summary> |
| | | 14 | | /// <content> |
| | | 15 | | /// Provides complete finite-triangle contacts through exact support-fan admission and matched witnesses. |
| | | 16 | | /// </content> |
| | | 17 | | internal static partial class TriangleConeContact |
| | | 18 | | { |
| | | 19 | | internal static bool TryGetContact(FixedTriangle triangle, Vector3d origin, FixedQuaternion rotation, |
| | | 20 | | Vector3d center, FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius, |
| | | 21 | | out FixedContactAnchors contact) |
| | | 22 | | { |
| | 140 | 23 | | contact = default; |
| | 140 | 24 | | if (triangle.IsDegenerate) |
| | 3 | 25 | | return false; |
| | 137 | 26 | | if (radius == Fixed64.Zero) |
| | 4 | 27 | | return TriangleCylinderContact.TryGetContact(triangle, origin, rotation, center, coneRotation, |
| | 4 | 28 | | Signed192.Raw(height), radius, out contact, out _); |
| | 133 | 29 | | var geometry = new TriangleCircularGeometry(triangle, origin, rotation, center, coneRotation, |
| | 133 | 30 | | Signed192.Raw(height), radius); |
| | 133 | 31 | | return TryGetContact(triangle, geometry, origin, rotation, center, coneRotation, height, radius, out contact); |
| | | 32 | | } |
| | | 33 | | |
| | | 34 | | internal static bool TryGetContact(FixedTriangle triangle, in ScaledRigidFrame3d sourceFrame, |
| | | 35 | | Vector3d center, FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius, |
| | | 36 | | out FixedContactAnchors contact) |
| | | 37 | | { |
| | 11 | 38 | | if (sourceFrame.IsRigid) |
| | 1 | 39 | | return TryGetContact(triangle, sourceFrame.RigidOrigin, sourceFrame.Rotation, |
| | 1 | 40 | | center, coneRotation, height, radius, out contact); |
| | 10 | 41 | | contact = default; |
| | 10 | 42 | | if (radius == Fixed64.Zero) |
| | 1 | 43 | | return TriangleCylinderContact.TryGetContact(triangle, sourceFrame, center, coneRotation, |
| | 1 | 44 | | Signed192.Raw(height), radius, out contact, out _); |
| | 9 | 45 | | var geometry = new TriangleCircularGeometry(triangle, sourceFrame, center, coneRotation, Signed192.Raw(height), |
| | | 46 | | // Physical scale, rather than authored epsilon area, owns this fan. |
| | 6 | 47 | | if (geometry.FaceNormal.IsZero) return false; |
| | 4 | 48 | | return TryGetContact(triangle, geometry, center, coneRotation, center, coneRotation, height, radius, out contact |
| | | 49 | | } |
| | | 50 | | |
| | | 51 | | private static bool TryGetContact(FixedTriangle triangle, in TriangleCircularGeometry geometry, |
| | | 52 | | Vector3d origin, FixedQuaternion rotation, Vector3d center, FixedQuaternion coneRotation, |
| | | 53 | | Fixed64 height, Fixed64 radius, out FixedContactAnchors contact) |
| | | 54 | | { |
| | 137 | 55 | | contact = default; |
| | 137 | 56 | | Span<ulong> bestValues = stackalloc ulong[ConvexContactCandidate.Slots * Words]; |
| | 137 | 57 | | Span<int> bestSigns = stackalloc int[ConvexContactCandidate.Slots]; |
| | 137 | 58 | | var selection = new TriangleConeContactSelection(bestValues, bestSigns); |
| | 137 | 59 | | bool faceMinimumCertified = KeepAnalytic(geometry, height, radius, ref selection); |
| | 137 | 60 | | if (selection.Separated) |
| | 23 | 61 | | return false; |
| | 114 | 62 | | var best = selection.Candidate; |
| | 114 | 63 | | bool rootWinner = false; |
| | 342 | 64 | | Vector3d normal = default, radialPoint = default, point = default; |
| | | 65 | | // Generator normals can mix rational and radical components; |
| | | 66 | | // only rational local normals use the principal-axis cancellation. |
| | | 67 | | Fixed64 depth; |
| | | 68 | | bool clamped; |
| | 114 | 69 | | if (selection.Feature == TriangleConeContactSelection.Generator) |
| | 40 | 70 | | WideConvexPrismRelations.GetRoundedConvexContactCandidateDepth(best, Fixed64.Zero, out depth, out clamped); |
| | | 71 | | else |
| | 74 | 72 | | geometry.GetAnalyticDepth(best, bestSigns, selection.Mask, out depth, out clamped); |
| | | 73 | | // Both exact face orientations bound this winner by half the cone's |
| | | 74 | | // width. Diameter=max(2R,sqrt(R^2+H^2))<=2*MaxValue, so it cannot clamp. |
| | | 75 | | // Exact orthogonal frames and the generator seam preserve this bound. |
| | | 76 | | System.Diagnostics.Debug.Assert(!clamped); |
| | 114 | 77 | | if (!faceMinimumCertified && !TriangleConeRimContacts.TryGetContact(geometry, triangle, height, radius, best, de |
| | 114 | 78 | | out rootWinner, out normal, out radialPoint, out point, out depth, out clamped)) |
| | 1 | 79 | | return false; |
| | | 80 | | FixedPointAnchor coneAnchor; |
| | 113 | 81 | | if (rootWinner) |
| | 15 | 82 | | coneAnchor = TriangleCircularGeometry.GetSupport(center, coneRotation, Signed192.Raw(height), radialPoint, 1 |
| | | 83 | | else |
| | | 84 | | { |
| | 98 | 85 | | GetAnalyticWitnesses(geometry, triangle, height, radius, center, coneRotation, |
| | 98 | 86 | | ref selection, out normal, out point, out coneAnchor); |
| | | 87 | | } |
| | 113 | 88 | | contact = new FixedContactAnchors(new FixedPointAnchor(origin, rotation, point), coneAnchor, |
| | 113 | 89 | | normal, depth, clamped); |
| | 113 | 90 | | return true; |
| | | 91 | | } |
| | | 92 | | |
| | | 93 | | private static void GetAnalyticWitnesses(in TriangleCircularGeometry geometry, FixedTriangle triangle, |
| | | 94 | | Fixed64 height, Fixed64 radius, Vector3d center, FixedQuaternion coneRotation, |
| | | 95 | | ref TriangleConeContactSelection selection, out Vector3d normal, out Vector3d point, |
| | | 96 | | out FixedPointAnchor coneAnchor) |
| | | 97 | | { |
| | 98 | 98 | | Vector3d radialPoint = default; |
| | | 99 | | // Ranking and rim admission use only exact gap fields. Transform |
| | | 100 | | // the retained analytic normal once; witnesses keep its local form. |
| | 98 | 101 | | Span<ulong> localNormal = stackalloc ulong[7 * Words]; |
| | 98 | 102 | | Span<int> localSigns = stackalloc int[7]; |
| | 196 | 103 | | selection.Values[..(7 * Words)].CopyTo(localNormal); selection.Signs[..7].CopyTo(localSigns); |
| | 98 | 104 | | normal = TransformAnalyticNormal(geometry, localNormal, localSigns, ref selection); |
| | 98 | 105 | | if (selection.Feature == TriangleConeContactSelection.Generator) |
| | 40 | 106 | | TriangleConeWitnesses.GetGenerator(geometry, triangle, height, radius, selection.Mask, |
| | 40 | 107 | | localNormal, localSigns, center, coneRotation, out point, out coneAnchor); |
| | 58 | 108 | | else if (selection.Feature == TriangleConeContactSelection.BasePole) |
| | | 109 | | { |
| | 24 | 110 | | point = TriangleCylinderWitnesses.GetCapPoint(geometry, triangle, selection.Mask, out radialPoint); |
| | 24 | 111 | | coneAnchor = TriangleCircularGeometry.GetSupport(center, coneRotation, Signed192.Raw(height), radialPoint, 1 |
| | | 112 | | } |
| | | 113 | | else |
| | | 114 | | { |
| | 34 | 115 | | bool apex = selection.Feature == TriangleConeContactSelection.Apex; |
| | 34 | 116 | | point = TriangleCylinderRimWitnesses.GetAnalyticPoint(geometry, triangle, selection.Mask, |
| | 34 | 117 | | localNormal[..(3 * Words)], localSigns[..3], apex ? -1 : 1, apex ? default : geometry.Radius); |
| | 34 | 118 | | if (!apex) |
| | | 119 | | { |
| | | 120 | | // Scale the exact radial direction before normalization; |
| | | 121 | | // a sub-raw full-normal component must not erase a rim. |
| | 42 | 122 | | Slot(localNormal, 1).Clear(); localSigns[1] = 0; |
| | 21 | 123 | | var radialCandidate = new ConvexContactCandidate(localNormal, localSigns, 0); |
| | 21 | 124 | | radialPoint = -WideConvexPrismRelations.GetConvexContactCandidateScaledNormal(radialCandidate, radius); |
| | | 125 | | } |
| | 34 | 126 | | coneAnchor = TriangleCircularGeometry.GetSupport(center, coneRotation, Signed192.Raw(height), radialPoint, a |
| | | 127 | | } |
| | 34 | 128 | | } |
| | | 129 | | |
| | | 130 | | private static Vector3d TransformAnalyticNormal(in TriangleCircularGeometry geometry, |
| | | 131 | | scoped ReadOnlySpan<ulong> localNormal, scoped ReadOnlySpan<int> localSigns, ref TriangleConeContactSelection se |
| | | 132 | | { |
| | 98 | 133 | | WriteWorldDirection(geometry.WorldBasis, localNormal[..(3 * Words)], localSigns[..3], |
| | 98 | 134 | | selection.Values[..(3 * Words)], selection.Signs[..3]); |
| | 98 | 135 | | WriteWorldDirection(geometry.WorldBasis, localNormal.Slice(3 * Words, 3 * Words), localSigns.Slice(3, 3), |
| | 98 | 136 | | selection.Values.Slice(3 * Words, 3 * Words), selection.Signs.Slice(3, 3)); |
| | 98 | 137 | | return WideConvexPrismRelations.GetConvexContactCandidateNormal(selection.Candidate); |
| | | 138 | | } |
| | | 139 | | |
| | | 140 | | private static bool KeepAnalytic(in TriangleCircularGeometry geometry, Fixed64 height, Fixed64 radius, |
| | | 141 | | ref TriangleConeContactSelection selection) |
| | | 142 | | { |
| | 137 | 143 | | Span<ulong> direction = stackalloc ulong[3 * Words]; |
| | 137 | 144 | | Span<int> signs = stackalloc int[3]; |
| | 137 | 145 | | WriteDirection(new WideDirection3d(default, Signed576.One, default), direction, signs); |
| | 137 | 146 | | KeepAxis(geometry, direction, signs, true, ref selection); |
| | | 147 | | // Horizontal faces duplicate the already tested +/-Up support masks |
| | | 148 | | // and gaps. Their equal candidates retain the earlier axial winner. |
| | 137 | 149 | | if (!geometry.FaceNormal.X.IsZero || !geometry.FaceNormal.Z.IsZero) |
| | | 150 | | { |
| | 102 | 151 | | WriteDirection(geometry.FaceNormal, direction, signs); |
| | 102 | 152 | | KeepAxis(geometry, direction, signs, true, ref selection); |
| | | 153 | | } |
| | 152 | 154 | | if (selection.Separated) return false; |
| | 156 | 155 | | if (HasFaceMinimumCertificate(geometry, selection.Candidate)) return true; |
| | 88 | 156 | | TriangleConeGeneratorFeatures.KeepAll(geometry, height, radius, ref selection); |
| | 664 | 157 | | for (int vertex = 0; vertex < 3 && !selection.Separated; vertex++) |
| | | 158 | | { |
| | 488 | 159 | | WideDirection3d p = geometry.Vertex(vertex), edge = geometry.Edge(vertex); |
| | 244 | 160 | | WriteDirection(new WideDirection3d(p.X, default, p.Z), direction, signs); |
| | 244 | 161 | | KeepAxis(geometry, direction, signs, false, ref selection); |
| | 244 | 162 | | WriteDirection(new WideDirection3d(WideArithmetic.Negate(edge.Z), default, edge.X), direction, signs); |
| | 244 | 163 | | KeepAxis(geometry, direction, signs, false, ref selection); |
| | 244 | 164 | | if (edge.Y.IsZero || edge.X.IsZero && edge.Z.IsZero) |
| | | 165 | | continue; |
| | 212 | 166 | | CircularRimContactAlgebra.GetBasis(edge, out WideDirection3d first, out WideDirection3d second); |
| | 424 | 167 | | WriteDirection(first, direction, signs); KeepAxis(geometry, direction, signs, false, ref selection); |
| | 424 | 168 | | WriteDirection(second, direction, signs); KeepAxis(geometry, direction, signs, false, ref selection); |
| | 212 | 169 | | ProjectPerpendicular(edge, new WideDirection3d(default, Signed576.One, default), direction, signs); |
| | 212 | 170 | | KeepAxis(geometry, direction, signs, false, ref selection); |
| | 212 | 171 | | ProjectPerpendicular(edge, geometry.CapOffset(vertex, 1), direction, signs); |
| | 212 | 172 | | KeepAxis(geometry, direction, signs, false, ref selection); |
| | | 173 | | } |
| | 88 | 174 | | return false; |
| | | 175 | | } |
| | | 176 | | |
| | | 177 | | private static bool HasFaceMinimumCertificate(in TriangleCircularGeometry geometry, ConvexContactCandidate best) |
| | | 178 | | { |
| | | 179 | | // A cone contains its full axial segment and each base-disk diameter. |
| | | 180 | | // After the nonnegative +/-face tests, a face-plane radius-d disk |
| | | 181 | | // about the segment's projection minus that segment contains ball(d). |
| | | 182 | | // The retained support attains d, proving the global minimum. |
| | 122 | 183 | | if (geometry.FaceNormal.X.IsZero && geometry.FaceNormal.Z.IsZero) |
| | 33 | 184 | | return geometry.HasFaceDiskMinimumCertificate(new WideDirection3d(default, Signed576.One, default), best); |
| | 89 | 185 | | if (!geometry.FaceNormal.Y.IsZero) |
| | 78 | 186 | | return false; |
| | 11 | 187 | | WideDirection3d axis = geometry.FaceNormal.Z.IsZero |
| | 11 | 188 | | ? new WideDirection3d(Signed576.One, default, default) |
| | 11 | 189 | | : geometry.FaceNormal.X.IsZero ? new WideDirection3d(default, default, Signed576.One) : default; |
| | 11 | 190 | | if (axis.IsZero) |
| | 3 | 191 | | return false; |
| | | 192 | | // Translate only the certificate to the base center. Original |
| | | 193 | | // geometry and selection remain authoritative for paired witnesses. |
| | | 194 | | // Canonical axes keep tangents <231 and shifted dots <431 bits, |
| | | 195 | | // within the shared certificate's established fixed-carrier bounds. |
| | 8 | 196 | | TriangleCircularGeometry baseCentered = geometry.AtCoreRegion(new WideDirection3d(default, geometry.HalfHeight, |
| | 8 | 197 | | return baseCentered.HasFaceDiskMinimumCertificate(axis, best); |
| | | 198 | | } |
| | | 199 | | |
| | | 200 | | private static void KeepAxis(in TriangleCircularGeometry geometry, scoped Span<ulong> direction, |
| | | 201 | | scoped Span<int> directionSigns, bool compactGenerator, ref TriangleConeContactSelection selection) |
| | | 202 | | { |
| | 1575 | 203 | | if (selection.Separated || directionSigns[0] == 0 && directionSigns[1] == 0 && directionSigns[2] == 0) |
| | 20 | 204 | | return; |
| | 9314 | 205 | | for (int orientation = 0; orientation < 2 && !selection.Separated; orientation++) |
| | | 206 | | { |
| | 3102 | 207 | | KeepDirection(geometry, direction, directionSigns, compactGenerator, ref selection); |
| | 34122 | 208 | | for (int component = 0; component < 3; component++) directionSigns[component] = -directionSigns[component]; |
| | | 209 | | } |
| | 1555 | 210 | | } |
| | | 211 | | |
| | | 212 | | private static void KeepDirection(in TriangleCircularGeometry geometry, scoped ReadOnlySpan<ulong> direction, |
| | | 213 | | scoped ReadOnlySpan<int> directionSigns, bool compactGenerator, ref TriangleConeContactSelection selection) |
| | | 214 | | { |
| | 3102 | 215 | | Span<ulong> values = stackalloc ulong[ConvexContactCandidate.Slots * Words]; |
| | 3102 | 216 | | Span<int> signs = stackalloc int[ConvexContactCandidate.Slots]; |
| | 3102 | 217 | | Span<ulong> work = stackalloc ulong[10 * Words]; |
| | 9306 | 218 | | Span<ulong> rational = Slot(work, 0), current = Slot(work, 1), difference = Slot(work, 2); |
| | 9306 | 219 | | Span<ulong> temporary = Slot(work, 3), radial = Slot(work, 4), radiusSquared = Slot(work, 5); |
| | 12408 | 220 | | Span<ulong> metric = Slot(work, 6), denominator = Slot(work, 7), axial = Slot(work, 8), scale = Slot(work, 9); |
| | 6204 | 221 | | values.Clear(); signs.Clear(); |
| | 6204 | 222 | | direction.CopyTo(values); directionSigns.CopyTo(signs); |
| | 3102 | 223 | | Dot(geometry.A, direction, directionSigns, rational, out int rationalSign); |
| | 3102 | 224 | | int mask = 1; |
| | 18612 | 225 | | for (int vertex = 1; vertex < 3; vertex++) |
| | | 226 | | { |
| | 6204 | 227 | | Dot(geometry.Vertex(vertex), direction, directionSigns, current, out int currentSign); |
| | 12408 | 228 | | current.CopyTo(difference); int comparison = currentSign; |
| | 6204 | 229 | | Add(rational, -rationalSign, difference, ref comparison); |
| | 10908 | 230 | | if (comparison > 0) { current.CopyTo(rational); rationalSign = currentSign; mask = 1 << vertex; } |
| | 6828 | 231 | | else if (comparison == 0) mask |= 1 << vertex; |
| | | 232 | | } |
| | 3102 | 233 | | WideArithmetic.MultiplyMagnitudes(Slot(direction, 0), Slot(direction, 0), radial); |
| | 3102 | 234 | | WideArithmetic.MultiplyMagnitudes(Slot(direction, 2), Slot(direction, 2), temporary); |
| | 3102 | 235 | | WideArithmetic.AddMagnitudeInto(temporary, radial); |
| | 3102 | 236 | | Import(geometry.Radius, current); |
| | 3102 | 237 | | WideArithmetic.MultiplyMagnitudes(current, current, radiusSquared); |
| | 3102 | 238 | | WideArithmetic.MultiplyMagnitudes(radial, radiusSquared, current); |
| | 3102 | 239 | | current.CopyTo(radiusSquared); |
| | 3102 | 240 | | Import(geometry.HalfHeight, current); |
| | 3102 | 241 | | WideArithmetic.MultiplyMagnitudes(current, Slot(direction, 1), axial); |
| | 3102 | 242 | | int side = 1; |
| | 3102 | 243 | | if (directionSigns[1] < 0) |
| | | 244 | | { |
| | 851 | 245 | | WideArithmetic.AddEqualMagnitudes(axial, axial, current); |
| | 851 | 246 | | WideArithmetic.MultiplyMagnitudes(current, current, temporary); |
| | 851 | 247 | | side = WideArithmetic.CompareMagnitudeEqualLength(radiusSquared, temporary); |
| | | 248 | | } |
| | 3102 | 249 | | SumSquares(direction, metric); |
| | | 250 | | // Larger K=0/projected chart directions defer generator equality to |
| | | 251 | | // the complete fan, without building an unused normalized denominator. |
| | 3102 | 252 | | if (side == 0 && !compactGenerator) |
| | 16 | 253 | | return; |
| | 3086 | 254 | | Import(Signed576.ExtendValue(geometry.RawScale), scale); |
| | 3086 | 255 | | WideArithmetic.MultiplyMagnitudes(scale, scale, temporary); |
| | 3086 | 256 | | WideArithmetic.MultiplyMagnitudes(metric, temporary, denominator); |
| | 3086 | 257 | | if (side == 0) |
| | | 258 | | { |
| | 5 | 259 | | TriangleConeGeneratorFeatures.Keep(geometry, values, signs, denominator, ref selection); |
| | 5 | 260 | | return; |
| | | 261 | | } |
| | 3081 | 262 | | bool apex = side < 0; |
| | 3081 | 263 | | Add(axial, directionSigns[1] * (apex ? -1 : 1), rational, ref rationalSign); |
| | 3639 | 264 | | if (apex) radiusSquared.Clear(); |
| | 3081 | 265 | | int gapSign = BuildRadialCandidate(rational, rationalSign, radiusSquared, denominator, values, signs); |
| | 3081 | 266 | | int feature = apex ? TriangleConeContactSelection.Apex |
| | 3081 | 267 | | : IsZero(radial) ? TriangleConeContactSelection.BasePole : TriangleConeContactSelection.BaseRim; |
| | 3081 | 268 | | selection.Keep(values, signs, gapSign, mask, feature); |
| | 3081 | 269 | | } |
| | | 270 | | |
| | | 271 | | } |