| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleConeGeneratorFeatures.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>The exact fixed-latitude generator normal circle and its triangle fan.</summary> |
| | | 15 | | internal static class TriangleConeGeneratorFeatures |
| | | 16 | | { |
| | | 17 | | internal static void KeepAll(in TriangleCircularGeometry geometry, Fixed64 height, Fixed64 radius, |
| | | 18 | | ref TriangleConeContactSelection selection) |
| | | 19 | | { |
| | 88 | 20 | | Span<ulong> values = stackalloc ulong[ConvexContactCandidate.Slots * Words]; |
| | 88 | 21 | | Span<int> signs = stackalloc int[ConvexContactCandidate.Slots]; |
| | 88 | 22 | | Span<ulong> work = stackalloc ulong[7 * Words]; |
| | 264 | 23 | | Span<ulong> hSquared = Slot(work, 0), rSquared = Slot(work, 1), length = Slot(work, 2); |
| | 176 | 24 | | Span<ulong> metric = Slot(work, 3), product = Slot(work, 4); |
| | 176 | 25 | | Signed192 h = Signed192.Raw(height), r = Signed192.Raw(radius); |
| | 88 | 26 | | Import(WideArithmetic.MultiplySigned192(h, h), hSquared); |
| | 88 | 27 | | Import(WideArithmetic.MultiplySigned192(r, r), rSquared); |
| | 88 | 28 | | WideArithmetic.AddEqualMagnitudes(hSquared, rSquared, length); |
| | | 29 | | // All generator directions share the raw-coordinate scale. Include |
| | | 30 | | // its square once in the length factor used by every gap denominator. |
| | 88 | 31 | | Import(Signed576.ExtendValue(geometry.RawScale), product); |
| | 88 | 32 | | WideArithmetic.MultiplyMagnitudes(product, product, Slot(work, 6)); |
| | 88 | 33 | | WideArithmetic.MultiplyMagnitudes(length, Slot(work, 6), product); |
| | 88 | 34 | | product.CopyTo(length); |
| | 176 | 35 | | values.Clear(); signs.Clear(); |
| | 88 | 36 | | Write(values, signs, 0, Signed832.ExtendValue(Signed576.ExtendValue(h))); |
| | 88 | 37 | | Write(values, signs, 1, Signed832.ExtendValue(Signed576.ExtendValue(WideArithmetic.SubtractSigned192(default, r) |
| | | 38 | | // A zero-radial vertex can own the whole constant support circle. |
| | 88 | 39 | | Keep(geometry, values, signs, length, ref selection); |
| | 690 | 40 | | for (int vertex = 0; vertex < 3 && !selection.Separated; vertex++) |
| | | 41 | | { |
| | 257 | 42 | | WideDirection3d p = geometry.Vertex(vertex); |
| | 257 | 43 | | Signed832 radial = CircularRimContactAlgebra.RadialDot(p, p); |
| | 257 | 44 | | if (radial.Sign != 0) |
| | | 45 | | { |
| | 498 | 46 | | values.Clear(); signs.Clear(); |
| | 249 | 47 | | Write(values, signs, 0, Signed832.ExtendValue(WideArithmetic.MultiplySigned576(p.X, h)), -1); |
| | 249 | 48 | | Write(values, signs, 2, Signed832.ExtendValue(WideArithmetic.MultiplySigned576(p.Z, h)), -1); |
| | 249 | 49 | | Write(values, signs, 4, Signed832.ExtendValue(Signed576.ExtendValue(r)), -1); |
| | 249 | 50 | | Import(radial, Slot(values, 6)); |
| | 249 | 51 | | WideArithmetic.MultiplyMagnitudes(Slot(values, 6), length, metric); |
| | 1494 | 52 | | for (int orientation = -1; orientation <= 1; orientation += 2) |
| | | 53 | | { |
| | 498 | 54 | | Keep(geometry, values, signs, metric, ref selection); |
| | 996 | 55 | | signs[0] = -signs[0]; signs[2] = -signs[2]; |
| | | 56 | | } |
| | | 57 | | } |
| | 257 | 58 | | WideDirection3d e = geometry.Edge(vertex); |
| | 257 | 59 | | Signed832 eSquared = CircularRimContactAlgebra.RadialDot(e, e); |
| | 257 | 60 | | if (eSquared.Sign == 0) |
| | | 61 | | continue; |
| | 506 | 62 | | values.Clear(); signs.Clear(); |
| | 253 | 63 | | Import(eSquared, metric); |
| | 253 | 64 | | WideArithmetic.MultiplyMagnitudes(metric, hSquared, Slot(values, 6)); |
| | 253 | 65 | | Import(WideArithmetic.MultiplySigned576(e.Y, e.Y), product); |
| | 253 | 66 | | WideArithmetic.MultiplyMagnitudes(product, rSquared, Slot(work, 5)); |
| | 253 | 67 | | int discriminantSign = 1; |
| | 253 | 68 | | Add(Slot(work, 5), -1, Slot(values, 6), ref discriminantSign); |
| | 253 | 69 | | if (discriminantSign < 0) |
| | | 70 | | continue; |
| | 206 | 71 | | Write(values, signs, 0, WideArithmetic.MultiplySigned832(WideArithmetic.MultiplySigned576(e.Y, e.X), Signed1 |
| | 206 | 72 | | Write(values, signs, 1, WideArithmetic.MultiplySigned832(eSquared, Signed192.Raw(radius)), -1); |
| | 206 | 73 | | Write(values, signs, 2, WideArithmetic.MultiplySigned832(WideArithmetic.MultiplySigned576(e.Y, e.Z), Signed1 |
| | 206 | 74 | | Write(values, signs, 3, Signed832.ExtendValue(e.Z), -1); |
| | 206 | 75 | | Write(values, signs, 5, Signed832.ExtendValue(e.X)); |
| | 206 | 76 | | WideArithmetic.MultiplyMagnitudes(metric, metric, product); |
| | 206 | 77 | | WideArithmetic.MultiplyMagnitudes(product, length, metric); |
| | 206 | 78 | | Keep(geometry, values, signs, metric, ref selection); |
| | 206 | 79 | | if (discriminantSign != 0 && !selection.Separated) |
| | | 80 | | { |
| | | 81 | | // Selection preserves the local input: the other quadratic |
| | | 82 | | // branch changes only the two radial radical signs. |
| | 396 | 83 | | signs[3] = -signs[3]; signs[5] = -signs[5]; |
| | 198 | 84 | | Keep(geometry, values, signs, metric, ref selection); |
| | | 85 | | } |
| | | 86 | | } |
| | 88 | 87 | | } |
| | | 88 | | |
| | | 89 | | internal static void Keep(in TriangleCircularGeometry geometry, scoped Span<ulong> values, scoped Span<int> signs, |
| | | 90 | | scoped ReadOnlySpan<ulong> denominator, ref TriangleConeContactSelection selection) |
| | | 91 | | { |
| | 995 | 92 | | Span<ulong> work = stackalloc ulong[6 * Words]; |
| | 995 | 93 | | Span<int> workSigns = stackalloc int[6]; |
| | 2985 | 94 | | ContactQuadratic maximum = At(work, workSigns, 0), current = At(work, workSigns, 1), difference = At(work, workS |
| | 995 | 95 | | ReadOnlySpan<ulong> root = Slot(values, 6); |
| | 995 | 96 | | Dot(geometry.A, values, signs, maximum); |
| | 995 | 97 | | int mask = 1; |
| | 5970 | 98 | | for (int vertex = 1; vertex < 3; vertex++) |
| | | 99 | | { |
| | 1990 | 100 | | Dot(geometry.Vertex(vertex), values, signs, current); |
| | 3980 | 101 | | current.CopyTo(difference); difference.Add(maximum, -1); |
| | 1990 | 102 | | int comparison = difference.Sign(root); |
| | 3318 | 103 | | if (comparison > 0) { current.CopyTo(maximum); mask = 1 << vertex; } |
| | 1675 | 104 | | else if (comparison == 0) mask |= 1 << vertex; |
| | | 105 | | } |
| | 995 | 106 | | Component(values, signs, 1, current); |
| | 995 | 107 | | Scale(current, Signed832.ExtendValue(geometry.HalfHeight), difference); |
| | 995 | 108 | | maximum.Add(difference, -1); |
| | 995 | 109 | | int gapSign = BuildQuadraticCandidate(maximum, root, denominator, values, signs); |
| | 995 | 110 | | selection.Keep(values, signs, gapSign, mask, TriangleConeContactSelection.Generator); |
| | 995 | 111 | | } |
| | | 112 | | |
| | | 113 | | internal static void Dot(in WideDirection3d axis, ReadOnlySpan<ulong> normal, ReadOnlySpan<int> signs, ContactQuadra |
| | | 114 | | { |
| | 2985 | 115 | | CylinderContactAlgebra.Dot(axis, normal[..(3 * Words)], signs[..3], result.Rational, out int rationalSign); |
| | 2985 | 116 | | CylinderContactAlgebra.Dot(axis, normal.Slice(3 * Words, 3 * Words), signs.Slice(3, 3), result.Radical, out int |
| | 5970 | 117 | | result.Signs[0] = rationalSign; result.Signs[1] = radicalSign; |
| | 2985 | 118 | | } |
| | | 119 | | |
| | | 120 | | internal static void Component(ReadOnlySpan<ulong> normal, ReadOnlySpan<int> signs, int component, ContactQuadratic |
| | | 121 | | { |
| | 3110 | 122 | | Slot(normal, component).CopyTo(result.Rational); Slot(normal, component + 3).CopyTo(result.Radical); |
| | 3110 | 123 | | result.Signs[0] = signs[component]; result.Signs[1] = signs[component + 3]; |
| | 1555 | 124 | | } |
| | | 125 | | |
| | | 126 | | private static void Write(Span<ulong> values, Span<int> signs, int slot, Signed832 value, int orientation = 1) |
| | | 127 | | { |
| | 3906 | 128 | | Import(value, Slot(values, slot)); signs[slot] = orientation * value.Sign; |
| | 1953 | 129 | | } |
| | | 130 | | } |