| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleCylinderAnalyticFeatures.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>Face, cap-pole, vertex/side and edge/side boundaries of the support fan.</summary> |
| | | 14 | | internal static class TriangleCylinderAnalyticFeatures |
| | | 15 | | { |
| | 7496 | 16 | | private static WideDirection3d Up => new(default, Signed576.One, default); |
| | | 17 | | |
| | | 18 | | internal static bool TryGetSeed(in TriangleCircularGeometry geometry, in WideDirection3d coreOffset, in WideDirectio |
| | | 19 | | Span<ulong> values, Span<int> signs, Span<ulong> direction, Span<int> directionSigns, |
| | | 20 | | out int gapSign, out int supportMask, out int coreSign, in CylinderHullSupport hull, bool includeSeam) |
| | | 21 | | { |
| | 903 | 22 | | var selection = new Selection(values, signs, direction, directionSigns); |
| | 1022 | 23 | | if (includeSeam) KeepSeam(geometry, coreAxis, ref selection, hull); |
| | 1806 | 24 | | Span<ulong> normal = stackalloc ulong[3 * Words]; Span<int> normalSigns = stackalloc int[3]; |
| | 903 | 25 | | WideDirection3d face = geometry.FaceNormal; |
| | 903 | 26 | | if (!face.IsZero && WideDirection3d.Dot(face, coreAxis).Sign == 0) |
| | | 27 | | { |
| | | 28 | | // Match the complete fan's compact seam normal, retaining its |
| | | 29 | | // witness widths even when the hull face carries unreduced scale. |
| | 370 | 30 | | face = WideDirection3d.Cross(geometry.Edge(0), coreAxis); |
| | 446 | 31 | | if (face.IsZero) face = WideDirection3d.Cross(geometry.Edge(1), coreAxis); |
| | | 32 | | } |
| | 903 | 33 | | WriteDirection(face, normal, normalSigns); |
| | 7224 | 34 | | for (int region = -1; region <= 1; region++) |
| | | 35 | | { |
| | 2709 | 36 | | var endpoint = region == 0 ? geometry : geometry.AtCoreRegion(coreOffset, region); |
| | 2709 | 37 | | KeepAxis(endpoint, coreAxis, region, normal, normalSigns, ref selection, hull); |
| | | 38 | | } |
| | 2709 | 39 | | gapSign = selection.GapSign; supportMask = selection.SupportMask; coreSign = selection.CoreSign; |
| | 904 | 40 | | if (!selection.HasValue) return false; |
| | 902 | 41 | | WriteWorldDirection(geometry.WorldBasis, direction, directionSigns, values, signs); |
| | 902 | 42 | | return gapSign >= 0; |
| | | 43 | | } |
| | | 44 | | |
| | | 45 | | internal static bool TryGetBest(in TriangleCircularGeometry geometry, in WideDirection3d coreOffset, in WideDirectio |
| | | 46 | | Span<ulong> bestValues, Span<int> bestSigns, Span<ulong> bestDirection, |
| | | 47 | | Span<int> bestDirectionSigns, out int gapSign, out int supportMask, out int coreSign, out bool faceMinimumCertif |
| | | 48 | | in CylinderHullSupport hull = default) |
| | | 49 | | { |
| | 2308 | 50 | | var selection = new Selection(bestValues, bestSigns, bestDirection, bestDirectionSigns); |
| | 2308 | 51 | | faceMinimumCertified = false; |
| | 2308 | 52 | | if (coreAxis.IsZero) |
| | 1850 | 53 | | KeepRegion(geometry, coreAxis, 0, ref selection, out faceMinimumCertified, hull); |
| | | 54 | | else |
| | | 55 | | { |
| | 458 | 56 | | KeepSeam(geometry, coreAxis, ref selection, hull); |
| | | 57 | | // The centered cylinder is contained in the stadium. Its existing |
| | | 58 | | // inscribed ball therefore remains a lower bound. An exactly |
| | | 59 | | // core-perpendicular face has the same support in both shapes; |
| | | 60 | | // Up or Up x core already attained it in the explicit seam fan. |
| | 458 | 61 | | faceMinimumCertified = hull.IsEmpty && selection.GapSign >= 0 |
| | 458 | 62 | | && WideDirection3d.Dot(geometry.FaceNormal, coreAxis).Sign == 0 |
| | 458 | 63 | | && geometry.HasFaceMinimumCertificate(); |
| | 2430 | 64 | | for (int region = -1; region <= 1 && selection.GapSign >= 0 && !faceMinimumCertified; region += 2) |
| | | 65 | | { |
| | 757 | 66 | | TriangleCircularGeometry endpoint = geometry.AtCoreRegion(coreOffset, region); |
| | 757 | 67 | | KeepRegion(endpoint, coreAxis, region, ref selection, out _, hull); |
| | | 68 | | } |
| | 458 | 69 | | if (hull.IsEmpty && !faceMinimumCertified && selection.GapSign >= 0) |
| | 75 | 70 | | faceMinimumCertified = geometry.HasFaceDiskMinimumCertificate(coreAxis, |
| | 75 | 71 | | new ConvexContactCandidate(selection.Values, selection.Signs, selection.GapSign)); |
| | | 72 | | } |
| | | 73 | | // Selection and its face certificates read gap fields only. Transform |
| | | 74 | | // the retained local direction once, after the exact winner is known. |
| | 2308 | 75 | | WriteWorldDirection(geometry.WorldBasis, selection.Direction, selection.DirectionSigns, selection.Values, select |
| | 6924 | 76 | | gapSign = selection.GapSign; supportMask = selection.SupportMask; coreSign = selection.CoreSign; |
| | 2308 | 77 | | return gapSign >= 0; |
| | | 78 | | } |
| | | 79 | | |
| | | 80 | | private static void KeepSeam(in TriangleCircularGeometry geometry, in WideDirection3d coreAxis, ref Selection select |
| | | 81 | | { |
| | 577 | 82 | | Span<ulong> direction = stackalloc ulong[3 * Words]; |
| | 577 | 83 | | Span<int> signs = stackalloc int[3]; |
| | 577 | 84 | | WriteDirection(Up, direction, signs); |
| | 577 | 85 | | KeepAxis(geometry, coreAxis, 0, direction, signs, ref selection, hull); |
| | 577 | 86 | | WriteDirection(new WideDirection3d(WideArithmetic.Negate(coreAxis.Z), default, coreAxis.X), direction, signs); |
| | 577 | 87 | | KeepAxis(geometry, coreAxis, 0, direction, signs, ref selection, hull); |
| | 4616 | 88 | | for (int edge = 0; edge < 3; edge++) |
| | | 89 | | { |
| | 1731 | 90 | | WriteDirection(WideDirection3d.Cross(geometry.Edge(edge), coreAxis), direction, signs); |
| | 1731 | 91 | | KeepAxis(geometry, coreAxis, 0, direction, signs, ref selection, hull); |
| | | 92 | | } |
| | 577 | 93 | | } |
| | | 94 | | |
| | | 95 | | private static void KeepRegion(in TriangleCircularGeometry geometry, in WideDirection3d coreAxis, int region, |
| | | 96 | | ref Selection selection, out bool faceMinimumCertified, in CylinderHullSupport hull) |
| | | 97 | | { |
| | 2607 | 98 | | Span<ulong> direction = stackalloc ulong[3 * Words]; |
| | 2607 | 99 | | Span<int> signs = stackalloc int[3]; |
| | 2607 | 100 | | WriteDirection(Up, direction, signs); |
| | 2607 | 101 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | | 102 | | // Horizontal face directions duplicate the already tested +/-Up, |
| | | 103 | | // including their support masks and first-winner tie ordering. |
| | 2607 | 104 | | if (!geometry.FaceNormal.X.IsZero || !geometry.FaceNormal.Z.IsZero) |
| | | 105 | | { |
| | 2284 | 106 | | WriteDirection(geometry.FaceNormal, direction, signs); |
| | 2284 | 107 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | | 108 | | } |
| | 2607 | 109 | | faceMinimumCertified = hull.IsEmpty && coreAxis.IsZero && selection.GapSign >= 0 && geometry.HasFaceMinimumCerti |
| | 17846 | 110 | | for (int vertex = 0; vertex < 3 && selection.GapSign >= 0 && !faceMinimumCertified; vertex++) |
| | | 111 | | { |
| | 6318 | 112 | | WideDirection3d point = geometry.Vertex(vertex); |
| | 6318 | 113 | | WriteDirection(new WideDirection3d(point.X, default, point.Z), direction, signs); |
| | 6318 | 114 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | 6318 | 115 | | WideDirection3d edge = geometry.Edge(vertex); |
| | 6318 | 116 | | bool oblique = !edge.Y.IsZero && (!edge.X.IsZero || !edge.Z.IsZero); |
| | 6318 | 117 | | if (oblique) |
| | | 118 | | { |
| | 4312 | 119 | | CircularRimContactAlgebra.GetBasis(edge, out WideDirection3d first, out WideDirection3d second); |
| | 4312 | 120 | | WriteDirection(first, direction, signs); |
| | 4312 | 121 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | 4312 | 122 | | WriteDirection(second, direction, signs); |
| | 4312 | 123 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | | 124 | | } |
| | 6318 | 125 | | WriteDirection(new WideDirection3d(WideArithmetic.Negate(edge.Z), default, edge.X), direction, signs); |
| | 6318 | 126 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | | 127 | | // Principal axes and K=0 directions are rational and must be |
| | | 128 | | // included separately from the stationary quartic's division by K. |
| | | 129 | | // Parallel/perpendicular edges project a circle/rectangle. Their |
| | | 130 | | // positive minima are the vertex-radial, pole, side or face |
| | | 131 | | // boundaries above; an admitted negative interior also has a |
| | | 132 | | // negative admitted boundary. Only oblique ellipses need extras. |
| | 6318 | 133 | | if (oblique) |
| | | 134 | | { |
| | 4312 | 135 | | ProjectPerpendicular(edge, Up, direction, signs); |
| | 4312 | 136 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | | 137 | | } |
| | 37900 | 138 | | for (int cap = -1; cap <= 1; cap += 2) |
| | | 139 | | { |
| | 12634 | 140 | | if (oblique) |
| | | 141 | | { |
| | 8622 | 142 | | ProjectPerpendicular(edge, geometry.CapOffset(vertex, cap), direction, signs); |
| | 8622 | 143 | | KeepAxis(geometry, coreAxis, region, direction, signs, ref selection, hull); |
| | | 144 | | } |
| | 12634 | 145 | | if (HasVertexRimSeparation(geometry, vertex, cap, coreAxis, region, hull)) |
| | | 146 | | { |
| | 2 | 147 | | selection.GapSign = -1; |
| | 2 | 148 | | return; |
| | | 149 | | } |
| | | 150 | | } |
| | | 151 | | } |
| | 2605 | 152 | | } |
| | | 153 | | |
| | | 154 | | private static void KeepAxis(in TriangleCircularGeometry geometry, in WideDirection3d coreAxis, int region, scoped S |
| | | 155 | | scoped Span<int> directionSigns, ref Selection selection, in CylinderHullSupport hull) |
| | | 156 | | { |
| | 44679 | 157 | | if (selection.HasValue && selection.GapSign < 0) |
| | 295 | 158 | | return; |
| | 44384 | 159 | | if (directionSigns[0] == 0 && directionSigns[1] == 0 && directionSigns[2] == 0) |
| | 596 | 160 | | return; |
| | 43788 | 161 | | Span<ulong> values = stackalloc ulong[ConvexContactCandidate.Slots * Words]; |
| | 43788 | 162 | | Span<int> signs = stackalloc int[ConvexContactCandidate.Slots]; |
| | 43788 | 163 | | Span<ulong> projection = stackalloc ulong[Words]; |
| | 43788 | 164 | | int alignment = 0; |
| | 43788 | 165 | | if (!coreAxis.IsZero) |
| | 16235 | 166 | | Dot(coreAxis, direction, directionSigns, projection, out alignment); |
| | 43788 | 167 | | bool invariantsReady = false; |
| | 262728 | 168 | | for (int orientation = 0; orientation < 2; orientation++) |
| | | 169 | | { |
| | 87576 | 170 | | if (coreAxis.IsZero || (region == 0 ? alignment == 0 : alignment * region > 0)) |
| | | 171 | | { |
| | 71397 | 172 | | if (!hull.IsEmpty) |
| | | 173 | | { |
| | 56846 | 174 | | int mask = geometry.GetSupportMask(direction, directionSigns); |
| | 56846 | 175 | | int vertex = (mask & 1) != 0 ? 0 : (mask & 2) != 0 ? 1 : 2; |
| | 56846 | 176 | | if (!hull.Supports(geometry, geometry.Vertex(vertex), direction, directionSigns)) goto Reverse; |
| | | 177 | | } |
| | 41271 | 178 | | int gapSign = BuildAxis(geometry, direction, directionSigns, values, signs, invariantsReady, out int sup |
| | 41271 | 179 | | invariantsReady = true; |
| | 41271 | 180 | | var candidate = new ConvexContactCandidate(values, signs, gapSign); |
| | 41271 | 181 | | if (!selection.HasValue || WideConvexPrismRelations.CompareConvexContactCandidates(candidate, |
| | 41271 | 182 | | new ConvexContactCandidate(selection.Values, selection.Signs, selection.GapSign)) < 0) |
| | | 183 | | { |
| | 12624 | 184 | | values.CopyTo(selection.Values); signs.CopyTo(selection.Signs); |
| | 12624 | 185 | | direction.CopyTo(selection.Direction); directionSigns.CopyTo(selection.DirectionSigns); |
| | 18936 | 186 | | selection.HasValue = true; selection.GapSign = gapSign; selection.SupportMask = supportMask; |
| | 6312 | 187 | | selection.CoreSign = region; |
| | | 188 | | } |
| | | 189 | | } |
| | | 190 | | Reverse: |
| | 700608 | 191 | | for (int component = 0; component < 3; component++) |
| | 262728 | 192 | | directionSigns[component] = -directionSigns[component]; |
| | 87576 | 193 | | alignment = -alignment; |
| | | 194 | | } |
| | 43788 | 195 | | } |
| | | 196 | | |
| | | 197 | | private static int BuildAxis(in TriangleCircularGeometry geometry, |
| | | 198 | | ReadOnlySpan<ulong> direction, ReadOnlySpan<int> directionSigns, |
| | | 199 | | Span<ulong> values, Span<int> signs, bool invariantsReady, out int mask) |
| | | 200 | | { |
| | 82542 | 201 | | values[..(9 * Words)].Clear(); signs.Clear(); |
| | 41271 | 202 | | Span<ulong> work = stackalloc ulong[8 * Words]; |
| | 123813 | 203 | | Span<ulong> rational = Slot(work, 0), current = Slot(work, 1), difference = Slot(work, 2); |
| | 123813 | 204 | | Span<ulong> temporary = Slot(work, 3), radial = Slot(work, 4), scale = Slot(work, 5); |
| | 82542 | 205 | | Span<ulong> metric = Slot(work, 6), denominator = Slot(work, 7); |
| | 41271 | 206 | | Dot(geometry.A, direction, directionSigns, rational, out int rationalSign); |
| | 41271 | 207 | | mask = 1; |
| | 247626 | 208 | | for (int vertex = 1; vertex < 3; vertex++) |
| | | 209 | | { |
| | 82542 | 210 | | Dot(geometry.Vertex(vertex), direction, directionSigns, current, out int currentSign); |
| | 82542 | 211 | | current.CopyTo(difference); |
| | 82542 | 212 | | int comparison = currentSign; |
| | 82542 | 213 | | Add(rational, -rationalSign, difference, ref comparison); |
| | 82542 | 214 | | if (comparison > 0) |
| | | 215 | | { |
| | 64938 | 216 | | current.CopyTo(rational); rationalSign = currentSign; mask = 1 << vertex; |
| | | 217 | | } |
| | 60896 | 218 | | else if (comparison == 0) |
| | 31870 | 219 | | mask |= 1 << vertex; |
| | | 220 | | } |
| | 41271 | 221 | | Import(geometry.HalfHeight, current); |
| | 41271 | 222 | | WideArithmetic.MultiplyMagnitudes(current, Slot(direction, 1), temporary); |
| | 41271 | 223 | | Add(temporary, 1, rational, ref rationalSign); |
| | | 224 | | // +/-N have identical R²|N_h|² and RawScale²|N|². Borrow the |
| | | 225 | | // working candidate's disjoint slots only after an admitted build; |
| | | 226 | | // a core region may admit the second orientation alone. |
| | 41271 | 227 | | if (invariantsReady) |
| | 7585 | 228 | | return BuildRadialCandidate(rational, rationalSign, Slot(values, 9), Slot(values, 10), values, signs); |
| | 33686 | 229 | | WideArithmetic.MultiplyMagnitudes(Slot(direction, 0), Slot(direction, 0), radial); |
| | 33686 | 230 | | WideArithmetic.MultiplyMagnitudes(Slot(direction, 2), Slot(direction, 2), temporary); |
| | 33686 | 231 | | WideArithmetic.AddMagnitudeInto(temporary, radial); |
| | 33686 | 232 | | Import(geometry.Radius, current); |
| | 33686 | 233 | | WideArithmetic.MultiplyMagnitudes(current, current, temporary); |
| | 33686 | 234 | | WideArithmetic.MultiplyMagnitudes(radial, temporary, current); |
| | 33686 | 235 | | SumSquares(direction, metric); |
| | 33686 | 236 | | Import(Signed576.ExtendValue(geometry.RawScale), scale); |
| | 33686 | 237 | | WideArithmetic.MultiplyMagnitudes(scale, scale, temporary); |
| | 33686 | 238 | | WideArithmetic.MultiplyMagnitudes(metric, temporary, denominator); |
| | 33686 | 239 | | return BuildRadialCandidate(rational, rationalSign, current, denominator, values, signs); |
| | | 240 | | } |
| | | 241 | | |
| | | 242 | | private static bool HasVertexRimSeparation(in TriangleCircularGeometry geometry, int vertex, int cap, |
| | | 243 | | in WideDirection3d coreAxis, int region, in CylinderHullSupport hull) |
| | | 244 | | { |
| | 12634 | 245 | | WideDirection3d c = geometry.CapOffset(vertex, cap); |
| | 12634 | 246 | | if (c.Y.Sign * cap > 0) |
| | 7715 | 247 | | return false; |
| | 4919 | 248 | | Signed832 radialSquared = WideArithmetic.AddSigned832( |
| | 4919 | 249 | | WideArithmetic.MultiplySigned576(c.X, c.X), WideArithmetic.MultiplySigned576(c.Z, c.Z)); |
| | 4919 | 250 | | if (WideArithmetic.SubtractSigned832(radialSquared, |
| | 4919 | 251 | | WideArithmetic.MultiplySigned576(geometry.Radius, geometry.Radius)).Sign <= 0) |
| | 802 | 252 | | return false; |
| | 4117 | 253 | | Span<ulong> work = stackalloc ulong[4 * Words]; |
| | 16468 | 254 | | Span<ulong> rational = Slot(work, 0), radical = Slot(work, 1), root = Slot(work, 2), radius = Slot(work, 3); |
| | 8234 | 255 | | Import(radialSquared, root); Import(geometry.Radius, radius); |
| | | 256 | | // The endpoint's closest-rim residual is a separator of the whole |
| | | 257 | | // stadium only in this endpoint's open core-normal hemisphere. |
| | 4117 | 258 | | if (!coreAxis.IsZero && ResidualProjectionSign(c, coreAxis, rational, radical, root, radius) * region <= 0) |
| | 1513 | 259 | | return false; |
| | 7178 | 260 | | for (int other = 0; other < 3; other++) |
| | | 261 | | { |
| | 3576 | 262 | | if (other == vertex) |
| | | 263 | | continue; |
| | 2690 | 264 | | if (ResidualProjectionSign(c, geometry.EdgeFromTo(other, vertex), rational, radical, root, radius) < 0) |
| | 2591 | 265 | | return false; |
| | | 266 | | } |
| | 60 | 267 | | for (int other = 0; other < hull.Count; other++) |
| | 28 | 268 | | if (ResidualProjectionSign(c, TriangleCircularGeometry.Subtract(geometry.Vertex(vertex), geometry.TransformH |
| | 28 | 269 | | rational, radical, root, radius) < 0) |
| | 11 | 270 | | return false; |
| | 2 | 271 | | return true; |
| | | 272 | | } |
| | | 273 | | |
| | | 274 | | private static int ResidualProjectionSign(in WideDirection3d c, in WideDirection3d axis, Span<ulong> rational, |
| | | 275 | | Span<ulong> radical, ReadOnlySpan<ulong> root, ReadOnlySpan<ulong> radius) |
| | | 276 | | { |
| | 4360 | 277 | | Signed832 planar = WideArithmetic.AddSigned832( |
| | 4360 | 278 | | WideArithmetic.MultiplySigned576(c.X, axis.X), WideArithmetic.MultiplySigned576(c.Z, axis.Z)); |
| | 4360 | 279 | | Import(planar, radical); |
| | 4360 | 280 | | WideArithmetic.MultiplyMagnitudes(radical, radius, rational); |
| | 4360 | 281 | | Signed832 full = WideDirection3d.Dot(c, axis); |
| | 4360 | 282 | | Import(full, radical); |
| | 4360 | 283 | | return WideConvexPrismRelations.GetConvexContactCandidateQuadraticSign( |
| | 4360 | 284 | | rational, planar.Sign, radical, -full.Sign, root); |
| | | 285 | | } |
| | | 286 | | |
| | | 287 | | private ref struct Selection |
| | | 288 | | { |
| | | 289 | | internal Span<ulong> Values, Direction; |
| | | 290 | | internal Span<int> Signs, DirectionSigns; |
| | | 291 | | internal int GapSign, SupportMask, CoreSign; |
| | | 292 | | internal bool HasValue; |
| | | 293 | | internal Selection(Span<ulong> values, Span<int> signs, Span<ulong> direction, Span<int> directionSigns) |
| | | 294 | | { |
| | 12844 | 295 | | Values = values; Signs = signs; Direction = direction; DirectionSigns = directionSigns; |
| | 6422 | 296 | | GapSign = SupportMask = CoreSign = 0; HasValue = false; |
| | 3211 | 297 | | } |
| | | 298 | | } |
| | | 299 | | } |