| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleCapsuleSlabWitnesses.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 | | using static FixedMathSharp.Geometry.ContactQuadratic; |
| | | 11 | | using static FixedMathSharp.Geometry.TriangleQuadraticSlice; |
| | | 12 | | |
| | | 13 | | namespace FixedMathSharp.Geometry; |
| | | 14 | | |
| | | 15 | | /// <summary>Matched witnesses on the stadium prism's free-core support seam.</summary> |
| | | 16 | | internal static class TriangleCapsuleSlabWitnesses |
| | | 17 | | { |
| | | 18 | | // Rigid seam products fit forty words (<2520 bits). Affine seam normals |
| | | 19 | | // stay below 344 bits, blended weights below 2515, and materialization |
| | | 20 | | // coefficients below 4000: eighty words retain complete products. |
| | | 21 | | // Rational cap admission squares stay below 3440 bits; quadratic sign |
| | | 22 | | // comparisons reserve their complete squared scratch separately. |
| | | 23 | | internal static void GetPoints(in TriangleCircularGeometry geometry, FixedTriangle triangle, |
| | | 24 | | int mask, ReadOnlySpan<ulong> normal, ReadOnlySpan<int> normalSigns, |
| | | 25 | | WideDirection3d coreOffset, Vector2d authoredAxis, Fixed64 coreLength, ref Vector3d radialPoint, |
| | | 26 | | out Vector3d trianglePoint, out Vector3d coreAndAxialPoint, out FixedPointAnchorTerm3d exactTerm) |
| | | 27 | | { |
| | 100 | 28 | | bool admitted = TryGetPoints(geometry, triangle, mask, normal, normalSigns, coreOffset, authoredAxis, coreLength |
| | 100 | 29 | | ref radialPoint, out trianglePoint, out coreAndAxialPoint, out exactTerm); |
| | | 30 | | System.Diagnostics.Debug.Assert(admitted); |
| | 100 | 31 | | } |
| | | 32 | | |
| | | 33 | | internal static bool TryGetPoints(in TriangleCircularGeometry geometry, FixedTriangle triangle, |
| | | 34 | | int mask, ReadOnlySpan<ulong> normal, ReadOnlySpan<int> normalSigns, |
| | | 35 | | WideDirection3d coreOffset, Vector2d authoredAxis, Fixed64 coreLength, ref Vector3d radialPoint, |
| | | 36 | | out Vector3d trianglePoint, out Vector3d coreAndAxialPoint, out FixedPointAnchorTerm3d exactTerm) |
| | | 37 | | { |
| | 204 | 38 | | int Words = geometry.ParameterWords; |
| | 408 | 39 | | trianglePoint = coreAndAxialPoint = default; exactTerm = default; |
| | 204 | 40 | | WideDirection3d axis = new(Signed576.ExtendValue(Signed192.Raw(authoredAxis.X)), default, |
| | 204 | 41 | | Signed576.ExtendValue(Signed192.Raw(authoredAxis.Y))); |
| | 204 | 42 | | Signed832 bound = WideDirection3d.Dot(coreOffset, axis); |
| | 204 | 43 | | Span<Signed832> projections = stackalloc Signed832[3]; |
| | 1632 | 44 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 612 | 45 | | projections[vertex] = WideDirection3d.Dot(geometry.Vertex(vertex), axis); |
| | 204 | 46 | | Span<ulong> weights = stackalloc ulong[6 * Words]; |
| | 204 | 47 | | Span<int> weightSigns = stackalloc int[6]; |
| | 204 | 48 | | Span<ulong> root = stackalloc ulong[Words]; |
| | 612 | 49 | | weights.Clear(); weightSigns.Clear(); root.Clear(); |
| | 204 | 50 | | bool pole = normalSigns[0] == 0 && normalSigns[2] == 0; |
| | 204 | 51 | | int cap = normalSigns[1]; |
| | 204 | 52 | | WideDirection3d compact = new(ReadComponent(normal, normalSigns, 0), |
| | 204 | 53 | | ReadComponent(normal, normalSigns, 1), ReadComponent(normal, normalSigns, 2)); |
| | 204 | 54 | | Signed832 metric = WideArithmetic.AddSigned832(WideArithmetic.MultiplySigned576(compact.X, compact.X), |
| | 204 | 55 | | WideArithmetic.MultiplySigned576(compact.Z, compact.Z)); |
| | 204 | 56 | | Import(metric, root); |
| | 204 | 57 | | bool admitted = pole |
| | 204 | 58 | | ? TryGetCapWeights(geometry, mask, axis, coreOffset, projections, bound, weights, weightSigns) |
| | 204 | 59 | | : cap == 0 ? TryGetSideWeights(geometry, mask, projections, bound, weights, weightSigns) |
| | 204 | 60 | | : TryGetObliqueWeights(geometry, mask, axis, compact, cap, projections, bound, root, weights, weightSigns); |
| | 210 | 61 | | if (!admitted) return false; |
| | | 62 | | |
| | 198 | 63 | | GetMaterials(geometry, triangle, axis, authoredAxis, coreLength, compact, metric, projections, bound, |
| | 198 | 64 | | cap, pole, root, weights, weightSigns, ref radialPoint, out trianglePoint, out coreAndAxialPoint, out exactT |
| | 198 | 65 | | return true; |
| | | 66 | | } |
| | | 67 | | |
| | | 68 | | private static bool TryGetSideWeights(in TriangleCircularGeometry geometry, int mask, |
| | | 69 | | ReadOnlySpan<Signed832> projections, Signed832 coreBound, |
| | | 70 | | Span<ulong> weights, Span<int> weightSigns) |
| | | 71 | | { |
| | 126 | 72 | | int Words = geometry.ParameterWords; |
| | 126 | 73 | | Span<Signed832> axial = stackalloc Signed832[3]; |
| | 126 | 74 | | Signed832 half = Signed832.ExtendValue(geometry.HalfHeight); |
| | 606 | 75 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 76 | | { |
| | 244 | 77 | | axial[vertex] = Signed832.ExtendValue(geometry.Vertex(vertex).Y); |
| | 244 | 78 | | if ((mask & (1 << vertex)) != 0 && InInterval(projections[vertex], coreBound) |
| | 244 | 79 | | && InInterval(axial[vertex], half)) |
| | | 80 | | { |
| | 67 | 81 | | At(weights, weightSigns, vertex, weights.Length / weightSigns.Length).Set(Signed832.ExtendValue(Signed57 |
| | 67 | 82 | | return true; |
| | | 83 | | } |
| | | 84 | | } |
| | | 85 | | // After excluding admitted vertices, test both axial boundaries. |
| | | 86 | | // If neither meets the feature, a nonempty intersection must meet |
| | | 87 | | // the negative core boundary: one confined to the positive boundary |
| | | 88 | | // would end at an admitted vertex or an already-tested axial side. |
| | 59 | 89 | | Span<ulong> distances = stackalloc ulong[6 * Words]; |
| | 59 | 90 | | Span<int> signs = stackalloc int[6]; |
| | 118 | 91 | | Span<ulong> root = stackalloc ulong[Words]; root.Clear(); |
| | 136 | 92 | | for (int side = -1; side <= 1; side += 2) |
| | | 93 | | { |
| | 512 | 94 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 192 | 95 | | At(distances, signs, vertex, distances.Length / signs.Length).Set(WideArithmetic.SubtractSigned832(axial |
| | 64 | 96 | | if (TrySlice(mask, distances, signs, projections, coreBound, root, weights, weightSigns)) |
| | 55 | 97 | | return true; |
| | | 98 | | } |
| | 32 | 99 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 12 | 100 | | At(distances, signs, vertex, distances.Length / signs.Length).Set(WideArithmetic.AddSigned832(projections[ve |
| | 4 | 101 | | return TrySlice(mask, distances, signs, axial, half, root, weights, weightSigns); |
| | | 102 | | } |
| | | 103 | | |
| | | 104 | | private static bool TryGetCapWeights(in TriangleCircularGeometry geometry, int mask, WideDirection3d axis, |
| | | 105 | | WideDirection3d coreOffset, ReadOnlySpan<Signed832> projections, Signed832 bound, |
| | | 106 | | Span<ulong> weights, Span<int> weightSigns) |
| | | 107 | | { |
| | 67 | 108 | | int Words = geometry.ParameterWords; |
| | 67 | 109 | | Span<ulong> distances = stackalloc ulong[6 * Words]; |
| | 67 | 110 | | Span<int> signs = stackalloc int[6]; |
| | 134 | 111 | | Span<ulong> root = stackalloc ulong[Words]; root.Clear(); |
| | 67 | 112 | | WideDirection3d transverse = new(WideArithmetic.Negate(axis.Z), default, axis.X); |
| | 536 | 113 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 201 | 114 | | At(distances, signs, vertex, distances.Length / signs.Length).Set(WideDirection3d.Dot(geometry.Vertex(vertex |
| | 67 | 115 | | if (TrySlice(mask, distances, signs, projections, bound, root, weights, weightSigns)) |
| | 57 | 116 | | return true; |
| | | 117 | | |
| | | 118 | | // With no core/feature intersection, the closest planar pair has a |
| | | 119 | | // triangle vertex or a core endpoint. These are witness constructions, |
| | | 120 | | // not an endpoint-cylinder union used to classify the stadium. |
| | 56 | 121 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 122 | | { |
| | 22 | 123 | | if ((mask & (1 << vertex)) == 0) |
| | | 124 | | continue; |
| | 44 | 125 | | weights.Clear(); weightSigns.Clear(); |
| | 22 | 126 | | At(weights, weightSigns, vertex, weights.Length / weightSigns.Length).Set(Signed832.ExtendValue(Signed576.On |
| | 22 | 127 | | if (WithinRadius(geometry, axis, projections, bound, weights, weightSigns)) |
| | 4 | 128 | | return true; |
| | | 129 | | } |
| | 6 | 130 | | GetEndpointCapWeights(geometry, mask, coreOffset, -1, weights, weightSigns); |
| | 6 | 131 | | if (WithinRadius(geometry, axis, projections, bound, weights, weightSigns)) |
| | 3 | 132 | | return true; |
| | | 133 | | // The preceding exhaustive closest-feature cases have failed; the |
| | | 134 | | // other core endpoint must supply the admitted cap witness. |
| | 3 | 135 | | GetEndpointCapWeights(geometry, mask, coreOffset, 1, weights, weightSigns); |
| | 3 | 136 | | return WithinRadius(geometry, axis, projections, bound, weights, weightSigns); |
| | | 137 | | } |
| | | 138 | | |
| | | 139 | | private static void GetEndpointCapWeights(in TriangleCircularGeometry geometry, int mask, WideDirection3d coreOffset |
| | | 140 | | int side, Span<ulong> weights, Span<int> weightSigns) |
| | | 141 | | { |
| | 9 | 142 | | int Words = geometry.ParameterWords; |
| | 9 | 143 | | Span<ulong> rationalWeights = stackalloc ulong[3 * CylinderContactAlgebra.Words]; |
| | 9 | 144 | | TriangleCircularGeometry endpoint = geometry.AtCoreRegion(coreOffset, side); |
| | 9 | 145 | | TriangleCylinderWitnesses.GetCapWeights(endpoint, mask, rationalWeights); |
| | 18 | 146 | | weights.Clear(); weightSigns.Clear(); |
| | 72 | 147 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 148 | | { |
| | 27 | 149 | | Slot(rationalWeights, vertex).CopyTo(At(weights, weightSigns, vertex, weights.Length / weightSigns.Length).R |
| | 27 | 150 | | At(weights, weightSigns, vertex, weights.Length / weightSigns.Length).Signs[0] = IsZero(Slot(rationalWeights |
| | | 151 | | } |
| | 9 | 152 | | } |
| | | 153 | | |
| | | 154 | | private static bool TryGetObliqueWeights(in TriangleCircularGeometry geometry, int mask, WideDirection3d axis, |
| | | 155 | | WideDirection3d compact, int cap, |
| | | 156 | | ReadOnlySpan<Signed832> projections, Signed832 bound, ReadOnlySpan<ulong> root, |
| | | 157 | | Span<ulong> weights, Span<int> weightSigns) |
| | | 158 | | { |
| | 11 | 159 | | int Words = geometry.ParameterWords; |
| | | 160 | | // Seam ownership supplies Up, its planar perpendicular, or one |
| | | 161 | | // retained edge crossed with the raw authored axis (<230 bits). |
| | 11 | 162 | | WideDirection3d tangent = WideDirection3d.Cross(axis, compact); |
| | | 163 | | |
| | 11 | 164 | | Span<ulong> distances = stackalloc ulong[6 * Words]; |
| | 11 | 165 | | Span<int> signs = stackalloc int[6]; |
| | 11 | 166 | | Span<ulong> scratch = stackalloc ulong[4 * Words]; |
| | 11 | 167 | | Span<int> scratchSigns = stackalloc int[4]; |
| | 22 | 168 | | ContactQuadratic radical = At(scratch, scratchSigns, 0, scratch.Length / scratchSigns.Length), factor = At(scrat |
| | | 169 | | // sqrt(K)*tangent·(v-p) = |
| | | 170 | | // (tangent·v + cap*H*tangent.Y)*sqrt(K) - R*n.Y*tangent.Y. |
| | 11 | 171 | | radical.Set(WideArithmetic.MultiplySigned576(compact.Y, tangent.Y)); |
| | 11 | 172 | | Scale(radical, Signed832.ExtendValue(geometry.Radius), factor); |
| | 88 | 173 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 174 | | { |
| | 33 | 175 | | ContactQuadratic distance = At(distances, signs, vertex, distances.Length / signs.Length); |
| | 33 | 176 | | factor.CopyTo(distance, -1); |
| | 33 | 177 | | Signed832 axial = WideArithmetic.MultiplySigned576(geometry.HalfHeight, tangent.Y); |
| | 33 | 178 | | Signed832 value = WideArithmetic.AddSigned832(WideDirection3d.Dot(geometry.Vertex(vertex), tangent), |
| | 33 | 179 | | cap > 0 ? axial : Negate(axial)); |
| | 66 | 180 | | Import(value, distance.Radical); distance.Signs[1] = value.Sign; |
| | | 181 | | } |
| | 11 | 182 | | return TrySlice(mask, distances, signs, projections, bound, root, weights, weightSigns); |
| | | 183 | | } |
| | | 184 | | |
| | | 185 | | private static bool TrySlice(int mask, Span<ulong> distances, Span<int> distanceSigns, |
| | | 186 | | ReadOnlySpan<Signed832> projections, Signed832 bound, ReadOnlySpan<ulong> root, |
| | | 187 | | Span<ulong> weights, Span<int> weightSigns) |
| | | 188 | | { |
| | 146 | 189 | | int Words = weights.Length / weightSigns.Length; |
| | 146 | 190 | | Span<ulong> values = stackalloc ulong[8 * Words]; |
| | 146 | 191 | | Span<int> signs = stackalloc int[8]; |
| | 1168 | 192 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 438 | 193 | | At(values, signs, vertex, values.Length / signs.Length).Set(projections[vertex]); |
| | 146 | 194 | | At(values, signs, 3, values.Length / signs.Length).Set(bound); |
| | 146 | 195 | | return TriangleQuadraticSlice.TryGetWeights(mask, distances, distanceSigns, |
| | 146 | 196 | | values[..(6 * Words)], signs[..6], At(values, signs, 3, values.Length / signs.Length), root, |
| | 146 | 197 | | weights, weightSigns, out _); |
| | | 198 | | } |
| | | 199 | | |
| | | 200 | | private static bool WithinRadius(in TriangleCircularGeometry geometry, WideDirection3d axis, |
| | | 201 | | ReadOnlySpan<Signed832> projections, Signed832 bound, Span<ulong> weights, Span<int> signs) |
| | | 202 | | { |
| | 31 | 203 | | int Words = geometry.ParameterWords; |
| | 31 | 204 | | Span<ulong> values = stackalloc ulong[16 * Words]; |
| | 31 | 205 | | Span<int> valueSigns = stackalloc int[16]; |
| | 62 | 206 | | Span<ulong> root = stackalloc ulong[Words]; root.Clear(); |
| | 62 | 207 | | ContactQuadratic denominator = At(values, valueSigns, 0, values.Length / valueSigns.Length), projection = At(val |
| | 62 | 208 | | ContactQuadratic coordinate = At(values, valueSigns, 2, values.Length / valueSigns.Length), scaled = At(values, |
| | 62 | 209 | | ContactQuadratic difference = At(values, valueSigns, 4, values.Length / valueSigns.Length), squared = At(values, |
| | 62 | 210 | | ContactQuadratic sum = At(values, valueSigns, 6, values.Length / valueSigns.Length), limit = At(values, valueSig |
| | 31 | 211 | | SumWeights(weights, signs, denominator); |
| | 31 | 212 | | Sum(projections, weights, signs, projection); |
| | 31 | 213 | | ClampProjection(denominator, projection, bound, root); |
| | 31 | 214 | | Signed832 axisSquared = axis.SquaredLength; |
| | 31 | 215 | | sum.Clear(); |
| | 186 | 216 | | for (int component = 0; component < 3; component += 2) |
| | | 217 | | { |
| | 62 | 218 | | SumCoordinate(geometry, weights, signs, component, coordinate); |
| | 62 | 219 | | Scale(coordinate, axisSquared, difference); |
| | 62 | 220 | | Scale(projection, Signed832.ExtendValue(Component(axis, component)), scaled); |
| | 62 | 221 | | difference.Add(scaled, -1); |
| | 62 | 222 | | Multiply(difference, difference, root, squared); |
| | 62 | 223 | | sum.Add(squared); |
| | | 224 | | } |
| | 31 | 225 | | Signed832 radiusAxis = WideArithmetic.MultiplySigned576(Signed576.NarrowValue(axisSquared), geometry.Radius); |
| | 31 | 226 | | Scale(denominator, radiusAxis, limit); |
| | 31 | 227 | | Multiply(limit, limit, root, squared); |
| | 31 | 228 | | sum.Add(squared, -1); |
| | 31 | 229 | | return sum.Sign(root) <= 0; |
| | | 230 | | } |
| | | 231 | | |
| | | 232 | | private static void GetMaterials(in TriangleCircularGeometry geometry, FixedTriangle triangle, WideDirection3d axis, |
| | | 233 | | Vector2d authoredAxis, Fixed64 coreLength, |
| | | 234 | | WideDirection3d normal, Signed832 metric, ReadOnlySpan<Signed832> projections, Signed832 bound, |
| | | 235 | | int cap, bool pole, ReadOnlySpan<ulong> root, Span<ulong> weights, Span<int> signs, |
| | | 236 | | ref Vector3d radialPoint, out Vector3d trianglePoint, out Vector3d coreAndAxialPoint, |
| | | 237 | | out FixedPointAnchorTerm3d exactTerm) |
| | | 238 | | { |
| | 198 | 239 | | int Words = geometry.ParameterWords; |
| | 198 | 240 | | Span<ulong> values = stackalloc ulong[16 * Words]; |
| | 198 | 241 | | Span<int> valueSigns = stackalloc int[16]; |
| | 396 | 242 | | ContactQuadratic denominator = At(values, valueSigns, 0, values.Length / valueSigns.Length), projection = At(val |
| | 396 | 243 | | ContactQuadratic coordinate = At(values, valueSigns, 2, values.Length / valueSigns.Length), coreNumerator = At(v |
| | 396 | 244 | | ContactQuadratic coreDenominator = At(values, valueSigns, 4, values.Length / valueSigns.Length), product = At(va |
| | 396 | 245 | | ContactQuadratic combinedNumerator = At(values, valueSigns, 6, values.Length / valueSigns.Length), combinedDenom |
| | 198 | 246 | | SumWeights(weights, signs, denominator); |
| | 198 | 247 | | Sum(projections, weights, signs, projection); |
| | 198 | 248 | | if (pole) |
| | 65 | 249 | | ClampProjection(denominator, projection, bound, root); |
| | 198 | 250 | | int endpointSign = projection.Sign(root); |
| | 198 | 251 | | Scale(denominator, bound, coordinate); |
| | 396 | 252 | | coordinate.MultiplySign(endpointSign); coordinate.Add(projection, -1); |
| | 198 | 253 | | if (coordinate.Sign(root) != 0) |
| | 71 | 254 | | endpointSign = 0; |
| | 198 | 255 | | Vector3d endpointCore = endpointSign == 0 ? default |
| | 198 | 256 | | : TriangleCapsuleSlabEndpointWitnesses.GetCore(authoredAxis, coreLength, -endpointSign, out _); |
| | 198 | 257 | | int integralMask = 0; |
| | 198 | 258 | | trianglePoint = new Vector3d( |
| | 198 | 259 | | RoundTriangleCoordinate(geometry, triangle, 0, weights, signs, denominator, root), |
| | 198 | 260 | | RoundTriangleCoordinate(geometry, triangle, 1, weights, signs, denominator, root), |
| | 198 | 261 | | RoundTriangleCoordinate(geometry, triangle, 2, weights, signs, denominator, root)); |
| | | 262 | | |
| | 198 | 263 | | Signed832 axisSquared = axis.SquaredLength; |
| | 198 | 264 | | Signed832 scale = WideArithmetic.MultiplySigned832(axisSquared, geometry.RawScale); |
| | 198 | 265 | | Scale(denominator, scale, coreDenominator); |
| | 198 | 266 | | Vector3d core = default; |
| | 198 | 267 | | radialPoint = default; |
| | 1188 | 268 | | for (int component = 0; component < 3; component += 2) |
| | | 269 | | { |
| | 396 | 270 | | Scale(projection, Signed832.ExtendValue(Component(axis, component)), coreNumerator); |
| | | 271 | | int radialSign; |
| | 396 | 272 | | if (pole) |
| | | 273 | | { |
| | | 274 | | // The cap's combined planar point is q itself, not two |
| | | 275 | | // independently rounded core/projection components. |
| | 130 | 276 | | SumCoordinate(geometry, weights, signs, component, coordinate); |
| | 130 | 277 | | Scale(coordinate, axisSquared, combinedNumerator); |
| | 130 | 278 | | coreDenominator.CopyTo(combinedDenominator); |
| | 260 | 279 | | combinedNumerator.CopyTo(product); product.Add(coreNumerator, -1); |
| | 130 | 280 | | radialSign = product.Sign(root); |
| | | 281 | | } |
| | | 282 | | else |
| | | 283 | | { |
| | | 284 | | // K*C - R*n_i*sumWeights*a²*sqrt(K), over K times |
| | | 285 | | // C's denominator, is the exact combined support coordinate. |
| | 266 | 286 | | Scale(coreNumerator, metric, combinedNumerator); |
| | 266 | 287 | | Scale(coreDenominator, metric, combinedDenominator); |
| | 266 | 288 | | Scale(denominator, axisSquared, product); |
| | 266 | 289 | | Signed832 radialFactor = WideArithmetic.MultiplySigned576(geometry.Radius, Component(normal, component)) |
| | 266 | 290 | | Scale(product, radialFactor, coordinate); |
| | 266 | 291 | | MultiplyRoot(coordinate, root, product); |
| | 266 | 292 | | combinedNumerator.Add(product, -1); |
| | 266 | 293 | | radialSign = -Component(normal, component).Sign * geometry.Radius.Sign; |
| | | 294 | | } |
| | 396 | 295 | | if (endpointSign != 0) |
| | | 296 | | { |
| | | 297 | | // An integral radial offset leaves the exact endpoint's |
| | | 298 | | // half-core residual meaningful. Test the bounded radial |
| | | 299 | | // value itself, never rounded(P)-rounded(C), which can |
| | | 300 | | // differ by one at a half tie or exceed the scalar domain. |
| | 154 | 301 | | if (!pole) |
| | 138 | 302 | | product.MultiplySign(-1); |
| | 154 | 303 | | Fixed64 radial = RoundRatio(product, combinedDenominator, root); |
| | 154 | 304 | | Scale(combinedDenominator, Signed832.ExtendValue(Signed576.ExtendValue(Signed192.Raw(radial))), coordina |
| | 154 | 305 | | coordinate.Add(product, -1); |
| | 154 | 306 | | if (coordinate.Sign(root) == 0) |
| | | 307 | | { |
| | 303 | 308 | | if (component == 0) { core.X = endpointCore.X; radialPoint.X = radial; } |
| | 150 | 309 | | else { core.Z = endpointCore.Z; radialPoint.Z = radial; } |
| | 151 | 310 | | integralMask |= 1 << component; |
| | 151 | 311 | | continue; |
| | | 312 | | } |
| | | 313 | | } |
| | | 314 | | // An integer base on the radial side of C keeps P-base within |
| | | 315 | | // [-Max,Max], even when P itself cannot fit a scalar. Its parity |
| | | 316 | | // participates in the residual tie, preserving one rounding of P. |
| | 245 | 317 | | Fixed64 presentation = GetCorePresentation(coreNumerator, coreDenominator, radialSign, root); |
| | 245 | 318 | | Scale(combinedDenominator, Signed832.ExtendValue(Signed576.ExtendValue(Signed192.Raw(presentation))), produc |
| | 245 | 319 | | combinedNumerator.Add(product, -1); |
| | 245 | 320 | | Fixed64 residual = RoundRatio(combinedNumerator, combinedDenominator, root, |
| | 245 | 321 | | parityOffset: presentation.m_rawValue); |
| | 489 | 322 | | if (component == 0) { core.X = presentation; radialPoint.X = residual; } |
| | 246 | 323 | | else { core.Z = presentation; radialPoint.Z = residual; } |
| | | 324 | | } |
| | 198 | 325 | | Scale(denominator, Signed832.ExtendValue(Signed576.ExtendValue(geometry.RawScale)), coreDenominator); |
| | 198 | 326 | | if (cap == 0) |
| | 124 | 327 | | SumCoordinate(geometry, weights, signs, 1, coreNumerator); |
| | | 328 | | else |
| | 74 | 329 | | Scale(denominator, Signed832.ExtendValue(cap > 0 ? WideArithmetic.Negate(geometry.HalfHeight) : geometry.Hal |
| | 198 | 330 | | core.Y = RoundRatio(coreNumerator, coreDenominator, root); |
| | 198 | 331 | | coreAndAxialPoint = core; |
| | 198 | 332 | | exactTerm = integralMask == 0 ? default : TriangleCapsuleSlabEndpointWitnesses.CreateTerm( |
| | 198 | 333 | | authoredAxis, endpointSign > 0 ? coreLength : -coreLength, core, integralMask); |
| | 198 | 334 | | } |
| | | 335 | | |
| | | 336 | | private static Fixed64 GetCorePresentation(ContactQuadratic numerator, ContactQuadratic denominator, int radialSign, |
| | | 337 | | { |
| | 245 | 338 | | int Words = denominator.FieldWords; |
| | 245 | 339 | | Fixed64 rounded = RoundRatio(numerator, denominator, root); |
| | 245 | 340 | | Span<ulong> values = stackalloc ulong[2 * Words]; |
| | 245 | 341 | | Span<int> signs = stackalloc int[2]; |
| | 245 | 342 | | ContactQuadratic difference = At(values, signs, 0, values.Length / signs.Length); |
| | 245 | 343 | | Scale(denominator, Signed832.ExtendValue(Signed576.ExtendValue(Signed192.Raw(rounded))), difference); |
| | 490 | 344 | | difference.MultiplySign(-1); difference.Add(numerator); |
| | 245 | 345 | | int comparison = difference.Sign(root); |
| | 245 | 346 | | if (radialSign >= 0 && comparison > 0) |
| | 20 | 347 | | return Fixed64.FromRaw(rounded.m_rawValue + 1); |
| | 225 | 348 | | if (radialSign < 0 && comparison < 0) |
| | 27 | 349 | | return Fixed64.FromRaw(rounded.m_rawValue - 1); |
| | 198 | 350 | | return rounded; |
| | | 351 | | } |
| | | 352 | | |
| | | 353 | | private static void ClampProjection(ContactQuadratic denominator, ContactQuadratic projection, Signed832 bound, Read |
| | | 354 | | { |
| | 96 | 355 | | int Words = denominator.FieldWords; |
| | 96 | 356 | | Span<ulong> values = stackalloc ulong[4 * Words]; |
| | 96 | 357 | | Span<int> signs = stackalloc int[4]; |
| | 192 | 358 | | ContactQuadratic limit = At(values, signs, 0, values.Length / signs.Length), difference = At(values, signs, 1, v |
| | 96 | 359 | | Scale(denominator, bound, limit); |
| | 192 | 360 | | projection.CopyTo(difference); difference.Add(limit, -1); |
| | 96 | 361 | | if (difference.Sign(root) > 0) |
| | 23 | 362 | | limit.CopyTo(projection); |
| | | 363 | | else |
| | | 364 | | { |
| | 146 | 365 | | projection.CopyTo(difference); difference.Add(limit); |
| | 73 | 366 | | if (difference.Sign(root) < 0) |
| | 11 | 367 | | limit.CopyTo(projection, -1); |
| | | 368 | | } |
| | 73 | 369 | | } |
| | | 370 | | |
| | | 371 | | private static void SumCoordinate(in TriangleCircularGeometry geometry, Span<ulong> weights, |
| | | 372 | | Span<int> signs, int component, ContactQuadratic result) |
| | | 373 | | { |
| | 316 | 374 | | Span<Signed832> coordinates = stackalloc Signed832[3]; |
| | 2528 | 375 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 948 | 376 | | coordinates[vertex] = Signed832.ExtendValue(Component(geometry.Vertex(vertex), component)); |
| | 316 | 377 | | Sum(coordinates, weights, signs, result); |
| | 316 | 378 | | } |
| | | 379 | | |
| | | 380 | | private static bool InInterval(Signed832 value, Signed832 bound) => |
| | 363 | 381 | | WideArithmetic.SubtractSigned832(value, bound).Sign <= 0 |
| | 363 | 382 | | && WideArithmetic.AddSigned832(value, bound).Sign >= 0; |
| | 201 | 383 | | private static Signed832 Negate(Signed832 value) => WideArithmetic.SubtractSigned832(default, value); |
| | | 384 | | private static Signed576 ReadComponent(ReadOnlySpan<ulong> values, ReadOnlySpan<int> signs, int component) |
| | | 385 | | { |
| | 612 | 386 | | ReadOnlySpan<ulong> value = Slot(values, component); |
| | 612 | 387 | | Signed576 result = new(value[8], value[7], value[6], value[5], value[4], value[3], value[2], value[1], value[0]) |
| | 612 | 388 | | return signs[component] < 0 ? WideArithmetic.Negate(result) : result; |
| | | 389 | | } |
| | | 390 | | } |