| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleConeWitnesses.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 authored-frame witnesses on the cone's free generator.</summary> |
| | | 16 | | internal static class TriangleConeWitnesses |
| | | 17 | | { |
| | | 18 | | internal static void GetGenerator(in TriangleCircularGeometry geometry, FixedTriangle triangle, |
| | | 19 | | Fixed64 height, Fixed64 radius, int mask, ReadOnlySpan<ulong> normal, ReadOnlySpan<int> normalSigns, |
| | | 20 | | Vector3d center, FixedQuaternion rotation, out Vector3d trianglePoint, out FixedPointAnchor coneAnchor) |
| | | 21 | | { |
| | 80 | 22 | | trianglePoint = default; coneAnchor = default; |
| | 40 | 23 | | ReadOnlySpan<ulong> root = Slot(normal, 6); |
| | | 24 | | // Only compact face/generator owners enter here: effective normal |
| | | 25 | | // height <458 bits. Oversized projected axes explicitly defer equality. |
| | 40 | 26 | | Span<ulong> d = stackalloc ulong[6 * Words]; |
| | 40 | 27 | | Span<int> dSigns = stackalloc int[6]; |
| | 40 | 28 | | Span<ulong> distances = stackalloc ulong[6 * Words]; |
| | 40 | 29 | | Span<int> distanceSigns = stackalloc int[6]; |
| | 40 | 30 | | Span<ulong> projections = stackalloc ulong[6 * Words]; |
| | 40 | 31 | | Span<int> projectionSigns = stackalloc int[6]; |
| | 40 | 32 | | Span<ulong> weights = stackalloc ulong[6 * Words]; |
| | 40 | 33 | | Span<int> weightSigns = stackalloc int[6]; |
| | 40 | 34 | | Span<ulong> work = stackalloc ulong[14 * Words]; |
| | 40 | 35 | | Span<int> workSigns = stackalloc int[14]; |
| | 80 | 36 | | ContactQuadratic component = At(work, workSigns, 0), product = At(work, workSigns, 1); |
| | 80 | 37 | | ContactQuadratic bound = At(work, workSigns, 2), sum = At(work, workSigns, 3); |
| | 120 | 38 | | ContactQuadratic denominator = At(work, workSigns, 4), tNumerator = At(work, workSigns, 5), tDenominator = At(wo |
| | 80 | 39 | | Signed192 h = Signed192.Raw(height), r = Signed192.Raw(radius); |
| | 40 | 40 | | Signed832 hh = Signed832.ExtendValue(WideArithmetic.MultiplySigned192(h, h)); |
| | 40 | 41 | | Signed832 rr = Signed832.ExtendValue(WideArithmetic.MultiplySigned192(r, r)); |
| | 40 | 42 | | Signed832 length = WideArithmetic.AddSigned832(hh, rr); |
| | 320 | 43 | | for (int axis = 0; axis < 3; axis++) |
| | | 44 | | { |
| | 120 | 45 | | TriangleConeGeneratorFeatures.Component(normal, normalSigns, axis, component); |
| | 120 | 46 | | Scale(component, axis == 1 ? hh : Negate(rr), At(d, dSigns, axis)); |
| | | 47 | | } |
| | 40 | 48 | | TriangleConeGeneratorFeatures.Component(normal, normalSigns, 1, component); |
| | 40 | 49 | | Scale(component, Negate(Signed832.ExtendValue(Signed576.ExtendValue(h))), product); |
| | 40 | 50 | | Scale(product, length, component); |
| | 40 | 51 | | Scale(component, Signed832.ExtendValue(Signed576.ExtendValue(geometry.RawScale)), bound); |
| | | 52 | | System.Diagnostics.Debug.Assert(bound.Sign(root) > 0); |
| | 320 | 53 | | for (int vertex = 0; vertex < 3; vertex++) |
| | | 54 | | { |
| | 120 | 55 | | WideDirection3d p = geometry.Vertex(vertex); |
| | 120 | 56 | | ContactQuadratic distance = At(distances, distanceSigns, vertex); |
| | 120 | 57 | | TriangleConeGeneratorFeatures.Component(normal, normalSigns, 0, component); |
| | 120 | 58 | | Scale(component, Signed832.ExtendValue(p.Z), distance); |
| | 120 | 59 | | TriangleConeGeneratorFeatures.Component(normal, normalSigns, 2, component); |
| | 120 | 60 | | Scale(component, Signed832.ExtendValue(p.X), product); |
| | 120 | 61 | | distance.Add(product, -1); |
| | 120 | 62 | | sum.Clear(); |
| | 960 | 63 | | for (int axis = 0; axis < 3; axis++) |
| | | 64 | | { |
| | 360 | 65 | | Signed576 coordinate = CylinderContactAlgebra.Component(p, axis); |
| | 480 | 66 | | if (axis == 1) coordinate = WideArithmetic.SubtractSigned576(coordinate, geometry.HalfHeight); |
| | 360 | 67 | | Scale(At(d, dSigns, axis), Signed832.ExtendValue(coordinate), product); |
| | 360 | 68 | | sum.Add(product); |
| | | 69 | | } |
| | 120 | 70 | | ContactQuadratic projection = At(projections, projectionSigns, vertex); |
| | 360 | 71 | | sum.CopyTo(projection); projection.Add(sum); projection.Add(bound, -1); |
| | | 72 | | } |
| | 80 | 73 | | weights.Clear(); weightSigns.Clear(); |
| | 40 | 74 | | bool found = TryGetWeights(mask, distances, distanceSigns, projections, projectionSigns, bound, root, |
| | 40 | 75 | | weights, weightSigns, out bool centered); |
| | | 76 | | // The complete triangle support fan admitted this exact generator and |
| | | 77 | | // selected feature together, so their finite slice contains a witness. |
| | | 78 | | // General slice callers still own TryGetWeights' legitimate failure; |
| | | 79 | | // this matched-feature owner never declines or changes range handling. |
| | | 80 | | System.Diagnostics.Debug.Assert(found); |
| | 40 | 81 | | SumWeights(weights, weightSigns, denominator); |
| | 40 | 82 | | trianglePoint = new Vector3d( |
| | 40 | 83 | | RoundTriangleCoordinate(geometry, triangle, 0, weights, weightSigns, denominator, root), |
| | 40 | 84 | | RoundTriangleCoordinate(geometry, triangle, 1, weights, weightSigns, denominator, root), |
| | 40 | 85 | | RoundTriangleCoordinate(geometry, triangle, 2, weights, weightSigns, denominator, root)); |
| | 40 | 86 | | if (centered) |
| | | 87 | | { |
| | | 88 | | // The larger blended triangle weights must not be reprojected: |
| | | 89 | | // their semantic generator parameter is exactly one half. |
| | 2 | 90 | | tNumerator.Set(Signed832.ExtendValue(Signed576.ExtendValue(Signed192.Signed(1)))); |
| | 2 | 91 | | tDenominator.Set(Signed832.ExtendValue(Signed576.ExtendValue(Signed192.Signed(2)))); |
| | | 92 | | } |
| | | 93 | | else |
| | | 94 | | { |
| | 38 | 95 | | SumWeighted(projections, projectionSigns, weights, weightSigns, root, tNumerator); |
| | 38 | 96 | | Multiply(denominator, bound, root, tDenominator); |
| | 38 | 97 | | tNumerator.Add(tDenominator); |
| | 76 | 98 | | tDenominator.CopyTo(product); tDenominator.Add(product); |
| | | 99 | | } |
| | 40 | 100 | | int endpoint = tNumerator.Sign(root) == 0 ? 1 : 0; |
| | 80 | 101 | | tNumerator.CopyTo(component); component.Add(tDenominator, -1); |
| | 41 | 102 | | if (component.Sign(root) == 0) endpoint = -1; |
| | 40 | 103 | | Vector3d conePoint = new( |
| | 40 | 104 | | RoundRadial(0, normal, normalSigns, rr, h, tNumerator, tDenominator, root, radius), |
| | 40 | 105 | | RoundAxial(h, tNumerator, tDenominator, root), |
| | 40 | 106 | | RoundRadial(2, normal, normalSigns, rr, h, tNumerator, tDenominator, root, radius)); |
| | 40 | 107 | | coneAnchor = endpoint == 0 ? new FixedPointAnchor(center, rotation, conePoint) |
| | 40 | 108 | | : TriangleCircularGeometry.GetSupport(center, rotation, Signed192.Raw(height), |
| | 40 | 109 | | new Vector3d(conePoint.X, Fixed64.Zero, conePoint.Z), -endpoint); |
| | 40 | 110 | | } |
| | | 111 | | |
| | | 112 | | private static Fixed64 RoundRadial(int axis, ReadOnlySpan<ulong> normal, ReadOnlySpan<int> signs, |
| | | 113 | | Signed832 radiusSquared, Signed192 height, ContactQuadratic tNumerator, ContactQuadratic tDenominator, |
| | | 114 | | ReadOnlySpan<ulong> root, Fixed64 radius) |
| | | 115 | | { |
| | | 116 | | // Endpoint weights <658 bits give t numerators <1448 and final |
| | | 117 | | // coordinate numerators <2040. Center blends use t=1/2 directly. |
| | 80 | 118 | | Span<ulong> work = stackalloc ulong[8 * Words]; |
| | 80 | 119 | | Span<int> workSigns = stackalloc int[8]; |
| | 160 | 120 | | ContactQuadratic component = At(work, workSigns, 0), scaled = At(work, workSigns, 1); |
| | 160 | 121 | | ContactQuadratic numerator = At(work, workSigns, 2), denominator = At(work, workSigns, 3); |
| | 80 | 122 | | TriangleConeGeneratorFeatures.Component(normal, signs, axis, component); |
| | 80 | 123 | | Scale(component, radiusSquared, scaled); |
| | 80 | 124 | | Multiply(scaled, tNumerator, root, numerator); |
| | 80 | 125 | | TriangleConeGeneratorFeatures.Component(normal, signs, 1, component); |
| | 80 | 126 | | Scale(component, Signed832.ExtendValue(Signed576.ExtendValue(height)), scaled); |
| | 80 | 127 | | Multiply(scaled, tDenominator, root, denominator); |
| | 160 | 128 | | numerator.MultiplySign(-1); denominator.MultiplySign(-1); |
| | 80 | 129 | | return RoundRatio(numerator, denominator, root, -radius.m_rawValue, radius.m_rawValue); |
| | | 130 | | } |
| | | 131 | | |
| | | 132 | | private static Fixed64 RoundAxial(Signed192 height, ContactQuadratic tNumerator, |
| | | 133 | | ContactQuadratic tDenominator, ReadOnlySpan<ulong> root) |
| | | 134 | | { |
| | 40 | 135 | | Span<ulong> work = stackalloc ulong[6 * Words]; |
| | 40 | 136 | | Span<int> signs = stackalloc int[6]; |
| | 120 | 137 | | ContactQuadratic difference = At(work, signs, 0), numerator = At(work, signs, 1), denominator = At(work, signs, |
| | 120 | 138 | | tDenominator.CopyTo(difference); difference.Add(tNumerator, -1); difference.Add(tNumerator, -1); |
| | 40 | 139 | | Scale(difference, Signed832.ExtendValue(Signed576.ExtendValue(height)), numerator); |
| | 80 | 140 | | tDenominator.CopyTo(denominator); denominator.Add(tDenominator); |
| | 40 | 141 | | return RoundRatio(numerator, denominator, root); |
| | | 142 | | } |
| | | 143 | | |
| | 120 | 144 | | private static Signed832 Negate(Signed832 value) => WideArithmetic.SubtractSigned832(default, value); |
| | | 145 | | } |