| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleQuadraticSlice.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 | | |
| | | 12 | | namespace FixedMathSharp.Geometry; |
| | | 13 | | |
| | | 14 | | /// <summary>Exact triangle-feature plane slicing and closed quadratic interval admission.</summary> |
| | | 15 | | internal static class TriangleQuadraticSlice |
| | | 16 | | { |
| | | 17 | | /// <summary> |
| | | 18 | | /// Slices the selected feature by one exact plane, then admits its free |
| | | 19 | | /// interval coordinate. Quadratic weights retain A+B*sqrt(K) throughout; |
| | | 20 | | /// no intersection parameter or support point rounds before projection. |
| | | 21 | | /// </summary> |
| | | 22 | | internal static bool TryGetWeights(int mask, Span<ulong> distances, Span<int> distanceSigns, |
| | | 23 | | Span<ulong> projections, Span<int> projectionSigns, ContactQuadratic bound, ReadOnlySpan<ulong> root, |
| | | 24 | | Span<ulong> weights, Span<int> weightSigns, out bool centered) |
| | | 25 | | { |
| | 186 | 26 | | int Words = bound.FieldWords; |
| | 186 | 27 | | centered = false; |
| | 186 | 28 | | Span<int> planeSigns = stackalloc int[3]; |
| | 186 | 29 | | Span<ulong> candidates = stackalloc ulong[12 * Words]; |
| | 186 | 30 | | Span<int> candidateSigns = stackalloc int[12]; |
| | 372 | 31 | | candidates.Clear(); candidateSigns.Clear(); |
| | 186 | 32 | | int count = 0; |
| | 1488 | 33 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 34 | | { |
| | 558 | 35 | | planeSigns[vertex] = At(distances, distanceSigns, vertex, distances.Length / distanceSigns.Length).Sign(root |
| | 558 | 36 | | if ((mask & (1 << vertex)) != 0 && planeSigns[vertex] == 0) |
| | 25 | 37 | | At(candidates, candidateSigns, 3 * count++ + vertex, candidates.Length / candidateSigns.Length).Set(Sign |
| | | 38 | | } |
| | 1488 | 39 | | for (int edge = 0; edge < 3; edge++) |
| | | 40 | | { |
| | 558 | 41 | | int next = (edge + 1) % 3; |
| | 558 | 42 | | if ((mask & (1 << edge)) == 0 || (mask & (1 << next)) == 0 |
| | 558 | 43 | | || planeSigns[edge] * planeSigns[next] >= 0) |
| | | 44 | | continue; |
| | 219 | 45 | | At(distances, distanceSigns, next, distances.Length / distanceSigns.Length).CopyTo(At(candidates, candidateS |
| | 219 | 46 | | At(distances, distanceSigns, edge, distances.Length / distanceSigns.Length).CopyTo(At(candidates, candidateS |
| | 219 | 47 | | count++; |
| | | 48 | | } |
| | | 49 | | System.Diagnostics.Debug.Assert(count <= 2); |
| | 186 | 50 | | Span<ulong> values = stackalloc ulong[12 * Words]; |
| | 186 | 51 | | Span<int> valueSigns = stackalloc int[12]; |
| | 372 | 52 | | ContactQuadratic denominator = At(values, valueSigns, 0, values.Length / valueSigns.Length), limit = At(values, |
| | 558 | 53 | | ContactQuadratic comparison = At(values, valueSigns, 2, values.Length / valueSigns.Length), first = At(values, v |
| | 186 | 54 | | Span<int> endpointSigns = stackalloc int[2]; |
| | 522 | 55 | | for (int index = 0; index < count; index++) |
| | | 56 | | { |
| | 231 | 57 | | Span<ulong> candidate = candidates.Slice(index * 6 * Words, 6 * Words); |
| | 231 | 58 | | Span<int> signs = candidateSigns.Slice(index * 6, 6); |
| | 231 | 59 | | SumWeights(candidate, signs, denominator); |
| | 231 | 60 | | ContactQuadratic projection = At(values, valueSigns, 4 + index, values.Length / valueSigns.Length); |
| | 231 | 61 | | SumWeighted(projections, projectionSigns, candidate, signs, root, projection); |
| | 231 | 62 | | endpointSigns[index] = projection.Sign(root); |
| | 231 | 63 | | Multiply(denominator, bound, root, limit); |
| | 462 | 64 | | projection.CopyTo(comparison); comparison.Add(limit, -1); |
| | 231 | 65 | | if (comparison.Sign(root) > 0) |
| | | 66 | | continue; |
| | 368 | 67 | | projection.CopyTo(comparison); comparison.Add(limit); |
| | 184 | 68 | | if (comparison.Sign(root) < 0) |
| | | 69 | | continue; |
| | 312 | 70 | | candidate.CopyTo(weights); signs.CopyTo(weightSigns); |
| | 156 | 71 | | return true; |
| | | 72 | | } |
| | 30 | 73 | | if (count != 2 || endpointSigns[0] * endpointSigns[1] >= 0) |
| | 23 | 74 | | return false; |
| | | 75 | | // Neither endpoint is admitted, but they straddle the interval. A |
| | | 76 | | // direct blend at coordinate zero keeps one quadratic field and |
| | | 77 | | // avoids nested rounded ratios (or a growing clipping polygon). |
| | 7 | 78 | | centered = true; |
| | 7 | 79 | | ContactQuadratic product = At(values, valueSigns, 3, values.Length / valueSigns.Length); |
| | 56 | 80 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 81 | | { |
| | 21 | 82 | | ContactQuadratic result = At(weights, weightSigns, vertex, weights.Length / weightSigns.Length); |
| | 21 | 83 | | Multiply(At(candidates, candidateSigns, vertex, candidates.Length / candidateSigns.Length), second, root, re |
| | 21 | 84 | | result.MultiplySign(endpointSigns[1]); |
| | 21 | 85 | | Multiply(At(candidates, candidateSigns, vertex + 3, candidates.Length / candidateSigns.Length), first, root, |
| | 21 | 86 | | result.Add(product, endpointSigns[0]); |
| | | 87 | | } |
| | 7 | 88 | | return true; |
| | | 89 | | } |
| | | 90 | | |
| | | 91 | | internal static void SumWeights(Span<ulong> weights, Span<int> signs, ContactQuadratic result) |
| | | 92 | | { |
| | 500 | 93 | | result.Clear(); |
| | 4000 | 94 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 1500 | 95 | | result.Add(At(weights, signs, vertex, weights.Length / signs.Length)); |
| | 500 | 96 | | } |
| | | 97 | | |
| | | 98 | | internal static void SumWeighted(Span<ulong> coordinates, Span<int> coordinateSigns, |
| | | 99 | | Span<ulong> weights, Span<int> weightSigns, ReadOnlySpan<ulong> root, ContactQuadratic result) |
| | | 100 | | { |
| | 269 | 101 | | int Words = result.FieldWords; |
| | 269 | 102 | | Span<ulong> work = stackalloc ulong[2 * Words]; |
| | 269 | 103 | | Span<int> signs = stackalloc int[2]; |
| | 269 | 104 | | ContactQuadratic product = At(work, signs, 0, work.Length / signs.Length); |
| | 269 | 105 | | result.Clear(); |
| | 2152 | 106 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 107 | | { |
| | 807 | 108 | | Multiply(At(coordinates, coordinateSigns, vertex, coordinates.Length / coordinateSigns.Length), At(weights, |
| | 807 | 109 | | result.Add(product); |
| | | 110 | | } |
| | 269 | 111 | | } |
| | | 112 | | internal static Fixed64 RoundTriangleCoordinate(in TriangleCircularGeometry geometry, FixedTriangle triangle, int co |
| | | 113 | | Span<ulong> weights, Span<int> signs, ContactQuadratic denominator, ReadOnlySpan<ulong> root) |
| | | 114 | | { |
| | 714 | 115 | | int Words = denominator.FieldWords; |
| | 714 | 116 | | Span<Signed576> coordinates = stackalloc Signed576[3]; |
| | 5712 | 117 | | for (int vertex = 0; vertex < 3; vertex++) |
| | 2142 | 118 | | coordinates[vertex] = geometry.WitnessCoordinate(triangle, vertex, component); |
| | 714 | 119 | | Span<ulong> values = stackalloc ulong[(geometry.IsAffine ? 4 : 2) * Words]; |
| | 714 | 120 | | Span<int> valueSigns = stackalloc int[geometry.IsAffine ? 4 : 2]; |
| | 714 | 121 | | ContactQuadratic numerator = At(values, valueSigns, 0, values.Length / valueSigns.Length); |
| | 714 | 122 | | Sum(coordinates, weights, signs, numerator); |
| | 1203 | 123 | | if (!geometry.IsAffine) return RoundRatio(numerator, denominator, root); |
| | 225 | 124 | | ContactQuadratic scaledDenominator = At(values, valueSigns, 1, values.Length / valueSigns.Length); |
| | 225 | 125 | | Scale(denominator, Signed576.ExtendValue(geometry.RawScale), scaledDenominator); |
| | 225 | 126 | | return RoundRatio(numerator, scaledDenominator, root); |
| | | 127 | | } |
| | | 128 | | |
| | | 129 | | internal static void Sum(ReadOnlySpan<Signed832> coordinates, Span<ulong> weights, Span<int> signs, ContactQuadratic |
| | | 130 | | { |
| | 545 | 131 | | int Words = result.FieldWords; |
| | 545 | 132 | | Span<ulong> values = stackalloc ulong[2 * Words]; |
| | 545 | 133 | | Span<int> productSigns = stackalloc int[2]; |
| | 545 | 134 | | ContactQuadratic product = At(values, productSigns, 0, values.Length / productSigns.Length); |
| | 545 | 135 | | result.Clear(); |
| | 4360 | 136 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 137 | | { |
| | 1635 | 138 | | Scale(At(weights, signs, vertex, weights.Length / signs.Length), coordinates[vertex], product); |
| | 1635 | 139 | | result.Add(product); |
| | | 140 | | } |
| | 545 | 141 | | } |
| | | 142 | | |
| | | 143 | | internal static void Sum(ReadOnlySpan<Signed576> coordinates, Span<ulong> weights, Span<int> signs, ContactQuadratic |
| | | 144 | | { |
| | 714 | 145 | | int Words = result.FieldWords; |
| | 714 | 146 | | Span<ulong> values = stackalloc ulong[2 * Words]; |
| | 714 | 147 | | Span<int> productSigns = stackalloc int[2]; |
| | 714 | 148 | | ContactQuadratic product = At(values, productSigns, 0, values.Length / productSigns.Length); |
| | 714 | 149 | | result.Clear(); |
| | 5712 | 150 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 151 | | { |
| | 2142 | 152 | | Scale(At(weights, signs, vertex, weights.Length / signs.Length), coordinates[vertex], product); |
| | 2142 | 153 | | result.Add(product); |
| | | 154 | | } |
| | 714 | 155 | | } |
| | | 156 | | |
| | | 157 | | } |